//  Approximator.cpp -- Analysis of 1D Mondrian scenes
//
//
// DESCRIPTION
//
// Copyright © Tim Bahls, Daniel Scharstien 2003.
//
///////////////////////////////////////////////////////////////////////////



#pragma once

#include "Error.h"
#include "Image.h"
#include "Color.h"
#include "Approximator.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 -5
#define MAXVEL 5
#define MINLEN 5
#define MAXLEN 300


///////////////////////////////////////////////////////////////////////////
// class CRegApprox - a single region

CRegApprox::CRegApprox(int iid, int inFrames)
{
	id = iid;
	minVel=MINVEL;
	maxVel=MAXVEL;
	minLen=MINLEN;
	maxLen=MAXLEN;
	nFrames=inFrames;
	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;
	}
}

bool CRegApprox::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);
	return a;
}



bool CRegApprox::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 CRegApprox::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 CRegApprox::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 CRegApprox::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 CRegApprox::operator == (CRegApprox param) {
	return (param.id==id);
}



///////////////////////////////////////////////////////////////////////////
// class CApproximator - all regions

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


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


int CApproximator::compare(int a,int b)   
{
	if(a==-1){//everything is below the boundary
		return 2;
	}
	if(b==-1){
		return -2;
	}
	return relative[a][b];
}

//sets high priority closer
bool CApproximator::setCloser(int c, int f)
{

	if(c==f){

		printf("Whoops: highSetCloser(%d,%d)\n",c,f);
		return false;
	}

	switch(compare(c,f)){

		case -2:
			verbose=true;
			printf("WARNING: setCloserThan(");
			p(c);
			printf(",");
			p(f);
			printf(") when the opposite is recorded\n");

			throw CError("setCloserThan(%d)", f);
			return false;
			break;
		case 0:
			relative[c][f]=2;
			relative[f][c]=-2;
			if(verbose){
				printf("     ");
				p(c);
				printf(" closer than ");
				p(f);
				printf("\n");
			}
			return true;
			break;
		case 2:
			return false;
			break;
		default:
			printf("Screwy value--compare(%d,%d)=%d\n",c,f,compare(c,f));
			relative[c][f]=2;
			relative[f][c]=-2;
			return true;
	}
	return false;
}




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

void CApproximator::addid(int a){

	int oldMax=maxID;
	maxID=a;

	CRegApprox * b=new CRegApprox[maxID+1];
	for(int i=0;i<=maxID;i++){
		if(i<=oldMax){
			b[i]=region[i];
		}else{
			b[i]=null;
		}
	}
	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 CApproximator::getPixID(int row, int col){
	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]=CRegApprox(a,nFrames);
		nRegions++;
	}
	return a;
}



// create new region if necessary, add a new component, and record its left edge
void CApproximator::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 boundary of current region
void CApproximator::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
void CApproximator::readFrame(CByteImage iim, int frame)
{
	im=iim;
	CShape sh = im.Shape();
	int w = sh.width, h = sh.height;
	maxWidth=w;
	data[frame]=new int[w];
	int row=h/2;
	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;
		}
	}
	//update last extent:
	endRegion(oldid, frame, w-1, -1);
}

void CApproximator::printData()
{
	printf("%d regions observed\n", nRegions);
	for (int i=0; i <= maxID; i++) {
		if (region[i]==null)
			continue;
		printf("     ");
		p(i);
		printf(":");
		CRegApprox 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 CApproximator::possibilityElimination(){
	for (int i=0; i <= maxID; i++) {
		CRegApprox r=region[i];
		if (r==null)
			continue;
		bool change2=true;
		while(change2==true){
			change2=false;
			change2|=region[i].restrictPosition();
			change2|=region[i].restrictVel();
			change2|=region[i].restrictLen();
		}
	}
}

void CApproximator::occlusionDetection(){
	if(verbose)
		printf("Occlusion Detection:\n");
	for (int i=0; i <= maxID; i++) {
		CRegApprox 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])==2){
					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])==2){
					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);	
				}
			}
		}
	}
}






//like it sounds
void CApproximator::transitiveClosure()
{
	if(verbose)
		printf("Computing Transitive closure:\n");
	bool newEdge = true;
	while (newEdge) {
		newEdge = false;
		for (int i=0; i <= maxID; i++) {
			if (region[i]==null)
				continue;
			for (int j=0; j <= maxID; j++) {
				if(compare(i,j)==2){//definite
					for (int k=0; k <= maxID; k++) {
						if(compare(j,k)==2&&i!=k){//definite
							newEdge |= setCloser(i,k);//definite
							newEdge;
						}
					}
				}
			}
		}
	}
}



void CApproximator::reportResults()
{
	printf("\nResults: %d regions observed\n", nRegions);
	for (int i=0; i <= maxID; i++) {
		CRegApprox r=region[i];
		if (r==null)
			continue;
		printf("   ");
		p(i);
		printf(" vel:");
		justify(r.minVel);
		printf("~");
		justify(r.maxVel);
		printf(" len:");
		justify(r.minLen);
		printf("~");
		justify(r.maxLen);

		printf(" Over: ");
		for (int j=0; j <= maxID; j++) {
			if(compare(i,j)==2){
				p(j);
				printf(" ");
				//times++;
			}         
		}
		printf("\n");

	}

}


// infer depth ordering, lengths and motion
int CApproximator::goGoGo()
{
	times=0;	
	int loopStopper=0;
	while (loopStopper<=10) {
		occlusionDetection();
		possibilityElimination();
		transitiveClosure();
		loopStopper++;
	}
for (int i=0; i <= maxID; i++) {
	for (int j=0; j <= maxID; j++) {
		if(compare(i,j)==2)
			times++;

	}

}


	if(verbose)
		reportResults();




	return times;
}


void CApproximator::checkAccuracy(CScene scene)
{

	
	
	for (int i=1; i <= maxID; i++) {
		CRegApprox reg=region[i];
		if (reg==null)
			continue;
		CRect rect=scene.getByID(i);
		//printf("again: expected id=%d actual id=%d\n",i,rect.color.getId());
		if(rect.vel.x>reg.maxVel||rect.vel.x<reg.minVel){


			printf("i=%d rect=%d\n",i,rect.color.getId());
			printf("ERROR! This velocity is false: %d <= %d <= %d\n",(int)reg.minVel,(int)rect.vel.x,(int)reg.maxVel);
			throw CError("Velocity incorrect: %d",(int)rect.vel.x);
		}

		

		for(int j=1; j<=maxID;j++){


			//printf("j=%d\n",j);
			CRect rect2=scene.getByID(j);

			//printf("again: expected id=%d actual id=%d\n",j,rect2.color.getId());


			if(compare(i,j)==-2&&rect.z<rect2.z){
				verbose=true;
				printf("ERROR! ");
				p(i);
				printf(" not behind ");
				p(j);
				printf("!\n");
				throw CError("Relative Distances incorrect");
			}
			if(compare(i,j)==2&&rect.z>rect2.z){
				verbose=true;
				printf("ERROR! ");
				p(i);
				printf(" doesn't beat ");
				p(j);
				printf("!\n");
				throw CError("Relative Distances incorrect");
			}
		}

			if(compare(0,i)==2){
				verbose=true;
				printf("ERROR! ");
				p(i);
				printf(" doesn't beat ");
				p(0);
               // printf("         j=%d\n",j);

				throw CError("Nothing behind white");

		}
		}
	

if(verbose)
	printf("All true\n");

}