#pragma once

#include <boost/shared_ptr.hpp>
#include <vector>
#include <gl/GL.h>
#include <glut.h>
#include <cv.h>
#include <wx/wx.h>
#include <string>
#include <sstream>
#include "model.h"

#include "FramePipeline.h"
#include "Capture.h"
#include "tsai.h"

#define POINTS 48
#define NEAR_PLANE 10
#define FAR_PLANE 1000
#define BOARD_WIDTH 8
#define BOARD_HEIGHT 6
#define ROTATION_MATRIX_SIZE 9
#define TRANSLATION_VECTOR_SIZE 3
#define MODELVIEW_PROJECTION_SIZE 16
#define SQUARE_SIZE 2.5

using namespace std;
using namespace boost;

class SimpleFramePipeline : public FramePipeline
{
 private:
	int elapsedFrames, fps;
	wxStopWatch timer;
	wxStopWatch updateTimer;
	int frameProcessingTimeAvg;

 	bool mustDrawPreProcessedImage,
	 mustDrawPatternPoints,
	mustDrawOcclusionWireframe,
	mustDrawAxis;

	bool initial;

	Model model;


 public:
 
	SimpleFramePipeline(void);

	~SimpleFramePipeline(void);
 
	virtual void execute(shared_ptr<Capture> capture);


 	shared_ptr<IplImage> preProcessingPhase(shared_ptr<Capture> capture) {
  		IplImage *src = capture->getCurrentFrame();
  		
  		shared_ptr<IplImage> r(cvCreateImage( cvGetSize(src), IPL_DEPTH_8U, 1 ),releaseImage);
  		shared_ptr<IplImage> g(cvCreateImage( cvGetSize(src), IPL_DEPTH_8U, 1 ),releaseImage);
  		shared_ptr<IplImage> b(cvCreateImage( cvGetSize(src), IPL_DEPTH_8U, 1 ),releaseImage);
  		
  		cvSplit( src, r.get(), g.get(), b.get(), NULL );
  		
  		shared_ptr<IplImage> s(cvCreateImage( cvGetSize(src), IPL_DEPTH_8U, 1 ),releaseImage);
  		
  		cvAddWeighted( r.get(), 1./3., g.get(), 1./3., 0.0, s.get() );
 		cvAddWeighted( s.get(), 2./3., b.get(), 1./3., 0.0, s.get() );

		return s;
 	}
 
 	int findPatternPhase(shared_ptr<Capture> capture, IplImage * preProcessedImage, vector<CvPoint2D32f> & patternPoints, int & numPointsFound )
 	{
 		int flags=CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_NORMALIZE_IMAGE;

 		int allFound = cvFindChessboardCorners(preProcessedImage, cvSize(BOARD_WIDTH,BOARD_HEIGHT),&patternPoints.front(),&numPointsFound,flags);
		
		cvFindCornerSubPix(preProcessedImage,&patternPoints.front(),numPointsFound,cvSize(4,4),cvSize(1,1),cvTermCriteria(CV_TERMCRIT_EPS,5,0.1f));

		if(mustDrawPatternPoints)
			cvDrawChessboardCorners(preProcessedImage,cvSize(BOARD_WIDTH,BOARD_HEIGHT),&patternPoints.front(),numPointsFound,allFound);

		return allFound;
 	}
 
 	void cameraCalibrationPhase(shared_ptr<Capture> capture, vector<CvPoint2D32f> & patternPoints, int numPointsFound, int allFound) {
 		double camPosX, camPosY, camPosZ, f;
 
 		int width = capture->getWidth();
 		int height = capture->getHeight();

		static double worldX[POINTS], worldY[POINTS], screenX[POINTS], screenY[POINTS],
		newWorldX[POINTS], newWorldY[POINTS], newScreenX[POINTS], newScreenY[POINTS] ;
		static double rotation[ROTATION_MATRIX_SIZE], translation[TRANSLATION_VECTOR_SIZE], 
			modelview[MODELVIEW_PROJECTION_SIZE], projection[MODELVIEW_PROJECTION_SIZE] ;
 
 		if (allFound!=0){
 			for (int i=0; i<numPointsFound; ++i)	{		
 				screenX[i]=patternPoints[i].x - (width/2);
 				screenY[i]=patternPoints[i].y - (height/2);
 			}
 
 			int z=0;
 			for (int i=0; i<BOARD_HEIGHT;i++){
 				for (int j=0;j<BOARD_WIDTH;j++){
 					worldX[z] = j* SQUARE_SIZE;
 					worldY[z] = i* SQUARE_SIZE;
 					z++;
 				}	
 			}
			
			tsaiBuildNewData(numPointsFound,worldX,worldY,screenX,screenY,BOARD_WIDTH,BOARD_HEIGHT,newWorldX,newWorldY,newScreenX,newScreenY);
			if(initial)
 				tsaiCalibration2D(numPointsFound,newWorldX, newWorldY, newScreenX, newScreenY, &f,translation,rotation);
			else
				tsaiCalibration2D_Tr(numPointsFound,newWorldX, newWorldY, newScreenX, newScreenY, f,translation,rotation);

 			tsaiCameraPosition(&camPosX, &camPosY, &camPosZ, translation, rotation);
 			tsaiOGLModelViewMatrix(translation,rotation,modelview);
 			tsaiOGLProjectionMatrix(f, NEAR_PLANE, FAR_PLANE, width, height, projection);

			glMatrixMode(GL_PROJECTION);	
			glLoadIdentity();
			glLoadMatrixd(projection);	
			glMatrixMode(GL_MODELVIEW);	
			glLoadIdentity();
			glLoadMatrixd(modelview);
		}
 	}
 
 	void drawBackgroundPhase(shared_ptr<Capture> capture, IplImage * preProcessedImage) {
 		if(mustDrawPreProcessedImage)
			glDrawPixels(capture->getWidth(),capture->getHeight(),GL_LUMINANCE, GL_UNSIGNED_BYTE, preProcessedImage->imageData);
 		else
 			glDrawPixels(capture->getWidth(),capture->getHeight(),GL_BGR_EXT, GL_UNSIGNED_BYTE, capture->getCurrentFrame()->imageData);
 	}
 
 	void occlusionPhase() {
		glPushAttrib(GL_COLOR_BUFFER_BIT | GL_ENABLE_BIT);
		glPushMatrix();

			glEnable(GL_DEPTH_TEST);
			glTranslatef(0.0f,12.5f,0.0f);
			glColorMask(false,false,false,true);

			glPushMatrix();
				glBegin(GL_QUADS);

				#if 0
 					glVertex3i(0,5,0);
 					glVertex3i(-24,5,0);
 					glVertex3i(-24,5,13);
 					glVertex3i(0,5,13);
 
 					glVertex3i(0,5,0);
 					glVertex3i(0,5,13);
 					glVertex3i(0,24,13);
 					glVertex3i(0,24,0);
 					
 					glVertex3i(0,5,13);
 					glVertex3i(-24,5,13);
 					glVertex3i(-24,24,13);
 					glVertex3i(0,24,13);
				#endif

				glVertex3f(0.0f,5.0f,0.0f);
				glVertex3f(-24.0f,5.0f,0.0f);
				glVertex3f(-24.0f,5.0f,7.5f);
				glVertex3f(0.0f,5.0f,7.5f);

				glVertex3f(0.0f,5.0f,0.0f);
				glVertex3f(0.0f,5.0f,7.5f);
				glVertex3f(0,22.0f,7.5f);
				glVertex3f(0,22.0f,0.0f);

				glVertex3f(0.0f,5.0f,7.5f);
				glVertex3f(-24.0f,5.0f,7.5f);
				glVertex3f(-24.0f,22.0f,7.5f);
				glVertex3f(0.0f,22.0f,7.5f);
				glEnd();
			glPopMatrix();
		glPopMatrix();
		glPopAttrib();
 	}
 
 	void drawScenePhase() {

		glPushMatrix();
			glTranslatef(0.0f,12.5f,0.0f);

			drawAxis();

			glPushAttrib(GL_ENABLE_BIT);
				glEnable(GL_DEPTH_TEST);	
				glEnable(GL_CULL_FACE);
				glEnable(GL_LIGHTING);
				glEnable(GL_LIGHT0);
				glEnable(GL_AUTO_NORMAL);
				glEnable(GL_NORMALIZE);
				glFrontFace(GL_CW);
				glCullFace(GL_BACK);
				glMaterialf (GL_FRONT, GL_SHININESS, 64.0);

					glPushMatrix();		
						glTranslatef(-15.0f,30.0f,0.0f);
						glRotatef(90.0f,1.0f,0.0f,0.0f);
						glutSolidTeapot(5.0f);
					glPopMatrix();


			glPopAttrib();

			glPushAttrib(GL_ALL_ATTRIB_BITS);
/*
				glEnable(GL_LIGHTING);
				glEnable(GL_LIGHT0);
				glEnable(GL_DEPTH_TEST);*/

				glPushMatrix();
					glScalef(0.1f,0.1f,0.1f);
					float rotation = model.getRotation();
					float translation = model.getTranslation();

					glRotatef(rotation,0.0f,0.0f,1.0f);
					glTranslatef(translation,0.0f,0.0f);
					model.setState(model.STATE_IDLE,0.3f);
					model.onRender();
				glPopMatrix();
			glPopAttrib();

			draw2D();

		glPopMatrix();

 	}

	void draw2D() {
		glPushAttrib(GL_ALL_ATTRIB_BITS);

			int vp[4];
			glGetIntegerv(GL_VIEWPORT,vp);
			glMatrixMode(GL_PROJECTION);
			glPushMatrix();
			glLoadIdentity();
			gluOrtho2D(0,vp[2],vp[3],0);

			glMatrixMode(GL_MODELVIEW);
			glPushMatrix();
			glLoadIdentity();
			glRasterPos2f(20.0, 20.0);

			stringstream sstr;
			sstr << "Tempo medio por frame: " << frameProcessingTimeAvg << " ms";
			
			glColor3i(1,1,1);

			for (int i = 0; i < sstr.str().length(); i++)
				glutBitmapCharacter(GLUT_BITMAP_HELVETICA_12,sstr.str()[i]);

			glRasterPos2f(20.0, 40.0);

			sstr.str("");
			sstr << fps << " FPS";

			for (int i = 0; i < sstr.str().length(); i++)
				glutBitmapCharacter(GLUT_BITMAP_HELVETICA_12,sstr.str()[i]);

			glPopMatrix();

			glMatrixMode(GL_PROJECTION);
			glPopMatrix();

		glPopAttrib();
	}

	void drawAxis()
	{
		glPushAttrib(GL_COLOR_BUFFER_BIT);
			if(mustDrawAxis) {
				glLineWidth(2.0f);
				glBegin(GL_LINES);
					glColor3f(1.0f,0.0f,0.0f);
					glVertex4f(0.0f,0.0f,0.0f,1.0f);
					glVertex4f(1.0f,0.0f,0.0f,0.0f);

					glColor3f(0.0f,1.0f,0.0f);
					glVertex4f(0.0f,0.0f,0.0f,1.0f);
					glVertex4f(0.0f,1.0f,0.0f,0.0f);

					glColor3f(0.0f,0.0f,1.0f);
					glVertex4f(0.0f,0.0f,0.0f,1.0f);
					glVertex4f(0.0f,0.0f,1.0f,0.0f);
				glEnd();
			}

			if(mustDrawOcclusionWireframe) {

				glColor3f(0,0.5f,0.5f);
				glBegin(GL_LINE_STRIP);
					glVertex3f(0.0f,5.0f,0.0f);
					glVertex3f(-24.0f,5.0f,0.0f);
					glVertex3f(-24.0f,5.0f,7.5f);
					glVertex3f(0.0f,5.0f,7.5f);
				glEnd();

				glBegin(GL_LINE_STRIP);
					glVertex3f(0.0f,5.0f,0.0f);
					glVertex3f(0.0f,5.0f,7.5f);
					glVertex3f(0,22.0f,7.5f);
					glVertex3f(0,22.0f,0.0f);
				glEnd();

				glBegin(GL_LINE_STRIP);
					glVertex3f(0.0f,5.0f,7.5f);
					glVertex3f(-24.0f,5.0f,7.5f);
					glVertex3f(-24.0f,22.0f,7.5f);
					glVertex3f(0.0f,22.0f,7.5f);
				glEnd();

				#if 0
	 			glBegin(GL_LINE_STRIP);
	 				glVertex3i(0,5,0);
	 				glVertex3i(-24,5,0);
	 				glVertex3i(-24,5,13);
	 				glVertex3i(0,5,13);
	 			glEnd();
	 			glBegin(GL_LINE_STRIP);
	 				glVertex3i(0,5,0);
	 				glVertex3i(0,5,13);
	 				glVertex3i(0,24,13);
	 				glVertex3i(0,24,0);
	 			glEnd();
	 			glBegin(GL_LINE_STRIP);
	 				glVertex3i(0,5,13);
	 				glVertex3i(-24,5,13);
	 				glVertex3i(-24,24,13);
	 				glVertex3i(0,24,13);
	 			glEnd();
				#endif
			}
		glPopAttrib();
	}

 	virtual void init();
 
 	void setDrawPreProcessedImage(bool val) { mustDrawPreProcessedImage = val; }

	bool isDrawOcclusionWireframe() const { return mustDrawOcclusionWireframe; }
	void setDrawOcclusionWireframe(bool val) { mustDrawOcclusionWireframe = val; }

	bool isDrawAxis() const { return mustDrawAxis; }
	void setDrawAxis(bool val) { mustDrawAxis = val; }

	Model & getModel() { return model; }

};

