/*
 *  SOBIPlugin
 *
 *  Author: Andrew Salamon
 *
 *  Copyright (c) 2008 Machine Perception Laboratory
 *  University of California San Diego.
 *
 * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
 *
 *    1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
 *    2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution.
 *    3. The name of the author may not be used to endorse or promote products derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 */

#include "SOBIPlugin.h"

#include <sstream>

#include <hipersat/NicMatrix.h>
#include <hipersat/NicVector.h>
#include <hipersat/Sobi.h>
#include <hipersat/version.h>
#include <hipersat/sphering_matrix.h>
#include <hipersat/center_data.h>
#include <hipersat/average.h>
#include <hipersat/MatrixOperations.h>
#include <hipersat/RangeGenerator.h>
#include <hipersat/MatrixWriter.h>
#include <hipersat/DataFormat.h>
#include <hipersat/CERT_PseudoSqrt.h>


#include "PluginController.h"
#include "PluginBase.h"
#include "SVMPlugin.h"
#include "AfterFacePlugin.h"
#include "FacePlugin.h"
#include "FeaturePlugin.h"

#include "CERT_Config.h"
#include "CERT_Arguments.h"

#include "mp_CERT.h"

namespace CERT
{

SOBIPlugin::SOBIPlugin( ConfigItem *_config ) : AfterFacePlugin(_config) // , storage(18)
{
}

SOBIPlugin::~SOBIPlugin()
{
    if( isEnabled() )
        outputResults();
}

PluginBase *
SOBIPlugin::copy()
{
    return new SOBIPlugin(*this);
}

std::vector<std::string>
SOBIPlugin::getLabels() const
{
    return std::vector<std::string>();
}

void
SOBIPlugin::processImageUsingCert( RImage<float> &image, MP_CERT &cert )
{
    PluginController *pc = &cert.pluginController();

    if( !cert.didFindFace() )
        return;

    // These plugin id's should be in the config file
    PluginBase *auPlugin = pc->pluginWithID( "edu.ucsd.mplab.plugins.CERT4_0_Weights" );
    PluginBase *mlrPlugin = pc->pluginWithID( "edu.ucsd.mplab.plugins.MLR2_Weights" );
    if( auPlugin && mlrPlugin )
    {
        PluginResultsType auRes = auPlugin->getResults();
//#define ROW_MAJOR
#ifdef ROW_MAJOR
        unsigned int ind = 0;
        for( ind = 0; ind < auRes.size(); ++ind )
        {
            storage[ind].push_back( auRes[ind] );
        }
        PluginResultsType mlrRes = mlrPlugin->getResults();
        for( unsigned int mlrInd = 0; mlrInd < mlrRes.size(); ++mlrInd, ++ind )
        {
            storage[ind].push_back( mlrRes[mlrInd] );
        }
#else
        PluginResultsType mlrRes = mlrPlugin->getResults();
        for( unsigned int mlrInd = 0; mlrInd < mlrRes.size(); ++mlrInd )
        {
            auRes.push_back( mlrRes[mlrInd] );
        }
        storage.push_back( auRes );
#endif
    }

}

bool
SOBIPlugin::areResultsValid()
{
    return false;
}

double
SOBIPlugin::getResultsDouble()
{
    return 0.0;
}

void
centerAndSphere( NicMatrix<double> *spheringMatrix, NicMatrix<double> &inputData )
{
    // center the data
//    {
//        NicVector<double> avg( rows );
//        average( &inputData, &avg );
//        center_data( &inputData, &avg );
//        MatrixWriter<double>::writeMatrix( inputData, DataFormat::TEXT, "/tmp/CERT_Centered.txt" );
//    }


    if( spheringMatrix )
    {
        // compute the sphering matrix. Is this the same as whitening?
    //    computeSpheringMatrix( &inputData, spheringMatrix, true );
    //    MatrixWriter<double>::writeMatrix( *spheringMatrix, DataFormat::TEXT, "/tmp/CERT_SpheringMatrix.txt" );
        spheringMatrix->identity_matrix();

        // I need to go through a temp matrix otherwise we stomp on memory somehow.
        NicMatrix<double> temp( inputData.rows, inputData.columns );
        multiply( *spheringMatrix, inputData, temp );
        inputData = temp;
    //    MatrixWriter<double>::writeMatrix( temp, DataFormat::TEXT, "/tmp/CERT_Temp.txt" );
    //    multiply( *spheringMatrix, inputData );
    }
}

static void sqrtm( NicMatrix<double> &Rx, NicMatrix<double> &root )
{
#ifdef WIN32
#pragma message ("sqrtm (principal square root of a matrix) is not yet defined.")
#else
#warning "sqrtm (principal square root of a matrix) is not yet defined."
#endif
//    Yk+1 = (Yk + Zk-1) / 2, Y0 = A,
//    Zk+1 = (Zk + Yk-1) / 2, Z = I

//    NicMatrix<double> Y( Rx);
//    NicMatrix<double> Z( Rx.rows, Rx.columns );
//    NicMatrix<double> temp( Rx.rows, Rx.columns );
//    NicMatrix<double> temp2( Rx.rows, Rx.columns );
//
//    Z.identity_matrix();
//
//    for( int cnt = 5; cnt >= 0; --cnt )
//    {
//        SOBIPlugin::matrixAdd( Y, Z, temp, 0.5 ); // divide by 2
//        SOBIPlugin::matrixAdd( Z, Y, temp2, 0.5 ); // divide by 2
//        Y = temp;
//        Z = temp2;
//    }
////    root = Rx;
//    root = Y;

    pseudoSqrt( Rx, root /*, SalvagingAlgorithm::Spectral */ );
}

void
SOBIPlugin::outputResults()
{
#ifdef ROW_MAJOR
    unsigned int cols = storage[0].size();
    unsigned int rows = storage.size();
#else
    unsigned int rows = storage[0].size();
    unsigned int cols = storage.size();
#endif

    NicMatrix<double> inputData( rows, cols );

    for( unsigned int col = 0; col < cols; ++col )
    {
        NicVector<double> oneCol;
        std::vector<double> &oneColData( const_cast<std::vector<double> &>(storage[col] ) );
        double *colData = &oneColData.front();
        oneCol.externData( colData, storage[col].size() );
        inputData.add_column( &oneCol, col );
    }

    MatrixWriter<double>::writeMatrix( inputData, DataFormat::TEXT, "/tmp/CERT_1_InputData.txt", true );

    NicMatrix<double> *spheringMatrix = NULL;
//    spheringMatrix = new NicMatrix<double>( rows, rows );
    centerAndSphere( spheringMatrix, inputData );
//    MatrixWriter<double>::writeMatrix( inputData, DataFormat::TEXT, "/tmp/CERT_2_ InputAfterSphering.txt" );

    // setup for Sobi computation
    int* tau = 0;
    int tauLength = 0;
//    {
//        tauLength = 4;
//        tau = new int[ tauLength ];
//        for ( int i = 0; i < tauLength; ++i )
//        {
//            tau[ i ] = ( i+1 );
//        }
//    }
    if ( true )
    {
        vector<int> vTau(0);
//        stringstream sTau;
//        sTau << "0:" << cols << ":" << 4*cols;
        string sTau = "1:1:4"; // "0:<cols>:<4*cols>";
        RangeGenerator<int> range( sTau );
        range.getRange( vTau );
        tau = makeTau( vTau );
        tauLength = vTau.size();
    }

    NicMatrix<double> W( rows, rows );
    NicMatrix<double> A( rows, rows );
    NicMatrix<double> G( rows, rows );

    double tolerance = 1e-6;

    computeSobi( inputData, tau, tauLength, G, tolerance ); // weight matrix

    if ( tau != 0 ) delete [] tau;

    if ( spheringMatrix != 0 )
    {
        multiply( G, *spheringMatrix, W ); // unmixing matrix
    }
    else
    {
        W = G;
    }
    if( !invert<double>( W, A ) )
        std::cerr << "Unable to invert matrix W." << std::endl;

    if ( spheringMatrix != 0 ) delete spheringMatrix;

    // Somehow output W (mixing matrix) and ? (signal separation)
    MatrixWriter<double>::writeMatrix( W, DataFormat::TEXT, "/tmp/CERT_3_MixingMatrix.txt" );
    MatrixWriter<double>::writeMatrix( A, DataFormat::TEXT, "/tmp/CERT_4_Inverted.txt" );
    MatrixWriter<double>::writeMatrix( G, DataFormat::TEXT, "/tmp/CERT_5_G.txt" );

    // Start Claudia's pre/post processing here
    NicMatrix<double> Rx( rows, rows );
    multiply( inputData, inputData, Rx, false, true ); // Rx=(X*X')
    // This is the '/ T' portion
    for ( int i = 0; i < Rx.columns; ++i )
    {
        for ( int j = 0; j < Rx.rows; ++j )
        {
            Rx( j, i ) /= inputData.columns;
        }
    }

    NicMatrix<double> IBL( rows, rows );
    MatrixWriter<double>::writeMatrix( Rx, DataFormat::TEXT, "/tmp/CERT_sqrtm_before.txt" );
    sqrtm( Rx, IBL ); // should be: IBL=sqrtm(Rx);
    MatrixWriter<double>::writeMatrix( IBL, DataFormat::TEXT, "/tmp/CERT_sqrtm_after.txt" );

//load eegex
//X=eegex';
//[n,T]=size(X);
//Rx=(X*X')/T;
//IBL=sqrtm(Rx);
//W=sobi_hipers(X,4);
//V=inv(IBL)*W';
//SS=V'*X;

    NicMatrix<double> IBLinv( rows, rows );
    NicMatrix<double> V( rows, rows );

    if( !invert<double>( IBL, IBLinv ) )
        std::cerr << "Unable to invert matrix IBL." << std::endl;

    multiply( IBLinv, W, V, false, true ); // V=inv(IBL)*W';

    // S should equal W' * inputData (after sphering)
    NicMatrix<double> S( rows, cols );
    multiply( V, inputData, S, true, false );
    MatrixWriter<double>::writeMatrix( S, DataFormat::TEXT, "/tmp/CERT_6_S.txt" );

    mout() << "SOBI:\t" << std::endl;
//    printResults( storage[0], true );
//    mout() << std::endl;
}

std::ostream &
SOBIPlugin::mout()
{
    return CERT::Arguments::outputStream();
}

void
SOBIPlugin::matrixAdd( NicMatrix<double>& A, NicMatrix<double>& B, NicMatrix<double>& C, double mult,
                            bool transposeA, bool transposeB )
{
    int aRows = transposeA ? A.columns : A.rows;
    int aColumns = transposeA ? A.rows : A.columns;
    int bRows = transposeB ? B.columns : B.rows;
    int bColumns = transposeB ? B.rows : B.columns;

    if( (aRows != bRows) || (aColumns != bColumns) )
    {
        std::cerr << "Matrix dimensions must agree during add.";
        return;
    }

    for ( int aRow = 0; aRow < aRows; ++aRow )
    {
        for ( int aColumn = 0; aColumn < aColumns; ++aColumn )
        {
            double valA = transposeA ? A( aColumn, aRow ) : A(aRow, aColumn );
            double valB = transposeB ? B( aColumn, aRow ):  B(aRow, aColumn );
            C( aRow, aColumn ) = (valA + valB) * mult;
        }
    }
}

} // end namespace CERT