/** \file
 *
 *  Contains the Matrix6x6 class implementation.
 *
 *  Copyright (c) 2007,2008,2009 MBARI
 *  MBARI Proprietary Information.  All Rights Reserved
 *
 */

#include "Matrix6x6.h"

#include <math.h>

/// Empty constructior
Matrix6x6::Matrix6x6( )
//: DataValue( 36 * sizeof ( double ), MATRIX6X6 )
{
    value2d_[ 0 ] = value_;
    value2d_[ 1 ] = value_ + 6;
    value2d_[ 2 ] = value_ + 12;
    value2d_[ 3 ] = value_ + 18 ;
    value2d_[ 4 ] = value_ + 24;
    value2d_[ 5 ] = value_ + 30;
    *this = 0.0;
}

/// Single Value constructior
Matrix6x6::Matrix6x6( const double& init )
//: DataValue( 36 * sizeof ( double ), MATRIX6X6 )
{
    value2d_[ 0 ] = value_;
    value2d_[ 1 ] = value_ + 6;
    value2d_[ 2 ] = value_ + 12;
    value2d_[ 3 ] = value_ + 18 ;
    value2d_[ 4 ] = value_ + 24;
    value2d_[ 5 ] = value_ + 30;
    *this = init;
}

/// 4x Matrix3x3 constructior
Matrix6x6::Matrix6x6( Matrix3x3& m3x3i11, Matrix3x3& m3x3i12,
                      Matrix3x3& m3x3i21, Matrix3x3& m3x3i22 )
{
    value2d_[ 0 ] = value_;
    value2d_[ 1 ] = value_ + 6;
    value2d_[ 2 ] = value_ + 12;
    value2d_[ 3 ] = value_ + 18 ;
    value2d_[ 4 ] = value_ + 24;
    value2d_[ 5 ] = value_ + 30;
    set( 0, 0, m3x3i11 );
    set( 0, 3, m3x3i12 );
    set( 3, 0, m3x3i21 );
    set( 3, 3, m3x3i22 );
}

/// Coordinate transformation constructor
Matrix6x6::Matrix6x6( Point6D pos )
{
    value2d_[ 0 ] = value_;
    value2d_[ 1 ] = value_ + 6;
    value2d_[ 2 ] = value_ + 12;
    value2d_[ 3 ] = value_ + 18 ;
    value2d_[ 4 ] = value_ + 24;
    value2d_[ 5 ] = value_ + 30;
    float sinRol = sin( pos.getRoll() );
    float cosRol = cos( pos.getRoll() );
    float sinPch = sin( pos.getPitch() );
    float cosPch = cos( pos.getPitch() );
    float sinHdg = sin( pos.getHeading() );
    float cosHdg = cos( pos.getHeading() );
    float tanPch = tan( pos.getPitch() );
    float sinPchCosHdg = sinPch * cosHdg;
    float sinPchSinHdg = sinPch * sinHdg;

    // Top left 3x3 matrix
    value_[0]  =  cosPch * cosHdg;
    value_[1]  = -cosRol * sinHdg + sinRol * sinPchCosHdg;
    value_[2]  =  sinRol * sinHdg + cosRol * sinPchCosHdg;
    value_[6]  =  cosPch * sinHdg;
    value_[7]  =  cosRol * cosHdg + sinRol * sinPchSinHdg;
    value_[8]  = -sinRol * cosHdg + cosRol * sinPchSinHdg;
    value_[12] = -sinPch;
    value_[13] =  cosPch * sinRol;
    value_[14] =  cosPch * cosRol;

    // Top right 3x3 matrix
    value_[3]  = value_[4] = value_[5] = 0.0;
    value_[9]  = value_[10] = value_[11] = 0.0;
    value_[15] = value_[16] = value_[17] = 0.0;

    // Bottom left 3x3 matrix
    value_[18] = value_[19] = value_[20] = 0.0;
    value_[24] = value_[25] = value_[26] = 0.0;
    value_[30] = value_[31] = value_[32] = 0.0;

    // This 3x3 block relates body attitude
    // rates to Euler angle rates of change
    value_[21] =  1.0;
    value_[22] =  sinRol * tanPch;
    value_[23] =  cosRol * tanPch;
    value_[27] =  0.0;
    value_[28] =  cosRol;
    value_[29] = -sinRol;
    value_[33] =  0.0;
    value_[34] =  sinRol / cosPch;
    value_[35] =  cosRol / cosPch;

}

/// as a Str
Str Matrix6x6::toString() const
{
    Str strm( "[" );
    for( int i = 0; i < 6; ++i )
    {
        if( i > 0 )
        {
            strm += ",";
        }
        strm += "[";
        for( int j = 0; j < 6; ++j )
        {
            if( j > 0 )
            {
                strm += ",";
            }
            strm += Str( value_[ i * 6 + j ] );
        }
        strm += "]";
    }
    return strm + "]";
}

bool Matrix6x6::determinantZero()
{
    bool rowZero;
    bool colZero;
    for( int i = 0; i < 6; ++i )
    {
        rowZero = true;
        colZero = true;
        for( int j = 0; j < 6; ++j )
        {
            rowZero &= value_[ j * 6 + i ] == 0;
            colZero &= value_[ i * 6 + j ] == 0;
            if( !rowZero && !colZero )
            {
                break;
            }
        }
        if( rowZero || colZero )
        {
            return true;
        }
    }
    return false;
}

void Matrix6x6::invert()
{
    for( int i = 1; i < 6; i++ )        // normalize row 0
    {
        value_[ i ] /= value_[ 0 ];
    }
    for( int i = 1; i < 6; i++ )
    {
        for( int j = i; j < 6; j++ )        // do a column of L
        {
            double sum = 0.0;
            for( int k = 0; k < i; k++ )
            {
                sum += value_[ j * 6 + k ] * value_[ k * 6 + i ];
            }
            value_[ j * 6 + i ] -= sum;
        }
        if( i == 5 )
        {
            continue;
        }
        for( int j = i + 1; j < 6; j++ )        // do a row of U
        {
            double sum = 0.0;
            for( int k = 0; k < i; k++ )
            {
                sum += value_[ i * 6 + k ] * value_[ k * 6 + j ];
            }
            value_[ i * 6 + j ] = ( value_[ i * 6 + j ] - sum ) / value_[ i * 6 + i ];
        }
    }
    for( int i = 0; i < 6; i++ )        // invert L
    {
        for( int j = i; j < 6; j++ )
        {
            double getX = 1.0;
            if( i != j )
            {
                getX = 0.0;
                for( int k = i; k < j; k++ )
                {
                    getX -= value_[ j * 6 + k ] * value_[ k * 6 + i ];
                }
            }
            value_[ j * 6 + i ] = getX / value_[ j * 6 + j ];
        }
    }
    for( int i = 0; i < 6; i++ )        // invert U
    {
        for( int j = i; j < 6; j++ )
        {
            if( i == j )
            {
                continue;
            }
            double sum = 0.0;
            for( int k = i; k < j; k++ )
            {
                sum += value_[ k * 6 + j ] * ( ( i == k ) ? 1.0 : value_[ i * 6 + k ] );
            }
            value_[ i * 6 + j ] = -sum;
        }
    }
    for( int i = 0; i < 6; i++ )          // final inversion
    {
        for( int j = 0; j < 6; j++ )
        {
            double sum = 0.0;
            for( int k = ( ( i > j ) ? i : j ); k < 6; k++ )
            {
                sum += ( ( j == k ) ? 1.0 : value_[ j * 6 + k ] ) * value_[ k * 6 + i ];
            }
            value_[ j * 6 + i ] = sum;
        }
    }
};

void Matrix6x6::decomposeLU()
{
    //    int i, j, k, k1;
    //
    //    for ( k = 0; k < 5; ++k )
    //    {
    //        k1 = k + 1;
    //        for ( i = k1; i < 6; ++i )
    //        {
    //            if ( value_[ k * 7 ] != 0.0 )
    //            {
    //                value_[ i * 6 + k ] /= value_[ k * 7 ];
    //                for ( j = k1; j < 6; ++j )
    //                {
    //                    value_[ i * 6 + j ] -= value_[ i * 6 + k ] * value_[ k * 6 + j ];
    //                }
    //            }
    //            // else throw something?
    //        }
    //    }

    for( int k = 0; k < 6; k++ )   //loop over columns of A
    {
        for( int i = 0; i <= k; i++ )
        {
            //loop over elements of U matrix in column k
            for( int m = 0; m < i; m++ )
            {
                value_[ i * 6 + k ] = value_[ i * 6 + k ] - value_[ i * 6 + m ] * value_[ m * 6 + k ];
            }
        }
        for( int i = k + 1; i < 6; i++ )
        {
            //loop over elements of U matrix in column k
            for( int m = 0; m < k; m++ )
            {
                value_[ i * 6 + k ] = value_[ i * 6 + k ] - value_[ i * 6 + m ] * value_[ m * 6 + k ];
            }
            value_[ i * 6 + k ] = value_[ i * 6 + k ] / value_[ k * 7 ];
        }
    }
};

// Solve for getX in Ax = b, where this is the LU decompostion of
// M and b is supplied here.
void Matrix6x6::solveLU( Point6D& b, Point6D& getX ) const
{
    double getY[ 6 ];
    int i, j;

    /*
      solve Ly = b by forward substitution
    */
    for( i = 0; i < 6; ++i )
    {
        getY[ i ] = b[ i ];
        for( j = 0; j <= i - 1; ++j )
        {
            getY[ i ] -= value_[ i * 6 + j ] * getY[ j ];
        }
    }
    /*
      solve Ux = getY by backward substitution
    */
    for( i = 5; i >= 0; --i )
    {
        getX[ i ] = getY[ i ];
        for( j = 5; j >= i + 1; --j )
        {
            getX[ i ] -= value_[ i * 6 + j ] * getX[ j ];
        }
        getX[ i ] /= value_[ i * 6 + i ];
    }

}

bool Matrix6x6::operator==( const Matrix6x6 & rhs )
{
    for( int i = 0; i < 36; ++i )
    {
        if( value_[ i ] != rhs.value_[ i ] )
        {
            return false;
        }
    }
    return true;
}

Matrix6x6& Matrix6x6::operator=( const Matrix6x6 & rhs )
{
    for( int i = 0; i < 36; ++i )
    {
        value_[ i ] = rhs.value_[ i ];
    }
    return *this;
}

Matrix6x6& Matrix6x6::operator=( const double rhs )
{
    for( int i = 0; i < 36; ++i )
    {
        value_[ i ] = rhs;
    }
    return *this;
}

void Matrix6x6::set( int index1, int index2, const double value )
{
    if( index1 < 0 || index1 > 5 )
    {
        index1 = 0;
    }
    if( index2 < 0 || index2 > 5 )
    {
        index2 = 0;
    }
    value_[ index1 * 6 + index2 ] = value;
}

void Matrix6x6::set( int index1, int index2, Matrix3x3& value )
{
    if( index1 < 0 || index1 > 3 )
    {
        index1 = 0;
    }
    if( index2 < 0 || index2 > 3 )
    {
        index2 = 0;
    }
    for( int i = index1; i < index1 + 3; ++i )
    {
        for( int j = index2; j < index2 + 3; ++j )
        {
            value_[i * 6 + j] = value( i - index1, j - index2 );
        }
    }
}

/// 36 value set
void Matrix6x6::set( const double& v00, const double& v01, const double& v02, const double& v03, const double& v04, const double& v05,
                     const double& v10, const double& v11, const double& v12, const double& v13, const double& v14, const double& v15,
                     const double& v20, const double& v21, const double& v22, const double& v23, const double& v24, const double& v25,
                     const double& v30, const double& v31, const double& v32, const double& v33, const double& v34, const double& v35,
                     const double& v40, const double& v41, const double& v42, const double& v43, const double& v44, const double& v45,
                     const double& v50, const double& v51, const double& v52, const double& v53, const double& v54, const double& v55 )
{
    value_[ 0 ] = v00;
    value_[ 1 ] = v01;
    value_[ 2 ] = v02;
    value_[ 3 ] = v03;
    value_[ 4 ] = v04;
    value_[ 5 ] = v05;
    value_[ 6 ] = v10;
    value_[ 7 ] = v11;
    value_[ 8 ] = v12;
    value_[ 9 ] = v13;
    value_[ 10 ] = v14;
    value_[ 11 ] = v15;
    value_[ 12 ] = v20;
    value_[ 13 ] = v21;
    value_[ 14 ] = v22;
    value_[ 15 ] = v23;
    value_[ 16 ] = v24;
    value_[ 17 ] = v25;
    value_[ 18 ] = v30;
    value_[ 19 ] = v31;
    value_[ 20 ] = v32;
    value_[ 21 ] = v33;
    value_[ 22 ] = v34;
    value_[ 23 ] = v35;
    value_[ 24 ] = v40;
    value_[ 25 ] = v41;
    value_[ 26 ] = v42;
    value_[ 27 ] = v43;
    value_[ 28 ] = v44;
    value_[ 29 ] = v45;
    value_[ 30 ] = v50;
    value_[ 31 ] = v51;
    value_[ 32 ] = v52;
    value_[ 33 ] = v53;
    value_[ 34 ] = v54;
    value_[ 35 ] = v55;

}

double& Matrix6x6::operator()( int index1, int index2 )
{
    if( index1 < 0 || index1 > 5 )
    {
        index1 = 0;
    }
    if( index2 < 0 || index2 > 5 )
    {
        index2 = 0;
    }
    return value_[ index1 * 6 + index2 ];
}