//  Approx2D.h -- Analysis of 2D Mondrian scenes that can use float values.  
//Only a slight modification of Approximator, which is only a slight 
//modification of Deductor
//
//
// DESCRIPTION
//
// Copyright © Tim Bahls, Daniel Scharstien 2003.
//
///////////////////////////////////////////////////////////////////////////

#pragma once

#include "Error.h"
#include "Image.h"
#include "Color.h"
#include "Approx2D.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 MINVEL -77
#define MAXVEL 77
#define MINLEN 0
#define MAXLEN 1000

#define DEBUG1 0
#define DEBUG2 0


///////////////////////////////////////////////////////////////////////////
// class CRegApprox2D - a single region

CRegApprox2D::CRegApprox2D(int iid, const CApprox2D* master)
{
	id = iid;
	minVel=master->minVelocity;
	maxVel=master->maxVelocity;
	minLen=MINLEN;
	maxLen=MAXLEN;
	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;
	}
	minmax=new float[nFrames];
	for(int i=0; i<nFrames; i++){
		minmax[i]=MIN;
	}
	maxmin=new float[nFrames];
	for(int i=0; i<nFrames; i++){
		maxmin[i]=MAX;
	}
	minmin=new float[nFrames];
	for(int i=0; i<nFrames; i++){
		minmin[i]=MIN;
	}
	maxmax=new float[nFrames];
	for(int i=0; i<nFrames; i++){
		maxmax[i]=MAX;
	}
}

void CRegApprox2D::deleteOld()
{
	minLen=MINLEN;
	maxLen=MAXLEN;
	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++){
		minmax[i]=MIN;
	}
	for(int i=0; i<nFrames; i++){
		maxmin[i]=MAX;
	}
	for(int i=0; i<nFrames; i++){
		minmin[i]=MIN;
	}
	for(int i=0; i<nFrames; i++){
		maxmax[i]=MAX;
	}
}

bool CRegApprox2D::limitV(int f1, int f2, float*minA, float*maxA){
	bool a=false;
	float minV=(maxA[f1]-minA[f2])/(float)(f1-f2);
	float maxV=(minA[f1]-maxA[f2])/(float)(f1-f2);
	a |= max(minVel,minV);
	a |= min(maxVel,maxV);
	if(f1==f2) {
		printf("Divide by 0 minVel=%f maxVel=%f minV=%f maxV=%f\n",minVel,maxVel,minV,maxV);
		throw CError("Divide by 0 minVel=%f\n",minVel);
	}
	return a;
}



bool CRegApprox2D::restrictVel(){
	
	if(minVel==maxVel)
		return false;
	bool change=false;
	for(int f1=0; f1<nFrames-1;f1++){
		for(int f2=f1+1; f2<nFrames;f2++){
			change|=limitV(f1,f2,minmin,maxmin);//restrict velocity by position
			change|=limitV(f1,f2,minmax,maxmax);
		}
	}
	return change;
}

bool CRegApprox2D::restrictLen(){
	if(minLen==maxLen)
		return false;
	bool change=false;
	for(int f=0; f<nFrames;f++){
		if(maxmin[f]!=MAX&&minmax[f]!=MAX)
			change|=max(minLen,minmax[f]-maxmin[f]);//restrict length by position
		if(maxmax[f]!=MAX&&minmin[f]!=MAX)
			change|=min(maxLen,maxmax[f]-minmin[f]);
	}
	return change;
}

bool CRegApprox2D::limitP(int f1, int f2, float*&minA, float*&maxA){
	bool a=false;
	float minX1=minA[f2]+maxVel*(f1-f2);
	float maxX1=maxA[f2]+minVel*(f1-f2);
	float minX2=minA[f1]+minVel*(f2-f1);
	float maxX2=maxA[f1]+maxVel*(f2-f1);
	a|=max(minA[f1],minX1);
	a|=min(maxA[f1],maxX1);
	a|=max(minA[f2],minX2);
	a|=min(maxA[f2],maxX2);
	return a; 
}



bool CRegApprox2D::restrictPosition(){
	bool change=false;
	for(int f=0; f<nFrames;f++){	
		if(maxLen!=MAX&&maxmin[f]!=MAX)
			change|=min(maxmax[f],maxmin[f]+maxLen);//restrict position by length
		if(maxLen!=MAX&&minmax[f]!=MIN)
			change|=max(minmin[f],minmax[f]-maxLen);
		if(minmin[f]!=MIN)
			change|=max(minmax[f],minmin[f]+minLen);
		if(maxmax[f]!=MAX)
			change|=min(maxmin[f],maxmax[f]-minLen);
		for(int f2=f+1; f2<nFrames;f2++){
			change|=limitP(f,f2,minmin,maxmin);//restrict position by velocity
			change|=limitP(f,f2,minmax,maxmax);
		}
	}
	return change;
}

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



///////////////////////////////////////////////////////////////////////////
// class CApprox2D - all regions

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

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


void CApprox2D::p(int id){
	if(verbose)
		CCol(id).printName();
}


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



//sets high priority closer
bool CApprox2D::setCloser(int c, int f, int odds)
{
	if(c==f){
		printf("Whoops: highSetCloser(%d,%d)\n",c,f);
		return false;
	}
	if(odds==0){
		return false;
	}
	if(relative[c][f]>1000){
		if(relative[f][c]>=-1000)
			//return false;
		relative[f][c]-=odds;
		//return true;
	}else{
	relative[c][f]+=odds;
	}
	if(f+c==DEBUG1+DEBUG2&&f*c==DEBUG1*DEBUG2&&odds<50){
		printf("     ");
		p(c);
		printf(" may be closer than ");
		p(f);
		printf(".  The odds are %d\n",odds);
		return true;
	}
}

//Goes about extending and copying maxID, relative, minmax, and found.
void CApprox2D::addid(int a){

	int oldMax=maxID;
	maxID=a;
	CRegApprox2D * b=new CRegApprox2D[maxID+1];
	for(int i=0;i<=maxID;i++){
		if(i<=oldMax){
			b[i]=region[i];
		}else{
			b[i]=null;
		}
	}
	if(oldMax>0){
		delete region;
	}
	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;
}


//returns the id of the color
int CApprox2D::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= cc.getId();
		if (a>maxID){
			addid(a);
		}
		if(region[a]==null){
			region[a]=CRegApprox2D(a,this);
			nRegions++;
		}
		return a;
}

void CApprox2D::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 CApprox2D::checkForT(int row, int col, int displace)
{
	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
		}
	}
}

// Record its left edge of region.
void CApprox2D::startRegion(int id, int frame, int x, int prevID)
{
	if(region[id].xmins[frame]==UNKNOWN){
		region[id].xmins[frame]=x;
		region[id].leftIDs[frame]=prevID;
		region[id].maxmin[frame]=(float)x+0.5f;
	}
}

// record right edge of current region
void CApprox2D::endRegion(int id, int frame, int x, int nextID)
{
	region[id].xmaxes[frame]=x;
	region[id].rightIDs[frame]=nextID;
	region[id].minmax[frame]=(float)x-0.5f;
}

// read a frame: look at all pixels in center scanline and initialize / update
// regions and their edges.  Also search for T-Junctions
void CApprox2D::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];
	int oldid = getPixID(row,0);
	startRegion(oldid, frame, 0, -1);
	data[frame][0]=oldid;
	for (int x = 1; x < w; x++) {
		int newid = getPixID(row,x);
		data[frame][x]=newid;
		if(newid!=oldid){      
			endRegion(oldid, frame, x-1, newid);
			startRegion(newid, frame, x, 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);
		}
	}
	//update last extent:
	endRegion(oldid, frame, w-1, -1);
}

void CApprox2D::printData()
{
	printf("%d regions observed\n", nRegions);
	for (int i=0; i <= maxID; i++) {
		if (region[i]==null)
			continue;
		printf("     ");
		p(i);
		printf(":");
		CRegApprox2D r = region[i]; 
		for (int f=0; f < nFrames; f++) {
			printf("  f%d:", f);        
			if(r.xmins[f]==UNKNOWN)
				continue;
			printf("%d-%d,", r.xmins[f],r.xmaxes[f]);
		}
		printf("\n");

	}
}

void CApprox2D::possibilityElimination(){
	for (int i=0; i <= maxID; i++) {
		CRegApprox2D r=region[i];
		if (r==null)
			continue;
		bool change2=true;
		change2=false;
		change2|=region[i].restrictPosition();
		change2|=region[i].restrictVel();
		change2|=region[i].restrictLen();
	}
}

//If Region A is known to be there, but region B is seen, B<A
//If A<B and Region B is seen at point p, then A does not extend to p
void CApprox2D::occlusionDetection()
{
	for (int i=0; i <= maxID; i++) {
		CRegApprox2D r=region[i];
		if (r==null)
			continue;
		for(int f=0; f<nFrames;f++){
			int start1=ROUND(r.maxmin[f]);
			start1=__min(start1,maxWidth-1);
			start1=__min(start1,ROUND(r.minmax[f]));
			int end1=ROUND(r.minmin[f]);
			end1=__max(end1,0);
			for(int x=start1;x>=end1;x--){
				if(compare(i,data[f][x])>40){
					r.minmin[f]=(float)x+0.5f;//if i extended this far, it would've been found.  Thus it doesn't.
					break;
				}
			}
			int start2=ROUND(r.minmax[f]);
			start2=__max(start2,0);
			start2=__max(start2,ROUND(r.maxmin[f]));
			int end2=ROUND(r.maxmax[f]);
			end2=__min(end2,(int)maxWidth-1);
			for(int x=start2;x<=end2;x++){
				if(compare(i,data[f][x])>40){
					r.maxmax[f]=(float)x+0.5f;//if i extended this far, it would've been found.  Thus it doesn't.
					break;
				}
			}

			if(r.maxmin[f]==MAX||r.maxmin[f]==MIN)
				continue;
			for(int x=__max(0,ROUND(r.maxmin[f]));x<=__min(ROUND(r.minmax[f])-1,maxWidth-1);x++){
				int id=data[f][x];
				if(id!=i){
					setCloser(id,i,50);	
				}
			}
		}
	}
}


void CApprox2D::stereoRestriction()
{
	for (int i=0; i <= maxID; i++) {
		CRegApprox2D r1=region[i];
		if (r1==null)
			continue;
		for (int j=0; j <= maxID; j++) {
		CRegApprox2D r2=region[j];
		if (i==j||r2==null)
			continue;
		if(r1.minVel>r2.maxVel){
			setCloser(i,j,30);
		}
		if(compare(i,j)>8){
			r2.min(region[j].maxVel,r1.maxVel);
			r1.max(region[i].minVel,r2.minVel);
		}



		}
	}
}

//like it sounds
void CApprox2D::transitiveClosure()
{
	for (int i=0; i <= maxID; i++) {
		if (region[i]==null)
			continue;
		for (int j=0; j <= maxID; j++) {
			if(compare(i,j)>12){
				for (int k=0; k <= maxID; k++) {
					if(compare(j,k)>12&&i!=k){	
						if(i*k==DEBUG1*DEBUG2&&i+k==DEBUG1*DEBUG2){
							p(i);printf("%d",compare(i,j));p(j);printf("%d",compare(j,k));p(k);printf("\n");
						}
						setCloser(i,k,__min(45,__min(compare(i,j),compare(j,k))));

					}
				}
			}
		}
	}
}

void CApprox2D::reportResults()
{
	printf("\nResults: %d regions observed\n", nRegions);
	for (int i=0; i <= maxID; i++) {
		CRegApprox2D r=region[i];
		if (r==null)
			continue;
		printf("   ");
		p(i);
		printf(" vel:");
		justify(r.minVel);
		printf("~");
		justify(r.maxVel);
		printf(" Over: ");
		for (int j=0; j <= maxID; j++) {
			if(compare(i,j)>4){
				p(j);
				printf(" ");
			}         
		}
		printf("\n");
	}
}


// infer depth ordering, lengths and motion
int CApprox2D::goGoGo(CByteImage* imgs, bool stereo,float iminVelocity, float imaxVelocity)
{
	minVelocity=iminVelocity;
	maxVelocity=imaxVelocity;
	times=0;
	for(int row=0;row<imgs[0].Shape().height;row++){
		for(int i=0; i<=maxID;i++){
			if(region[i]==null) continue;
			region[i].deleteOld();
		}
		for(int f=0;f<nFrames;f++){

			readFrame(imgs[f],f,row);
		}
		int loopStopper=0;
		while (loopStopper<5) {
			possibilityElimination();
			occlusionDetection();	
			loopStopper++;
		}
		for(int i=0; i<=maxID;i++){
			if(region[i]==null) continue;
			region[i].deleteOld();
		}

	}
	for(int i=0;i<10;i++){
		if(stereo){
			stereoRestriction();
		}
		transitiveClosure();
	}
	for (int i=0; i <= maxID; i++) {
		for (int j=0; j <= maxID; j++) {
			if(compare(i,j)>6)
			{
				times++;
			}
		}
	}
	if(verbose)
		reportResults();
	return times;
}


void CApprox2D::checkAccuracy(CScene scene)
{
	int errors=0;
	for (int i=1; i <= maxID; i++) {
		CRegApprox2D reg=region[i];
		if (reg==null)
			continue;
		CRect rect=scene.getByID(i);
		if(rect.vel.x>reg.maxVel||rect.vel.x<reg.minVel){

			errors++;
			printf("ERROR! The velocity for "); 
			p(i);	
			printf(" is false: %f <= %f <= %f\n",reg.minVel,rect.vel.x,reg.maxVel);
			errors++;
		}
		for(int j=1; j<=maxID;j++){
			CRect rect2=scene.getByID(j);
			if(compare(i,j)>6&&rect.z>rect2.z){
				verbose=true;
				printf("ERROR! ");
				p(i);
				printf(" doesn't beat ");
				p(j);
				printf("!  fc=%d cf=%d\n",relative[i][j],relative[j][i]);
				errors++;
			}
		}
		if(compare(0,i)>6){
			verbose=true;
			printf("ERROR! ");
			p(i);
			printf(" not behind ");
			p(0);
			printf("!  fc=%d cf=%d\n",relative[i][0],relative[0][i]);
			errors++;
		}
	}
	if(errors==0){
		printf("All true\n");
	}else{
		printf("%d errors\n",errors);
	}
}