#include "Transform.h"
#include <math.h>

Transform::Transform()
{
}

Transform Transform::getInverse()
{
    Transform inv;
    inv.m_matrix = m_matrix.getInverse();
    return inv;
}

void Transform::lookAt(const Vector & eye,
                       const Vector & focus,
                       const Vector & up)
{
    Vector forward = focus - eye;
    forward.normalize();
    Vector perpendicular_up = (up - up.proj(forward)).normalize();
    Vector right = forward * perpendicular_up;
    Matrix mult;
    mult[0][0] = right[0];
    mult[0][1] = right[1];
    mult[0][2] = right[2];
    mult[1][0] = forward[0];
    mult[1][1] = forward[1];
    mult[1][2] = forward[2];
    mult[2][0] = perpendicular_up[0];
    mult[2][1] = perpendicular_up[1];
    mult[2][2] = perpendicular_up[2];
    m_matrix *= mult;
    translate(-eye[0], -eye[1], -eye[2]);
}

void Transform::translate(double x, double y, double z)
{
    Matrix t = Matrix::identity();
    t[0][3] = x;
    t[1][3] = y;
    t[2][3] = z;
    m_matrix *= t;
}

void Transform::rotate(double angle, double xv, double yv, double zv)
{
    /* formula from http://en.wikipedia.org/wiki/Rotation_matrix */
    Vector l(xv, yv, zv);
    l.normalize();

    double c = cos(M_PI * angle / 180.0);
    double s = sin(M_PI * angle / 180.0);

    double lx2 = l[0] * l[0];
    double ly2 = l[1] * l[1];
    double lz2 = l[2] * l[2];

    Matrix t = Matrix::identity();
    t[0][0] = lx2 + (1 - lx2) * c;
    t[0][1] = l[0] * l[1] * (1 - c) - l[2] * s;
    t[0][2] = l[0] * l[2] * (1 - c) + l[1] * s;

    t[1][0] = l[0] * l[1] * (1 - c) + l[2] * s;
    t[1][1] = ly2 + (1 - ly2) * c;
    t[1][2] = l[1] * l[2] * (1 - c) - l[0] * s;

    t[2][0] = l[0] * l[2] * (1 - c) - l[1] * s;
    t[2][1] = l[1] * l[2] * (1 - c) + l[0] * s;
    t[2][2] = lz2 + (1 - lz2) * c;

    m_matrix *= t;
}

void Transform::scale(double xs, double ys, double zs)
{
    Matrix t = Matrix::identity();
    t[0][0] = xs;
    t[1][1] = ys;
    t[2][2] = zs;
    m_matrix *= t;
}

Vector Transform::transform_point(const Vector & v)
{
    return m_matrix * v;
}

Vector Transform::transform_direction(const Vector & v)
{
    return m_matrix % v;
}

Vector Transform::transform_normal(const Vector & v)
{
    return (m_matrix % v).normalize();
}

Ray Transform::transform_ray(const Ray & r)
{
    Vector newPosition = m_matrix * r.getOrigin();
    Vector newDirection = m_matrix % r.getDirection();
    Ray res(newPosition, newDirection);
    return res;
}