#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

}

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;
	}

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

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

	cvReleaseImage(&poc);
	return maxloc;
}


int phase_correlation(IplImage *ref,const fftw_complex *img1 , IplImage *tpl,CvPoint * max){
	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);
	IplImage *poc=cvCreateImage( cvSize(ref->width,ref->height ), IPL_DEPTH_64F,ref->nChannels);

	cvSetZero(img_tpl);


	img_copy_image_beside(tpl,img_tpl,0);

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

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

	/* setup pointers to images */
	uchar 	*tpl_data = ( uchar* ) img_tpl->imageData;
	double 	*poc_data = ( double* )poc->imageData;
	
	/* allocate FFTW input and output arrays */
	fftw_complex *img2 = ( fftw_complex* )fftw_malloc( sizeof( fftw_complex ) * width * height );
	fftw_complex *res  = ( 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 );
	fftw_plan ifft_res = fftw_plan_dft_1d( width * height, res,  res,  FFTW_BACKWARD, 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 the cross power spectrum */
	for( 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( i = 0 ; i < fft_size ; i++ ) {
        poc_data[i] = res[i][0] / ( double )fft_size;
	}

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


	// find the maximum value and its location 
	CvPoint minloc, maxloc;
	double  minval, maxval;
	cvMinMaxLoc( poc, &minval, &maxval, &minloc, &maxloc, 0 );
	if(max!=NULL){
		max->x=maxloc.x;
		max->y=maxloc.y;
	}
	// print it 
	TEXT("Maxval at "<<maxloc.x<<" , "<<maxloc.y<<" = "<<maxval<<"\n");
	TEXT("Minval at "<<minloc.x<<" , "<<minloc.y<<" = "<<minval<<"\n");

	cvReleaseImage(&poc);
	cvReleaseImage(&img_tpl);


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

	
	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)
{
	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);
	IplImage *poc=cvCreateImage( cvSize(ref->width,ref->height ), IPL_DEPTH_64F,ref->nChannels);

	cvSetZero(img_tpl);

/*	CvRect rect=cvRect(0,0,tpl->width,tpl->height);
	cvSetImageROI(img_tpl,rect);
	cvCopy(tpl,img_tpl);
	cvResetImageROI(img_tpl);
*/
//	img_rotate(tpl,180);
	img_copy_image_beside(tpl,img_tpl,0);
/*
cvShowImage( "obraz je", img_tpl);
cvWaitKey( 0 );
cvShowImage( "obraz je", ref);
cvWaitKey( 0 );
*/
	cvSaveImage("meta.png", ref);
	cvSaveImage("template.png", img_tpl);

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

	/* setup pointers to images */
	uchar 	*ref_data = ( uchar* ) ref->imageData;
	uchar 	*tpl_data = ( uchar* ) img_tpl->imageData;
	double 	*poc_data = ( double* )poc->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 );
	fftw_complex *res  = ( 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 );
	fftw_plan ifft_res = fftw_plan_dft_1d( width * height, res,  res,  FFTW_BACKWARD, 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 the cross power spectrum */
	for( 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( i = 0 ; i < fft_size ; i++ ) {
        poc_data[i] = res[i][0] / ( double )fft_size;
	}


CvPoint zk=phase_correlation(img1 ,img2,cvSize(width,height));
cout<<"zkouska MAX at "<<zk.x<<" , "<<zk.y<<"\n";

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


	// find the maximum value and its location 
	CvPoint minloc, maxloc;
	double  minval, maxval;
	cvMinMaxLoc( poc, &minval, &maxval, &minloc, &maxloc, 0 );
	if(max!=NULL){
		max->x=maxloc.x;
		max->y=maxloc.y;
	}
	// print it 
	cout<<"Maxval at "<<maxloc.x<<" , "<<maxloc.y<<" = "<<maxval<<"\n";
	TEXT("Maxval at "<<maxloc.x<<" , "<<maxloc.y<<" = "<<maxval<<"\n");
	TEXT("Minval at "<<minloc.x<<" , "<<minloc.y<<" = "<<minval<<"\n");

	cvReleaseImage(&poc);
	cvReleaseImage(&img_tpl);


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

	
	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){

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";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;
}





