/****************************************************************************
 * Copyright (C) 2009-2015 EPAM Systems
 * 
 * This file is part of Indigo toolkit.
 * 
 * This file may be distributed and/or modified under the terms of the
 * GNU General Public License version 3 as published by the Free Software
 * Foundation and appearing in the file LICENSE.GPL included in the
 * packaging of this file.
 * 
 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
 ***************************************************************************/

#include <string.h>

#include "math/algebra.h"

using namespace indigo;

IMPL_ERROR(Transform3f, "transform3f");

void Transform3f::copy (const Transform3f &other)
{
   memcpy(elements, other.elements, 16 * sizeof(float));
}

void Transform3f::getOrigin (Vec3f &origin)
{
   origin.set(elements[12], elements[13], elements[14]);
}

bool Transform3f::inversion (const Transform3f &matr)
{
   if (&matr == this)
      throw Error("can not do inversion() of self");

   if ((float)fabs(matr.elements[3]) > EPSILON ||
       (float)fabs(matr.elements[7]) > EPSILON ||
       (float)fabs(matr.elements[11]) > EPSILON)
      return false;

   elements[0] = matr.elements[0];
   elements[1] = matr.elements[4];
   elements[2] = matr.elements[8];
   elements[3] = 0;
   elements[4] = matr.elements[1];
   elements[5] = matr.elements[5];
   elements[6] = matr.elements[9];
   elements[7] = 0;
   elements[8] = matr.elements[2];
   elements[9] = matr.elements[6];
   elements[10] = matr.elements[10];
   elements[11] = 0;

   elements[12] = - matr.elements[0] * matr.elements[12]
                  - matr.elements[1] * matr.elements[13]
                  - matr.elements[2] * matr.elements[14];
   elements[13] = - matr.elements[4] * matr.elements[12]
                  - matr.elements[5] * matr.elements[13]
                  - matr.elements[6] * matr.elements[14];
   elements[14] = - matr.elements[8] * matr.elements[12]
                  - matr.elements[9] * matr.elements[13]
                  - matr.elements[10]* matr.elements[14];

   elements[15] = 1.f;

   return true;
}

void Transform3f::rotation (float x, float y, float z, float angle)
{
   float len = (float)sqrt(x * x + y * y + z * z);
   float Sin = (float)sin(angle), Cos = (float)cos(angle), Vers = 1 - Cos;

   if (len > EPSILON)
   {
      x /= len;
      y /= len;
      z /= len;
   }

   elements[0] = x * x + Cos * (1 - x * x);
   elements[1] = x * Vers * y - z * Sin;
   elements[2] = x * Vers * z + y * Sin;
   elements[3] = 0;

   elements[4] = x * Vers * y + z * Sin;
   elements[5] = y * y + Cos * (1 - y * y);
   elements[6] = y * Vers * z - x * Sin;
   elements[7] = 0;

   elements[8] = x * Vers * z - y * Sin;
   elements[9] = y * Vers * z + x * Sin;
   elements[10]= z * z + Cos * (1 - z * z);
   elements[11] = 0;

   elements[12] = 0;
   elements[13] = 0;
   elements[14] = 0;
   elements[15] = 1;
}

void Transform3f::transform (const Transform3f &transform)
{
   Transform3f tmp;

   tmp.composition(*this, transform);
   copy(tmp);
}

void Transform3f::transformLocal (const Transform3f &transform)
{
   Transform3f tmp;

   tmp.composition(transform, *this);
   copy(tmp);
}

void Transform3f::rotateX (float angle)
{
   Transform3f rot;

   rot.rotationX(angle);
   transform(rot);
}

void Transform3f::rotateY (float angle)
{
   Transform3f rot;

   rot.rotationY(angle);
   transform(rot);
}

void Transform3f::rotateZ (float angle)
{
   Transform3f rot;

   rot.rotationZ(angle);
   transform(rot);
}

void Transform3f::rotateXLocal (float angle)
{
   Transform3f rot;

   rot.rotationX(angle);
   transformLocal(rot);
}

void Transform3f::rotateYLocal (float angle)
{
   Transform3f rot;

   rot.rotationY(angle);
   transformLocal(rot);
}

void Transform3f::rotateZLocal (float angle)
{
   Transform3f rot;

   rot.rotationZ(angle);
   transformLocal(rot);
}

void Transform3f::translate (const Vec3f &translation)
{
   elements[12] += translation.x;
   elements[13] += translation.y;
   elements[14] += translation.z;
}

void Transform3f::translateInv (const Vec3f &translation)
{
   elements[12] -= translation.x;
   elements[13] -= translation.y;
   elements[14] -= translation.z;
}

void Transform3f::rotationX (float angle)
{
   float sine = (float)sin(angle), cosine = (float)cos(angle);

   memset(elements, 0, 16 * sizeof(float));

   elements[0] = 1.f;
   elements[5] = cosine;
   elements[6] = sine;
   elements[9] = -sine;
   elements[10] = cosine;
   elements[15] = 1.f;
}

void Transform3f::rotationY (float angle)
{
   float sine = (float)sin(angle), cosine = (float)cos(angle);

   memset(elements, 0, 16 * sizeof(float));

   elements[0] = cosine;
   elements[2] = -sine;
   elements[8] = sine;
   elements[10] = cosine;
   elements[5]  = 1.f;
   elements[15] = 1.f;
}

void Transform3f::rotationZ (float angle)
{
   float sine = (float)sin(angle), cosine = (float)cos(angle);

   memset(elements, 0, 16 * sizeof(float));

   elements[0] = cosine;
   elements[1] = sine;
   elements[4] = -sine;
   elements[5] = cosine;
   elements[10] = 1.f;
   elements[15] = 1.f;
}

void Transform3f::composition (const Transform3f &a, const Transform3f &b)
{
   elements[0]  = a.elements[0] * b.elements[0] +
      a.elements[1] * b.elements[4] + a.elements[2] * b.elements[8];
   elements[1]  = a.elements[0] * b.elements[1] +
      a.elements[1] * b.elements[5] + a.elements[2] * b.elements[9];
   elements[2]  = a.elements[0] * b.elements[2] +
      a.elements[1] * b.elements[6] + a.elements[2] * b.elements[10];
   elements[3]  = 0;
   elements[4]  = a.elements[4] * b.elements[0] +
      a.elements[5] * b.elements[4] + a.elements[6] * b.elements[8];
   elements[5]  = a.elements[4] * b.elements[1] +
      a.elements[5] * b.elements[5] + a.elements[6] * b.elements[9];
   elements[6]  = a.elements[4] * b.elements[2] +
      a.elements[5] * b.elements[6] + a.elements[6] * b.elements[10];
   elements[7]  = 0;
   elements[8]  = a.elements[8] * b.elements[0] +
      a.elements[9] * b.elements[4] + a.elements[10] * b.elements[8];
   elements[9]  = a.elements[8] * b.elements[1] +
      a.elements[9] * b.elements[5] + a.elements[10] * b.elements[9];
   elements[10] = a.elements[8] * b.elements[2] +
      a.elements[9] * b.elements[6] + a.elements[10] *b.elements[10];
   elements[11] = 0;
   elements[12] = a.elements[12] * b.elements[0] +
      a.elements[13] * b.elements[4] + 
      a.elements[14] * b.elements[8] + b.elements[12];
   elements[13] = a.elements[12] * b.elements[1] +
      a.elements[13] * b.elements[5] + 
      a.elements[14] * b.elements[9] + b.elements[13];
   elements[14] = a.elements[12] * b.elements[2] +
      a.elements[13] * b.elements[6] +                   
      a.elements[14] * b.elements[10] + b.elements[14];
   elements[15] = 1;
}

void Transform3f::identity (void)
{
   memset(elements, 0, 16 * sizeof(float));

   elements[0] = 1.f;
   elements[5] = 1.f;
   elements[10] = 1.f;
   elements[15] = 1.f;
}

void Transform3f::translateLocal (float x, float y, float z)
{
   elements[12] += elements[0] * x + elements[4] * y + elements[8] * z;
   elements[13] += elements[1] * x + elements[5] * y + elements[9] * z;
   elements[14] += elements[2] * x + elements[6] * y + elements[10]* z;
}

void Transform3f::translateLocal (const Vec3f &translation)
{
   translateLocal(translation.x, translation.y, translation.z);
}

void Transform3f::translateLocalInv (const Vec3f &translation)
{
   translateLocal(-translation.x, -translation.y, -translation.z);
}

void Transform3f::setOrigin (float x, float y, float z)
{
   elements[12] = x;
   elements[13] = y;
   elements[14] = z;
}

void Transform3f::setOrigin (const Vec3f &origin)
{
   setOrigin(origin.x, origin.y, origin.z);
}

bool Transform3f::rotationVecVec (const Vec3f &v1, const Vec3f &v2)
{
   Vec3f v1_norm, v2_norm;

   if (!v1_norm.normalization(v1) || !v2_norm.normalization(v2))
      return false;

   Vec3f cross;

   cross.cross(v1_norm, v2_norm);   

   if (!cross.normalize())
   {
      // cross product have zero length -> v1 & v2 are codirectional
      identity();
      return true;
   }

   float dot = Vec3f::dot(v1_norm, v2_norm);
   
   float ang;
   
   if (dot > 1 - EPSILON)
      ang = 0;
   else if (dot < -1 + EPSILON)
      ang = -PI;
   else
      ang = -(float)acos(dot);

   rotation(cross.x, cross.y, cross.z, ang);
   return true;
}