#include "korelace.h"



korelace::korelace(my_image* image)
{
    img=NULL;
    img_ref=NULL;
    fftw_ref=NULL;
    fftw=NULL;

    this->location.x=0;
    this->location.y=0;

    img_ref=new my_image(image);
    fftw_ref=img2ffw(img_ref);
}

korelace::~korelace(){
    if(img_ref!=NULL)
        delete img_ref;
    if(img!=NULL)
        delete img;
    if(fftw_ref!=NULL)
        fftw_free(fftw_ref);
    if(fftw!=NULL)
        fftw_free(fftw);
}


void korelace::find(my_image* image){
    if(img!=NULL)
        delete img;
    if(fftw!=NULL)
        delete fftw;

    BOD f;
    f.x=img_ref->Width();
    f.y=img_ref->Height();

    img=new my_image(image);
    fftw=img2ffw(img,f);


    //this->stejne(x,y,f);

    //BOD loc= this->phase_correlation(x,fftw,f);
    this->location= this->phase_correlation(fftw_ref,fftw,f);
    location.x=f.x-location.x;
    location.y=f.y-location.y;

    //prepocteni na stred sablony
    location.x=location.x+(img->Width()/2);
    location.y=location.y+(img->Height()/2);

    //std::cout<<"nalezl jsem maximum na souradnicich  X="<<location.x<<" Y="<<location.y<<"\n"<<std::endl;

}



fftw_complex*korelace::my(const IplImage *img,BOD size){
       fftw_complex *in, *out;
       fftw_plan p;

      int N=size.x*size.y;

       in = (fftw_complex*) fftw_malloc(sizeof(fftw_complex) * N);
       out = (fftw_complex*) fftw_malloc(sizeof(fftw_complex) * N);

       IplImage *poc=cvCreateImage(cvSize(size.x,size.y), IPL_DEPTH_8U,1);//!!!!!!!!!!!!!!!!!
       uchar 	*poc_data = ( uchar* )poc->imageData;


       for(int j=0;j<size.y;j++)
           for(int i=0;i<size.x;i++){
           if(j<img->height && i<img->width){
               CvScalar s=cvGet2D(img,j,i);
               in[i+j*size.x][0]=s.val[0];
               poc_data[i+j*size.x]=s.val[0];
           }else{
               in[i+j*size.x][0]=0.0;
               poc_data[i+j*size.x]=0;
           }

               in[i+j*size.x][1]=0.0;
        }


       /*cvShowImage("one",poc);
       cvWaitKey(0);//*/
       cvReleaseImage(&poc);


       p = fftw_plan_dft_1d(N, in, out, FFTW_FORWARD, FFTW_ESTIMATE);

       fftw_execute(p); /* repeat as needed */


       fftw_destroy_plan(p);
       fftw_free(in);
       return out;
}




fftw_complex* korelace::img2ffw(my_image *img){//funkce alokuje pamet!
    BOD size;
    size.x=img->Width();
    size.y=img->Height();
    return this->img2ffw(img,size);

}


fftw_complex* korelace::img2ffw(my_image *img,BOD size){//funkce alokuje pamet!

       fftw_complex *in, *out;
       fftw_plan p;

      int N=size.x*size.y;
      int width=img->Width();
      int height=img->Height();

       in = (fftw_complex*) fftw_malloc(sizeof(fftw_complex) * N);
       out = (fftw_complex*) fftw_malloc(sizeof(fftw_complex) * N);


       for(int j=0;j<size.y;j++)
           for(int i=0;i<size.x;i++){
           if(j<height && i<width){
               in[i+j*size.x][0]=img->get(i,j);
           }else{
               in[i+j*size.x][0]=0.0;
           }

               in[i+j*size.x][1]=0.0;
        }

       p = fftw_plan_dft_1d(N, in, out, FFTW_FORWARD, FFTW_ESTIMATE);
       fftw_execute(p); /* repeat as needed */

       fftw_destroy_plan(p);
       fftw_free(in);
       return out;

}



void korelace::ffw2show(fftw_complex * plane,BOD size){//nefunkcni


    int fft_size = size.x * size.y;

    IplImage *poc=cvCreateImage(cvSize(size.x,size.y), IPL_DEPTH_64F,1);//!!!!!!!!!!!!!!!!!

    double 	*poc_data = ( double* )poc->imageData;

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

    fftw_plan ifft_res = fftw_plan_dft_1d( size.x * size.y, res,  res,  FFTW_BACKWARD, FFTW_ESTIMATE );

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

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

    cvShowImage("zpetna", poc);
    cvWaitKey(0);

}


BOD korelace::phase_correlation(fftw_complex *img1 ,fftw_complex *img2,BOD 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.x * size.y;

        IplImage *poc=cvCreateImage(cvSize(size.x,size.y), IPL_DEPTH_64F,1);//!!!!!!!!!!!!!!!!!

        double 	*poc_data = ( double* )poc->imageData;
        double	tmp;

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

        fftw_plan ifft_res = fftw_plan_dft_1d( size.x * size.y, 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)std::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);

        BOD ret;
        ret.x=maxloc.x;
        ret.y=maxloc.y;

        return ret;
}








void korelace::stejne(my_image *img1, my_image *img2){
    if(img1->Width()!=img2->Width()){
        std::cout<<"RUZNE !!!!!!!!!!!!!!!!!!!!!!\n!!!!!!!!!!!!!!!!!!!!!\n!!!!!!!!!!!!!!!!!!\n";
        return;
    }
    if(img1->Height()!=img2->Height()){
        std::cout<<"RUZNE !!!!!!!!!!!!!!!!!!!!!!\n!!!!!!!!!!!!!!!!!!!!!\n!!!!!!!!!!!!!!!!!!\n";
        return;
    }

    for(int j=0;j<img1->Height();j++)
        for(int i=0;i<img1->Width();i++){
        if(img1->get(i,j)!=img2->get(i,j)){
            std::cout<<"RUZNE v hodnote!!!!!!!!!!!!!!!!!!!!!!\n!!!!!!!!!!!!!!!!!!!!!\n!!!!!!!!!!!!!!!!!!\n";
            return;
        }
    }
    std::cout<<"Vlozene obrazy jsou stejne \n";
}

void korelace::stejne(fftw_complex *img1, fftw_complex *img2,BOD size){

    for(int j=0;j<size.y*size.x;j++){
        if(img1[j][0]!=img2[j][0]){
            //std::cout<<"RUZNE v hodnote index "<<j<<" hodnoty: "<<std::setprecision(10)<<img1[j][0]<<" a "<< img2[j][0]<<"!!!!!!!!!!!!!!!!!!!!!!\n!!!!!!!!!!!!!!!!!!!!!\n!!!!!!!!!!!!!!!!!!\n";
            return;
        }
        if(img1[j][1]!=img2[j][1]){

            std::cout<<"RUZNE v hodnote index "<<j<<" hodnoty: " <<img1[j][1]<<" a "<< img2[j][1]<<"!!!!!!!!!!!!!!!!!!!!!!\n!!!!!!!!!!!!!!!!!!!!!\n!!!!!!!!!!!!!!!!!!\n";
            return;
        }
    }
    std::cout<<"Vlozene fftw jsou stejne \n";




}
