#include<cmath>
#include<algorithm>

#ifdef HAS_OPENCV3
#include <opencv2/core.hpp> //Any OPENCV3 code
#else
#include <opencv2/core/core.hpp> //Any Opencv2 code
#endif
#include"lines.h"
#include "geometry.h"
#include "cvutils.h"
using namespace cv;

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

// 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);


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);

	//these are the "horizontal" lines
	unsigned *maxdistances = max2_distance(hull);
	cv::Point corn_1, corn_2, corn_3, corn_4;
	corn_1 = hull[maxdistances[0]];
	corn_2 = hull[(maxdistances[0]+1)%hull.size()];
	corn_3 = hull[maxdistances[1]];
	corn_4 = hull[(maxdistances[1]+1)%hull.size()];

#ifdef _DEBUG
	img = cv::imread(fname,CV_LOAD_IMAGE_COLOR); // uncomment to this to display image with colors
	std::cout << "Maxdist1:" << maxdistances[0] << " " << corn_1 << corn_2 << std::endl;
	std::cout << "Maxdist2:" << maxdistances[1] << " " << corn_3 << corn_4 << std::endl;
	cv::circle(img,corn_1,dotwidth>>1,MAGENTA,2);
	cv::circle(img,corn_2,dotwidth>>2,MAGENTA,2);
	cv::circle(img,corn_3,dotwidth>>2,BROWN,2);
	cv::circle(img,corn_4,dotwidth>>2,BROWN,2);
	cv::namedWindow("win", CV_GUI_NORMAL);
	// cv::imshow("4 corners",img);
	// cv::waitKey(0);
#endif

	std::vector<std::vector<cv::Point>> verticals;
	verticals.push_back(find_longest_line(hull, (maxdistances[0]+1)%hull.size(), maxdistances[1]));
	std::cout << maxdistances[1] << std::endl;
	std::cout << maxdistances[1]+1 << std::endl;
	std::cout << (maxdistances[1]+1)%hull.size() << std::endl;
	verticals.push_back(find_longest_line(hull, (maxdistances[1]+1)%hull.size(), maxdistances[0]));
	free(maxdistances);

#ifdef _DEBUG
	//img = cv::imread(fname,CV_LOAD_IMAGE_COLOR); // uncomment to this to display image with colors
	auto color = MAGENTA;
	for( auto line: verticals )
	{
		color = (color==BLUE) ? MAGENTA : BLUE; //flip color for the two lines
		cv::Point p1 = line[0];
		cv::Point p2 = line[line.size()-1];
		cv::line(img, p1, p2, color, dotwidth>>4);
		std::cout << "vertical: " << line[0] << line[line.size()-1] << std::endl;
		for(cv::Point p: line)
		{
			cv::circle(img,p,dotwidth>>3,color,-1);
		}
	}
	cv::imshow("win",img);
	while(1)
	{
		if( cv::waitKey(0) == 113 )
			break;
	}
	img.release();
#endif

	// theta is the angle of the line connecting point 1 and 2; it will be the
	// rotation of our new coordinate system
	double theta = atan(((double)corn_1.y - corn_2.y)/(corn_1.x - corn_2.x));
	std::cout << "Theta = " << theta << std::endl;
	assert(map_point(theta, corn_1, corn_1).x == 0);
	assert(map_point(theta, corn_1, corn_1).y == 0);
	assert(map_point(theta, corn_1, corn_2).y == 0);

	return EXIT_SUCCESS;
}

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