#include "circle_detect.h"


void mysobel(rgb_img_t &src, rgb_img_t &dst, int dir)
{
	int const rows = MAX_HEIGHT;
	int const cols = MAX_WIDTH;

	rgb_img_t img0(rows, cols);

	if (dir)
	{
		hls::Sobel<1,0,3>(src, img0);
	}
	else
	{
		hls::Sobel<0,1,3>(src, img0);
	}

	hls::ConvertScaleAbs(img0, dst);

}

void mysobelxy(rgb_img_t &src, rgb_img_t &dst)
{
	int const rows = MAX_HEIGHT;
	int const cols = MAX_WIDTH;

	rgb_img_t img0(rows, cols);
	rgb_img_t img1(rows, cols);
	rgb_img_t img2(rows, cols);
	rgb_img_t img3(rows, cols);

	hls::Duplicate(src, img0, img1);
	mysobel(img0, img2, 1);
	mysobel(img1, img3, 0);
	hls::AddWeighted(img2, 1, img3, 1, 0, dst);
}


void circle(rgb_img_t &src, rgb_img_t &dst){
	int const rows = MAX_HEIGHT;
	int const cols = MAX_WIDTH;
	int const delta = 1;
	int const circles = N_CIRCLE;
	// 1/sqrt(2) pour un rayon aussi grand que le diametre d'image
	int const diag = (rows+cols)*0.5;
	uint16_t rho;
	typedef hls::Mat<diag, 180/delta, HLS_12U> M_M;
	typedef hls::Mat<circles, 2, HLS_16U> M_Max;

	M_M M;
	M_Max Max;

	rgb_pix_t red(255,0,0);

	for(int d=0; d<diag; d++){
		Max<d,0> = 0;
		Max<d,1> = 0;
	}

	rgb_img_t img0(rows, cols);

	hls::CvtColor<HLS_GRAY2RGB>(src, dst);

	for (uint16_t r=0; r<rows; r++){
		for (uint16_t c=0; c<cols; c++){
			if(src(r,c) != 0){
				for (uint16_t t=0; t<180; t+=delta){
					rho = ceil(r*hls::cos(t)+c*hls::sin(t));
					M(rho,t) = M(rho,t) + 1;
				}
			}
		}
	}

	uint8_t m = 0;
	uint16_t threshold = 50;
	uint16_t x0, y0;

	while(m<circles){
		for (uint16_t c=0; c<circles; c++){
			for (uint16_t r=0; r<diag; r++){
				for (uint16_t t=0; t<180; t+=delta){
					if(M(r,t) > threshold){
						m++;

						x0 = ceil(rho * cos(t));
						y0 = ceil(rho * sin(t));

						Max(m,0) = x0 + 1000 * (-sin(t));
						Max(m,1) = y0 + 1000 * (cos(t));

						dst(Max(m,0),Max(m,1)) = red;
						dst(Max(m,0)+1,Max(m,1)) = red;
						dst(Max(m,0),Max(m,1)+1) = red;
						dst(Max(m,0+1),Max(m,1)+1) = red;
					}
				}
			}
		}
	}
}

void sobelfoo(stream_t &stream_in, stream_t &stream_out)
{
	int const rows = MAX_HEIGHT;
	int const cols = MAX_WIDTH;

	rgb_img_t img0(rows, cols);
	rgb_img_t img1(rows, cols);
	rgb_img_t img2(rows, cols);
	rgb_img_t img3(rows, cols);
	rgb_img_t img4(rows, cols);

	hls::AXIvideo2Mat(stream_in, img0);
	hls::CvtColor<HLS_RGB2GRAY>(img0, img1);
	hls::GaussianBlur<5,5>(img1, img2, (double)5, (double)5);

	mysobelxy(img2,img3);
	circle(img3, img4);


	hls::Mat2AXIvideo(img4, stream_out);
}



void circle_detect(stream_t &stream_in, stream_t &stream_out)
{

	int const rows = MAX_HEIGHT;
	int const cols = MAX_WIDTH;

	sobelfoo(stream_in, stream_out);
}