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

Transform::Transform()
{
    m_matrix = Matrix::identity();
}

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

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