#include <math.h>                   /* acos(), M_PI */

#include "Polygon.h"

#define FP_EQUAL(x,y) (fabs((x)-(y)) < 1E-3)

/*
 * from http://local.wasp.uwa.edu.au/~pbourke/geometry/insidepoly/
 * Return the angle between two vectors on a plane
 * The angle is from vector 1 to vector 2, positive anticlockwise
 * The result is between -pi -> pi
*/
static double angle2D(double x1, double y1, double x2, double y2)
{
    double dtheta, theta1, theta2;

    theta1 = atan2(y1,x1);
    theta2 = atan2(y2,x2);
    dtheta = theta2 - theta1;
    while (dtheta > M_PI)
        dtheta -= 2.0 * M_PI;
    while (dtheta < -M_PI)
        dtheta += 2.0 * M_PI;

    return dtheta;
}

static bool similarPoint(const Vector & v, const Vector & w)
{
    return FP_EQUAL(v[0], w[0]) && FP_EQUAL(v[1], w[1]) && FP_EQUAL(v[2], w[2]);
}

Polygon & Polygon::add(const Vector & v)
{
    int sz = size();
    if (sz < 1 || !similarPoint(v, *(*this)[sz-1]))
    {
        push_back(new Vector(v));
    }
    return *this;
}

Polygon & Polygon::add(refptr<Vector> v)
{
    int sz = size();
    if (sz < 1 || !similarPoint(*v, *(*this)[sz-1]))
    {
        push_back(v);
    }
    return *this;
}

bool Polygon::containsPoint2D(const Vector & v)
{
    if (size() < 3)
    {
        return false;
    }
    if (similarPoint(*(*this)[0], *(*this)[size()-1]))
    {
        pop_back();
    }
    double angle_sum = 0.0;
    for (int i = 0, sz = size(); i < sz; i++)
    {
        Vector v1 = *(*this)[i] - v;
        Vector v2 = *(*this)[(i+1) % sz] - v;
        double angle = angle2D(v1[0], v1[1], v2[0], v2[1]);
        angle_sum += angle;
    }
    return FP_EQUAL(angle_sum, 2.0 * M_PI);
}

bool Polygon::containsPointConvex(const Vector & v)
{
    if (size() < 3)
    {
        return false;
    }
    if (similarPoint(*(*this)[0], *(*this)[size()-1]))
    {
        pop_back();
    }
    double angle_sum = 0.0;
    for (int i = 0, sz = size(); i < sz; i++)
    {
        Vector v1 = *(*this)[i] - v;
        Vector v2 = *(*this)[(i+1) % sz] - v;
        double cosine = (v1 % v2) / (v1.mag() * v2.mag());
        double angle = acos(cosine);
        angle_sum += angle;
    }
    return FP_EQUAL(angle_sum, 2.0 * M_PI);
}