#include<vector>
#include<opencv\cv.h>
#include<opencv\highgui.h>
#include<opencv2/imgproc/imgproc.hpp>
#include<cstdlib>
#include<iostream>
#include<omp.h>
//#include<glut.h>
#include<GL/freeglut.h>
#include<GL/gl.h>
#include<GL/glu.h>
#include<GL/glut.h>
#include"reconstruction.h"

using namespace cv;
using namespace std;

bool finish=false;

//Called when a key is pressed
void handleKeypress(unsigned char key, //The key that was pressed
                    int x, int y) {    //The current mouse coordinates
    switch (key) {
        case 27: //Escape key
    		finish=true;
    }
}

//Initializes 3D rendering
void initRendering() {
    //Makes 3D drawing work when something is in front of something else
    glEnable(GL_DEPTH_TEST);
	glEnable(GL_COLOR_MATERIAL);
	glEnable(GL_LIGHTING); //Enable lighting
	glEnable(GL_LIGHT0); //Enable light #0
	glEnable(GL_LIGHT1); //Enable light #1
	glEnable(GL_NORMALIZE); //Automatically normalize normals
	glShadeModel(GL_SMOOTH); //Enable smooth shading
}

//Called when the window is resized
void handleResize(int w, int h) {
    //Tell OpenGL how to convert from coordinates to pixel values
    glViewport(0, 0, w, h);
    
    glMatrixMode(GL_PROJECTION); //Switch to setting the camera perspective
    
    //Set the camera perspective
    glLoadIdentity(); //Reset the camera
    gluPerspective(45.0,                  //The camera angle
                   (double)w / (double)h, //The width-to-height ratio
                   1.0,                   //The near z clipping coordinate
                   200.0);                //The far z clipping coordinate
}

float _angle = -70.0f;
vector<Point3f> vertexpoints;
float lenx=0.0,leny=0.0,lenz=0.0;
const float camlen=480.0;
const float camwidth=640.0;
bool finalrender;

//Draws the 3D scene
void drawScene() {
    //Clear information from last draw
	float vx=0.0,vy=0.0,vz=0.0,size=0.0;
	float vertsize=float(vertexpoints.size());
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
	
	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();
	
	glTranslatef(0.0f, 0.0f, -8.0f);
	
	//Add ambient light
	GLfloat ambientColor[] = {0.2f, 0.2f, 0.2f, 1.0f}; //Color (0.2, 0.2, 0.2)
	glLightModelfv(GL_LIGHT_MODEL_AMBIENT, ambientColor);
	
	//Add positioned light
	GLfloat lightColor0[] = {0.5f, 0.5f, 0.5f, 1.0f}; //Color (0.5, 0.5, 0.5)
	GLfloat lightPos0[] = {4.0f, 0.0f, 8.0f, 1.0f}; //Positioned at (4, 0, 8)
	glLightfv(GL_LIGHT0, GL_DIFFUSE, lightColor0);
	glLightfv(GL_LIGHT0, GL_POSITION, lightPos0);
	
	//Add directed light
	GLfloat lightColor1[] = {0.5f, 0.2f, 0.2f, 1.0f}; //Color (0.5, 0.2, 0.2)
	//Coming from the direction (-1, 0.5, 0.5)
	GLfloat lightPos1[] = {-1.0f, 0.5f, 0.5f, 0.0f};
	glLightfv(GL_LIGHT1, GL_DIFFUSE, lightColor1);
	glLightfv(GL_LIGHT1, GL_POSITION, lightPos1);
	// dont need if only one shape glPushMatrix();  //opengl uses matrices, saves the transform state
	
	glRotatef(_angle, 0.0f, 1.0f, 0.0f);	//rotate by 70 degree anticlockwise
	glRotatef(-90.0f, 1.0f, 0.0f, 0.0f);
	glColor3f(1.0f, 1.0f, 0.0f);	//set color of shape orangey
	if(finalrender==false)
	{
		glBegin(GL_POINTS); //Begin points coordinates		
		for(int v=0;v<vertsize;v++)
		{
			vx=float(vertexpoints[v].x);
			vy=float(vertexpoints[v].y);
			vz=float(vertexpoints[v].z);
			glVertex3f((-1.5+(3.0*(vx/lenx))),(-1.0+2.0*(vy/leny)),(-1.5+(3.0*(vz/lenz))));
			//idea behind this is to limit the size between -1.5 to 1.5 for x and z direction and -1 to 1 for y direction
		}
		
		glEnd(); //End points coordinates
	}
	else
	{
		glBegin(GL_TRIANGLE_STRIP); //Begin points coordinates		
		for(int v=0;v<vertsize;v++)
		{
			vx=float(vertexpoints[v].x);
			vy=float(vertexpoints[v].y);
			vz=float(vertexpoints[v].z);
			glVertex3f((-1.5+(3.0*(vx/lenx))),(-1.0+2.0*(vy/leny)),(-1.5+(3.0*(vz/lenz))));
			//idea behind this is to limit the size between -1.5 to 1.5 for x and z direction and -1 to 1 for y direction
		}
		
		glEnd(); //End points coordinates
	}
	//glPopMatrix(); // restore transform state	dont need it as only shape being drawn
	
	glutSwapBuffers(); //Send the 3D scene to the screen
}

void Update(int value)
{
	_angle+=2.0f; //increase by 20 degrees 
	if(_angle > 360){
		_angle -= 360;
	}

	glutPostRedisplay(); //redisplay new picture
	glutTimerFunc(25, Update, 0); //call update function every 25ms.decrease and it will call it quicker and will rotate faster
}

bool PtInRect(float x, float y)
{
	if((x<camwidth)&&(x>0)&&(y<camlen)&&(y>0))
	{
		return true;
	}
	else return false;
}
							
static int win;

int main(int argc, char** argv)
{
	glutInit(&argc, argv);
	glutInitDisplayMode(GLUT_DOUBLE|GLUT_RGB|GLUT_DEPTH);
	glutInitWindowSize(400,400);

	Mat image;
	Mat Rot, Tran;
	int numcorners;
	
	Mat icovar;
	Scalar meanmat;
	double covar[3][3]={{35.2189, 146.3495, 105.9640},{146.3495,801.1402,527.6974},{105.9640,527.6974,553.3654}};
	meanmat[0]=15.5662;
	meanmat[1]=118.3597;
	meanmat[2]=48.5153;

	Mat covmat(3,3,CV_64F,covar);
	
	Mat mask = Mat::zeros(camlen, camwidth, CV_8UC1);	//create matrix same size as image which is 480 by 640 based on the webcam capture
	icovar=inversemat(covmat);		//determinant of covariance matrix is zero. SOLVED
	
	float distance = 250;
	int elemsize=3;
	
	Mat element = getStructuringElement(0, Size( 2*elemsize + 1, 2*elemsize+1 ), Point( elemsize, elemsize ) );
	
	Mat corners;
	cout<<"Enter number of corners to detect (must be greater than 4) e.g 5: "<<endl;
    cin>>numcorners;
		
	vector<vector<Point3f>> object_points;
    vector<vector<Point2f>> image_points;

	vector<Point3f> obj;
	vector<Point2f> img;
    
	vector<Point3f> threedpoint;
	vector<Point2f> projectedpoints;
	
	Mat intrinsic = Mat(3, 3, CV_32FC1);
	Mat distCoeffs;
	vector<Mat> rvecs;
	vector<Mat> tvecs;
	
	intrinsic.ptr<float>(0)[0] = 1;
	intrinsic.ptr<float>(1)[1] = 1;
	float check=0.0;
	Mat silhouette;
	int objtemp=0;
	float xmax=580,xmin=80,ymin=190;
	VideoCapture webcam;
	webcam.open(-1);	
	
	bool render=false;

	//distance for note purpose
	//rectangle horizontally dot to dot 2620 vertically 1750mm
	//square horizontally dot to do 1733 vertically 1750mm
	cout<<"Enter the distance between the two marked corners in the x direction (mm): "<<endl;
	cin>>lenx;
	cout<<"---------------------------------------------------------------------"<<endl;
	cout<<"NOTE:Make sure you Adjust the parameters of the camera i.e. brightness\nto the required amount. Press 'r' when ready to render and press 's' to generate the final 3D model"<<endl;
	cout<<"---------------------------------------------------------------------"<<endl;
	cout<<"Enter the distance between the two marked corners in the y direction (mm): "<<endl;
	cin>>leny;
	cout<<"Enter the height of the object with extra 1cm for space (mm): "<<endl;
	cin>>lenz;

	int sz[] = {lenx,leny,lenz};
	Mat threedimension(3,sz,CV_32F,Scalar::all(1.0));  //create 3dim matrix, type 32 filled with 1s.
	//cout<<threedimension.at<float>(0,0,4);		// but its not coordinates its elements so you are accessing elements in a 3d array not finding y value
	                                                //you are getting the value in the first row, first column and 4th element along z axis
													//to access x value of point 3f of ith element it is a[i].x; easy.	                                            
												//same with point2f
	//cout<<threedimension.at<float>(0,0,0);    //now it works, had to do float not ints.
	if(!webcam.isOpened())
	{
		cout<<"\nThe Camera is being used by another application, make sure all applications using the camera are closed and try running this program again."<<endl;
		system("PAUSE");
		return 0;
	}

	obj.push_back(Point3f(0,0,0));
	obj.push_back(Point3f(lenx,0,0));
	obj.push_back(Point3f(0,leny,0));
	obj.push_back(Point3f(lenx,leny,0));
	
	win=glutCreateWindow("Temporary Visual of 3D Model");
	initRendering();
	while(1)
	{
		//copy webcam stream to image
		webcam>>image;
		glutKeyboardFunc(handleKeypress);
		glutReshapeFunc(handleResize);
		int key=waitKey(1);
		if(key=='r'){render=true;}
		#pragma omp parallel sections
		{
			#pragma omp section
			{silhouette=imagesegmentation(image,icovar,meanmat,distance,mask,element); }
			#pragma omp section
			{corners=Cornerdetect(image,corners,numcorners);}
		}
		if(corners.rows>4)
		{
			//#pragma omp parallel for
			for(int i=(corners.rows-4);i<corners.rows;i++)
			{
				if((corners.at<float>(i,0)>xmin)&&(corners.at<float>(i,1)>ymin)&&(corners.at<float>(i,0)<xmax))
				{
					//draws circle on image, at centre at point, color, thickness, line type, 
					circle(image,corners.at<Point2f>(i),3,CV_RGB(255,0,0),1,8,0);
					//obj.push_back(Point3f(float(objtemp/2), float(objtemp%2), 0.0f));		//setting up the units of calibration
					img.push_back(corners.at<Point2f>(i));		
					objtemp++;
				}
			}
			if(objtemp==4)
			{
				image_points.push_back(img);
				object_points.push_back(obj);
				calibrateCamera(object_points, image_points, image.size(), intrinsic, distCoeffs, rvecs, tvecs); 
				Rot=rvecs[0];
				Tran=tvecs[0];
								
				if(render)
				{
					//#pragma omp parallel for	//unhandled exception error
					for(int l=0;l<int(lenx);l++)
					{
						for(int w=0;w<int(leny);w++)
						{
							for(int h=0;h<int(lenz);h++)
							{
								threedpoint.push_back(Point3f(l,w,h));		
							}
						}
					}
				
					projectPoints(threedpoint,Rot,Tran,intrinsic,distCoeffs,projectedpoints);
					for(int index=0;index<projectedpoints.size();index++)
					{
						if(PtInRect(projectedpoints[index].x,projectedpoints[index].y))
						{
						//access each element of threedimension.at<float>(x,y,z)
							float dx=threedpoint[index].x, dy=threedpoint[index].y,dz=threedpoint[index].z;
							check = threedimension.at<float>(dx,dy,dz);
							if(check==1.0)   //-1 rows and -1 cols check into it, solved cannot check the matrix directly so assign it to a variable then check
							{
								vertexpoints.push_back(Point3f(dx,dy,dz));
								if(float(mask.at<uchar>(projectedpoints[index]))==255.0)
								{
									
									threedimension.at<float>(dx,dy,dz)=0.0;
								}
								
							}
							else if((check==0.0)&&(int(mask.at<uchar>(projectedpoints[index]))==0))
							{
								threedimension.at<float>(dx,dy,dz)=1.0;
								vertexpoints.push_back(Point3f(dx,dy,dz));
							}
						}
					}
					glutDisplayFunc(drawScene);
					glutTimerFunc(25, Update, 0); //call update function every 25ms.decrease and it will call it quicker and will rotate faster
					glutMainLoopEvent();
					
				}
				
				imshow("original", image);
				waitKey(30);
				imshow("mask",mask);
				waitKey(30);			//this is to give the processor some time to display the image
				//rendering over here with the displays, loop through each points which is a one to get a list of vectors (vertexpoints) then draw using those points by looping through it
				//after rendering clear it so it doesn't stack on top		
			}
		}
		if(finish==false){vertexpoints.clear();}
		objtemp=0;
		img.clear();
		image_points.clear();
		object_points.clear();
		if(finish){break;}
		//from here you can specifiy your own glutmainloop event which ones to do, this will do one iteration and continue through
		//out the while loop hopefully
	}
	
	webcam.release();
	destroyWindow("original");
	destroyWindow("mask");
	cout<<"The Final 3D Model is Being Rendered and will be displayed Shortly."<<endl;
	finalrender=true;
	//finalrender is for switching between different rendering 
	//check to see if any of the neighbour voxels are zero and then just push those vertices to the vertex and render triangle fan
	//we have the final 3d array
	
	for(int l=0;l<int(lenx);l++)
	{
		for(int w=0;w<int(leny);w++)
		{
			for(int h=0;h<int(lenz);h++)
			{
				if(threedimension.at<float>(l,w,h)==1)
				{
					//check if above, left or right, behind or infront is zero it is an edge. no diagonals that would be inside.
					//obviously no edge at the bottom so leave the bottom as it is. so check for w is 0 if it is at the bottom of the model, then push the vertex to the draw program
					if((threedimension.at<float>(l+1,w,h)==0)||(threedimension.at<float>(l-1,w,h)==0)||(threedimension.at<float>(l,w+1,h)==0)||(threedimension.at<float>(l,w,h)==0)||(threedimension.at<float>(l,w,h+1)==0)||(threedimension.at<float>(l,w,h-1)==0)||w==0)
					{
						vertexpoints.push_back(Point3f(l,w,h));
					}
				}
			}
		}
	}

	glutReshapeFunc(handleResize);
	glutDisplayFunc(drawScene);
	glutTimerFunc(25, Update, 0); //call update function every 25ms.decrease and it will call it quicker and will rotate faster
	glutMainLoop();
	return 0;
}