#include <amsmathutil25/amsmathutil25.hpp>

namespace ams
{


vec2::vec2()
{
   x = 0.0;
   y = 0.0; 
}

vec2::vec2(double _x, double _y)
{
    x = _x;
    y = _y;
}

vec2& vec2::operator=(const vec2 &rhs)
{
    if(this!=&rhs)
    {
        this->x = rhs.x;
        this->y = rhs.y;
    }

    return *this;
}

bool vec2::operator==(vec2 rhs)
{
    bool ret = 1;
    ret = ret & (rhs.x == x);
    ret = ret & (rhs.y == y);
    return ret;
}

vec2::vec2(const vec2 &rhs)
{
    this->x = rhs.x;
    this->y = rhs.y;
    return;
}

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

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

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

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

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

double& vec2::operator[](int ind)
{
    if(ind==0) return x;
    else if(ind==1) return y;
    return x;
}

const double& vec2::operator[](int ind) const
{
    if(ind==0) return x;
    else if(ind==1) return y;
    return x;
}

// vec2 vector operations
double vec2_dot(vec2 v1, vec2 v2)
{
    double ret = v1.x*v2.x + v1.y*v2.y;
    return ret;
}

double vec2_norm(vec2 v)
{
    double ret = ::sqrt(v.x*v.x+v.y*v.y);
    return ret;
}

vec2 vec2_normalize(vec2 v)
{
    vec2 ret = vec2(0,0);
    double nrm = vec2_norm(v);
    if(nrm>0.0)
    {
        ret = v/nrm;
    }
    return ret;
}

double vec2_dist(vec2 v1, vec2 v2)
{
    return vec2_norm(v1-v2);
}

double vec2_cross(vec2 v1, vec2 v2)
{
    double ret = v1.x*v2.y - v1.y*v2.x;
    return ret;
}

vec2 vec2_proj(vec2 v1, vec2 v2)
{
    vec2 ret;
    vec2 v2n = vec2_normalize(v2);
    ret = v2n*vec2_dot(v1,v2n);
    return ret;
}

vec2 vec2_hodgedual(vec2 v)
{
    vec2 ret;
    ret.x = -v.y;
    ret.y = v.x;
    return ret;
}

double vec2_arg(vec2 v)
{
    double ret;
    ret = ::atan2(v.y,v.x);
    if(ret<0.0)
    {
        ret = ret + 2.0*ams::pi;
    }
    return ret;
}

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

double vnorm(vec2 v)
{
    return vec2_norm(v);
}

vec2 vnormalize(vec2 v)
{
    return vec2_normalize(v);
}

double vdist(vec2 v1, vec2 v2)
{
    return vec2_dist(v1,v2);
}

double vcross(vec2 v1, vec2 v2)
{
    return vec2_cross(v1,v2);
}

vec2 vproj(vec2 v1, vec2 v2)
{
    return vec2_proj(v1,v2);
}

// mat2 constructors and operators
mat2::mat2()
{
    int I;
    for(I=0;I<4;I++) data[I] = 0.0;
    return;
}

mat2::mat2(double _xx, double _yx, double _xy, double _yy)
{
    data[0] = _xx;
    data[1] = _yx;
    data[2] = _xy;
    data[3] = _yy;
    return;
}

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

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

mat2::mat2(const vec2 _col1, const vec2 _col2)
{
    data[0] = _col1[0];
    data[1] = _col1[1];
    data[2] = _col2[0];
    data[3] = _col2[1];
}

mat2& mat2::operator=(const mat2 rhs)
{
    int I;
    for(I=0;I<4;I++)
    {
        data[I] = rhs.data[I];
    }
    return *this;
}

bool mat2::operator==(const mat2 rhs)
{
    bool ret = 1;
    int I;
    for(I=0;I<4;I++)
    {
        if(data[I]!=rhs.data[I])
        {
            ret = 0;
            break;
        }
    }

    return ret;
}

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

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

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

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

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

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

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

double& mat2::operator()(int I, int J)
{
    if(I==0&&J==0) return data[0];
    if(I==1&&J==0) return data[1];
    if(I==0&&J==1) return data[2];
    if(I==1&&J==1) return data[3];
    return data[0];
}

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

const double& mat2::operator()(int I, int J) const
{
    if(I==0&&J==0) return data[0];
    if(I==1&&J==0) return data[1];
    if(I==0&&J==1) return data[2];
    if(I==1&&J==1) return data[3];
    return data[0];
}

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

double& mat2::at(int I, int J)
{
    return (*this)(I,J);
}
const double& mat2::at(int I, int J) const
{
    return (*this)(I,J);
}

double mat2::det()
{
    double ret = 0.0;
    ret = ret + (*this)(0,0)*(*this)(1,1);
    ret = ret - (*this)(1,0)*(*this)(0,1);
    return ret;
}

mat2 mat2::inverse()
{
    mat2 ret = mat2_zeros();
    double dt = this->det();

    if(dt>=vecmat_det_small || dt<=-vecmat_det_small)
    {
        ret(0,0) = (*this)(1,1)/dt;
        ret(0,1) = -(*this)(0,1)/dt;
        ret(1,0) = -(*this)(1,0)/dt;
        ret(1,1) = (*this)(0,0)/dt;
    }
    return ret;
}

void mat2::invert()
{
    mat2 q = this->inverse();
    *this = q;
    return;
}

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

void mat2::dotranspose()
{
    mat2 t = this->transpose();
    (*this) = t;
    return;
}

// mat2 matrix operations
mat2 mat2_eye()
{
    mat2 ret;
    ret(0,0) = 1.0;
    ret(1,1) = 1.0;
    ret(0,1) = 0.0;
    ret(1,0) = 0.0;
    return ret;
}

mat2 mat2_zeros()
{
    mat2 ret;
    int I;
    for(I=0;I<4;I++) ret.data[I] = 0.0;
    return ret;
}

mat2 mat2_ones()
{
    mat2 ret;
    int I;
    for(I=0;I<4;I++) ret.data[I] = 1.0;
    return ret;
}

mat2 mat2_mult(mat2 a, mat2 b)
{
    return a*b;
}

vec2 mat2_mult(vec2 a, mat2 b)
{
    vec2 ret = vec2(0,0);
    int I,J;
    for(I=0;I<2;I++)
    for(J=0;J<2;J++)
        ret[J] += a[I]*b(I,J);
    return ret;
}

vec2 mat2_mult(mat2 a, vec2 b)
{
    return a*b;
}

double mat2_det(mat2 a)
{
    return a.det();
}

mat2 mat2_inverse(mat2 a)
{
    return a.inverse();
}

mat2 mat2_transpose(mat2 a)
{
    return a.transpose();
}

}; // end namespace ams
