//  PhotoAnalysis.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 "PhotoAnalysis.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 MINLEN 0
#define MAXLEN MAX

#define MAXSIMILARITY 55.0f


///////////////////////////////////////////////////////////////////////////
// class CRegPhotoAnalysis - a single region




CRegPhotoAnalysis::CRegPhotoAnalysis(int iid, const CPhotoAnalysis* master)
{
	id = iid;
	
	maxVelocity=master->maxVelocity;
	minVelocity=master->minVelocity;

	minVelGuess=(float)minVelocity;
	maxVelGuess=(float)maxVelocity;

	minVelData=new int[10*(maxVelocity-minVelocity)+1];
	maxVelData=new int[10*(maxVelocity-minVelocity)+1];

	for(int i=0;i<=10*(maxVelocity-minVelocity);i++){
		minVelData[i]=0;
	}
	for(int i=0;i<=10*(maxVelocity-minVelocity);i++){
		maxVelData[i]=0;
	}
	minLen=MINLEN;
	maxLen=MAXLEN;
	nFrames=master->nFrames;
	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 CRegPhotoAnalysis::deleteOld()
{
	minLen=MINLEN;
	maxLen=MAXLEN;
	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;
	}
}

void CRegPhotoAnalysis::velLessThan(float max, int odds)
{
	int maxx=ROUND(10*(max-minVelocity));
	if(maxx<0||maxx>10*(maxVelocity-minVelocity)){
		return;
	}
	for(maxx++; maxx<=10*(maxVelocity-minVelocity); maxx++){
		maxVelData[maxx]+=odds;
	}
	guessVelocity();
}

void CRegPhotoAnalysis::velMoreThan(float min, int odds)
{
	int minn=ROUND(10*(min-minVelocity));
	if(minn>10*(maxVelocity-minVelocity)||minn<0){
		return;
	}
	for(minn--; minn>=0; minn--){
		minVelData[minn]+=odds;
	}
	guessVelocity();
}

void CRegPhotoAnalysis::guessVelocity()
{
	int first=0;
	while(minVelData[first]>maxVelData[first]&&first<10*(maxVelocity-minVelocity)){
		first++;
	}
	int last=first;
	while(minVelData[last]>=maxVelData[last]&&last<10*(maxVelocity-minVelocity)){
		last++;
	}
	minVelGuess=minVelocity+(((float)first)/10.0f);
	maxVelGuess=minVelocity+(((float)last)/10.0f);
}

void CRegPhotoAnalysis::limitV(int f1, int f2, float*minA, float*maxA){
	float minV=(maxA[f1]-minA[f2])/(float)(f1-f2);
	float maxV=(minA[f1]-maxA[f2])/(float)(f1-f2);
	velLessThan(maxV, 1);
	velMoreThan(minV,1);
}

void CRegPhotoAnalysis::restrictVel(){
	for(int f1=0; f1<nFrames-1;f1++){
		for(int f2=f1+1; f2<nFrames;f2++){
			limitV(f1,f2,minmin,maxmin);//restrict velocity by position
			limitV(f1,f2,minmax,maxmax);
		}
	}
}

//Curiously, this seems to be more harm than help. At least for stereo scenes.
/*
void CRegPhotoAnalysis::restrictLen(){
	if(minLen==maxLen)
		return;
	for(int f=0; f<nFrames;f++){
		if(maxmin[f]!=MAX&&minmax[f]!=MAX)
			max(minLen,minmax[f]-maxmin[f]);//restrict length by position
		if(maxmax[f]!=MAX&&minmin[f]!=MAX)
			min(maxLen,maxmax[f]-minmin[f]);
	}
}

void CRegPhotoAnalysis::limitP(int f1, int f2, float*&minA, float*&maxA){
	float minX1=minA[f2]+maxVelGuess*(f1-f2);
	float maxX1=maxA[f2]+minVelGuess*(f1-f2);
	float minX2=minA[f1]+minVelGuess*(f2-f1);
	float maxX2=maxA[f1]+maxVelGuess*(f2-f1);
	max(minA[f1],minX1);
	min(maxA[f1],maxX1);
	max(minA[f2],minX2);
	min(maxA[f2],maxX2);
}


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

*/


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



///////////////////////////////////////////////////////////////////////////
// class CPhotoAnalysis - all regions

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

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


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


int CPhotoAnalysis::compare(int a,int b)   
{
	if(relative[a][b]>9000&&relative[b][a]>9000)
		return 0;
	return relative[a][b]-relative[b][a];
}

//sets high priority closer
void CPhotoAnalysis::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)
			//return false;
		relative[f][c]-=odds;
		//return true;
	}else{
	relative[c][f]+=odds;
	}
}

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

	int oldMax=maxID;
	maxID=a;
	CRegPhotoAnalysis * b=new CRegPhotoAnalysis[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;
}

int CPhotoAnalysis::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 CPhotoAnalysis::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);
		if(region[a]==null){
			region[a]=CRegPhotoAnalysis(a,this);
			nRegions++;
		}
		return a;
}

void CPhotoAnalysis::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 CPhotoAnalysis::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 CPhotoAnalysis::startRegion(int id, int frame, int x, int prevID)
{
	if(region[id].maxmin[frame]==MAX){
		region[id].maxmin[frame]=(float)x+0.5f;
	}
}

// record right edge of current region
void CPhotoAnalysis::endRegion(int id, int frame, int x, int 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 CPhotoAnalysis::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 CPhotoAnalysis::possibilityElimination(){
	for (int i=0; i <= maxID; i++) {
		//CRegPhotoAnalysis r=region[i];
		if (region[i]==null)
			continue;
		//region[i].restrictPosition();
		region[i].restrictVel();
		//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 CPhotoAnalysis::occlusionDetection()
{
	for (int i=0; i <= maxID; i++) {
		CRegPhotoAnalysis 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,10);	
				}
			}
		}
	}
}


void CPhotoAnalysis::stereoRestriction()
{
	for (int i=0; i <= maxID; i++) {
		CRegPhotoAnalysis r1=region[i];
		if (r1==null)
			continue;
		for (int j=0; j <= maxID; j++) {
			CRegPhotoAnalysis r2=region[j];
			if (i==j||r2==null)
				continue;
			if(r1.minVelGuess>r2.maxVelGuess){
				setCloser(i,j,20);
			}
			if(compare(i,j)>8){
				region[i].velMoreThan(r2.minVelGuess,1);
				region[j].velLessThan(r1.minVelGuess,1);
			}
		}
	}
}

//like it sounds
void CPhotoAnalysis::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)
				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 CPhotoAnalysis::reportResults()
{
	printf("\nResults: %d regions observed\n", nRegions);
	for (int i=0; i <= maxID; i++) {
		CRegPhotoAnalysis r=region[i];
		if (r==null)
			continue;
		printf("   ");
		p(i);
		printf(" vel:");
		justify(r.minVelGuess);
		if(r.minVelGuess==r.maxVelGuess){
			printf("=");
		}else{
			printf("~");
		}
		justify(r.maxVelGuess);		
		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 CPhotoAnalysis::goGoGo(CByteImage* imgs, bool stereo, int iminVelocity, int imaxVelocity)
{
	minVelocity=iminVelocity;
	maxVelocity=imaxVelocity;
	times=0;
	if(verbose){
		printf("Possibility Elimination and Occlusion Detection\n");
	}
	for(int row=0;row<imgs[0].Shape().height;row+=10){

		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++;
		}
	}

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

	for(int i=0;i<3000;i++){
		if(stereo){
			stereoRestriction();
		}
		transitiveClosure();
	}
	if(verbose)
		reportResults();
	return times;
}