#include "Cvh.h"

using namespace CALIB; 

CALIB::Cvh::Cvh()
{
}

CALIB::Cvh::~ Cvh()
{
}

void CALIB::Cvh::PrintCvMatrix(CvMat * mat,std::string name)
{
	std::cout << "************************************" << std::endl;
	std::cout << name << std::endl;
	std::cout << "[" << std::endl;
	for(int i = 0;i < mat->rows;i++)
	{
		for(int j = 0;j < mat->cols;j++)
		{
			std::cout << cvmGet(mat,i,j) << '\t';
		}
		if(i != mat->rows-1)
			std::cout << ";"<< std::endl;
	}
	std::cout << "];" << std::endl;
	std::cout << "##################" << std::endl;
      
}

IplImage * CALIB::Cvh::convertRGBtoGray(IplImage * src)
{		 
	/* get image properties */
	int width  = src->width;
	int height = src->height;
	 
	/* create new image for the grayscale version */
	IplImage *dst = cvCreateImage( cvSize( width, height ), IPL_DEPTH_8U, 1 );
	 
	/* CV_RGB2GRAY: convert RGB image to grayscale */
	cvCvtColor( src, dst, CV_RGB2GRAY );
	return dst;
}

CvMat * CALIB::Cvh::MatrixAddCols(CvMat * mat1,CvMat *mat2)
{
	int cols = mat1->cols + mat2->cols;
	int rows = mat1->rows;
	CvMat * result = cvCreateMat(rows,cols,CV_64FC1);
      
	//copy first matrix to new one
	for(int i = 0;i < mat1->rows;i++)
	{
		for(int j = 0;j < mat1->cols;j++)
		{
			cvmSet(result,i,j,cvmGet(mat1,i,j));
		}
	}
      
	//add cols of second matrix to new matrix
	for(int i = 0;i < mat2->rows;i++)
	{
		for(int j = 0;j < mat2->cols;j++)
		{
			cvmSet(result,i,j+mat1->cols,cvmGet(mat2,i,j));
		}
	}
	return result;
}

CvMat * CALIB::Cvh::MatrixAddRows(CvMat * mat1,CvMat *mat2)
{
	int rows = mat1->rows + mat2->rows;
	int cols = mat1->cols;
	CvMat * result = cvCreateMat(rows,cols,CV_64FC1);
      
	//copy first matrix to new one
	for(int i = 0;i < mat1->rows;i++)
	{
		for(int j = 0;j < mat1->cols;j++)
		{
			cvmSet(result,i,j,cvmGet(mat1,i,j));
		}
	}
      
	//add rows of second matrix to new matrix
	for(int i = 0;i < mat2->rows;i++)
	{
		for(int j = 0;j < mat2->cols;j++)
		{
			cvmSet(result,i+mat1->rows,j,cvmGet(mat2,i,j));
		}
	}
	return result;
}

CvMat * CALIB::Cvh::MatMul(CvMat * mat1,CvMat * mat2)
{
  CvMat * dst = cvCreateMat(mat1->rows,mat2->cols,CV_64FC1);
  for(int row = 0;row < dst->rows;row++)
  {
    for(int col = 0;col < dst->cols;col++)
    {
      double sum = 0;
      for(int i = 0;i < mat1->cols;i++)
      {
				sum += cvmGet(mat1,row,i) * cvmGet(mat2,i,col);	
      }
      cvmSet(dst,row,col,sum);
    }
  }
  return dst;
}

IplImage * CALIB::Cvh::LoadImage(const char * filename)
{
	IplImage *img = 0;

	//load the image,
	img = cvLoadImage( filename, CV_LOAD_IMAGE_COLOR );

	if( img == 0 ) 
	{
		std::cerr << "Cannot load file " << filename << "!" << std::endl;
		return NULL;
	}
	return img;
}

void CALIB::Cvh::MergeMatrixes(CvMat * src1,CvMat * src2,CvMat * dst)
{
	bool useRows = false;
	int size = src1->cols;
	int recordSize = src1->rows;
	
	if(src1->rows > src1->cols)
	{
		useRows = true;
		size = src1->rows;
		recordSize = src1->cols;
	}	
	for(int i = 0;i < size;i++)
	{
		for(int j = 0;j < recordSize;j++)
		{
			if(useRows) 
				cvmSet(dst,i,j,cvmGet(src1,i,j));
			else
				cvmSet(dst,j,i,cvmGet(src1,j,i));
		}
	}
	
	useRows = false;
	size = src2->cols;
	recordSize = src2->rows;
	
	if(src2->rows > src2->cols)
	{
		useRows = true;
		size = src2->rows;
		recordSize = src2->cols;
	}	
	for(int i = 0;i < size;i++)
	{
		
		for(int j = 0;j < recordSize;j++)
		{
			if(useRows) 
			{			
				cvmSet(dst,i + src1->rows,j,cvmGet(src2,i,j));
			}
			else
			{				
				cvmSet(dst,j,i + src1->cols,cvmGet(src2,j,i));
			}
		}
	}
}

std::vector<geometry::Vector2Df> * CALIB::Cvh::ConvertMatToVector2D(CvMat *mat)
{
	int i = 0;
	int max = mat->rows;
	
	if(mat->cols > mat->rows)
	{
		max = mat->cols;
	}
	
	std::vector<geometry::Vector2Df> * res = new std::vector<geometry::Vector2Df>();
	for(int i = 0;i < max;i++)
	{
		if(mat->cols > mat->rows)
		{
			res->push_back(geometry::Vector2Df(cvmGet(mat,ZERO_INDEX,i),cvmGet(mat,FIRST_INDEX,i)));
		}
		else
		{
			res->push_back(geometry::Vector2Df(cvmGet(mat,i,ZERO_INDEX),cvmGet(mat,i,FIRST_INDEX)));
		}
		
	}
	return res;
}

void CALIB::Cvh::ConvertMatToVector2D(CvMat *mat,std::vector<geometry::Vector2Df> * dstPoints2D)
{
	int i = 0;
	int max = mat->rows;
	
	if(mat->cols > mat->rows)
	{
		max = mat->cols;
	}
	
	for(int i = 0;i < max;i++)
	{
		if(mat->cols > mat->rows)
		{
			dstPoints2D->push_back(geometry::Vector2Df(cvmGet(mat,ZERO_INDEX,i),cvmGet(mat,FIRST_INDEX,i)));
		}
		else
		{
			dstPoints2D->push_back(geometry::Vector2Df(cvmGet(mat,i,ZERO_INDEX),cvmGet(mat,i,FIRST_INDEX)));
		}
		
	}
}

CvMat * CALIB::Cvh::ConvertVector2DToMat(std::vector<geometry::Vector2Df> points)
{
		CvMat * pts = cvCreateMat(TWOD_VECTOR_LENGHT, points.size(), CV_64FC1); 
		
		for(int i = 0; i < points.size(); i++){
				cvmSet(pts, ZERO_INDEX, i, points.at(i).GetX()); 
				cvmSet(pts, FIRST_INDEX, i, points.at(i).GetY()); 
		}
		return pts; 		
}

void CALIB::Cvh::ConvertPoints2DToMat(CvPoint2D32f* points,int numPoints,CvMat * dst)
{		
	for(int i = 0; i < numPoints; i++)
	{
		cvmSet(dst, ZERO_INDEX, i, points[i].x); 
		cvmSet(dst, FIRST_INDEX, i, points[i].y); 
	}	
}

CvMat * CALIB::Cvh::CreateMat2D(int size)
{
	return Cvh::CreateMat(TWOD_VECTOR_LENGHT, size); 
}

CvMat * CALIB::Cvh::CreateMat(int rows,int cols,CvScalar value)
{
	CvMat * mat = cvCreateMat(rows,cols, CV_64FC1); 
	cvSetIdentity(mat,value);
	return mat;
}


CvMat * CALIB::Cvh::GetChackerboardInnerCorners(int x, int y, int size,int rows,int cols)
{
	rows -= 1;
	cols -= 1;
	CvMat * dst = Cvh::CreateMat2D(rows*cols);


	int origX = x + size;
	y = y + size;	
	int index = 0;
	for(int i = 0;i < rows;i++)
	{
		x = origX;		
		for(int j = 0;j < cols;j++)
		{			
			cvmSet(dst, ZERO_INDEX, index,x); 
			cvmSet(dst, FIRST_INDEX, index, y); 
			x+= size;		
			index++;
		}
		y+= size;
	}
	return dst;
}

CvMat * CALIB::Cvh::GetChackerboardCorners(int x, int y, int size,int rows,int cols)
{
	CvMat * dst = Cvh::CreateMat2D(rows*cols*2);

	int origX = x + size;
	y = y + size;
	int index = 0;
	for(int i = 0;i < rows;i++)
	{
		x = origX;
		for(int j = 0;j < cols;j++)
		{
			cvmSet(dst, ZERO_INDEX, index,x);
			cvmSet(dst, FIRST_INDEX, index, y);
			cvmSet(dst, ZERO_INDEX, index+1,x+size);
			cvmSet(dst, FIRST_INDEX, index+1, y+size);

			x+= size;
			index+=2;
		}
		y+= size;
	}
	return dst;
}

void CALIB::Cvh::TransformPoints(CvMat * src,CvMat * dst,CvMat * homography)
{
	bool debug = false;
	if(debug)
		PrintCvMatrix(homography,"homography");
	CvMat * tmpInputPoint = Cvh::CreateMat(TWO_DIM_HOMOGENOUS,1);
	cvmSet(tmpInputPoint,2,0,1);

	for(int i = 0;i < src->cols;i++)
	{
		cvmSet(tmpInputPoint,0,0,cvmGet(src,0,i));
		cvmSet(tmpInputPoint,1,0,cvmGet(src,1,i));				
		
		if(debug)
			PrintCvMatrix(tmpInputPoint,"tmpInputPoint");
		CvMat * tmpOutputPoint = MatMul(homography,tmpInputPoint);
		
		if(debug)
			PrintCvMatrix(tmpOutputPoint,"tmpOutputPoint");
	
		double w = cvmGet(tmpOutputPoint,2,0);
		cvmSet(dst,0,i,cvmGet(tmpOutputPoint,0,0)/w);
		cvmSet(dst,1,i,cvmGet(tmpOutputPoint,1,0)/w);
				
		cvReleaseMat(&tmpOutputPoint); 
	}
	if(debug)
		PrintCvMatrix(dst,"CALC - output transformed points");
	cvReleaseMat(&tmpInputPoint);
}


CvMat * CALIB::Cvh::ConvertVector3DToMat(std::vector<geometry::Vector3Dd> points)
{
		CvMat * pts = cvCreateMat(THREED_VECTOR_LENGHT, points.size(), CV_64FC1); 
		
		for(int i = 0; i < points.size(); i++){
				cvmSet(pts, ZERO_INDEX, i, points.at(i).GetX()); 
				cvmSet(pts, FIRST_INDEX, i, points.at(i).GetY()); 
				cvmSet(pts, SECOND_INDEX, i, points.at(i).GetZ()); 
		}
		
		return pts; 		
}

std::vector<geometry::Vector3Dd> * CALIB::Cvh::ConvertMatToVector3D(CvMat *mat)
{
	int i = 0;
	int max = mat->rows;
	
	if(mat->cols > mat->rows)
	{
		max = mat->cols;
	}
	
	std::vector<geometry::Vector3Dd> * res = new std::vector<geometry::Vector3Dd>();
	for(int i = 0;i < max;i++)
	{
		if(mat->cols > mat->rows)
		{
			res->push_back(geometry::Vector3Dd(cvmGet(mat,ZERO_INDEX,i),cvmGet(mat,FIRST_INDEX,i),cvmGet(mat,SECOND_INDEX,i)));
		}
		else
		{
			res->push_back(geometry::Vector3Dd(cvmGet(mat,i,ZERO_INDEX),cvmGet(mat,i,FIRST_INDEX),cvmGet(mat,i,SECOND_INDEX)));
		}
	}
	return res;		
}

CvMat * CALIB::Cvh::ConvertMat3To4(CvMat * src,bool addDiagonalValue)
{	
	CvMat * dstMat = Cvh::CreateMat(4,4,cvRealScalar(1));
	Cvh::CopyMatrix(src,dstMat);
	if(!addDiagonalValue)
		cvmSet(dstMat,3,3,0);
	return dstMat;
}

void CALIB::Cvh::CopyMatrix(CvMat * src,CvMat * dst)
{
	for(int i = 0;i < src->rows;i++)
	{
		for(int j = 0;j < src->cols;j++)
		{
			cvmSet(dst,i,j,cvmGet(src,i,j));			
		}
	}
}

void CALIB::Cvh::CalculateHomography(CvMat* srcPlanePoints, CvMat* dstPlanePoints,CvMat * homography)
{
	cvSetIdentity(homography,cvScalar(0));
	cvFindHomography(srcPlanePoints,dstPlanePoints,homography);
	//cvFindHomography(srcPlanePoints,dstPlanePoints,homography,CV_RANSAC,0.5);

//	Cvh::PrintCvMatrix(srcPlanePoints,"srcPlanePoints");
//	Cvh::PrintCvMatrix(dstPlanePoints,"dstPlanePoints");
//	Cvh::PrintCvMatrix(homography,"homography");
}

IplImage * CALIB::Cvh::CreateIplimage(int width,int height,unsigned char * imageData,int channels,int depth,bool flipY)
{
	IplImage * cvImage = cvCreateImage(cvSize(width,height),depth,channels);
	if(!cvImage)
		return NULL;
	if(imageData != NULL)
		memcpy(cvImage->imageData,imageData,channels*width*height);
	if(flipY)
	{
		cvFlip(cvImage,NULL);
	}
	return cvImage;
}



IplImage * CALIB::Cvh::DivImages(IplImage * dividendImage,IplImage * aliquotImage)
{
	IplImage* resultImage = cvCreateImage( cvSize( dividendImage->width, dividendImage->height ), IPL_DEPTH_8U, 1 );
	unsigned char alChar,diChar;
	float divendent,aliquot;
	float intensity;

	for(int i = 0;i < dividendImage->imageSize;i++)
	{
		diChar = dividendImage->imageData[i];
		alChar = aliquotImage->imageData[i];
		divendent = diChar;
		aliquot = alChar;

		if(divendent != aliquot && aliquot != 0)
		{
			//Linear function y = x
//			intensity = (divendent*GRAY_MAX)/aliquot;

			//Sin function y=sin(x) x-> <0;pi/2>
			intensity = (divendent*M_PI_2)/aliquot;
			intensity = sin(intensity);
			intensity = intensity*GRAY_MAX;

			//Function log(x)
//			float aa = 32;
//			intensity = ((float)divendent*aa)/(float)aliquot;
//			intensity = log(intensity);
//			if(intensity < 0)
//				intensity = 0;
//			intensity = intensity/(log(aa));

			//Function 1/x
//			intensity = ((float)divendent*2)/(float)aliquot;
//			intensity = ((-1/intensity)+10.0f)/10.0f;
//			if(intensity < 0)
//				intensity = 0;
//			intensity = intensity*GRAY_MAX;

			if(intensity > GRAY_MAX)
				intensity = GRAY_MAX;
			else if(intensity < 0)
			{
				intensity = 0;
			}
		}
		else
			intensity = GRAY_MAX;

		unsigned char intensChar = intensity;
		resultImage->imageData[i] = intensChar;
	}

	return resultImage;
}

void CALIB::Cvh::AddGrayToAlphaChannel(IplImage * grayImage,IplImage * dstImage)
{
	//B,G,R,A
	cvMerge(NULL,NULL,NULL,grayImage,dstImage);
}
