#include "CheckerDetector.h"	

//#include <dkt/server/GrayScaleFilter.h>
//#include <cv.h>

using namespace CALIB;

CheckerDetector::CheckerDetector()
{
	this->rows = 0;
	this->columns = 0;
	this->cvCorners = NULL;
}

bool CheckerDetector::Init(std::vector<std::string> parameter){
	if(parameter.size() != 3) return false;
	return this->Init(atoi(parameter.at(0).c_str()),atoi(parameter.at(1).c_str()),atoi(parameter.at(2).c_str()));
}

bool CheckerDetector::Init(int rows,int cols,int border){
	this->rows = rows-1;
	this->columns = cols-1;
	int myborder = border;
	if(this->rows > 0 && this->columns > 0) 
	{				
		this->InitPoints(this->rows,this->columns);
		return true;
	}
	return false;
}

void CheckerDetector::InitPoints(int rows,int cols)
{			
	//create result matrix
	if(this->cvCorners == NULL)
		delete this->cvCorners;
	this->cvCorners = new CvPoint2D32f[rows * cols];
	//clear previous results
	this->points.clear();
}


bool CheckerDetector::Detect(IplImage* cvImage, int rows,int cols)
{	
	if(rows < 0)
		rows = this->rows;
	if(cols < 0)
		cols = this->columns;


	this->InitPoints(rows,cols);
	int boardFound = 0;
	int numCorners = 0;

	//try four different thresholds to obtain checker
	int thresholds[] = {CV_CALIB_CB_ADAPTIVE_THRESH, 0,CV_CALIB_CB_NORMALIZE_IMAGE, CV_CALIB_CB_FILTER_QUADS};
	for(int j = 0; j < 4; j++)
	{
			boardFound = cvFindChessboardCorners(cvImage,
									cvSize(cols,rows),
									this->cvCorners, &numCorners,thresholds[j]);
			std::cerr << "CheckerDetector: " << numCorners <<
					" corners found (try" << j << " , thres "
					<< j << ")" << std::endl;

			//if all corners were found in the correct order
			if(numCorners == (rows * cols) && boardFound != 0)
			{
				std::cerr << "Correct corners found" << std::endl;
				//This needs a grayscale image, otherwise a strange error occurs
				//find subpixel corners
				cvFindCornerSubPix(cvImage, this->cvCorners,
														numCorners, cvSize(3, 3),
														cvSize(-1,-1),
														cvTermCriteria(CV_TERMCRIT_ITER+
														CV_TERMCRIT_EPS,5,1));
				bool normal = true;
				if(this->cvCorners[0].x > this->cvCorners[1].x)
					normal = false;
				//save corner points
				for(int k = 0; k < numCorners; k++)
				{
					geometry::Vector2Df p(this->cvCorners[k].x,this->cvCorners[k].y);
					if(normal)
						this->points.push_back(p);
					else
						this->points.insert(this->points.begin(),p);
				}//end for numCorners												
				return true;
			}//end if boardFound
	}//end for all thresholds
	return false;
}

bool CheckerDetector::DrawCorners(IplImage* cvImage, int rows,int cols)
{
	if(rows < 0)
		rows = this->rows;
	if(cols < 0)
		cols = this->columns;

	cvDrawChessboardCorners(cvImage,cvSize(cols,rows),this->cvCorners,cols* rows,true);
	return true;
}

void CheckerDetector::Print(){
	std::cerr << "Checker Detector: " << std::endl;
	std::cerr << "\t" << this->rows << " rows - "
			<< this->columns << " columns" << std::endl;
}
