#include "Calibration\CalibrationUnit.h"

CALIB::CalibrationUnit::CalibrationUnit(int projectorsCount,int projectorsInRow,int width,int height)
{
	this->count = projectorsCount;
	this->cols = projectorsInRow;
	this->rows = projectorsCount/projectorsInRow;

	for(int i = 0;i < projectorsCount;i++)
	{
		Projector * projector = new Projector(i,width,height);
		this->addProjector(projector);
		projector->Load();
	}

	this->display = new Projector(ProjectorIndex::MULTI,projectorsInRow*width,(projectorsCount/projectorsInRow)*height);
	this->display->Load();

}

Projector * CALIB::CalibrationUnit::GetProjector(int index)
{
	if(index >= 0 && index < this->projectors.size())
		return this->projectors.at(index);
	else if(index ==  ProjectorIndex::MULTI)
	{	
		return this->display;
	}
	else
		return NULL;
}

void CALIB::CalibrationUnit::CalculateHomographyDisplay(int projectorIndex,CvMat* displayPoints,CvMat* projectorPoints)
{
	this->calculateHomography(displayPoints,projectorPoints,HomogType::DP,projectorIndex);
	this->calculateHomography(projectorPoints,displayPoints,HomogType::PD,projectorIndex);
}

void CALIB::CalibrationUnit::calculateHomography(vector<geometry::Vector2Df> * srcPlanePoints, vector<geometry::Vector2Df> * dstPlanePoints,int homographyType,int projectorIndex)
{
	CvMat * srcPlanePointsCv = Cvh::ConvertVector2DToMat(*srcPlanePoints);
	CvMat * dstPlanePointsCv = Cvh::ConvertVector2DToMat(*dstPlanePoints);
	this->calculateHomography(srcPlanePointsCv,dstPlanePointsCv,homographyType,projectorIndex);
	cvReleaseMat(&srcPlanePointsCv);
	cvReleaseMat(&dstPlanePointsCv);
}

void CALIB::CalibrationUnit::calculateHomography(CvMat* srcPlanePoints, CvMat* dstPlanePoints,int homographyType,int projectorIndex)
{
	Projector * projector = this->GetProjector(projectorIndex);
	CvMat * homography  = projector->GetHomography(homographyType);
	Cvh::CalculateHomography(srcPlanePoints, dstPlanePoints,homography);
	//Cvh::PrintCvMatrix(homography,"homography");
	projector->Save();
}

CvMat * CALIB::CalibrationUnit::GetPoints(int projectorIndex,int coordSystem)
{
	Projector * projector = this->GetProjector(projectorIndex);
	return projector->GetCornerPoints(coordSystem);
}

CvMat *   CALIB::CalibrationUnit::TransformPoints(int projectorIndex,int homogType,CvMat * points)
{
	Projector * projector = this->GetProjector(projectorIndex);
	CvMat * dst = Cvh::CreateMat(points->rows,points->cols);
	projector->TranformPoints(homogType,points,dst);
	return dst;
}

void CALIB::CalibrationUnit::ResetCalibration()
{
	for(int i = 0;i < this->projectors.size();i++)
	{
		 this->projectors.at(i)->SetCalibrated(false);
	}
}

bool CALIB::CalibrationUnit::CalibrateDisplay(int baseProjectorIndex)
{
	if(!this->CalculateOverlapPolygons())
		return false;

	if(baseProjectorIndex == -1)
	{
		baseProjectorIndex = this->getBaseProjectorIndex();
	}

	this->calculatePDHomography(baseProjectorIndex);
	if(!this->calculatePDHomographyAllProj(baseProjectorIndex))
	{
		for(int i = 0;i < this->projectors.size();i++)
		{
			this->ResetCalibration();
			if(this->CalibrateDisplay(i))
				break;
		}
	}

	return true;
}

bool CALIB::CalibrationUnit::calculatePDHomographyAllProj(int baseProjectorIndex)
{
	for(int i = 0;i < this->projectors.size();i++)
	{
		if(i == baseProjectorIndex)
			continue;
		if(!this->calculatePDHomography(i))
			return false;
	}
	return true;
}

void CALIB::CalibrationUnit::calculatePPHomography(int p1,int p2,CvMat * h12)
{
	Projector * projector1 = this->projectors.at(p1);
	projector1->CalculateHomographyCamera(p2);

	Projector * projector2 = this->projectors.at(p2);
	projector2->CalculateHomographyCamera(p1);

	std::vector<geometry::Vector2Df> pointsP1;
	std::vector<geometry::Vector2Df> pointsP2;

	//p1 points
	std::vector<geometry::Vector2Df> * pointsProjectedP1 = projector1->GetPoints(PointsType::PROJECTED,p2);
	pointsP1 = *pointsProjectedP1;
	std::vector<geometry::Vector2Df> * pointsDetectedP1 = projector1->GetPoints(PointsType::DETECTED,p2);
	pointsP2 = *(projector2->TranformPoints(CPO,pointsDetectedP1));

	//p2 points
	std::vector<geometry::Vector2Df> * pointsProjectedP2 = projector2->GetPoints(PointsType::PROJECTED,p1);
	pointsP2.insert(pointsP2.end(),pointsProjectedP2->begin(),pointsProjectedP2->end());
	std::vector<geometry::Vector2Df> * pointsDetectedP2 = projector2->GetPoints(PointsType::DETECTED,p1);
	std::vector<geometry::Vector2Df> * pointsTransformed = projector1->TranformPoints(CPO,pointsDetectedP2);
	pointsP1.insert(pointsP1.end(),pointsTransformed->begin(),pointsTransformed->end());

	CvMat * srcPlanePointsCv = Cvh::ConvertVector2DToMat(pointsP1);
	CvMat * dstPlanePointsCv = Cvh::ConvertVector2DToMat(pointsP2);

	Cvh::CalculateHomography(srcPlanePointsCv,dstPlanePointsCv,h12);

	cvReleaseMat(&srcPlanePointsCv);
	cvReleaseMat(&dstPlanePointsCv);
}

bool CALIB::CalibrationUnit::calculatePDHomography(int pIndex)
{
	Projector * projector = this->GetProjector(pIndex);
	bool isBaseProjector = true;
	//projector points
	std::vector<geometry::Vector2Df> pointsP;
	//display points
	std::vector<geometry::Vector2Df> pointsD;
	for(int i=0;i < this->projectors.size();i++)
	{
		if(i == pIndex)
			continue;

		Projector * adjacentProjector = this->GetProjector(i);
		if(!adjacentProjector->IsCalibrated())
			continue;
		isBaseProjector = false;

		/*******/
		if(!projector->CalculateHomographyCamera(i))
			continue;
		adjacentProjector->CalculateHomographyCamera(pIndex);

		//projected points by adjacent projector
		std::vector<geometry::Vector2Df> * pointsProjectedPA = adjacentProjector->GetPoints(PointsType::PROJECTED,pIndex);
		bool empty = pointsProjectedPA->empty();
		int size = pointsProjectedPA->size();

		std::vector<geometry::Vector2Df> * pointsDisplayPA2 = adjacentProjector->TranformPoints(PD,pointsProjectedPA);
		pointsD.insert(pointsD.end(),pointsDisplayPA2->begin(),pointsDisplayPA2->end());
		std::vector<geometry::Vector2Df> * pointsDetectedP = adjacentProjector->GetPoints(PointsType::DETECTED,pIndex);
		empty = pointsDetectedP->empty();
		size = pointsDetectedP->size();

		std::vector<geometry::Vector2Df> * pointsTransformedP = projector->TranformPoints(CPO,pointsDetectedP);
		pointsP.insert(pointsP.end(),pointsTransformedP->begin(),pointsTransformedP->end());
		delete pointsTransformedP;
		delete pointsDisplayPA2;

		//projected points by projector which is calibrating
		std::vector<geometry::Vector2Df> * pointsProjectedP = projector->GetPoints(PointsType::PROJECTED,i);
		pointsP.insert(pointsP.end(),pointsProjectedP->begin(),pointsProjectedP->end());
		std::vector<geometry::Vector2Df> * pointsDetectedPA = projector->GetPoints(PointsType::DETECTED,i);
		std::vector<geometry::Vector2Df> * pointsTransformedPA = adjacentProjector->TranformPoints(CPO,pointsDetectedPA);
		std::vector<geometry::Vector2Df> * pointsDisplayPA = adjacentProjector->TranformPoints(PD,pointsTransformedPA);
		pointsD.insert(pointsD.end(),pointsDisplayPA->begin(),pointsDisplayPA->end());
		delete pointsTransformedPA;
		delete pointsDisplayPA;
	}

	if(isBaseProjector)
	{
		projector->InitDisplayBase();
		this->calculateDisplayTransformation(projector,pIndex);
	}
	else
	{
		if(pointsP.empty())
		{
			projector->SetCalibrated(false);
			return false;
		}
		//compute calibration parameters
		this->calculateHomography(&pointsD,&pointsP,DP,pIndex);
		this->calculateHomography(&pointsP,&pointsD,PD,pIndex);
	}
	projector->SetCalibrated(true);
	return true;
}

bool CALIB::CalibrationUnit::CalculateOverlapPolygons()
{
	for(int i = 0;i < this->projectors.size();i++)
	{
		Projector * projector1 = this->projectors.at(i);
		geometry::Polygon2Df * polygon1 = projector1->GetSelfPolygon();
		if(polygon1 == NULL)
			return false;

		for(int j = 0;j < this->projectors.size();j++)
		{
			if(i >= j)
				continue;
			Projector * projector2 = this->projectors.at(j);
			geometry::Polygon2Df * polygon2 = projector2->GetSelfPolygon();
			if(polygon2 == NULL)
				return false;

			std::vector<geometry::Polygon2Df*> v;
			v.push_back(polygon1);
			v.push_back(polygon2);

			geometry::Polygon2Df * intersecPolygon = new geometry::Polygon2Df();
			intersecPolygon->GetCombinedPolygon(v,true,geometry::Polygon2Df::AND);
			projector1->SetPolygon(j,intersecPolygon);
			projector2->SetPolygon(i,intersecPolygon);

			//intersecPolygon->Print();
		}
	}

	for(int j = 0;j < this->projectors.size();j++)
	{
		 this->projectors.at(j)->CalculateOvelapImage();
	}
	return true;
}

IplImage * CALIB::CalibrationUnit::CombineProjectorAreasIntoImage(int projectorsCount,CvScalar color,bool addValues)
{
	if(projectorsCount <= 0)
		projectorsCount = this->projectors.size();
	IplImage * tmpImage = Cvh::CreateIplimage(this->cameraImageWidth,this->cameraImageHeight,NULL,1);
	IplImage * cameraImage = Cvh::CreateIplimage(this->cameraImageWidth,this->cameraImageHeight,NULL,1);
	CvPoint cvPoints[CORNER_COUNT];

	for(int j = 0;j < projectorsCount;j++)
	{
		Projector * projector = this->projectors.at(j);
		geometry::Polygon2Df * polygon = projector->GetSelfPolygon();
		if(polygon == NULL)
			return NULL;

		vector<geometry::Vector2Df> points = polygon->GetPoints();

		for(int i = 0; i < points.size();i++)
		{
			cvPoints[i] = cvPoint(points.at(i).GetX(),points.at(i).GetY());
		}
		if(!addValues)
			cvFillConvexPoly(cameraImage,cvPoints,points.size(),color);
		else
		{
			cvSet(tmpImage, cvScalar(0));
			cvFillConvexPoly(tmpImage,cvPoints,points.size(),color);
			cvAdd(cameraImage,tmpImage,cameraImage);
		}
	}

	if(addValues)
		cvReleaseImage(&tmpImage);
	//cvSaveImage("cameraImage.jpg",cameraImage);

	return cameraImage;
}

geometry::Polygon2Df* CALIB::CalibrationUnit::CombineProjectorPolygons()
{
	std::vector<geometry::Polygon2Df*> polygonVector;
	geometry::Polygon2Df intersecPolygon = geometry::Polygon2Df();
	geometry::Polygon2Df tmpPolygon = geometry::Polygon2Df();

	for(int j = 0;j < this->projectors.size();j++)
	{
		polygonVector.clear();
		Projector * projector = this->projectors.at(j);
		geometry::Polygon2Df * polygon = projector->GetSelfPolygon();
		if(polygon == NULL)
			return NULL;
		if(j == 0)
		{
			intersecPolygon.SetPoints(polygon->GetPoints());
			continue;
		}
		polygonVector.push_back(polygon);
		polygonVector.push_back(&intersecPolygon);

		tmpPolygon.GetCombinedPolygon(polygonVector,true,geometry::Polygon2Df::OR);
		intersecPolygon = tmpPolygon;
		//intersecPolygon.Print();
	}

	geometry::Polygon2Df * resultPolygon = new geometry::Polygon2Df(intersecPolygon.GetPoints());
	return resultPolygon;
}


CvMat * CALIB::CalibrationUnit::GetHomography(int projectorIndex ,int type, bool as4x4)
{
	CvMat * homography = this->GetProjector(projectorIndex)->GetHomography(type);
	if(as4x4)
	{
		return Cvh::ConvertMat3To4(homography,true);
	}
	return homography;
}

std::vector<CvMat*> * CALIB::CalibrationUnit::GetAdjacentProjectroHomographies(int projectorIndex)
{
	Projector * p = this->GetProjector(projectorIndex);
	CvMat * DPbase = p->GetHomography(DP);
	std::vector<CvMat*> * v = new std::vector<CvMat*>();
	for(int i = 0;i < this->count;i++)
	{
		if(i == projectorIndex)
			continue;
		CvMat * mat = Cvh::CreateMat(TWO_DIM_HOMOGENOUS,TWO_DIM_HOMOGENOUS);
		CvMat * PDmat = this->GetHomography(i,PD,false);
		mat = Cvh::MatMul(DPbase,PDmat);
		v->push_back(mat);
	}

	return v;
}


void CALIB::CalibrationUnit::adjustDisplayRatioScale(vector<geometry::Vector2Df> * displayPoints,float  projectorWidth,float projectorHeight)
{
	float ratio = projectorWidth/projectorHeight;

	geometry::Vector2Df  * p0 = (&displayPoints->at(0));
	geometry::Vector2Df  * p1 = (&displayPoints->at(1));
	geometry::Vector2Df  * p2 = (&displayPoints->at(2));
	geometry::Vector2Df  * p3 = (&displayPoints->at(3));


	geometry::Vector2Df topLine = (*p1) - (*p0);
	geometry::Vector2Df leftLine = (*p3) - (*p0);
	geometry::Vector2Df rightLine = (*p2) - (*p1);
	geometry::Vector2Df bottomLine = (*p2) - (*p3);

	float width = topLine.GetLength();
	float height = leftLine.GetLength();

	if((width/height) < ratio)
	{
		//shift bottom line
		float newHeight = (width/projectorWidth)*projectorHeight;
		float shiftCoeficient = newHeight/height;
		geometry::Vector2Df shiftLeftLine =  leftLine * shiftCoeficient;
		geometry::Vector2Df shiftRightLine =  rightLine * shiftCoeficient;
		(*p2) =(*p1) + shiftRightLine;
		(*p3) = (*p0) + shiftLeftLine;
	}
	else
	{
		//shift right line
		float newWidth = (height/projectorHeight)*projectorWidth;
		float shiftCoeficient = newWidth/width;
		geometry::Vector2Df shiftTopLine =  topLine * shiftCoeficient;
		geometry::Vector2Df shiftBottomLine =  bottomLine * shiftCoeficient;
		(*p1) = (*p0) + shiftTopLine;
		(*p2) = (*p3) + shiftBottomLine;
	}
}

void CALIB::CalibrationUnit::adjustDisplayArea(vector<geometry::Vector2Df> * displayPoints,IplImage* projectorUnionImage,geometry::Polygon2Df* projectorUnionPolygon)
{
	bool addVector = true;
	geometry::Vector2Df  * p0 = (&displayPoints->at(0));
	geometry::Vector2Df  * p1 = (&displayPoints->at(1));
	geometry::Vector2Df  * p2 = (&displayPoints->at(2));
	geometry::Vector2Df  * p3 = (&displayPoints->at(3));

	geometry::Vector2Df pShift10 = (*p1) - (*p0);
	geometry::Vector2Df pShift23 = (*p2) - (*p3);
	geometry::Vector2Df pShift21 = (*p2) - (*p1);
	geometry::Vector2Df pShift30 = (*p3) - (*p0);

	geometry::Vector2Df pTShift10,pTShift23,pTShift21,pTShift30;
	pTShift10 = pShift10;
	float shiftCoeficient = INITIAL_SHIFT_COEFFICIENT;
	bool shiftSuccess = false;

	float fragment = INITIAL_VACTOR_FRAGMENT;
	pShift10 = pShift10 * fragment;
	pShift23 = pShift23 * fragment;
	pShift21 = pShift21 * fragment;
	pShift30 = pShift30 * fragment;

	while(pTShift10.GetLength() > DISPLAY_BORDER_WIDTH || pTShift30.GetLength() > DISPLAY_BORDER_WIDTH || pTShift23.GetLength() > DISPLAY_BORDER_WIDTH || pTShift21.GetLength() > DISPLAY_BORDER_WIDTH)
	{
		pTShift10 = pShift10*shiftCoeficient;
		pTShift23 = pShift23*shiftCoeficient;
		pTShift21 = pShift21*shiftCoeficient;
		pTShift30 = pShift30*shiftCoeficient;

		//left border line
		bool shiftSuccess = this->shiftLine(projectorUnionImage,projectorUnionPolygon,p0,p3,pTShift10,pTShift23,!addVector);
		//top borderline line
		shiftSuccess |= this->shiftLine(projectorUnionImage,projectorUnionPolygon,p0,p1,pTShift30,pTShift21,!addVector);
		//right border line
		shiftSuccess |= this->shiftLine(projectorUnionImage,projectorUnionPolygon,p1,p2,pTShift10,pTShift23,addVector);
		//bottom border line
		shiftSuccess |= this->shiftLine(projectorUnionImage,projectorUnionPolygon,p3,p2,pTShift30,pTShift21,addVector);


		if(!shiftSuccess)
		{
			shiftCoeficient = shiftCoeficient/2;
//			if(shiftCoeficient < 1)
//				shiftCoeficient = 1;
		}
	}
}

bool CALIB::CalibrationUnit::shiftLine(IplImage* projectorUnionImage,geometry::Polygon2Df *projectorUnionPolygon,geometry::Vector2Df * lp1,geometry::Vector2Df * lp2,geometry::Vector2Df pShift1,geometry::Vector2Df pShift2,bool add)
{
	geometry::Vector2Df nlp1;
	geometry::Vector2Df nlp2;

	if(!add)
	{
		nlp1 = *lp1 - pShift1;
		nlp2 = *lp2 - pShift2;
	}
	else
	{
		nlp1 = *lp1 + pShift1;
		nlp2 = *lp2 + pShift2;
	}

	if(this->pointsInsidePolygon(projectorUnionImage,nlp1,nlp2))
	{
		//check if line is not intersecting
		if(!this->lineIntersectPolygon(projectorUnionPolygon,&nlp1,&nlp2))
		{
			*lp1 = nlp1;
			*lp2 = nlp2;
			return true;
		}
	}
	return false;
}


bool CALIB::CalibrationUnit::Test()
{
	IplImage * image = this->CombineProjectorAreasIntoImage();
	this->pointsInsidePolygon(image,geometry::Vector2Df(60,60),geometry::Vector2Df(200,200));
	return true;

//	geometry::Vector2Df p1 = geometry::Vector2Df(684,600);
//	geometry::Vector2Df p2 = geometry::Vector2Df(684,1385);

//	std::vector<geometry::Vector2Df> points;
//	points.push_back(geometry::Vector2Df(200,200));
//	points.push_back(geometry::Vector2Df(300,1500));
//	points.push_back(geometry::Vector2Df(1500,1500));
//	points.push_back(geometry::Vector2Df(1500,200));
//	geometry::Polygon2Df polygon = geometry::Polygon2Df(points);
//
//	std::vector<geometry::Vector2Df> points2;
//	points2.push_back(geometry::Vector2Df(1300,200));
//	points2.push_back(geometry::Vector2Df(1300,1500));
//	points2.push_back(geometry::Vector2Df(2000,1500));
//	points2.push_back(geometry::Vector2Df(2000,200));
//	geometry::Polygon2Df polygon2 = geometry::Polygon2Df(points2);

	std::vector<geometry::Vector2Df> points3;
	points3.push_back(geometry::Vector2Df(0,0));
	points3.push_back(geometry::Vector2Df(20,0));
	points3.push_back(geometry::Vector2Df(20,50));
	points3.push_back(geometry::Vector2Df(30,50));
	points3.push_back(geometry::Vector2Df(30,110));
	points3.push_back(geometry::Vector2Df(10,110));
	points3.push_back(geometry::Vector2Df(10,100));
	points3.push_back(geometry::Vector2Df(0,100));
	geometry::Polygon2Df polygon3 = geometry::Polygon2Df(points3);

	geometry::Vector2Df p1 = geometry::Vector2Df(10,50);
	geometry::Vector2Df p2 = geometry::Vector2Df(25,100);

	std::vector<geometry::Vector2Df> points;
	points.push_back(geometry::Vector2Df(0,0));
	points.push_back(geometry::Vector2Df(20,0));
	points.push_back(geometry::Vector2Df(20,50));
	points.push_back(geometry::Vector2Df(30,50));
	points.push_back(geometry::Vector2Df(30,110));
	points.push_back(geometry::Vector2Df(20,100));
	points.push_back(geometry::Vector2Df(0,100));
	geometry::Polygon2Df polygon = geometry::Polygon2Df(points);

	std::vector<geometry::Vector2Df> points2;
	points2.push_back(geometry::Vector2Df(10,50));
	points2.push_back(geometry::Vector2Df(30,50));
	points2.push_back(geometry::Vector2Df(30,110));
	points2.push_back(geometry::Vector2Df(10,110));
	geometry::Polygon2Df polygon2 = geometry::Polygon2Df(points2);

	std::vector<geometry::Polygon2Df*> polygonVector;

	polygonVector.push_back(&polygon);
	polygonVector.push_back(&polygon2);
	geometry::Polygon2Df * intersecPolygon = new geometry::Polygon2Df();
//	geometry::Polygon2Df * intersecPolygon = polygon.GetCombinedPolygon(&polygon2);

	intersecPolygon->GetCombinedPolygon(polygonVector,false,geometry::Polygon2Df::OR);
//	intersecPolygon.AddPointsToCombinedPolygon()
	intersecPolygon->Print();

	bool c = polygon.IsPointInside(p1);
	bool c3 = polygon3.IsPointInside(p1);
	bool c1 = intersecPolygon->IsPointInside(p1);
	//bool a = this->lineInsidePolygon(intersecPolygon,&p1,&p2);
	return false;
}

bool CALIB::CalibrationUnit::pointsInsidePolygon(IplImage* projectorUnitImage,geometry::Vector2Df p1,geometry::Vector2Df p2)
{
	vector<geometry::Vector2Df> points;
	points.push_back(p1);
	points.push_back(p2);
	return this->pointsInsidePolygon(projectorUnitImage,points);
}

bool CALIB::CalibrationUnit::pointsInsidePolygon(IplImage* projectorUnitImage,vector<geometry::Vector2Df> points)
{
	CvScalar pixel;
	CvScalar maxValue = cvScalar(UCHAR_MAX);
	int x,y;

	for(int i = 0;i < points.size();i++)
	{
		geometry::Vector2Df point = points.at(i);
		x = point.GetX();
		y = point.GetY();
		if(x < 0 || x >  projectorUnitImage->width)
			return false;
		if(y < 0 || y >  projectorUnitImage->height)
			return false;

		pixel = cvGet2D(projectorUnitImage,y,x);
		if(pixel.val[0] < maxValue.val[0])
			return false;
	}

	return true;
}

bool CALIB::CalibrationUnit::lineIntersectPolygon(geometry::Polygon2Df* polygon,geometry::Vector2Df* p1,geometry::Vector2Df* p2)
{
	geometry::LineSegment2Df line;
	line.SetValues((*p1),(*p2));
	vector<geometry::Vector2Df> intersections = polygon->GetIntersection(line);
	if(intersections.size() <= 0)
		return false;
	else
		return true;
}

void CALIB::CalibrationUnit::shiftDisplayCornerPointsIntoCenter(vector<geometry::Vector2Df> * cornerPoints,int projectorIndex)
{
	//shift vector for left upper and right bottom point
	geometry::Vector2Df shiftVector1 = (cornerPoints->at(2)-cornerPoints->at(0))*PROJECTOR_SHIFT_COEFFICIENT;
	//shift vector for right upper and left bottom point
	geometry::Vector2Df shiftVector2 = (cornerPoints->at(1)-cornerPoints->at(3))*PROJECTOR_SHIFT_COEFFICIENT;

	cornerPoints->at(0) = cornerPoints->at(0) + shiftVector1;
	cornerPoints->at(2) = cornerPoints->at(2) - shiftVector1;

	cornerPoints->at(3) = cornerPoints->at(3) + shiftVector2;
	cornerPoints->at(1) = cornerPoints->at(1) - shiftVector2;
}

vector<geometry::Vector2Df> * CALIB::CalibrationUnit::prepareOriginalDisplayCornerPoints(Projector * projector)
{
	/* Get and transform original points to left upper corner.*/
	vector<geometry::Vector2Df> * cornerPointsOriginal = Cvh::ConvertMatToVector2D(projector->GetCornerPoints(DISPLAY));
	geometry::Vector2Df shiftVector = cornerPointsOriginal->at(0);
	for(int i = 0; i < cornerPointsOriginal->size();i++)
	{
		cornerPointsOriginal->at(i) = cornerPointsOriginal->at(i) - shiftVector;

		if(this->rows > 1)
			cornerPointsOriginal->at(i) =  cornerPointsOriginal->at(i) * 2;
		else //sfift in X axis
		{
			float x = cornerPointsOriginal->at(i).GetX();
			cornerPointsOriginal->at(i).SetX(2*x);
		}
	}
	return cornerPointsOriginal;
}

//TODO: Posunout obdelnik do stredu displeje
//* Odladit pro Base projektor index != 0
//* Nastavit pomer stran

void CALIB::CalibrationUnit::calculateDisplayTransformation(Projector * projector,int projectorIndex)
{
	/* Get and transform original points to left upper corner.*/
	vector<geometry::Vector2Df> * cornerPointsOriginal = this->prepareOriginalDisplayCornerPoints(projector);
	/*Create initial display rectangle for display area adjustment into projectors union*/
	vector<geometry::Vector2Df> * cornerPointsShifted = Cvh::ConvertMatToVector2D(projector->GetCornerPoints(DISPLAY));
	this->shiftDisplayCornerPointsIntoCenter(cornerPointsShifted,projectorIndex);

	/*Transform display points to camera image coordinate system*/
	vector<geometry::Vector2Df> * cornerPointsProjector = projector->TranformPoints(DP,cornerPointsShifted);
	vector<geometry::Vector2Df> * cornerPointsCamera = projector->TranformPoints(PC,cornerPointsProjector);

	/*Create intersection polygons*/
	IplImage* projectorUnionImage = this->CombineProjectorAreasIntoImage();
	geometry::Polygon2Df* projectorUnionPolygon = this->CombineProjectorPolygons();

//	for(int i = 0;i < cornerPointsCamera->size();i++)
//	{
//		geometry::Vector2Df a = cornerPointsCamera->at(i);
//	}
//	cornerPointsCamera->clear();
//	int x = 800;
//	int y = 600;
//	int sx = 200;
//	int sy = 100;
//	cornerPointsCamera->push_back(geometry::Vector2Df(x,y));
//	cornerPointsCamera->push_back(geometry::Vector2Df(x+sx,y));
//	cornerPointsCamera->push_back(geometry::Vector2Df(x+sx,y+sy));
//	cornerPointsCamera->push_back(geometry::Vector2Df(x,y+sy));


	/*Fit display rectangle into projectors union polygon*/
	this->adjustDisplayArea(cornerPointsCamera,projectorUnionImage,projectorUnionPolygon);
	/*Resize display to fit projector axis ratio*/
	this->adjustDisplayRatioScale(cornerPointsCamera,projector->GetWidth(),projector->GetHeight());

	/*Transform display points back to display coordinate system*/
	cornerPointsProjector = projector->TranformPoints(CP,cornerPointsCamera);
	cornerPointsShifted = projector->TranformPoints(PD,cornerPointsProjector);

	/*Calculate transformation between original and new display position */
	this->calculateHomography(cornerPointsOriginal,cornerPointsShifted,DP,projectorIndex);
	CvMat * tmp = Cvh::MatMul(projector->GetHomography(DP),projector->GetHomography(PK));
	Cvh::CopyMatrix(tmp,projector->GetHomography(DP));
	cvReleaseMat(&tmp);
	cvInvert(projector->GetHomography(DP),projector->GetHomography(PD));
}

int CALIB::CalibrationUnit::getBaseProjectorIndex()
{
	float keystoneValue = this->projectors.at(0)->GetKeystoneDistortionValue();
	int index = 0;
	float curKeystoneValue = 0;
	for(int i = 1;i < this->projectors.size();i++)
	{
		curKeystoneValue = this->projectors.at(i)->GetKeystoneDistortionValue();
		if(keystoneValue > curKeystoneValue)
		{
			index = i;
			keystoneValue = curKeystoneValue;
		}
	}
	return index;
}

void CALIB::CalibrationUnit::CalculateLuminanceAttenuation()
{
	COLOR minLuminance;
	minLuminance.R = 255;
	minLuminance.G = 255;
	minLuminance.B = 255;

	for(int i = 0;i < this->projectors.size();i++)
	{
		COLOR luminance =   this->projectors.at(i)->GetLuminanceAttenuation();
		if(luminance.R < minLuminance.R)
			minLuminance.R = luminance.R;
		if(luminance.G < minLuminance.G)
			minLuminance.G = luminance.G;
		if(luminance.B < minLuminance.B)
			minLuminance.B = luminance.B;
	}

	for(int i = 0;i < this->projectors.size();i++)
	{
		Projector * p = this->projectors.at(i);
		COLOR luminance = p->GetLuminanceAttenuation();
		p->SetLuminanceAttenuationR(minLuminance.R/luminance.R);
		p->SetLuminanceAttenuationG(minLuminance.G/luminance.G);
		p->SetLuminanceAttenuationB(minLuminance.B/luminance.B);
	}
}
