#include <osg/Matrix>
#include <osg/Quat>
#include <osg/Notify>
#include <osg/Types>

#include <cstdlib> //memcpy
#include <cmath> //acos

using namespace osg;


#define DEG2RAD(x)    ((x)*M_PI/180.0)
#define RAD2DEG(x)    ((x)*180.0/M_PI)


//#define WARN_DEPRECATED
#define ANGLES_IN_DEGREES

#define SET_ROW(row, v1, v2, v3, v4 )    \
    _mat[(row)][0] = (v1); \
    _mat[(row)][1] = (v2); \
    _mat[(row)][2] = (v3); \
    _mat[(row)][3] = (v4);

#define INNER_PRODUCT(a,b,r,c) \
     ((a)._mat[r][0] * (b)._mat[0][c]) \
    +((a)._mat[r][1] * (b)._mat[1][c]) \
    +((a)._mat[r][2] * (b)._mat[2][c]) \
    +((a)._mat[r][3] * (b)._mat[3][c])


Matrix::Matrix() : Object(), fully_realized(false) {}

Matrix::Matrix( const Matrix& other ) : Object()
{
    set( (float const * const) other._mat );
}

Matrix::Matrix( float const * const def )
{    
    set( def ); 
}

Matrix::Matrix( float a00, float a01, float a02, float a03,
                float a10, float a11, float a12, float a13,
                float a20, float a21, float a22, float a23,
                float a30, float a31, float a32, float a33)
{
    SET_ROW(0, a00, a01, a02, a03 )
    SET_ROW(1, a10, a11, a12, a13 )
    SET_ROW(2, a20, a21, a22, a23 )
    SET_ROW(3, a30, a31, a32, a33 )

    fully_realized = true;
}


Matrix& Matrix::operator = (const Matrix& other ) {
    if( &other == this ) return *this;
    set((const float*)other._mat);
    return *this;
}

void Matrix::set( float const * const def ) {
    memcpy( _mat, def, sizeof(_mat) );
    fully_realized = true;
}


void Matrix::set( float a00, float a01, float a02, float a03,
                  float a10, float a11, float a12, float a13,
                  float a20, float a21, float a22, float a23,
                  float a30, float a31, float a32, float a33)
{
    SET_ROW(0, a00, a01, a02, a03 )
    SET_ROW(1, a10, a11, a12, a13 )
    SET_ROW(2, a20, a21, a22, a23 )
    SET_ROW(3, a30, a31, a32, a33 )

    fully_realized = true;
}

void Matrix::setTrans( float tx, float ty, float tz )
{
#ifdef WARN_DEPRECATED
    notify(NOTICE) << "Matrix::setTrans is deprecated.";
#endif
    _mat[3][0] = tx;
    _mat[3][1] = ty;
    _mat[3][2] = tz;
}


void Matrix::setTrans( const Vec3& v )
{
#ifdef WARN_DEPRECATED
    notify(NOTICE) << "Matrix::setTrans is deprecated.";
#endif
    _mat[3][0] = v[0];
    _mat[3][1] = v[1];
    _mat[3][2] = v[2];
}

void Matrix::makeIdent()
{
    SET_ROW(0,    1, 0, 0, 0 )
    SET_ROW(1,    0, 1, 0, 0 )
    SET_ROW(2,    0, 0, 1, 0 )
    SET_ROW(3,    0, 0, 0, 1 )
    
    fully_realized = true;
}

void Matrix::makeScale( const Vec3& v )
{
    makeScale(v[0], v[1], v[2] );
}

void Matrix::makeScale( float x, float y, float z )
{
    SET_ROW(0,    x, 0, 0, 0 )
    SET_ROW(1,    0, y, 0, 0 )
    SET_ROW(2,    0, 0, z, 0 )
    SET_ROW(3,    0, 0, 0, 1 )

    fully_realized = true;
}

void Matrix::makeTrans( const Vec3& v )
{
    makeTrans( v[0], v[1], v[2] );
}

void Matrix::makeTrans( float x, float y, float z )
{
    SET_ROW(0,    1, 0, 0, 0 )
    SET_ROW(1,    0, 1, 0, 0 )
    SET_ROW(2,    0, 0, 1, 0 )
    SET_ROW(3,    x, y, z, 1 )

    fully_realized = true;
}

void Matrix::makeRot( const Vec3& from, const Vec3& to )
{
    double d = from * to; // dot product == cos( angle between from & to )
    if( d < 0.9999 ) {
        double angle = acos(d);
#ifdef ANGLES_IN_DEGREES
        angle = RAD2DEG(angle);
#endif
        Vec3 axis = to ^ from; //we know ((to) x (from)) is perpendicular to both
        makeRot( angle, axis );
    }        
    else 
        makeIdent();
}

void Matrix::makeRot( float angle, const Vec3& axis )
{
    makeRot( angle, axis.x(), axis.y(), axis.z() );
}

void Matrix::makeRot( float angle, float x, float y, float z ) {
    float d = sqrt( x*x + y*y + z*z );
    if( d == 0 )
        return;

#ifdef ANGLES_IN_DEGREES
    angle = DEG2RAD(angle);
#endif

    float sin_half = sin( angle/2 );
    float cos_half = cos( angle/2 );

    Quat q( sin_half * (x/d),
            sin_half * (y/d),
            sin_half * (z/d),
            cos_half );//NOTE: original used a private quaternion made of doubles
    makeRot( q );        // but Quat stores the values in a Vec4 made of floats.  
}

void Matrix::makeRot( const Quat& q ) {
    // taken from Shoemake/ACM SIGGRAPH 89
    Vec4 v = q.asVec4();

    double xs = 2 * v.x(); //assume q is already normalized? assert?
    double ys = 2 * v.y(); // if not, xs = 2 * v.x() / d, ys = 2 * v.y() / d
    double zs = 2 * v.z(); // and zs = 2 * v.z() /d where d = v.length2()

    double xx = xs * v.x();
    double xy = ys * v.x();
    double xz = zs * v.x();
    double yy = ys * v.y();
    double yz = zs * v.y();
    double zz = zs * v.z();
    double wx = xs * v.w();
    double wy = ys * v.w();
    double wz = zs * v.w();

    SET_ROW(0,    1.0-(yy+zz),    xy - wz,    xz + wy,    0.0 )
    SET_ROW(1,    xy + wz,        1.0-(xx+zz),yz - wx,    0.0 )
    SET_ROW(2,    xz - wy,        yz + wx,    1.0-(xx+yy),0.0 )
    SET_ROW(3,    0.0,            0.0,        0.0,        1.0 )

    fully_realized = true;
}

void Matrix::makeRot( float yaw, float pitch, float roll)
{
#ifdef ANGLES_IN_DEGREES
    yaw = DEG2RAD(yaw);
    pitch = DEG2RAD(pitch);
    roll = DEG2RAD(roll);
#endif

    // lifted straight from SOLID library v1.01 Quaternion.h
    // available from http://www.win.tue.nl/~gino/solid/
    // and also distributed under the LGPL
    float cosYaw = cos(yaw / 2);
    float sinYaw = sin(yaw / 2);
    float cosPitch = cos(pitch / 2);
    float sinPitch = sin(pitch / 2);
    float cosRoll = cos(roll / 2);
    float sinRoll = sin(roll / 2);
    Quat q(sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw,
            cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw,
            cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw,
            cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw);
    makeRot( q );
}

void Matrix::mult( const Matrix& lhs, const Matrix& rhs )
{
// PRECONDITION: We assume neither &lhs nor &rhs == this
// if it did, use preMult or postMult instead
    _mat[0][0] = INNER_PRODUCT(lhs, rhs, 0, 0);
    _mat[0][1] = INNER_PRODUCT(lhs, rhs, 0, 1);
    _mat[0][2] = INNER_PRODUCT(lhs, rhs, 0, 2);
    _mat[0][3] = INNER_PRODUCT(lhs, rhs, 0, 3);
    _mat[1][0] = INNER_PRODUCT(lhs, rhs, 1, 0);
    _mat[1][1] = INNER_PRODUCT(lhs, rhs, 1, 1);
    _mat[1][2] = INNER_PRODUCT(lhs, rhs, 1, 2);
    _mat[1][3] = INNER_PRODUCT(lhs, rhs, 1, 3);
    _mat[2][0] = INNER_PRODUCT(lhs, rhs, 2, 0);
    _mat[2][1] = INNER_PRODUCT(lhs, rhs, 2, 1);
    _mat[2][2] = INNER_PRODUCT(lhs, rhs, 2, 2);
    _mat[2][3] = INNER_PRODUCT(lhs, rhs, 2, 3);
    _mat[3][0] = INNER_PRODUCT(lhs, rhs, 3, 0);
    _mat[3][1] = INNER_PRODUCT(lhs, rhs, 3, 1);
    _mat[3][2] = INNER_PRODUCT(lhs, rhs, 3, 2);
    _mat[3][3] = INNER_PRODUCT(lhs, rhs, 3, 3);
    fully_realized = true;
}

void Matrix::preMult( const Matrix& other )
{
    if( !fully_realized ) {
        //act as if this were an identity Matrix
        set((const float*)other._mat);
        return;
    }

    // brute force method requiring a copy
    //Matrix tmp(other* *this);
    // *this = tmp;

    // more efficient method just use a float[4] for temporary storage.
    float t[4];
    for(int col=0; col<4; ++col) {
        t[0] = INNER_PRODUCT( other, *this, 0, col );
        t[1] = INNER_PRODUCT( other, *this, 1, col );
        t[2] = INNER_PRODUCT( other, *this, 2, col );
        t[3] = INNER_PRODUCT( other, *this, 3, col );
        _mat[0][col] = t[0];
        _mat[1][col] = t[1];
        _mat[2][col] = t[2];
        _mat[3][col] = t[3];
    }

}

void Matrix::postMult( const Matrix& other )
{
    if( !fully_realized ) {
        //act as if this were an identity Matrix
        set((const float*)other._mat);
        return;
    }
    // brute force method requiring a copy
    //Matrix tmp(*this * other);
    // *this = tmp;

    // more efficient method just use a float[4] for temporary storage.
    float t[4];
    for(int row=0; row<4; ++row)
    {
        t[0] = INNER_PRODUCT( *this, other, row, 0 );
        t[1] = INNER_PRODUCT( *this, other, row, 1 );
        t[2] = INNER_PRODUCT( *this, other, row, 2 );
        t[3] = INNER_PRODUCT( *this, other, row, 3 );
        SET_ROW(row, t[0], t[1], t[2], t[3] )
    }
}

#undef SET_ROW
#undef INNER_PRODUCT

bool Matrix::invert( const Matrix& _m )
{

    if (&_m==this) 
    {
        Matrix tm(_m);
        return invert(tm);
    }
    /*if ( _m._mat[0][3] == 0.0 
        && _m._mat[1][3] == 0.0 
        && _m._mat[2][3] == 0.0 
        && _m._mat[3][3] == 1.0 )
    {
        return invertAffine( _m );
    }*/

    // code lifted from VR Juggler.
    // not cleanly added, but seems to work.  RO.
    const float*  a = reinterpret_cast<const float*>(_m._mat);
    float*  b = reinterpret_cast<float*>(_mat);

    int     n = 4;
    int     i, j, k;
    int     r[ 4], c[ 4], row[ 4], col[ 4];
    float  m[ 4][ 4*2], pivot, max_m, tmp_m, fac;

    /* Initialization */
    for ( i = 0; i < n; i ++ )
    {
        r[ i] = c[ i] = 0;
        row[ i] = col[ i] = 0;
    }

    /* Set working Matrix */
    for ( i = 0; i < n; i++ )
    {
        for ( j = 0; j < n; j++ )
        {
            m[ i][ j] = a[ i * n + j];
            m[ i][ j + n] = ( i == j ) ? 1.0 : 0.0 ;
        }
    }

    /* Begin of loop */
    for ( k = 0; k < n; k++ )
    {
        /* Choosing the pivot */
        for ( i = 0, max_m = 0; i < n; i++ )
        {
            if ( row[ i]  ) continue;
            for ( j = 0; j < n; j++ )
            {
                if ( col[ j] ) continue;
                tmp_m = fabs( m[ i][j]);
                if ( tmp_m > max_m)
                {
                    max_m = tmp_m;
                    r[ k] = i;
                    c[ k] = j;
                }
            }
        }
        row[ r[k] ] = col[ c[k] ] = 1;
        pivot = m[ r[ k] ][ c[ k] ];

        if ( fabs( pivot) <= 1e-20)
        {
            notify(WARN) << "*** pivot = %f in mat_inv. ***\n";
            //exit( 0);
            return false;
        }

        /* Normalization */
        for ( j = 0; j < 2*n; j++ )
        {
            if ( j == c[ k] )
                m[ r[ k]][ j] = 1.0;
            else
                m[ r[ k]][ j] /=pivot;
        }

        /* Reduction */
        for ( i = 0; i < n; i++ )
        {
            if ( i == r[ k] )
                continue;

            for ( j=0, fac = m[ i][ c[k]];j < 2*n; j++ )
            {
                if ( j == c[ k] )
                    m[ i][ j] =0.0;
                else
                    m[ i][ j] -=fac * m[ r[k]][ j];
            }
        }
    }

    /* Assign invers to a Matrix */
    for ( i = 0; i < n; i++ )
        for ( j = 0; j < n; j++ )
            row[ i] = ( c[ j] == i ) ? r[j] : row[ i];

    for ( i = 0; i < n; i++ )
        for ( j = 0; j < n; j++ )
            b[ i * n +  j] = m[ row[ i]][j + n];

    return true;                 // It worked
}

const double PRECISION_LIMIT = 1.0e-15;

bool Matrix::invertAffine( const Matrix& _m )
{
    // adapted from Graphics Gems II.
    // 
    // This method treats the Matrix as a block Matrix and calculates
    // the inverse of one subMatrix, improving performance over something
    // that inverts any non-singular Matrix:
    //                           -1
    //    -1    [ A  0 ] -1    [ A    0 ]
    //   M   =  [      ]    =  [   -1   ]
    //          [ C  1 ]       [-CA   1 ]
    //
    // returns true if _m is nonsingular, and (*this) contains its inverse
    // otherwise returns false. (*this unchanged)

    // assert( this->isAffine())?
    double det_1, pos, neg, temp;

    pos = neg = 0.0;

#define ACCUMULATE \
    { \
        if(temp >= 0.0) pos += temp; \
        else neg += temp; \
    }

    temp = _m._mat[0][0] * _m._mat[1][1] * _m._mat[2][2]; ACCUMULATE;
    temp = _m._mat[0][1] * _m._mat[1][2] * _m._mat[2][0]; ACCUMULATE;
    temp = _m._mat[0][2] * _m._mat[1][0] * _m._mat[2][1]; ACCUMULATE;

    temp = - _m._mat[0][2] * _m._mat[1][1] * _m._mat[2][0]; ACCUMULATE;
    temp = - _m._mat[0][1] * _m._mat[1][0] * _m._mat[2][2]; ACCUMULATE;
    temp = - _m._mat[0][0] * _m._mat[1][2] * _m._mat[2][1]; ACCUMULATE;

    det_1 = pos + neg;

    if( (det_1 == 0.0) || (abs(det_1/(pos-neg)) < PRECISION_LIMIT )) {
        // _m has no inverse
        notify(WARN) << "Matrix::invert(): Matrix has no inverse." << endl;
        return false;
    }

    // inverse is adj(A)/det(A)
    det_1 = 1.0 / det_1;

    _mat[0][0] = (_m._mat[1][1] * _m._mat[2][2] - _m._mat[1][2] * _m._mat[2][1]) * det_1;
    _mat[1][0] = (_m._mat[1][0] * _m._mat[2][2] - _m._mat[1][2] * _m._mat[2][0]) * det_1;
    _mat[2][0] = (_m._mat[1][0] * _m._mat[2][1] - _m._mat[1][1] * _m._mat[2][0]) * det_1;
    _mat[0][1] = (_m._mat[0][1] * _m._mat[2][2] - _m._mat[0][2] * _m._mat[2][1]) * det_1;
    _mat[1][1] = (_m._mat[0][0] * _m._mat[2][2] - _m._mat[0][2] * _m._mat[2][0]) * det_1;
    _mat[2][1] = (_m._mat[0][0] * _m._mat[2][1] - _m._mat[0][1] * _m._mat[2][0]) * det_1;
    _mat[0][2] = (_m._mat[0][1] * _m._mat[1][2] - _m._mat[0][2] * _m._mat[1][1]) * det_1;
    _mat[1][2] = (_m._mat[0][0] * _m._mat[1][2] - _m._mat[0][2] * _m._mat[1][0]) * det_1;
    _mat[2][2] = (_m._mat[0][0] * _m._mat[1][1] - _m._mat[0][1] * _m._mat[1][0]) * det_1;

    // calculate -C * inv(A)
    _mat[3][0] = -(_m._mat[3][0] * _mat[0][0] + _m._mat[3][1] * _mat[1][0] + _m._mat[3][2] * _mat[2][0] );
    _mat[3][1] = -(_m._mat[3][0] * _mat[0][1] + _m._mat[3][1] * _mat[1][1] + _m._mat[3][2] * _mat[2][1] );
    _mat[3][2] = -(_m._mat[3][0] * _mat[0][2] + _m._mat[3][1] * _mat[1][2] + _m._mat[3][2] * _mat[2][2] );

    _mat[0][3] = 0.0;
    _mat[1][3] = 0.0;
    _mat[2][3] = 0.0;
    _mat[3][3] = 1.0;

    fully_realized = true;
    return true;
}

//Deprecated methods 
void Matrix::copy( const Matrix& other) {
#ifdef WARN_DEPRECATED
    notify(NOTICE) << "Matrix::copy is deprecated.  Use = instead.";
#endif
    (*this) = other;
}
void Matrix::preScale( float sx, float sy, float sz, const Matrix& m ) {
#ifdef WARN_DEPRECATED
    notify(NOTICE) << "Matrix::preScale is deprecated. Use result = (Matrix::scale * m) instead.";
#endif
    (*this) = ( scale(sx,sy,sz) * m );
}
void Matrix::postScale( const Matrix& m, float sx, float sy, float sz ) {
#ifdef WARN_DEPRECATED
    notify(NOTICE) << "Matrix::postScale is deprecated.  Use result = (m * Matrix::scale()) instead.";
#endif
    (*this) = ( m * scale(sx,sy,sz) );
}
void Matrix::preScale( float sx, float sy, float sz ) {
#ifdef WARN_DEPRECATED
    notify(NOTICE) << "Matrix::preScale is deprecated. Use M.preMult( Matrix::scale ) instead.";
#endif
    preMult( scale(sx,sy,sz) );
}
void Matrix::postScale( float sx, float sy, float sz ) {
#ifdef WARN_DEPRECATED
    notify(NOTICE) << "Matrix::postScale is deprecated.  Use M.postMult( Matrix::scale ) instead.";
#endif
    postMult( scale(sx,sy,sz) );
}
void Matrix::preTrans( float tx, float ty, float tz, const Matrix& m ) {
#ifdef WARN_DEPRECATED
    notify(NOTICE) << "Matrix::preTrans is deprecated.  Use result = Matrix::trans * m instead.";
#endif
    (*this) = trans(tx,ty,tz) * m;
}
void Matrix::postTrans( const Matrix& m, float tx, float ty, float tz ) {
#ifdef WARN_DEPRECATED
    notify(NOTICE) << "Matrix::postTrans is deprecated.  Use result = m * Matrix::trans instead.";
#endif
    (*this) = m * trans(tx,ty,tz);
}
void Matrix::preTrans( float tx, float ty, float tz ) {
#ifdef WARN_DEPRECATED
    notify(NOTICE) << "Matrix::preTrans is deprecated.  Use result = Matrix::trans * m instead.";
#endif
    preMult( trans(tx,ty,tz) );
}
void Matrix::postTrans( float sx, float sy, float sz ) {
#ifdef WARN_DEPRECATED
    notify(NOTICE) << "Matrix::postTrans is deprecated.  Use result = m * Matrix::trans instead.";
#endif
    postMult( trans(sx,sy,sz) );
}
void Matrix::preRot( float deg, float x, float y, float z, const Matrix& m  ) {
#ifdef WARN_DEPRECATED
    notify(NOTICE) << "Matrix::preRot is deprecated.  Use result = Matrix::rot * m instead.";
#endif
    (*this) = rotate(deg,x,y,z) * m;
}
void Matrix::postRot( const Matrix& m, float deg, float x, float y, float z ) {
#ifdef WARN_DEPRECATED
    notify(NOTICE) << "Matrix::postRot is deprecated.  Use result = m * Matrix::rotate instead.";
#endif
    (*this) = m * rotate(deg,x,y,z);
}
void Matrix::preRot( float deg, float x, float y, float z ) {
#ifdef WARN_DEPRECATED
    notify(NOTICE) << "Matrix::preRot is deprecated.  Use m.preMult( Matrix::rotate ) instead.";
#endif
    preMult( rotate(deg,x,y,z) );
}
void Matrix::postRot( float deg, float x, float y, float z ) {
#ifdef WARN_DEPRECATED
    notify(NOTICE) << "Matrix::postRot is deprecated.  Use m.postMult( Matrix::rotate ) instead.";
#endif
    postMult( rotate(deg,x,y,z) );
}