#ifdef NICE_USELIB_MEX
/** 
* @author Johannes Ruehle
* @date 25-04-2014
* @brief Matlab-Interface for the Extremely randomized clustering forest ERC
*/

// STL includes
#include <math.h>
#include <matrix.h>
#include <mex.h>

// NICE-core includes
#include <core/basics/Config.h>
#include <core/basics/Timer.h>
#include <core/vector/MatrixT.h>
#include <core/vector/VectorT.h>

// CodebookRandomForest stuff
#include "vislearning/features/simplefeatures/CodebookRandomForest.h"

#include "vislearning/features/fpfeatures/VectorFeature.h"

// Interface for conversion between Matlab and C objects
#include "gp-hik-core/matlab/classHandleMtoC.h"
#include "gp-hik-core/matlab/ConverterMatlabToNICE.h"
#include "gp-hik-core/matlab/ConverterNICEToMatlab.h"

using namespace std; //C basics
using namespace NICE;  // nice-core

#define DEBUG_VERBOSE

NICE::Config parseParametersERC(const mxArray *prhs[], int nrhs)
{
  NICE::Config conf;

  // Check parameters
  if ( nrhs % 2 == 1 )
  {
      mexErrMsgTxt("parseParametersERC: uneven number of config arguments.");
  }

  // now run over all given parameter specifications
  // and add them to the config
  for( int i=0; i < nrhs; i+=2 )
  {
    std::string variable = MatlabConversion::convertMatlabToString(prhs[i]);
    
    /////////////
    //CodebookRandomForest( int maxDepth

//    number_of_trees = conf->gI(section, "number_of_trees", 20 );
//    features_per_tree = conf->gD(section, "features_per_tree", 1.0 );
//    samples_per_tree  = conf->gD(section, "samples_per_tree", 0.2 );
//    use_simple_balancing = conf->gB(section, "use_simple_balancing", false);
//    weight_examples = conf->gB(section, "weight_examples", false);
//    memory_efficient = conf->gB(section, "memory_efficient", false);

    //std::string builder_section = conf->gS(section, "builder_section", "DTBRandom");

#ifdef DEBUG_VERBOSE
    std::cerr << "config variable: "<< variable << std::endl;
#endif
    if(variable == "conf")
    {
        // if first argument is the filename of an existing config file,
        // read the config accordingly

        conf = NICE::Config ( MatlabConversion::convertMatlabToString( prhs[i+1] )  );
#ifdef DEBUG_VERBOSE
        std::cerr << "conf " << MatlabConversion::convertMatlabToString( prhs[i+1] ) << std::endl;
#endif
    }
    else if( variable == "number_of_trees")
    {
        if ( mxIsInt32( prhs[i+1] ) )
        {
            int value = MatlabConversion::convertMatlabToInt32(prhs[i+1]);
            conf.sI("RandomForest", variable, value);
#ifdef DEBUG_VERBOSE
            std::cerr << "number_of_trees " << value << std::endl;
#endif
        }
        else
        {
            std::string errorMsg = "Unexpected parameter value for \'" +  variable + "\'. Int32 expected.";
            mexErrMsgIdAndTxt( "mexnice:error", errorMsg.c_str() );
        }

    }
    else if( variable == "maxDepthTree")
    {
        if ( mxIsInt32( prhs[i+1] ) )
        {
            int value = MatlabConversion::convertMatlabToInt32(prhs[i+1]);
            conf.sI("CodebookRandomForest", variable, value);
#ifdef DEBUG_VERBOSE
            std::cerr << "maxDepthTree " << value << std::endl;
#endif
        }
        else
        {
            std::string errorMsg = "Unexpected parameter value for \'" +  variable + "\'. Int32 expected.";
            mexErrMsgIdAndTxt( "mexnice:error", errorMsg.c_str() );
        }

    }
    else if( variable == "verbose")
    {
        if ( mxIsLogical( prhs[i+1] ) )
        {
            bool bVerbose = MatlabConversion::convertMatlabToBool(prhs[i+1]);
            conf.sB("CodebookRandomForest", variable, bVerbose);
#ifdef DEBUG_VERBOSE
            std::cerr << "verbose " << bVerbose << std::endl;
#endif
        }
        else
        {
            std::string errorMsg = "Unexpected parameter value for \'" +  variable + "\'. Boolean expected.";
            mexErrMsgIdAndTxt( "mexnice:error", errorMsg.c_str() );
        }

    }

  }

  return conf;
}

// MAIN MATLAB FUNCTION
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{    
#ifdef DEBUG_VERBOSE
    std::cerr << "Verbose Debug Output on (compiled with debug definition)." << std::endl;
#endif

    // get the command string specifying what to do
    if (nrhs < 1)
        mexErrMsgTxt("No commands and options passed... Aborting!");        
    
    if( !mxIsChar( prhs[0] ) )
        mexErrMsgTxt("First argument needs to be the command, ie.e, the class method to call... Aborting!");        
    
    std::string cmd = MatlabConversion::convertMatlabToString( prhs[0] );
      
        
    // in all other cases, there should be a second input,
    // which the be the class instance handle
    if (nrhs < 2)
      mexErrMsgTxt("Second input should be a class instance handle.");
    
    // delete object
    if ( !strcmp("delete", cmd.c_str() ) )
    {
        // Destroy the C++ object
        MatlabConversion::destroyObject<OBJREC::CodebookRandomForest>(prhs[1]);
        return;
    }
    
    ////////////////////////////////////////
    //  Check which class method to call  //
    ////////////////////////////////////////
    
    
    // standard train - assumes initialized object
    if (!strcmp("createAndTrain", cmd.c_str() ))
    {
        // Check parameters
        if (nlhs < 0 || nrhs < 4 )
        {
            mexErrMsgTxt("Train: Unexpected arguments.");
        }
        
        //------------- read the data --------------
        if (nrhs < 4)
        {
            mexErrMsgTxt("needs at least 2 matrix inputs, first the training features, second the sample labels");
            return;
        }

        const mxArray *t_pArrTrainData   = prhs[1];
        const mxArray *t_pArrTrainLabels = prhs[2];


        //----------------- parse config options  -------------
        NICE::Config conf = parseParametersERC(prhs+3, nrhs-3 );

        int iNumFeatureDimension = mxGetM( t_pArrTrainData ); // feature dimensions
#ifdef DEBUG_VERBOSE
        std::cerr << "iNumFeatureDimension " << iNumFeatureDimension << std::endl;
#endif
        //----------------- create examples object -------------
        NICE::Vector t_vecLabelsTrain = MatlabConversion::convertDoubleVectorToNice( t_pArrTrainLabels );
        NICE::Matrix t_matDataTrain   = MatlabConversion::convertDoubleMatrixToNice( t_pArrTrainData   );

        OBJREC::Examples examplesTrain;

        bool bRet = OBJREC::Examples::wrapExamplesAroundFeatureMatrix( t_matDataTrain, t_vecLabelsTrain, examplesTrain );
        if( !bRet )
        {
            mexErrMsgTxt("createAndTrain: Error creating Examples from raw feature matrix and labels.");
        }

        //----------------- create raw feature mapping -------------
        OBJREC::FeaturePool fp;
        OBJREC::VectorFeature *pVecFeature = new OBJREC::VectorFeature(iNumFeatureDimension);
        pVecFeature->explode(fp);

#ifdef DEBUG_VERBOSE
        //----------------- debug features -------------
        OBJREC::Example t_Exp = examplesTrain[0].second;
        NICE::Vector t_FeatVector;
        fp.calcFeatureVector(t_Exp, t_FeatVector);
        std::cerr << "first full Feature Vec: " <<t_FeatVector << std::endl;
#endif
        //----------------- train our random Forest -------------
        OBJREC::FPCRandomForests *pRandForest = new OBJREC::FPCRandomForests(&conf,"RandomForest");
        pRandForest->train(fp, examplesTrain);

        //----------------- create codebook ERC clusterer -------------
        int nMaxDepth        = conf.gI("CodebookRandomForest", "maxDepthTree",10);
        int nMaxCodebookSize = conf.gI("CodebookRandomForest", "maxCodebookSize",100);
#ifdef DEBUG_VERBOSE
        std::cerr << "maxDepthTree " << nMaxDepth << std::endl;
        std::cerr << "nMaxCodebookSize " << nMaxCodebookSize << std::endl;
#endif
        OBJREC::CodebookRandomForest *pCodebookRandomForest = new OBJREC::CodebookRandomForest(pRandForest, nMaxDepth,nMaxCodebookSize);

        // handle to the C++ instance
        plhs[0] = MatlabConversion::convertPtr2Mat<OBJREC::CodebookRandomForest>( pCodebookRandomForest );

        //----------------- clean up -------------

        delete pVecFeature;
        pVecFeature = NULL;
        // delete all "exploded" features, they are internally cloned in the random trees anyway
        fp.destroy();
        //
        examplesTrain.clean();

        return;
    }
    ///// generate Histogram over trees
    else if (!strcmp("generateHistogram", cmd.c_str() ))
    {
        //------------- read the data --------------
        if (nrhs < 3)
        {
            mexErrMsgTxt("needs at least 1 matrix inputs, first the training features");
            return;
        }

        //----------------- convert ptr of trained codebook forest -------------
        OBJREC::CodebookRandomForest *pCodebookRandomForest = MatlabConversion::convertMat2Ptr<OBJREC::CodebookRandomForest>(prhs[1]);
        if( pCodebookRandomForest == NULL )
        {
            mexErrMsgTxt("classify: No valid trained classifier given");
        }

        //----------------- convert matlab data into NICE data -------------
        const mxArray *t_pArrTrainData   = prhs[2];

        NICE::Matrix matDataTrain = MatlabConversion::convertDoubleMatrixToNice( t_pArrTrainData   );
        size_t numTrainSamples      = matDataTrain.cols();
        size_t iNumFeatureDimension = matDataTrain.rows();
        size_t iNumCodewords        = pCodebookRandomForest->getCodebookSize();
#ifdef DEBUG_VERBOSE
        std::cerr << "numTrainSamples "      << numTrainSamples         << std::endl;
        std::cerr << "iNumFeatureDimension " << iNumFeatureDimension    << std::endl;
        std::cerr << "iNumCodewords "        << iNumCodewords           << std::endl;
#endif

        //----------------- parse config options  -------------
        bool bVerboseOutput = false;
        if( nrhs > 3)
        {
            NICE::Config conf = parseParametersERC(prhs+3, nrhs-3 );
            bVerboseOutput = conf.gB("CodebookRandomForest", "verbose", false);
        }

        //----------------- quantize samples into histogram -------------
        NICE::Vector histogram(iNumCodewords, 0.0f);

        const double *pDataPtr = matDataTrain.getDataPointer();
        int t_iCodebookEntry; double t_fWeight; double t_fDistance;
        for (size_t i = 0; i < numTrainSamples; i++, pDataPtr+= iNumFeatureDimension )
        {
            const NICE::Vector t_VecTrainData( pDataPtr , iNumFeatureDimension);
            pCodebookRandomForest->voteVQ(t_VecTrainData, histogram, t_iCodebookEntry, t_fWeight, t_fDistance );
            if(bVerboseOutput)
                std::cerr << i << ": " << "CBEntry " << t_iCodebookEntry << " Weight: " << t_fWeight << " Distance: " << t_fDistance << std::endl;
        }

        //----------------- convert NICE histogram into MATLAB data -------------
        plhs[0] = MatlabConversion::convertVectorFromNice(histogram);

        return;
    }
    ///// get distribution of classes per sample
    else if (!strcmp("calcClassDistributionPerSample", cmd.c_str() ))
    {
        //------------- read the data --------------
        if (nrhs < 3)
        {
            mexErrMsgTxt("needs at least 1 matrix inputs, first the training features");
            return;
        }

        //----------------- convert ptr of trained codebook forest -------------
        OBJREC::CodebookRandomForest *pCodebookRandomForest = MatlabConversion::convertMat2Ptr<OBJREC::CodebookRandomForest>(prhs[1]);
        if( pCodebookRandomForest == NULL )
        {
            mexErrMsgTxt("classify: No valid trained classifier given");
        }

        //----------------- convert matlab data into NICE data -------------
        const mxArray *t_pArrTrainData   = prhs[2];

        NICE::Matrix matData = MatlabConversion::convertDoubleMatrixToNice( t_pArrTrainData   );
        size_t numTrainSamples      = matData.cols();
        size_t iNumFeatureDimension = matData.rows();
#ifdef DEBUG_VERBOSE
        std::cerr << "numTrainSamples "      << numTrainSamples         << std::endl;
        std::cerr << "iNumFeatureDimension " << iNumFeatureDimension    << std::endl;
#endif

        //----------------- parse config options  -------------
        bool bVerboseOutput = false;
        if( nrhs > 3)
        {
            NICE::Config conf = parseParametersERC(prhs+3, nrhs-3 );
            bVerboseOutput = conf.gB("CodebookRandomForest", "verbose", false);
        }

        //----------------- quantize samples into histogram -------------
        const double *pDataPtr = matData.getDataPointer();
        for (size_t i = 0; i < numTrainSamples; i++, pDataPtr+= iNumFeatureDimension )
        {
            NICE::SparseVector votes;
            NICE::Vector distribution;
            const NICE::Vector t_VecTrainData( pDataPtr , iNumFeatureDimension);
            pCodebookRandomForest->voteAndClassify(t_VecTrainData, votes, distribution);
            if(bVerboseOutput)
            {
                NICE::Vector t_fullVector;
                votes.convertToVectorT( t_fullVector );
                std::cerr << i << ": " << "votes " << t_fullVector << " distribution: " << distribution << std::endl;
            }
        }

        //----------------- convert NICE histogram into MATLAB data -------------
        //plhs[0] = MatlabConversion::convertVectorFromNice(histogram);
        plhs[0] =  mxCreateLogicalScalar( true );

        return;
    }
    // store codebook random forest to file
    else if ( strcmp("storeToFile", cmd.c_str()) == 0 )
    {
        //------------- read the data --------------
        if (nrhs != 3)
        {
            mexErrMsgTxt("needs a string for filename to save to");
            return;
        }

        //----------------- convert ptr of trained codebook forest -------------
        OBJREC::CodebookRandomForest *pCodebookRandomForest = MatlabConversion::convertMat2Ptr<OBJREC::CodebookRandomForest>(prhs[1]);
        if( pCodebookRandomForest == NULL )
        {
            mexErrMsgTxt("classify: No valid trained classifier given");
        }

        bool bSuccess = false;

        try
        {
            std::string sStoreFilename = MatlabConversion::convertMatlabToString( prhs[2] );
            std::ofstream ofs;
            ofs.open (sStoreFilename.c_str(), std::ofstream::out);
            pCodebookRandomForest->store( ofs );
            ofs.close();
            bSuccess = true;
        }
        catch( std::exception &e)
        {
            std::cerr << "exception occured: " << e.what() << std::endl;
            mexErrMsgTxt("storing failed");
        }

        plhs[0] =  mxCreateLogicalScalar( bSuccess );

        return;
    }
    // restore codebook random forest from file
    else if (!strcmp("restoreFromFile", cmd.c_str() ))
    {
        //------------- read the data --------------
        if (nrhs != 2)
        {
            mexErrMsgTxt("needs a string for filename to load from");
            return;
        }

        //----------------- convert ptr of trained codebook forest -------------
        OBJREC::CodebookRandomForest *pRestoredCRF = new OBJREC::CodebookRandomForest(-1, -1);

        bool bSuccess = false;

        try
        {
            std::string sStoreFilename = MatlabConversion::convertMatlabToString( prhs[1] );
            std::ifstream ifs;
            ifs.open( sStoreFilename.c_str() );
            pRestoredCRF->restore( ifs );
            ifs.close();
            bSuccess = true;
        }
        catch( std::exception &e)
        {
            std::cerr << "exception occured: " << e.what() << std::endl;
            mexErrMsgTxt("restoring failed");
        }

        // handle to the C++ instance
        if(bSuccess)
            plhs[0] = MatlabConversion::convertPtr2Mat<OBJREC::CodebookRandomForest>( pRestoredCRF );
        else
            plhs[0] = mxCreateLogicalScalar(false);

        return;
    }

    
    // Got here, so command not recognized
    
    std::string errorMsg (cmd.c_str() );
    errorMsg += "--command not recognized.";
    mexErrMsgTxt( errorMsg.c_str() );

}
#endif