#include "Plane.h"
#include "util/Solver.h"
#include <math.h>
#include <iostream>
using namespace std;

Plane::Plane(double a, double b, double c, double d)
{
    m_a = a;
    m_b = b;
    m_c = c;
    m_d = d;
}

Shape::IntersectList Plane::intersect(const Ray & ray)
{
    Ray ray_inv = m_inverse.transform_ray(ray);

    IntersectList res;
    /*
     * Plane equation: ax + by + cz + d = 0
     * Ray equation:   R = R0 + tRd
     *                 x = R0x + tRdx
     *                 y = R0y + tRdy
     *                 z = R0z + tRdz
     * Combined: a(R0x + tRdx) + b(R0y + tRdy) + c(R0z + tRdz) + d = 0
     *           aR0x + (t)aRdx + bR0y + (t)bRdy + cR0z + (t)cRdz + d = 0
     *           (t)(aRdx + bRdy + cRdz) + aR0x + bR0y + cR0z + d = 0
     */
    LinearSolver solver(  m_a * ray.getDirection()[0]
                        + m_b * ray.getDirection()[1]
                        + m_c * ray.getDirection()[2],
                          m_a * ray.getOrigin()[0]
                        + m_b * ray.getOrigin()[1]
                        + m_c * ray.getOrigin()[2]
                        + m_d);
    Solver::Result solutions = solver.solve();
    if (solutions.numResults > 0)
    {
        res.push_back(m_transform.transform_point(ray_inv[solutions.results[0]]));
    }
    return res;
}

Vector Plane::getNormalAt(const Vector & pt)
{
    Vector normal(m_a, m_b, m_c);
    normal.normalize();
    return m_transform.transform_normal(normal);
}