#include <stim/image/image.h>
#include <cmath>
#include <conio.h>
#include <stim/visualization/colormap.h>
#include <stim/image/image_contour_detection.h>

void array_multiply(float* lhs, float rhs, unsigned int N);
void array_add(float* ptr1, float* ptr2, float* sum, unsigned int N);

stim::image<float> func_mPb_theta(stim::image<float> lab, float theta, unsigned int w, unsigned int h){

	stim::image<float> mPb_theta(w, h, 1);

	stim::image<float> pic_light, pic_colora, pic_colorb;	
	pic_light = lab.channel(0);
	pic_colora = lab.channel(1);
	pic_colorb = lab.channel(2);

	unsigned int N = w * h;
	float sigma = 2; 
	unsigned int sigma_n = 3;
	unsigned r1 = 3;
	unsigned r2 = 5;
	unsigned r3 = 10;
	unsigned r4 = 20;
	float alpha[9] = {1,1,1,1,1,1,1,1,1};


	stim::image<float> l1,l2,l3,a1,a2,a3,b1,b2,b3;

	l1 = gaussian_derivative_filter_odd(pic_light, sigma, sigma_n, r1 * 2, theta, w, h);
	stim::cpu2image(l1.data(), "data_output/l1_tex2.bmp", w, h, stim::cmBrewer);
	l2 = gaussian_derivative_filter_odd(pic_light, sigma, sigma_n, r2 * 2, theta, w, h);
	stim::cpu2image(l2.data(), "data_output/l2_tex.bmp", w, h, stim::cmBrewer);
	l3 = gaussian_derivative_filter_odd(pic_light, sigma, sigma_n, r3 * 2, theta, w, h);
	stim::cpu2image(l3.data(), "data_output/l3_tex.bmp", w, h, stim::cmBrewer);
	a1 = gaussian_derivative_filter_odd(pic_colora, sigma, sigma_n, r2 * 2, theta, w, h);
	stim::cpu2image(a1.data(), "data_output/a1_tex.bmp", w, h, stim::cmBrewer);
	a2 = gaussian_derivative_filter_odd(pic_colora, sigma, sigma_n, r3 * 2, theta, w, h);
	stim::cpu2image(a2.data(), "data_output/a2_tex.bmp", w, h, stim::cmBrewer);
	a3 = gaussian_derivative_filter_odd(pic_colora, sigma, sigma_n, r4 * 2, theta, w, h);
	stim::cpu2image(a3.data(), "data_output/a3_tex.bmp", w, h, stim::cmBrewer);
	b1 = gaussian_derivative_filter_odd(pic_colorb, sigma, sigma_n, r2 * 2, theta, w, h);
	stim::cpu2image(b1.data(), "data_output/b1_tex.bmp", w, h, stim::cmBrewer);
	b2 = gaussian_derivative_filter_odd(pic_colorb, sigma, sigma_n, r3 * 2, theta, w, h);
	stim::cpu2image(b2.data(), "data_output/b2_tex.bmp", w, h, stim::cmBrewer);
	b3 = gaussian_derivative_filter_odd(pic_colorb, sigma, sigma_n, r4 * 2, theta, w, h);
	stim::cpu2image(b3.data(), "data_output/b3_tex.bmp", w, h, stim::cmBrewer);

	/*for (unsigned i = 0; i<N; i++){

		mPb_theta.data()[i] = l1.data()[i] * alpha[0] +
							  l2.data()[i] * alpha[1] +
							  l3.data()[i] * alpha[2] +
							  a1.data()[i] * alpha[3] +
							  a2.data()[i] * alpha[4] +
							  a3.data()[i] * alpha[5] +
							  b1.data()[i] * alpha[6] +
							  b2.data()[i] * alpha[7] +
							  b3.data()[i] * alpha[8] ;
	
	}*/


	array_multiply(l1.data(), alpha[0], N);
	//stim::cpu2image(l1.data(), "data_output/array_add_l1.bmp", w, h, stim::cmBrewer);
	array_multiply(l2.data(), alpha[1], N);
	//stim::cpu2image(l2.data(), "data_output/array_add_l2.bmp", w, h, stim::cmBrewer);
	array_multiply(l3.data(), alpha[2], N);
	array_multiply(a1.data(), alpha[3], N);
	array_multiply(a2.data(), alpha[4], N);
	array_multiply(a3.data(), alpha[5], N);
	array_multiply(b1.data(), alpha[6], N);
	array_multiply(b2.data(), alpha[7], N);
	array_multiply(b3.data(), alpha[8], N);

	array_add(l1.data(), l2.data(), mPb_theta.data(), N);
	//stim::cpu2image(sum, "data_output/array_add_sum.bmp", w, h, stim::cmBrewer);
	array_add(mPb_theta.data(), l3.data(), mPb_theta.data(), N);
	array_add(mPb_theta.data(), a1.data(), mPb_theta.data(), N);
	array_add(mPb_theta.data(), a2.data(), mPb_theta.data(), N);
	array_add(mPb_theta.data(), a3.data(), mPb_theta.data(), N);
	array_add(mPb_theta.data(), b1.data(), mPb_theta.data(), N);
	array_add(mPb_theta.data(), b2.data(), mPb_theta.data(), N);
	array_add(mPb_theta.data(), b3.data(), mPb_theta.data(), N);

	//stim::cpu2image(mPb_theta.data(), "data_output/mPb_theta0_1.bmp", w, h, stim::cmBrewer);


	//getch();

	return mPb_theta;
}