#include <amsculib2/amsculib2.hpp>

namespace amscuda
{


//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.0f;

    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, const cuvect3f &cvin)
{
    int I,J;
    cuvect3f ret;
    for(I=0;I<3;I++)
    {
        ret[I] = 0.0f;
        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.0f;
        }
    }

    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(const 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.0f;
    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;
}

};