#include <amsculib2/amsculib2.hpp>

namespace amscuda
{

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

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

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

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

    __host__ __device__ cuvect3f cuvect3f::operator+(cuvect3f lhs)
    {
        cuvect3f ret;
        ret.x = x+lhs.x;
        ret.y = y+lhs.y;
        ret.z = z+lhs.z;

        return ret;
    }

    __host__ __device__ cuvect3f cuvect3f::operator-(cuvect3f lhs)
    {
        cuvect3f ret;
        ret.x = x-lhs.x;
        ret.y = y-lhs.y;
        ret.z = z-lhs.z;

        return ret;
    }

    __host__ __device__ cuvect3f cuvect3f::operator*(float lhs)
    {
        cuvect3f ret;
        ret.x = x*lhs;
        ret.y = y*lhs;
        ret.z = z*lhs;
        return ret;
    }

    __host__ __device__ cuvect3f cuvect3f::operator/(float lhs)
    {
        cuvect3f ret;
        ret.x = x/lhs;
        ret.y = y/lhs;
        ret.z = z/lhs;
        return ret;
    }

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


    __host__ __device__ float cuvect3f_dot(cuvect3f a, cuvect3f b)
    {
        float ret = a.x*b.x+a.y*b.y+a.z*b.z;

        return ret;
    }

    __host__ __device__ cuvect3f cuvect3f_cross(cuvect3f a, cuvect3f b)
    {
        cuvect3f 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 cuvect3f_norm(cuvect3f a)
    {
        float ret;
        ret = ::sqrtf(a.x*a.x+a.y*a.y+a.z*a.z);
        return ret;
    }

    __host__ __device__ cuvect3f cuvect3f_normalize(cuvect3f a)
    {
        cuvect3f 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.0; ret.y = 0.0; ret.z = 0.0;
        }

        return ret;
    }

    __host__ __device__ cuvect3f cuvect3f_proj(cuvect3f a, cuvect3f b)
    {
        cuvect3f ret;
        cuvect3f bn = cuvect3f_normalize(b);
        float m = cuvect3f_dot(a,bn);
        ret = bn*m;
        return ret;
    }

//transposes a 3x3 (9 element) matrix
__host__ __device__ void mat3f_transpose(float *mat3inout)
{
    int I,J;
    float matint[9];
    for(I=0;I<9;I++)
    {
        matint[I] = mat3inout[I];
    }

    for(I=0;I<3;I++)
    {
        for(J=0;J<3;J++)
        {
            mat3inout[I+J*3] = matint[J+I*3];
        }
    }

    return;
}

//copies src to dest
__host__ __device__ void mat3f_copy(float *mat3f_dest, const float *mat3f_src)
{
    int I;
    if(mat3f_dest==NULL || mat3f_src==NULL)
        return;

    for(I=0;I<9;I++)
        mat3f_dest[I] = mat3f_src[I];

    return;
}


__host__ __device__ float mat3f_det(float *mat3in)
{
    float ret = 0.0;

    ret = ret + mat3in[0+0*3]*mat3in[1+1*3]*mat3in[2+2*3];
    ret = ret + mat3in[0+1*3]*mat3in[1+2*3]*mat3in[2+0*3];
    ret = ret + mat3in[0+2*3]*mat3in[1+0*3]*mat3in[2+1*3];
    ret = ret - mat3in[0+0*3]*mat3in[1+2*3]*mat3in[2+1*3];
    ret = ret - mat3in[0+1*3]*mat3in[1+0*3]*mat3in[2+2*3];
    ret = ret - mat3in[0+2*3]*mat3in[1+1*3]*mat3in[2+0*3];

    return ret;
}

//inverts a 3x3 (9 element) matrix
__host__ __device__ void mat3f_inverse(float *mat3inout)
{
    int I;
    float matint[9];
    float det = mat3f_det(mat3inout);

    for(I=0;I<9;I++)
    {
        matint[I] = mat3inout[I];
    }

    mat3inout[0+0*3] =  (matint[1+1*3]*matint[2+2*3]-matint[1+2*3]*matint[2+1*3])/det;
    mat3inout[0+1*3] = -(matint[1+0*3]*matint[2+2*3]-matint[1+2*3]*matint[2+0*3])/det;
    mat3inout[0+2*3] =  (matint[1+0*3]*matint[2+1*3]-matint[1+1*3]*matint[2+0*3])/det;
    mat3inout[1+0*3] = -(matint[0+1*3]*matint[2+2*3]-matint[0+2*3]*matint[2+1*3])/det;
    mat3inout[1+1*3] =  (matint[0+0*3]*matint[2+2*3]-matint[0+2*3]*matint[2+0*3])/det;
    mat3inout[1+2*3] = -(matint[0+0*3]*matint[2+1*3]-matint[0+1*3]*matint[2+0*3])/det;
    mat3inout[2+0*3] =  (matint[0+1*3]*matint[1+2*3]-matint[0+2*3]*matint[1+1*3])/det;
    mat3inout[2+1*3] = -(matint[0+0*3]*matint[1+2*3]-matint[0+2*3]*matint[1+0*3])/det;
    mat3inout[2+2*3] =  (matint[0+0*3]*matint[1+1*3]-matint[0+1*3]*matint[1+0*3])/det;

    mat3f_transpose(mat3inout);

    return;
}

__host__ __device__ cuvect3f mat3f_mult(float *mat3in, cuvect3f cvin)
{
    int I,J;
    cuvect3f ret;
    for(I=0;I<3;I++)
    {
        ret[I] = 0.0;
        for(J=0;J<3;J++)
        {
            ret[I] = ret[I] + mat3in[I+3*J]*cvin[J];
        }
    }

    return ret;
}

__host__ __device__ void mat3f_mult(float *matina, float *matinb, float *matout)
{
    float wrk[9];
    int I,J,K;

    for(I=0;I<3;I++)
    {
        for(J=0;J<3;J++)
        {
            wrk[I+3*J] = 0.0;
        }
    }

    for(I=0;I<3;I++)
    {
        for(J=0;J<3;J++)
        {
            for(K=0;K<3;K++)
            {
                wrk[I+3*K] = wrk[I+3*K] + matina[I+3*J]*matinb[J+3*K];
            }
        }
    }

    for(I=0;I<3;I++)
    {
        for(J=0;J<3;J++)
        {
            matout[I+3*J] = wrk[I+3*J];
        }
    }

    return;
}

__host__ __device__ void mat3f_hodgedual(cuvect3f vecin, float *matout)
{
    matout[0 + 0*3] = 0.0f;
    matout[1 + 0*3] = -vecin[2];
    matout[2 + 0*3] = vecin[1];

    matout[0 + 1*3] = vecin[2];
    matout[1 + 1*3] = 0.0f;
    matout[2 + 1*3] = -vecin[0];

    matout[0 + 2*3] = -vecin[1];
    matout[1 + 2*3] = vecin[0];
    matout[2 + 2*3] = 0.0f;
    return;
}

__host__ __device__ void mat3f_hodgedual(float *matin, cuvect3f vecout)
{
    vecout[0] = 0.5*(matin[1 + 2*3] - matin[2 + 1*3]);
    vecout[1] = 0.5*(matin[2 + 0*3] - matin[0 + 2*3]);
    vecout[2] = 0.5*(matin[0 + 1*3] - matin[1 + 0*3]);
    
    return;
}

//returns direction cosine rotation matrix from axis and angle
__host__ __device__ void mat3f_rot_from_axisangle(cuvect3f axis, float angle, float *matout)
{
    int I;
    float H[9];
    float Hsq[9];
    float II[9];

    for(I=0;I<9;I++) II[I] = 0.0;
    II[0+0*3] = 1.0;
    II[1+1*3] = 1.0;
    II[2+2*3] = 1.0;

    axis = cuvect3f_normalize(axis);
    
    mat3f_hodgedual(axis,H);
    mat3f_mult(H,H,Hsq);

    for(I=0;I<9;I++)
    {
        matout[I] = (II[I] + Hsq[I]) + H[I]*sinf(angle) - Hsq[I]*cosf(angle);
    }

    return;
}



__host__ void test_cudavectf_logic1()
{
    //3 dim vector and matrix functional tests on host side

    printf("3 dim vector and matrix functional tests on host side\n");
    
    cuvect3f a,b,c;
    float ma[9],mb[9],mc[9];

    int I,J;

    for(I=0;I<3;I++)
    {
        for(J=0;J<3;J++)
        {
            ma[I+3*J] = ((float) rand())/((float) RAND_MAX);
            mb[I+3*J] = ma[I+3*J];
        }
    }

    mat3f_inverse(mb);
    mat3f_mult(ma,mb,mc);

    for(I=0;I<3;I++)
    {
        for(J=0;J<3;J++)
        {
            printf("ma[%d,%d] = %1.3f\n",I,J,ma[I+3*J]);
        }
    }
    for(I=0;I<3;I++)
    {
        for(J=0;J<3;J++)
        {
            printf("mb[%d,%d] = %1.3f\n",I,J,mb[I+3*J]);
        }
    }
    for(I=0;I<3;I++)
    {
        for(J=0;J<3;J++)
        {
            printf("mc[%d,%d] = %1.3f\n",I,J,mc[I+3*J]);
        }
    }

    a = cuvect3f(1,1,1);
    b = mat3f_mult(ma,a);
    b = mat3f_mult(mb,b);
    
    for(I=0;I<3;I++)
    {
        printf("a[%d] = %1.3f, b[%d] = %1.3f\n",I,a[I],I,b[I]);
    }

    a = cuvect3f(1,0,1);
    b = cuvect3f(0,1,-1);
    c = a+b;

    for(I=0;I<3;I++)
    {
        printf("a[%d] = %1.3f, b[%d] = %1.3f, c[%d] = %1.3f\n",I,a[I],I,b[I],I,c[I]);
    }

    c = c/2.0;

    for(I=0;I<3;I++)
    {
        printf("a[%d] = %1.3f, b[%d] = %1.3f, c[%d] = %1.3f\n",I,a[I],I,b[I],I,c[I]);
    }

    c = cuvect3f_cross(a,b);

    for(I=0;I<3;I++)
    {
        printf("a[%d] = %1.3f, b[%d] = %1.3f, c[%d] = %1.3f\n",I,a[I],I,b[I],I,c[I]);
    }

    printf("c dot a = %1.3f, c dot b = %1.3f\n",cuvect3f_dot(c,a),cuvect3f_dot(c,b));

    printf("norm(a)=%1.3f, norm(b)=%1.3f, norm(c)=%1.3f\n",cuvect3f_norm(a),cuvect3f_norm(b),cuvect3f_norm(c));
    a = cuvect3f_normalize(a);
    b = cuvect3f_normalize(b);
    c = cuvect3f_normalize(c);

    for(I=0;I<3;I++)
    {
        printf("a[%d] = %1.3f, b[%d] = %1.3f, c[%d] = %1.3f\n",I,a[I],I,b[I],I,c[I]);
    }
    printf("c dot a = %1.3f, c dot b = %1.3f\n",cuvect3f_dot(c,a),cuvect3f_dot(c,b));
    printf("norm(a)=%1.3f, norm(b)=%1.3f, norm(c)=%1.3f\n",cuvect3f_norm(a),cuvect3f_norm(b),cuvect3f_norm(c));

    return;
}



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

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

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

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

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

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

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

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

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

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

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

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

    return ret;
}

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

    ret = ret + this->at(0,0)*this->at(1,1)*this->at(2,2);
    ret = ret + this->at(0,1)*this->at(1,2)*this->at(2,0);
    ret = ret + this->at(0,2)*this->at(1,0)*this->at(2,1);
    ret = ret - this->at(0,0)*this->at(1,2)*this->at(2,1);
    ret = ret - this->at(0,1)*this->at(1,0)*this->at(2,2);
    ret = ret - this->at(0,2)*this->at(1,1)*this->at(2,0);

    return ret;
}

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

__host__ __device__ cumat3f cumat3f::inverse()
{
    cumat3f q;
    float dt = q.det();
    if(dt!=0.0)
    {
        q(0,0) =  (this->at(1,1)*this->at(2,2)-this->at(1,2)*this->at(2,1))/dt;
        q(0,1) = -(this->at(1,0)*this->at(2,2)-this->at(1,2)*this->at(2,0))/dt;
        q(0,2) =  (this->at(1,0)*this->at(2,1)-this->at(1,1)*this->at(2,0))/dt;
        q(1,0) = -(this->at(0,1)*this->at(2,2)-this->at(0,2)*this->at(2,1))/dt;
        q(1,1) =  (this->at(0,0)*this->at(2,2)-this->at(0,2)*this->at(2,0))/dt;
        q(1,2) = -(this->at(0,0)*this->at(2,1)-this->at(0,1)*this->at(2,0))/dt;
        q(2,0) =  (this->at(0,1)*this->at(1,2)-this->at(0,2)*this->at(1,1))/dt;
        q(2,1) = -(this->at(0,0)*this->at(1,2)-this->at(0,2)*this->at(1,0))/dt;
        q(2,2) =  (this->at(0,0)*this->at(1,1)-this->at(0,1)*this->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;
}

};