#include <iostream>
#include <string>
#include <algorithm>
#include <fstream>

#include "Helper\Cvh.h"
#include "Helper\Calibration.h"
#include "Camera\CannonCamera.h"
#include "Camera\CameraCalibration.h"
#include "Detection/CheckerDetector.h"
#include "Calibration/CalibrationUnit.h"
#include "Rendering\RenderingUnit.h"

using namespace CALIB;
using namespace PROJ;
using namespace std;

#define CONFIG_FILE "config.txt"
#define WINDOW_CAMERA_NAME "Projector calibration - camera view"
#define WINDOW_PROJECTOR_NAME "Projector calibration - projector view"

/* Window properties */
#define DEF_WIN_WIDTH 1024
#define DEF_WIN_HEIGHT 768
#define DEF_WIN_POS_X 20
#define DEF_WIN_POS_Y 20

#define FULL_WIN_POS_X 1280
#define FULL_WIN_POS_Y 0
#define FULL_WIN_OFFSET_X 0//-8
#define FULL_WIN_OFFSET_Y 0//-30

/*Presunout do parametrů*/
#define PROJECTOR_HORIZONTAL 2
#define PROJECTOR_VERTICAL 2
#define PROJECTOR_WIDTH 800
#define PROJECTOR_HEIGHT 600

/* Chackerboard pattern properties*/
#define CHACKERBOARD_COLS 32//24 -- velikost ctverecku je mocnina dvou
#define CHACKERBOARD_ROWS 24 //18 -- velikost ctverecku je mocnina dvou
#define CHACKERBOARD_ROWS_MIN 3
#define CHACKERBOARD_BORDER_X 16 //1
//#define CHACKERBOARD_BORDER_Y 1 //vypocte se automaticky
#define GRID_STEP_SIZE 50
#define INTENSITY_CALIBRATION_PROJECTOR_LIMIT 2

/* Physical/Printed Chackerboard pattern properties*/
#define CHACKERBOARD_PRINTED_COLS 10
#define CHACKERBOARD_PRINTED_ROWS 7
#define CHACKERBOARD_PRINTED_SQUARE 30

#define IMAGE_DIRECTORY "images"
#define REAL_PROJECTOR_INTENSITY 0.5f
#define INPUT_PROJECTOR_INTENSITY 1.0f

#define VECTORS_DIRECTORY "vec"
#define VECTORS_OUT_DIRECTORY "vecOut"
#define VECTORS_IMG_DIRECTORY "vecImg"
#define VECTORS_IMG_MODIFIED_DIRECTORY "vecImgMod"

//Mesh coordinates filename
#define MESH_COORDINATES_FILENAME "coordinates.csv"
#define MESH_COORDINATES_PROCESSED_FILENAME "result.mat"

int projectorHorizontalCount = PROJECTOR_HORIZONTAL;
int projectorVerticalCount = PROJECTOR_VERTICAL;
int projectorWidth = PROJECTOR_WIDTH;
int projectorHeight = PROJECTOR_HEIGHT;
int projectorWindowPositionX = FULL_WIN_POS_X;
int projectorWindowPositionY = FULL_WIN_POS_Y;
int chRows = CHACKERBOARD_ROWS;
int chCols = CHACKERBOARD_COLS;
int displayWidth;
int displayHeight;
int projectorCount;

//CAMERA window setup
int winCamId;
/** Fullscreen mode for camera window */
bool winCamFullscreen = false;
/** Width of the preview camera window */
int winCamWidth = DEF_WIN_WIDTH;
/** Height of the preview camera window */
int winCamHeight = DEF_WIN_HEIGHT;
/** Scale of camera image. */
geometry::Vector2Df imgCamScale = geometry::Vector2Df(1,1);
/** Projector window id.*/
int winProjId;
/** Fullscreen mode for camera window */
bool winProjFullscreen = false;
/** Width of the projector output window */
int winProjWidth = DEF_WIN_WIDTH;
/** Height of the projector output window */
int winProjHeight = DEF_WIN_HEIGHT;
/** OpenGL main window */
int window = 0;
/**Current proejctor index.*/
int projectorIndex = UPPER_LEFT;
/** Current color channel for luminance calibration. */
int luminanceChannelIndex = 0;
/** Camera image texture. */
GLuint cameraTextureId = -1;
/** Chackerboard texture id. */
GLuint chessTextureId = -1;

CannonCamera * camera;
CameraCalibration * cameraCalibration;
CheckerDetector * detector;
CheckerDetector * detector2; //detector for physical/printed pattern detection
CalibrationUnit * calibrationUnit;
RenderingUnit * renderingUnit;
/** Camera view projector corner points. */
vector<geometry::Vector2Df> points[ProjectorIndex::FULLSCREEN];
/** Application mode **/
int mode = 0;
/** Use keystone correction yes/no **/
bool keystoneCorrection = false;
/** Run calibration process **/
bool runCalibrationProcess = false;
bool readyForNextPicture = true;
/** Show grid for testing. */
bool showGrid = false;
bool prepareLigthMaps = false;

/** Test images  **/
vector<string> testImageFiles;
vector<GLuint> imageTextureIds;
int testImageIndex = 0;
char * chackerboardImageName = "chess.png";
vector<geometry::Vector2Df> stabilizationPoints;
CvMat * stabilizationPointsCv;


/** Test vectors. **/
vector<string> testVectorFiles;
string currentVectorFile;
CvMat * testVectorsHomography;

/** For debug without camera **/
bool cameraOffline = false;

 /** Intensity calibration coeficients **/
float gamma = 0.2;
float intensityValue = 127;
float step = intensityValue;

bool loadImage = false;
CvMat * chackerboardInnerCorners;
int chackerboardCornersCols;
int chackerboardCornersRows;

/** Index of projected pattern. */
enum PatternIndex
{
	CHACKERBOARD = 0,
	BLACK = 1,
	WHITE,
	TEXTURE = 4
};

/** Index of applicaiton mode. */
enum Mode
{
	CALIBRATE_GEOMETRY = 0,
	CALIBRATE_GAMMA = 1,
	CALIBRATE_LUMINANCE = 2,
	CALIBRATE_CAMERA = 3,
	TEST_HOMOGRAPHY = 4,
	TEST_CALIBRATION = 5,
	TEST_INTENSITY = 6,
	TEST_MESH_WARPING = 7,
};

enum LuminanceChannel
{
	R = 0,
	G = 1,
	B = 2
};

/** Current projected pattern.*/
int patternIndex = CHACKERBOARD;

/**
 * Switch on/off fullscreen mode for camera or projector window.
 */
void winSwitchProjFullscreen()
{
	if(winProjFullscreen)
	{
		glutReshapeWindow(DEF_WIN_WIDTH, DEF_WIN_HEIGHT);
		glutPositionWindow(DEF_WIN_POS_X,DEF_WIN_POS_Y);
	}
	else
	{
		glutPositionWindow(projectorWindowPositionX + FULL_WIN_OFFSET_X,projectorWindowPositionY + FULL_WIN_OFFSET_Y);
		glutReshapeWindow(displayWidth ,displayHeight);
	}
	winProjFullscreen = !winProjFullscreen;
}

/**
 * Switch on/off fullscreen mode for camera window
 */
void winSwitchCamFullscreen()
{
	if(winCamFullscreen)
	{
		glutReshapeWindow(DEF_WIN_WIDTH,DEF_WIN_HEIGHT);
		glutPositionWindow(DEF_WIN_POS_X,DEF_WIN_POS_Y);
	}
	else
	  glutFullScreen();
	winCamFullscreen = !winCamFullscreen;
}

/**
 * Switch on/off fullscreen mode for camera or projector window
 */
void switchFullscreen()
{
	if(glutGetWindow() == winProjId)
		winSwitchProjFullscreen();
	else
		winSwitchCamFullscreen();
}

/**
 * Draws pattern made by coded markers.
 */
void DrawTexture(float x1,float y1,float x2,float y2)
{
	if(cameraTextureId != -1)
	{
		GL::DrawTexture(&cameraTextureId,x1,y1,x2,y2);
	}
	return;
}

/**
 * Loads projector corner points from projecotrs
 */
void LoadProjectorCornerPoints()
{
	int i =0;
	for(i = 0;i < projectorCount;i++)
	{
		vector<geometry::Vector2Df> * p = &(points[i]);
		CvMat * cornerPoints = calibrationUnit->GetPoints(i,CAMERA);
		p->clear();
		Cvh::ConvertMatToVector2D(cornerPoints,p);
		cvReleaseMat(&cornerPoints);
	}
}

/**
 * Draw result of camera image processing.
 */
void DrawProcessingResult()
{
	int i =0;
	for(i = 0;i < projectorCount;i++)
	{
		//smazat
		//break;

		glColor3f(0.0f,1.0f,0.0f);
		GL::DrawVector(points[i],imgCamScale,GL_LINE_LOOP);
		//GL::DrawRectangle(points[i].at(0).GetX()*imgCamScale.GetX(),points[i].at(0).GetY()*imgCamScale.GetY(),points[i].at(2).GetX()*imgCamScale.GetX(),points[i].at(2).GetY()*imgCamScale.GetY(),0.0f,0.0f,1.0f);
		glColor3f(1.0f,1.0f,1.0f);
	}
}

/**
 * Draw camera image.
 */
void DrawCam()
{
	DrawTexture(0.0f,0.0f,(float)winCamWidth,(float)winCamHeight);
	DrawProcessingResult();
}

/**
 * Returns next projector number.
 */
int GetNextProjectorIndex()
{
	int index = projectorIndex;
	if(index+1 == projectorCount)
	{
		index = 0;
	}
	else
	{
		index++;
	}
	return index;
}

/**
 * Switch to next projector.
 */
void nextProjector()
{
	projectorIndex = GetNextProjectorIndex();
}

void nextLuminanceChannel()
{
	luminanceChannelIndex++;
	if(luminanceChannelIndex > LuminanceChannel::B)
		luminanceChannelIndex = LuminanceChannel::R;
}

/**
 * Recalculate homographie between projectors.
 */
void CalculateHomography(bool recalibrate = false,int projectorIndex = -1)
{
	if(recalibrate)
		calibrationUnit->ResetCalibration();
	/*prepareLigthMaps = */calibrationUnit->CalibrateDisplay(projectorIndex);
	return;
}

/**
 * Calculate new value for gamma coeficient.
 */
void AdjustIntensityCoeficient(float pointsAverage,float overlapPointAverage)
{
	float newStep = step/2.0f;
	if(newStep < 1)
	{
		float intensity = intensityValue/255.0f;//(float)UCHAR_MAX;
		std::cout << "Final Intensity: " << "(float):" << intensity << std::endl;
		//gamma = log10(intensity)/log10(REAL_PROJECTOR_INTENSITY);
		//gamma = log10(2)/(log10(INPUT_PROJECTOR_INTENSITY)-log10(intensity));
		gamma = (log10(INPUT_PROJECTOR_INTENSITY)-log10(intensity))/log10(2);
		std::cout << "Gamma found!!!" << "Gamma(float):" << gamma << std::endl;
		std::cout << intensity << "	" << gamma << std::endl;

		prepareLigthMaps = true;
		//exit(0);
		return;
	}
	else
		step = round(newStep);

	if(overlapPointAverage > pointsAverage)
	{
		intensityValue -= step;
	}
	else
		intensityValue += step;
}

void ProcessGammaImage(IplImage * image)
{
	IplImage * tmpImage = cvCreateImage( cvSize( image->width, image->height), IPL_DEPTH_8U, 1 );
	// Perform a Gaussian blur ( Convolving with 3x3 //11 X 11 Gaussian).
	cvSmooth( image, tmpImage, CV_GAUSSIAN, 11, 11);
	//cvSaveImage("grayImageGauss.png",tmpImage);

	IplImage * areaImage = calibrationUnit->CombineProjectorAreasIntoImage(INTENSITY_CALIBRATION_PROJECTOR_LIMIT,cvScalar(1),true);
	//cvSaveImage("grayAreaImage.png",areaImage);

	CvScalar pixel;
	CvScalar areaPixel;

	float pointsSum = 0;
	int pointsCounter = 0;
	float overlapPointsSum = 0;
	int overlapPointsCounter = 0;
	int areaType = 0;
	float pixelValue = 0;
	unsigned char aT,pV;

	for(int row = 0;row < areaImage->height;row++)
	{
		int rowSize = row*areaImage->width;
		for(int column = 0;column < areaImage->width;column++)
		{
			//areaPixel = cvGet2D(areaImage,row,column);
			aT = areaImage->imageData[rowSize+column];
			areaType = aT;
			//areaType = areaPixel.val[0];
			if(areaType > 0)
			{
				//pixel = cvGet2D(tmpImage,row,column);
				pV = tmpImage->imageData[rowSize+column];
				pixelValue = pV;
				if(areaType == 1)
				{
					pointsCounter++;
					pointsSum += pixelValue;//pixel.val[0];
				}
				else if(areaType == 2)
				{
					overlapPointsCounter++;
					overlapPointsSum += pixelValue;//pixel.val[0];
				}
			}
		}
	}

	float pointsAverage = pointsSum/(float)pointsCounter;
	float overlapPointAverage = overlapPointsSum/(float)overlapPointsCounter;
	std::cout << "pointsAverage:" << pointsAverage << " overlapPointAverage:" << overlapPointAverage << std::endl;
	AdjustIntensityCoeficient(pointsAverage,overlapPointAverage);
}

void ComputeLUpoint(int projectorIndex,int *x,int *y,bool useMyInverseSetup = false,bool myInverseY = false)
{
	*x = 0;
	*y = 0;
	bool inverseY = true;
	if(projectorVerticalCount <= 1)
		inverseY = false;
	if(useMyInverseSetup)
	{
		inverseY = myInverseY;
	}
	Common::countCoordinates(projectorIndex,projectorWidth,projectorHeight,x,y,inverseY);
}

void ProcessLuminanceImage(IplImage * image)
{
	IplImage * gaussImage = cvCreateImage( cvSize( image->width, image->height), IPL_DEPTH_8U, 1 );
	// Perform a Gaussian blur ( Convolving with 3x3 //11 X 11 Gaussian).
	cvSmooth( image, gaussImage, CV_GAUSSIAN, 11, 11);
	//cvSaveImage("grayImageGauss.png",tmpImage);
	IplImage * tmpImage = Cvh::CreateIplimage(projectorWidth,projectorHeight,NULL,1);
	//obrazek se musi vyriznout

	int currentProjectorCoordinateX = 0;
	int currentProjectorCoordinateY = 0;
	ComputeLUpoint(projectorIndex,&currentProjectorCoordinateX,&currentProjectorCoordinateY,true,false);

	CvMat * hShift = cvCreateMat(3,3,CV_64FC1);
	cvSetIdentity(hShift,cvScalar(1));

	cvSet2D(hShift,0,2,cvScalar(-currentProjectorCoordinateX));
	cvSet2D(hShift,1,2,cvScalar(-currentProjectorCoordinateY));

	CvMat * homography = calibrationUnit->GetHomography(projectorIndex,HomogType::CP);
	CvMat * h = Cvh::MatMul(hShift,homography);

	cvWarpPerspective(gaussImage, tmpImage,h, tmpImage->imageSize);


	float pointsCounter = 0;
	float pointsSum = 0;
	float pixelValue = 0;
	unsigned char aT,pV;
	float min = 255;

	int minRow = tmpImage->height/4;
	int maxRow = minRow*3;
	int minColumn = tmpImage->width/4;
	int maxColumn = minColumn*3;

	for(int row = minRow;row < maxRow;row++)
	{
		int rowSize = row*tmpImage->width;
		for(int column = minColumn;column < maxColumn;column++)
		{
			pV = tmpImage->imageData[rowSize+column];
			pixelValue = pV;
			pointsCounter++;
			pointsSum += pixelValue;
			if(pixelValue < min)
				min = pixelValue;
		}
	}
	float pointsAverage = pointsSum/(float)pointsCounter;

	Projector * p = calibrationUnit->GetProjector(projectorIndex);
	switch(luminanceChannelIndex)
	{
		case LuminanceChannel::R:
			p->SetLuminanceAttenuationR(pointsAverage);
			break;
		case LuminanceChannel::G:
			p->SetLuminanceAttenuationG(pointsAverage);
			break;
		case LuminanceChannel::B:
			p->SetLuminanceAttenuationB(pointsAverage);
			break;
	}

	std::cout << "pointsAverage:" << pointsAverage << " Minimum:"<< min << std::endl;

	cvShowImage("tet",tmpImage);
	cvSaveImage("LuminanceTest.jpg",tmpImage);
	cvReleaseImage(&gaussImage);
	//cvReleaseImage(&tmpImage);
}

bool compensateImageShift(IplImage * img)
{
	IplImage * grayImage = Cvh::convertRGBtoGray(img);

	if(stabilizationPointsCv)
	{
		std::vector<geometry::Vector2Df> * cameraImagePoints = &points[0];
		geometry::Vector2Df p1 = cameraImagePoints->at(0);
		geometry::Vector2Df p2 = cameraImagePoints->at(2);
		cvDrawRect(grayImage,cvPoint(p1.GetX(),p1.GetY()),cvPoint(p2.GetX(),p2.GetY()),cvScalar(0,0,0),CV_FILLED);
	}
	//cvSaveImage("test01a.jpg",grayImage);

	if(detector->Detect(grayImage,6,9))
	{
		detector->DrawCorners(img,6,9);
		vector<geometry::Vector2Df> * detectedPoints = detector->GetPoints();
		if(!stabilizationPointsCv)
		{
			cvSaveImage("test01stab.jpg",img);
			stabilizationPointsCv = Cvh::ConvertVector2DToMat(*detectedPoints);
		}
		else
		{
			CvMat * detectedPointsCv = Cvh::ConvertVector2DToMat(*detectedPoints);

			CvMat * homography = Cvh::CreateMat(3,3);
			Cvh::CalculateHomography(detectedPointsCv,stabilizationPointsCv,homography);

			Cvh::PrintCvMatrix(stabilizationPointsCv,"stabilizationPointsCv");
			Cvh::PrintCvMatrix(detectedPointsCv,"detectedPointsCv");

			IplImage * tmpImage;
			tmpImage = cvCloneImage(img);
			cvZero(img);
			cvSaveImage("test01.jpg",tmpImage);
			cvWarpPerspective(tmpImage,img,homography, tmpImage->imageSize);
			cvSaveImage("test02.jpg",img);
			cvReleaseImage(&tmpImage);
		}
		cvReleaseImage(&grayImage);
		return true;
	}
	cvReleaseImage(&grayImage);
	return false;

}

/**
 * Process captured image.
 * Process algorithm is chosen according to current mode
 */
bool ProcessImage()
{
//	if(!loadImage)
//		return false;
//
//	loadImage = false;
	IplImage * img = camera->GetImage();
	//img = Cvh::LoadImage("IMG_0001.JPG");

	if(img == NULL)
		return false;

	//compensateImageShift(img);

	//img = Cvh::LoadImage("IMG_0004.JPG");

	IplImage * grayImage = Cvh::convertRGBtoGray(img);
	cvSaveImage("grayImage.png",grayImage);
	Projector * projector = calibrationUnit->GetProjector(projectorIndex);

	switch(mode)
	{
		case CALIBRATE_GEOMETRY:
		{
			if(!detector->Detect(grayImage))
				break;
			projector->SetPoints(PointsType::DETECTED,detector->GetPoints());
			detector->DrawCorners(img);

			calibrationUnit->SetCameraImageSize(img->width,img->height);
			if(projector != NULL)
			{
				projector->CalculateHomographyCamera(NONE_INDEX,!keystoneCorrection);
				CalculateHomography();
			}
			LoadProjectorCornerPoints();

			//uncomment to measure displacement vectors
//			testVectorsHomography = Calibration::CalculatePointsShift(&points[0],chackerboardInnerCorners,chackerboardCornersRows,chackerboardCornersCols,MESH_COORDINATES_FILENAME,projector,chRows,chCols,testVectorsHomography);
//			Calibration::AddMotionVectorsIntoImage(projector->GetPointsMeshOriginal(),projector->GetPoints(PointsType::DETECTED),&points[0],testVectorsHomography,img,true);
//			cvSaveImage("test.jpg",img);
			break;
		}
		case TEST_MESH_WARPING:
		{
			if(!detector->Detect(grayImage))
				break;
			 projector->SetPoints(PointsType::DETECTED,detector->GetPoints());

			 string filename = MESH_COORDINATES_FILENAME;
			 string imageFilename = "";

			 if(!currentVectorFile.empty())
			 {
				 filename = currentVectorFile.replace(0,currentVectorFile.find("/"),VECTORS_OUT_DIRECTORY);
				 imageFilename = currentVectorFile.replace(0,currentVectorFile.find("/"),VECTORS_IMG_DIRECTORY);
				 imageFilename = imageFilename.replace(imageFilename.find('.'),4,".jpg");
				 cvSaveImage(imageFilename.c_str(),img);
			 }

			 Calibration::CalculatePointsShift(&points[0],chackerboardInnerCorners,chackerboardCornersRows,chackerboardCornersCols,filename,projector,chRows,chCols,testVectorsHomography);
			 Calibration::AddMotionVectorsIntoImage(projector->GetPointsMeshOriginal(),projector->GetPoints(PointsType::DETECTED),&points[0],testVectorsHomography,img,true);

			 if(!currentVectorFile.empty())
			 {
				 imageFilename = imageFilename.replace(0,imageFilename.find("/"),VECTORS_IMG_MODIFIED_DIRECTORY);
				 cvSaveImage(imageFilename.c_str(),img);
			 }
			 cvSaveImage("testCorrected.jpg",img);
			 break;
		}
		case CALIBRATE_CAMERA:
		{
			vector<string> files;
			files = Common::GetDir("cam",files);

			for(int i = 0;i < files.size();i++)
			{
				IplImage * tmpImg = Cvh::LoadImage(files.at(i).c_str());
				IplImage * gi = Cvh::convertRGBtoGray(tmpImg);
				cameraCalibration->SetImgSize(tmpImg->width,tmpImg->height);
				//cvShowImage("gi",gi);
				detector2->Detect(gi);
				cameraCalibration->AddPointPairs(Cvh::ConvertVector2DToMat(*detector2->GetPoints()));
				cvReleaseImage(&tmpImg);
				cvReleaseImage(&gi);
			}

			cameraCalibration->Run();
			IplImage * test = Cvh::LoadImage("cam/left04.jpg");
			IplImage * gi = Cvh::convertRGBtoGray(test);
			detector2->Detect(gi);
			vector<geometry::Vector2Df> pointsDistorted = vector<geometry::Vector2Df>(*detector2->GetPoints());

			cameraCalibration->UndistortImage(gi);


			detector2->Detect(gi);
			vector<geometry::Vector2Df> pointsUndistorted = vector<geometry::Vector2Df>(*detector2->GetPoints());

			CvMat * pointsUndistortedCv = Cvh::ConvertVector2DToMat(pointsUndistorted);
			CvMat * homography = Cvh::CreateMat(3,3);
			Cvh::CalculateHomography(pointsUndistortedCv,cameraCalibration->GetCorners(),homography);
			vector<geometry::Vector2Df> * originalChessCorners = Cvh::ConvertMatToVector2D(cameraCalibration->GetCorners());

			CvMat * pointsDistortedCv = Cvh::ConvertVector2DToMat(pointsDistorted);
			CvMat * tmpPointsCv = Cvh::CreateMat(pointsDistortedCv->rows,pointsDistortedCv->cols);
			 Cvh::TransformPoints(pointsDistortedCv,tmpPointsCv,homography);
			 vector<geometry::Vector2Df> * detectedChessCorners = Cvh::ConvertMatToVector2D(tmpPointsCv);


			vector<geometry::Vector2Df> pointsMesh;
			Calibration::CalculatePointsShift(originalChessCorners,detectedChessCorners,detector2->GetRows(),detector2->GetColumns(),MESH_COORDINATES_FILENAME,0,0,&pointsMesh);
			Calibration::AddMotionVectorsIntoImage(&pointsUndistorted,&pointsDistorted,test,1);

			cvSaveImage("distortVec.jpg",test);
			cvSaveImage("undistort.jpg",gi);

//			if(!detector2->Detect(grayImage))
//				break;
//
//			cameraCalibration->SetImgSize(img->width,img->height);
//			cameraCalibration->AddPointPairs(Cvh::ConvertVector2DToMat(*detector2->GetPoints()));
			break;
		}
		case CALIBRATE_GAMMA:
			ProcessGammaImage(grayImage);
			break;
		case CALIBRATE_LUMINANCE:
			ProcessLuminanceImage(grayImage);
			break;
	}

	//set camera image scale
	imgCamScale.SetX((float)winCamWidth/(float)img->width);
	imgCamScale.SetY((float)winCamHeight/(float)img->height);

	cameraTextureId = 0;
	GL::LoadTextureFromCvImage(img,&cameraTextureId);
	cvReleaseImage(&img);
	cvReleaseImage(&grayImage);
	return true;
}

/**
 * Draw chackerborad.
 */
void DrawChackerborad(int x,int y,int width,int height,bool withKeystoneCorrecrion = false)
{
	int originX = x;
	int originY = y;

	GL::DrawRectangle(x,y,x+width,y+height,1.0f);

	int borderX = CHACKERBOARD_BORDER_X;
	width -= 2*borderX;
	//border Y
	int size = width/chCols;
	int borderY = (height-(chRows*size))/2;
	//border X
	int border = width%chCols;
	if(border != 0)
	{
		borderX += (border/2);
		width -= border;
	}

	x += borderX;
	y += borderY;

	if(!withKeystoneCorrecrion)
	{
		GL::DrawChackerboard(x,y, size,chRows,chCols);
		chackerboardInnerCorners = Cvh::GetChackerboardCornersAll(x,y, size,chRows,chCols);

		chackerboardCornersCols = chCols+1;
		chackerboardCornersRows = chRows+1;
	}
	else
	{
		CvMat * pProjector = Cvh::GetChackerboardCorners(x,y, size,chRows,chCols);
		//Cvh::PrintCvMatrix(pProjector,"pProjector");

		CvMat * pKeystone = calibrationUnit->TransformPoints(projectorIndex,PK,pProjector);
		//Cvh::PrintCvMatrix(pKeystone,"pKeystone");
		std::vector<geometry::Vector2Df> * pkv = Cvh::ConvertMatToVector2D(pKeystone);
		GL::DrawChackerboard(chRows,chCols,*pkv);
	}

	CvMat * projectedCorners = Cvh::GetChackerboardInnerCorners(x,y, size,chRows,chCols);
	Projector * projector = calibrationUnit->GetProjector(projectorIndex);
	projector->SetPoints(PointsType::PROJECTED,Cvh::ConvertMatToVector2D(projectedCorners));
	cvReleaseMat(&projectedCorners);
}

void DrawStrips(int x,int y,int width,int height)
{
	GL::DrawStrips(x,y,width,height,false,8,16);//4,24);
	GL::DrawStrips(x+width,y,width,height,true,8,8);
}

void DrawPattern(int x,int y,int width,int height,int patternId)
{
	int x1 = x + width;
	int y1 = y + height;
	int borderX,borderY;

	switch(patternId)
	{
		case WHITE:
			GL::DrawRectangle(x,y,x1,y1,1.0f);
			break;
		case BLACK:
			GL::DrawRectangle(x,y,x1,y1,0.0f);
			break;
		case CHACKERBOARD:
			//DrawStrips(x,y,width,height);
			DrawChackerborad(x,y,width,height);
			break;
		case TEXTURE:
			DrawTexture(x,y,x1,y1);
			break;
	}
	return;
}

/**
 * Draws pattern made by coded markers.
 */
void DrawPattern(int x,int y,int width,int height)
{
	DrawPattern(x,y,width,height,patternIndex);
	return;
}

GLuint GetTestImageTextureId();


/**
 * Draw pattern, which is then visualized by projector.
 */
void DrawGeometryCalibrationImages()
{
	int luCornerX = 0; //left upper corner X
	int luCornerY = 0; //left upper corner Y
	int dW = projectorWidth;
	int dH = projectorHeight;
	Common::countCoordinates(projectorIndex,dW,dH,&luCornerX,&luCornerY,false);

	if(!keystoneCorrection)
		DrawPattern(luCornerX,luCornerY,dW,dH);
	else
	{
		if(chessTextureId == -1)
		{
			IplImage * img = NULL;
			img = Cvh::LoadImage(chackerboardImageName);
			GL::LoadTextureFromCvImage(img,&chessTextureId);
		}
		CvMat * pKeystoneCv = calibrationUnit->GetPoints(projectorIndex,PROJECTOR_KEYSTONE);
		vector<geometry::Vector2Df> * pKeystone = Cvh::ConvertMatToVector2D(pKeystoneCv);
		GL::DrawTexture(&chessTextureId,pKeystone);
	}
	glDisable(GL_BLEND);
}

void DrawIntensityCalibrationImages()
{
	int dW = projectorWidth;
	int dH = projectorHeight;

	for(int i = 0;i < INTENSITY_CALIBRATION_PROJECTOR_LIMIT;i++)
	{
		int secondProjectorIndex = i+1;
		if(secondProjectorIndex >= INTENSITY_CALIBRATION_PROJECTOR_LIMIT)
			secondProjectorIndex = i-1;

		int luCornerX = 0; //left upper corner X
		int luCornerY = 0; //left upper corner Y
		Common::countCoordinates(i,dW,dH,&luCornerX,&luCornerY,false);

		GL::DrawRectangle(luCornerX,luCornerY,luCornerX+dW,luCornerY+dH,INPUT_PROJECTOR_INTENSITY);
		//DrawPattern(luCornerX,luCornerY,dW,dH,PatternIndex::WHITE);
		//vykreslit polygon do mista prekryvu
		Projector * p = calibrationUnit->GetProjector(i);
		vector<geometry::Vector2Df> * points = p->GetPolygonPoints(secondProjectorIndex,HomogType::CP);

		float intensity = intensityValue/(float)UCHAR_MAX;
		glColor3f(intensity,intensity,intensity);
		GL::DrawQuad(points);
		glColor3f(1.0f, 1.0f, 1.0f);
	}
}

void DrawLuminanceAttenuationMaps(bool drawRect);
/**
 * Draw pattern, which is then visualized by projector.
 */
void DrawLuminanceCalibrationImages()
{
	int luCornerX = 0; //left upper corner X
	int luCornerY = 0; //left upper corner Y
	int dW = projectorWidth;
	int dH = projectorHeight;
	Common::countCoordinates(projectorIndex,dW,dH,&luCornerX,&luCornerY,false);

	float r,g,b;
	r = g = b = 0;
	switch(luminanceChannelIndex)
	{
		case LuminanceChannel::R:
			r = 1.0f;
			break;
		case LuminanceChannel::G:
			g = 1.0f;
			break;
		case LuminanceChannel::B:
			b = 1.0f;
			break;
	}

	GL::DrawRectangle(luCornerX,luCornerY,luCornerX+dW,luCornerY+dH,r,g,b);
	//DrawLuminanceAttenuationMaps(false);
}

void DrawTestMarkers()
{
	int width = projectorWidth;
	int height = projectorHeight;

	int nextIndex = GetNextProjectorIndex();
	Projector * p1 = calibrationUnit->GetProjector(projectorIndex);
	Projector * p2 = calibrationUnit->GetProjector(nextIndex);
	std::vector<geometry::Vector2Df> * projectedPointsP1 = p1->GetPoints(PROJECTED,nextIndex);
	std::vector<geometry::Vector2Df> * detectedPointsP1 = p1->GetPoints(DETECTED,nextIndex);
	std::vector<geometry::Vector2Df> * projectedPointsP2 = p2->TranformPoints(CPO,detectedPointsP1);

	glColor3f(1,0.5,0);
	GL::DrawMarks(projectedPointsP1);
	glColor3f(0,0.5,1);
	GL::DrawMarks(projectedPointsP2);
}

/**
 * Grabs the current openGL error and displays it
 * on the console (if any).
 */
void DisplayGLError()
{
	GLenum error = glGetError();
	if(error != GL_NO_ERROR)
	{
		printf("GL Error: %s\n", gluErrorString(error));
	}
}

/**
 * Initialize orthographic projection
 * @param width Window width
 * @param height Window height
 */
void initOrthoProjection(int width,int height)
{
	//Reset current setup
	glViewport(0,0,width,height);
	//clears the color and the buffers
	glClearColor(0.0f, 0.0f, 0.0f, 1.0f);
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);


	//------------- CAMERA PIXEL COORDINATES ----------------------
	//sets the projection matrix
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	glOrtho(0, width, height, 0, 0.01, 1000.0);

	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();
	glTranslatef(0.0f, 0.0f, -1.0f);
}

GLuint GetTestImageTextureId()
{
	//load image from primary screen
	GLuint textureId = 1;
	IplImage * img = Common::GetScreeny(L"testScreenshot.jpg", 75);
	//IplImage * img  = Cvh::LoadImage("testScreenshot.jpg");
	glDeleteTextures(1,  &textureId);
	GL::LoadTextureFromCvImage(img,&textureId);
	cvReleaseImage(&img);
	return textureId;

	//load test images
//	GLuint textureId = testImageIndex;
//	if(testImageIndex >= imageTextureIds.size())
//	{
//		IplImage * img = NULL;
//		img = Cvh::LoadImage(testImageFiles.at(testImageIndex).c_str());
//		GL::LoadTextureFromCvImage(img,&textureId);
//		imageTextureIds.push_back(textureId);
//	}
//	else
//		textureId = imageTextureIds.at(testImageIndex);
//
//	return textureId;
}

void InitViewport(int index)
{
	int x = 0;
	int y = 0;
	ComputeLUpoint(index,&x,&y);
	glScissor(x,y,projectorWidth,projectorHeight);
}

void DrawLigthMaps(bool drawRect = false)
{
	int x = 0; //left upper corner X
	int y = 0; //left upper corner Y
	int w = projectorWidth;
	int h = projectorHeight;

	for(int i = 0;i < projectorCount;i++)
	{
		Projector * p = calibrationUnit->GetProjector(i);
		ComputeLUpoint(i,&x,&y,true,false);
		GLuint pt = p->GetLightMapTexture();

		if(drawRect)
		{
			glColor3f(1.0f,1.0f,1.0f);
			glRecti(x,y,x+w,y+h);
		}

		glEnable(GL_BLEND);
		 	 glBlendFunc (GL_ONE_MINUS_SRC_ALPHA,GL_SRC_ALPHA);
			GL::DrawTexture(&pt,x,y,x+w,y+h);

		glDisable(GL_BLEND);
	}
}

void DrawLuminanceAttenuationMaps(bool drawRect = false)
{
	int x = 0; //left upper corner X
	int y = 0; //left upper corner Y
	int w = projectorWidth;
	int h = projectorHeight;

	for(int i = 0;i < projectorCount;i++)
	{
		Projector * p = calibrationUnit->GetProjector(i);
		ComputeLUpoint(i,&x,&y,true,false);
		//GLuint pt = p->GetLamTexture();

		if(drawRect)
		{
			glColor3f(1.0f,1.0f,1.0f);
			glRecti(x,y,x+w,y+h);
		}

		glEnable(GL_BLEND);
		 	 //glBlendFunc (GL_ONE_MINUS_SRC_ALPHA,GL_SRC_ALPHA);
			glBlendFunc(GL_DST_COLOR, GL_ZERO); //multiplication by drawn color
			//GL::DrawTexture(&pt,x,y,x+w,y+h);
			float r,g,b;
			COLOR luminanceAttenuation = p->GetLuminanceAttenuation();

//			float lumR = luminanceAttenuation.R;
//			float lumG = luminanceAttenuation.G;
//			float lumB = luminanceAttenuation.B;

			float lumR = pow(luminanceAttenuation.R,gamma);
			float lumG = pow(luminanceAttenuation.G,gamma);
			float lumB = pow(luminanceAttenuation.B,gamma);

		 	glColor3f(lumR,lumG,lumB);
		 	glRecti(x,y,x+w,y+h);

		glDisable(GL_BLEND);
	}
}

void CreateProjectorLightMaps(IplImage * referenceImage,float gamma)
{
	if(referenceImage == NULL)
		return;

	int x = 0; //left upper corner X
	int y = 0; //left upper corner Y

	//cvSaveImage("referenceImage.jpg",referenceImage);
	for(int i = 0;i < projectorCount;i++)
	{
		Projector * p = calibrationUnit->GetProjector(i);
		ComputeLUpoint(i,&x,&y);
		IplImage * image = GL::TakeScreenshot(x,y,projectorWidth,projectorHeight,false);
		cvSaveImage("p1image.jpg",image);
		IplImage * lightMap = Cvh::DivImages(referenceImage,image,gamma);
		p->SetLightMap(lightMap);
	}
}

void PrepareLightMaps(float gamma)
{
	int x = 0;
	int y = 0;
	IplImage * referenceImage = NULL;
	float maxIntensity = 1.0f;

	glEnable(GL_SCISSOR_TEST);
	for(int i = 0;i < projectorCount;i++)
	{
		InitViewport(i);
		Projector * p = calibrationUnit->GetProjector(i);
		if(p->GetLightMap() != NULL)
			continue;

		//dont invert Y axis -> drawing
		ComputeLUpoint(i,&x,&y,true,false);
		GL::DrawIntensityMap(x,y,projectorWidth,projectorHeight,maxIntensity);
		if(referenceImage == NULL)
		{
			//invert Y axis acording to projector layout -> take a screenshot
			ComputeLUpoint(i,&x,&y);
			referenceImage = GL::TakeScreenshot(x,y,projectorWidth,projectorHeight,false);
			cvSaveImage("referenceImage.jpg",referenceImage);
		}

		std::vector<CvMat*> * matVector = calibrationUnit->GetAdjacentProjectroHomographies(i);

		//adjactent projector index
		int pIndex = 0;
		for(int j = 0;j < matVector->size();j++)
		{
			if(pIndex == i)
				pIndex++;

			glPushMatrix();
			CvMat * h = matVector->at(j);
			GLfloat * transformMat = GL::MatrixCvToGL(h);
			GL::SetupTransformMatrix(transformMat);

			//dont invert Y axis -> drawing
			ComputeLUpoint(pIndex,&x,&y,true,false);
			GL::DrawIntensityMap(x,y,projectorWidth,projectorHeight,maxIntensity);
			glPopMatrix();
			pIndex++;

		}
	}
	glDisable(GL_SCISSOR_TEST);
	CreateProjectorLightMaps(referenceImage,gamma);
}

void PrepareLightMapsCv(float gamma)
{
	int x = 0;
	int y = 0;
	IplImage * referenceImage = Cvh::GetIntensityMap(projectorWidth,projectorHeight);
	IplImage * tmpImage = Cvh::CreateIplimage(projectorWidth,projectorHeight,NULL,1);
	//cvSaveImage("referenceImage.jpg",referenceImage);

	for(int i = 0;i < projectorCount;i++)
	{
		Projector * p = calibrationUnit->GetProjector(i);
		if(p->GetLightMap() != NULL)
			continue;

		ComputeLUpoint(i,&x,&y,true,false);
		int currentProjectorCoordinateX = x;
		int currentProjectorCoordinateY = y;
		IplImage * overlapProjectorsImage = cvCloneImage(referenceImage);

		std::vector<CvMat*> * matVector = calibrationUnit->GetAdjacentProjectroHomographies(i);
		int pIndex = 0;
		for(int j = 0;j < matVector->size();j++)
		{
			if(pIndex == i)
				pIndex++;

			cvZero(tmpImage);
			CvMat * h = matVector->at(j);
			//dont invert Y axis -> drawing
			ComputeLUpoint(pIndex,&x,&y,true,false);

			CvMat * hShift = cvCreateMat(3,3,CV_64FC1);
			cvSetIdentity(hShift,cvScalar(1));

			int shiftX = x-currentProjectorCoordinateX;
			int shiftY = y-currentProjectorCoordinateY;
			cvSet2D(hShift,0,2,cvScalar(shiftX));
			cvSet2D(hShift,1,2,cvScalar(shiftY));

			CvMat * homography;
			if(shiftX > 0)
				homography = Cvh::MatMul(h,hShift);
			else
				homography = Cvh::MatMul(hShift,h);

			cvWarpPerspective(referenceImage, tmpImage,homography, tmpImage->imageSize);
			//cvSaveImage("tmpImage.jpg",tmpImage);
			cvAdd(overlapProjectorsImage,tmpImage,overlapProjectorsImage);
			pIndex++;
		}

		IplImage * lightMap = Cvh::DivImages(referenceImage,overlapProjectorsImage,gamma);
		cvReleaseImage(&overlapProjectorsImage);
		p->SetLightMap(lightMap);
	}
}


/**
 * Draw prewarped image.
 * Utilizes result of calibration.
 */
void DrawPrewarpedImage()
{
	int x1,y1,x2,y2,step;
	step = GRID_STEP_SIZE;
	x1 = 0;
	y1 = 0;
	x2 = displayWidth;//1400;
	y2 = displayHeight;//900;

	vector<geometry::Vector2Df> cornerPoints;
	GL::RectToPoins(x1,y1,x2-x1,y2-y1,&cornerPoints);


	for(int i = 0;i < projectorCount;i++)
	{
		CvMat * homography = calibrationUnit->GetHomography(i,HomogType::DP);
		GLfloat * transformMat = GL::MatrixCvToGL(homography);
		GLuint textureId = GetTestImageTextureId();

		glEnable(GL_SCISSOR_TEST);
		InitViewport(i);
		glPushMatrix();
		GL::SetupTransformMatrix(transformMat);
		GL::DrawTexture(&textureId,&cornerPoints);
		if(showGrid)
			GL::DrawGrid(x1,x2,y1,y2,step,1.0f,1.0f,1.0f);

		glPopMatrix();
		glDisable(GL_SCISSOR_TEST);
	}

	//GL::DrawRectangle(0,0,displayWidth,displayHeight,1.0f);

	DrawLuminanceAttenuationMaps();
	DrawLigthMaps();
}

/**
 * Redisplay projector window
 */
void displayWinProj(void)
{
	initOrthoProjection(winProjWidth,winProjHeight);
	Projector * p = calibrationUnit->GetProjector(projectorIndex);
	switch(mode)
	{
//		case Mode::TEST_MESH_WARPING_FROM_FILE:
//			Calibration::LoadMeshPoints(p,MESH_COORDINATES_PROCESSED_FILENAME);
//			renderingUnit->DrawTextureOnMesh(p,GetTestImageTextureId());
//			break;
		case Mode::TEST_MESH_WARPING:
			//load params from file
			//Calibration::LoadMeshPoints(p,MESH_COORDINATES_FILENAME);
			//load params from file, if some parameter is set
			renderingUnit->DrawTextureOnMesh(p,GetTestImageTextureId());
			break;
		case Mode::TEST_CALIBRATION:
			if(prepareLigthMaps)
			{
				PrepareLightMaps(gamma); //calculate light maps
				prepareLigthMaps = false;
			}
			DrawPrewarpedImage();
			break;
		case Mode::CALIBRATE_GAMMA:
			//vykresli bílé čtverce s tím že v překryvové oblasti musejí být pouze šedé
			//možnost nastavit velikost intenzity v překryvové oblasati
			DrawIntensityCalibrationImages();
			break;
		case Mode::CALIBRATE_LUMINANCE:
			DrawLuminanceCalibrationImages();
			//vykresluje postupne vsechny barevne kanaly pro vsechny projektory
		    //vysledek se nafoti a ulozi se mapa pro kazdy projektor
			break;
		case Mode::TEST_HOMOGRAPHY:
			DrawTestMarkers();
			break;
		case Mode::TEST_INTENSITY:
			prepareLigthMaps = true;
			if(prepareLigthMaps)
			{
				PrepareLightMaps(gamma); //calculate light maps
				prepareLigthMaps = false;
			}
//			PrepareLightMapsCv(gamma);
			DrawLigthMaps(true);
			break;
		case Mode::CALIBRATE_GEOMETRY:
		default:
			DrawGeometryCalibrationImages(); //draw chackerboards
			break;
	}
	DisplayGLError();
	glutSwapBuffers();
}

/**
 * Redisplay Camera window
 */
void displayWinCam(void)
{
	initOrthoProjection(winCamWidth,winCamHeight);
	DrawCam();
	DisplayGLError();
	glutSwapBuffers();
}

/**
 * Redisplay camera an projector windows
 */
static void redisplayWindows()
{
	int currentWindow = glutGetWindow();
	glutPostRedisplay();
	if(currentWindow == winCamId)
		glutSetWindow(winProjId);
	else
		glutSetWindow(winCamId);
	glutPostRedisplay();
	glutSetWindow(currentWindow);
}

/**
 * GLUT idle function.
 * Updates the room model (frame capture, pose estimation ...)
 * and calls the display function.
 */
static void idle()
{
	//sets texture for camera
	glutSetWindow(winCamId);
	bool imageProcessed = ProcessImage();
	if(runCalibrationProcess)
	{
		switch(mode)
		{
			case CALIBRATE_GEOMETRY:
				if(!imageProcessed && readyForNextPicture)
				{
					if(projectorIndex == 0)
						calibrationUnit->ResetCalibration();
					camera->TakePicture();
					loadImage = true;
					readyForNextPicture = false;
				}
				else if(imageProcessed)
				{
					nextProjector();
					if(projectorIndex == 0)
					{
						//mode = CALIBRATE_LUMINANCE;
						mode = CALIBRATE_GAMMA;

						luminanceChannelIndex = 0;
						//mode = CALIBRATE_GAMMA;
						//bez kalibrace intensity
//						prepareLigthMaps = true;
//						mode = TEST_CALIBRATION;
					}
					redisplayWindows();
					readyForNextPicture = true;
				}
				break;
			case CALIBRATE_LUMINANCE:
				if(!imageProcessed && readyForNextPicture)
				{
					camera->TakePicture();
					readyForNextPicture = false;
				}
				else if(imageProcessed)
				{
					nextProjector();
					if(projectorIndex == 0)
					{
						nextLuminanceChannel();
						if(luminanceChannelIndex == LuminanceChannel::R)
						{
							calibrationUnit->CalculateLuminanceAttenuation();
							mode = CALIBRATE_GAMMA;

							//smazar
							mode = TEST_CALIBRATION;
							prepareLigthMaps = true;
						}
					}
					redisplayWindows();
					readyForNextPicture = true;

				}
				break;
			case CALIBRATE_GAMMA:
				if(!imageProcessed && !prepareLigthMaps && readyForNextPicture)
				{
					camera->TakePicture();
					readyForNextPicture = false;
				}
				else if(prepareLigthMaps)
					mode = TEST_CALIBRATION;

				if(imageProcessed)
					readyForNextPicture = true;

				break;
			case TEST_CALIBRATION:
				runCalibrationProcess = false;
				break;
			case TEST_MESH_WARPING:
				if(!imageProcessed && readyForNextPicture && !currentVectorFile.empty())
				{
					camera->TakePicture();
					readyForNextPicture = false;
				}
				else if(imageProcessed || currentVectorFile.empty())
				{
					if(testVectorFiles.empty())
					{
						runCalibrationProcess = false;
						break;
					}

					currentVectorFile = (*testVectorFiles.begin());
					testVectorFiles.erase(testVectorFiles.begin());
					Calibration::LoadMeshPoints(calibrationUnit->GetProjector(projectorIndex),currentVectorFile);
					readyForNextPicture = true;
				}
				break;
		}
	}
	redisplayWindows();
}

bool initTestVectors();
/**
 * GLUT keyboard function.
 * Exits the program on Esc key.
 * @param key The key pressed.
 * @param x The x coordinate of the mouse.
 * @param y The y coordinate of the mouse.
 */
static void keyboard(unsigned char key, int x, int y)
{
	switch(key)
	{
		case 27:
			exit(0);
			break;
		case 'f':
		case 'F':
			switchFullscreen();
		  break;
		case 'n':
		case 'N':
			nextProjector();
		  break;
		case 'p':
		case 'P':
			mode = Mode::TEST_INTENSITY;
		  break;
		case '+':
			if(Common::doubleNum(chRows,true) > projectorWidth)
					break;
				chRows = Common::doubleNum(chRows,true);
				chCols = Common::doubleNum(chCols,true);
				detector->Init(chRows,chCols,0);
				cameraTextureId = -1;
		  break;
		case '-':
				if(chRows <= CHACKERBOARD_ROWS_MIN)
					break;
				chRows = Common::doubleNum(chRows,false);
				chCols = Common::doubleNum(chCols,false);
				detector->Init(chRows,chCols,0);
				cameraTextureId = -1;
		  break;
		case 't':
		case 'T':
			camera->TakePicture();
			loadImage = true;
			break;
		case 'C':
		case 'c':
			mode = Mode::CALIBRATE_GEOMETRY;
			break;
		case 'V':
		case 'v':
			mode = Mode::CALIBRATE_CAMERA;
			break;
		case 'I':
		case 'i':
			mode = Mode::CALIBRATE_GAMMA;
			break;
		case 'L':
		case 'l':
			mode = Mode::CALIBRATE_LUMINANCE;
			break;
		case 'A':
		case 'a':
			mode = Mode::TEST_CALIBRATION;
			break;
		case 'H':
		case 'h':
			mode = Mode::TEST_HOMOGRAPHY;
			break;
		case 'j':
		case 'J':
			Calibration::LoadMeshPoints(calibrationUnit->GetProjector(projectorIndex),MESH_COORDINATES_PROCESSED_FILENAME);
			break;
		case 'm':
		case 'M':
			mode = Mode::TEST_MESH_WARPING;
			chRows = chRows-2;
			chCols = chCols-2;
			detector->Init(chRows,chCols,0);
			cameraTextureId = -1;
			initTestVectors();
			break;
		case 'k':  //keystone correction
		case 'K':
			keystoneCorrection = !keystoneCorrection;
			break;
		case 's':
		case 'S':
			showGrid = !showGrid;
			break;
		case 'r':
		case 'R':
			if(!runCalibrationProcess)
				readyForNextPicture = true;
			runCalibrationProcess = !runCalibrationProcess;
			break;
		case 'Q':
		case 'q':
			testImageIndex++;
			if(testImageIndex >= testImageFiles.size())
				testImageIndex = 0;
			break;
		case '1':
			CalculateHomography(true,0);
			break;
		case '2':
			CalculateHomography(true,1);
			break;
		case '3':
			CalculateHomography(true,2);
			break;
		case '4':
			CalculateHomography(true,3);
			break;
	}
}

/**
 * Reshapes the scene.
 * @param w The new width of the window.
 * @param h The new height of the window.
 */
static void reshape(int w, int h)
{
	if (h==0)// Zabezpečení proti dělení nulou
	{
		h=1;// Nastaví výąku na jedna
	}
	glViewport(0,0,(GLsizei)w,(GLsizei)h);// Resetuje aktuální nastavení
	glutPostRedisplay();
}

/**
 * Resize projector window.
 * @param w New window width.
 * @param h New window height.
 */
static void reshapeWinProj(int w, int h)
{
	winProjWidth = w;
	winProjHeight = h;
	reshape(w,h);
}

/**
 * Resize camera window.
 * @param w New window width.
 * @param h New window height.
 */
static void reshapeWinCam(int w, int h)
{
	if(imgCamScale.GetX() != 1)
	{
		imgCamScale.SetX((imgCamScale.GetX()/winCamWidth)*w);
		imgCamScale.SetY((imgCamScale.GetY()/winCamHeight)*h);
	}

	winCamWidth = w;
	winCamHeight = h;
	reshape(w,h);
}

/**
 * Close window procedure.
 */
static void closeWindow()
{
	exit(0);
}

static int createWindow(char* name)
{
	int windowId = glutCreateWindow(name);
	//define a window position for second window
	glutPositionWindow(DEF_WIN_POS_X,DEF_WIN_POS_Y);
	//glutWMCloseFunc(closeWindow);
	glutKeyboardFunc(keyboard);
	return windowId;
}

/**
 * Inits OpenGL, sets the callback functions and general OpenGL settings.
 */
static void myInitGL(int argc, char **argv){

	//init glut
	glutInit(&argc, argv);

	//init display mode
	glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGB | GLUT_DEPTH);
	//window
	glutInitWindowSize(winCamWidth, winCamHeight);

	//camera window
	winCamId = createWindow(WINDOW_CAMERA_NAME);
	// register callbacks for subwindow, which is now current
	glutReshapeFunc(reshapeWinCam);
	glutDisplayFunc(displayWinCam);
	//glutKeyboardFunc(keyboardWinCam);

	//projector window
	winProjId = createWindow(WINDOW_PROJECTOR_NAME);
	// register callbacks for subwindow, which is now current
	glutReshapeFunc(reshapeWinProj);
	glutDisplayFunc(displayWinProj);
	//glutKeyboardFunc(keyboardWinProj);

	glutIdleFunc(idle);

	//set openGL parameters...
	glDisable(GL_DEPTH_TEST);
	glShadeModel(GL_SMOOTH);

	//viewport
	glViewport(0, 0, winCamWidth, winCamHeight);
}

/**
 * Init basic display parameters.
 */
void initDisplayParams()
{
    string inputLine;
	ifstream infile;
	infile.open (CONFIG_FILE);

	if(infile.fail())
	{
		std::cerr << "Failed to open configuration file with name " << CONFIG_FILE << std::endl;
	}

	while(!infile.eof()) // To get you all the lines.
	{
		getline(infile,inputLine); // Saves the line in STRING.
		int delimiterIndex = inputLine.find_first_of(":",0);
		string identifier = inputLine.substr(0,delimiterIndex);
		string value = inputLine.substr(delimiterIndex+1);

		if(identifier == "PROJECTORS_HORIZONTAL_COUNT")
			projectorHorizontalCount = atoi(value.c_str());
		else if(identifier == "PROJECTOR_VERTICAL_COUNT")
			projectorVerticalCount = atoi(value.c_str());
		else if(identifier == "PROJECTOR_WIDTH")
			projectorWidth = atoi(value.c_str());
		else if(identifier == "PROJECTOR_HEIGHT")
			projectorHeight = atoi(value.c_str());
		else if(identifier == "PROJECTOR_WIN_POSITION_X")
			projectorWindowPositionX = atoi(value.c_str());
		else if(identifier == "PROJECTOR_WIN_POSITION_Y")
			projectorWindowPositionY = atoi(value.c_str());
	}
	infile.close();
}

/**
 * Calculate display parameters.
 */
void calculateParams()
{
	displayWidth = projectorWidth*projectorHorizontalCount;
	displayHeight = projectorHeight*projectorVerticalCount;
	projectorCount = projectorHorizontalCount*projectorVerticalCount;
}

bool initTestImages()
{
	testImageFiles = vector<string>();
	imageTextureIds = vector<GLuint>();
	testImageFiles = Common::GetDir(IMAGE_DIRECTORY,testImageFiles);
	if(testImageFiles.empty())
	{
		cout << "Missing test images folder or folder is empty!";
		return false;
	}
	return true;
}

bool initTestVectors()
{
	testVectorFiles.clear();
	string templateFilename = string(VECTORS_DIRECTORY) + string("/") + string(MESH_COORDINATES_FILENAME);
	testVectorFiles.push_back(templateFilename);
	testVectorFiles = Common::GetDir(VECTORS_DIRECTORY,testVectorFiles);
	testVectorFiles.pop_back();
	if(testVectorFiles.empty())
	{
		cout << "Missing test vectors folder or folder is empty!";
		return false;
	}
	return true;
}

int main( int argc, char** argv )
{
	if(!initTestImages())
		return -1;

	//initTestVectors();

	//load variables
	initDisplayParams();
	calculateParams();

	if(!cameraOffline)
		camera = new CannonCamera();

	calibrationUnit = new CalibrationUnit(projectorCount,projectorHorizontalCount,projectorWidth,projectorHeight);
	calibrationUnit->CalculateOverlapPolygons();
	LoadProjectorCornerPoints();
	detector  = new CheckerDetector();
	detector->Init(chRows,chCols,0);

	detector2 = new CheckerDetector();
	detector2->Init(CHACKERBOARD_PRINTED_ROWS,CHACKERBOARD_PRINTED_COLS,0);

	renderingUnit = new RenderingUnit(projectorCount,projectorHorizontalCount,projectorWidth,projectorHeight,0,0);
	cameraCalibration = new CameraCalibration();
	cameraCalibration->CalcChessboardCorners(CHACKERBOARD_PRINTED_ROWS-1,CHACKERBOARD_PRINTED_COLS-1,CHACKERBOARD_PRINTED_SQUARE);

	myInitGL(argc, argv);
	glutMainLoop();

	return 0;
}
