#include <iostream>

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

#include "geometry.h"

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

double dist(Point a, Point b)
{
	return cv::norm(a-b);
}

double inner_angle(Point a, Point middle, Point b)
{
	Point v1 = a - middle;
	Point v2 = b - middle;
	double v1_length = dist(Point(0, 0), v1);
	double v2_length = dist(Point(0, 0), v2);
	return acos( (v1.x*v2.x + v1.y*v2.y) / (v1_length*v2_length) );
}

CoordinateSystem::CoordinateSystem(double angle, Point a)
{
    theta = angle;
    origin = a;
    precompute();
    assert(map(a) == Point(0,0));
    assert(unmap(Point(0,0)) == a);
}
CoordinateSystem::CoordinateSystem(Point a, Point b)
{
	theta = atan2(b.y-a.y, b.x-a.x);
    origin = a;
    precompute();
    assert(map(a) == Point(0,0));
    assert(unmap(Point(0,0)) == a);
    assert(map(b).y == 0);
    assert(map(b).x > 0);
}

void CoordinateSystem::precompute() {
    double costheta = cos(theta);
    double sintheta = sin(theta);
	R(0,0) = costheta;
	R(0,1) = -sintheta;
	R(1,0) = sintheta;
	R(1,1) = costheta;
	Rt(0,0) = costheta;
	Rt(0,1) = sintheta;
	Rt(1,0) = -sintheta;
	Rt(1,1) = costheta;
}
Point CoordinateSystem::map(Point tomap) {
	//that's the algorithm: res = Rt*(tomap - origin)
	cv::Vec<double,2> p_as_vector;
	Point p;
	p = tomap - origin;
	p_as_vector[0] = p.x;
	p_as_vector[1] = p.y;

	cv::Vec<double,2> res_as_vector = Rt*p_as_vector;
    return Point(res_as_vector);
}
Point CoordinateSystem::unmap(Point tounmap) {
    cv::Vec<double,2> q_as_vector;
    q_as_vector[0] = tounmap.x;
    q_as_vector[1] = tounmap.y;
    Point RQ = Point(R*q_as_vector);
    return Point(RQ + origin);
}

Point
map_point(double theta, Point origin, Point tomap)
{
    auto cs = CoordinateSystem(theta, origin);
    return cs.map(tomap);
}

Point
unmap_point(double theta, Point origin, Point tounmap)
{
    return CoordinateSystem(theta, origin).unmap(tounmap);
}
	//that's the algorithm: res = Rt*(tomap - origin)
cv::Vec<double,3>
get_line(Point p, Point q)
{
    cv::Vec<double,3> line; //the three positions are a,b,c as in: ax + by + c = 0
    Point vector = q-p;
    line(0) = - vector.y;
    line(1) = vector.x;
    line(2) = vector.y * p.x - vector.x * p.y;
    assert(is_in_line(line, p));
    assert(is_in_line(line, q));
    return line;
}

bool
is_in_line(cv::Vec<double,3> line, Point p)
{
    return line_value(line, p) == 0;
}

bool
is_above_line(cv::Vec<double,3> line, Point p)
{
    return line_value(line, p) > 0;
}
double line_value(cv::Vec<double,3> line, Point p)
{
    return line(0)*p.x + line(1)*p.y + line(2);
}

cv::Point
further_point_from_line(cv::Vec<double,3> line, std::vector<Point> points)
{
    //distance = abs(a*p.x+b*p.y+c) / sqrt(a^2+b^2)
    Point further = points[0];
    double maxdist = -1;
    for(Point p: points)
    {
        double numerator = abs(line(0)*p.x + line(1)*p.y + line(2));
        if(numerator > maxdist) {
            maxdist = numerator;
            further = p;
        }
    }
    return further;
}