#include <amsculib2/amsculib2.hpp>

namespace amscuda
{

    __host__ __device__ cuvect2f::cuvect2f()
    {
        x = 0.0f; y = 0.0f;
        return;
    }

    __host__ __device__ cuvect2f::~cuvect2f()
    {
        x = 0.0f; y = 0.0f;
        return;
    }

    __host__ __device__ float& cuvect2f::operator[](const int &I)
    {
        if(I==0) return x;
        if(I==1) return y;
        return x;
    }

    __host__ __device__ const float& cuvect2f::operator[](const int &I) const
    {
        if(I==0) return x;
        if(I==1) return y;
        return x;
    }

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

        return ret;
    }

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

        return ret;
    }

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

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

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

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

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

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


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

    __host__ __device__ float cuvect2f_dot(const cuvect2f &a, const cuvect2f &b)
    {
        float ret = a.x*b.x+a.y*b.y;
        return ret;
    }

    __host__ __device__ float cuvect2f_cross(const cuvect2f &a, const cuvect2f &b)
    {
        float ret = a.x*b.y-a.y*b.x;
        return ret;
    }

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

    __host__ __device__ cuvect2f cuvect2f_normalize(const cuvect2f &a)
    {
        cuvect2f ret;
        float m = cuvect2f_norm(a);
        if(m>0.0)
        {
            ret.x = a.x/m; ret.y = a.y/m;
        }
        else
        {
            ret.x = 0.0f; ret.y = 0.0f;
        }
        return ret;
    }

    __host__ __device__ cuvect2f cuvect2f_proj(const cuvect2f &a, const cuvect2f &b)
    {
        cuvect2f ret;
        cuvect2f bn = cuvect2f_normalize(b);
        float m = cuvect2f_dot(a,bn);
        ret = bn*m;
        return ret;
    }

    __host__ __device__ cumat2f::cumat2f()
    {
        m00 = 0.0f;
        m01 = 0.0f;
        m10 = 0.0f;
        m11 = 0.0f;

        return;
    }
    
    __host__ __device__ cumat2f::~cumat2f()
    {
        m00 = 0.0f;
        m01 = 0.0f;
        m10 = 0.0f;
        m11 = 0.0f;
        return;
    }
    
    __host__ __device__ float& cumat2f::operator[](const int &I)
    {
        if(I==0) return m00;
        if(I==1) return m10;
        if(I==2) return m01;
        if(I==3) return m11;
        
        return m00;
    }

    __host__ __device__ const float& cumat2f::operator[](const int &I) const 
    {
        if(I==0) return m00;
        if(I==1) return m10;
        if(I==2) return m01;
        if(I==3) return m11;
        
        return m00;
    }
    
    __host__ __device__ float& cumat2f::operator()(const int &I, const int &J)
    {
        if(I==0 && J==0) return m00;
        if(I==1 && J==0) return m10;
        if(I==0 && J==1) return m01;
        if(I==1 && J==1) return m11;
    
        return m00;
    }
    
    
    __host__ __device__ float& cumat2f::at(const int &I, const int &J)
    {
        if(I==0 && J==0) return m00;
        if(I==1 && J==0) return m10;
        if(I==0 && J==1) return m01;
        if(I==1 && J==1) return m11;

        return m00;
    }
    
    __host__ __device__ const float& cumat2f::operator()(const int &I, const int &J) const 
    {
        if(I==0 && J==0) return m00;
        if(I==1 && J==0) return m10;
        if(I==0 && J==1) return m01;
        if(I==1 && J==1) return m11;
    
        return m00;
    }
    
    __host__ __device__ const float& cumat2f::at(const int &I, const int &J) const 
    {
        if(I==0 && J==0) return m00;
        if(I==1 && J==0) return m10;
        if(I==0 && J==1) return m01;
        if(I==1 && J==1) return m11;
    
        return m00;
    }
    
    
    __host__ __device__ cumat2f cumat2f::operator+(const cumat2f &rhs)
    {
        cumat2f ret;
        ret.m00 = m00 + rhs.m00;
        ret.m10 = m10 + rhs.m10;
        ret.m01 = m01 + rhs.m01;
        ret.m11 = m11 + rhs.m11;
        
    
        return ret;
    }
    
    __host__ __device__ cumat2f cumat2f::operator-(const cumat2f &rhs)
    {
        cumat2f ret;
        ret.m00 = m00 - rhs.m00;
        ret.m10 = m10 - rhs.m10;
        ret.m01 = m01 - rhs.m01;
        ret.m11 = m11 - rhs.m11;

        return ret;
    }
    
    __host__ __device__ cumat2f cumat2f::operator*(const float &rhs)
    {
        cumat2f ret;
        ret.m00 = m00 * rhs;
        ret.m10 = m10 * rhs;
        ret.m01 = m01 * rhs;
        ret.m11 = m11 * rhs;
        return ret;
    }
    
    __host__ __device__ cumat2f cumat2f::operator/(const float &rhs)
    {
        cumat2f ret;
        ret.m00 = m00 / rhs;
        ret.m10 = m10 / rhs;
        ret.m01 = m01 / rhs;
        ret.m11 = m11 / rhs;
        return ret;
    }
            
    __host__ __device__ cuvect2f cumat2f::operator*(const cuvect2f &rhs)
    {
        cuvect2f ret;
        
        ret.x = m00*rhs.x + m01*rhs.y;
        ret.y = m10*rhs.x + m11*rhs.y;
    
        return ret;
    }
    
    __host__ __device__ cumat2f cumat2f::operator*(const cumat2f &rhs)
    {
        cumat2f ret;
    
        ret.m00 = m00*rhs.m00 + m01*rhs.m10;
        ret.m01 = m00*rhs.m01 + m01*rhs.m11;
        ret.m10 = m10*rhs.m00 + m11*rhs.m10;
        ret.m11 = m10*rhs.m01 + m11*rhs.m11;
    
        return ret;
    }
    
    __host__ __device__ float cumat2f::det()
    {
        float ret = 0.0;
    
        ret += m00*m11;
        ret -= m01*m10;
    
        return ret;
    }
    
    __host__ __device__ cumat2f cumat2f::transpose()
    {
        cumat2f ret;
    
        ret.m00 = m00;
        ret.m01 = m10;
        ret.m10 = m01;
        ret.m11 = m11;
    
        return ret;
    }
    
    __host__ __device__ cumat2f cumat2f::inverse()
    {
        cumat2f q;
        float dt = det();
        if(dt!=0)
        {
            q(0,0) = q(1,1)/dt;
            q(0,1) = -q(0,1)/dt;
            q(1,0) = -q(1,0)/dt;
            q(1,1) = q(0,0)/dt;

        }
        else
        {
            q(0,0) = inf;
            q(0,1) = inf;
            q(1,0) = inf;
            q(1,1) = inf;
        }
    
        return q;
    }
    
    __host__ __device__ cumat2f operator-(const cumat2f &rhs)
    {
        cumat2f ret;
        ret.m00 = -rhs.m00;
        ret.m10 = -rhs.m10;
        ret.m01 = -rhs.m01;
        ret.m11 = -rhs.m11;
    
        return ret;
    }
    
    __host__ __device__ cumat2f& cumat2f::operator+=(const cumat2f &rhs)
    {
        m00 += rhs.m00;
        m10 += rhs.m10;
        m01 += rhs.m01;
        m11 += rhs.m11;
    
        return *this;
    }
    
    __host__ __device__ cumat2f& cumat2f::operator-=(const cumat2f &rhs)
    {
        m00 -= rhs.m00;
        m10 -= rhs.m10;
        m01 -= rhs.m01;
        m11 -= rhs.m11;
    
        return *this;
    }
    
    
    
    __host__ __device__ cumat2f& cumat2f::operator/=(const float &rhs)
    {
        m00 /= rhs;
        m10 /= rhs;
        m01 /= rhs;
        m11 /= rhs;
    
        return *this;
    }
    
    __host__ __device__ cumat2f& cumat2f::operator*=(const float &rhs)
    {
        m00 *= rhs;
        m10 *= rhs;
        m01 *= rhs;
        m11 *= rhs;
    
        return *this;
    }
    
    __host__ __device__ cumat2f& cumat2f::operator*=(const cumat2f &rhs)
    {
        cumat2f tmp;
    
        tmp.m00 = m00*rhs.m00 + m01*rhs.m10; 
        tmp.m01 = m00*rhs.m01 + m01*rhs.m11; 
        tmp.m10 = m10*rhs.m00 + m11*rhs.m10; 
        tmp.m11 = m10*rhs.m01 + m11*rhs.m11;
    
        (*this) = tmp;
    
        return *this;
    }
    
    __host__ __device__ cumat2f::cumat2f(
        const float & _m00, const float & _m01,
        const float & _m10, const float & _m11
    )
    {
        m00 = _m00;
        m01 = _m01;
        m10 = _m10;
        m11 = _m11;
    }
    
    __host__ __device__ float* cumat2f::data()
    {
         //pointer to float[9] representation of matrix
        return (float*) this;
    }
    
    __host__ __device__ const float* cumat2f::data() const
    {
         //pointer to float[9] representation of matrix
        return (const float*) this;
    }
    


///////////////////////////
//legacy array operations//
///////////////////////////

//copies src to dest
__host__ __device__ void mat2f_copy(float *mat2f_dest, const float *mat2f_src)
{
    mat2f_dest[0+0*2] = mat2f_src[0+0*2];
    mat2f_dest[1+0*2] = mat2f_src[1+0*2];
    mat2f_dest[0+1*2] = mat2f_src[0+1*2];
    mat2f_dest[1+1*2] = mat2f_src[1+1*2];
    
    return;
}

//inverts mat?inout[4]
__host__ __device__ void mat2f_inverse(float *mat2inout)
{
    float det = mat2inout[0+0*2]*mat2inout[1+1*2]-mat2inout[0+1*2]*mat2inout[1+0*2];
    float mat2in[4];

    mat2f_copy(mat2in,mat2inout);
    mat2inout[0+0*2] = mat2inout[1+1*2]/det;
    mat2inout[1+0*2] = -mat2inout[1+0*2]/det;
    mat2inout[0+1*2] = -mat2inout[0+1*2]/det;
    mat2inout[1+1*2] = mat2inout[0+0*2]/det;
    
    return;
}

//rotatin matrix from angle
__host__ __device__ void mat2f_rot_from_angle(float angle, float *mat2)
{
    mat2[0+0*2] = ::cosf(angle);
    mat2[1+0*2] = ::sinf(angle);
    mat2[0+1*2] = -::sinf(angle);
    mat2[1+1*2] = ::cosf(angle);
    return;
}

//multiplies c = a*b
__host__ __device__ void mat2f_mult(float *mat2a, float *mat2b, float *mat2c)
{
    float mat2a_in[4];
    float mat2b_in[4];

    if(mat2a==NULL || mat2b==NULL || mat2c==NULL)
    {
        return;
    }

    mat2f_copy(mat2a_in,mat2a);
    mat2f_copy(mat2b_in,mat2b);
    
    mat2c[0+0*2] = mat2a_in[0+0*2]*mat2b_in[0+0*2] + mat2a_in[1+0*2]*mat2b_in[0+1*2];
    mat2c[1+0*2] = mat2a_in[0+0*2]*mat2b_in[1+0*2] + mat2a_in[1+0*2]*mat2b_in[1+1*2];
    mat2c[0+1*2] = mat2a_in[0+1*2]*mat2b_in[0+0*2] + mat2a_in[1+1*2]*mat2b_in[0+1*2];
    mat2c[1+1*2] = mat2a_in[0+1*2]*mat2b_in[1+0*2] + mat2a_in[1+1*2]*mat2b_in[1+1*2];

    return;
}

// ret = a*b
__host__ __device__ cuvect2f mat2f_mult(float *mat2a, const cuvect2f &b)
{
    cuvect2f ret;
    ret.x = b.x*mat2a[0+0*2] + b.y*mat2a[1+0*2];
    ret.y = b.x*mat2a[0+1*2] + b.y*mat2a[1+1*2];
    return ret;
}

__host__ __device__ cuvect2f operator-(cuvect2f rhs)
{
    cuvect2f ret;
    ret[0] = -rhs[0];
    ret[1] = -rhs[1];
    return ret;
}

__host__ __device__ cumat2f operator-(cumat2f rhs)
{
    cumat2f ret;
    int I;
    for(I=0;I<4;I++) ret[I] = -rhs[I];
    return ret;
}

void test_cuvect2f_1()
{


    return;
}

};