#include <iostream>

#ifdef HAS_OPENCV3
#include <opencv2/core.hpp> //Any OPENCV3 code
#else
#include <opencv2/core/core.hpp> //Any Opencv2 code
#endif

#include "geometry.h"
#include "cvutils.h"


#define likely(x) __builtin_expect(!!(x), 1)
#define unlikely(x) __builtin_expect(!!(x), 0)

// 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
unsigned* max2_distance(std::vector<cv::Point> hull)
{
#ifdef _DEBUG
	std::cout << "Hull = ";
	for(auto p: hull) {
		std::cout << p << " ";
	}
#endif
	std::cout << std::endl;
	unsigned *idx = (unsigned*) 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()];
		double 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;
}
bool similar_fit(std::vector<cv::Point> group, cv::Point newpoint) {
	// TODO: it should perform a fit instead of just taking first and last
	assert(group.size()>=2);
	return similar_fit(group[0], group[group.size()-1], newpoint);
}

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

/* find_longest_line: walks along the hull starting at "begin" and ending at "end", and find the
 *                    longest (well, not strictl: it's a very simple eurhistics) group of points
 *                    that are aligned we don't care about the number of points, but about the
 *                    euclidean distance between the first and the last
 *  arg hull:  the whole convex hull
 *  arg begin: index of the array to start from
 *  arg end:   index of the array to end. It can be lower than start, in which case the hull is used
 *             "circularly"
 *  returns:   a vector of useful points. Just use the first and the last, do not expect that the points in
 *             between are added to the vector
 */
std::vector<cv::Point> //two points, actually
find_longest_line(std::vector<cv::Point> hull, unsigned begin, unsigned end) //the hull can be open
{
	std::vector<cv::Point> bestline, thisline;
	int bestdistance = 0;
	thisline.push_back(hull[(begin)%hull.size()]);
	thisline.push_back(hull[(begin+1)%hull.size()]);
	for(unsigned i=(begin+2)%hull.size(); i!=end; i++)
	{
        assert(2<=thisline.size());
		if(i==hull.size()) {
			i=0;
        }
        assert(i < hull.size());
		if(similar_fit(thisline, hull[i])) {
			thisline.push_back(hull[i]);
		} else { // considering if the just-finished line is the best
			double thisdistance = dist(thisline[0],thisline[thisline.size()-1]);
			if(thisdistance>bestdistance) {
				bestline = thisline;
				bestdistance = thisdistance;
			}
			thisline.clear();
			assert(bestline.size()>=2);
			thisline.push_back(hull[(i-1)%hull.size()]);
			thisline.push_back(hull[i]);
		};
	}
	if(bestline.size()==0) { // this is the case only when the first line is the best line
		assert(0==bestdistance);
		bestline = thisline;
	}
	assert(bestline.size()>=2);
	return bestline;
}