/***
 **     libface - Library of face recognition and supporting algorithms
        Copyright (c) 2003 Stefan Farthofer

	This file is part of libface, which is

        free software; you can redistribute it and/or modify
        it under the terms of the GNU General Public License as published by
        the Free Software Foundation; either version 2 of the License, or
        (at your option) any later version.

        This program is distributed in the hope that it will be useful,
        but WITHOUT ANY WARRANTY; without even the implied warranty of
        MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
        GNU General Public License for more details.

        You should have received a copy of the GNU General Public License
        along with this program; if not, write to the Free Software
        Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

	For further information seek us at http://sourceforge.net/projects/openbio/
**	or write an email to dimitri.pissarenko@gmx.net or farthofer@chello.at.
***/

#include <stdio.h>
#include <malloc.h>
#include <memory.h>
#include <math.h>
#include <IL/IL.h>
#include "frbase.h"
#include "fr.h"
#include "recoalgo.h"
#include "eigenfaces.h"
#include "matrix.h"

/* functions used internally */
void _efSortIdx(float* v, int* idx, unsigned int nrImages);

/* this is used to construct the FRrecoAlgo struct */
FRrecoAlgo* _efGetAlgo(void) {
	FRrecoAlgo* algo;

	algo = (FRrecoAlgo*)malloc(sizeof(FRrecoAlgo));
	if (algo == NULL) return NULL;
	memset(algo, 0, sizeof(algo));

	algo->init = _efInit;
	algo->getName = _efGetName;
	algo->getType = _efGetType;

	algo->setupAlgoInstance = _efSetupAlgoInstance;
	algo->getParametersSize = _efGetParametersSize;
	algo->serializeParameters = _efSerializeParameters;
	algo->freeParameters = _efFreeParameters;
	algo->copyParameters = _efCopyParameters;

	algo->computeTrait = _efComputeTrait;
	algo->compareTraits = _efCompareTraits;
	algo->getTraitSize = _efGetTraitSize;
	algo->serializeTrait = _efSerializeTrait;
	algo->freeTrait = _efFreeTrait;

	algo->trainedDataToImages = _efTrainedDataToImages;
	algo->trainedDataToText = _efTrainedDataToText;
	algo->traitToImages = _efTraitToImages;
	algo->traitToText = _efTraitToText;
	return algo;
}

int _efInit(void) {
	return FR_OK;
}

const char* _efGetName(void) {
	return "Eigenfaces";
}

int _efGetType(void) {
	return FR_TYPE_EIGENFACES;
}

/* don't blame me for the goto's unless you have a better idea for the error handling */
/* maybe something like an allocation stack.... */
int _efSetupAlgoInstance(FRrecognitionParameters* gParms, FRalgoParam* algoParms, unsigned int nrAlgoParms, FRimage* images, unsigned int nrImages, BYTE** parms) {
	unsigned int nrFaces = 0, i, j, * idx;
	unsigned int pixels = gParms->width*gParms->height;
	float* matA, * matAt, * average, * matL, * vecValues, * matEvSmall, * temp;
	int err = FR_ERR_NOMEM;

	/* check if we got enough training images */
	if (nrImages < 2)
		return FR_ERR_INVALID_PARMS;

	/* process parameters */
	for (i=0; i < nrAlgoParms; i++) {
		switch(algoParms[i].id) {
			case EF_FACES:
				nrFaces = *(int*)algoParms[i].data;
				break;
			default:
				break;
		}
	}

	if (nrFaces > nrImages-1 || nrFaces < 1)
		nrFaces = nrImages-1;

	/* allocations */
	if (!(matA = (float*) malloc(sizeof(float)*pixels*nrImages))) goto LmatA;
	if (!(matAt = (float*) malloc(sizeof(float)*pixels*nrImages))) goto LmatAt;
	if (!(matL = (float*) malloc(sizeof(float)*nrImages*nrImages))) goto LmatL;
	if (!(vecValues = (float*) malloc(nrImages*sizeof(float)))) goto LvecValues;
	if (!(matEvSmall = (float*) malloc(nrImages*nrImages*sizeof(float)))) goto LmatEvSmall;
	if (!(idx = (unsigned int*) malloc(sizeof(unsigned int)*nrImages))) goto Lidx;
	if (!(*parms = (BYTE*) malloc(sizeof(int) + pixels*(nrFaces+1)*sizeof(float)))) goto Lparms;
	average = (float*)(*parms + sizeof(int));
	memset(average, 0, (size_t)pixels*sizeof(float));
	*(int*)*parms = nrFaces;

	/* generate matA - copy values and compute average, each column holds an image */
	for (i=0; i < pixels; i++) {
		for (j=0; j < nrImages; j++) {
			matA[j+i*nrImages] = images[j].imgdata[i];
			average[i] += images[j].imgdata[i];
		}
		average[i] /= nrImages;
	}

	/* generate matA - subtract average */
	for (i=0; i < pixels; i++) {
		for (j=0; j < nrImages; j++) {
			matA[j+i*nrImages] -= average[i];
		}
	}

	/* now calculate A^t and then L=A^t*A */
	_matTransposeD(matAt, matA, pixels, nrImages);
	_matMultiplyD(matL, matAt, matA, nrImages, pixels, nrImages);

	/* now calculate eigenvectors and values of L and sort */
	if (err =_matEigenSD(matL, nrImages, vecValues, matEvSmall)) goto Lcleanup;
	_matTransposeQ(matEvSmall, nrImages); /* we the want vectors in the rows */
	_efSortIdx(vecValues, idx, nrImages);



	/* and calculate the eigenvectors of C=A*A^t
	 * the i-the eigenvector is calculated such that 
	 * e_i = A*es_i 
	 * where es_i is the i-th eigenvector of the small matrix
	 */
	temp = (float*)(average + pixels);
	for (i=0; i < nrFaces; i++, temp+=pixels) {
		_matMultiplyD(temp, matA, matEvSmall+idx[i]*nrImages, pixels, nrImages, 1);
	}

	/* note: since the last operation that touched err must have set it to FR_OK to reach
	 *		 this point we don't have to set it now
	 */


	/* deallocate everything */
Lcleanup:
Lparms:
	free(idx);
Lidx:
	free(matEvSmall);
LmatEvSmall:
	free(vecValues);
LvecValues:
	free(matL);
LmatL:
	free(matAt);
LmatAt:
	free(matA);
LmatA:
	return err;
}

size_t _efGetParametersSize(FRrecognitionParameters* gParms, BYTE* parms) {
	int pixels = gParms->width*gParms->height;
	/* one int for number of eigenfaces, average picture, eigenfaces */
	return sizeof(int)+(pixels*(*((int*)parms))+pixels)*sizeof(float);
}

int _efSerializeParameters(FRrecognitionParameters* gParms, BYTE** parms, BYTE** mem, size_t maxsz, BYTE direction) {
	int nrFaces, pixels = gParms->width*gParms->height;

	/* sanity check */
	if (direction == FR_OUT && maxsz < _efGetParametersSize(gParms, *parms))
		return FR_ERR_ALLOCMORE;

	if (direction == FR_IN) { /* we need the size in advance, so we can allocate memory */
		nrFaces = **((int **)mem);
		*parms = (BYTE*) malloc(sizeof(int)+(nrFaces+1)*pixels*sizeof(float));
		if (*parms == NULL) return FR_ERR_NOMEM;
	} else {
		nrFaces = *((int*)*parms);
	}

	/* copy number of eigenfaces and float data */
	frCopyMem(mem, *parms, sizeof(int), direction);
	frCopyMem(mem, *parms, (nrFaces+1)*pixels*sizeof(float), direction);
	return FR_OK;
}


void _efFreeParameters(FRrecognitionParameters* gParms, BYTE** parms) {
	free(*parms);
	*parms = NULL;
}

int _efCopyParameters(FRrecognitionParameters* gParms, BYTE** parmsDst, BYTE *parmsSrc) {
	size_t sz;

	sz = _efGetParametersSize(gParms, parmsSrc);
	*parmsDst = (BYTE*) malloc(sz);
	if (*parmsDst == NULL) return FR_ERR_NOMEM;
	memcpy((void*)*parmsDst, (const void*)parmsSrc,sz);
	return FR_OK;
}

/* our traits are weights for eigenfaces, these are calculate with this formula:
 * w_i = ef_i * (image - average)
 * note that we don't have to transpose one of the vectors since the memory
 * layout of a row vector is the same as that of a column vector
 */
int _efComputeTrait(FRrecognitionParameters* gParms, BYTE* parms, FRimage* image, BYTE** trait) {
	float* img, * ef, * w;
	int err = FR_ERR_NOMEM, i;
	unsigned int pixels = gParms->width*gParms->height;

	if (!(img = (float*)malloc(sizeof(float)*pixels))) goto Limg;
	if (!(*trait = (BYTE*)malloc(sizeof(float)* *(int*)parms))) goto Ltrait;

	_matSubtractD(img, image->imgdata, (float*)(parms+sizeof(int)), pixels, 1);
	ef = (float*)(parms+sizeof(int)+pixels*sizeof(float));
	w = (float*)*trait;

	for (i=*(int*)parms-1; i >= 0; i--, w++, ef+=pixels) {
		_matMultiplyD(w, ef, img, 1, pixels, 1);
	}

	err = FR_OK;


Ltrait:
	free(img);
Limg:
	return err;
}

/* compares two weight sets, if the distance is less than a threshold, the range [0, threshold]
 * is normalized to [0,1], for distance we use 
 */
int _efCompareTraits(FRrecognitionParameters* gParms, BYTE* parms, BYTE* trait1, BYTE* trait2, float* probability) {
	int i;
	float f;

	*probability = 0;

	for(i=*(int*)parms-1; i >= 0; i--) {
		printf("\nFactor for ef%2d:  ", i);
		f = (((float*)trait1)[i] - ((float*)trait2)[i])*(((float*)trait1)[i] - ((float*)trait2)[i]);
		while (f > 1.0f) {
			printf("*");
			f = f/10;
		}
		*probability += (((float*)trait1)[i] - ((float*)trait2)[i])*(((float*)trait1)[i] - ((float*)trait2)[i]);
	}
	*probability = (float)sqrt(*probability);

	printf("\ndistance is %f\n", *probability);
	if (*probability < EF_DIST_TRESH) {
		*probability /= EF_DIST_TRESH;
		*probability -= 1;
		*probability *= -1;
	} else {
		*probability = 0.0f;
	}

    return FR_OK;
}

size_t _efGetTraitSize(FRrecognitionParameters* gParms, BYTE* parms, BYTE* trait) {
	return sizeof(float)*(*((int*)parms));
}

int _efSerializeTrait(FRrecognitionParameters* gParms, BYTE* parms, BYTE** trait, BYTE** mem, size_t maxsz, BYTE direction) {
	size_t sz;

	sz = sizeof(float) * *((int*)parms);
	if (direction == FR_IN) {
		*trait = (BYTE*) malloc(sz);
		if (*trait == NULL) return FR_ERR_NOMEM;
	} else if (sz < maxsz) return FR_ERR_ALLOCMORE;

	frCopyMem(mem, *trait, sz, direction);
	return FR_OK;
}

void _efFreeTrait(FRrecognitionParameters* gParms, BYTE* parms, BYTE** trait) {
	free(*trait);
	*trait = NULL;
}

int _efTrainedDataToImages(FRrecognitionParameters* gParms, BYTE* parms, FRimage** images, unsigned int* nrImages) {
	float min, max, * efdata, * temp;
	int pixels = gParms->width * gParms->height, i, j, nrEf, err = FR_ERR_NOMEM;

	nrEf = *(int*)parms;

	if (! (*images = (FRimage*) malloc(sizeof(FRimage)* (nrEf+1)))) return FR_ERR_NOMEM;

	/* the first image is our average image ... */
	if (!((**images).imgdata = malloc(sizeof(float)*pixels))) {
		free(*images);
		return FR_ERR_NOMEM;
	}
	memcpy((**images).imgdata, parms+sizeof(int), pixels*sizeof(float));
	(**images).height = gParms->height;
	(**images).width = gParms->width;


	efdata = (float*) (parms + sizeof(int) + pixels*sizeof(float));

	/* we normalize the images to [0,1], maybe I'll find a better way later */
	min = efdata[0];
	max = efdata[0];
	for (i = pixels * nrEf - 1; i > 0; i--) { /* we don't have to consider efdata[0] since this was used for init */
		if (efdata[i] > max) max = efdata[i];
		else if (efdata[i] < min) min = efdata[i];
	}
	max -= min;

	for (i = 1; i < nrEf+1; i++) {
		if (!((*images)[i].imgdata = malloc(sizeof(float) * pixels))) break;
		(*images)[i].height = gParms->height;
		(*images)[i].width = gParms->width;
		temp = (*images)[i].imgdata;
		for (j = 0; j < pixels; j++, temp++, efdata++) 
			*temp = (*efdata-min)/max;	/* copy and normalize */
	}

	if (i != nrEf+1) { /* something went wrong, dealloc */
		for(i--; i > 0; i--) {
			free((*images)[i].imgdata);
		}
		free(*images);
	} else { /* everything ok */
		err = FR_OK;
		*nrImages = nrEf+1;
	}

	return err;
}

int _efTrainedDataToText(FRrecognitionParameters* gParms, BYTE* parms, char** text) {
	return FR_ERR_NOTIMPL;
}

int _efTraitToImages(FRrecognitionParameters* gParms, BYTE* parms, BYTE* trait, FRimage** images, unsigned int* nrImages) {
	int i,j, nrFaces = *(int*)parms, pixels = gParms->width*gParms->height;
	float* avg = (float*) (parms+sizeof(int)), * ef = avg+pixels, * weights = (float*) trait;
	float min, max;

	/* allocate space for image structs*/
	if ((*images = (FRimage*) malloc(sizeof(FRimage)*(nrFaces+1))) == NULL) return FR_ERR_NOMEM;

	/* compute components */
	for (i=0; i < nrFaces; i++) {
		if (((*images)[i].imgdata = (float*) malloc(sizeof(float)*pixels)) == NULL) break;
		(*images)[i].height = gParms->height;
		(*images)[i].width = gParms->width;
		_matMultiplyD((*images)[i].imgdata, weights+i, ef+pixels*i,1,1,pixels);
	}

	if (i < nrFaces) { /* something went wrong */
		for (; i >= 0; i--) {
			free((*images)[i].imgdata);
		}
		return FR_ERR_NOMEM;
	}

	if (((*images)[i].imgdata = (float*) malloc(sizeof(float)*pixels)) == NULL) {
		for (; i >= 0; i--) {
			free((*images)[i].imgdata);
		}
		return FR_ERR_NOMEM;
	}

	(*images)[i].height = gParms->height;
	(*images)[i].width = gParms->width;
	memset((*images)[i].imgdata, 0, sizeof(float)*pixels);
	/* add images up to reconstructed view */
	for (i=0; i < nrFaces; i++) {
		_matAdd((*images)[nrFaces].imgdata, (*images)[i].imgdata, 1, pixels);
	}

	/* add average to complete reconstruction */
	_matAdd((*images)[nrFaces].imgdata, avg, 1, pixels);
	*nrImages = nrFaces+1;

	/* get min and max */
	min = (*images)[0].imgdata[0];
	max = (*images)[0].imgdata[0];
	for (i=0; i < nrFaces+1; i++) {
		for (j=0; j < pixels; j++) {
			if ((*images)[i].imgdata[j] > max) max = (*images)[i].imgdata[j];
			if ((*images)[i].imgdata[j] < min) min = (*images)[i].imgdata[j];
		}
	}

	printf("\n MAX=%f   MIN=%f\n", max, min);
		
	return FR_OK;
}

int _efTraitToText(FRrecognitionParameters* gParms, BYTE* parms, BYTE* trait, char** text) {
	return FR_ERR_NOTIMPL;
}

void _efSortIdx(float* values, int* index, unsigned int nr) {
	unsigned int i, j, maxIndex;
	float max;

	//init index
	for (i=0; i < nr; i++)
		index[i] = i;

	//sort
	for (i=0; i < nr; i++) {
		max = values[index[i]];
		maxIndex = i;
		for (j=i+1; j < nr; j++) {
			if (max < values[index[j]]) {
				maxIndex = j;
				max = values[index[j]];
			}
		}
		if (maxIndex != i) {
			j = index[i];
			index[i] = index[maxIndex];
			index[maxIndex] = j;
		}
	}
}