#include <amsculib2/amsculib2.hpp>

namespace amscuda
{

////////////
//cuvect4ff//
////////////

__host__ __device__ cuvect4f::cuvect4f()
{
    x = 0.0f; y = 0.0f; z = 0.0f; w = 0.0f;
    return;
}

__host__ __device__ cuvect4f::~cuvect4f()
{
    x = 0.0f; y = 0.0f; z = 0.0f; w = 0.0f;
    return;
}

__host__ __device__ float& cuvect4f::operator[](const int &I)
{
    if(I==0) return x;
    if(I==1) return y;
    if(I==2) return z;
    if(I==3) return w;
    return x;
}

__host__ __device__ const float& cuvect4f::operator[](const int &I) const
{
    if(I==0) return x;
    if(I==1) return y;
    if(I==2) return z;
    if(I==3) return w;
    return x;
}

__host__ __device__ cuvect4f cuvect4f::operator+(const cuvect4f &rhs)
{
    cuvect4f ret;
    ret.x = x+rhs.x;
    ret.y = y+rhs.y;
    ret.z = z+rhs.z;
    ret.w = w+rhs.w;

    return ret;
}

__host__ __device__ cuvect4f cuvect4f::operator-(const cuvect4f &rhs)
{
    cuvect4f ret;
    ret.x = x-rhs.x;
    ret.y = y-rhs.y;
    ret.z = z-rhs.z;
    ret.w = w-rhs.w;

    return ret;
}

__host__ __device__ cuvect4f cuvect4f::operator*(const float &rhs)
{
    cuvect4f ret;
    ret.x = x*rhs;
    ret.y = y*rhs;
    ret.z = z*rhs;
    ret.w = w*rhs;
    return ret;
}

__host__ __device__ cuvect4f cuvect4f::operator/(const float &rhs)
{
    cuvect4f ret;
    ret.x = x/rhs;
    ret.y = y/rhs;
    ret.z = z/rhs;
    ret.w = w/rhs;
    return ret;
}

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

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

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

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

__host__ __device__ cuvect4f operator-(const cuvect4f &rhs)
{
    cuvect4f ret;
    ret[0] = -rhs[0];
    ret[1] = -rhs[1];
    ret[2] = -rhs[2];
    ret[3] = -rhs[3];
    return ret;
}


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


__host__ __device__ cumat4f::cumat4f()
{
    m00 = 0.0f;
    m10 = 0.0f;
    m20 = 0.0f;
    m30 = 0.0f;

    m01 = 0.0f;
    m11 = 0.0f;
    m21 = 0.0f;
    m31 = 0.0f;

    m02 = 0.0f;
    m12 = 0.0f;
    m22 = 0.0f;
    m32 = 0.0f;

    m03 = 0.0f;
    m13 = 0.0f;
    m23 = 0.0f;
    m33 = 0.0f;
    
    return;
}

__host__ __device__ cumat4f::~cumat4f()
{
    m00 = 0.0f;
    m10 = 0.0f;
    m20 = 0.0f;
    m30 = 0.0f;

    m01 = 0.0f;
    m11 = 0.0f;
    m21 = 0.0f;
    m31 = 0.0f;

    m02 = 0.0f;
    m12 = 0.0f;
    m22 = 0.0f;
    m32 = 0.0f;

    m03 = 0.0f;
    m13 = 0.0f;
    m23 = 0.0f;
    m33 = 0.0f;

    return;
}

__host__ __device__ cumat4f::cumat4f(
    const float & _m00, const float & _m01, const float & _m02, const float & _m03,
    const float & _m10, const float & _m11, const float & _m12, const float & _m13,
    const float & _m20, const float & _m21, const float & _m22, const float & _m23,
    const float & _m30, const float & _m31, const float & _m32, const float & _m33
)
{
    m00 = _m00;
    m10 = _m10;
    m20 = _m20;
    m30 = _m30;

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

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

    m03 = _m03;
    m13 = _m13;
    m23 = _m23;
    m33 = _m33;
    
    return;
}

__host__ __device__ cumat4f::cumat4f(const float *data16)
{
    m00 = data16[0];
    m10 = data16[1];
    m20 = data16[2];
    m30 = data16[3];

    m01 = data16[4];
    m11 = data16[5];
    m21 = data16[6];
    m31 = data16[7];

    m02 = data16[8];
    m12 = data16[9];
    m22 = data16[10];
    m32 = data16[11];

    m03 = data16[12];
    m13 = data16[13];
    m23 = data16[14];
    m33 = data16[15];

    return;
}

__host__ __device__ float& cumat4f::operator[](const int &I)
{
    switch(I)
    {
        case 0: return m00;
        case 1: return m10;
        case 2: return m20;
        case 3: return m30;

        case 4: return m01;
        case 5: return m11;
        case 6: return m21;
        case 7: return m31;

        case 8: return m02;
        case 9: return m12;
        case 10: return m22;
        case 11: return m32;

        case 12: return m03;
        case 13: return m13;
        case 14: return m23;
        case 15: return m33;
    }
    
    return m00;
}

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


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

__host__ __device__ const float& cumat4f::operator[](const int &I) const 
{
    switch(I)
    {
        case 0: return m00;
        case 1: return m10;
        case 2: return m20;
        case 3: return m30;

        case 4: return m01;
        case 5: return m11;
        case 6: return m21;
        case 7: return m31;

        case 8: return m02;
        case 9: return m12;
        case 10: return m22;
        case 11: return m32;

        case 12: return m03;
        case 13: return m13;
        case 14: return m23;
        case 15: return m33;
    }
    
    return m00;
}

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


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

__host__ __device__ cumat4f cumat4f::operator+(const cumat4f &rhs)
{
    cumat4f ret;
    ret.m00 = m00 + rhs.m00;
    ret.m10 = m10 + rhs.m10;
    ret.m20 = m20 + rhs.m20;
    ret.m30 = m30 + rhs.m30;
    ret.m01 = m01 + rhs.m01;
    ret.m11 = m11 + rhs.m11;
    ret.m21 = m21 + rhs.m21;
    ret.m31 = m31 + rhs.m31;
    ret.m02 = m02 + rhs.m02;
    ret.m12 = m12 + rhs.m12;
    ret.m22 = m22 + rhs.m22;
    ret.m32 = m32 + rhs.m32;
    ret.m03 = m03 + rhs.m03;
    ret.m13 = m13 + rhs.m13;
    ret.m23 = m23 + rhs.m23;
    ret.m33 = m33 + rhs.m33;
    return ret;
}

__host__ __device__ cumat4f cumat4f::operator-(const cumat4f &rhs)
{
    cumat4f ret;
    ret.m00 = m00 - rhs.m00;
    ret.m10 = m10 - rhs.m10;
    ret.m20 = m20 - rhs.m20;
    ret.m30 = m30 - rhs.m30;
    ret.m01 = m01 - rhs.m01;
    ret.m11 = m11 - rhs.m11;
    ret.m21 = m21 - rhs.m21;
    ret.m31 = m31 - rhs.m31;
    ret.m02 = m02 - rhs.m02;
    ret.m12 = m12 - rhs.m12;
    ret.m22 = m22 - rhs.m22;
    ret.m32 = m32 - rhs.m32;
    ret.m03 = m03 - rhs.m03;
    ret.m13 = m13 - rhs.m13;
    ret.m23 = m23 - rhs.m23;
    ret.m33 = m33 - rhs.m33;
    return ret;
}

__host__ __device__ cumat4f cumat4f::operator*(const float &rhs)
{
    cumat4f ret;
    ret.m00 = m00 * rhs;
    ret.m10 = m10 * rhs;
    ret.m20 = m20 * rhs;
    ret.m30 = m30 * rhs;
    ret.m01 = m01 * rhs;
    ret.m11 = m11 * rhs;
    ret.m21 = m21 * rhs;
    ret.m31 = m31 * rhs;
    ret.m02 = m02 * rhs;
    ret.m12 = m12 * rhs;
    ret.m22 = m22 * rhs;
    ret.m32 = m32 * rhs;
    ret.m03 = m03 * rhs;
    ret.m13 = m13 * rhs;
    ret.m23 = m23 * rhs;
    ret.m33 = m33 * rhs;
    return ret;
}

__host__ __device__ cumat4f cumat4f::operator/(const float &rhs)
{
    cumat4f ret;
    ret.m00 = m00 / rhs;
    ret.m10 = m10 / rhs;
    ret.m20 = m20 / rhs;
    ret.m30 = m30 / rhs;
    ret.m01 = m01 / rhs;
    ret.m11 = m11 / rhs;
    ret.m21 = m21 / rhs;
    ret.m31 = m31 / rhs;
    ret.m02 = m02 / rhs;
    ret.m12 = m12 / rhs;
    ret.m22 = m22 / rhs;
    ret.m32 = m32 / rhs;
    ret.m03 = m03 / rhs;
    ret.m13 = m13 / rhs;
    ret.m23 = m23 / rhs;
    ret.m33 = m33 / rhs;
    return ret;
}
        
__host__ __device__ cuvect4f cumat4f::operator*(const cuvect4f &rhs)
{
    cuvect4f ret;
    ret.x = m00*rhs.x + m01*rhs.y + m02*rhs.z + m03*rhs.w;
    ret.y = m10*rhs.x + m11*rhs.y + m12*rhs.z + m13*rhs.w;
    ret.z = m20*rhs.x + m21*rhs.y + m22*rhs.z + m23*rhs.w;
    ret.w = m30*rhs.x + m31*rhs.y + m32*rhs.z + m33*rhs.w;
    return ret;
}


__host__ __device__ cumat4f cumat4f::operator*(const cumat4f &rhs)
{
    cumat4f ret;
    ret.m00 = m00*rhs.m00 + m01*rhs.m10 + m02*rhs.m20 + m03*rhs.m30;
    ret.m01 = m00*rhs.m01 + m01*rhs.m11 + m02*rhs.m21 + m03*rhs.m31;
    ret.m02 = m00*rhs.m02 + m01*rhs.m12 + m02*rhs.m22 + m03*rhs.m32;
    ret.m03 = m00*rhs.m03 + m01*rhs.m13 + m02*rhs.m23 + m03*rhs.m33;
    ret.m10 = m10*rhs.m00 + m11*rhs.m10 + m12*rhs.m20 + m13*rhs.m30;
    ret.m11 = m10*rhs.m01 + m11*rhs.m11 + m12*rhs.m21 + m13*rhs.m31;
    ret.m12 = m10*rhs.m02 + m11*rhs.m12 + m12*rhs.m22 + m13*rhs.m32;
    ret.m13 = m10*rhs.m03 + m11*rhs.m13 + m12*rhs.m23 + m13*rhs.m33;
    ret.m20 = m20*rhs.m00 + m21*rhs.m10 + m22*rhs.m20 + m23*rhs.m30;
    ret.m21 = m20*rhs.m01 + m21*rhs.m11 + m22*rhs.m21 + m23*rhs.m31;
    ret.m22 = m20*rhs.m02 + m21*rhs.m12 + m22*rhs.m22 + m23*rhs.m32;
    ret.m23 = m20*rhs.m03 + m21*rhs.m13 + m22*rhs.m23 + m23*rhs.m33;
    ret.m30 = m30*rhs.m00 + m31*rhs.m10 + m32*rhs.m20 + m33*rhs.m30;
    ret.m31 = m30*rhs.m01 + m31*rhs.m11 + m32*rhs.m21 + m33*rhs.m31;
    ret.m32 = m30*rhs.m02 + m31*rhs.m12 + m32*rhs.m22 + m33*rhs.m32;
    ret.m33 = m30*rhs.m03 + m31*rhs.m13 + m32*rhs.m23 + m33*rhs.m33;
    return ret;
}
__host__ __device__ cumat4f cumat4f::transpose()
{
    cumat4f q;
    q.m00 = m00;
    q.m10 = m01;
    q.m20 = m02;
    q.m30 = m03;
    
    q.m01 = m10;
    q.m11 = m11;
    q.m21 = m12;
    q.m31 = m13;
    
    q.m02 = m20;
    q.m12 = m21;
    q.m22 = m22;
    q.m32 = m23;
    
    q.m03 = m30;
    q.m13 = m31;
    q.m23 = m32;
    q.m33 = m33;
    
    return q;
}

__host__ __device__ float cumat4f::det()
{
    float dt;
    dt = m03*m12*m21*m30 -
        m02*m13*m21*m30 -
        m03*m11*m22*m30 +
        m01*m13*m22*m30 +
        m02*m11*m23*m30 -
        m01*m12*m23*m30 -
        m03*m12*m20*m31 +
        m02*m13*m20*m31 +
        m03*m10*m22*m31 -
        m00*m13*m22*m31 -
        m02*m10*m23*m31 +
        m00*m12*m23*m31 +
        m03*m11*m20*m32 -
        m01*m13*m20*m32 -
        m03*m10*m21*m32 +
        m00*m13*m21*m32 +
        m01*m10*m23*m32 -
        m00*m11*m23*m32 -
        m02*m11*m20*m33 +
        m01*m12*m20*m33 +
        m02*m10*m21*m33 -
        m00*m12*m21*m33 -
        m01*m10*m22*m33 +
        m00*m11*m22*m33;

    return dt;
}

__host__ __device__ cumat4f cumat4f::inverse()
{
    cumat4f mret;
    float dt = this->det();
    float idet;

    if(dt>1E-7 || dt<-1E-7)
    {
      idet = 1.0f/dt;
      mret.m00 = -m13*m22*m31 + m12*m23*m31 + m13*m21*m32 - m11*m23*m32 - m12*m21*m33 + m11*m22*m33;
      mret.m01 =  m03*m22*m31 - m02*m23*m31 - m03*m21*m32 + m01*m23*m32 + m02*m21*m33 - m01*m22*m33;
      mret.m02 = -m03*m12*m31 + m02*m13*m31 + m03*m11*m32 - m01*m13*m32 - m02*m11*m33 + m01*m12*m33;
      mret.m03 =  m03*m12*m21 - m02*m13*m21 - m03*m11*m22 + m01*m13*m22 + m02*m11*m23 - m01*m12*m23;
      mret.m10 =  m13*m22*m30 - m12*m23*m30 - m13*m20*m32 + m10*m23*m32 + m12*m20*m33 - m10*m22*m33;
      mret.m11 = -m03*m22*m30 + m02*m23*m30 + m03*m20*m32 - m00*m23*m32 - m02*m20*m33 + m00*m22*m33;
      mret.m12 =  m03*m12*m30 - m02*m13*m30 - m03*m10*m32 + m00*m13*m32 + m02*m10*m33 - m00*m12*m33;
      mret.m13 = -m03*m12*m20 + m02*m13*m20 + m03*m10*m22 - m00*m13*m22 - m02*m10*m23 + m00*m12*m23;
      mret.m20 = -m13*m21*m30 + m11*m23*m30 + m13*m20*m31 - m10*m23*m31 - m11*m20*m33 + m10*m21*m33;
      mret.m21 =  m03*m21*m30 - m01*m23*m30 - m03*m20*m31 + m00*m23*m31 + m01*m20*m33 - m00*m21*m33;
      mret.m22 = -m03*m11*m30 + m01*m13*m30 + m03*m10*m31 - m00*m13*m31 - m01*m10*m33 + m00*m11*m33;
      mret.m23 =  m03*m11*m20 - m01*m13*m20 - m03*m10*m21 + m00*m13*m21 + m01*m10*m23 - m00*m11*m23;
      mret.m30 =  m12*m21*m30 - m11*m22*m30 - m12*m20*m31 + m10*m22*m31 + m11*m20*m32 - m10*m21*m32;
      mret.m31 = -m02*m21*m30 + m01*m22*m30 + m02*m20*m31 - m00*m22*m31 - m01*m20*m32 + m00*m21*m32;
      mret.m32 =  m02*m11*m30 - m01*m12*m30 - m02*m10*m31 + m00*m12*m31 + m01*m10*m32 - m00*m11*m32;
      mret.m33 = -m02*m11*m20 + m01*m12*m20 + m02*m10*m21 - m00*m12*m21 - m01*m10*m22 + m00*m11*m22;
      mret.m00 *= idet;
      mret.m01 *= idet;
      mret.m02 *= idet;
      mret.m03 *= idet;
      mret.m10 *= idet;
      mret.m11 *= idet;
      mret.m12 *= idet;
      mret.m13 *= idet;
      mret.m20 *= idet;
      mret.m21 *= idet;
      mret.m22 *= idet;
      mret.m23 *= idet;
      mret.m30 *= idet;
      mret.m31 *= idet;
      mret.m32 *= idet;
      mret.m33 *= idet;
    }
    else
    {
      mret.m00 = inf;
      mret.m10 = inf;
      mret.m20 = inf;
      mret.m30 = inf;
      mret.m01 = inf;
      mret.m11 = inf;
      mret.m21 = inf;
      mret.m31 = inf;
      mret.m02 = inf;
      mret.m12 = inf;
      mret.m22 = inf;
      mret.m32 = inf;
      mret.m03 = inf;
      mret.m13 = inf;
      mret.m23 = inf;
      mret.m33 = inf;
    }
    //this was STUPID. Gaah. Computer algebra system saves the day? I'd be surprised if this didn't end up *slower* than gaussian elimination. Don't do this again!

    return mret;
}

__host__ __device__ cumat4f operator-(const cumat4f &rhs)
{
    cumat4f ret;
    ret.m00 = -rhs.m00;
    ret.m10 = -rhs.m10;
    ret.m20 = -rhs.m20;
    ret.m30 = -rhs.m30;
    ret.m01 = -rhs.m01;
    ret.m11 = -rhs.m11;
    ret.m21 = -rhs.m21;
    ret.m31 = -rhs.m31;
    ret.m02 = -rhs.m02;
    ret.m12 = -rhs.m12;
    ret.m22 = -rhs.m22;
    ret.m32 = -rhs.m32;
    ret.m03 = -rhs.m03;
    ret.m13 = -rhs.m13;
    ret.m23 = -rhs.m23;
    ret.m33 = -rhs.m33;
    return ret;
}

__host__ __device__ cumat4f& cumat4f::operator+=(const cumat4f &rhs)
{
    m00 += rhs.m00;  
    m01 += rhs.m01;  
    m02 += rhs.m02;  
    m03 += rhs.m03;
    m10 += rhs.m10;  
    m11 += rhs.m11;  
    m12 += rhs.m12;  
    m13 += rhs.m13;
    m20 += rhs.m20;  
    m21 += rhs.m21;  
    m22 += rhs.m22;  
    m23 += rhs.m23;
    m30 += rhs.m30;  
    m31 += rhs.m31;  
    m32 += rhs.m32;  
    m33 += rhs.m33;
    return *this;
}

__host__ __device__ cumat4f& cumat4f::operator-=(const cumat4f &rhs)
{
    m00 -= rhs.m00;  
    m01 -= rhs.m01;  
    m02 -= rhs.m02;  
    m03 -= rhs.m03;
    m10 -= rhs.m10;  
    m11 -= rhs.m11;  
    m12 -= rhs.m12;  
    m13 -= rhs.m13;
    m20 -= rhs.m20;  
    m21 -= rhs.m21; 
    m22 -= rhs.m22;  
    m23 -= rhs.m23;
    m30 -= rhs.m30;  
    m31 -= rhs.m31;  
    m32 -= rhs.m32;  
    m33 -= rhs.m33;
    return *this;
}

__host__ __device__ cumat4f& cumat4f::operator*=(const float &rhs)
{
    m00 *= rhs;  
    m01 *= rhs;  
    m02 *= rhs;  
    m03 *= rhs;
    m10 *= rhs;  
    m11 *= rhs;  
    m12 *= rhs;  
    m13 *= rhs;
    m20 *= rhs;  
    m21 *= rhs;  
    m22 *= rhs;  
    m23 *= rhs;
    m30 *= rhs;  
    m31 *= rhs;  
    m32 *= rhs;  
    m33 *= rhs;
    return *this;
}

__host__ __device__ cumat4f& cumat4f::operator/=(const float &rhs)
{
    float irhs = 1.0f / rhs;
    m00 *= irhs;  
    m01 *= irhs;  
    m02 *= irhs;  
    m03 *= irhs;
    m10 *= irhs;  
    m11 *= irhs;  
    m12 *= irhs;  
    m13 *= irhs;
    m20 *= irhs;  
    m21 *= irhs;  
    m22 *= irhs;  
    m23 *= irhs;
    m30 *= irhs;  
    m31 *= irhs;  
    m32 *= irhs;  
    m33 *= irhs;
    return *this;
}

__host__ __device__ cumat4f& cumat4f::operator*=(const cumat4f &rhs)
{
    cumat4f tmp = (*this);
    (*this) = tmp*rhs;
    return *this;;
}


__host__ __device__ float cuvect4f_dot(cuvect4f &a, cuvect4f &b)
{
    float ret = 0.0f;
    ret = a.x*b.x + a.y*b.y + a.z*b.z + a.w*b.w;
    return ret;
}

__host__ __device__ float cuvect4f_norm(cuvect4f &a)
{
    float ret = 0.0f;
    ret = ::sqrtf(cuvect4f_dot(a,a));
    return ret;
}

__host__ __device__ cuvect4f cuvect4f_normalize(cuvect4f &a)
{
    cuvect4f ret = cuvect4f(0.0f,0.0f,0.0f,0.0f);
    float nrm = cuvect4f_norm(a);
    if(nrm>0.0f)
        ret = a/nrm;
    return ret;
}

__host__ __device__ cuvect4f cuvect4f_proj(cuvect4f &a, cuvect4f &b)
{
    cuvect4f ret;
    cuvect4f bn = cuvect4f_normalize(b);
    float d = cuvect4f_dot(a,bn);
    ret = bn*d;
    return ret;
}




}; //namespace amscuda
