#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 "training.h"
#include "feature.h"
#include "BestWC.h"

BestWC::BestWC() {
	desiredDetect=0;
	desiredfalsepos=0;
	currentdetect=0;
	currentfalsepos=1;
	Tmax=30;
	numpos=0;
	numneg=0;
	combinedError=1;
	bestClassifiersAlpha=0;
	isFuzzy=false;
	constantAlpha=false;
}

BestWC::BestWC(float desDetect, float desfalsepos, int pos, int neg) {	//desired detection rate + falsepos rate
	desiredDetect=desDetect;
	desiredfalsepos=desfalsepos;
	currentdetect=0;		//current falsenegative rate
	currentfalsepos=1;		//current falsepositive rate
	Tmax=30;				
	numpos=pos;				//p number of positive training examples
	numneg=neg;				//n number of negative training examples
	combinedError=1;		//combined error of strong classifier
	bestClassifiersAlpha=0;	//current alpha of combined strong classifier

	alphaHX=new float[pos+neg];
	for (int i=0; i<(pos+neg); i++) {
		alphaHX[i]=0;
	}
	isFuzzy=false;
	constantAlpha=false;
}

BestWC::~BestWC() {
	desiredDetect=0;
	desiredfalsepos=0;

	for (int di=0; di<bestClassifiers.size(); ++di){			
		training *e = (training*) bestClassifiers[di];
		delete e;
	}

	delete [] alphaHX;
}

void BestWC::findBestWC(std::vector<training*>& trList, int indexFirstWC, std::vector<feature*>& featList,
						std::vector<IplImage*>& positiveIntegrals, std::vector<IplImage*>& negativeIntegrals,
						std::vector<IplImage*>& reds, IplImage *EObins[MAX_DATABASE_ENTRIES][6]) {
	bool alreadyFound(int* alfound, int ind, int numindices);
	
	training *e=(training*) trList[indexFirstWC];
	float ealpha=e->alpha;
	if (constantAlpha)
		ealpha=1.0;
	//float currentCE=e->ce;	//current cumulative error of strong classifier
	float currentCE=500;
	//std::vector<trainingRecord*> bestrecords=e->getUnsortedTrainRecs();
	int *indices=new int[Tmax];
	int indicesCounter=0;

	//initialize the alpha x h(x) array for faster calculations later
	for (int k=0; k<(numpos+numneg); k++) {
		//trainingRecord *trb=(trainingRecord*) bestrecords[k];
		feature *f= (feature*) featList[e->featureNumber];
		f->setfeature(e->featoffsetX, e->featoffsetY, e->featScaleX, e->featScaleY);
		float bval=0;
		if (k<numpos) {
			bval=(float)f->calcValue((IplImage*) positiveIntegrals[k], EObins[k], (IplImage*) reds[k]);
		} else {
			bval=(float)f->calcValue((IplImage*) negativeIntegrals[k-numpos], EObins[k], (IplImage*) reds[k]);
		}

		float decision=0;	//1 to -1 fuzzy decision 
		
		if (e->s*bval<=e->s*e->theta) {
			if (abs(e->pm-e->theta)<=0.00001)
				decision=1;
			else {
				if (!isFuzzy)
					decision=1;
				else
					decision = min(1,abs((bval-e->theta)/(e->pm-e->theta)));
			}
		} else {
			if (abs(e->nm-e->theta)<=0.00001)
				decision =-1;
			else {
				if (!isFuzzy)
					decision=-1;
				else
					decision = max(-1,-abs((bval-e->theta)/(e->nm-e->theta)));
			}
		}
		
		alphaHX[k]=decision*ealpha;
	}
	
	training *er=new training(e);
	bestClassifiers.push_back(er);
	indices[indicesCounter]=indexFirstWC;
	indicesCounter++;

	currentdetect=(float)(numpos-e->missed)/(float)numpos;
	currentfalsepos=(float)e->falsepos/(float)numneg;

	short T=1;
	int curfalsepos=e->falsepos;	//best falsepos in loop
	int curmissed=e->missed;

	printf("currentdetect = %f currentfalsepos = %f at stage %i\n", currentdetect, currentfalsepos, 0);

	while ((currentdetect<=desiredDetect)||(currentfalsepos>=desiredfalsepos)) {
		int falsepos=0;
		int missed=0;
		float loopCE=currentCE;	//temp Current error of proposed WC 
		int bestindex=-1;	//index of proposed next WC addition

		for (int i=0; i<trList.size(); i++) {	//for each WC
			training *t=(training*) trList[i];	//get the WC

			feature *f1= (feature*) featList[t->featureNumber];	//corresponding feature
			f1->setfeature(t->featoffsetX, t->featoffsetY, t->featScaleX, t->featScaleY);
			
			float Ttheta=t->theta;
			//std::vector<trainingRecord*> unsorted=t->getUnsortedTrainRecs();	//get trainingvectors
			float alpha=t->alpha;	//weight of current WC
			if (constantAlpha)
				alpha=1.0;
			float alphaVal=0;

			//for every positive example
			for (int p=0; p<numpos; p++) {
				//trainingRecord *trr=(trainingRecord*) unsorted[p];
				float val=(float)f1->calcValue((IplImage*) positiveIntegrals[p], EObins[p], (IplImage*) reds[p]);
				//int val=trr->getVal();
				
				float decision=0;	//1 to -1 fuzzy decision 
				if (t->s*val<=t->s*Ttheta) {
					if (abs(t->pm-Ttheta)<=0.00001)
						decision=1;
					else {
						if (!isFuzzy)
							decision=1;
						else
							decision = min(1,abs((val-t->theta)/(t->pm-t->theta)));
					}
				} else {
					if (abs(t->nm-Ttheta)<=0.00001)
						decision =-1;
					else {
						if (!isFuzzy)
							decision=-1;
						else
							decision = max(-1,-abs((val-t->theta)/(t->nm-t->theta)));
					}
				}
				
				alphaVal=alpha*decision + alphaHX[p];
				
				if (alphaVal<0) {
					missed++;
				}
			}

			for (int n=numpos; n<numpos+numneg; n++) {
				//trainingRecord *trr=(trainingRecord*) unsorted[n];
				float val=(float)f1->calcValue((IplImage*) negativeIntegrals[n-numpos], EObins[n], (IplImage*) reds[n]);

				float decision=0;	//1 or -1 decision of 
				if (t->s*val<=t->s*Ttheta) {
					if (abs(t->pm-Ttheta)<=0.00001)
						decision=1;
					else {
						if (!isFuzzy)
							decision=1;
						else
							decision = min(1,abs((val-t->theta)/(t->pm-t->theta)));
					}
				} else {
					if (abs(t->nm-Ttheta)<=0.00001)
						decision =-1;
					else {
						if (!isFuzzy)
							decision=-1;
						else
							decision = max(-1,-abs((val-t->theta)/(t->nm-t->theta)));
					}
				}

				alphaVal=alpha*decision + alphaHX[n];

				if (alphaVal>=0) {
					falsepos++;
				}
			}
			//use falsepos and missed to find error and weight.
			float ce=((float)missed+(float)falsepos)/(float)(numpos+numneg);
			
			if ((ce<loopCE)&&(!alreadyFound(indices, i, indicesCounter))) {
				bestindex=i;
				loopCE=ce;
				curfalsepos=falsepos;
				curmissed=missed;
			}
			missed=0;
			falsepos=0;
		}

		//index of next WC to be added to best classifier is found
		if (bestindex>=0) {	//if best classifier exists
			training *t1=(training*) trList[bestindex];
			//std::vector<trainingRecord*> bestaddition=t->getUnsortedTrainRecs();
			feature *f2= (feature*) featList[t1->featureNumber];	//corresponding feature
			f2->setfeature(t1->featoffsetX, t1->featoffsetY, t1->featScaleX, t1->featScaleY);

			float alphaAdd=t1->alpha;
			if (constantAlpha)
				alphaAdd=1.0;
			float Ttheta=t1->theta;
			
			//update the alpha*h(x) array.
			for (k=0; k<(numpos+numneg); k++) {
				//trainingRecord *trb=(trainingRecord*) bestaddition[k];
				//float bval=(float)trb->getVal();
				float bval=0;
				if (k<numpos) {
					bval=(float)f2->calcValue((IplImage*) positiveIntegrals[k], EObins[k], (IplImage*) reds[k]);
				} else {
					bval=(float)f2->calcValue((IplImage*) negativeIntegrals[k-numpos], EObins[k], (IplImage*) reds[k]);
				}

				float decision=0;	//1 or -1 decision of 
				if (t1->s*bval<=t1->s*Ttheta) {
					if (abs(t1->pm-Ttheta)<=0.00001)
						decision=1;
					else {
						if (!isFuzzy)
							decision=1;
						else
							decision = min(1,abs((bval-t1->theta)/(t1->pm-t1->theta)));
					}
				} else {
					if (abs(t1->nm-Ttheta)<=0.00001)
						decision =-1;
					else {
						if (!isFuzzy)
							decision=-1;
						else
							decision = max(-1,-abs((bval-t1->theta)/(t1->nm-t1->theta)));
					}
				}
				alphaHX[k]+=decision*alphaAdd;
			}	
			training *er=new training(t1);
			bestClassifiers.push_back(er);
			currentdetect=(float)(numpos-curmissed)/(float)numpos;
			currentfalsepos=(float)(curfalsepos)/(float)numneg;

			printf("currentdetect = %f currentfalsepos = %f at stage %i\n", currentdetect, currentfalsepos, T);
			
			indices[indicesCounter]=bestindex;	//add the best index to the array so that it is not chosen again by the machine
			indicesCounter++;

		} else {
			printf("no additional wc can increase accuracy\n");
			T=Tmax;
		}
		T++;
		if (T>=Tmax) {
			printf("Tmax reached\n");
			break;
		}
	}
	printf("final detection rate is %f\n", currentdetect);
	printf("final falsepos rate is  %f\n", currentfalsepos);
	delete [] indices;
}

bool alreadyFound(int* alfound, int ind, int numindices) {

	for (int i=0; i<numindices; i++) {
		if (ind==alfound[i])
			return true;
	}

	return false;
}

std::vector<training*>& BestWC::getBestWCs() {
	return bestClassifiers;
}

void BestWC::printWCs() {

	FILE *pInFile = fopen("series.txt", "w");

	for (int mil=0; mil<bestClassifiers.size(); mil++) {
		training * e= (training*) bestClassifiers[mil];
		//fprintf(pInFile, "#name: ");
		fprintf(pInFile, "featno %i x %i y %i scalex %i scaley %i CE %f theta %f sign %i alpha %f pm %f nm %f\n", e->featureNumber, e->featoffsetX, e->featoffsetY, e->featScaleX, e->featScaleY, e->ce, e->theta, (int)e->s, e->alpha, e->pm, e->nm);
	}

	fclose(pInFile);

	for (mil=0; mil<bestClassifiers.size(); mil++) {
		training * e= (training*) bestClassifiers[mil];
		//printf("featno = %i, ", t->featureNumber);
		//printf("it is featno %i, with offset x %i, and offsety %i, scalex %i, scaley %i\n", e->featureNumber, e->featoffsetX, e->featoffsetY, e->featScaleX, e->featScaleY);
		//if (isFuzzy)
			printf("featno %i x %i y %i scalex %i scaley %i CE %f theta %f sign %i alpha %f pm %f nm %f\n", e->featureNumber, e->featoffsetX, e->featoffsetY, e->featScaleX, e->featScaleY, e->ce, e->theta, (int)e->s, e->alpha, e->pm, e->nm);
		//else
		//	printf("featno %i x %i y %i scalex %i scaley %i CE %f theta %f sign %i alpha %f\n", e->featureNumber, e->featoffsetX, e->featoffsetY, e->featScaleX, e->featScaleY, e->ce, e->theta, (int)e->s, e->alpha);
	}

	printf("here is the alpha*h(x) array\n");

	for (int i=0; i<(numpos+numneg); i++) {
		printf("%f\n", alphaHX[i]);
	}
}

void BestWC::showWCs(IplImage *demo) {
	for (int mil=0; mil<bestClassifiers.size(); mil++) {
		training * e= (training*) bestClassifiers[mil];
		IplImage *c = cvCloneImage(demo);
		CvPoint p1 = cvPoint(e->featoffsetX,e->featoffsetY);
		CvPoint p2 = cvPoint(e->featoffsetX+e->featScaleX*2,e->featoffsetY+e->featScaleY*2);
		if (e->featureNumber==0) {
			cvRectangle( c, p1, p2, CV_RGB(255,0,0), 1 );
			printf("horizontal rigth\n");
		}else if (e->featureNumber==1) {
			cvRectangle( c, p1, p2, CV_RGB(0,255,0), 1 );
			printf("bin 2\n");
		}else if (e->featureNumber==2) {
			cvRectangle( c, p1, p2, CV_RGB(0,0,255), 1 );
			printf("vertical\n");
		}else if (e->featureNumber==3) {
			cvRectangle( c, p1, p2, CV_RGB(255,255,0), 1 );
			printf("bin 4\n");
		}else if (e->featureNumber==4) {
			cvRectangle( c, p1, p2, CV_RGB(255,255,0), 1 );
			printf("horizontal left\n");
		}else if (e->featureNumber==5) {
			cvRectangle( c, p1, p2, CV_RGB(255,255,0), 1 );
			printf("horizontal left and right\n");
		}else {
			cvRectangle( c, p1, p2, CV_RGB(255,255,255), 1 );
			printf("reds\n");
		}
		cvNamedWindow("best", 1);
		cvShowImage( "best", c);
		cvWaitKey(0);
		cvReleaseImage(&c);
		//printf("it is featno %i, with offset x %i, and offsety %i, scalex %i, scaley %i\n", e->featureNumber, e->featoffsetX, e->featoffsetY, e->featScaleX, e->featScaleY);
	}
	cvDestroyWindow("best");
}

#ifdef _EiC
main(1,"BestWC.cpp");
#endif