#include "SFMatrix34f.h"
#include "X3DBaseTypes.h"

#include <iostream>

using namespace X3DTK;
using namespace std;

const SFMatrix34f SFMatrix34f::null(0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f);
const SFMatrix34f SFMatrix34f::identity(1.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f);

SFMatrix34f::SFMatrix34f() 
: _11(0.0f), _21(0.0f), _31(0.0f), _12(0.0f), _22(0.0f), _32(0.0f), _13(0.0f), _23(0.0f), _33(0.0f), _14(0.0f), _24(0.0f), _34(0.0f)
{
}

SFMatrix34f::SFMatrix34f(float _11, float _21, float _31, float _12, float _22, float _32, float _13, float _23, float _33, float _14, float _24, float _34)
{
  this->_11 = _11;
  this->_21 = _21;
  this->_31 = _31;

  this->_12 = _12;
  this->_22 = _22;
  this->_32 = _32;

  this->_13 = _13;
  this->_23 = _23;
  this->_33 = _33;

  this->_14 = _14;
  this->_24 = _24;
  this->_34 = _34;
}

SFMatrix34f::SFMatrix34f(const float GLMatrix[16])
{
  _11 = GLMatrix[0];
  _21 = GLMatrix[1];
  _31 = GLMatrix[2];
  
  _12 = GLMatrix[4];
  _22 = GLMatrix[5];
  _32 = GLMatrix[6];
  
  _13 = GLMatrix[8];
  _23 = GLMatrix[9];
  _33 = GLMatrix[10];
  
  _14 = GLMatrix[12];
  _24 = GLMatrix[13];
  _34 = GLMatrix[14];
}


SFMatrix34f::SFMatrix34f(const SFVec3f &vx, const SFVec3f &vy, const SFVec3f &vz, const SFPoint3f &p)
{
  _11 = vx.x;
  _21 = vx.y;
  _31 = vx.z;

  _12 = vy.x;
  _22 = vy.y;
  _32 = vy.z;

  _13 = vz.x;
  _23 = vz.y;
  _33 = vz.z;

  _14 = p.x;
  _24 = p.y;
  _34 = p.z;
}

SFMatrix34f::SFMatrix34f(const SFVec3f &translation, const SFVec3f &center, const SFRotation &rotation, const SFRotation &scaleOrientation, const SFVec3f &scale)
{
  SFMatrix34f Tr = SFMatrix34f::translation(translation);
  SFMatrix34f C = SFMatrix34f::translation(center);
  SFVec3f v(rotation.x, rotation.y, rotation.z);
  SFMatrix34f R = SFMatrix34f::rotation(rotation.angle, v);
  SFVec3f vs(scaleOrientation.x, scaleOrientation.y, scaleOrientation.z);
  SFMatrix34f SR = SFMatrix34f::rotation(scaleOrientation.angle, vs);
  SFMatrix34f S = scale34(scale.x, scale.y, scale.z);
  SFMatrix34f iSR = SFMatrix34f::rotation(-scaleOrientation.angle, vs);
  SFMatrix34f iC = SFMatrix34f::translation(-1.0f*center);
  *this = Tr*C*R*SR*S*iSR*iC;
}

SFMatrix34f::SFMatrix34f(const SFMatrix34f &m)
{
  _11 = m._11;
  _21 = m._21;
  _31 = m._31;

  _12 = m._12;
  _22 = m._22;
  _32 = m._32;

  _13 = m._13;
  _23 = m._23;
  _33 = m._33;

  _14 = m._14;
  _24 = m._24;
  _34 = m._34;
}

SFMatrix34f X3DTK::operator+ (const SFMatrix34f &m1, const SFMatrix34f &m2)
{
  SFMatrix34f res;

  res._11 = m1._11 + m2._11;
  res._21 = m1._21 + m2._21;
  res._31 = m1._31 + m2._31;

  res._12 = m1._12 + m2._12;
  res._22 = m1._22 + m2._22;
  res._32 = m1._32 + m2._32;

  res._13 = m1._13 + m2._13;
  res._23 = m1._23 + m2._23;
  res._33 = m1._33 + m2._33;

  res._14 = m1._14 + m2._14;
  res._24 = m1._24 + m2._24;
  res._34 = m1._34 + m2._34;

  return res;
}

SFMatrix34f X3DTK::operator- (const SFMatrix34f &m1, const SFMatrix34f &m2)
{
  SFMatrix34f res;

  res._11 = m1._11 - m2._11;
  res._21 = m1._21 - m2._21;
  res._31 = m1._31 - m2._31;

  res._12 = m1._12 - m2._12;
  res._22 = m1._22 - m2._22;
  res._32 = m1._32 - m2._32;

  res._13 = m1._13 - m2._13;
  res._23 = m1._23 - m2._23;
  res._33 = m1._33 - m2._33;

  res._14 = m1._14 - m2._14;
  res._24 = m1._24 - m2._24;
  res._34 = m1._34 - m2._34;

  return res;
}

SFMatrix34f X3DTK::operator* (const SFMatrix34f &m1, const SFMatrix34f &m2)
{
  SFMatrix34f res;

  res._11 = m1._11 * m2._11 + m1._12 * m2._21 + m1._13 * m2._31;
  res._21 = m1._21 * m2._11 + m1._22 * m2._21 + m1._23 * m2._31;
  res._31 = m1._31 * m2._11 + m1._32 * m2._21 + m1._33 * m2._31;

  res._12 = m1._11 * m2._12 + m1._12 * m2._22 + m1._13 * m2._32;
  res._22 = m1._21 * m2._12 + m1._22 * m2._22 + m1._23 * m2._32;
  res._32 = m1._31 * m2._12 + m1._32 * m2._22 + m1._33 * m2._32;

  res._13 = m1._11 * m2._13 + m1._12 * m2._23 + m1._13 * m2._33;
  res._23 = m1._21 * m2._13 + m1._22 * m2._23 + m1._23 * m2._33;
  res._33 = m1._31 * m2._13 + m1._32 * m2._23 + m1._33 * m2._33;

  res._14 = m1._11 * m2._14 + m1._12 * m2._24 + m1._13 * m2._34 + m1._14;
  res._24 = m1._21 * m2._14 + m1._22 * m2._24 + m1._23 * m2._34 + m1._24;
  res._34 = m1._31 * m2._14 + m1._32 * m2._24 + m1._33 * m2._34 + m1._34;

  return res;
}

SFMatrix34f X3DTK::operator* (const float a, const SFMatrix34f &m)
{
  SFMatrix34f res;

  res._11 = a * m._11;
  res._21 = a * m._21;
  res._31 = a * m._31;

  res._12 = a * m._12;
  res._22 = a * m._22;
  res._32 = a * m._32;

  res._13 = a * m._13;
  res._23 = a * m._23;
  res._33 = a * m._33;

  res._14 = a * m._14;
  res._24 = a * m._24;
  res._34 = a * m._34;
	
  return res;
}

SFVec3f X3DTK::operator* (const SFMatrix34f &m, const SFVec3f &v)
{
  SFVec3f res;

  res.x = m._11 * v.x + m._12 * v.y + m._13 * v.z;
  res.y = m._21 * v.x + m._22 * v.y + m._23 * v.z;
  res.z = m._31 * v.x + m._32 * v.y + m._33 * v.z;
	
  return res;
}

SFPoint3f X3DTK::operator* (const SFMatrix34f &m, const SFPoint3f &v)
{
  SFPoint3f res;

  res.x = m._11 * v.x + m._12 * v.y + m._13 * v.z + m._14;
  res.y = m._21 * v.x + m._22 * v.y + m._23 * v.z + m._24;
  res.z = m._31 * v.x + m._32 * v.y + m._33 * v.z + m._34;
	
  return res;
}

bool X3DTK::operator== (const SFMatrix34f &A, const SFMatrix34f &B)
{
  return ((A._11 == B._11) && (A._21 == B._21) && (A._31 == B._31)
       && (A._12 == B._12) && (A._22 == B._22) && (A._32 == B._32)
       && (A._13 == B._13) && (A._23 == B._23) && (A._33 == B._33)
       && (A._14 == B._14) && (A._24 == B._24) && (A._34 == B._34));
}

bool X3DTK::operator!= (const SFMatrix34f &A, const SFMatrix34f &B)
{
  return ((A._11 != B._11) || (A._21 != B._21) || (A._31 != B._31)
       || (A._12 != B._12) || (A._22 != B._22) || (A._32 != B._32)
       || (A._13 != B._13) || (A._23 != B._23) || (A._33 != B._33)
       || (A._14 != B._14) || (A._24 != B._24) || (A._34 != B._34));
}

SFMatrix34f SFMatrix34f :: i()
{
  SFMatrix34f res;
  float d;

  d = _11 * (_22 * _33 - _32 * _23) - _21 * (_12 * _33 - _32 * _13) + _31 * (_12 * _23 - _22 * _13);
	
  res._11 = (_22 * _33 - _32 * _23)/d;
  res._12 = -(_12 * _33 - _32 * _13)/d;
  res._13 = (_12 * _23 - _22 * _13)/d;

  res._21 = -(_21 * _33 - _31 * _23)/d;
  res._22 = (_11 * _33 - _31 * _13)/d;
  res._23 = -(_11 * _23 - _21 * _13)/d;

  res._31 = (_21 * _32 - _31 * _22)/d;
  res._32 = -(_11 * _32 - _31 * _12)/d;
  res._33 = (_11 * _22 - _21 * _12)/d;

  res._14 = -res._11 * _14 - res._12 * _24 - res._13 * _34;
  res._24 = -res._21 * _14 - res._22 * _24 - res._23 * _34;
  res._34 = -res._31 * _14 - res._32 * _24 - res._33 * _34;

  return res;
}

SFMatrix34f SFMatrix34f::iu()
{
  SFMatrix34f res;

  res._11 = _11;
  res._12 = _21;
  res._13 = _31;

  res._21 = _12;
  res._22 = _22;
  res._23 = _32;

  res._31 = _13;
  res._32 = _23;
  res._33 = _33;

  res._14 = -_11*_14 - _21*_24 - _31*_34;
  res._24 = -_12*_14 - _22*_24 - _32*_34;
  res._34 = -_13*_14 - _23*_24 - _33*_34;

  return res;
}

float *SFMatrix34f::toFloat16() const
{
  static float M[16];
  M[0] = _11;
  M[1] = _21;
  M[2] = _31;
  M[3] = 0.0f;

  M[4] = _12;
  M[5] = _22;
  M[6] = _32;
  M[7] = 0.0f;

  M[8] = _13;
  M[9] = _23;
  M[10] = _33;
  M[11] = 0.0f;

  M[12] = _14;
  M[13] = _24;
  M[14] = _34;
  M[15] = 1.0f;
  
  return M;
}

SFMatrix34f SFMatrix34f::scale34(float sx, float sy, float sz)
{
  SFMatrix34f res;
	
  res._11 = sx;
  res._21 = 0.0;
  res._31 = 0.0;

  res._12 = 0.0;
  res._22 = sy;
  res._32 = 0.0;

  res._13 = 0.0;
  res._23 = 0.0;
  res._33 = sz;

  res._14 = 0.0;
  res._24 = 0.0;
  res._34 = 0.0;

  return res;
}

SFMatrix34f SFMatrix34f::rotationX(float angle)
{
  SFMatrix34f res;

  res._11 = 1.0;
  res._21 = 0.0;
  res._31 = 0.0;

  res._12 = 0.0;
  res._22 = cosf(angle);
  res._32 = sinf(angle);

  res._13 = 0.0;
  res._23 = -sinf(angle);
  res._33 = cosf(angle);

  res._14 = 0.0;
  res._24 = 0.0;
  res._34 = 0.0;

  return res;
}

SFMatrix34f SFMatrix34f::rotationY(float angle)
{
  SFMatrix34f res;
	
  res._11 = cosf(angle);
  res._21 = 0.0;
  res._31 = -sinf(angle);

  res._12 = 0.0;
  res._22 = 1.0;
  res._32 = 0.0;

  res._13 = sinf(angle);
  res._23 = 0.0;
  res._33 = cosf(angle);

  res._14 = 0.0;
  res._24 = 0.0;
  res._34 = 0.0;

  return res;
}

SFMatrix34f SFMatrix34f::rotationZ(float angle)
{
  SFMatrix34f res;
	
  res._11 = cosf(angle);
  res._21 = -sinf(angle);
  res._31 = 0.0f;

  res._12 = sinf(angle);
  res._22 = cosf(angle);
  res._32 = 0.0;

  res._13 = 0.0f;
  res._23 = 0.0;
  res._33 = 1.0f;

  res._14 = 0.0;
  res._24 = 0.0;
  res._34 = 0.0;

  return res;
}

SFMatrix34f SFMatrix34f::translation(const SFVec3f &t)
{
  return SFMatrix34f(1.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 1.0f, t.x, t.y, t.z);
}

SFMatrix34f SFMatrix34f::rotation(float angle, const SFVec3f &v)
{
  SFVec3f u(v.normalized());
  SFMatrix34f S(0.0f, u.z, -u.y, -u.z, 0.0f, u.x, u.y, -u.x, 0.0f, 0.0f, 0.0f, 0.0f);
  SFMatrix34f uTu(u.x*u.x, u.y*u.x, u.z*u.x, u.x*u.y, u.y*u.y, u.z*u.y, u.x*u.z, u.y*u.z, u.z*u.z, 0.0f, 0.0f, 0.0f);
  
  return (uTu + cosf(angle)*(SFMatrix34f::identity - uTu) + sinf(angle)*S);
}