/*****************************************************************************
 * ass5lighting.cpp
 *****************************************************************************
 *
 * Description: Assignment 5 (COMPSCI775)
 *
 * Topic: Structured Lighting
 *
 * (c) 2002 Christian Graf, Uli Schroeder, YongTao Zou (Group 7)
 *
 *****************************************************************************
 */

#include <fstream.h>
#include "HalconCpp.h"
#include <math.h>
#include <stdlib.h>
#include <String.h>

int calibMatrix[256][5], reconstMatrix[256][256][36];
int xyProjection[256][256], zyProjection[256][256], xzProjection[256][256];
double lookupTable[256];

void calibration() {
	// search maximum positions in calibration images 
	double calibTableZ[5] = {-120,-84,-48,-12,24};
	double maxXPos[5];
	int maxVal, curVal;
	for (int i = 0; i < 5; i++) {
		maxVal = 0;
		for (int x = 0; x < 256; x++) {
			curVal = calibMatrix[x][i];
			if (curVal > maxVal) {
				maxVal = curVal;
				maxXPos[i] = x;
			}
		}
		// cout << "x=" << maxXPos[i] << endl;
	}
	// build lookup table with linear interpolation
	double x1 = maxXPos[0]+maxXPos[1];
	double x2 = maxXPos[2]+maxXPos[3]+ maxXPos[4];
	double z1 = calibTableZ[0]+calibTableZ[1];
	double z2 = calibTableZ[2]+calibTableZ[3]+calibTableZ[4];
	double a = (3*z1-2*z2)/(3*x1-2*x2);
	double b = z2-z1-a*(x2-x1);
	for (int i = 0; i < 256; i++) {
		lookupTable[i] = a*i+b;
		// cout << "lookup[" << i <<"]=" << lookupTable[i] << endl;
	}
} // end calibration

void nonMaximaSupression() {
	for (int j = 0; j < 36; j++) {
		for (int y = 0; y < 256; y++) {
			for (int x = 1; x < 255; x++) {
				if((reconstMatrix[x+1][y][j] >= reconstMatrix[x][y][j]) || (reconstMatrix[x-1][y][j] > reconstMatrix[x][y][j])) {
					reconstMatrix[x][y][j] = 0;
				}
			}
  		}
  	}
}

void reconstProjections(String aFilename) {
	double k = 3.14159/180*10;
	double xNew, yNew, zNew, zDist;
	for (int y = 0; y < 256; y++) {
		for (int x = 0; x < 256; x++) {
			for (int j = 0; j < 36; j++) {
				if (reconstMatrix[x][y][j] > 100) {
					zDist = lookupTable[x];
					xNew = zDist*cos(j*k);
					yNew = lookupTable[y]/3;
					zNew = zDist*sin(j*k);
					if (xNew <= 0) {
						xNew=128-abs(xNew);
					} else {
						xNew+=128;
					}
					if (zNew <= 0) {
						zNew=128-abs(zNew);
					} else {
						zNew+=128;
					}
					xyProjection[(int)xNew][(int)y] = 255;
					zyProjection[(int)zNew][(int)y] = 255;
					xzProjection[(int)xNew][(int)zNew] = 255;
				}
			}
  		}
  	}
	ofstream outXyPPM, outZyPPM, outXzPPM;
	outXyPPM.open(aFilename+"XY.ppm");
	outZyPPM.open(aFilename+"ZY.ppm");
	outXzPPM.open(aFilename+"XZ.ppm");
	outXyPPM << "P3\n";
	outXyPPM << "# (c) 2002 Christian Graf, Uli Schroeder, YongTao Zou (Group 7)\n";
	outXyPPM << "256 256\n";
	outXyPPM << "255\n";
	outZyPPM << "P3\n";
	outZyPPM << "# (c) 2002 Christian Graf, Uli Schroeder, YongTao Zou (Group 7)\n";
	outZyPPM << "256 256\n";
	outZyPPM << "255\n";
	outXzPPM << "P3\n";
	outXzPPM << "# (c) 2002 Christian Graf, Uli Schroeder, YongTao Zou (Group 7)\n";
	outXzPPM << "256 256\n";
	outXzPPM << "255\n";
	for (int y = 0; y < 256; y++) {
		for (int x = 0; x < 256; x++) {
			outXyPPM << " " << xyProjection[x][y] << " " << xyProjection[x][y] << " " << xyProjection[x][y];
			outZyPPM << " " << zyProjection[x][y] << " " << zyProjection[x][y] << " " << zyProjection[x][y];
			outXzPPM << " " << xzProjection[x][y] << " " << xzProjection[x][y] << " " << xzProjection[x][y];
			if (x%4 == 0) {
				outXyPPM << "\n";
				outZyPPM << "\n";
				outXzPPM << "\n";
			}
		}
	}
	outXyPPM.close();
	outZyPPM.close();
	outXzPPM.close();
}

void reconstVRML(String aFilename) {
	double k = 3.14159/180*10;
	double xNew, yNew, zNew, zDist;
	ofstream outVRMLSpheres, outVRMLPoints, outXyPPM, outZyPPM, outXzPPM;
	outVRMLSpheres.open(aFilename+"S.wrl");
	outVRMLPoints.open(aFilename+"P.wrl");
	outVRMLSpheres << "#VRML V2.0 utf8\n\n";
	outVRMLSpheres << "WorldInfo {\n";
	outVRMLSpheres << "	title \"775Ass5-G7\"\n";
	outVRMLSpheres << "	info [ \"(c) 2002 Christian Graf, Uli Schroeder, YongTao Zou (Group 7)\" ]\n";
	outVRMLSpheres << "}\n\n";
	outVRMLSpheres << "Viewpoint {\n";
	outVRMLSpheres << "	position  0 0 150\n";
	outVRMLSpheres << "}\n\n";
	outVRMLSpheres << "Background {\n";
	outVRMLSpheres << "	skyColor .05 .05 .15\n";
	outVRMLSpheres << "}\n\n";
	outVRMLSpheres << "NavigationInfo {\n";
	outVRMLSpheres << "	type \"EXAMINE\"\n";
	outVRMLSpheres << "}\n\n";
	outVRMLSpheres << "Transform {\n";
	outVRMLSpheres << "	children [\n";
	outVRMLSpheres << "		Transform {\n";
	outVRMLSpheres << "			translation 0 0 0\n";
	outVRMLSpheres << "			children [\n";
	outVRMLSpheres << "				Shape {\n";
	outVRMLSpheres << "					appearance DEF myMat Appearance {\n";
	outVRMLSpheres << "						material Material {\n";
	outVRMLSpheres << "							specularColor .1 .1 .1\n";
	outVRMLSpheres << "							diffuseColor .05 .7 .05\n";
	outVRMLSpheres << "							shininess 1\n";
	outVRMLSpheres << "							ambientIntensity .8\n";
	outVRMLSpheres << "						}\n";
	outVRMLSpheres << "					}\n";
	outVRMLSpheres << "					geometry DEF mySph Sphere {\n";
	outVRMLSpheres << "						radius .25\n";
	outVRMLSpheres << "					}\n";
	outVRMLSpheres << "				}\n";
	outVRMLSpheres << "			]\n";
	outVRMLSpheres << "		}\n";
	outVRMLPoints << "#VRML V2.0 utf8\n\n";
	outVRMLPoints << "WorldInfo {\n";
	outVRMLPoints << "	title \"775Ass5-G7\"\n";
	outVRMLPoints << "	info [ \"(c) 2002 Christian Graf, Uli Schroeder, YongTao Zou (Group 7)\" ]\n";
	outVRMLPoints << "}\n\n";
	outVRMLPoints << "Viewpoint {\n";
	outVRMLPoints << "	position  0 0 150\n";
	outVRMLPoints << "}\n\n";
	outVRMLPoints << "Background {\n";
	outVRMLPoints << "	skyColor .05 .05 .15\n";
	outVRMLPoints << "}\n\n";
	outVRMLPoints << "NavigationInfo {\n";
	outVRMLPoints << "	type \"EXAMINE\"\n";
	outVRMLPoints << "}\n\n";
	outVRMLPoints << "Shape {\n";
	outVRMLPoints << "	appearance Appearance {\n";
	outVRMLPoints << "		material Material {\n";
	outVRMLPoints << "			emissiveColor .2 1 .2\n";
	outVRMLPoints << "		}\n";
	outVRMLPoints << "	}\n";
	outVRMLPoints << "	geometry PointSet {\n";
	outVRMLPoints << "		coord DEF mypts Coordinate {\n";
	outVRMLPoints << "			point [\n";
	for (int y = 0; y < 256; y++) {
		for (int x = 0; x < 256; x++) {
			for (int j = 0; j < 36; j++) {
				if (reconstMatrix[x][y][j] > 100) {
					zDist = lookupTable[x];
					xNew = zDist*cos(j*k);
					yNew = lookupTable[y]/3;
					zNew = zDist*sin(j*k);
					outVRMLSpheres << "		Transform {\n";
					outVRMLSpheres << "			translation " << xNew << " " << yNew << " " << zNew << "\n";
					outVRMLSpheres << "			children [\n";
					outVRMLSpheres << "				Shape {\n";
					outVRMLSpheres << "					appearance USE myMat\n";
					outVRMLSpheres << "					geometry USE mySph\n";
					outVRMLSpheres << "				}\n";
					outVRMLSpheres << "			]\n";
					outVRMLSpheres << "		}\n";
					outVRMLPoints << "				" << xNew << " " << yNew << " " << zNew << ",\n";
				}
			}
  		}
  	}
	outVRMLSpheres << "	]\n";
	outVRMLSpheres << "}\n";
	outVRMLPoints << "			]\n";
	outVRMLPoints << "		}\n";
	outVRMLPoints << "	}\n";
	outVRMLPoints << "}\n";
	outVRMLSpheres.close();
	outVRMLPoints.close();
}

int main(int argc, char* argv[]) {
	HImage myImage;
	// show informational header
	cout << "----------------------------------" << endl;
	cout << " Assignment 5 (COMPSCI775):       " << endl;
	cout << " Structured Lighting              " << endl;
	cout << "----------------------------------" << endl;
	cout << " Group 7:                         " << endl;
	cout << " Christian Graf                   " << endl;
	cout << " Uli Schroeder                    " << endl;
	cout << " YongTao Zou                      " << endl;
	cout << "----------------------------------" << endl;
	// check for correct number of command line arguments and show help if neccessary
	if (argc != 3) {
		cout << " Wrong number of arguments!" << endl << endl;
		cout << " ./ass5lighting [calibfiles] [reconfiles]" << endl << endl;
		cout << "    [calibfiles]  basic name of calibration images (not the numbers and .bmp)" << endl;
		cout << "    [reconfiles]  basic name of reconstruction images (not the numbers and .bmp)" << endl;
		cout << " Example:" << endl;
		cout << "   ./ass5lighting calibplane teapot" << endl << endl;
		return(-1);
	}
	// do calibration
	cout << "Reading calibration images..." << endl;
	for (int i = 0; i < 5; i++) {
		myImage = HImage((String)argv[1] + (String)(49+i) + ".bmp");
		for (int x = 0; x < 256; x++) {
			calibMatrix[x][i] = myImage.GetPixVal(x, 128);
		}
		// cout << "Reading " << ((String)argv[1] + (String)(49+i) + ".bmp") << "..." << endl;
	}
	cout << "Starting calibration..." << endl;
	calibration();
	// do reconstruction
	cout << "Reading image set..." << endl;
	for (int i = 0; i < 4; i++) {
		for (int j = 0; j < 10; j++) {
			if (i*10+j < 37) {
				if ((i == 0) && (j == 0)) { j++; }
				myImage = HImage((String)argv[2] + (String)(48+i) + (String)(48+j) + ".bmp");
				for (int y = 0; y < 256; y++) {
					for (int x = 0; x < 256; x++) {
						reconstMatrix[x][y][i*10+j-1] = myImage.GetPixVal(x, y);
					}
				}
				// cout << "Reading " << ((String)argv[2] + (String)(48+i) + (String)(48+j) + ".bmp") << "..." << endl;
			}
		}
	}
	cout << "Starting reconstruction..." << endl;
	// supress bright pixels that are not a maximum
	nonMaximaSupression();
	reconstVRML((String)argv[2]);
	reconstProjections((String)argv[2]);
	cout << "Finished processing..." << endl << endl;
	cout << " The End!" << endl << endl << endl;
	return(0);
}