#include <amsmathutil25/amsmathutil25.hpp>

namespace ams
{

vec2f::vec2f()
{
   x = 0.0f;
   y = 0.0f; 
}

vec2f::vec2f(float _x, float _y)
{
    x = _x;
    y = _y;
}

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

    return *this;
}

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

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

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

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

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

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

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

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

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

float vec2f_dot(vec2f v1, vec2f v2)
{
    float ret = v1.x*v2.x + v1.y*v2.y;
    return ret;
}

float vec2f_norm(vec2f v)
{
    float ret = ::sqrtf(v.x*v.x+v.y*v.y);
    return ret;
}

vec2f vec2f_normalize(vec2f v)
{
    vec2f ret = vec2f(0.0f,0.0f);
    float nrm = vec2f_norm(v);
    if(nrm>0.0f)
    {
        ret = v/nrm;
    }
    return ret;
}

float vec2f_dist(vec2f v1, vec2f v2)
{
    return vec2f_norm(v1-v2);
}

float vec2f_cross(vec2f v1, vec2f v2)
{
    float ret = v1.x*v2.y - v1.y*v2.x;
    return ret;
}

vec2f vec2f_proj(vec2f v1, vec2f v2)
{
    vec2f ret;
    vec2f v2n = vec2f_normalize(v2);
    ret = v2n*vec2f_dot(v1,v2n);
    return ret;
}

vec2f vec2f_hodgedual(vec2f v)
{
    vec2f ret;
    ret.x = -v.y;
    ret.y = v.x;
    return ret;
}

float vec2f_arg(vec2f v)
{
    float ret;
    ret = ::atan2f(v.y,v.x);
    if(ret<0.0f)
    {
        ret = ret + 2.0f*ams::pif;
    }
    return ret;
}

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

float vnorm(vec2f v)
{
    return vec2f_norm(v);
}

vec2f vnormalize(vec2f v)
{
    return vec2f_normalize(v);
}

float vdist(vec2f v1, vec2f v2)
{
    return vec2f_dist(v1,v2);
}

float vcross(vec2f v1, vec2f v2)
{
    return vec2f_cross(v1,v2);
}

vec2f vproj(vec2f v1, vec2f v2)
{
    return vec2f_proj(v1,v2);
}

mat2f::mat2f()
{
    int I;
    for(I=0;I<4;I++) data[I] = 0.0f;
    return;
}

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

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

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

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

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

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

    return ret;
}

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

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

mat2f mat2f::operator*(mat2f rhs) const
{
    mat2f ret = mat2f(0.0f,0.0f,0.0f,0.0f);
    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;
}

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

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

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

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

float& mat2f::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];
}

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

const float& mat2f::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 float& mat2f::operator[](int I) const
{
    return data[I];
}

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

float mat2f::det()
{
    float ret = 0.0f;
    ret = ret + (*this)(0,0)*(*this)(1,1);
    ret = ret - (*this)(1,0)*(*this)(0,1);
    return ret;
}

mat2f mat2f::inverse()
{
    mat2f ret = mat2f_zeros();
    float 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 mat2f::invert()
{
    mat2f q = this->inverse();
    *this = q;
    return;
}

mat2f mat2f::transpose()
{
    mat2f 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 mat2f::dotranspose()
{
    mat2f t = this->transpose();
    (*this) = t;
    return;
}

// mat2f matrix operations
mat2f mat2f_eye()
{
    mat2f ret;
    ret(0,0) = 1.0f;
    ret(1,1) = 1.0f;
    ret(0,1) = 0.0f;
    ret(1,0) = 0.0f;
    return ret;
}

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

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

mat2f mat2f_mult(mat2f a, mat2f b)
{
    return a*b;
}

vec2f mat2f_mult(vec2f a, mat2f b)
{
    vec2f ret = vec2f(0.0f,0.0f);
    int I,J;
    for(I=0;I<2;I++)
    for(J=0;J<2;J++)
        ret[J] += a[I]*b(I,J);
    return ret;
}

vec2f mat2f_mult(mat2f a, vec2f b)
{
    return a*b;
}

float mat2f_det(mat2f a)
{
    return a.det();
}

mat2f mat2f_inverse(mat2f a)
{
    return a.inverse();
}

mat2f mat2f_transpose(mat2f a)
{
    return a.transpose();
}

}; // end namespace ams