#include <amsculib2/amsculib2.hpp>

namespace amscuda
{

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

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

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

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

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

        return ret;
    }

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

        return ret;
    }

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

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

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


    __host__ __device__ double cuvect3_dot(cuvect3 a, cuvect3 b)
    {
        double ret = a.x*b.x+a.y*b.y+a.z*b.z;

        return ret;
    }

    __host__ __device__ cuvect3 cuvect3_cross(cuvect3 a, cuvect3 b)
    {
        cuvect3 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__ double cuvect3_norm(cuvect3 a)
    {
        double ret;
        ret = ::sqrt(a.x*a.x+a.y*a.y+a.z*a.z);
        return ret;
    }

    __host__ __device__ cuvect3 cuvect3_normalize(cuvect3 a)
    {
        cuvect3 ret;
        double m;
        m = ::sqrt(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__ cuvect3 cuvect3_proj(cuvect3 a, cuvect3 b)
    {
        cuvect3 ret;
        cuvect3 bn = cuvect3_normalize(b);
        double m = cuvect3_dot(a,bn);
        ret = bn*m;
        return ret;
    }

//transposes a 3x3 (9 element) matrix
__host__ __device__ void mat3_transpose(double *mat3inout)
{
    int I,J;
    double 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 mat3_copy(double *mat3_dest, const double *mat3_src)
{
    int I;
    if(mat3_dest==NULL || mat3_src==NULL)
        return;

    for(I=0;I<9;I++)
        mat3_dest[I] = mat3_src[I];

    return;
}


__host__ __device__ double mat3_det(double *mat3in)
{
    double 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 mat3_inverse(double *mat3inout)
{
    int I;
    double matint[9];
    double det = mat3_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;

    mat3_transpose(mat3inout);

    return;
}

__host__ __device__ cuvect3 mat3_mult(double *mat3in, cuvect3 cvin)
{
    int I,J;
    cuvect3 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 mat3_mult(double *matina, double *matinb, double *matout)
{
    double 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 mat3_hodgedual(cuvect3 vecin, double *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 mat3_hodgedual(double *matin, cuvect3 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 mat3_rot_from_axisangle(cuvect3 axis, double angle, double *matout)
{
    int I;
    double H[9];
    double Hsq[9];
    double 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 = cuvect3_normalize(axis);
    
    mat3_hodgedual(axis,H);
    mat3_mult(H,H,Hsq);

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

    return;
}



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

    printf("3 dim vector and matrix functional tests on host side\n");
    
    cuvect3 a,b,c;
    double 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] = ((double) rand())/((double) RAND_MAX);
            mb[I+3*J] = ma[I+3*J];
        }
    }

    mat3_inverse(mb);
    mat3_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 = cuvect3(1,1,1);
    b = mat3_mult(ma,a);
    b = mat3_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 = cuvect3(1,0,1);
    b = cuvect3(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 = cuvect3_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",cuvect3_dot(c,a),cuvect3_dot(c,b));

    printf("norm(a)=%1.3f, norm(b)=%1.3f, norm(c)=%1.3f\n",cuvect3_norm(a),cuvect3_norm(b),cuvect3_norm(c));
    a = cuvect3_normalize(a);
    b = cuvect3_normalize(b);
    c = cuvect3_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",cuvect3_dot(c,a),cuvect3_dot(c,b));
    printf("norm(a)=%1.3f, norm(b)=%1.3f, norm(c)=%1.3f\n",cuvect3_norm(a),cuvect3_norm(b),cuvect3_norm(c));

    return;
}



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

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

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

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

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

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

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

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

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

__host__ __device__ cuvect3 cumat3::operator*(cuvect3 lhs)
{
    cuvect3 ret = cuvect3(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__ cumat3 cumat3::operator*(cumat3 lhs)
{
    cumat3 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__ double cumat3::det()
{
    double 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__ cumat3 cumat3::transpose()
{
    cumat3 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__ cumat3 cumat3::inverse()
{
    cumat3 q;
    double 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;
}

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

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

};