#include <amsmathutil25/amsmathutil25.hpp>

namespace ams
{

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

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

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

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

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

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

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

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

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

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

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

const double& vec3::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)
double vec3_dot(vec3 v1, vec3 v2)
{
    double ret = 0.0;
    ret += v1.x*v2.x;
    ret += v1.y*v2.y;
    ret += v1.z*v2.z;
    return ret;
}

double vec3_norm(vec3 v)
{
    double ret = ::sqrt(v.x*v.x+v.y*v.y+v.z*v.z);
    return ret;
}

vec3 vec3_normalize(vec3 v)
{
    vec3 ret = vec3(0,0,0);
    double nrm = vec3_norm(v);
    if(nrm>0.0)
    {
        ret = v/nrm;
    }
    return ret;
}

double vec3_dist(vec3 v1, vec3 v2)
{
    return vec3_norm(v1-v2);
}

vec3 vec3_cross(vec3 v1, vec3 v2)
{
    vec3 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;
}

vec3 vec3_proj(vec3 v1, vec3 v2)
{
    vec3 ret;
    vec3 v2n = vec3_normalize(v2);
    ret = v2n*vec3_dot(v1,v2n);
    return ret;
}

mat3 vec3_hodgedual(vec3 v)
{
    mat3 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)
double vdot(vec3 v1, vec3 v2)
{
    return vec3_dot(v1,v2);
}

double vnorm(vec3 v)
{
    return vec3_norm(v);
}

vec3 vnormalize(vec3 v)
{
    return vec3_normalize(v);
}

double vdist(vec3 v1, vec3 v2)
{
    return vec3_dist(v1,v2);
}

vec3 vcross(vec3 v1, vec3 v2)
{
    return vec3_cross(v1,v2);
}

vec3 vproj(vec3 v1, vec3 v2)
{
    return vec3_proj(v1,v2);
}

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

mat3::mat3(double _xx, double _yx, double _zx, double _xy, double _yy, double _zy, double _xz, double _yz, double _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;
}

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

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

mat3::mat3(const vec3 _col1, const vec3 _col2, const vec3 _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;
}

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

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

    return ret;
}

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

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

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

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

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

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

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

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

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

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

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

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

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

// mat3 member functions
double mat3::det()
{
    double 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;
}

mat3 mat3::inverse()
{
    mat3 ret = mat3_zeros();
	double 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 mat3::invert()
{
    mat3 m = this->inverse();
    *this = m;
}

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

// matrix operations (standalone functions)
mat3 mat3_eye()
{
    mat3 ret = mat3_zeros();
    ret(0,0) = 1.0;
    ret(1,1) = 1.0;
    ret(2,2) = 1.0;
    return ret;
}

mat3 mat3_zeros()
{
    mat3 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;
}

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

mat3 mat3_mult(mat3 a, mat3 b)
{
    return a*b;
}

vec3 mat3_mult(vec3 a, mat3 b)
{
    vec3 ret = vec3(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;
}

vec3 mat3_mult(mat3 a, vec3 b)
{
    return a*b;
}

double mat3_det(mat3 a)
{
    return a.det();
}

mat3 mat3_inverse(mat3 a)
{
    return a.inverse();
}

mat3 mat3_transpose(mat3 a)
{
    return a.transpose();
}

};