#include <math.h>

#include <iostream>

#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()
{
}

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(), n_offsets = m_offsets.size();
    for (int p = 0; p < n_polygons; p++)
    {
        refptr<Polygon> polygon = m_polygons[p];
        double distance = 0.0;
        Vector scale(1.0, 1.0, 1.0);
        Vector shift(0.0, 0.0, 0.0);
        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 = *(*polygon)[pt] * scale + shift;
                Vector p2 = *(*polygon)[(pt+1) % n_pts] * scale + shift;
                Vector p3 = *(*polygon)[(pt+1) % n_pts]
                    * offset.scale + offset.shift;
                Vector p4 = *(*polygon)[pt] * offset.scale + offset.shift;
                p3[2] += offset.distance;
                p4[2] += 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];
                if (ray_inv.getDirection() % normal < 0) /* skip backfaces */
                {
                    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)
                    {
                        if (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.containsPoint(ipoint))
                            {
                                res.add(Intersection(_this,
                                            m_transform.transform_point(ipoint),
                                            m_transform.transform_normal(normal)));
                            }
                        }
                    }
                }
            }
            distance += offset.distance;
            scale = offset.scale;
            shift = offset.shift;
        }
    }

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