#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);
	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;
}

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

	this->calculatePDHomography(baseProjectorIndex);

	for(int i = 0;i < this->projectors.size();i++)
	{
		if(i == baseProjectorIndex)
			continue;
		this->calculatePDHomography(i);
	}

	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);
}

void 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;

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

		//projected points by adjacent projector
		std::vector<geometry::Vector2Df> * pointsProjectedPA = adjacentProjector->GetPoints(PointsType::PROJECTED,pIndex);
		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);
		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();
	else
	{
		//compute calibration parameters
		this->calculateHomography(&pointsD,&pointsP,DP,pIndex);
		this->calculateHomography(&pointsP,&pointsD,PD,pIndex);
	}
	projector->SetCalibrated(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();
		}
	}
	return true;
}


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;
}
