#include <amsmathutil25/amsmathutil25.hpp>

namespace ams
{

vec3f::vec3f()
{
    x = 0.0;
    y = 0.0;
    z = 0.0;
    return;
}

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

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

vec3f& vec3f::operator=(const vec3f &rhs)
{
    this->x = rhs.x;
    this->y = rhs.y;
    this->z = rhs.z;
    return *this;
}

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

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

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

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

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

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

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

const float& vec3f::operator[](int ind) const
{
    if(ind==0) return x;
    else if(ind==1) return y;
    else if(ind==2) return z;
    return x;
}

// vector operations (new nomenclature)
float vec3f_dot(vec3f v1, vec3f v2)
{
    float ret = 0.0;
    ret += v1.x*v2.x;
    ret += v1.y*v2.y;
    ret += v1.z*v2.z;
    return ret;
}

float vec3f_norm(vec3f v)
{
    float ret = ::sqrtf(v.x*v.x+v.y*v.y+v.z*v.z);
    return ret;
}

vec3f vec3f_normalize(vec3f v)
{
    vec3f ret = vec3f(0,0,0);
    float nrm = vec3f_norm(v);
    if(nrm>0.0)
    {
        ret = v/nrm;
    }
    return ret;
}

float vec3f_dist(vec3f v1, vec3f v2)
{
    return vec3f_norm(v1-v2);
}

vec3f vec3f_cross(vec3f v1, vec3f v2)
{
    vec3f ret;
    ret[0] = v1[1]*v2[2] - v1[2]*v2[1];
    ret[1] = v1[2]*v2[0] - v1[0]*v2[2];
    ret[2] = v1[0]*v2[1] - v1[1]*v2[0];
    return ret;
}

vec3f vec3f_proj(vec3f v1, vec3f v2)
{
    vec3f ret;
    vec3f v2n = vec3f_normalize(v2);
    ret = v2n*vec3f_dot(v1,v2n);
    return ret;
}

mat3f vec3f_hodgedual(vec3f v)
{
    mat3f ret;

    ret(0,1) = v[2];
    ret(1,2) = v[0];
    ret(2,0) = v[1];

    ret(1,0) = -v[2];
    ret(2,1) = -v[0];
    ret(0,2) = -v[1];

    ret(0,0) = 0.0;
    ret(1,1) = 0.0;
    ret(2,2) = 0.0;

    return ret;
}

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

float vnorm(vec3f v)
{
    return vec3f_norm(v);
}

vec3f vnormalize(vec3f v)
{
    return vec3f_normalize(v);
}

float vdist(vec3f v1, vec3f v2)
{
    return vec3f_dist(v1,v2);
}

vec3f vcross(vec3f v1, vec3f v2)
{
    return vec3f_cross(v1,v2);
}

vec3f vproj(vec3f v1, vec3f v2)
{
    return vec3f_proj(v1,v2);
}

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

mat3f::mat3f(float _xx, float _yx, float _zx, float _xy, float _yy, float _zy, float _xz, float _yz, float _zz)
{
    data[0] = _xx;
    data[1] = _yx;
    data[2] = _zx;
    
    data[3] = _xy;
    data[4] = _yy;
    data[5] = _zy;
    
    data[6] = _xz;
    data[7] = _yz;
    data[8] = _zz;
    
    return;
}

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

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

mat3f::mat3f(const vec3f _col1, const vec3f _col2, const vec3f _col3)
{
    data[0] = _col1[0];
    data[1] = _col1[1];
    data[2] = _col1[2];

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

    data[6] = _col3[0];
    data[7] = _col3[1];
    data[8] = _col3[2];
    
    return;
}

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

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

    return ret;
}

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

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

mat3f mat3f::operator*(mat3f rhs) const
{
    mat3f ret = mat3f_zeros();
    int I,J,K;
    for(I=0;I<3;I++)
    for(J=0;J<3;J++)
    for(K=0;K<3;K++)
        ret(I,K) += + (*this)(I,J)*rhs(J,K);
    return ret;
}

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

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

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

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

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

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

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

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

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

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

// mat3f member functions
float mat3f::det()
{
    float ret = 0.0;
    ret += (*this)(0,0)*(*this)(1,1)*(*this)(2,2);
    ret += (*this)(0,1)*(*this)(1,2)*(*this)(2,0);
    ret += (*this)(0,2)*(*this)(1,0)*(*this)(2,1);

    ret -= (*this)(2,0)*(*this)(1,1)*(*this)(0,2);
    ret -= (*this)(2,1)*(*this)(1,2)*(*this)(0,0);
    ret -= (*this)(2,2)*(*this)(1,0)*(*this)(0,1);
    
    return ret;
}

mat3f mat3f::inverse()
{
    mat3f ret = mat3f_zeros();
	float dt = this->det();
	if(dt>vecmat_det_small||dt<-vecmat_det_small)
	{
        ret(0,0) =  (this->at(1,1) * this->at(2,2) - this->at(1,2) * this->at(2,1))/dt;
        ret(0,1) = -(this->at(1,0) * this->at(2,2) - this->at(1,2) * this->at(2,0))/dt;
        ret(0,2) =  (this->at(1,0) * this->at(2,1) - this->at(1,1) * this->at(2,0))/dt;
        ret(1,0) = -(this->at(0,1) * this->at(2,2) - this->at(0,2) * this->at(2,1))/dt;
        ret(1,1) =  (this->at(0,0) * this->at(2,2) - this->at(0,2) * this->at(2,0))/dt;
        ret(1,2) = -(this->at(0,0) * this->at(2,1) - this->at(0,1) * this->at(2,0))/dt;
        ret(2,0) =  (this->at(0,1) * this->at(1,2) - this->at(0,2) * this->at(1,1))/dt;
        ret(2,1) = -(this->at(0,0) * this->at(1,2) - this->at(0,2) * this->at(1,0))/dt;
        ret(2,2) =  (this->at(0,0) * this->at(1,1) - this->at(0,1) * this->at(1,0))/dt;
    }

    return ret;
}

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

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

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

// matrix operations (standalone functions)
mat3f mat3f_eye()
{
    mat3f ret = mat3f_zeros();
    ret(0,0) = 1.0;
    ret(1,1) = 1.0;
    ret(2,2) = 1.0;
    return ret;
}

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

mat3f mat3f_ones()
{
    mat3f ret;
    int I;
    for(I=0;I<9;I++) ret.data[I] = 1.0;
    return ret;
}

mat3f mat3f_mult(mat3f a, mat3f b)
{
    return a*b;
}

vec3f mat3f_mult(vec3f a, mat3f b)
{
    vec3f ret = vec3f(0,0,0);
    int I,J;
    for(I=0;I<3;I++)
    for(J=0;J<3;J++)
        ret[J] += a[I] * b(I,J);

    return ret;
}

vec3f mat3f_mult(mat3f a, vec3f b)
{
    return a*b;
}

float mat3f_det(mat3f a)
{
    return a.det();
}

mat3f mat3f_inverse(mat3f a)
{
    return a.inverse();
}

mat3f mat3f_transpose(mat3f a)
{
    return a.transpose();
}

};