#include <amsculib3/amsculib3.hpp>

namespace amscuda
{

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

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

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

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

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

    return x;
}

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

    return x;
}

__host__ __device__ cuvec4f cuvec4f::operator+(const cuvec4f& rhs) const
{
    cuvec4f 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__ cuvec4f cuvec4f::operator-(const cuvec4f& rhs) const
{
    cuvec4f 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__ cuvec4f cuvec4f::operator*(const cuvec4f& rhs) const
{
    //Elementwise product
    cuvec4f 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__ cuvec4f cuvec4f::operator/(const cuvec4f& rhs) const
{
    //Elementwise division
    cuvec4f 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__ cuvec4f operator*(const cuvec4f& lhs, const float& rhs)
{
    cuvec4f ret;
    ret.x = lhs.x*rhs;
    ret.y = lhs.y*rhs;
    ret.z = lhs.z*rhs;
    ret.w = lhs.w*rhs;
    return ret;
}

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

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

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

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

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

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

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

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


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

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

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

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

    m30 = 0;
    m31 = 0;
    m32 = 0;
    m33 = 0;

    return;
}

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

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

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

    //m30 = 0;
    //m31 = 0;
    //m32 = 0;
    //m33 = 0;

    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__ 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__ float& cumat4f::operator()(const int &I, const int &J)
{
    return (*this)[I+4*J];
}

__host__ __device__ const float& cumat4f::operator()(const int &I, const int &J) const
{
    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::at(const int &I, const int &J) const
{
    return (*this)[I+4*J];
}

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

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

__host__ __device__ cumat4f cumat4f::operator+(const cumat4f& rhs) const
{
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) const
{
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) const
{
cumat4f ret; //should be zeroed in constructor
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 operator*(const cumat4f& lhs, const float& rhs)
{
cumat4f ret;
ret.m00=lhs.m00*rhs;
ret.m10=lhs.m10*rhs;
ret.m20=lhs.m20*rhs;
ret.m30=lhs.m30*rhs;
ret.m01=lhs.m01*rhs;
ret.m11=lhs.m11*rhs;
ret.m21=lhs.m21*rhs;
ret.m31=lhs.m31*rhs;
ret.m02=lhs.m02*rhs;
ret.m12=lhs.m12*rhs;
ret.m22=lhs.m22*rhs;
ret.m32=lhs.m32*rhs;
ret.m03=lhs.m03*rhs;
ret.m13=lhs.m13*rhs;
ret.m23=lhs.m23*rhs;
ret.m33=lhs.m33*rhs;
return ret;
}

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

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

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

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

__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;
m10 += rhs.m10;
m20 += rhs.m20;
m30 += rhs.m30;
m01 += rhs.m01;
m11 += rhs.m11;
m21 += rhs.m21;
m31 += rhs.m31;
m02 += rhs.m02;
m12 += rhs.m12;
m22 += rhs.m22;
m32 += rhs.m32;
m03 += rhs.m03;
m13 += rhs.m13;
m23 += rhs.m23;
m33 += rhs.m33;
return *this;
}

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

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

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

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

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

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

__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;
}

//////////////////////////
// Standalone Functions //
//////////////////////////

__host__ __device__ float cuvec4f_dot(cuvec4f &a, cuvec4f &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 cuvec4f_norm(cuvec4f &a)
{
    float ret = 0.0f;
    ret = ::sqrtf(cuvec4f_dot(a,a));
    return ret;
}

__host__ __device__ cuvec4f cuvec4f_normalize(cuvec4f &a)
{
    cuvec4f ret = cuvec4f(0.0f,0.0f,0.0f,0.0f);
    float nrm = cuvec4f_norm(a);
    if(nrm>0.0f)
        ret = a/nrm;
    return ret;
}

__host__ __device__ cuvec4f cuvec4f_proj(cuvec4f &a, cuvec4f &b)
{
    cuvec4f ret;
    cuvec4f bn = cuvec4f_normalize(b);
    float d = cuvec4f_dot(a,bn);
    ret = bn*d;
    return ret;
}

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

}; //namespace amscuda
