#include <algorithm>

#include "collision.h"
#include "object_s.h"
#include "obstacle_s.h"

using namespace JFK::server;

/* Sedgewick : Streckenschnitt */

static int
ccw(double x1, double y1, double x2, double y2, double x3, double y3)
{
    double          dx1 = x2 - x1;
    double          dy1 = y2 - y1;
    double          dx2 = x3 - x1;
    double          dy2 = y3 - y1;

    if (dx1 * dy2 > dy1 * dx2) return 1;
    if (dx1 * dy2 < dy1 * dx2) return -1;

    if ((dx1 * dx2 < 0) || (dy1 * dy2 < 0)) return -1;
    if ((dx1 * dx1 + dy1 * dy1) < (dx2 * dx2 + dy2 * dy2)) return 1;

    return 0;
}

static bool
intersect(double l1x1, double l1y1, double l1x2, double l1y2,
          double l2x1, double l2y1, double l2x2, double l2y2)
{
    return (ccw(l1x1, l1y1, l1x2, l1y2, l2x1, l2y1) *
             ccw(l1x1, l1y1, l1x2, l1y2, l2x2, l2y2) <= 0) &&
           (ccw(l2x1, l2y1, l2x2, l2y2, l1x1, l1y1) *
             ccw(l2x1, l2y1, l2x2, l2y2, l1x2, l1y2) <= 0);
}

/* --- Sedgewick end --- */

/* Case 1 : Collision between a single point (x3,y3) with radius r2
            and a moving object (x1,y1) moves to (x2,y2)            */
static bool
coll(double x1, double y1, double x2, double y2, double r1,
     double x3, double y3, double r2)
{
    double          d1x = x2 - x1;
    double          d1y = y2 - y1;
    double          d2x = x3 - x1;
    double          d2y = y3 - y1;
    double          d3x = x3 - x2;
    double          d3y = y3 - y2;

    // Scalar Product -> sc<0 => alpha > 90°
    double          sc1 = d1x * d2x + d1y * d2y;
    // Scalar Product
    double          sc2 = d3x * (-d1x) + d3y * (-d1y);

    double          sqr_h;

    if (sc1 >= 0 && sc2 >= 0 && (sc1 != 0 || sc2 != 0))
        // (x3,y3) between (x1,y1) and (x2,y2)
        // so get distance p1/p2 -> p3
    {
        sqr_h = ((d2x * d2x + d2y * d2y) * (d1x * d1x + d1y * d1y) -
                 (d1x * d2x + d1y * d2y) * (d1x * d2x + d1y * d2y)) /
                (d1x * d1x + d1y * d1y);
    }
    else if (sc1 < 0)           // Angle in p1 > 90°
    {
        sqr_h = d2x * d2x + d2y * d2y;
    }
    else                        // Angle in p2 > 90 °
    {
        sqr_h = d3x * d3x + d3y * d3y;
    }
    return (r1 + r2) * (r1 + r2) >= sqr_h;
}


/* Case 2 : Collision between two moving objects */

static bool
coll2(double x1, double y1, double x2, double y2, double r1,
      double x3, double y3, double x4, double y4, double r2)
// For two moving objects or obstacle-collision
// 4 times coll + intersection
{
    bool            check;

    // 4 times coll for every point

    check = (coll(x1, y1, x2, y2, r1, x3, y3, r2) ||
             coll(x1, y1, x2, y2, r1, x4, y4, r2) ||
             coll(x3, y3, x4, y4, r2, x1, y1, r1) ||
             coll(x3, y3, x4, y4, r2, x2, y2, r1));

    if (!check)
    {
        check = intersect(x1, y1, x2, y2, x3, y3, x4, y4);
    }

    return check;
}

bool
JFK::server::collision(object* obj1, object* obj2, double t)
{
    if (obj1->classname() == "obstacle")
    {
        /* This code only handles one obstacle, so return false for two
         * obstacles. The shouldn't overlap anyway. */
        if (obj2->classname() == "obstacle") return false;

        /* The code also assumes that obj2 is an obstacle (if any), so swap
         * them. */
        std::swap(obj1, obj2);
    }

    double          x1 = obj1->x;
    double          y1 = obj1->y;
    double          x2 = obj1->x + obj1->v * obj1->dx * t;
    double          y2 = obj1->y + obj1->v * obj1->dy * t;

    double          x3 = obj2->x;
    double          y3 = obj2->y;
    double          x4 = obj2->x + obj2->v * obj2->dx * t;
    double          y4 = obj2->y + obj2->v * obj2->dy * t;

    size_t          i;

    if (obj1->v == 0 && obj2->v == 0)
        // Collision only possible if explosion as nobody is moving...
    {
        return coll(x1, y1, x1, y1, obj1->r, x3, y3, obj2->r);
        // should do it...
    }
    else if (obj2->classname() == "obstacle")
    {
        // Check coll2 with all lines (obstacle has radius=0)
        obstacle*       obs = dynamic_cast<obstacle*>(obj2);

        for (i = 0; i < obs->p.size(); i++)
        {
            x3 = obs->x + obs->p[i].x;
            y3 = obs->y + obs->p[i].y;
            x4 = obs->x + obs->p[(i + 1) % obs->p.size()].x;
            y4 = obs->y + obs->p[(i + 1) % obs->p.size()].y;

            if (coll2(x1, y1, x2, y2, obj1->r, x3, y3, x4, y4, 0.0))
                return true;
        }
        return false;
    }
    else
        // Every object has a radius (!=obstacle), 
        // one is moving (v != 0)
    {
        if (obj2->v == 0)
        {
            // obj2 does not move -> normal collision-detection
            return coll(x1, y1, x2, y2, obj1->r, x3, y3, obj2->r);
        }
        else if (obj1->v == 0)
        {
            // same for obj1
            return coll(x3, y3, x4, y4, obj2->r, x1, y1, obj1->r);
        }
        else
        {
            // both objects are moving 
            return coll2(x1, y1, x2, y2, obj1->r, x3, y3, x4, y4, obj2->r);
        }
    }
}

bool 
JFK::server::in_obstacle (object* obj, obstacle* obs)
{
    double          x1 = obj->x;
    double          y1 = obj->y;

    double          x2;
    double          y2;
    double          x3;
    double          y3;

    int cc = 0; // count cuts
    int i = 0;
    
    bool online = false;

    double inf=9999999999.9; // +infinite

    for (i = 0; i < obs->p.size() && !online; i++)
    {
        x2 = obs->x + obs->p[i].x;
        y2 = obs->y + obs->p[i].y;
        x3 = obs->x + obs->p[(i + 1) % obs->p.size()].x;
        y3 = obs->y + obs->p[(i + 1) % obs->p.size()].y;

//        if (coll2(x2, y2, x3, y3, 0.1, x1, y1, inf, y1, obj->r))
//            cc++;

        if (coll2(x2, y2, x3, y3, 0.0, x1, y1, inf, y1, 0.0))
            cc++;
	    
        online = (coll(x2, y2, x3, y3, 0.1, x1, y1, obj->r));
    }    

    return (cc % 2 != 0 || online);
}