#include <math.h>

#include <iostream>
#include <algorithm>                /* sort() */

#include "Extrude.h"
#include "util/Polygon.h"
#include "util/Solver.h"

using namespace std;

#define FP_EQUAL(x,y) (fabs((x)-(y)) < 0.000001)

Extrude::Extrude()
{
}

class IntersectListComparator
{
    public:
        IntersectListComparator(Vector start) : m_start(start) {}
        bool operator()(const Shape::Intersection & i1,
                const Shape::Intersection & i2) const
        {
            double d1 = (i1.position - m_start).mag2();
            double d2 = (i2.position - m_start).mag2();
            return d1 < d2;
        }
    protected:
        Vector m_start;
};

Shape::IntersectionList Extrude::intersect(refptr<Shape> _this, const Ray & ray)
{
    Ray ray_inv = m_inverse.transform_ray(ray);
    IntersectionList res;
    int n_polygons = m_polygons.size();
    int n_offsets = m_offsets.size();

    if (n_polygons < 1 || n_offsets < 1)
        return res;

    double distance = 0.0;
    Vector scale(1.0, 1.0, 1.0);
    Vector shift(0.0, 0.0, 0.0);
    for (int p = 0; p < n_polygons; p++)
    {
        refptr<Polygon> polygon = m_polygons[p];
        for (int o = 0; o < n_offsets; o++)
        {
            Offset & offset = m_offsets[o];
            for (int pt = 0, n_pts = polygon->size(); pt < n_pts; pt++)
            {
                Vector p1 = scale.mult(*(*polygon)[pt]) + shift;
                Vector p2 = scale.mult(*(*polygon)[(pt+1) % n_pts]) + shift;
                Vector p3 = scale.mult(offset.scale)
                    .mult(*(*polygon)[(pt+1) % n_pts])
                    + shift + offset.shift;
                Vector p4 = scale.mult(offset.scale).mult(*(*polygon)[pt])
                    + shift + offset.shift;
                p1[2] += distance;
                p2[2] += distance;
                p3[2] += distance + offset.distance;
                p4[2] += distance + offset.distance;
                Vector e1 = p2 - p1;
                Vector e2 = p3 - p2;
                Vector normal = (e1 * e2).normalize();
                double a = normal[0], b = normal[1], c = normal[2];
                double d = -(a * p1[0] + b * p1[1] + c * p1[2]);
                LinearSolver solver(  a * ray_inv.getDirection()[0]
                                    + b * ray_inv.getDirection()[1]
                                    + c * ray_inv.getDirection()[2],
                                      a * ray_inv.getOrigin()[0]
                                    + b * ray_inv.getOrigin()[1]
                                    + c * ray_inv.getOrigin()[2]
                                    + d);
                Solver::Result solutions = solver.solve();
                if (solutions.numResults > 0 && solutions.results[0] > 0.0)
                {
                    Vector ipoint = ray_inv[solutions.results[0]];
                    Polygon quad;
                    quad.add(p1).add(p2).add(p3).add(p4);
                    if (quad.containsPointConvex(ipoint))
                    {
                        res.add(Intersection(_this,
                                    m_transform.transform_point(ipoint),
                                    m_transform.transform_normal(normal)));
                    }
                }
            }
            distance += offset.distance;
            scale = scale.mult(offset.scale);
            shift += offset.shift;
        }
    }

    double a = 0, b = 0, c = -1.0, d = 0.0;
    LinearSolver solver(  a * ray_inv.getDirection()[0]
                        + b * ray_inv.getDirection()[1]
                        + c * ray_inv.getDirection()[2],
                          a * ray_inv.getOrigin()[0]
                        + b * ray_inv.getOrigin()[1]
                        + c * ray_inv.getOrigin()[2]
                        + d);
    Solver::Result solutions = solver.solve();
    if (solutions.numResults > 0 && solutions.results[0] > 0.0)
    {
        Vector ipoint = ray_inv[solutions.results[0]];
        for (int p = 0; p < n_polygons; p++)
        {
            refptr<Polygon> polygon = m_polygons[p];
            if (polygon->containsPoint2D(ipoint))
            {
                res.add(Intersection(_this,
                            m_transform.transform_point(ipoint),
                            m_transform.transform_normal(Vector(a, b, c))));
            }
        }
    }
    if (scale[0] > 0.0 && scale[1] > 0.0)
    {
        a = 0, b = 0, c = 1.0, d = -distance;
        LinearSolver solver(  a * ray_inv.getDirection()[0]
                            + b * ray_inv.getDirection()[1]
                            + c * ray_inv.getDirection()[2],
                              a * ray_inv.getOrigin()[0]
                            + b * ray_inv.getOrigin()[1]
                            + c * ray_inv.getOrigin()[2]
                            + d);
        Solver::Result solutions = solver.solve();
        if (solutions.numResults > 0 && solutions.results[0] > 0.0)
        {
            Vector ipoint = ray_inv[solutions.results[0]];
            for (int p = 0; p < n_polygons; p++)
            {
                Polygon tp = *m_polygons[p];
                for (int i = 0, sz = tp.size(); i < sz; i++)
                {
                    tp[i] = new Vector(tp[i]->mult(scale) + shift);
                    (*tp[i])[2] += distance;
                }
                if (tp.containsPoint2D(ipoint))
                {
                    res.add(Intersection(_this,
                                m_transform.transform_point(ipoint),
                                m_transform.transform_normal(Vector(a, b, c))));
                    break;
                }
            }
        }
    }

    sort(res.begin(), res.end(), IntersectListComparator(ray.getOrigin()));

    return res;
}

void Extrude::addPolygon(refptr<Polygon> polygon)
{
    m_polygons.push_back(polygon);
}

void Extrude::addOffset(double distance,
        const Vector & scale, const Vector & shift)
{
    m_offsets.push_back(Offset(distance, scale, shift));
}

refptr<Shape> Extrude::clone()
{
    return new Extrude(*this);
}