#include <stdio.h>
#include <string.h>
#include "cv.h"
#include "cvaux.h"
#include "highgui.h"

IplImage ** faceImgArr		= 0; // array of face images
IplImage * pAvgTrainImg		= 0;
int nTrainFaces 			= 0;
char win_name[20];

int loadFaceImgArray(char * filename)
{
	FILE * imgListFile = 0;
	char imgFilename[512];
	int iFace, nFaces=0;


	// open the input file
	if( !(imgListFile = fopen(filename, "r")) )
	{
		fprintf(stderr, "Can\'t open file %s\n", filename);
		return 0;
	}

	// count the number of faces
	while( fgets(imgFilename, 512, imgListFile) ) ++nFaces;
	rewind(imgListFile);

	// allocate the face-image array and person number matrix
	faceImgArr        = (IplImage **)cvAlloc( nFaces*sizeof(IplImage *) );
	
	// store the face images in an array
	for(iFace=0; iFace<nFaces; iFace++)
	{
		int aux = 0;
		// read person number and name of image file
		fscanf(imgListFile, "%d %s", &aux, imgFilename);
		// load the face image
		faceImgArr[iFace] = cvLoadImage(imgFilename, CV_LOAD_IMAGE_GRAYSCALE);

		if( !faceImgArr[iFace] )
		{
			fprintf(stderr, "Can\'t load image from %s\n", imgFilename);
			return 0;
		}
	}

	fclose(imgListFile);

	return nFaces;
}

void calcMeanImage(int nrFaces, IplImage** imgArray, int faceStep, 
					CvSize size, float *avg, int avgStep) 
{
	int i,j,k;
	float m = 1.0f / (float) nrFaces;
	float* bf = avg;
	
	for( i = 0; i < size.height; i++, bf += avgStep)
		for( j = 0; j < size.width; j++ )
			bf[j] = 0.f;
	
	for( i = 0; i < nrFaces; i++ )
	{
		uchar* bu    = (uchar *)imgArray[i]->imageData;
		bf  = avg;
		for( k = 0; k < size.height; k++, bf += avgStep, bu += faceStep )
			for( j = 0; j < size.width; j++ )
				bf[j] += bu[j];
	}
	
	bf = avg;
	for( i = 0; i < size.height; i++, bf += avgStep)
		for( j = 0; j < size.width; j++ ) {
			bf[j] *= m;
		}
}

void calcCovarMatrix(int nrFaces, IplImage** imgArray, int faceStep, 
				CvSize size, float *avg, int avgStep, float *covarMatrix) 
{
	int i,j,k,l;
	
	for( i = 0; i < nrFaces; i++ )
        {
            uchar *bu = (uchar *)imgArray[i]->imageData;

            for( j = i; j < nrFaces; j++ )
            {
                int k, l;
                float w = 0.f;
                float *a = avg;
                uchar *bu1 = bu;
                uchar *bu2 = (uchar *)imgArray[j]->imageData;

                for( k = 0; k < size.height;
                     k++, bu1 += faceStep, bu2 += faceStep, a += avgStep )
                {
                    for( l = 0; l < size.width - 3; l += 4 )
                    {
                        float f = a[l];
                        uchar u1 = bu1[l];
                        uchar u2 = bu2[l];

                        w += (u1 - f) * (u2 - f);
                        f = a[l + 1];
                        u1 = bu1[l + 1];
                        u2 = bu2[l + 1];
                        w += (u1 - f) * (u2 - f);
                        f = a[l + 2];
                        u1 = bu1[l + 2];
                        u2 = bu2[l + 2];
                        w += (u1 - f) * (u2 - f);
                        f = a[l + 3];
                        u1 = bu1[l + 3];
                        u2 = bu2[l + 3];
                        w += (u1 - f) * (u2 - f);
                    }
                    for( ; l < size.width; l++ )
                    {
                        float f = a[l];
                        uchar u1 = bu1[l];
                        uchar u2 = bu2[l];

                        w += (u1 - f) * (u2 - f);
                    }
                }

                covarMatrix[i * nrFaces + j] = covarMatrix[j * nrFaces + i] = w;
            }
        }
}

int main(int argc, char *argv[]) {
	int faceImgStep, avgStep;
	CvSize faceImgSize;
	float *covarMat;
	float *eigVect;
	float *eigValue;
	
	// load training data
	nTrainFaces = loadFaceImgArray("train.txt");
	if( nTrainFaces < 2 )
	{
		fprintf(stderr,
		        "Need 2 or more training faces\n"
		        "Input file contains only %d\n", nTrainFaces);
		return;
	}
	
	faceImgSize.width  = faceImgArr[0]->width;
	faceImgSize.height = faceImgArr[0]->height;
	faceImgStep = faceImgArr[0]->widthStep/sizeof(uchar);
	pAvgTrainImg = cvCreateImage(faceImgSize, IPL_DEPTH_32F, 1);
	avgStep = pAvgTrainImg->widthStep/sizeof(float);
	
	calcMeanImage( nTrainFaces, 
				faceImgArr, 
				faceImgStep, 
				faceImgSize, 
				(float *)pAvgTrainImg->imageData, 
				avgStep );
	
	covarMat = (float *) cvAlloc( sizeof( float ) * nTrainFaces * nTrainFaces );
	
	calcCovarMatrix( nTrainFaces, 
				faceImgArr, 
				faceImgStep, 
				faceImgSize, 
				(float *)pAvgTrainImg->imageData, 
				avgStep,
				covarMat );
	
	eigVect = (float *) cvAlloc( sizeof( float ) * nTrainFaces * nTrainFaces);
	eigVals = (float *) cvAlloc( sizeof( float ) * nTrainFaces );
	
	// jacobi call
	
	
	//~ /* Eigen objects number determination */
	//~ if( calcLimit->type != CV_TERMCRIT_NUMBER )
	//~ {
		//~ for( i = 0; i < m1; i++ )
			//~ if( fabs( eigVals[i] / eigVals[0] ) < calcLimit->epsilon )
				//~ break;
		//~ m1 = calcLimit->max_iter = i;
	//~ }
	//~ else
		//~ m1 = calcLimit->max_iter;
	//~ calcLimit->epsilon = (float) fabs( eigVals[m1 - 1] / eigVals[0] );

	//~ for( i = 0; i < m1; i++ )
		//~ eigVals[i] = (float) (1.0 / sqrt( (double)eigVals[i] ));
	
	//~ for(i=0; i<nTrainFaces; i++)
	//~ {	
		//~ sprintf(win_name,"win %d", i);
		//~ cvNamedWindow(win_name, CV_WINDOW_AUTOSIZE); 
		//~ cvMoveWindow(win_name, i*100, i*100); // offset from the UL corner of the screen
		//~ cvShowImage(win_name,faceImgArr[i]);
	//~ }
	sprintf(win_name,"win1");
	cvNamedWindow(win_name, CV_WINDOW_AUTOSIZE); 
	cvShowImage(win_name,pAvgTrainImg);
	while(1) {
		if( (char)27==cvWaitKey(20) ) break;
	}
	cvDestroyWindow(win_name);
	
	//~ for(i=0; i<nTrainFaces; i++)
	//~ {	
		//~ sprintf(win_name,"win %d", i);
		//~ cvDestroyWindow(win_name);
	//~ }
	
	return 0;
}