#include <amsculib3/amsculib3.hpp>

namespace amscuda
{

//////////////////
// Vector Class //
//////////////////

__host__ __device__ cuvec3f::cuvec3f() 
{
    x = 0; y = 0; z = 0; 
    return;
}

__host__ __device__ cuvec3f::~cuvec3f() 
{
    x = 0; y = 0; z = 0; 
    return;
}

__host__ __device__ cuvec3f::cuvec3f(const float &_x, const float &_y, const float &_z) 
{
    x = _x; y = _y; z = _z; 
    return;
}

__host__ __device__ float& cuvec3f::operator[](const int &I)
{
    switch(I)
    {
        case 0:
            return x;
        case 1:
            return y;
        case 2:
            return z;
    }

    return x;
}

__host__ __device__ const float& cuvec3f::operator[](const int &I) const
{
    switch(I)
    {
        case 0:
            return x;
        case 1:
            return y;
        case 2:
            return z;
    }

    return x;
}

__host__ __device__ cuvec3f cuvec3f::operator+(const cuvec3f& rhs) const
{
    cuvec3f ret;
    ret.x = x + rhs.x;
    ret.y = y + rhs.y;
    ret.z = z + rhs.z;
    return ret;
}

__host__ __device__ cuvec3f cuvec3f::operator-(const cuvec3f& rhs) const
{
    cuvec3f ret;
    ret.x = x - rhs.x;
    ret.y = y - rhs.y;
    ret.z = z - rhs.z;
    return ret;
}

__host__ __device__ cuvec3f cuvec3f::operator*(const cuvec3f& rhs) const
{
    //Elementwise product
    cuvec3f ret;
    ret.x = x * rhs.x;
    ret.y = y * rhs.y;
    ret.z = z * rhs.z;
    return ret;
}

__host__ __device__ cuvec3f cuvec3f::operator/(const cuvec3f& rhs) const
{
    //Elementwise division
    cuvec3f ret;
    ret.x = x / rhs.x;
    ret.y = y / rhs.y;
    ret.z = z / rhs.z;
    return ret;
}

__host__ __device__ cuvec3f operator*(const cuvec3f& lhs, const float& rhs)
{
    cuvec3f ret;
    ret.x = lhs.x*rhs;
    ret.y = lhs.y*rhs;
    ret.z = lhs.z*rhs;
    return ret;
}

__host__ __device__ cuvec3f operator*(const float& lhs, const cuvec3f& rhs)
{
    cuvec3f ret;
    ret.x = lhs*rhs.x;
    ret.y = lhs*rhs.y;
    ret.z = lhs*rhs.z;
    return ret;
}

__host__ __device__ cuvec3f operator/(const cuvec3f& lhs, const float& rhs)
{
    cuvec3f ret;
    ret.x = lhs.x/rhs;
    ret.y = lhs.y/rhs;
    ret.z = lhs.z/rhs;
    return ret;
}

__host__ __device__ cuvec3f operator/(const float& lhs, const cuvec3f& rhs)
{
    cuvec3f ret;
    ret.x = lhs/rhs.x;
    ret.y = lhs/rhs.y;
    ret.z = lhs/rhs.z;
    return ret;
}

__host__ __device__ cuvec3f operator-(const cuvec3f& other)
{
    cuvec3f ret;
    ret.x = -other.x;
    ret.y = -other.y;
    ret.z = -other.z;
    return ret;
}

__host__ __device__ cuvec3f& cuvec3f::operator+=(const cuvec3f& rhs)
{
    x += rhs.x;
    y += rhs.y;
    z += rhs.z;
    return *this;
}

__host__ __device__ cuvec3f& cuvec3f::operator-=(const cuvec3f& rhs)
{
    x -= rhs.x;
    y -= rhs.y;
    z -= rhs.z;
    return *this;
}

__host__ __device__ cuvec3f& cuvec3f::operator*=(const float& rhs)
{
    x *= rhs;
    y *= rhs;
    z *= rhs;
    return *this;
}

__host__ __device__ cuvec3f& cuvec3f::operator/=(const float& rhs)
{
    x /= rhs;
    y /= rhs;
    z /= rhs;
    return *this;
}


//////////////////
// Matrix Class //
//////////////////

__host__ __device__ cumat3f::cumat3f() 
{
    m00 = 0;
    m01 = 0;
    m02 = 0;

    m10 = 0;
    m11 = 0;
    m12 = 0;

    m20 = 0;
    m21 = 0;
    m22 = 0;

    return;
}

__host__ __device__ cumat3f::~cumat3f() 
{
    //m00 = 0;
    //m01 = 0;
    //m02 = 0;

    //m10 = 0;
    //m11 = 0;
    //m12 = 0;

    //m20 = 0;
    //m21 = 0;
    //m22 = 0;

    return;
}

__host__ __device__ cumat3f::cumat3f(
    const float& _m00, const float& _m01, const float& _m02, 
    const float& _m10, const float& _m11, const float& _m12, 
    const float& _m20, const float& _m21, const float& _m22
)
{
    m00 = _m00; 
    m10 = _m10; 
    m20 = _m20; 

    m01 = _m01; 
    m11 = _m11; 
    m21 = _m21; 

    m02 = _m02; 
    m12 = _m12; 
    m22 = _m22; 


    return;
}

__host__ __device__ cumat3f::cumat3f(const float* data9)
{
    m00 = data9[0]; 
    m10 = data9[1]; 
    m20 = data9[2]; 

    m01 = data9[3]; 
    m11 = data9[4]; 
    m21 = data9[5]; 

    m02 = data9[6]; 
    m12 = data9[7]; 
    m22 = data9[8]; 


    return;
}

__host__ __device__ float& cumat3f::operator[](const int &I)
{
    switch(I)
    {
        case 0:
            return m00;
        case 1:
            return m10;
        case 2:
            return m20;
        case 3:
            return m01;
        case 4:
            return m11;
        case 5:
            return m21;
        case 6:
            return m02;
        case 7:
            return m12;
        case 8:
            return m22;
    }

    return m00;
}

__host__ __device__ const float& cumat3f::operator[](const int &I) const
{
    switch(I)
    {
        case 0:
            return m00;
        case 1:
            return m10;
        case 2:
            return m20;
        case 3:
            return m01;
        case 4:
            return m11;
        case 5:
            return m21;
        case 6:
            return m02;
        case 7:
            return m12;
        case 8:
            return m22;
    }

    return m00;
}

__host__ __device__ float& cumat3f::operator()(const int &I, const int &J)
{
    return (*this)[I+3*J];
}

__host__ __device__ const float& cumat3f::operator()(const int &I, const int &J) const
{
    return (*this)[I+3*J];
}

__host__ __device__ float& cumat3f::at(const int &I, const int &J)
{
    return (*this)[I+3*J];
}

__host__ __device__ const float& cumat3f::at(const int &I, const int &J) const
{
    return (*this)[I+3*J];
}

__host__ __device__ float* cumat3f::data()
{
return (float*)this;
}

__host__ __device__ const float* cumat3f::data() const
{
return (float*)this;
}

__host__ __device__ cumat3f cumat3f::operator+(const cumat3f& rhs) const
{
cumat3f ret;
ret.m00 = m00 + rhs.m00;
ret.m10 = m10 + rhs.m10;
ret.m20 = m20 + rhs.m20;

ret.m01 = m01 + rhs.m01;
ret.m11 = m11 + rhs.m11;
ret.m21 = m21 + rhs.m21;

ret.m02 = m02 + rhs.m02;
ret.m12 = m12 + rhs.m12;
ret.m22 = m22 + rhs.m22;

return ret;
}

__host__ __device__ cumat3f cumat3f::operator-(const cumat3f& rhs) const
{
cumat3f ret;
ret.m00 = m00 - rhs.m00;
ret.m10 = m10 - rhs.m10;
ret.m20 = m20 - rhs.m20;

ret.m01 = m01 - rhs.m01;
ret.m11 = m11 - rhs.m11;
ret.m21 = m21 - rhs.m21;

ret.m02 = m02 - rhs.m02;
ret.m12 = m12 - rhs.m12;
ret.m22 = m22 - rhs.m22;

return ret;
}

__host__ __device__ cumat3f cumat3f::operator*(const cumat3f& rhs) const
{
cumat3f ret; //should be zeroed in constructor
ret.m00 = m00*rhs.m00 + m01*rhs.m10 + m02*rhs.m20;
ret.m01 = m00*rhs.m01 + m01*rhs.m11 + m02*rhs.m21;
ret.m02 = m00*rhs.m02 + m01*rhs.m12 + m02*rhs.m22;

ret.m10 = m10*rhs.m00 + m11*rhs.m10 + m12*rhs.m20;
ret.m11 = m10*rhs.m01 + m11*rhs.m11 + m12*rhs.m21;
ret.m12 = m10*rhs.m02 + m11*rhs.m12 + m12*rhs.m22;

ret.m20 = m20*rhs.m00 + m21*rhs.m10 + m22*rhs.m20;
ret.m21 = m20*rhs.m01 + m21*rhs.m11 + m22*rhs.m21;
ret.m22 = m20*rhs.m02 + m21*rhs.m12 + m22*rhs.m22;

return ret;
}

__host__ __device__ cumat3f operator*(const cumat3f& lhs, const float& rhs)
{
cumat3f ret;
ret.m00=lhs.m00*rhs;
ret.m10=lhs.m10*rhs;
ret.m20=lhs.m20*rhs;
ret.m01=lhs.m01*rhs;
ret.m11=lhs.m11*rhs;
ret.m21=lhs.m21*rhs;
ret.m02=lhs.m02*rhs;
ret.m12=lhs.m12*rhs;
ret.m22=lhs.m22*rhs;
return ret;
}

__host__ __device__ cumat3f operator/(const cumat3f& lhs, const float& rhs)
{
cumat3f ret;
ret.m00=lhs.m00/rhs;
ret.m10=lhs.m10/rhs;
ret.m20=lhs.m20/rhs;
ret.m01=lhs.m01/rhs;
ret.m11=lhs.m11/rhs;
ret.m21=lhs.m21/rhs;
ret.m02=lhs.m02/rhs;
ret.m12=lhs.m12/rhs;
ret.m22=lhs.m22/rhs;
return ret;
}

__host__ __device__ cumat3f operator*(const float& lhs, const cumat3f& rhs)
{
cumat3f ret;
ret.m00=lhs*rhs.m00;
ret.m10=lhs*rhs.m10;
ret.m20=lhs*rhs.m20;
ret.m01=lhs*rhs.m01;
ret.m11=lhs*rhs.m11;
ret.m21=lhs*rhs.m21;
ret.m02=lhs*rhs.m02;
ret.m12=lhs*rhs.m12;
ret.m22=lhs*rhs.m22;
return ret;
}

__host__ __device__ cuvec3f operator*(const cumat3f& lhs, const cuvec3f& rhs)
{
cuvec3f ret;
ret.x = lhs.m00*rhs.x + lhs.m01*rhs.y + lhs.m02*rhs.z;
ret.y = lhs.m10*rhs.x + lhs.m11*rhs.y + lhs.m12*rhs.z;
ret.z = lhs.m20*rhs.x + lhs.m21*rhs.y + lhs.m22*rhs.z;
return ret;
}

__host__ __device__ cuvec3f operator*(const cuvec3f& lhs, const cumat3f& rhs)
{
cuvec3f ret;
ret.x = lhs.x*rhs.m00 + lhs.y*rhs.m10 + lhs.z*rhs.m20;
ret.y = lhs.x*rhs.m01 + lhs.y*rhs.m11 + lhs.z*rhs.m21;
ret.z = lhs.x*rhs.m02 + lhs.y*rhs.m12 + lhs.z*rhs.m22;
return ret;
}

__host__ __device__ cumat3f operator-(const cumat3f& rhs)
{
cumat3f ret;
ret.m00 = -rhs.m00;
ret.m10 = -rhs.m10;
ret.m20 = -rhs.m20;
ret.m01 = -rhs.m01;
ret.m11 = -rhs.m11;
ret.m21 = -rhs.m21;
ret.m02 = -rhs.m02;
ret.m12 = -rhs.m12;
ret.m22 = -rhs.m22;
return ret;
}

__host__ __device__ cumat3f& cumat3f::operator+=(const cumat3f& rhs)
{
m00 += rhs.m00;
m10 += rhs.m10;
m20 += rhs.m20;
m01 += rhs.m01;
m11 += rhs.m11;
m21 += rhs.m21;
m02 += rhs.m02;
m12 += rhs.m12;
m22 += rhs.m22;
return *this;
}

__host__ __device__ cumat3f& cumat3f::operator-=(const cumat3f& rhs)
{
m00 -= rhs.m00;
m10 -= rhs.m10;
m20 -= rhs.m20;
m01 -= rhs.m01;
m11 -= rhs.m11;
m21 -= rhs.m21;
m02 -= rhs.m02;
m12 -= rhs.m12;
m22 -= rhs.m22;
return *this;
}

__host__ __device__ cumat3f& cumat3f::operator*=(const float& rhs)
{
m00 *= rhs;
m10 *= rhs;
m20 *= rhs;
m01 *= rhs;
m11 *= rhs;
m21 *= rhs;
m02 *= rhs;
m12 *= rhs;
m22 *= rhs;
return *this;
}

__host__ __device__ cumat3f& cumat3f::operator/=(const float& rhs)
{
m00 /= rhs;
m10 /= rhs;
m20 /= rhs;
m01 /= rhs;
m11 /= rhs;
m21 /= rhs;
m02 /= rhs;
m12 /= rhs;
m22 /= rhs;
return *this;
}

__host__ __device__ cumat3f& cumat3f::operator*=(const cumat3f& rhs)
{
cumat3f tmp = *this;
m00 = tmp.m00*rhs.m00 + tmp.m01*rhs.m10 + tmp.m02*rhs.m20;
m01 = tmp.m00*rhs.m01 + tmp.m01*rhs.m11 + tmp.m02*rhs.m21;
m02 = tmp.m00*rhs.m02 + tmp.m01*rhs.m12 + tmp.m02*rhs.m22;
m10 = tmp.m10*rhs.m00 + tmp.m11*rhs.m10 + tmp.m12*rhs.m20;
m11 = tmp.m10*rhs.m01 + tmp.m11*rhs.m11 + tmp.m12*rhs.m21;
m12 = tmp.m10*rhs.m02 + tmp.m11*rhs.m12 + tmp.m12*rhs.m22;
m20 = tmp.m20*rhs.m00 + tmp.m21*rhs.m10 + tmp.m22*rhs.m20;
m21 = tmp.m20*rhs.m01 + tmp.m21*rhs.m11 + tmp.m22*rhs.m21;
m22 = tmp.m20*rhs.m02 + tmp.m21*rhs.m12 + tmp.m22*rhs.m22;
return *this;
}

__host__ __device__ cumat3f cumat3f::transpose() const
{
cumat3f ret;
ret.m00 = m00;
ret.m10 = m01;
ret.m20 = m02;
ret.m01 = m10;
ret.m11 = m11;
ret.m21 = m12;
ret.m02 = m20;
ret.m12 = m21;
ret.m22 = m22;
return ret;
}


/////////////////////
// Det and Inverse //
/////////////////////

__host__ __device__ float cumat3f::det()
{
    float ret = 0.0f;

    ret += m00*m11*m22;
    ret += m01*m12*m20;
    ret += m02*m10*m21;
    ret -= m20*m11*m02;
    ret -= m21*m12*m00;
    ret -= m22*m10*m01;

    return ret;
}

__host__ __device__ cumat3f cumat3f::inverse()
{
    cumat3f q;
    float dt = det();
    float idt;
    if(dt!=0.0)
    {
        idt = 1.0f/dt;
        q(0,0) =  (at(1,1)*at(2,2)-at(1,2)*at(2,1))*idt;
        q(1,0) = -(at(1,0)*at(2,2)-at(1,2)*at(2,0))*idt;
        q(2,0) =  (at(1,0)*at(2,1)-at(1,1)*at(2,0))*idt;
        q(0,1) = -(at(0,1)*at(2,2)-at(0,2)*at(2,1))*idt;
        q(1,1) =  (at(0,0)*at(2,2)-at(0,2)*at(2,0))*idt;
        q(2,1) = -(at(0,0)*at(2,1)-at(0,1)*at(2,0))*idt;
        q(0,2) =  (at(0,1)*at(1,2)-at(0,2)*at(1,1))*idt;
        q(1,2) = -(at(0,0)*at(1,2)-at(0,2)*at(1,0))*idt;
        q(2,2) =  (at(0,0)*at(1,1)-at(0,1)*at(1,0))*idt;
        // q(0,0) =  (at(1,1)*at(2,2)-at(1,2)*at(2,1))/dt;
        // q(0,1) = -(at(1,0)*at(2,2)-at(1,2)*at(2,0))/dt;
        // q(0,2) =  (at(1,0)*at(2,1)-at(1,1)*at(2,0))/dt;
        // q(1,0) = -(at(0,1)*at(2,2)-at(0,2)*at(2,1))/dt;
        // q(1,1) =  (at(0,0)*at(2,2)-at(0,2)*at(2,0))/dt;
        // q(1,2) = -(at(0,0)*at(2,1)-at(0,1)*at(2,0))/dt;
        // q(2,0) =  (at(0,1)*at(1,2)-at(0,2)*at(1,1))/dt;
        // q(2,1) = -(at(0,0)*at(1,2)-at(0,2)*at(1,0))/dt;
        // q(2,2) =  (at(0,0)*at(1,1)-at(0,1)*at(1,0))/dt;
        // q = q.transpose();
    }
    else
    {
        q(0,0) = inf;
        q(0,1) = inf;
        q(0,2) = inf;
        q(1,0) = inf;
        q(1,1) = inf;
        q(1,2) = inf;
        q(2,0) = inf;
        q(2,1) = inf;
        q(2,2) = inf;
    }

    return q;
}

//////////////////////////
// Standalone functions //
//////////////////////////

__host__ __device__ float cuvec3f_dot(const cuvec3f &a, const cuvec3f &b)
{
    float ret = a.x*b.x+a.y*b.y+a.z*b.z;

    return ret;
}

__host__ __device__ cuvec3f cuvec3f_cross(const cuvec3f &a, const cuvec3f &b)
{
    cuvec3f ret;
    ret[0] = a[1]*b[2]-a[2]*b[1];
    ret[1] = a[2]*b[0]-a[0]*b[2];
    ret[2] = a[0]*b[1]-a[1]*b[0];

    return ret;
}

__host__ __device__ float cuvec3f_norm(const cuvec3f &a)
{
    float ret;
    ret = ::sqrtf(a.x*a.x+a.y*a.y+a.z*a.z);
    return ret;
}

__host__ __device__ cuvec3f cuvec3f_normalize(const cuvec3f &a)
{
    cuvec3f ret;
    float m;
    m = ::sqrtf(a.x*a.x+a.y*a.y+a.z*a.z);
    if(m>0.0)
    {
        ret.x = a.x/m; ret.y = a.y/m; ret.z = a.z/m;
    }
    else
    {
        ret.x = 0.0f; ret.y = 0.0f; ret.z = 0.0f;
    }

    return ret;
}

__host__ __device__ cuvec3f cuvec3f_proj(const cuvec3f &a, const cuvec3f &b)
{
    cuvec3f ret;
    cuvec3f bn = cuvec3f_normalize(b);
    float m = cuvec3f_dot(a,bn);
    ret = bn*m;
    return ret;
}

__host__ __device__ cumat3f hodge_dual(const cuvec3f &vin)
{
    cumat3f ret;

    ret.m00 = 0.0f;
    ret.m11 = 0.0f;
    ret.m22 = 0.0f;

    ret.m01 = vin.z;
    ret.m12 = vin.x;
    ret.m20 = vin.y;

    ret.m10 = -vin.z;
    ret.m21 = -vin.x;
    ret.m02 = -vin.y;

    return ret;
}

__host__ __device__ cuvec3f hodge_dual(const cumat3f &min)
{
    cuvec3f ret;

    ret.x = 0.5f*(min.m12 - min.m21);
    ret.y = 0.5f*(min.m20 - min.m02);
    ret.z = 0.5f*(min.m01 - min.m10);

    return ret;
}

__host__ __device__ const cumat3f cumat3f_eye()
{
    cumat3f ret;
    ret.m00 = 1.0f;
    ret.m11 = 1.0f;
    ret.m22 = 1.0f;
    
    return ret;
}

__host__ __device__ const cumat3f cumat3f_zeros()
{
    cumat3f ret;
    return ret;
}

__host__ __device__ cumat3f rotmat_from_axisangle(const cuvec3f &axis, const float &angle)
{
    cumat3f ret = cumat3f_zeros();
    cumat3f H;
    cuvec3f _naxis;

    _naxis = cuvec3f_normalize(axis);
    H = hodge_dual(_naxis);

    ret += H*sinf(angle);
    H = H*H;
    ret -= H*cosf(angle);
    ret += (H + cumat3f_eye());

    return ret;
}


///////////
// Tests //
///////////

__host__ void test_cudavectf_logic1()
{

}



};