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

#define PI 3.1415926

void conv2(float* img, float* mask, float* cpu_copy, unsigned int w, unsigned int h, unsigned int M);
void array_abs(float* img, unsigned int N);
void array_multiply(float* lhs, float rhs, unsigned int N);

// winsize = 2 * r, side of mask = winsize + 1
stim::image<float> gaussian_derivative_filter_odd(stim::image<float> image, float sigma, unsigned int sigma_n, unsigned int winsize, float theta, unsigned int w, unsigned int h){

	stim::image<float> mask_x(winsize+1, winsize+1), mask_y(winsize+1, winsize+1), mask_theta(winsize+1, winsize+1), derivative_x, derivative_y, derivative_theta(w, h);
	//float* ptr = mask_x.data();

	//mask_x.load("101087.bmp");
	//float s[169];
	//float *ptr = s;

	// set parameters
	unsigned N = w * h;
	float theta_r = (theta * PI)/180;

	float step = (2*sigma*sigma_n)/winsize;

	for (unsigned j = 0; j <= winsize; j++){
		for (unsigned i = 0; i<= winsize; i++){

			float x = (-1)*sigma*sigma_n + i * step;          //range of x
			float y = (-1)*sigma*sigma_n + j * step;          //range of y

			// create the x-oriented gaussian derivative filter mask_x
			mask_x.data()[j*(winsize+1) + i] = (-1) * x * exp((-1)*(pow(x, 2))/(2*pow(sigma, 2))) * exp((-1)*(pow(y, 2))/(2*pow(sigma, 2)));
			// create the y-oriented gaussian derivative filter mask_y
			mask_y.data()[j*(winsize+1) + i] = (-1) * y * exp((-1)*(pow(y, 2))/(2*pow(sigma, 2))) * exp((-1)*(pow(x, 2))/(2*pow(sigma, 2)));
			// create the mask_theta
			mask_theta.data()[j*(winsize+1) + i] = cos(theta_r) * mask_x.data()[j*(winsize+1) + i] + sin(theta_r) * mask_y.data()[j*(winsize+1) + i] ;
		
		}
	}

	//stim::cpu2image(mask_x.data(), "data_output/cmapgray_mask_x.bmp", winsize+1, winsize+1, stim::cmBrewer);
	
	stim::cpu2image(image.data(), "data_output/image.bmp", w, h, stim::cmBrewer);


	stim::cpu2image(mask_theta.data(), "data_output/mask.bmp", winsize+1, winsize+1, stim::cmBrewer);

	// 2D convolution
	//derivative_theta = image.convolve2(mask_theta);
	//stim::cpu2image(derivative_theta.data(), "data_output/derivative_theta1.bmp", w, h, stim::cmBrewer);
	conv2(image.data(), mask_theta.data(), derivative_theta.data(), w, h, winsize+1);
	//stim::cpu2image(derivative_theta.data(), "data_output/derivative_theta_tex1.bmp", w, h, stim::cmBrewer);

	//array_abs(derivative_theta.data(), N);
	
	/*for (unsigned k = 0; k < w * h; k++){
		
		derivative_theta.data()[k] = abs(derivative_theta.data()[k]);

	}*/

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

	/*float max = derivative_theta.max();

	array_multiply(derivative_theta.data(), 1/max, N);*/

	/*(
	for (unsigned k = 0; k < w * h; k++){
		
		derivative_theta.data()[k] = derivative_theta.data()[k]/max;

	})*/

	//float max2 = derivative_theta.max();

	//stim::cpu2image(derivative_theta.data(), "data_output/cmap_colorb_gradient_theta90_r5.bmp", w, h, stim::cmBrewer);
	//derivative_x.save("data_output/gradient_x.bmp");

	return derivative_theta;

}