#include "funkce.h"

//#define TEXT(t) (cout<<t)
#define TEXT(t) 

int vypis(void){

printf("ahoj\n");
return 2;
}



void prekladac(void){
#ifndef __cplusplus
  puts("Přeloženo jako čisté C");
#else
  puts("Přeloženo jako C++");
#endif

}

fftw_complex* img2fft(const IplImage *img){//funkce alokuje pamet!
//prevede obraz do fft

	/* setup pointers to images */
	uchar 	*data = ( uchar* ) img->imageData;
	
	/* allocate FFTW input and output arrays */
	fftw_complex *img1 = ( fftw_complex* )fftw_malloc( sizeof( fftw_complex ) * img->width * img->height );
	
	/* setup FFTW plans */
	fftw_plan fft_img1 = fftw_plan_dft_1d( img->width * img->height, img1, img1, FFTW_FORWARD,  FFTW_ESTIMATE );
	
	/* load images' data to FFTW input */
	for(int i = 0, k = 0 ; i < img->height ; i++ ) {
		for(int j = 0 ; j < img->width ; j++, k++ ) {
			img1[k][0] = ( double )data[i * img->widthStep+ j];
			img1[k][1] = 0.0;
		}
	}
	
	/* obtain the FFT of img1 */
	fftw_execute( fft_img1 );
	/*free plan*/
	fftw_destroy_plan(fft_img1);
	return img1;
}






CvPoint phase_correlation(const fftw_complex *img1 ,const fftw_complex *img2,CvSize size){
/*funkce vypocita konvoluci, a vraci pozici maxima
vstupni parametry do funkce jsou pole obrazku JIZ prevedenych do FFT !!!
size-velikost obrazku
 */
	int fft_size = size.width * size.height;
	IplImage *poc=cvCreateImage(size, IPL_DEPTH_64F,1);
	double 	*poc_data = ( double* )poc->imageData;
	double	tmp;

	fftw_complex *res  = ( fftw_complex* )fftw_malloc( sizeof( fftw_complex ) * size.width * size.height );	//mozna chyba v parametru size

	fftw_plan ifft_res = fftw_plan_dft_1d( size.width * size.height, res,  res,  FFTW_BACKWARD, FFTW_ESTIMATE );

	/* obtain the cross power spectrum */
	for(int i = 0; i < fft_size ; i++ ) {
		res[i][0] = ( img2[i][0] * img1[i][0] ) - ( img2[i][1] * ( -img1[i][1] ) );
		res[i][1] = ( img2[i][0] * ( -img1[i][1] ) ) + ( img2[i][1] * img1[i][0] );

		tmp = sqrt( pow( res[i][0], 2.0 ) + pow( res[i][1], 2.0 ) );
		if(tmp==0)cout<<"POZOR DELENI NULOU\n";
		res[i][0] /= tmp;
		res[i][1] /= tmp;
	}

	/* obtain the phase correlation array */
	fftw_execute(ifft_res);

	/* normalize and copy to result image */
	for(int i = 0 ; i < fft_size ; i++ ) {
	        poc_data[i] = res[i][0] / ( double )fft_size;
	}

	// find the maximum value and its location 
	CvPoint minloc, maxloc;
	double  minval, maxval;
	cvMinMaxLoc( poc, &minval, &maxval, &minloc, &maxloc, 0 );

	/* deallocate FFTW arrays and plans */
	fftw_destroy_plan( ifft_res );
	fftw_free( res );	
	cvReleaseImage(&poc);

	return maxloc;
}


int phase_correlation(IplImage *ref,const fftw_complex *img1 , IplImage *tpl,CvPoint * max){
/*
hybridni funkce, ktera jako parametr ma i kasicke obrazky (obrazek ref je metashape - je tu pouze kvuli rozmerum)
dalsi parametr je img1 ktery predstavuje ref->fftw
tpl je klasicky obrazek
max- je navratova hodnota maxima
*/
	if(ref==NULL || img1==NULL||tpl==NULL){cout<<"POZOR na nektery VSTUPNI OBRAZ JE PRAZNY !! -fce phase_correlation koncim\n";return 0;}
	if(ref->height!=tpl->height){
		TEXT("POZOR zadane nestejne rozmery obrazku ! \n");
		TEXT("KONCIM\n");
	}
	IplImage *img_tpl=cvCreateImage( cvSize(ref->width,ref->height ),ref->depth,ref->nChannels);

	cvSetZero(img_tpl);
//	img_rotate(tpl,180);
	if(img_copy_image_beside(tpl,img_tpl,0)!=0)cout<<"Chyba pri kopirovani OBRAZU!!!\n";

	cvSaveImage("meta.png", ref);
	cvSaveImage("template.png", img_tpl);

	int 	i, j, k;
	
	/* get image properties */
	int width  	 = ref->width;
	int height   = ref->height;
	int step     = ref->widthStep;

	/* setup pointers to images */
	uchar 	*tpl_data = ( uchar* ) img_tpl->imageData;
	
	/* allocate FFTW input and output arrays */
	fftw_complex *img2 = ( fftw_complex* )fftw_malloc( sizeof( fftw_complex ) * width * height );

	
	/* setup FFTW plans */
	fftw_plan fft_img2 = fftw_plan_dft_1d( width * height, img2, img2, FFTW_FORWARD,  FFTW_ESTIMATE );

	
	/* load images' data to FFTW input */
	for( i = 0, k = 0 ; i < height ; i++ ) {
		for( j = 0 ; j < width ; j++, k++ ) {
			img2[k][0] = ( double )tpl_data[i * step + j];
			img2[k][1] = 0.0;
		}
	}
	
	
	/* obtain the FFT of img2 */
	fftw_execute( fft_img2 );
	
	/*obtain corelation*/
	CvPoint maxloc=phase_correlation(img1 ,img2,cvSize(width,height));

	/* deallocate FFTW arrays and plans */
	fftw_destroy_plan( fft_img2 );
	fftw_free( img2 );

	// print it 
	TEXT("Maxval at "<<maxloc.x<<" , "<<maxloc.y<<"\n");

	cvReleaseImage(&img_tpl);

	int ret;
	ret=((maxloc.x+(tpl->width/2))/tpl->width)-1;
	if(ret<0)ret=(ref->width/tpl->width)-1;

	if(max!=NULL){
		max->x=maxloc.x;
		max->y=maxloc.y;
	}
	return ret;
}



/*
 * get phase correlation from two images and save result to the third image
 */
int phase_correlation(const IplImage *ref, IplImage *tpl,CvPoint * max)
{
/*MEZISTUPNOVA FUNKCE - VYCHAZI Z ORIGINALU
vstup jsou kalsicke obrazky, ref muze mit vetsi sirku (metashape), pokud se tak stane zvetsi se i tpl
tato funkce prevede oba vstupni obrazky do FFT a zavola funkcy phase_corerelation ktera vypocte max ze vstupnich obrazku
*/
	if(ref->height!=tpl->height){
		TEXT("POZOR zadane nestejne rozmery obrazku ! \n");
		TEXT("KONCIM\n");
	}
	IplImage *img_tpl=cvCreateImage( cvSize(ref->width,ref->height ),ref->depth,ref->nChannels);

	cvSetZero(img_tpl);
//	img_rotate(tpl,180);
	if(img_copy_image_beside(tpl,img_tpl,0)!=0)cout<<"chyba pri kopirovaní obrazu \n!!";

	cvSaveImage("meta.png", ref);
	cvSaveImage("template.png", img_tpl);

	int 	i, j, k;
	
	/* get image properties */
	int width  	 = ref->width;
	int height   = ref->height;
	int step     = ref->widthStep;

	/* setup pointers to images */
	uchar 	*ref_data = ( uchar* ) ref->imageData;
	uchar 	*tpl_data = ( uchar* ) img_tpl->imageData;
	
	/* allocate FFTW input and output arrays */
	fftw_complex *img1 = ( fftw_complex* )fftw_malloc( sizeof( fftw_complex ) * width * height );
	fftw_complex *img2 = ( fftw_complex* )fftw_malloc( sizeof( fftw_complex ) * width * height );

	
	/* setup FFTW plans */
	fftw_plan fft_img1 = fftw_plan_dft_1d( width * height, img1, img1, FFTW_FORWARD,  FFTW_ESTIMATE );
	fftw_plan fft_img2 = fftw_plan_dft_1d( width * height, img2, img2, FFTW_FORWARD,  FFTW_ESTIMATE );

	
	/* load images' data to FFTW input */
	for( i = 0, k = 0 ; i < height ; i++ ) {
		for( j = 0 ; j < width ; j++, k++ ) {
			img1[k][0] = ( double )ref_data[i * step + j];
			img1[k][1] = 0.0;
			
			img2[k][0] = ( double )tpl_data[i * step + j];
			img2[k][1] = 0.0;
		}
	}
	
	/* obtain the FFT of img1 */
	fftw_execute( fft_img1 );
	
	/* obtain the FFT of img2 */
	fftw_execute( fft_img2 );
	
	/*obtain corelation*/
	CvPoint maxloc=phase_correlation(img1 ,img2,cvSize(width,height));

	/* deallocate FFTW arrays and plans */
	fftw_destroy_plan( fft_img1 );
	fftw_destroy_plan( fft_img2 );
	fftw_free( img1 );
	fftw_free( img2 );

	// print it 
	TEXT("Maxval at "<<maxloc.x<<" , "<<maxloc.y<<"\n");

	cvReleaseImage(&img_tpl);

	int ret;
	ret=((maxloc.x+(tpl->width/2))/tpl->width)-1;
	if(ret<0)ret=(ref->width/tpl->width)-1;

	if(max!=NULL){
		max->x=maxloc.x;
		max->y=maxloc.y;
	}
	return ret;
}



void img_rotate(IplImage * img,double angle){
	img_rotate(img,angle, cvPoint(img->width/2,img->height/2));
}

void img_rotate(IplImage * img,double angle,CvPoint center){
CvPoint2D32f centr;
CvMat *mapMatrix = cvCreateMat( 2, 3, CV_32FC1 );
IplImage * dest= cvCreateImage(cvSize(img->width,img->height), IPL_DEPTH_8U,img->nChannels);
centr.x=center.x;
centr.y=center.y;


cv2DRotationMatrix(centr,angle,1.0, mapMatrix);
cvWarpAffine( img,dest,mapMatrix);

cvCopy(dest, img);

cvReleaseMat(&mapMatrix);
cvReleaseImage(&dest);
}




IplImage * img_create_meta_shape(const IplImage * img,CvPoint center,CvSize size,float angle,float delta_angle){//fce alokuje pamet

if(size.width<=0||size.height<=0){cout<<"SPATNE ROZMERY size\n";return NULL;}
if(delta_angle<=0){cout<<"CHYBA!! delta nemuze byt zaporna nebo rovna nule!\n";return NULL;}

int image_count=(2*(angle/delta_angle))+1;
IplImage *meta=cvCreateImage(cvSize(size.width*image_count,size.height),IPL_DEPTH_8U,img->nChannels);
IplImage *img_pom=cvCreateImage(cvSize(img->width,img->height),IPL_DEPTH_8U,img->nChannels);//abych nezprznil vstupni obrazek
IplImage *img_window=cvCreateImage(size,IPL_DEPTH_8U,img->nChannels);
CvRect rect=cvRect(center.x-(size.width/2),center.y-(size.height/2),size.width,size.height);

cvZero(meta);

TEXT("pocetu uhlu bude "<< image_count << "\n");

float act_angle=(-1*angle);
for (int i=0;i<image_count;i++,act_angle+=delta_angle){//otacej a kopiruj obrazy do meta sablony 
	TEXT("uhel c. "<< i << " =>"<< act_angle<<"\n");

	cvCopy(img,img_pom);	
	img_rotate(img_pom,act_angle,center);

	cvSetImageROI(img_pom,rect);
	cvCopy(img_pom,img_window);
	cvResetImageROI(img_pom);

	if(img_copy_image_beside(img_window,meta,i)!=0){cout<<"chyba pri kopirovani metasablony\n";break;}
}


cvReleaseImage(&img_window);
cvReleaseImage(&img_pom);
return meta;
}






int img_copy_image_beside(const IplImage * img_src,IplImage * img_big,float start){
/*KOMENTAR
img_src - vstupni obraz ktery se nakopiruje do img_big
img_big - velky obraz do ktereho je kopirovano
start - pocet sirky obrazu img_src od ktereho se bude zacinat kopirovat do img_big, kopirovat budu od  ( start*img_src->width)

fce vraci -1 pokud jsou zadany spatne rozmery (img_src se nevleze do img_big)
*/
CvRect rect=cvRect(start*img_src->width,0,img_src->width,img_src->height);


//osetreni mezi
if(img_src->height>img_big->height){cout << "Delka vstupniho obrazu je vetsi nez vystupniho!!\nKONEC.\n";;return -1;}
if(rect.x+img_src->width>img_big->width){cout << "Sirka vstupniho obrazu je vetsi nez vystupniho!!\nKONEC.\n";;return -1;}

cvSetImageROI(img_big,rect);
cvCopy(img_src,img_big);
cvResetImageROI(img_big);

return 0;
}





IplImage * Create_Shape(const IplImage * img,CvPoint center,CvSize size,CvPoint shift,float angle){//!alokuje novou pamet
/*
Funkce vytvori vyrez ze vstupniho obrazu. Vyrez muze byt otocen a nasledne i posunut.
Stred otaceni je "center", obraz se nejdrive otoci a potom posune, pokud je zadano
img 	-	vstupni veliky obraz (vyfoceny)
center 	-	bod prostredku vyrezu
size 	-	velikost vyrezu
shift 	-	posun vyrezu(default 0)
angle	-	otoceni vyrezu (default 0)
*/
	if(size.width<=0||size.height<=0){cout<<"SPATNE ROZMERY size\n";return NULL;}
	IplImage *img_pom=cvCreateImage(cvSize(img->width,img->height),IPL_DEPTH_8U,img->nChannels);//abych nezprznil vstupni obrazek(otacenim)

	cvCopy(img,img_pom);

	if(angle!=0){
		TEXT("sablona bude otocena o "<<angle<<"\n");
		img_rotate(img_pom,angle);
	}
	if(shift.x!=0 || shift.y!=0){
		TEXT("sablona bude posunuta o "<<shift.x <<"x"<<shift.y<<"\n");
		center.x+=shift.x;
		center.y+=shift.y;
	}


	CvRect rect=cvRect(center.x-(size.width/2),center.y-(size.height/2),size.width,size.height);


//kontrola mezi obrazu:
if(img_pom->height<rect.y+rect.height){cout << "Delka vstupniho obrazu je vetsi nez vystupniho!!\nKONEC.\n";;return NULL;}
if(img_pom->width <rect.x+rect.width){cout << "Sirka vstupniho obrazu je vetsi nez vystupniho!!\nKONEC.\n";;return NULL;}


	IplImage *shape=cvCreateImage(size,IPL_DEPTH_8U,img->nChannels);

	cvSetImageROI(img_pom,rect);
	cvCopy(img_pom,shape);
	cvResetImageROI(img_pom);

	cvReleaseImage(&img_pom);

return shape;
}








IplImage* Create_Shape(const IplImage*img_ref,CvSize window,CvPoint* shift){//!!pozor funkce alokuje pamet
//tato funkce vola funkci call back, uzivatel tlacitkem vyberestred sablony
//pokud je zadan shif pak do nej ulozi pozici stredu ROI !! ->absolutni hodnota x a y souradnice!!!!!!!

		IplImage* img_pom=cvCreateImage(cvSize(img_ref->width,img_ref->height),img_ref->depth,img_ref->nChannels);
		CALL_BACK_STRUCT call_back_variable;

		call_back_variable.orig=img_ref;
		call_back_variable.size=window;
		call_back_variable.img=img_pom;

		cvShowImage( "call back", img_ref );
		cvSetMouseCallback( "call back", my_mouse_callback, (void*) &call_back_variable);
		cvWaitKey();
		cvDestroyWindow("call back");

		IplImage* img_ret=cvCreateImage(window,img_ref->depth,img_ref->nChannels);

		cvCopy(img_pom,img_ret);

		cvReleaseImage(&img_pom);
		if(shift!=NULL){
			shift->x=call_back_variable.center_ret.x;
			shift->y=call_back_variable.center_ret.y;
		}
	return img_ret;
}

void my_mouse_callback( int event, int x, int y, int flags, void* param ){
	CALL_BACK_STRUCT* var = (CALL_BACK_STRUCT*) param;
	CvScalar bod;
	CvPoint p1=cvPoint(x-(var->size.width/2),y-(var->size.height/2));
	CvPoint p2=cvPoint(x+(var->size.width/2),y+(var->size.height/2));
	switch( event ){
		case CV_EVENT_MOUSEMOVE: 
			break;
		case CV_EVENT_LBUTTONDOWN:
			cvResetImageROI(var->img);
			cvCopy(var->orig,var->img);
			if(p1.x<0||p2.x>var->img->width || p1.y<0||p2.y>var->img->height)cout<<"POZOR ROZMERY OKNA MIMO ROZSAH!!";
			cvRectangle(var->img,p1,p2,cvScalar(0,0,0));
			cvShowImage( "call back", var->img);
			bod=cvGet2D(var->img,y,x);
			cvSetImageROI(var->img,cvRect(p1.x,p1.y,var->size.width,var->size.height));
			cout<<"souradnice stredu jsou "<<x<<"x"<<y<<"->"<<bod.val[0]<<"\n";
			var->center_ret.x=x;
			var->center_ret.y=y;
		break;
	case CV_EVENT_LBUTTONUP:

			break;
	}
}




void view_result(const IplImage *img_ref,const IplImage * img_shape,CvPoint shift,float angle){
	IplImage* img_view=cvCreateImage(cvSize(img_ref->width,img_ref->height),img_ref->depth,3);
	cvSetZero(img_view);

//kopiruju ref
	for (int i = 0,k=0; i < img_ref->width; i++,k+=3 )
	 for (int j = 0,l=0; j < img_ref->height; j++){
	  ((uchar*)(img_view->imageData +img_view->widthStep*j))[k] = ((uchar*)(img_ref->imageData + img_ref->widthStep*j))[i];
	  ((uchar*)(img_view->imageData +img_view->widthStep*j))[k+2] = ((uchar*)(img_ref->imageData + img_ref->widthStep*j))[i];
	}

//kopiruju shape
	//orotuju shape:
	img_rotate(img_view,angle);

	shift.x=shift.x-(img_shape->width/2);
	shift.y=shift.y-(img_shape->height/2);
	for (int i = 0,k=(shift.x*3)+1; i < img_shape->width; i++,k+=3 )
	 for (int j = 0,l=shift.y; j < img_shape->height; j++,l++)
	  ((uchar*)(img_view->imageData +img_view->widthStep*l))[k] = ((uchar*)(img_shape->imageData + img_shape->widthStep*j))[i];


	cvShowImage( "view result", img_view );
	cvWaitKey(40);
	//cvDestroyWindow("view result");

	cvReleaseImage(&img_view);
}










