#include <amsmathutil25/amsmathutil25.hpp>

namespace ams
{

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

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

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

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

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

// vec4f arithmetic operators
vec4f vec4f::operator+(vec4f rhs) const
{
    vec4f 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;
}

vec4f vec4f::operator-(vec4f rhs) const
{
    vec4f 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;
}

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

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

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

// vec4f subscript operators
float& vec4f::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 float& vec4f::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)
float vec4f_dot(vec4f v1, vec4f v2)
{
    float ret = 0.0f;
    ret += v1.x*v2.x;
    ret += v1.y*v2.y;
    ret += v1.z*v2.z;
    ret += v1.w*v2.w;
    return ret;
}

float vec4f_norm(vec4f v)
{
    float ret = ::sqrtf(v.x*v.x+v.y*v.y+v.z*v.z+v.w*v.w);
    return ret;
}

vec4f vec4f_normalize(vec4f v)
{
    vec4f ret = vec4f(0,0,0,0);
    float nrm = vec4f_norm(v);
    if(nrm>0.0)
    {
        ret = v/nrm;
    }
    return ret;
}

float vec4f_dist(vec4f v1, vec4f v2)
{
    return vec4f_norm(v1-v2);
}

vec4f vec4f_proj(vec4f v1, vec4f v2)
{
    vec4f ret;
    vec4f v2n = vec4f_normalize(v2);
    ret = v2n*vec4f_dot(v1,v2n);
    return ret;
}

// vector operations (old nomenclature)
float vdot(vec4f v1, vec4f v2)
{
    return vec4f_dot(v1,v2);
}

float vnorm(vec4f v)
{
    return vec4f_norm(v);
}

vec4f vnormalize(vec4f v)
{
    return vec4f_normalize(v);
}

float vdist(vec4f v1, vec4f v2)
{
    return vec4f_dist(v1,v2);
}

vec4f vproj(vec4f v1, vec4f v2)
{
    return vec4f_proj(v1,v2);
}

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

mat4f::mat4f(
    float _xx, float _yx, float _zx, float _wx,
    float _xy, float _yy, float _zy, float _wy,
    float _xz, float _yz, float _zz, float _wz,
    float _xw, float _yw, float _zw, float _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;
}

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

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

mat4f::mat4f(const vec4f _col1, const vec4f _col2, const vec4f _col3, const vec4f _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;
}

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

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

    return ret;
}

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

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

mat4f mat4f::operator*(mat4f rhs) const
{
    mat4f ret = mat4f_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;
}

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

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

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

vec4f mat4f::operator*(vec4f rhs) const
{
    vec4f ret = vec4f(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;
}

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

float& mat4f::operator[](int I)
{
    return data[I];
}

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

const float& mat4f::operator[](int I) const
{
    return data[I];
}

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

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

float mat4f::det()
{
    float ret = 0.0f;
    float a00,a01,a02,a03;
    float a10,a11,a12,a13;
    float a20,a21,a22,a23;
    float 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;
}

mat4f mat4f::inverse()
{
    mat4f ret = mat4f_zeros();
    float dt = this->det();
    
    float a00,a01,a02,a03;
    float a10,a11,a12,a13;
    float a20,a21,a22,a23;
    float a30,a31,a32,a33;
  
    float b00,b01,b02,b03;
    float b10,b11,b12,b13;
    float b20,b21,b22,b23;
    float 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 mat4f::invert()
{
    mat4f m = this->inverse();
    *this = m;
}

mat4f mat4f::transpose()
{
    mat4f 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 mat4f::dotranspose()
{
    mat4f m = this->transpose();
    *this = m;
}

// matrix operations (standalone functions)
mat4f mat4f_eye()
{
    mat4f ret = mat4f_zeros();
    ret(0,0) = 1.0f;
    ret(1,1) = 1.0f;
    ret(2,2) = 1.0f;
    ret(3,3) = 1.0f;
    return ret;
}

mat4f mat4f_zeros()
{
    mat4f 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;
}

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

mat4f mat4f_mult(mat4f a, mat4f b)
{
    return a*b;
}

vec4f mat4f_mult(vec4f a, mat4f b)
{
    vec4f ret = vec4f(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;
}

vec4f mat4f_mult(mat4f a, vec4f b)
{
    return a*b;
}

float mat4f_det(mat4f a)
{
    return a.det();
}

mat4f mat4f_inverse(mat4f a)
{
    return a.inverse();
}

mat4f mat4f_transpose(mat4f a)
{
    return a.transpose();
}

};