//#include "stdafx.h"

#ifdef _CH_
//#pragma package <opencv>
#endif

#ifndef _EiC
#include "cv.h"
#include "cvaux.h"
#include "highgui.h"
#endif

#include <stdio.h>
#include <string.h>
#include <assert.h>
#include <math.h>
#include <float.h>
#include <limits.h>
#include <ctype.h>
#include <errno.h>
#include <iomanip.h>

#include "ART.h"
#include "ImageDatabase.h"
#include "Histogram.h"
#include "skin2.h"

#define PI 3.141516
#define VnmWidth 7
#define VnmHeight 3

ART::ART(){
	originX=0;
	originY=0;
	N=7;
	M=3;
	radius=0;
	diameter=0;
	subwidth=0;
	subheight=0;
	R=NULL;
	ALPHA=NULL;
	CReal=NULL;
	CIm=NULL;
}

ART::~ART() {
	originX=0;
	originY=0;
	N=0;
	M=0;
	radius=0;
	for (int i=0; i<subwidth; i++) {
		delete [] R[i];
		delete [] ALPHA[i];
		delete [] CReal[i];
		delete [] CIm[i];
	}
	delete [] R;
	delete [] ALPHA;
	delete [] CReal;
	delete [] CIm;
}

void ART::artTransform(IplImage *whole1, IplImage *sub1){
	
	CvSize size1;
	size1=cvGetSize(sub1);
	subwidth=size1.width;
	subheight=size1.height;

	R=new double *[subwidth];
	CReal=new double *[subwidth];
	ALPHA=new double *[subwidth];
	CIm=new double *[subwidth];
	for (int i=0; i<subwidth; i++) {
		R[i]=new double[subheight];
		ALPHA[i]=new double[subheight];
		CReal[i]=new double[subheight];
		CIm[i]=new double[subheight];
	}

	for (i=0; i<subwidth; i++) {
		for (int j=0; j<subheight; j++) {
			R[i][j]=0;
			ALPHA[i][j]=0;
			CReal[i][j]=0;
			CIm[i][j]=0;
		}
	}

	pCoordinates(sub1, R, ALPHA, true);					//compute polar coordinates
	
	for (i=0; i<size1.height; i++) {		
		for (int j=0; j<size1.width; j++) {
			printf("%2.2f ", R[j][i]);
		}
		printf("\n");
	}

	printf("\n");

	for (i=0; i<size1.height; i++) {		
		for (int j=0; j<size1.width; j++) {
			printf("%2.2f ", ALPHA[j][i]);
		}
		printf("\n");
	}

	ComputeC(R, ALPHA, sub1, CReal, CIm);

	printf("Real values: \n");
	for (i=0; i<VnmWidth; i++) {		
		for (int j=0; j<VnmHeight; j++) {
			printf("%7.1f ",CReal[i][j]);
		}
		printf("\n");
	}

	printf("Imaginary values: \n");

	for (i=0; i<VnmWidth; i++) {		
		for (int j=0; j<VnmHeight; j++) {
			printf("%7.1f ",CIm[i][j]);
		}
		printf("\n");
	}

	double *realarr=MatrixArray(CReal);	//real array za uporedjivanje u minkovskim
	realarr=MinkPrep(realarr);			//cumulative sums for Minkovski
	double *Imarr=MinkPrep(MatrixArray(CIm));

	findMatch(whole1, realarr, Imarr);
	
	printf("gotov ArtTransform\n");
	delete [] realarr;
	delete [] Imarr;
	//exit();
}

void ART::pCoordinates(IplImage *M, double **r2, double **alpha2, bool isSub) {
	//printf("starting pCoordinates transform\n");
	
	CvSize size;
	size=cvGetSize(M);
	int x=(int)(size.width/2);
	int y=(int)(size.height/2);
	if (isSub) {
		originX=x;
		originY=y;
		printf("picture has size %i %i\n", size.width, size.height);
		printf("originX= %i originY= %i\n", originX, originY);
	}
	int rad=MIN(size.width, size.height)/2;
	diameter=MIN(size.width, size.height);
	if (isSub) {
		radius=rad;
		printf("rad = %i\n", rad);
	}
	for (int i=0; i<size.width; i++) {
		for (int j=0; j<size.height; j++) {

			int reali = i - x;	//relative coordinates to origin.
			int realj = -(j - y);

			double r1=sqrt(reali*reali + realj*realj);	//r value
			double rval=r1/rad;
			//rval=(floor(10000*rval))/10000;
			//printf("%f\n", rval);
			//cvWaitKey(0);

			r2[i][j]=rval;

			double alpha1=0;
			if (reali==0) {
				if (realj>0)
					alpha1=PI/2;
				if (realj<0)
					alpha1=(1.5*PI);
				if (realj==0)
					alpha1=0;
			} else {
				alpha1=atan2(realj,reali);
				if (realj<0)
					alpha1+=2*PI;
			}
			//alpha1=(floor(10000*alpha1))/10000;
			alpha2[i][j]=alpha1;	//alpha value
		}
	}
	//printf("end pCoordinates transform\n");
}

void ART::ComputeC(double **r1, double **alpha1, IplImage *input, double **CnmR, double **CnmI) {

	/*for (int z=0; z<subheight; z++) {		
		for (int j=0; j<subwidth; j++) {
			printf("%2.2f ", r1[j][z]);
		}
		printf("\n");
	}*/

	CvSize size;
	size=cvGetSize(input);
	IplImage *I;
	I = cvCreateImage( size, IPL_DEPTH_8U, 1);
	int dms = input->nChannels;
	//int dms = cvGetDims( input, 0 );
	if (dms>1)
		cvCvtColor( input, I, CV_BGR2GRAY);
	else
		I=cvCloneImage(input);

	double co=0;
	double si=0;
	double alpha=0;
	double Rnalpha=0;
	double Rna=0;
	double actsin=0;	//used to determine mod of m*alpha before sin function
	double malpha=0;
	double imdata=0;
	double rrr=0;
	for (int x=0; x<size.width; x++) {
		for (int y=0; y<size.height; y++) {
			rrr=r1[x][y];
			if (rrr<=1) {

				imdata=((uchar*)(I->imageData + y*I->widthStep))[x];
				alpha=alpha1[x][y];

				for (int n=0; n<N; n++) {		
					for (int m=0; m<M; m++) {
						malpha=m*alpha;
						co=cos(malpha);
						if (n==0)
							Rnalpha=1.0;
						else {
							Rna=n*PI*alpha;
							Rnalpha=2*cos(Rna);
						}
						
						//CnmR[n][m]+=(co*Rnalpha*rrr*imdata)/(2*PI);
						CnmR[n][m]+=(co*Rnalpha*rrr*imdata);
						malpha=(m+1)*alpha;
						actsin=floor((malpha)/PI);
						si=malpha-actsin*PI;
						if (si<0.001)
							si=0;
						si=sin(si);
						//CnmI[n][m]+=(si*Rnalpha*rrr*imdata)/(2*PI);
						CnmI[n][m]+=(si*Rnalpha*rrr*imdata);
					}
				}
			}
		}
	}
	cvReleaseImage(&I);
}

void ART::findMatch(IplImage *whole1, double *SubC, double *SubI){
	printf("finding match\n");
	CvSize size;
	size=cvGetSize(whole1);

	IplImage* r_plane = cvCreateImage( size, 8, 1 );
    IplImage* g_plane = cvCreateImage( size, 8, 1 );
    IplImage* b_plane = cvCreateImage( size, 8, 1 );
	cvCvtPixToPlane( whole1, b_plane, g_plane, r_plane, NULL );

	IplImage *I;		//temp comparison image	
	IplImage *BW;		//grayscale whole1
	BW = cvCreateImage(size, IPL_DEPTH_8U, 1);
	I = cvCreateImage( cvSize(diameter,diameter), IPL_DEPTH_8U, 1);	
	int dms = whole1->nChannels;
	if (dms>1)
		cvCvtColor( whole1, BW, CV_BGR2GRAY);
	else
		BW=cvCloneImage(whole1);

	double *temparray;	//temp comparison array used in Minkovski
	double *tempImarr;
	double **tempR    =new double *[diameter];
	double **tempAlpha=new double *[diameter];
	double **tempCReal=new double *[diameter];
	double **tempCIm  =new double *[diameter];
	for (int k=0; k<diameter; k++) {
		tempR[k]=new double[diameter];
		tempAlpha[k]=new double[diameter];
		tempCReal[k]=new double[diameter];
		tempCIm[k]=new double[diameter];
	}
	for (k=0; k<diameter; k++) {		//init to 0;
		for (int t=0; t<diameter; t++) {
			tempR[k][t]=0;
			tempAlpha[k][t]=0;
			tempCReal[k][t]=0;
			tempCIm[k][t]=0;
		}
	}

	pCoordinates(I, tempR, tempAlpha, false);
	int tempi=0;	//current i and j of best match
	int tempj=0;
	int Imi=0;		//current i and j of imaginary array comparison
	int Imj=0;
	double tempscore=0;	//current best score, the lower the better
	double tempIm=0;
	double tempMin=1000000;
	double MinIm=1000000;

	for (int i=0; i<size.width-diameter; i++) {
		for (int j=0; j<size.height-diameter; j++) {

			int r=((uchar*)(r_plane->imageData + r_plane->widthStep*(j+radius)))[i+radius];
			int g=((uchar*)(g_plane->imageData + g_plane->widthStep*(j+radius)))[i+radius];
			int b=((uchar*)(b_plane->imageData + b_plane->widthStep*(j+radius)))[i+radius];

			if (r+g+b>0) {

				for (int x=0; x<diameter; x++) {		//fill temp image I
					for (int y=0; y<diameter; y++) {
						((uchar*)(I->imageData + y*I->widthStep))[x]=
							((uchar*)(BW->imageData + (j+y)*BW->widthStep))[i+x];
					}
				}
				ComputeC(tempR, tempAlpha, I, tempCReal, tempCIm);

				temparray=MinkPrep(MatrixArray(tempCReal));
				tempscore=Minkovski(temparray, SubC, 2, 21);

				tempImarr=MinkPrep(MatrixArray(tempCIm));
				tempIm=Minkovski(tempImarr, SubI, 2, 21);

				/*printf("temparray	SubC\n");
				for (int q=0; q<30; q++) {
				printf("%7.1f	%7.1f\n", temparray[q], SubC[q]);
				}
				//cvWaitKey(0);
				printf("\n");*/

				if (tempscore<=tempMin) {
					tempMin=tempscore;
					tempi=i;
					tempj=j;
				}
				if (tempIm<=MinIm) {
					MinIm=tempIm;
					Imi=i;
					Imj=j;
				}

				for(int d=0; d<diameter; d++) {
					for (int s=0; s<diameter; s++) {
						tempCReal[d][s]=0;
						tempCIm[d][s]=0;
					}
				}
				delete [] temparray;
				delete [] tempImarr;
			}
		}
		//printf("10 i\n");
	}

	IplImage *temp;
	IplImage *tempI;
	temp=cvCreateImage(cvSize(whole1->width, whole1->height), IPL_DEPTH_8U, whole1->nChannels);
	tempI=cvCreateImage(cvSize(whole1->width, whole1->height), IPL_DEPTH_8U, whole1->nChannels);
	temp=drawCircle(whole1, tempR, tempi, tempj);
	tempI=drawCircle(whole1, tempR, Imi, Imj);
	cvNamedWindow("ART", 0);
	cvNamedWindow("ARTIM", 0);
	cvShowImage("ART", temp);
	cvShowImage("ARTIM", tempI);
	printf("minimal Minkovski dinstance for reals: %f\n", tempMin);
	printf("minimal Minkovski dinstance for ims  : %f\n", MinIm);
	cvWaitKey(0);

	cvDestroyWindow("ART");
	cvDestroyWindow("ARTIM");
	cvReleaseImage(&temp);
	cvReleaseImage(&tempI);
	cvReleaseImage(&r_plane);
	cvReleaseImage(&g_plane);
	cvReleaseImage(&b_plane);
	cvReleaseImage(&I);
	cvReleaseImage(&BW);

	for (i=0; i<diameter; i++) {
		delete [] tempR[i];
		delete [] tempAlpha[i];
		delete [] tempCReal[i];
		delete [] tempCIm[i];
	}
	delete [] tempR;
	delete [] tempAlpha;
	delete [] tempCReal;
	delete [] tempCIm;
}

double *ART::MatrixArray(double **mat) {

	int comp(const double *vp, const double *vq);
	double *ret=new double[N*M];
	
	int count=0;
	for (int i=0; i<N; i++) {
		for (int j=0; j<M; j++) {
			ret[count]=mat[i][j];
			count++;
		}
	}
	//iquicksort(ret, 0, N*M);
	return ret;
}

double *ART::MinkPrep(double *realarr) {
	double temp=0;
	double *tar=new double[N*M];
	for (int i=0; i<N*M; i++) {
		temp+=realarr[i];
		tar[i]=temp;
	}
	return tar;
}

double ART::Minkovski(double *ar1, double *ar2, int p, int length) {

	double sum=0;
	for (int i=0; i<=length; i++) {
		sum+=pow(ar1[i]-ar2[i], p);
	}
	double exp=(1.0)/(double)p;
	sum=pow(sum, exp);
	return sum;
}

IplImage *ART::drawCircle(IplImage *source, double **R, int x, int y) {

	CvSize size;
	size=cvGetSize(source);
	IplImage* ret;
	ret=cvCreateImage(size, IPL_DEPTH_8U, 3);
	IplImage* r_plane = cvCreateImage( size, 8, 1 );
    IplImage* g_plane = cvCreateImage( size, 8, 1 );
    IplImage* b_plane = cvCreateImage( size, 8, 1 );
	cvCvtPixToPlane( source, b_plane, g_plane, r_plane, NULL );

	for (int i=x; i<x+diameter; i++) {
		for (int j=y; j<y+diameter; j++) {
			if ((R[i-x][j-y]>=0.9)&&(R[i-x][j-y]<=1.07)) {	//if circumference
				((uchar*)(r_plane->imageData + r_plane->widthStep*j))[i]=255;
				((uchar*)(g_plane->imageData + g_plane->widthStep*j))[i]=255;
				((uchar*)(b_plane->imageData + b_plane->widthStep*j))[i]=255;
			}
		}
	}

	cvCvtPlaneToPix(b_plane, g_plane, r_plane, 0, ret);

	cvReleaseImage(&r_plane);
	cvReleaseImage(&g_plane);
	cvReleaseImage(&b_plane);
	return ret;
}


void ART::init() {
	printf("ART initialized\n");
}

void ART::exit() {
	originX=0;
	originY=0;
	N=0;
	M=0;
	radius=0;
	for (int i=0; i<subwidth; i++) {
		delete [] R[i];
		delete [] ALPHA[i];
		delete [] CReal[i];
		delete [] CIm[i];
	}
	delete [] R;
	delete [] ALPHA;
	delete [] CReal;
	delete [] CIm;

}

void iswap(double *p, double *q)
{
        double temp;

        temp  = *p;
        *p    = *q;
        *q    = temp;
}

/*
int isplit(double *a, int f, int l)
{
        double    x;
        int      u, sp;
        void  iswap(double *p, double *q);

        x  = a[f];
        sp = f;
        for (u = f + 1; u <= l; ++u){
                if( a[u] < x){
                        ++sp;
                        iswap(&a[u], &a[sp]);
                }
        }

        iswap(&a[f], &a[sp]);
        return(sp);
}
*/

void ART::iquicksort(double *a, int fst, int lst)
{
        int     splitpt;
        int      isplit(double *a, int f, int l);

        if (fst < lst) {
                splitpt = isplit(a, fst, lst);
                iquicksort(a, fst, splitpt - 1);
                iquicksort(a, splitpt +1, lst);
        }

}