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

#define PI 3.1415926

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(mask_y.data(), "data_output/cmapgray_mask_y.bmp", winsize+1, winsize+1, stim::cmBrewer);


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

	// 2D convolution
	derivative_theta = image.convolve2(mask_theta);

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

	}

	float max = derivative_theta.max();

	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;

}