#include <amsmathutil25/amsmathutil25.hpp>

namespace ams
{

// vec4 constructors
vec4::vec4()
{
    x = 0.0;
    y = 0.0;
    z = 0.0;
    w = 0.0;
    return;
}

vec4::vec4(double _x, double _y, double _z, double _w)
{
    x = _x;
    y = _y;
    z = _z;
    w = _w;  
    return;
}

vec4::vec4(const vec4 &rhs)
{
    x = rhs.x;
    y = rhs.y;
    z = rhs.z;
    w = rhs.w;
    return;
}

// vec4 assignment and comparison operators
vec4& vec4::operator=(const vec4 &rhs)
{
    this->x = rhs.x;
    this->y = rhs.y;
    this->z = rhs.z;
    this->w = rhs.w;
    return *this;
}

bool vec4::operator==(vec4 rhs)
{
    return ((this->x == rhs.x) && (this->y == rhs.y) && (this->z == rhs.z) && (this->w == rhs.w));
}

// vec4 arithmetic operators
vec4 vec4::operator+(vec4 rhs) const
{
    vec4 ret;
    ret.x = this->x + rhs.x;
    ret.y = this->y + rhs.y;
    ret.z = this->z + rhs.z;
    ret.w = this->w + rhs.w;
    return ret;
}

vec4 vec4::operator-(vec4 rhs) const
{
    vec4 ret;
    ret.x = this->x - rhs.x;
    ret.y = this->y - rhs.y;
    ret.z = this->z - rhs.z;
    ret.w = this->w - rhs.w;
    return ret;
}

vec4 vec4::operator*(double rhs) const
{
    vec4 ret;
    ret.x = this->x*rhs;
    ret.y = this->y*rhs;
    ret.z = this->z*rhs;
    ret.w = this->w*rhs;
    return ret;
}

vec4 vec4::operator/(double rhs) const
{
    vec4 ret;
    ret.x = this->x/rhs;
    ret.y = this->y/rhs;
    ret.z = this->z/rhs;
    ret.w = this->w/rhs;
    return ret;
}

vec4 operator-(vec4 rhs)
{
    vec4 ret;
    ret.x = -rhs.x;
    ret.y = -rhs.y;
    ret.z = -rhs.z;
    ret.w = -rhs.w;
    return ret;
}

// vec4 subscript operators
double& vec4::operator[](int ind)
{
    if(ind==0) return x;
    else if(ind==1) return y;
    else if(ind==2) return z;
    else if(ind==3) return w;
    return x;
}

const double& vec4::operator[](int ind) const
{
    if(ind==0) return x;
    else if(ind==1) return y;
    else if(ind==2) return z;
    else if(ind==3) return w;
    return x;
}

// vector operations (new nomenclature)
double vec4_dot(vec4 v1, vec4 v2)
{
    double ret = 0.0;
    ret += v1.x*v2.x;
    ret += v1.y*v2.y;
    ret += v1.z*v2.z;
    ret += v1.w*v2.w;
    return ret;
}

double vec4_norm(vec4 v)
{
    double ret = ::sqrt(v.x*v.x+v.y*v.y+v.z*v.z+v.w*v.w);
    return ret;
}

vec4 vec4_normalize(vec4 v)
{
    vec4 ret = vec4(0,0,0,0);
    double nrm = vec4_norm(v);
    if(nrm>0.0)
    {
        ret = v/nrm;
    }
    return ret;
}

double vec4_dist(vec4 v1, vec4 v2)
{
    return vec4_norm(v1-v2);
}

vec4 vec4_proj(vec4 v1, vec4 v2)
{
    vec4 ret;
    vec4 v2n = vec4_normalize(v2);
    ret = v2n*vec4_dot(v1,v2n);
    return ret;
}

// vector operations (old nomenclature)
double vdot(vec4 v1, vec4 v2)
{
    return vec4_dot(v1,v2);
}

double vnorm(vec4 v)
{
    return vec4_norm(v);
}

vec4 vnormalize(vec4 v)
{
    return vec4_normalize(v);
}

double vdist(vec4 v1, vec4 v2)
{
    return vec4_dist(v1,v2);
}

vec4 vproj(vec4 v1, vec4 v2)
{
    return vec4_proj(v1,v2);
}

// mat4 constructors
mat4::mat4()
{
    int I;
    for(I=0;I<16;I++) data[I] = 0.0;
    return;
}

mat4::mat4(
    double _xx, double _yx, double _zx, double _wx,
    double _xy, double _yy, double _zy, double _wy,
    double _xz, double _yz, double _zz, double _wz,
    double _xw, double _yw, double _zw, double _ww
)
{
    data[0] = _xx;
    data[1] = _yx;
    data[2] = _zx;
    data[3] = _wx;
    
    data[4] = _xy;
    data[5] = _yy;
    data[6] = _zy;
    data[7] = _wy;
    
    data[8] = _xz;
    data[9] = _yz;
    data[10] = _zz;
    data[11] = _wz;
    
    data[12] = _xw;
    data[13] = _yw;
    data[14] = _zw;
    data[15] = _ww;
    
    return;
}

mat4::mat4(const double *_data)
{
    int I;
    for(I=0;I<16;I++) data[I] = _data[I];
    return;
}

mat4::mat4(const mat4 &rhs)
{
    int I;
    for(I=0;I<16;I++) data[I] = rhs.data[I];
    return;
}

mat4::mat4(const vec4 _col1, const vec4 _col2, const vec4 _col3, const vec4 _col4)
{
    data[0] = _col1[0];
    data[1] = _col1[1];
    data[2] = _col1[2];
    data[3] = _col1[3];

    data[4] = _col2[0];
    data[5] = _col2[1];
    data[6] = _col2[2];
    data[7] = _col2[3];

    data[8] = _col3[0];
    data[9] = _col3[1];
    data[10] = _col3[2];
    data[11] = _col3[3];

    data[12] = _col4[0];
    data[13] = _col4[1];
    data[14] = _col4[2];
    data[15] = _col4[3];
    
    return;
}

// mat4 assignment and comparison operators
mat4& mat4::operator=(const mat4 rhs)
{
    int I;
    for(I=0;I<16;I++) data[I] = rhs.data[I];
    return *this;
}

bool mat4::operator==(const mat4 rhs)
{
    bool ret = 1;
    int I;
    for(I=0;I<16;I++)
    {
        if(this->data[I]!=rhs.data[I])
        {
            ret = 0;
            break;
        }
    }

    return ret;
}

// mat4 arithmetic operators
mat4 mat4::operator+(mat4 rhs) const
{
    mat4 ret;
    int I;
    for(I=0;I<16;I++)
    {
        ret.data[I] = this->data[I] + rhs.data[I];
    }
    return ret;
}

mat4 mat4::operator-(mat4 rhs) const
{
    mat4 ret;
    int I;
    for(I=0;I<16;I++)
    {
        ret.data[I] = this->data[I] - rhs.data[I];
    }
    return ret;
}

mat4 mat4::operator*(mat4 rhs) const
{
    mat4 ret = mat4_zeros();
    int I,J,K;
    for(I=0;I<4;I++)
    for(J=0;J<4;J++)
    for(K=0;K<4;K++)
        ret(I,K) += (*this)(I,J)*rhs(J,K);
    return ret;
}

mat4 mat4::operator/(mat4 rhs) const
{
    mat4 rhsi = rhs.inverse();
    return (*this)*rhsi;
}

mat4 mat4::operator*(double rhs) const
{
    mat4 ret;
    int I;
    for(I=0;I<16;I++) ret.data[I] = this->data[I]*rhs;
    return ret;
}

mat4 mat4::operator/(double rhs) const
{
    mat4 ret;
    int I;
    for(I=0;I<16;I++) ret.data[I] = this->data[I]/rhs;
    return ret;
}

vec4 mat4::operator*(vec4 rhs) const
{
    vec4 ret = vec4(0,0,0,0);
    int I,J;
    for(I=0;I<4;I++)
    for(J=0;J<4;J++)
        ret[I] += (*this)(I,J)*rhs[J];
    return ret;
}

// mat4 access operators
double& mat4::operator()(int I, int J)
{
    int ind = I + 4*J;
    return data[ind];
}

double& mat4::operator[](int I)
{
    return data[I];
}

const double& mat4::operator()(int I, int J) const
{
    int ind = I + 4*J;
    return data[ind];
}

const double& mat4::operator[](int I) const
{
    return data[I];
}

double& mat4::at(int I, int J)
{
    int ind = I + 4*J;
    return data[ind];
}

const double& mat4::at(int I, int J) const
{
    int ind = I + 4*J;
    return data[ind];
}

double mat4::det()
{
    double ret = 0.0;
    double a00,a01,a02,a03;
    double a10,a11,a12,a13;
    double a20,a21,a22,a23;
    double a30,a31,a32,a33;
  
    a00 = this->at(0,0);
    a01 = this->at(0,1);
    a02 = this->at(0,2);
    a03 = this->at(0,3);
    a10 = this->at(1,0);
    a11 = this->at(1,1);
    a12 = this->at(1,2);
    a13 = this->at(1,3);
    a20 = this->at(2,0);
    a21 = this->at(2,1);
    a22 = this->at(2,2);
    a23 = this->at(2,3);
    a30 = this->at(3,0);
    a31 = this->at(3,1);
    a32 = this->at(3,2);
    a33 = this->at(3,3);
  
    ret = a03*a12*a21*a30 -
          a02*a13*a21*a30 -
          a03*a11*a22*a30 +
          a01*a13*a22*a30 +
          a02*a11*a23*a30 -
          a01*a12*a23*a30 -
          a03*a12*a20*a31 +
          a02*a13*a20*a31 +
          a03*a10*a22*a31 -
          a00*a13*a22*a31 -
          a02*a10*a23*a31 +
          a00*a12*a23*a31 +
          a03*a11*a20*a32 -
          a01*a13*a20*a32 -
          a03*a10*a21*a32 +
          a00*a13*a21*a32 +
          a01*a10*a23*a32 -
          a00*a11*a23*a32 -
          a02*a11*a20*a33 +
          a01*a12*a20*a33 +
          a02*a10*a21*a33 -
          a00*a12*a21*a33 -
          a01*a10*a22*a33 +
          a00*a11*a22*a33;

    return ret;
}

mat4 mat4::inverse()
{
    mat4 ret = mat4_zeros();
    double dt = this->det();
    
    double a00,a01,a02,a03;
    double a10,a11,a12,a13;
    double a20,a21,a22,a23;
    double a30,a31,a32,a33;
  
    double b00,b01,b02,b03;
    double b10,b11,b12,b13;
    double b20,b21,b22,b23;
    double b30,b31,b32,b33;
  
    a00 = this->at(0,0);
    a01 = this->at(0,1);
    a02 = this->at(0,2);
    a03 = this->at(0,3);
    a10 = this->at(1,0);
    a11 = this->at(1,1);
    a12 = this->at(1,2);
    a13 = this->at(1,3);
    a20 = this->at(2,0);
    a21 = this->at(2,1);
    a22 = this->at(2,2);
    a23 = this->at(2,3);
    a30 = this->at(3,0);
    a31 = this->at(3,1);
    a32 = this->at(3,2);
    a33 = this->at(3,3);
  
    if(ams::abs(dt)>vecmat_det_small)
    {
      b00 = -a13*a22*a31 + a12*a23*a31 + a13*a21*a32 - a11*a23*a32 - a12*a21*a33 + a11*a22*a33;
      b01 =  a03*a22*a31 - a02*a23*a31 - a03*a21*a32 + a01*a23*a32 + a02*a21*a33 - a01*a22*a33;
      b02 = -a03*a12*a31 + a02*a13*a31 + a03*a11*a32 - a01*a13*a32 - a02*a11*a33 + a01*a12*a33;
      b03 =  a03*a12*a21 - a02*a13*a21 - a03*a11*a22 + a01*a13*a22 + a02*a11*a23 - a01*a12*a23;
      b10 =  a13*a22*a30 - a12*a23*a30 - a13*a20*a32 + a10*a23*a32 + a12*a20*a33 - a10*a22*a33;
      b11 = -a03*a22*a30 + a02*a23*a30 + a03*a20*a32 - a00*a23*a32 - a02*a20*a33 + a00*a22*a33;
      b12 =  a03*a12*a30 - a02*a13*a30 - a03*a10*a32 + a00*a13*a32 + a02*a10*a33 - a00*a12*a33;
      b13 = -a03*a12*a20 + a02*a13*a20 + a03*a10*a22 - a00*a13*a22 - a02*a10*a23 + a00*a12*a23;
      b20 = -a13*a21*a30 + a11*a23*a30 + a13*a20*a31 - a10*a23*a31 - a11*a20*a33 + a10*a21*a33;
      b21 =  a03*a21*a30 - a01*a23*a30 - a03*a20*a31 + a00*a23*a31 + a01*a20*a33 - a00*a21*a33;
      b22 = -a03*a11*a30 + a01*a13*a30 + a03*a10*a31 - a00*a13*a31 - a01*a10*a33 + a00*a11*a33;
      b23 =  a03*a11*a20 - a01*a13*a20 - a03*a10*a21 + a00*a13*a21 + a01*a10*a23 - a00*a11*a23;
      b30 =  a12*a21*a30 - a11*a22*a30 - a12*a20*a31 + a10*a22*a31 + a11*a20*a32 - a10*a21*a32;
      b31 = -a02*a21*a30 + a01*a22*a30 + a02*a20*a31 - a00*a22*a31 - a01*a20*a32 + a00*a21*a32;
      b32 =  a02*a11*a30 - a01*a12*a30 - a02*a10*a31 + a00*a12*a31 + a01*a10*a32 - a00*a11*a32;
      b33 = -a02*a11*a20 + a01*a12*a20 + a02*a10*a21 - a00*a12*a21 - a01*a10*a22 + a00*a11*a22;
      b00 = b00/dt;
      b01 = b01/dt;
      b02 = b02/dt;
      b03 = b03/dt;
      b10 = b10/dt;
      b11 = b11/dt;
      b12 = b12/dt;
      b13 = b13/dt;
      b20 = b20/dt;
      b21 = b21/dt;
      b22 = b22/dt;
      b23 = b23/dt;
      b30 = b30/dt;
      b31 = b31/dt;
      b32 = b32/dt;
      b33 = b33/dt;
      ret(0,0) = b00;
      ret(0,1) = b01;
      ret(0,2) = b02;
      ret(0,3) = b03;
      ret(1,0) = b10;
      ret(1,1) = b11;
      ret(1,2) = b12;
      ret(1,3) = b13;
      ret(2,0) = b20;
      ret(2,1) = b21;
      ret(2,2) = b22;
      ret(2,3) = b23;
      ret(3,0) = b30;
      ret(3,1) = b31;
      ret(3,2) = b32;
      ret(3,3) = b33;
    }
    //this was STUPID. Gaah. Computer algebra system saves the day? I'd be surprised if this didn't end up *slower* than gaussian elimination. Don't do this again!

    return ret;
}

void mat4::invert()
{
    mat4 m = this->inverse();
    *this = m;
}

mat4 mat4::transpose()
{
    mat4 ret;
    int I,J;
    for(I=0;I<4;I++)
    for(J=0;J<4;J++)
        ret(I,J) = this->at(J,I);
    return ret;
}

void mat4::dotranspose()
{
    mat4 m = this->transpose();
    *this = m;
}

// matrix operations (standalone functions)
mat4 mat4_eye()
{
    mat4 ret = mat4_zeros();
    ret(0,0) = 1.0;
    ret(1,1) = 1.0;
    ret(2,2) = 1.0;
    ret(3,3) = 1.0;
    return ret;
}

mat4 mat4_zeros()
{
    mat4 ret;
    // for speed - the constructor already sets data[I] to 0.0
    // int I;
    // for(I=0;I<16;I++) ret.data[I] = 0.0;
    return ret;
}

mat4 mat4_ones()
{
    mat4 ret;
    int I;
    for(I=0;I<16;I++) ret.data[I] = 1.0;
    return ret;
}

mat4 mat4_mult(mat4 a, mat4 b)
{
    return a*b;
}

vec4 mat4_mult(vec4 a, mat4 b)
{
    vec4 ret = vec4(0,0,0,0);
    int I,J;
    for(I=0;I<4;I++)
    for(J=0;J<4;J++)
        ret[J] += a[I] * b(I,J);

    return ret;
}

vec4 mat4_mult(mat4 a, vec4 b)
{
    return a*b;
}

double mat4_det(mat4 a)
{
    return a.det();
}

mat4 mat4_inverse(mat4 a)
{
    return a.inverse();
}

mat4 mat4_transpose(mat4 a)
{
    return a.transpose();
}

};