#include "Calibration.h"

using namespace CALIB; 
using namespace std;

float CALIB::Calibration::saveStatistics(ofstream& fout,float origX,float origY, float shiftX,float shiftY,float L2sum,int projectorWidth,int projectorHeight)
{
//	origX = origX/(projectorWidth-1);
//	origY = origY/(projectorHeight-1);
//	shiftX = shiftX/(projectorWidth-1);
//	shiftY = shiftY/(projectorHeight-1);
	float L2norm = sqrt(pow(shiftX,2)+pow(shiftY,2));
	fout << origX << "," << origY << "," << shiftX << "," << shiftY /*<< ";"*/ << std::endl;//L2norm << std::endl;

	L2sum += pow(L2norm,2);
	return L2sum;
}

vector<double> & CALIB::Calibration::split(const string &s,char delim, vector<double> &elems)
{
    stringstream ss(s);
    string item;
    while (std::getline(ss, item, delim)) {
        stringstream conv( item );
        double number;
        conv >> number;
        elems.push_back(number);
    }
    return elems;
}

void CALIB::Calibration::NormalizeTexturePoints(std::vector<geometry::Vector2Df> * texturePoints)
{
	geometry::Vector2Df minPoint = texturePoints->at(0);
	geometry::Vector2Df maxPoint = texturePoints->at(texturePoints->size()-1)-texturePoints->at(0);
	for(int i = 0;i < texturePoints->size();i++)
	{
		geometry::Vector2Df tmpPoint = texturePoints->at(i)-minPoint;
		texturePoints->at(i).SetX(tmpPoint.GetX()/maxPoint.GetX());
		texturePoints->at(i).SetY(tmpPoint.GetY()/maxPoint.GetY());
	}
}

void CALIB::Calibration::LoadMeshPoints(Projector * p,string filename)
{
	int rows = 0;
	int cols = 0;
	LoadMeshPoints(p->GetPointsMesh(),p->GetPointsMeshTexture(),filename,p->GetWidth(),p->GetHeight(),&rows,&cols);
	p->SetMeshCols(cols);
	p->SetMeshRows(rows);
}

void CALIB::Calibration::LoadMeshPoints(std::vector<geometry::Vector2Df> * vertexPoints,std::vector<geometry::Vector2Df> * texturePoints,string filename,int projectorWidth,int projectorHeight,int * rows,int * cols)
{
	vertexPoints->clear();
	texturePoints->clear();

    ifstream  data(filename);

    string line;
    vector< double > numbers;
    int counter = 0;
    int rowsCounter = 0;
    int lastRowPosition = -1;

    //border around points
//    float border = 0;
//    float maxWidth = projectorWidth*(1-border);
//    float minWidth = projectorWidth*border;
//    float maxHeight = projectorHeight*(1-border);
//    float minHeight = projectorHeight*border;

    while(getline(data,line))
    {
        stringstream  lineStream(line);
        string        cell;

       	numbers.clear();
       	split( line, ',', numbers);

//       	numbers[0] = numbers[0]*(projectorWidth-1);
//       	numbers[1] = numbers[1]*(projectorHeight-1);
//       	numbers[2] = numbers[2]*(projectorWidth-1);
//       	numbers[3] = numbers[3]*(projectorHeight-1);

//       	if(numbers[0] < minWidth || numbers[0] > maxWidth || numbers[1] < minHeight || numbers[1] > maxHeight )
//       		continue;

       	counter++;

       	geometry::Vector2Df point(numbers[0],numbers[1]);
       	geometry::Vector2Df shift(numbers[2],numbers[3]);

       	texturePoints->push_back(point);
       	vertexPoints->push_back(point+shift);

       	if(lastRowPosition < 0 || lastRowPosition != numbers[1])
       		rowsCounter++;

       	lastRowPosition = numbers[1];
    }

    *rows = rowsCounter;
    *cols = counter/rowsCounter;
    data.close();
    NormalizeTexturePoints(texturePoints);
}


CvMat * CALIB::Calibration::CalculatePointsShift(std::vector<geometry::Vector2Df> * cameraImagePoints,CvMat * projectorImagePointsCv,int pointsRows,int pointsCols,string filenameSave,Projector * p,int currntChessRows,int currntChessCols,CvMat * homography)
{
	std::vector<geometry::Vector2Df> * pointsMesh = p->GetPointsMesh();
	std::vector<geometry::Vector2Df> * pointsMeshOriginal = p->GetPointsMeshOriginal();
	std::vector<geometry::Vector2Df> * pointsMeshTexture = p->GetPointsMeshTexture();
	pointsMeshOriginal->clear();
	pointsMesh->clear();
	pointsMeshTexture->clear();

	currntChessCols--;
	currntChessRows--;
	p->SetMeshCols(currntChessCols);
	p->SetMeshRows(currntChessRows);

	int x,y;
	x = y = 0;
	std::vector<geometry::Vector2Df> projectorPoints;
	projectorPoints.push_back(geometry::Vector2Df(x,y));
	projectorPoints.push_back(geometry::Vector2Df(p->GetWidth(),y));
	projectorPoints.push_back(geometry::Vector2Df(p->GetWidth(),p->GetHeight()));
	projectorPoints.push_back(geometry::Vector2Df(x,p->GetHeight()));

	initCameraImagePointsOrthogonal(cameraImagePoints);

	CvMat * projectorPointsCv = Cvh::ConvertVector2DToMat(projectorPoints);
	CvMat * cameraImagePointsCv = Cvh::ConvertVector2DToMat(*cameraImagePoints);

	if(!homography)
	{
		homography = Cvh::CreateMat(3,3);
		Cvh::CalculateHomography(cameraImagePointsCv,projectorPointsCv,homography);
	}

	//Cvh::PrintCvMatrix(projectorPointsCv,"projectorPointsCv");
	//Cvh::PrintCvMatrix(cameraImagePointsCv,"cameraImagePointsCv");


	cvReleaseMat(&projectorPointsCv);
	cvReleaseMat(&cameraImagePointsCv);

	std::vector<geometry::Vector2Df> * detectedInnerCorrnerPoints = p->GetPoints(PointsType::DETECTED);

	CvMat * detectedInnerCorrnerPointsCv = Cvh::ConvertVector2DToMat(*detectedInnerCorrnerPoints);
	CvMat * tmpPointsCv = Cvh::CreateMat(detectedInnerCorrnerPointsCv->rows,detectedInnerCorrnerPointsCv->cols);
	Cvh::TransformPoints(detectedInnerCorrnerPointsCv,tmpPointsCv,homography);
	std::vector<geometry::Vector2Df> * detectedInnerCorrners = Cvh::ConvertMatToVector2D(tmpPointsCv); //in projector coordinate frame
	std::vector<geometry::Vector2Df> * projectorInnerCorners = Cvh::ConvertMatToVector2D(projectorImagePointsCv);
	//Cvh::PrintCvMatrix(chackerboardInnerCorners,"chackerboardInnerCorners");

//	int index = 0;
	int allPointsIndex;
	float shiftX = 0;
	float shiftY = 0;

	remove(filenameSave.c_str());
	ofstream fout(filenameSave);
//	ofstream fout;
//	if(!filenameSave.empty())
//	{
//		//remove(filenameSave.c_str());
//		fout.open(filenameSave,"w+");
//		//ofstream fout(filenameSave);
//		//fout << "Original X;Original Y;Shift X;Shift Y;L2 norm" << std::endl;
//	}

	float L2sum = 0;

	int rowDif = (pointsRows-currntChessRows)/2;
	int colDif = (pointsCols-currntChessCols)/2;
	int rowIndexMin =  rowDif;
	int rowIndexMax =  pointsRows-rowDif;
	int colIndexMin =  colDif;
	int colIndexMax =  pointsCols-colDif;
	int innerCornerRowIndex = 0;
	int innerCornerColIndex = 0;
	int innerCornerIndex = 0;

	for(int i = 0;i < pointsRows;i++)
	{
		if(i < rowIndexMin || i >= rowIndexMax)
			continue;

		innerCornerColIndex = 0;
		for(int j = 0;j < pointsCols;j++)
		{
			if(j < colIndexMin || j >= colIndexMax)
					continue;

			allPointsIndex = i*pointsCols +j;
			innerCornerIndex = innerCornerRowIndex * currntChessCols + innerCornerColIndex;

			float x1 = projectorInnerCorners->at(allPointsIndex).GetX();
			float y1 = projectorInnerCorners->at(allPointsIndex).GetY();
			float x2 = detectedInnerCorrners->at(innerCornerIndex).GetX();
			float y2 = detectedInnerCorrners->at(innerCornerIndex).GetY();

			shiftX = projectorInnerCorners->at(allPointsIndex).GetX() - detectedInnerCorrners->at(innerCornerIndex).GetX();
			shiftY = projectorInnerCorners->at(allPointsIndex).GetY() - detectedInnerCorrners->at(innerCornerIndex).GetY();

			if(!filenameSave.empty())
				L2sum = Calibration::saveStatistics(fout,projectorInnerCorners->at(allPointsIndex).GetX(),projectorInnerCorners->at(allPointsIndex).GetY(), shiftX,shiftY,L2sum,p->GetWidth(),p->GetHeight());

			pointsMeshOriginal->push_back(projectorInnerCorners->at(allPointsIndex));
			pointsMeshTexture->push_back(projectorInnerCorners->at(allPointsIndex));
			pointsMesh->push_back(geometry::Vector2Df(projectorInnerCorners->at(allPointsIndex).GetX()+shiftX,projectorInnerCorners->at(allPointsIndex).GetY()+shiftY));

			innerCornerColIndex++;
		}
		innerCornerRowIndex++;
	}

	if(!filenameSave.empty())
	{
		std::cout << "L2 sum;" << pow(L2sum,0.5f)/innerCornerIndex << std::endl;
		fout.close();
	}
	NormalizeTexturePoints(pointsMeshTexture);
	return homography;
}


void CALIB::Calibration::CalculatePointsShift(vector<geometry::Vector2Df> * originalPoints,vector<geometry::Vector2Df> * shiftedPoints,int pointsRows,int pointsCols,string filenameSave,int width,int height,vector<geometry::Vector2Df> * pointsMesh)
{
	pointsMesh->clear();

	remove(filenameSave.c_str());
	ofstream fout(filenameSave);
	int index = 0;
	float shiftX = 0;
	float shiftY = 0;
	float L2sum = 0;
	for(int i = 0;i < pointsRows;i++)
	{
		for(int j = 0;j < pointsCols;j++)
		{
			shiftX = originalPoints->at(index).GetX() - shiftedPoints->at(index).GetX();
			shiftY = originalPoints->at(index).GetY() - shiftedPoints->at(index).GetY();

			if(!filenameSave.empty())
				L2sum = Calibration::saveStatistics(fout,originalPoints->at(index).GetX(),originalPoints->at(index).GetY(), shiftX,shiftY,L2sum,width,height);

			pointsMesh->push_back(geometry::Vector2Df(originalPoints->at(index).GetX()+shiftX,originalPoints->at(index).GetY()+shiftY));
			index++;
		}
	}

	if(!filenameSave.empty())
	{
		std::cout << "L2 sum;" << pow(L2sum,0.5f)/index << std::endl;
		fout.close();
	}

	return;
}

void CALIB::Calibration::SelectChessInnerPoints(CvMat * projectorImagePointsCv,int pointsRows,int pointsCols,int currntChessRows,int currntChessCols,Projector * p)
{
	p->SetMeshCols(currntChessCols-1);
	p->SetMeshRows(currntChessRows-1);
	std::vector<geometry::Vector2Df> * resultInnerCorners = p->GetPointsMeshOriginal();
	SelectChessInnerPoints(projectorImagePointsCv,pointsRows,pointsCols,currntChessRows,currntChessCols,resultInnerCorners);
}

void CALIB::Calibration::SelectChessInnerPoints(CvMat * projectorImagePointsCv,int pointsRows,int pointsCols,int currntChessRows,int currntChessCols,std::vector<geometry::Vector2Df> * resultInnerCorners)
{
	std::vector<geometry::Vector2Df> * projectedChessCorners = Cvh::ConvertMatToVector2D(projectorImagePointsCv);
	resultInnerCorners->clear();

	currntChessCols--;
	currntChessRows--;
	int index = 0;
	float shiftX = 0;
	float shiftY = 0;

	int rowDif = (pointsRows-currntChessRows)/2;
	int colDif = (pointsCols-currntChessCols)/2;
	int rowIndexMin =  rowDif;
	int rowIndexMax =  pointsRows-rowDif;
	int colIndexMin =  colDif;
	int colIndexMax =  pointsCols-colDif;

	for(int i = 0;i < pointsRows;i++)
	{
		if(i < rowIndexMin || i >= rowIndexMax)
			continue;

		for(int j = 0;j < pointsCols;j++)
		{
			if(j < colIndexMin || j >= colIndexMax)
					continue;

			index = i*pointsCols +j;
			resultInnerCorners->push_back(projectedChessCorners->at(index));
		}

	}
	int size = resultInnerCorners->size();
	delete projectedChessCorners;
	return;
}

void CALIB::Calibration::AddMotionVectorsIntoImage(std::vector<geometry::Vector2Df> * rightPositions,std::vector<geometry::Vector2Df> * currentPositions,std::vector<geometry::Vector2Df> * cameraImagePoints,CvMat * homography,IplImage * img,bool invertHomographyMatrix)
{
	initCameraImagePointsOrthogonal(cameraImagePoints);

	CvMat * h = Cvh::CreateMat(3,3);
	cvCopy(homography,h);
	if(invertHomographyMatrix)
	{
		CvMat * tmph = Cvh::CreateMat(3,3);
		cvInvert(h,tmph);
		cvCopy(tmph,h);
		cvReleaseMat(&tmph);
	}

	CvMat * tmp = Cvh::ConvertVector2DToMat(*rightPositions);
	CvMat * rightPositionsCv = Cvh::CreateMat(tmp->rows,tmp->cols);
	Cvh::TransformPoints(tmp,rightPositionsCv,h);
	cvReleaseMat(&tmp);

	CvMat * currentPositionsCv = Cvh::ConvertVector2DToMat(*currentPositions);

	AddMotionVectorsIntoImage(rightPositionsCv,currentPositionsCv,img);

	for(int j = 0;j < cameraImagePoints->size();j++)
	{
		geometry::Vector2Df p1 = cameraImagePoints->at(j);
		int index = j+1;
		if(index >= cameraImagePoints->size())
			index = 0;
		geometry::Vector2Df p2 = cameraImagePoints->at(index);

		CvScalar colorGreen = cvScalar( 0, 255, 0 );
		cvLine(img, cvPoint(p1.GetX(),p1.GetY()),cvPoint(p2.GetX(),p2.GetY()),colorGreen,1);
	}

	cvReleaseMat(&currentPositionsCv);
	cvReleaseMat(&rightPositionsCv);
}

void CALIB::Calibration::AddMotionVectorsIntoImage(std::vector<geometry::Vector2Df> * rightPositions,std::vector<geometry::Vector2Df> * currentPositions,IplImage * img,int lineWidth)
{
	CvMat * currentPositionsCv = Cvh::ConvertVector2DToMat(*currentPositions);
	CvMat * rightPositionsCv = Cvh::ConvertVector2DToMat(*rightPositions);

	AddMotionVectorsIntoImage(rightPositionsCv,currentPositionsCv,img,lineWidth);

	cvReleaseMat(&currentPositionsCv);
	cvReleaseMat(&rightPositionsCv);
}

void CALIB::Calibration::AddMotionVectorsIntoImage(CvMat * rightPositionsCv,CvMat * currentPositionsCv,IplImage * img,int lineWidth)
{
	CvScalar colorRed = cvScalar( 0, 0, 255 );
	CvScalar colorGreen = cvScalar( 0, 255, 0 );
	CvScalar colorBlue = cvScalar( 255, 0, 0 );

	for(int i = 0;i < currentPositionsCv->cols;i++)
	{
		CvPoint rightPoint = cvPoint(cvmGet(rightPositionsCv,0,i),cvmGet(rightPositionsCv,1,i));
		CvPoint currentPoint = cvPoint(cvmGet(currentPositionsCv,0,i),cvmGet(currentPositionsCv,1,i));
		cvLine(img, rightPoint,currentPoint,colorRed,lineWidth);
		cvCircle(img, currentPoint, lineWidth, colorGreen,-1);

		geometry::Vector2Df point1(rightPoint.x,rightPoint.y);
		geometry::Vector2Df point2(currentPoint.x,currentPoint.y);
		geometry::Vector2Df p = (point1-point2)*0.25;
		geometry::Vector2Df pn = p.GetNormalVector();
		geometry::Vector2Df pl = (point1 - p)+pn;
		geometry::Vector2Df pr = (point1 - p)-pn;

		cvLine(img, rightPoint,cvPoint(pl.GetX(),pl.GetY()),colorRed,lineWidth);
		cvLine(img, rightPoint,cvPoint(pr.GetX(),pr.GetY()),colorRed,lineWidth);
	}
}

void CALIB::Calibration::initCameraImagePointsOrthogonal(std::vector<geometry::Vector2Df> * cameraImagePoints)
{
	int a = 0;
	if(a)
	{
		int w = 2352;
		int h = 1568;

		geometry::Vector2Df center(w/2,h/2);
		geometry::Vector2Df diff = (cameraImagePoints->at(2) - cameraImagePoints->at(0))*0.5;

		cameraImagePoints->at(2) = center+diff;
		cameraImagePoints->at(0) = center-diff;
	}

	cameraImagePoints->at(1).SetX(cameraImagePoints->at(2).GetX());
	cameraImagePoints->at(1).SetY(cameraImagePoints->at(0).GetY());
	cameraImagePoints->at(3).SetX(cameraImagePoints->at(0).GetX());
	cameraImagePoints->at(3).SetY(cameraImagePoints->at(2).GetY());


}

//void CALIB::Calibration::ProcessLuminanceImage(IplImage * image,Projector * p,int projectorIndex)
//{
//	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);
//}
