#include <amsculib2/amsculib2.hpp>

namespace amscuda
{

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

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

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

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

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

    __host__ __device__ cuvec2f cuvec2f::operator+(const cuvec2f &lhs)
    {
        cuvec2f ret;
        ret.x = x + lhs.x;
        ret.y = y + lhs.y;
        return ret;
    }

    __host__ __device__ cuvec2f cuvec2f::operator-(const cuvec2f &lhs)
    {
        cuvec2f ret;
        ret.x = x - lhs.x;
        ret.y = y - lhs.y;
        return ret;
    }
    __host__ __device__ cuvec2f cuvec2f::operator*(const float &lhs)
    {
        cuvec2f ret;
        ret.x = x*lhs;
        ret.y = y*lhs;
        return ret;
    }

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

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

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

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

    __host__ __device__ cuvec2f cuvec2f_normalize(const cuvec2f &a)
    {
        cuvec2f ret;
        float m = cuvec2f_norm(a);
        if(m>0.0)
        {
            ret.x = a.x/m; ret.y = a.y/m;
        }
        else
        {
            ret.x = 0.0; ret.y = 0.0;
        }
        return ret;
    }

    __host__ __device__ cuvec2f cuvec2f_proj(const cuvec2f &a, const cuvec2f &b)
    {
        cuvec2f ret;
        cuvec2f bn = cuvec2f_normalize(b);
        float m = cuvec2f_dot(a,bn);
        ret = bn*m;
        return ret;
    }


__host__ __device__ cumat2f::cumat2f()
{
    int I;
    for(I=0;I<4;I++)
    {
        dat[I] = 0.0;
    }
    return;
}

__host__ __device__ cumat2f::~cumat2f()
{
    int I;
    for(I=0;I<4;I++)
    {
        dat[I] = 0.0;
    }
    return;
}

__host__ __device__ float& cumat2f::operator[](const int I)
{
    return dat[I];
}

__host__ __device__ float& cumat2f::operator()(const int I, const int J)
{
    return dat[I+2*J];
}

__host__ __device__ cumat2f cumat2f::operator+(cumat2f lhs)
{
    int I;
    cumat2f ret;
    for(I=0;I<4;I++)
    {
        ret.dat[I] = this->dat[I] + lhs.dat[I];
    }
    return ret;
}

__host__ __device__ cumat2f cumat2f::operator-(cumat2f lhs)
{
    int I;
    cumat2f ret;
    for(I=0;I<4;I++)
    {
        ret.dat[I] = this->dat[I] - lhs.dat[I];
    }
    return ret;
}

__host__ __device__ cumat2f cumat2f::operator*(float lhs)
{
    cumat2f ret;
    int I;
    for(I=0;I<4;I++)
    {
        ret.dat[I] = this->dat[I]*lhs;
    }
    return ret;
}

__host__ __device__ cumat2f cumat2f::operator/(float lhs)
{
    cumat2f ret;
    int I;
    for(I=0;I<4;I++)
    {
        ret.dat[I] = this->dat[I]/lhs;
    }
    return ret;
}

__host__ __device__ float& cumat2f::at(const int I, const int J)
{
    return dat[I+2*J];
}
        

__host__ __device__ cuvec2f cumat2f::operator*(cuvec2f lhs)
{
    cuvec2f ret = cuvec2f(0.0,0.0);
    int I,J;
    for(I=0;I<2;I++)
    {
        for(J=0;J<2;J++)
        {
            ret[I] = ret[I] + this->at(I,J)*lhs[J];
        }
    }
    return ret;
}

__host__ __device__ cumat2f cumat2f::operator*(cumat2f lhs)
{
    cumat2f ret;
    int I,J,K;

    for(I=0;I<2;I++)
    {
        for(J=0;J<2;J++)
        {
            ret(I,J) = 0.0;
            for(K=0;K<2;K++)
            {
                ret(I,J) = ret(I,J) + this->at(I,K)*lhs(K,J);
            }
        }
    }

    return ret;
}

__host__ __device__ float cumat2f::det()
{
    float ret = 0.0;
    ret = ret + this->at(0,0)*this->at(1,1);
    ret = ret - this->at(1,0)*this->at(0,1);
    return ret;
}

__host__ __device__ cumat2f cumat2f::transpose()
{
    cumat2f q;
    int I,J;
    for(I=0;I<2;I++)
    {
        for(J=0;J<2;J++)
        {
            q.at(I,J) = this->at(J,I);
        }
    }
    return q;
}

__host__ __device__ cumat2f cumat2f::inverse()
{
    cumat2f q;
    float dt = q.det();
    if(dt!=0.0)
    {
        q(0,0) = this->at(1,1)/dt;
        q(0,1) = -this->at(0,1)/dt;
        q(1,0) = -this->at(1,0)/dt;
        q(1,1) = this->at(0,0)/dt;
    }
    else
    {
        q(0,0) = inf;
        q(0,1) = inf;
        q(1,0) = inf;
        q(1,1) = inf;
    }

    return q;
}

//2x2 matrix operations
//matrix order is assumed to be mat[I,J] = mat[I+3*J]

//transpose a 2x2 matrix in place
__host__ __device__ void mat2f_transpose(float *mat2inout)
{
    float mat2f_in[4];

    mat2f_copy(mat2f_in,mat2inout);
    mat2inout[0+0*2] = mat2f_in[0+0*2];
    mat2inout[1+0*2] = mat2f_in[0+1*2];
    mat2inout[0+1*2] = mat2f_in[1+0*2];
    mat2inout[1+1*2] = mat2f_in[1+1*2];
    
    return;
}

//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__ cuvec2f mat2f_mult(float *mat2a, const cuvec2f &b)
{
    cuvec2f 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__ cuvec2f operator-(cuvec2f rhs)
{
    cuvec2f 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_cuvec2f_1()
{


    return;
}

};