// Normalized 2D Cross Correlation (NCC) C program: input: Color & Grayscale .png images!
// Program developed by Arturo Deza.
// Last update: 10/11/10

//1st input: image that should be analyzed;
//2nd input: image template that we want to find;
//Works best with .png image types.
//Permission to use, copy and modify this program is granted for commercial
//and Research purposes.
//In case you would like to contact the author/developer please email me at:
// mdezaf [at] uni.pe

#include "highgui.h"
#include "cv.h"

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <assert.h>
#include "math.h"
#include <float.h>
#include <limits.h>
#include <time.h>
#include <ctype.h>

int const MAX_X=600;
int const MAX_Y=600;
int const MAX_Z=360000;

void NCC1(IplImage I,IplImage T ,long double p[MAX_Z],int phase_v[2]);
long double vector_max(int height, int width, long double p[MAX_Z]);
void show_tops(long double tops);
void show_Pvalues(int width, int height,long double p[MAX_Z]);
void ComputeZ_map(IplImage Z,int phaseX,int phaseY,long double p[MAX_Z],long double tops, long double limit,int widthR,int heightR);
void phase_vector(IplImage T,int phase_v[2]);
void purge_pvector(IplImage I, IplImage T, long double t, long double p[MAX_Z]);
long double conv_int(int value);
//2D STRUCTURE: [MAX_Y][MAX_X];

int main(int argc, char **argv)
{

printf("Program Initialization!\n");

IplImage *Image = cvLoadImage(argv[1]);
IplImage *Template =cvLoadImage(argv[2]);
IplImage *ImageG=cvCreateImage(cvGetSize(Image),IPL_DEPTH_8U,1);
IplImage *TemplateG=cvCreateImage(cvGetSize(Template),IPL_DEPTH_8U,1);

cvCvtColor(Image,ImageG,CV_BGR2GRAY);
cvCvtColor(Template,TemplateG,CV_BGR2GRAY);

IplImage *ResultG =cvCreateImage(cvGetSize(ImageG),IPL_DEPTH_8U,1);


long double p1[MAX_Z];

long double tops;
long double limit;
int phase[2];//phase[0]=phaseY & phase[1]=phaseX;
int heightR,widthR;
widthR=ImageG->width-TemplateG->width+1;
heightR=ImageG->height-TemplateG->height+1;

//Initialiaze all p values to 0.

if ((Image == NULL)||(Template == NULL))
{
	puts("Unable to load the frame"); exit(0);//cout<<"error";
}

for (int i=1;i<=MAX_Y;i++)
	{for(int j=1;j<=MAX_X;j++)
		{//p1[i][j]=0;
		p1[(i-1)*(MAX_X)+j-1]=0;
		}
	}


printf("Image & Template loaded \n");
//----Grayscale Correlation 
cvNamedWindow("ImageG",CV_WINDOW_AUTOSIZE);
cvNamedWindow("TemplateG",CV_WINDOW_AUTOSIZE);
cvNamedWindow("ResultG",CV_WINDOW_AUTOSIZE);


//Primary Function:
phase_vector(*TemplateG,phase);
NCC1(*ImageG,*TemplateG,p1,phase);
tops=vector_max(heightR,widthR,p1);
//----- Threshold algorithm??
limit=tops*0.731;// This value can change, depending on tolerance levels.
//----- Threshold algorithm??
//----- Ideally it should be automated.
purge_pvector(*ImageG,*TemplateG,limit,p1);

//phase_vector(*TemplateG,phase);
show_Pvalues(widthR,heightR,p1);
show_tops(tops);
ComputeZ_map(*ResultG,phase[1],phase[0],p1,tops,limit,widthR,heightR);

cvShowImage("ImageG",ImageG);
cvShowImage("TemplateG",TemplateG);
cvShowImage("ResultG",ResultG);

cvSaveImage("NCC5G_Result.jpg",ResultG);

cvWaitKey(0);

cvReleaseImage(&ImageG);
cvReleaseImage(&TemplateG);
cvReleaseImage(&ResultG);

cvDestroyWindow("ImageG");
cvDestroyWindow("TemplateG");
cvDestroyWindow("ResultG");
printf("limit:%llf\n",limit);
//return (0);
}

void NCC1(IplImage I, IplImage T,long double p[MAX_Z],int phase_v[2])
{

//I Data Set
int a,b,i,j,k;
int heightI,widthI,stepI,channelI,heightT,widthT,stepT,channelT,widthZ,heightZ,phasey,phasex;
uchar *dataI, *dataT;

heightI=I.height;
widthI=I.width;
stepI=I.widthStep;
channelI=I.nChannels;
dataI=(uchar *) I.imageData;

//T Data Set
heightT=T.height;
widthT=T.width;
stepT=T.widthStep;
channelT=T.nChannels;
dataT=(uchar *) T.imageData; 


//Z frame dimensions for analysis.
widthZ=widthI-widthT+1;
heightZ=heightI-heightT+1;

phasey=phase_v[0];
phasex=phase_v[1];

//- Aux Data sets;
//datar= (uchar *) aux->imageData;
//channelr=aux->nChannels;
//stepr=aux->widthStep;

long double IS=0.0,IT=0.0;
long double P1=0.0,P2=0.0,P3=0.0;
/*
for (i=0;i<(heightI);i++)
	{for (j=0;j<(widthI);j++)
		{IS=IS+dataI[i*stepI+j*channelI];
		}
	}
*/
for(i=0;i<(heightT);i++)
	{for(j=0;j<(widthT);j++)
		{IT=IT+conv_int(dataT[i*stepT+j*channelT]);
		}
	}

//Remember IS and IT are mean values!
IT=IT/heightT/widthT;
//Process IS for every displacement

//Checked and compiled Correctly!!

printf("IS = %llf\n",IS);
printf("IT = %llf\n",IT);

for (a=1;a<=(heightZ);a++)
	{for (b=1;b<=(widthZ);b++)
		{
		//Reinitialize Parameters
		IS=0.0;
		P1=0.0;P2=0.0;P3=0.0;

		for (i=1;i<=heightT;i++)
			{for(j=1;j<=widthT;j++)
				{IS=IS+conv_int(dataI[(i+a-phasey-1)*stepI+(j+b-phasex-1)*channelI]);
				}
			}
				IS=IS/heightT/widthT;
				printf("IS = %llf\n",IS);

		for (i=0;i<(heightT);i++)
			{for (j=0;j<(widthT);j++)
				{P1=P1+conv_int((dataI[(a+i-1)*stepI+(b+j-1)*channelI]-IS)*(dataT[i*stepT+j*channelT]-IT));
				//printf("P1[%d][%d] done! \n",i,j);
				//printf("P1_dataI[%d][%d]=[%llf][%llf]\n",i,j,dataI[(a+i-1)*stepI+(b+j-1)*channelI],dataT[i*stepT+j*channelT]);
				}
			}
		for(i=0;i<(heightT);i++)
			{for(j=0;j<(widthT);j++)
				{P2=P2+conv_int(pow((dataI[(a+i-1)*stepI+(b+j-1)*channelI]-IS),2.0)); 
				//printf("P2[%d][%d] done! \n",i,j);
				//printf("P2_dataI[%d][%d]=[%llf]\n",i,j,dataI[(a+i-1)*stepI+(b+j-1)*channelI],dataT[i*stepT+j*channelT]);
				}
			}
		for(i=0;i<(heightT);i++)
			{for(j=0;j<(widthT);j++)
				{P3=P3+conv_int(pow((dataT[i*stepT+j*channelT]-IT),2.0)); 
				//printf("P3[%d][%d] done! \n",i,j);
				//printf("P3_dataI[%d][%d]=[%llf]\n",i,j,dataT[i*stepT+j*channelT]);
				}
			}
		printf("P1[%d][%d]=%llf\n",a,b,P1);
		printf("P2[%d][%d]=%llf\n",a,b,P2);
		printf("P3[%d][%d]=%llf\n",a,b,P3);		
		p[(a-1)*widthZ+b-1]=P1/(sqrt(P2*P3));
	
		printf("Parameter P[%d][%d] done ! : %llf\n",a,b,p[(a-1)*widthZ+b-1]);
		}
			
	}
printf("Parameters done!");
}

long double vector_max(int height, int width, long double (p[MAX_Z]))
{
long double max=0;
long double aux[MAX_Z];

for (int i=1;i<=height;i++)
	{for(int j=1;j<=width;j++)
		{
			aux[(i-1)*(width)+j-1]=p[(i-1)*(width)+j-1];			
		}	
		
	}
	for(int i=1;i<=height;i++)
		{for(int j=1;j<=width;j++)
			{if(aux[(i-1)*width+j-1]>max)
				{max=aux[(i-1)*width+j-1];
				}
			}
		}
return max;
}

void show_tops(long double tops)
{
printf("Tops = %llf\n",tops);
}

void show_Pvalues(int width, int height, long double p[MAX_Z])
{long double temp;
int i,j;
	for (i=height;i>=1;i--)
		{for(j=width;j>=1;j--)
			{temp=p[(i-1)*(width)+j-1];
			printf("p[%d][%d]=%llf \n",i,j,temp); 
			}
		}
		
}

void ComputeZ_map(IplImage Z,int phaseX,int phaseY,long double p[MAX_Z],long double tops, long double limit,int widthR,int heightR)
{
int i,j,k;
uchar *dataZ;
long double temp;
int heightZ=Z.height;
int widthZ=Z.width;
int stepZ=Z.widthStep;
int channelZ=Z.nChannels;
int cont=0;
//int DIM_Z=heightZ*widthZ;
int DIM_Z=widthR*heightR;
int contX=0;


dataZ=(uchar *) Z.imageData;

for(i=0;i<heightZ;i++)
	{for(j=0;j<widthZ;j++)
		{
		//If condition: Template is within phase shift boundaries and if it doesn't overcount "cont" 
			if(((i+1)>=phaseY)&&((j+1)>=phaseX)&&(cont<DIM_Z)&&(contX<widthR))
				{
					cont++;
					contX++;
					temp=p[(i-phaseY+1)*widthZ+j-phaseX+1];
					//printf("temp[%d][%d]=%llf",i+2-phaseY,j+2-phaseX,temp);
					if(temp!=0)					
						{
						dataZ[i*stepZ+j*channelZ]=255.0*((temp-limit)/(tops-limit));
						}
					else
						{dataZ[i*stepZ+j*channelZ]=0;
						}
				
				}
			else
				{dataZ[i*stepZ+j*channelZ]=0;
				}	
		}
	contX=0;
	}

}

void phase_vector(IplImage T,int phase_v[2])
{
int heightT=T.height;
int widthT=T.width;

if ((heightT+1)%2==0)
	{phase_v[0]=(heightT+1)/2;
	}
else
	{phase_v[0]=heightT/2;
	}
if ((widthT+1)%2==0)
	{phase_v[1]=(widthT+1)/2;
	}
else
	{phase_v[1]=widthT/2;
	}

printf("phaseY=%d\n",phase_v[0]);
printf("phaseX=%d\n",phase_v[1]);
}

void purge_pvector(IplImage I, IplImage T,long double t, long double p[MAX_Z])
{

int a,b,i,j,k;
int heightI,widthI,stepI,channelI,heightT,widthT,stepT,channelT,widthZ,heightZ;
uchar *dataI, *dataT;

heightI=I.height;
widthI=I.width;
stepI=I.widthStep;
channelI=I.nChannels;
dataI=(uchar *) I.imageData;

heightT=T.height;
widthT=T.width;
stepT=T.widthStep;
channelT=T.nChannels;
dataT=(uchar *) T.imageData; 

widthZ=widthI-widthT+1;
heightZ=heightI-heightT+1;

for(i=1;i<=heightZ;i++)
	{for(j=1;j<=widthZ;j++)
		{if((p[(i-1)*widthZ+j-1])<t)
			{p[(i-1)*widthZ+j-1]=0;
			}
		}
	}			
}

long double conv_int(int value)
{
return (value*100/255+100);
}