#include<cmath>
#include<algorithm>
#include<opencv2/opencv.hpp>
#include"lines.h"

// all those colors are still visible on b/w (the first component is not so low)
auto WHITE = cv::Scalar(255,255,255);
auto BLUE = cv::Scalar(200,50,50);
auto MAGENTA = cv::Scalar(110,10,200);
auto BROWN = cv::Scalar(90,100,60);

double dist(cv::Point a, cv::Point b)
{
		return sqrt((double)pow(a.x-b.x,2)+pow(a.y-b.y,2));
}

double inner_angle(cv::Point a, cv::Point middle, cv::Point b)
{
	cv::Point v1 = a - middle;
	cv::Point v2 = b - middle;
	double v1_length = dist(cv::Point(0, 0), v1);
	double v2_length = dist(cv::Point(0, 0), v2);
	return acos( (v1.x*v2.x + v1.y*v2.y) / (v1_length*v2_length) );
}
// takes the two points that are more distant from the next, ie:
// i st distance(hull[i], hull[i+1]) is maximum and 2nd maximum
int* max2_distance(std::vector<cv::Point> hull)
{
#ifdef _DEBUG
	std::cout << "Hull = ";
	for(auto p: hull) {
		std::cout << p << " ";
	}
#endif
	std::cout << std::endl;
	int *idx = (int*) calloc(2, sizeof(int));
	double distance[2] = {0};
	for( unsigned i=0; hull.size()>i; i++ )
	{
		cv::Point thispoint=hull[i];
		cv::Point nextpoint=hull[(i+1)%hull.size()];
		int d=dist(thispoint,nextpoint);
		if( d>distance[0] ) //the biggest till now
		{
			idx[1]=idx[0];
			idx[0]=i;
			distance[1]=distance[0];
			distance[0]=d;
		}
		else if( d>distance[1] ) // it is the second biggest till now
		{
			idx[1]=i;
			distance[1]=d;
		}
	}

	return idx;
}


/* check if a,b,newpoint belong to the same line (with minor approximation) */
bool similar_fit(cv::Point a, cv::Point b, cv::Point newpoint) {
	return similar_fit(a, b, newpoint, 5);
}

bool similar_fit(cv::Point a, cv::Point b, cv::Point newpoint, float tolerance_degrees) {
	double angle = inner_angle(a, b, newpoint) * 180 / M_PI;

	if( angle > (180-tolerance_degrees) &&  angle < (180+tolerance_degrees) ) {
		return true;
	}
	return false;
}
/* TODO: bool similar_fit(std::vector<cv::Point> group, cv::Point newpoint) {
 * TODO: such a prototype is better because it can do a fit on the group
 * instead of just taking the two sides
 */

/* simplify_hull will group all the points of the hull in groups that fit
 * each group is a "line"
 */

std::vector<std::vector<cv::Point>>
simplify_hull(std::vector<cv::Point> hull) {
	return simplify_hull( hull, 0 );
}
std::vector<std::vector<cv::Point>>
simplify_hull(std::vector<cv::Point> hull, double mindistance) {
	//each element is a group of point; a group of point is a vector of points that were
	//contigous and that "fit together"
	std::vector<std::vector<cv::Point>> pointgroups;
	std::vector<cv::Point> *group = new std::vector<cv::Point>();
	cv::Point last, current;
	group->push_back(hull[0]);
	group->push_back(hull[1]);
	for( unsigned i=2; hull.size()>i; i++ )
	{
		current = hull[i];
		last = (*group)[group->size()-1];
		if( similar_fit((*group)[0], last, hull[i]) )
		{
			group->push_back( hull[i] );
		}
		else
		{
			if( dist((*group)[0],last)>=mindistance )
				pointgroups.push_back(*group);
			group = new std::vector<cv::Point>();
			group->push_back(last);
			group->push_back(hull[i]);
		}
	}
	pointgroups.push_back(*group);
	//TODO: first and last might just be the same
	return pointgroups;
}

int main(int argc, char *argv[])
{
	char const *fname = "files/masckera.png";
	if( 1<argc )
		fname = argv[1];
	if( 2<argc) {
		std::cerr << "Too many arguments" << std::endl;
		return EXIT_FAILURE;
	}
	cv::Mat img;
	img=cv::imread(fname,CV_LOAD_IMAGE_GRAYSCALE);
	if( img.empty() ) {
		std::cerr << "Error opening image, aborting" << std::endl;
		return EXIT_FAILURE;
	}

	std::vector< std::vector<cv::Point> > contours;
	std::vector<cv::Vec4i> hierarchy;
	cv::findContours(img,contours,hierarchy,CV_RETR_EXTERNAL,CV_CHAIN_APPROX_SIMPLE);

	unsigned short dotwidth = img.cols >> 6;  // divide by 64, so efficient
	//img = cv::imread(fname,CV_LOAD_IMAGE_COLOR); // uncomment to this to display image with colors
	cv::drawContours(img,contours,-1,BROWN,dotwidth>>4);

	if( 0==contours.size() ) {
		std::cerr << "No contours found" << std::endl;
		return EXIT_FAILURE;
	}
	auto contour = contours[0];
	if( 1!=contours.size() )
	{
		// choosing the biggest contour in the image
		// you can test it with files/2contours.png
		for(auto cont: contours) {
			if(cont.size() > contour.size())
				contour = cont;
		}
	}

	std::vector<cv::Point> hull;
	cv::convexHull(cv::Mat(contour),hull,false);

	int *maxdistances = max2_distance(hull);

#ifdef _DEBUG
	//img = cv::imread(fname,CV_LOAD_IMAGE_COLOR); // uncomment to this to display image with colors
	std::cout << "Maxdist1: " << hull[maxdistances[0]] << hull[(maxdistances[0]+1)%hull.size()] << std::endl;
	std::cout << "Maxdist2: " << hull[maxdistances[1]] << hull[(maxdistances[1]+1)%hull.size()] << std::endl;
	cv::circle(img,hull[maxdistances[0]],dotwidth,WHITE,-1);
	cv::circle(img,hull[(maxdistances[0]+1)%hull.size()],dotwidth,WHITE,-1);
	cv::circle(img,hull[maxdistances[1]],dotwidth>>1,WHITE,-1);
	cv::circle(img,hull[(maxdistances[1]+1)%hull.size()],dotwidth>>1,WHITE,-1);
	// cv::namedWindow("win", CV_GUI_NORMAL);
	// cv::imshow("4 corners",img);
	// cv::waitKey(0);
#endif

	auto simplifiedHull = simplify_hull(hull, dist(
				hull[maxdistances[0]],
				hull[(maxdistances[0]+1)%hull.size()]) / 5
			);

#ifdef _DEBUG
	//img = cv::imread(fname,CV_LOAD_IMAGE_COLOR); // uncomment to this to display image with colors
	auto color = MAGENTA;
	for( auto group: simplifiedHull )
	{
		color = (color==BLUE) ? MAGENTA : BLUE; //flip color at every group
		cv::Point p1 = group[0];
		cv::Point p2 = group[group.size()-1];
		cv::arrowedLine(img, p1, p2, color, dotwidth>>2);
		std::cout << "simplified line: ";
		for(cv::Point p: group)
		{
			cv::circle(img,p,dotwidth>>2,color,dotwidth>>3);
			std::cout << "(" << p.x << "," << p.y << ") ";
		}
		std::cout << ";" << std::endl;
	}
	cv::imshow("win",img);
	while(1)
	{
		if( cv::waitKey(0) == 113 )
			break;
	}
#endif

	return EXIT_SUCCESS;
}

// vim: set noet ts=4 sw=4: