//  Rasterizor2D.cpp -- Analysis of 2D Mondrian Photographs
//
//
// DESCRIPTION
//
// Copyright © Timothy Bahls, Daniel Scharstien 2003.
//
///////////////////////////////////////////////////////////////////////////

#pragma once

#include "Error.h"
#include "Image.h"
#include "Color.h"
#include "Rasterizor2D.h"
#include "Utils.h"

#ifndef UNKNOWN
#define UNKNOWN -9876
#define UNKNOWNF -9876.5f
#define MAX 1234.5f
#define MIN -1234.5f
#ifndef WIN32
#define __max(a,b)  (((a) > (b)) ? (a) : (b))
#define __min(a,b)  (((a) < (b)) ? (a) : (b))
#endif
#endif

#define MAXSIMILARITY 55.0f




///////////////////////////////////////////////////////////////////////////
// class CRegRast2D - a single region


CRegRast2D::CRegRast2D(int iid, CRasterizor2D * master)
{
	id = iid;
	
	nFrames=master->nFrames;
	maxWidth=master->maxWidth;


	maxVelocity=master->maxVelocity;
	minVelocity=master->minVelocity;
	oddsSoFar=0;
	numberOfVels=maxVelocity-minVelocity+1;

	//minVel=minVelocity;
	//maxVel=maxVelocity;
	
	int*possVelInit=new int[numberOfVels];
	possVels=possVelInit-minVelocity;
	for(int v=minVelocity;v<=maxVelocity;v++){
		possVels[v]=0;
	}

	nFrames=master->nFrames;

/*	xmins=new int[nFrames];
	for(int i=0; i<nFrames;i++){
		xmins[i]=UNKNOWN;
	}
	xmaxes=new int[nFrames];
	for(int i=0; i<nFrames;i++){
		xmaxes[i]=UNKNOWN;
	}
	leftIDs=new int[nFrames];
	for(int i=0; i<nFrames;i++){
		leftIDs[i]=UNKNOWN;
	}
	rightIDs=new int[nFrames];
	for(int i=0; i<nFrames;i++){
		rightIDs[i]=UNKNOWN;
	}
	*/
	data=new int*[nFrames];
	for(int i=0;i<nFrames;i++){
        data[i]=new int[maxWidth];
		for(int x=0; x<maxWidth;x++){
            data[i][x]=0;
		}
	}

	

}

void CRegRast2D::deleteOld()
{
	/*
	for(int i=0; i<nFrames;i++){
		xmins[i]=UNKNOWN;
	}
	for(int i=0; i<nFrames;i++){
		xmaxes[i]=UNKNOWN;
	}
	for(int i=0; i<nFrames;i++){
		leftIDs[i]=UNKNOWN;
	}
	for(int i=0; i<nFrames;i++){
		rightIDs[i]=UNKNOWN;
	}
	*/
	
	for(int i=0;i<nFrames;i++){
        data[i]=new int[maxWidth];
		for(int x=0; x<maxWidth;x++){
            data[i][x]=0;
		}
	}

//for(int v=minVelocity;v<=maxVelocity;v++){
////		possVels[v]=0;
//	}
//oddsSoFar=0;
}

void CRegRast2D::velIsNot(int vel, int odds){
	possVels[vel]+=odds;
	oddsSoFar+=odds;
	velScale=((float)numberOfVels)/((float)oddsSoFar);
	//guessVelocity();
}

void CRegRast2D::velLessThan(int max, int odds)
{
	
	if(max<minVelocity||max>maxVelocity){
		return;
	}
	for(max++; max<=maxVelocity; max++){
		possVels[max]+=odds;
		oddsSoFar+=odds;
	}
	
	velScale=((float)numberOfVels)/((float)oddsSoFar);
	guessVelocity();
}


void CRegRast2D::velMoreThan(int min, int odds)
{
	
	if(min<minVelocity||min>maxVelocity){
		return;
	}
	for(min--; min>=minVelocity; min--){
		possVels[min]+=odds;
		oddsSoFar+=odds;
	}
	
	velScale=((float)numberOfVels)/((float)oddsSoFar);
	guessVelocity();
}


void CRegRast2D::guessVelocity()
{
	int minSoFar=possVels[minVelocity];
	int firstMinIndex=minVelocity;
	int lastMinIndex=minVelocity;
	for(int i=minVelocity+1;i<=maxVelocity;i++){
		int currVal=possVels[i];
		if(minSoFar>currVal){
			firstMinIndex=i;
			minSoFar=currVal;
		}
		if(minSoFar==currVal){
			lastMinIndex=i;
		}
	}

	
	minVel=firstMinIndex;
	maxVel=lastMinIndex;
}


void CRegRast2D::limitV(int f1, int f2)
{
	
	for(int x1=0;x1<maxWidth;x1++){
		if(data[f1][x1]<3&&data[f1][x1]<-3)
			continue;
		if(x1+minVelocity*(f2-f1)<0 || x1+maxVelocity*(f2-f1)>=maxWidth)
			continue;
		for(int v=minVelocity;v<=maxVelocity;v++){
			//if(possVels[v]*velScale>1.0f)
			//	continue;
			int testX=x1+v*(f2-f1);
			//if(testX<0||testX>=maxWidth)
				//continue;
			if(data[f2][testX]<3&&data[f2][testX]>-3)
				continue;
			if((data[f2][testX]<0)!=(data[f1][x1]<0)){
				velIsNot(v,1);
				if(v==19&&id==1){
					//printf("id=1 v=19 f1=%d f2=%d x1=%d testX=%d\n",f1,f2,x1,testX);
				}
			}
		}
	}
	//guessVelocity();
		/*for(int i=minVelocity;i<=maxVelocity;i++){
			if(possVels[i]>0){
				minVel=i;
				break;
			}
		}
		if(i>maxVelocity){
			throw CError("Region %d can have no velocity from MINVEL to MAXVEL\n",i);
		}
		for(int i=maxVelocity;i>=minVelocity;i--){
			if(possVels[i]*velScale<){
				maxVel=i;
				break;
			}
		}
*/
	
}


void CRegRast2D::restrictVel()
{
	
			for(int f1=0; f1<nFrames-1;f1++){
				for(int f2=f1+1; f2<nFrames;f2++){
					limitV(f1,f2);//restrict velocity by position
				}
			}
			guessVelocity();
}



void CRegRast2D::limitP(int f1, int f2)
{
	for(int x1=0;x1<maxWidth;x1++){
		bool allYes=true;
		bool allNo=true;
		if(x1+minVelocity*(f2-f1)<0 || x1+maxVelocity*(f2-f1)>=maxWidth)
			continue;
		for(int v=minVelocity;v<=maxVelocity;v++){
			if(possVels[v]*velScale>1.0f)
				continue;
			int currX=x1+v*(f2-f1);
			int curr=data[f2][currX];
			if (4>curr&&-4<curr){
				allYes=false;
				allNo=false;
				break;
			}else if(curr>0){
				allNo=false;
			}else if(curr<0){
				allYes=false;
			}
		}
		if(allNo){		
			data[f1][x1]-=1;
		}
		if(allYes){
			data[f1][x1]+=1;
		
		}
	}
}




void CRegRast2D::restrictPosition()
{

		for(int f=0; f<nFrames;f++){	
				for(int f2=f+1; f2<nFrames;f2++){
					limitP(f,f2);//restrict position by velocity
					limitP(f2,f);//restrict position by velocity
				}
		}
}

void CRegRast2D::convexHull()
{
	for(int f=0;f<nFrames;f++){
		int maxmin=UNKNOWN;
		int minmax=UNKNOWN;
		for(int j=0;j<maxWidth;j++){
			if(data[f][j]>3){
				maxmin=j;
				break;
			}
		}
		if(maxmin!=UNKNOWN){
			for(int k=maxWidth-1;k>=0;k--){
				if(data[f][k]>3){
					minmax=k;
					break;
				}
			}
			for(int x=maxmin;x<=minmax;x++){
				data[f][x]++;
			}
			int minmin=maxmin-1;
			while(minmin>=0&&data[f][minmin]>-2){
				minmin--;
			}
			for(;minmin>=0;minmin--){
				data[f][minmin]--;
			}
			int maxmax=minmax+1;
			while(maxmax<maxWidth&&data[f][maxmax]>-2){
				maxmax++;
			}
			for(;maxmax<maxWidth;maxmax++){
				data[f][maxmax]--;
			}
		}
	}

}

//bool CRegRast2D::operator == (CRegRast2D param)
//{
//	return (param.id==id);
//}

void CRegRast2D::printStuff(){
	CRegRast2D reg=*this;
	printf("%d's possVels is ",id);
	for(int v=minVelocity;v<=maxVelocity;v++){
		printf("%d ",reg.possVels[v]);
	}
	printf("\n");

	for(int f=0;f<nFrames;f++){
		for(int x=0;x<maxWidth;x++){
			//if(data[f][x]==id){
			//if(reg.data[f][x]==-1){
			//	printf("^");
			//}else if(reg.data[f][x]==0){
			//	printf("!");
			//}else if(reg.data[f][x]==1){
			//		printf("O");
			//}else{
			//	printf("*");
			//	}
			//}else{
			if(reg.data[f][x]==-1){
				printf(".");
			}else if(reg.data[f][x]==0){
				printf("?");
			}else if(reg.data[f][x]==1){
				printf("o");
			}else{
				printf("*");
			}
			//}
		}
		printf("\n");
	}
}




///////////////////////////////////////////////////////////////////////////
// class CRasterizor2D - all regions


CRasterizor2D::CRasterizor2D(int inFrames,bool iverbose)//,int iwidth)
  {
	nFrames=inFrames; 
	//nRegions=0; 
	maxID=-1;
	data=new int*[nFrames];
	allData=new int**[nFrames];
	//null=CRegRast2D();
	//null.id=-2;
	verbose=iverbose;	  
  }

// destructor
CRasterizor2D::~CRasterizor2D()
{
	for (int i = 0; i <=maxID; i++) {
		if (&region[i]!=0) {
			//delete &region[i];
		}
	}
}


void CRasterizor2D::p(int id){
	if(verbose){
		CCol c=colTable[id];
		char*a=c.describe();
		printf("%s",a);
	}
}


int CRasterizor2D::compare(int a,int b)   
{
	if(a==-1){//everything is below the boundary
		return 20000;
	}
	if(b==-1){
		return -20000;
	}
	if(relative[a][b]>9000&&relative[b][a]>9000)
		return 0;
	return relative[a][b]-relative[b][a];
}

/*
bool CRasterizor2D::setEdgeVel(int a, int b, float vel){
if(edgeVels[a][b]==UNKNOWNF){
edgeVels[a][b] = vel;
edgeVels[b][a] = vel;
return true;
}else{
if(edgeVels[a][b]==vel){
return false;
}else{
printf("WARNING: setEdgeVel(");
p(a);
printf(",");
p(b);
printf(",%f);",vel);

printf(") when %f is recorded\n",edgeVels[a][b]);
throw CError("setEdgeVel(%f)", vel);
return false;
}
}




}
*/

//sets c closer than f
void CRasterizor2D::setCloser(int c, int f, int odds)
{
	
	if(c==f){
		printf("Whoops: highSetCloser(%d,%d)\n",c,f);
		return;
	}
	if(odds==0){
		return;
	}
	if(relative[c][f]>10000){
		if(relative[f][c]>=-10000)
			relative[f][c]-=odds;
	}else{
		relative[c][f]+=odds;
	}
}




//Goes about extending and copying maxID, relative, minmax, and found.

void CRasterizor2D::addid(int a){
	int oldMax=maxID;
	maxID=a;
	CRegRast2D * b=new CRegRast2D[maxID+1];
	for(int i=0;i<=oldMax;i++){
			b[i]=region[i];
	}
	b[maxID]=CRegRast2D(a,this);
//	nRegions++;
	region=b;
	int ** c=new int*[maxID+1];
	for(int i=0;i<=maxID;i++){
		c[i]=new int[maxID+1];
	}
	for(int i=0;i<=maxID;i++){
		for(int j=0;j<=maxID;j++){
			if(i<=oldMax&&j<=oldMax){
				c[i][j]=relative[i][j];
			}else{
				if(i==j){
					c[i][j]=0;
				}else{
					c[i][j]=0;
				}
			}
		}
	}
	//if(oldMax>0){
	//delete [] relative;
	//}
	relative=c;
	//float ** d=new float*[maxID+1];
	//for(int i=0;i<=maxID;i++){
	//	d[i]=new float[maxID+1];
	//}
	//for(int i=0;i<=maxID;i++){
	//	for(int j=0;j<=maxID;j++){
	//		if(i<=oldMax&&j<=oldMax){
	//			d[i][j]=edgeVels[i][j];
	//		}else{
	//			d[i][j]=UNKNOWNF;
	//		}
	//	}
	//}
	//if(oldMax>0){
	//delete [] relative;
	//}
	//edgeVels=d;
}

int CRasterizor2D::colToID(CCol c){
	float minSimilarity=MAXSIMILARITY+1;
	int minIndex;
	for(int i=0;i<=maxID;i++){
		float f=c.similarity(colTable[i]);
		if(f<minSimilarity){
			if(f==0){
				return i;
			}
			minIndex=i;
			minSimilarity=f;
		}
	}
	if(MAXSIMILARITY >= minSimilarity){
		return minIndex;
	}
	CCol*a=new CCol[maxID+2];
	for(int i=0; i<=maxID;i++){
		a[i]=colTable[i];
	}
	colTable=a;
	colTable[i]=c;
	addid(i);
	return i;
}

//returns the id of the color
int CRasterizor2D::getPixID(int row, int col){
 
	if(row<0||row>=im.Shape().height||
		col<0||col>=im.Shape().width){
			return -1;//id for out of shape.  Closer than everything.
		}
		CCol *c=(CCol*)&im.Pixel(col,row,0);     //notice switch
		CCol cc=*c;
		int a=colToID(cc);
		
		return a;
}



void CRasterizor2D::analyzeT(int row, int col, int id1, int id2, int id3){
	int count1=0,count2=0, count3=0, count4=0;
	for (int i=0; i<4; i++){
		for(int j=0; j<4;j++){
			if(getPixID(row+i,col+j)==id1){
				count1++;
			}else if(getPixID(row+i,col+j)==id2){
				count2++;
			}else if(getPixID(row+i,col+j)==id3){
				count3++;
			}else{
				return;
			}
		}
	}
	if(count1>__max(count2,count3)+1){	
		setCloser(id1,id2,count1-count2);
		setCloser(id1,id3,count1-count3);
	}else if(count2>__max(count3,count1)+1){
		setCloser(id2,id3,count2-count3);
		setCloser(id2,id1,count2-count1);
	}else if(count3>__max(count2,count1)+1){
		setCloser(id3,id1,count3-count1);
		setCloser(id3,id2,count3-count2);
	}
}

void CRasterizor2D::checkForT(int row, int col, int displace)
{
	//printf("a");
	int aa=getPixID(row+displace,col);
	int ba=getPixID(row+displace,col+1);//aa!=ba
	int ab=getPixID(row+1-displace,col);
	int bb=getPixID(row+1-displace,col+1);
	if(ab!=aa&&ab!=ba){
		if(bb==ba){
			if(displace!=1){//if not already counted
				analyzeT(row-1, col-1,aa,ba,ab);
			}
		}else if(bb==ab){
			analyzeT(row-1, col-1,aa,ba,ab);
		}else{
			return;//fourway junction or two corners
		}
	}else{
		if(bb!=ba&&bb!=aa){
			if(ba==ab||(aa==ab&&displace==1)){
				return;//two corners or already counted
			}else{
				analyzeT(row-1, col-1,aa,ba,bb);
			}
		}else{
			return; //do nothing
		}
	}
}




// read a frame: look at all pixels in center scanline and initialize / update
// regions and their edges
void CRasterizor2D::readFrame(CByteImage iim, int frame,int row)
{
	im=iim;
	CShape sh = im.Shape();
	int w = sh.width, h = sh.height;
	maxWidth=w;
	data[frame]=new int[w];
	if(row==0){
		allData[frame]=new int*[w];
		for(int i=0;i<w;i++){
			allData[frame][i]=new int[h];
		}
	}
	int oldid = getPixID(row,0);
	data[frame][0]=oldid;
	allData[frame][0][row]=oldid;
	region[oldid].data[frame][0]=1000;
	for (int x = 1; x < w; x++) {
		int newid = getPixID(row,x);
		data[frame][x]=newid;
		allData[frame][x][row]=newid;
		region[newid].data[frame][x]=1000;
		if(newid!=oldid){      
			oldid = newid;
			if(row>1&&x>1&&row<=h-2&&x<=w-2)
				checkForT(row-1, x-1,1);
			if(row>2&&x>1&&row<=h-3&&x<=w-2)
				checkForT(row, x-1,0);
		}
	}	
}

void CRasterizor2D::possibilityElimination(){
	for (int i=0; i <= maxID; i++) {
		CRegRast2D r=region[i];
			region[i].restrictVel();
			region[i].restrictPosition();
			
			if(!concave){
				region[i].convexHull();
			}
	}
}


void CRasterizor2D::occlusionDetection()
{
	for (int i=0; i <= maxID; i++) {
		CRegRast2D r=region[i];
		for(int f=0; f<nFrames;f++){
			for(int x=0;x<maxWidth;x++){
				int curr=data[f][x];
				if (curr==i)
					continue;
				if(compare(i,curr)>4){
					region[i].data[f][x]-=2;//Reverse Occlusion
					//}
				}
				if(r.data[f][x]>1){
					setCloser(curr,i,50);//Occlusion Detection;	
				}
			}
		}
	}
	
}


void CRasterizor2D::stereoRestriction()
{
	for (int i=0; i <= maxID; i++) {
		CRegRast2D r1=region[i];
		for (int j=0; j <= maxID; j++) {
			CRegRast2D r2=region[j];
			if (i==j)
				continue;


			if(r1.minVel>r2.maxVel){
				setCloser(i,j,20);
			}
			if(compare(i,j)>8){
				region[i].velMoreThan(r2.minVel,1);
				region[j].velLessThan(r1.minVel,1);
			}
		}
	}
}



//like it sounds
void CRasterizor2D::transitiveClosure()
{
	for (int i=0; i <= maxID; i++) {
		for (int j=0; j <= maxID; j++) {
			if(compare(i,j)<=12)
				continue;
			for (int k=0; k <= maxID; k++) {
				if(compare(j,k)<=12||i==k)
					continue;
				setCloser(i,k,__min(45,__min(compare(i,j),compare(j,k))));
			}
		}
	}
}


void CRasterizor2D::reportResults()
{
	printf("\nResults: %d regions observed\n", maxID+1);
	for (int i=0; i <= maxID; i++) {
		CRegRast2D r=region[i];
		
		r.guessVelocity();
		printf("   ");
		p(i);
		printf(" vel");
		if(r.minVel==r.maxVel){
			printf(":%4d=%4d", (int)r.minVel,(int)r.maxVel);
		}else{
			printf(":%4d~%4d",(int)r.minVel,(int)r.maxVel);
		}
		printf(" Over: ");
		for (int j=0; j <= maxID; j++) {
			if(compare(i,j)>3){
				p(j);
				printf(" ");
			}         
		}
		printf("\n");
		
		//printf("\n");
		
	}

	//printStuff(1);
	
}





void CRasterizor2D::printStuff(int i)
{
	CRegRast2D reg=region[i];
	p(i);
	printf("'s possVels is ");
	int total=0;
	for(int v=minVelocity;v<=maxVelocity;v++){
		justify(((float)reg.possVels[v])*reg.velScale);
		printf(" ");
total+=reg.possVels[v];

		//printf("%d ",reg.possVels[v]);
	}
	printf("\n");
	//printf("total=%d number of Vels=%d velScale=%f divide=%f\n",total,reg.numberOfVels,reg.velScale,((float)reg.numberOfVels)/((float)total));
		

	return;
	for(int f=0;f<nFrames;f++){
		for(int x=0;x<maxWidth;x++){
			//printf("%4d ",reg.data[f][x]);

			
			if(data[f][x]==i){


				if(reg.data[f][x]<0){
					printf("^");
				}else if(reg.data[f][x]==0){
					printf("!");
				}else if(reg.data[f][x]>0){
					printf("O");
				}else{
					printf("*");
				}
			}else{
				if(reg.data[f][x]<0){
					printf(".");
				}else if(reg.data[f][x]==0){
					printf("?");
				}else if(reg.data[f][x]>0){
					printf("o");
				}else{
					printf("*");
				}
			}
			
		}
		printf("\n");
	}
	
}

// infer depth ordering, lengths and motion
int CRasterizor2D::goGoGo(CByteImage* imgs, bool stereo, int iminVelocity, int imaxVelocity, bool iconcave)
{
	minVelocity=iminVelocity;
	maxVelocity=imaxVelocity;
	concave=iconcave;
	times=0;
	maxHeight=imgs[0].Shape().height;
	if(verbose){
		printf("Possibility Elimination and Occlusion Detection\n");
	}
	for(int row=0;row<imgs[0].Shape().height;row++){
		//int row=200;
		for(int i=0; i<=maxID;i++){
			region[i].deleteOld();
		}
		for(int f=0;f<nFrames;f++){
			readFrame(imgs[f],f,row);
		}
		int loopStopper=0;
		//analyzeSingleEdges();
		//extendedT();
		
		while (loopStopper<2) {

			//if(maxID>=1)printf("%d Vel: min=%2d max=%2d oddsSoFar=%d\n",row,region[1].minVel,region[1].maxVel,region[1].oddsSoFar);
	//if(row>121&&row<125||row>250)
		//if(maxID>=1)printStuff(1);
		//if(row==200) printStuff(3);


			possibilityElimination();
			occlusionDetection();	
			loopStopper++;
		if(stereo){
			//stereoRestriction();
		}
		//transitiveClosure();
		
		
	
		}
//printf("Row %d\n\n",row);

		
	}
	if(verbose){
		printf("Stereo Restriction and Transitive Closure\n");	
	}

	for(int i=0;i<300;i++){
		if(stereo){
			stereoRestriction();
		}
	transitiveClosure();

	}
	if(verbose)
		reportResults();
	return times;
}