#include "Calibration\Projector.h"
using namespace CALIB; 

CALIB::Projector::Projector(int index,int width,int height)
{
	this->height = height;
	this->width = width;
	this->index = index;
	this->lightTextureId = -1;
	this->lightMap = NULL;
	int i;
	this->pointsProjected = new std::vector<geometry::Vector2Df>();
	this->pointsDetected = new std::vector<geometry::Vector2Df>();
	this->SetCalibrated(false);

	for(i = 0;i <= HomogType::CP;i++)
	{
		this->homography.push_back(Cvh::CreateMat(HOMOG_DIM,HOMOG_DIM));
	}
	this->initCornerPoints();
}

CALIB::Projector::~Projector()
{
	int i;
	int max = HomogType::CP;
	for(i = 0;i <= max;i++)
	{
		cvReleaseMat(&(this->homography.at(i)));
	}	
	cvReleaseMat(&this->cornerPoints);
}

void CALIB::Projector::initCornerPoints()
{
	this->cornerPoints = Cvh::CreateMat(TWO_DIM,CORNER_COUNT);
	
	int x = 0;
	int y = 0;
	

	int i = 0;
	for(i = 0;i < CORNER_COUNT;i++)
	{
		x = 0;
		y = 0;
		//comment
		Common::countCoordinates(this->index,this->width,this->height,&x,&y,false);
		switch(i)
		{
			case U_R:
				x += this->width;
			break;
			case B_L:
				y += this->height;
			break;
			case B_R:
				x += this->width;
				y += this->height;
			break;
			default:
				break;
		}
		cvmSet(this->cornerPoints,0,i,x);
		cvmSet(this->cornerPoints,1,i,y);
	}
}


void CALIB::Projector::InitDisplayBase()
{
	cvmCopy(this->homography.at(PK),this->homography.at(DP));
	cvmCopy(this->homography.at(KP),this->homography.at(PD));
}

CvMat * CALIB::Projector::GetHomography(int type)
{
	if(type >= 0 && type <= HomogType::CP)
		return this->homography.at(type);
	else
	{
		throw "Projector::GetHomography - Type out of index.";
		return NULL;
	}
}

void CALIB::Projector::SetHomography(int type,CvMat * value)
{
	if(value == NULL)
		return;

	if(type >= 0 && type <= HomogType::CP)
	{
		cvReleaseMat(&this->homography.at(type));
		this->homography.at(type) = value;
	}
	else
	{
		throw "Projector::SetHomography - Type out of index.";
	}
}

vector<geometry::Vector2Df> * CALIB::Projector::TranformPoints(int homographyType,vector<geometry::Vector2Df> * src)
{
	CvMat * srcCv = Cvh::ConvertVector2DToMat(*src);
	CvMat * dstCv = Cvh::CreateMat(srcCv->rows,srcCv->cols);

	Cvh::TransformPoints(srcCv,dstCv,this->GetHomography(homographyType));
	std::vector<geometry::Vector2Df> * result = Cvh::ConvertMatToVector2D(dstCv);
	cvReleaseMat(&srcCv);
	cvReleaseMat(&dstCv);
	return result;
}

void CALIB::Projector::TranformPoints(int homographyType,CvMat * src,CvMat * dst)
{
	Cvh::TransformPoints(src,dst,this->GetHomography(homographyType));
}

std::vector<geometry::Vector2Df> * CALIB::Projector::GetCornerPointsVector(int coordSystem)
{
	CvMat * cornerPointsCv = this->GetCornerPoints(coordSystem);
	vector<geometry::Vector2Df> * cornerPoints = Cvh::ConvertMatToVector2D(cornerPointsCv);
	cvReleaseMat(&cornerPointsCv);
	return cornerPoints;
}

CvMat * CALIB::Projector::GetCornerPoints(int coordSystem)
{
	CvMat * src = this->cornerPoints;
	CvMat * dst = NULL;

	int type = 0;
	switch(coordSystem)
	{
		case CAMERA:
			type = PC;
			break;
		case DISPLAY:
			type = PD;
			break;
		case PROJECTOR_KEYSTONE:
			type = PK;
			break;
		default:
			type = -1;
			break;
	}

	if(type >= 0)
	{
		dst = Cvh::CreateMat(src->rows,src->cols);
		this->TranformPoints(type,src,dst);
	}
	else
	{
		dst = cvCloneMat(src);
		src = NULL;
	}

	return dst;
}

std::vector<int> * CALIB::Projector::getOverlapPointsIndexes(int overlappedProjectorIndex)
{
	std::vector<int> * indexes = new std::vector<int>();
	if(this->pointsDetected == NULL)
		return indexes;
	else
	{
		geometry::Polygon2Df * overlapPolygon = this->GetPolygon(overlappedProjectorIndex);
		if(overlapPolygon == NULL)
			return indexes;

		//overlapPolygon->Print();
		//int size = this->pointsDetected->size();
		for(int i = 0;i < this->pointsDetected->size();i++)
		{
			this->pointsDetected->at(i).Print();
			if(overlapPolygon->IsPointInside(this->pointsDetected->at(i),true))
				indexes->push_back(i);
		}
		return indexes;
	}
}

void CALIB::Projector::SetPoints(int type,std::vector<geometry::Vector2Df> * points)
{
	if(type == PointsType::DETECTED)
	{
		this->pointsDetected->clear();
		this->pointsDetected->assign(points->begin(),points->end());
	}
	else
	{
		this->pointsProjected->clear();
		this->pointsProjected->assign(points->begin(),points->end());
	}
}


std::vector<geometry::Vector2Df> * CALIB::Projector::GetPoints(int type,int overlappedProjectorIndex)
{
	std::vector<geometry::Vector2Df> * points;

	if(type == PointsType::DETECTED)
		points = this->pointsDetected;
	else
		points = this->pointsProjected;

	if(overlappedProjectorIndex == NONE_INDEX)
		return points;

	std::vector<int> * indexes = this->getOverlapPointsIndexes(overlappedProjectorIndex);

	std::vector<geometry::Vector2Df> * chosenPoints = new std::vector<geometry::Vector2Df>();
	for(int i = 0;i < indexes->size();i++)
	{
		int index = indexes->at(i);
		chosenPoints->push_back(points->at(index));
	}
	return chosenPoints;
}

void CALIB::Projector::Save()
{
	int i;
	for(i = 0;i <= HomogType::CP;i++)
	{		
		cvSave(this->getFilename(i).c_str(), this->homography.at(i));		
	}
}

void CALIB::Projector::Load()
{
	int i;
	for(i = 0;i <= HomogType::CP;i++)
	{
		const char * filename = this->getFilename(i).c_str();
//		std::ifstream ifile(filename);
//		if(!ifile)
//			continue;

		CvMat * h = (CvMat*)cvLoad(filename);
		if(h == NULL)
			continue;

		//Cvh::PrintCvMatrix(h,"Loaded Homog");
		this->SetHomography(i,h);
	}
}

std::string CALIB::Projector::getFilename(int type,string prefix,string suffix)
{
	std::stringstream ss;

	if(prefix.size() == 0)
		prefix = PROJ_FILENAME_PREFIX;

	if(suffix.size() == 0)
		suffix = PROJ_FILENAME_SUFIX;

	if(type > 0)
		ss << prefix.c_str() << this->index << "_" << type << suffix;
	else
		ss << prefix.c_str() << this->index << suffix;
	//std::cout << ss.str() << std::endl;
	return ss.str();
}

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

void CALIB::Projector::calculateHomography(CvMat * srcPlanePointsCv, CvMat * dstPlanePointsCv,int homographyType)
{
	CvMat * homography  =  this->GetHomography(homographyType);
	Cvh::CalculateHomography(srcPlanePointsCv, dstPlanePointsCv,homography);
	this->Save();
}

void CALIB::Projector::CalculateHomographyCamera(int overlappedProjectorIndex,bool calcKeystone)
{
	vector<geometry::Vector2Df> * cameraPoints = this->GetPoints(PointsType::DETECTED,overlappedProjectorIndex);
	vector<geometry::Vector2Df> * projectorPoints = this->GetPoints(PointsType::PROJECTED,overlappedProjectorIndex);

	int homogCP = HomogType::CP;
	int homogPC = HomogType::PC;

	if(overlappedProjectorIndex != NONE_INDEX)
	{
		homogCP = HomogType::CPO;
		homogPC = HomogType::PCO;
	}
	this->calculateHomography(cameraPoints,projectorPoints,homogCP);
	this->calculateHomography(projectorPoints,cameraPoints,homogPC);
	if(overlappedProjectorIndex == NONE_INDEX)
	{
		if(calcKeystone)
			this->calculateKeystoneCorrection();
		this->initPolygons();
	}
	this->Save();
}

geometry::Vector2Df * CALIB::Projector::calculateKeystoneRatio()
{
	geometry::Vector2Df * ratio = new geometry::Vector2Df(1,1);
	vector<geometry::Vector2Df> * corners = this->GetCornerPointsVector(CAMERA);

	geometry::Vector2Df v01 = corners->at(0)-corners->at(1);    //  0 ------- 1
	geometry::Vector2Df v23 = corners->at(2)-corners->at(3);    //  |			|
	geometry::Vector2Df v13 = corners->at(1)-corners->at(3);	  //  |			|
	geometry::Vector2Df v02 = corners->at(0)-corners->at(2);	  //  2 ------- 3

	ratio->SetX(v01.GetLength()/v23.GetLength());
	ratio->SetY(v13.GetLength()/v02.GetLength());
	//ratio->Print();
	return ratio;
}

void CALIB::Projector::initPolygons()
{
	vector<geometry::Vector2Df> * cornerPoints = this->GetCornerPointsVector(CAMERA);
	vector<geometry::Vector2Df> * cornerPointsReversed = new vector<geometry::Vector2Df>(cornerPoints->rbegin(),cornerPoints->rend());

	geometry::Polygon2Df * polygon = new geometry::Polygon2Df(*cornerPointsReversed);
	this->SetPolygon(this->index,polygon);

	delete cornerPoints;
	delete cornerPointsReversed;
}

geometry::Polygon2Df * CALIB::Projector::GetSelfPolygon()
{
	return this->GetPolygon(this->index);
}

geometry::Polygon2Df * CALIB::Projector::GetPolygon(int index)
{
	map<int,geometry::Polygon2Df*>::iterator iterator= this->polygons.find(index);
	if(iterator == this->polygons.end())
	{
		return NULL;
	}
	return iterator->second;
}

void CALIB::Projector::SetPolygon(int index,geometry::Polygon2Df * polygon)
{
	map<int,geometry::Polygon2Df*>::iterator iterator= this->polygons.find(index);

	if(iterator != this->polygons.end())
	{
		iterator->second = polygon;
	}
	else
	{
		this->polygons.insert(pair<int,geometry::Polygon2Df*>(index,polygon));
	}
}

void CALIB::Projector::calculateKeystoneCorrection()
{
	int luCornerX = 0; //left upper corner X
	int luCornerY = 0; //left upper corner Y
	int dW = this->width;
	int dH = this->height;

	geometry::Vector2Df * ratio = this->calculateKeystoneRatio();

	CvMat * pProjectorCv = this->GetCornerPoints(PROJECTOR);
	vector<geometry::Vector2Df> * pProjector = Cvh::ConvertMatToVector2D(pProjectorCv);

	geometry::Vector2Df p0 = pProjector->at(0);			//  0 ------- 1
	geometry::Vector2Df p1 = pProjector->at(1);			//  |		  |
	geometry::Vector2Df p2 = pProjector->at(2);			//  |		  |
	geometry::Vector2Df p3 = pProjector->at(3);  		//  3 ------- 2

	float rX = ratio->GetX();
	if(rX > 1)
		rX = 1.0f/rX;
	float rY = ratio->GetY();
	if(rY > 1)
		rY = 1.0f/rY;

	float dWratio = dW * rX;
	float dHratio = dH * rY;

	float widthDiff = dW - dWratio;
	float heightDiff = dH - dHratio;

	//p1 ratio X processing
	if(ratio->GetX() > 1)
	{
		if(ratio->GetY() > 1)
		{
			p1.SetX(p1.GetX()-widthDiff);
			p1.SetY(p1.GetY()+heightDiff);
		}
		else
		{
			p0.SetX(p0.GetX()+widthDiff);
			p0.SetY(p0.GetY()+heightDiff);
		}
	}
	else
	{
		if(ratio->GetY() > 1)
		{
			p2.SetX(p2.GetX()-widthDiff);
			p2.SetY(p2.GetY()-heightDiff);
		}
		else
		{
			p3.SetX(p3.GetX()+widthDiff);
			p3.SetY(p3.GetY()-heightDiff);
		}
	}

	vector<geometry::Vector2Df> pKeystone;
	pKeystone.push_back(p0);
	pKeystone.push_back(p1);
	pKeystone.push_back(p2);
	pKeystone.push_back(p3);

	CvMat * pkCv =  Cvh::ConvertVector2DToMat(pKeystone);
	CvMat * ppCv = Cvh::ConvertVector2DToMat(*pProjector);
	this->calculateHomography(pkCv,ppCv,KP);
	this->calculateHomography(ppCv,pkCv,PK);
}

GLuint CALIB::Projector::GetLightMapTexture()
{
	if(this->lightTextureId == -1 && this->lightMap != NULL)
	{
		IplImage * textureImage = Cvh::CreateIplimage(this->lightMap->width,this->lightMap->height,NULL);
		Cvh::AddGrayToAlphaChannel(this->lightMap,textureImage);
		//cvShowImage("textureImage",this->lightMap);
		PROJ::GL::LoadTextureFromCvImage(textureImage,&this->lightTextureId,GL_RGBA,GL_BGRA);
	}
	return this->lightTextureId;
}

void CALIB::Projector::SetLightMap(IplImage * image)
{
	this->lightMap = image;
	string lightMapFilename = this->getFilename(NONE_INDEX,PROJ_LIGHTMAPFILE_PREFIX,PROJ_LIGHTMAPFILE_SUFIX);
	cvSaveImage(lightMapFilename.c_str(),image);
}
