// ............................................................... 3dcamera.cc
//   27.MAY.2015

#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <time.h>
#include <corso.h>

extern XVideoData xvd;

double func(double x,double y,double *par);

main()
{
	int di,dj,i,icol,ii,im1,ip1,ix,ix0,ixold,ixstart,iy0,iy,iyold,iystart,j,
	jm1,jp1,l,k,kk,nComm,nPar,nX,nY,nTot,skip,xc,yc;
	int *ind,*ParFont;
	unsigned long *color;
	double alpha,alphadeg,beta,betadeg,DeltaX,DeltaY,depth,dist,dz,focal,
	gamma,gammadeg,length,x,xLen,xScale,y,yLen,yScale,zHigh,zLow,zScale;
	double rnew[3],rold[3];
	const double deg2rad=M_PI/180.0;
	double *par;
	double **NewPt,**OldPt,**rotmat;
	char **comandi,**ParName,**ParVal;
	clock_t musec1,musec2;
	timespec ts1,ts2;
	XColor col;
	XPoint ptswap;
	XPoint pt2d[5];
	
	// ............................................................. modo grafico
	StartXWindow(800,600,0,0,"3D Camera",2);
	XSelectInput(xvd.display,xvd.win[0],ButtonPressMask|ButtonReleaseMask|
			PointerMotionMask|KeyPressMask|KeyReleaseMask|ExposureMask|
					StructureNotifyMask);
	xc=xvd.width[0]/2;
	yc=xvd.height[0]/2;
	// .......................................................... numero di punti
	nX=50;
	nY=50;
	nTot=nX*nY;
	xLen=300.0;
	yLen=300.0;
	dist=20000.0;
	focal=1000.0;
	DeltaX=2.0*xLen/double(nX);
	DeltaY=2.0*yLen/double(nY);
	// ...................................................... allocazione memoria
	color=new unsigned long [101];
	ind=new int [nTot];
	rotmat=new double * [3];
	for (i=0;i<3;i++) rotmat[i]=new double[3];
	NewPt=new double * [nTot];
	for (i=0;i<nTot;i++) NewPt[i]=new double [3];
	OldPt=new double * [nTot];
	for (i=0;i<nTot;i++) OldPt[i]=new double [3];
	// ............................................................ tavola colori
	/*
	for (i=0;i<100;i++)
	{
		col.red=int(65535.0*double(100.0-double(i))/100.0);
		col.green=int(65535.0*(50.0-double(abs(i-50)))/50.0);
		col.blue=int(65535.0*double(i)/100.0);
		XAllocColor(xvd.display,xvd.colormap,&col);
		color[i]=col.pixel;
	}
	*/
	for (i=0;i<50;i++)
	{
		col.red=int(65535.0*double(50.0-double(i))/50.0);
		col.green=int(65535.0*double(i)/50.0);
		col.blue=0;
		XAllocColor(xvd.display,xvd.colormap,&col);
		color[i]=col.pixel;
	}
	for (i=0;i<50;i++)
	{
		col.red=0;
		col.green=int(65535.0*double(50.0-double(i))/50.0);
		col.blue=int(65535.0*double(i)/50.0);
		XAllocColor(xvd.display,xvd.colormap,&col);
		color[50+i]=col.pixel;
	}
	// ............................................. allocazione memoria per menu'
	nPar=13;
	ParName=new char * [nPar];
	ParVal=new char * [nPar];
	for (i=0;i<nPar;i++) ParVal[i]=new char [24];
	ParName[0]="Origine x";
	ParName[1]="Origine y";
	ParName[2]="a";
	ParName[3]="b";
	ParName[4]="g";
	ParName[5]="a";
	ParName[6]="b";
	ParName[7]="c";
	ParName[8]="Distanza";
	ParName[9]="Focale";
	ParName[10]="Scala X";
	ParName[11]="Scala Y";
	ParName[12]="Scala Z";
	nComm=1;
	comandi=new char * [nComm];
	comandi[0]="Fine";
	ParFont=new int [nPar];
	for (i=0;i<nPar;i++) ParFont[i]=1;
	ParFont[2]=ParFont[3]=ParFont[4]=5;
	// .......................................................... allocate arrays
	par=new double [3];
	// ................................................................. costanti
	par[0]=1.0; //      a
	par[1]=0.0; //      b
	par[2]=0.0; //      c
	// ................................................. origine delle coordinate
	ix0=400;
	iy0=500;
	xScale=15.0;
	yScale=15.0;
	zScale=15.0;
	// ................................................................... angoli
	alphadeg=0.0;
	betadeg=0.0;
	gammadeg=0.0;
	// ....................................................... parametri per menu
	sprintf(ParVal[0],"%.d",ix0);
	sprintf(ParVal[1],"%.d",xvd.height[0]-iy0);
	sprintf(ParVal[2],"%.4lf",alphadeg);
	sprintf(ParVal[3],"%.4lf",betadeg);
	sprintf(ParVal[4],"%.4lf",gammadeg);
	sprintf(ParVal[5],"%.4lf",par[0]);
	sprintf(ParVal[6],"%.4lf",par[1]);
	sprintf(ParVal[7],"%.4lf",par[2]);
	sprintf(ParVal[8],"%.0lf",dist);
	sprintf(ParVal[9],"%.0lf",focal);
	sprintf(ParVal[10],"%.4lf",xScale);
	sprintf(ParVal[11],"%.4lf",yScale);
	sprintf(ParVal[12],"%.4lf",zScale);
	// .......................................................... disegna il menu
	XWinPermMenu(1,"3D Plot",ParFont,ParName,ParVal,nPar,16,comandi,nComm,0);
	// .............................................................. sincronizza
	XSync(xvd.display,TRUE);
	// .................................................................... ciclo
	ts1.tv_sec=0;
	for (;;)
	{
		XSync(xvd.display,FALSE);
		musec1=clock();
		// .......................................................... leggi il menu
		ii=XWinPermMenu(1,"3D Plot",ParFont,ParName,ParVal,nPar,16,comandi,nComm,1);
		if (ii==0) break;
		//if (ii==nComm)
		gammadeg+=0.1;
		if (TRUE)
		{ // ......................................................... leggi i dati
			ix0=atoi(ParVal[0]);
			iy0=xvd.height[0]-atoi(ParVal[1]);
			alphadeg=atof(ParVal[2]);
			betadeg=atof(ParVal[3]);
			//gammadeg=atof(ParVal[2]);
			par[0]=atof(ParVal[5]);
			par[1]=atof(ParVal[6]);
			par[2]=atof(ParVal[7]);
			dist=atof(ParVal[8]);
			focal=atof(ParVal[9]);
			xScale=atof(ParVal[10]);
			yScale=atof(ParVal[11]);
			zScale=atof(ParVal[12]);
			// .................................................... gradi -> radianti
			alpha=alphadeg*deg2rad;
			beta=betadeg*deg2rad;
			gamma=gammadeg*deg2rad;
			// ................................................. matrice di rotazione
			rotmat[0][0]=cos(gamma)*cos(beta);
			rotmat[0][1]=-sin(gamma)*cos(beta);
			rotmat[0][2]=sin(beta);
			rotmat[1][0]=cos(gamma)*sin(beta)*sin(alpha)+sin(gamma)*cos(alpha);
			rotmat[1][1]=cos(gamma)*cos(alpha)-sin(gamma)*sin(beta)*sin(alpha);
			rotmat[1][2]=-cos(beta)*sin(alpha);
			rotmat[2][0]=sin(gamma)*sin(alpha)-cos(gamma)*sin(beta)*cos(alpha);
			rotmat[2][1]=sin(gamma)*sin(beta)*cos(alpha)+sin(alpha)*cos(gamma);
			rotmat[2][2]=cos(beta)*cos(alpha);
			// ........................................... coordinate tridimensionali
			ii=0;
			for (i=0;i<nX;i++)
			{
				x=double(i)*DeltaX-xLen;
				for (j=0;j<nY;j++)
				{
					y=double(j)*DeltaY-yLen;
					OldPt[ii][0]=x*xScale;
					OldPt[ii][1]=y*yScale;
					OldPt[ii][2]=func(x,y,par)*zScale;
					if (ii==0) zLow=zHigh=OldPt[ii][2];
					else
					{
						if (zLow>OldPt[ii][2]) zLow=OldPt[ii][2];
						else if (zHigh<OldPt[ii][2]) zHigh=OldPt[ii][2];
					}
					// ........................................................ rotazione
					MatVectMult(rotmat,OldPt[ii],NewPt[ii],3);
					ii++;
				}
			}
			// ...................................................... altezza massima
			dz=zHigh-zLow;
			// ............................................... ordina i punti ruotati
			for (i=0;i<nTot;i++) ind[i]=i;
			VectDnPtSort(NewPt,ind,nTot,1);
			// .................................................. pulisci la finestra
			XSetForeground(xvd.display,xvd.gc,xvd.white);
			XFillRectangle(xvd.display,xvd.backpix[0],xvd.gc,0,0,800,600);
			// .............................................................. disegno
			for (l=0;l<nTot;l++)
			{
				ii=ind[l];
				// ....................................................... coordinata x
				i=ii/nY;
				if (i==0) continue;
				// ....................................................... coordinata y
				j=ii%nY;
				if (j==0) continue;
				// ....................................................................
				depth=NewPt[ii][1];
				//if ((dist+depth)<0.0) continue;
				// ................................................... proietta i punti
				kk=0;
				skip=FALSE;
				for (di=0;di>=-1;di--) for (dj=0;dj>=-1;dj--)
				{
					k=(i+di)*nY+(j+dj);
					/*
					pt2d[kk].x=ix0+int(NewPt[k][0]*dist/(dist+NewPt[k][1])+0.5);
					pt2d[kk].y=iy0-int(NewPt[k][2]*dist/(dist+NewPt[k][1])+0.5);
					*/
					pt2d[kk].x=xc+int((NewPt[k][0]+double(ix0-xc))
							*focal/(dist+NewPt[k][1])+0.5);
					pt2d[kk].y=yc+int((-NewPt[k][2]-double(yc-iy0))
							*focal/(dist+NewPt[k][1])+0.5);
					if ((dist+NewPt[k][1])<=1.0) skip=TRUE;
					kk++;
				}
				if (skip ) continue;
				ptswap=pt2d[2];pt2d[2]=pt2d[3];pt2d[3]=ptswap;
				pt2d[4]=pt2d[0];
				// ................................................ pulisci il poligono
				if (dz==0) XSetForeground(xvd.display,xvd.gc,xvd.white);
				else
				{
					icol=int(99.0*(OldPt[ii][2]-zLow)/dz);
					XSetForeground(xvd.display,xvd.gc,color[icol]);
				}
				XFillPolygon(xvd.display,xvd.backpix[0],xvd.gc,pt2d,4,Convex,
										 CoordModeOrigin);
				// ................................................ disegna il reticolo
				XSetForeground(xvd.display,xvd.gc,xvd.black);
				XDrawLines(xvd.display,xvd.backpix[0],xvd.gc,pt2d,5,
									 CoordModeOrigin);
			}
			// ..................................................... disegna gli assi
			XSetForeground(xvd.display,xvd.gc,xvd.blue[2]);
			// ............................................................... asse x
			rold[0]=500.0;rold[1]=0.0;rold[2]=0.0;
			MatVectMult(rotmat,rold,rnew,3);
			ix=ix0+int(rnew[0]+0.5);
			iy=iy0-int(rnew[2]+0.5);
			XDrawLine(xvd.display,xvd.backpix[0],xvd.gc,ix,iy,ix0,iy0);
			// ............................................................... asse y
			rold[0]=0.0;rold[1]=500.0;rold[2]=0.0;
			MatVectMult(rotmat,rold,rnew,3);
			ix=ix0+int(rnew[0]+0.5);
			iy=iy0-int(rnew[2]+0.5);
			XDrawLine(xvd.display,xvd.backpix[0],xvd.gc,ix,iy,ix0,iy0);
			// ............................................................... asse z
			rold[0]=0.0;rold[1]=0.0;rold[2]=500.0;
			MatVectMult(rotmat,rold,rnew,3);
			ix=ix0+int(rnew[0]+0.5);
			iy=iy0-int(rnew[2]+0.5);
			XDrawLine(xvd.display,xvd.backpix[0],xvd.gc,ix,iy,ix0,iy0);
		}
		// ...................................................... copia sul monitor
		XCopyArea(xvd.display,xvd.backpix[0],xvd.win[0],xvd.gc,0,0,800,600,0,0);
		// ................................................................ aspetta
		musec2=clock();
		ts1.tv_nsec=1000000-(musec2-musec1)*1000; 
		if (ts1.tv_nsec>0) nanosleep(&ts1,&ts2);
	}
	// ............................................................... erase menu
	XWinPermMenu(1,"3D Plot",ParFont,ParName,ParVal,nPar,16,comandi,nComm,2);
	// ............................................................. clear memory
	delete [] ParFont;
	delete [] par;
	delete [] comandi;
	for (i=nPar-1;i>=0;i--) delete ParVal[i];
	delete [] ParVal;
	delete [] ParName;
	delete [] OldPt;
	delete [] NewPt;
	for (i=2;i>=0;i--) delete [] rotmat[i];
	delete [] rotmat;
	delete [] ind;
	delete [] color;
	// ....................................................... chiudi le finestre
	CloseXWindow();
}

// ----------------------------------------------------------------------------

double func(double x,double y,double *par)
{
	double a,b,c;
	
	a=par[0];
	b=par[1];
	c=par[2];
	
	return 0.005*(a*(x*x+y*y)+b*x*y*y-c*x*x*x);
	//return 100.0*a*b*b*c*c/((x*x+b*b)*(y*y+c*c));
}