/*
 *  mp_CERT
 *
 *  Author: Andrew Salamon
 *  Date: Wed May 24, 2006
 *
 *  Copyright (c) 2006 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 "mp_CERT.h"

#include "mp_rotateFace.h"
#include "visualobject.h"
#include "mp_SVMWeights.h"

extern "C" {
#include "mptVersion.h"
#include "certVersion.h"
}

#include "CERT_Config.h"
#include "CERT_Arguments.h"
#include "AfterFacePlugin.h"
#include "FacePlugin.h"
#include "SVMPlugin.h"
#include "FeaturePlugin.h"

#include "FeatureAveraging.hpp"

static const MP_CERT::certSquare empty;

/** MP_CERT constructor.
 * Sets the directory for finding SVM weights files (defaults to "SVMWeights" in the current working directory).
 * Sets some parameters in both the eyefinder and face rotator.
 */
MP_CERT::MP_CERT( bool multiThreaded ) : patch( 96, 96 ), rotator( 96 ), gabor(NULL), foundFace(false), pc( *CERT::PluginController::getPluginController() ), sequential(false), numWindows(0), inputIsFaces(false)
{
    rotator.setEyeLenRate( 0.48 );
    if( multiThreaded )
        gabor = new mp_threadedGabor( patch, patch.width, patch.height );
    else
        gabor = new MP_Gabor( patch, patch.width, patch.height );
    MP_Gabor::preloadKernels();

    loadFeatureDetector();
    loadSVMWeights();

    clear();

    setSequential( CERT::Arguments::sequential() );
    setMaxTry( CERT::Arguments::maxTries() );
    setInputIsFaces( CERT::Arguments::inputIsFaces() );

    alignMethod = FeatureAlignment::methodFromString( pc.getFaceAlignmentMethod() );
}

void
MP_CERT::loadFeatureDetector()
{
    // clear existing features
    featureDetector.clearFeatures();

    std::vector<CERT::PluginBase *> featurePlugins = pc.pluginsForStep( CERT::PluginSteps::Feature );

    for( std::vector<CERT::PluginBase *>::iterator pluginIter = featurePlugins.begin(), lastPlugin = featurePlugins.end();
        pluginIter != lastPlugin;
        ++pluginIter )
    {
        CERT::FeaturePlugin *plugin = dynamic_cast<CERT::FeaturePlugin *>(*pluginIter);
        if( plugin )
        {
            FeatureData *fd = (FeatureData *)plugin->featuredata();
            Gprior *gp = (Gprior *)plugin->gprior();
            if( (NULL != fd) && (NULL != gp) )
            {
                featureDetector.addFeatureAndGprior( *fd, *gp );
//                featureDetector.loadFeatureFromFiles( plugin->getFeatureFile().c_str(), plugin->getGpriorFile().c_str() );
            }
            else
            {
                // Check to see if it provides an OWFProcessorBase. If so, install it in the featureDetector
                mpt::OWF::ProcessorBase *proc = plugin->getProcessor();
                if( proc )
                    featureDetector.addProcessor( proc );
            }
        }
    }

    featureDetector.setUseSequence( true );
    featureDetector.setCERT3_3_FaceBox();
    featureDetector.setMaxTry(3);

    if( pc.hasStringSetting( "fdproc" ) && (pc.getStringSetting( "fdproc" ) == "average") )
    {
        FeatureAlignment::FeatureAveraging::featureList list;
        std::vector< std::string > flist;

        flist.push_back( "left_eye_nasal" );
        flist.push_back( "left_eye_temporal" );
        list.push_back( FeatureAlignment::FeatureAveraging::featureListPair( "left_eye", flist ) );

        flist.clear();
        flist.push_back( "right_eye_nasal" );
        flist.push_back( "right_eye_temporal" );
        list.push_back( FeatureAlignment::FeatureAveraging::featureListPair( "right_eye", flist ) );

        flist.clear();
        flist.push_back( "mouth_left_corner" );
        flist.push_back( "mouth_right_corner" );
        list.push_back( FeatureAlignment::FeatureAveraging::featureListPair( "mouth", flist ) );

        FeatureAlignment::FeatureAveraging *avger = new FeatureAlignment::FeatureAveraging( list );
        featureDetector.setFeatureProcessor( avger );
    }
    else if( pc.hasMapSetting( "fdproc" ) )
    {
        FeatureAlignment::FeatureAveraging::featureList list;
        CERT::ConfigItem::PropertyType config = pc.getMapSetting( "fdproc" );

        for( CERT::ConfigItem::PropertyType::iterator iter = config.begin(), end = config.end();
             iter != end;
             ++iter )
        {
            // It might be a good idea to walk through all the features in iter->second and make sure they're enabled.
            // If not, give the user a warning.
            // It would also be good to make sure iter->first is valid, but I'm not sure how to do that.
            list.push_back( FeatureAlignment::FeatureAveraging::featureListPair( iter->first, iter->second ) );
        }

        FeatureAlignment::FeatureAveraging *avger = new FeatureAlignment::FeatureAveraging( list );
        featureDetector.setFeatureProcessor( avger );
    }

    if( pc.hasIntSetting( "minscale" ) )
        featureDetector.setMinScale( pc.getIntSetting( "minscale" ) );
    if( pc.hasIntSetting( "maxscale" ) )
        featureDetector.setMaxScale( pc.getIntSetting( "maxscale" ) );
}

void
MP_CERT::loadSVMWeights()
{
    // clear existing SVM plugins
    gabor->clearCategories();

    std::vector<CERT::PluginBase *> svmPlugins = pc.pluginsForStep( CERT::PluginSteps::SVM );

    for( std::vector<CERT::PluginBase *>::iterator pluginIter = svmPlugins.begin(), lastPlugin = svmPlugins.end();
        pluginIter != lastPlugin;
        ++pluginIter )
    {
        CERT::SVMPlugin *plugin = dynamic_cast<CERT::SVMPlugin *>(*pluginIter);
        if( plugin )
        {
            if( !addCategoryFromDirectory( plugin->internalName(), plugin->getWeightsDirectory() ) )
                std::cerr << "Error loading " << plugin->name() << " weights files from "<< plugin->getWeightsDirectory() << "." << std::endl;
        }
    }
}

MP_CERT::~MP_CERT()
{
    if( gabor )
        delete gabor;
}

void MP_CERT::clear()
{
    foundFace = false;
    gabor->clearYhat();
    clearSVMPlugins();
}

void
MP_CERT::processTask( CERT::Task &task )
{
    calcAUs( task );

    // Collect all of the plugin results into the task object.
//    collectResults();

//    doResultsPlugins( task.rimage ); // temporary for testing
}

/** Main entry point.
 * -# Finds one face. Will use the largest found face if there are more than one.
 * -# Aligns the eyes (rotates chosen face) and crops face patch.
 * -# Calculates and returns AU values.
 * -# Runs any 'after face' plugins.
 * /param[in] rimage
 */
void
MP_CERT::calcAUs( CERT::Task &task )
{
    clear();

    // This should make sure that we only run enabled svm plugins.
    loadSVMWeights();
    loadFeatureDetector();

    if( inputIsFaces )
    {
        ObjectWithFeatures blankOWF;
        foundFace = true;
        calcAUsFromFace( task.rimage );
        doAfterFacePlugins( task, blankOWF );
    }
    else
    {
        findFaces( task.rimage );

        // make a copy of the integral image for the after faces plugins that need it.
        RIntegral<float> *pfIntegral = static_cast<RIntegral<float>*>(featureDetector.getIntegralPtr().get());
        task.integral = *pfIntegral;

        if( foundFace )
        {
            // There are still some bugs in the code for running multiple faces per image, so for now just run the largest face.
//            OWFList allFaces = featureDetector.allFaces();
            OWFList allFaces;
            allFaces.push_back( *(featureDetector.getLargestFace()) );
            for( OWFList::iterator faceIter = allFaces.begin(), lastFace = allFaces.end();
                faceIter != lastFace;
                ++faceIter )
            {
                task.addFace( *faceIter );
                task.didFindFace = true;

                if( getPatchWithFeatures( task.rimage, task.currentFace().patch, *faceIter ) )
                {
                    std::vector<float> croppedPoints;
                    std::vector<double> doublePoints;
                    CFourCoordinates<float> points = rotator.getFaceBox();
                    points.vGetFourCoordinates( croppedPoints );
                    for( unsigned int i=0; i < croppedPoints.size(); ++i )
                        doublePoints.push_back( croppedPoints[i] );
                    task.currentFace().addResultsForPlugin( CERT::Task::croppedFaceID, doublePoints );
                    calcAUsFromFace( task.currentFace().patch );
                }

                doAfterFacePlugins( task , *faceIter );

                collectResults( task );

                clearSVMPlugins();
            }
        }
        else
        {
            clearSVMPlugins();
        }

    }
}

void
MP_CERT::processNoFace( CERT::Task &task )
{
    clear();

    // Always run Results plugins. They will need to check on their own to see if a face was found.
    doResultsPlugins( task );
}

void
MP_CERT::findFaces( RImage<float> &rimage )
{
    numWindows = featureDetector.findAllFaces( rimage, sequential );

    ObjectWithFeatures *largestObject = featureDetector.getLargestFace();

    if( largestObject )
    {
        foundFace = true;

//        face.x = largestObject->x;
//        face.y = largestObject->y;
//        face.width = largestObject->xSize;
//        face.height = largestObject->ySize;
//        face.scale = largestObject->scale;
    }
}

void
MP_CERT::doAfterFacePlugins( CERT::Task &task, ObjectWithFeatures &oneObject )
{
    // Most or all of these plugins (features, afterFaces and FacePlugin) need the featuredetector
    // to have been run on the image, so we don't call them if the input is already cropped faces.
    if( !inputIsFaces )
    {
        std::vector< CERT::PluginBase *> afterFacePlugins = pc.pluginsForStep( CERT::PluginSteps::afterFaces );
        for( std::vector<CERT::PluginBase *>::iterator pluginIter = afterFacePlugins.begin(), lastPlugin = afterFacePlugins.end();
            pluginIter != lastPlugin;
            ++pluginIter )
        {
            CERT::AfterFacePlugin *plugin = dynamic_cast<CERT::AfterFacePlugin *>(*pluginIter);
            if( plugin )
                plugin->processTask( task );
        }

//        CERT::FacePlugin *facePlugin = pc.getFaceDetector();
//        if( facePlugin )
//            facePlugin->processImageUsingCert( rimage, *this );

        // Give the feature detector plugins a chance to grab their results
        std::vector<CERT::PluginBase *> featurePlugins = pc.pluginsForStep( CERT::PluginSteps::Feature );
        for( std::vector<CERT::PluginBase *>::iterator pluginIter = featurePlugins.begin(), lastPlugin = featurePlugins.end();
            pluginIter != lastPlugin;
            ++pluginIter )
        {
            CERT::FeaturePlugin *plugin = dynamic_cast<CERT::FeaturePlugin *>(*pluginIter);
            if( plugin )
            {
                plugin->resultsFromObject( &oneObject );
            }
        }
    }
}

void
MP_CERT::collectResults( CERT::Task &task )
{
    task.didFindFace = foundFace;

//    if( !inputIsFaces )
    {
        std::vector<std::string> steps;
        steps.push_back( CERT::PluginSteps::SVM );
        steps.push_back( CERT::PluginSteps::afterFaces );
        steps.push_back( CERT::PluginSteps::Feature );

        std::vector<CERT::PluginBase *> plugins = pc.pluginsForSteps( steps );
        for( std::vector<CERT::PluginBase *>::iterator pluginIter = plugins.begin(), lastPlugin = plugins.end();
             pluginIter != lastPlugin;
             ++pluginIter )
        {
            if( (*pluginIter)->areResultsValid() )
            {
                if( CERT::PluginSteps::Feature == (*pluginIter)->step() )
                {
                    CERT::FeaturePlugin *plugin = dynamic_cast<CERT::FeaturePlugin *>(*pluginIter);
                    std::vector<std::string> labels = plugin->getLabels();
                    if( labels.size() > 0 )
                    {
                        for( std::vector<std::string>::iterator iter = labels.begin(), last = labels.end();
                             iter != last;
                             ++iter )
                        {
                            std::vector<double> feat = plugin->getResultsForLabel( *iter );
                            task.addResultsForPluginAndLabel( plugin->id(), *iter, feat );
                        }
                    }
                    else
                    {
                        task.addResultsForPlugin( plugin->id(), plugin->getResults() );
                    }
                }
                else
                {
                    task.addResultsForPlugin( (*pluginIter)->id(), (*pluginIter)->getResults() );
                }
            }
        }

        CERT::FacePlugin *facePlugin = pc.getFaceDetector();
        if( facePlugin && facePlugin->areResultsValid() )
        {
//            task.addResultsForPlugin( facePlugin->id(), facePlugin->getResults() );
            facePlugin->processTask( task );
        }
    }
}

/** Run all PostProcess and Output type plugins.
 */
void
MP_CERT::doResultsPlugins( CERT::Task &task )
{
    std::vector< CERT::PluginBase *> postPlugins = pc.pluginsForStep( CERT::PluginSteps::PostProcess );
    for( std::vector<CERT::PluginBase *>::iterator pluginIter = postPlugins.begin(), lastPlugin = postPlugins.end();
        pluginIter != lastPlugin;
        ++pluginIter )
    {
        CERT::AfterFacePlugin *plugin = dynamic_cast<CERT::AfterFacePlugin *>(*pluginIter);
        if( plugin )
            plugin->processTask( task );
    }

    postPlugins.clear();
    postPlugins = pc.pluginsForStep( CERT::PluginSteps::Output );
    for( std::vector<CERT::PluginBase *>::iterator pluginIter = postPlugins.begin(), lastPlugin = postPlugins.end();
        pluginIter != lastPlugin;
        ++pluginIter )
    {
        CERT::AfterFacePlugin *plugin = dynamic_cast<CERT::AfterFacePlugin *>(*pluginIter);
        if( plugin )
            plugin->processTask( task );
    }
}

std::map< std::string, mp_AUResults >
MP_CERT::getFrequencies()
{
    return gabor->getFrequencies();
}

bool
MP_CERT::didFindFace()
{
    return foundFace;
}

/** Secondary entry point.
 * Assumes that a face has already been found and the image has been cropped and rotated.
 * Calculates and returns AU values.
 * /param[in] rimage with a cropped and aligned face.
 */
void
MP_CERT::calcAUsFromFace( RImage<float> &facePatch )
{
    gabor->setPixels( facePatch );
    gabor->CERT_Gabor();

    std::vector< CERT::PluginBase *> SVMPlugins = pc.pluginsForStep( CERT::PluginSteps::SVM );

    // Update all SVM Plugins with their results and let them do any post processing (e.g. softmax)
    for( std::vector<CERT::PluginBase *>::iterator pluginIter = SVMPlugins.begin(), lastPlugin = SVMPlugins.end();
        pluginIter != lastPlugin;
        ++pluginIter )
    {
        CERT::SVMPlugin *plugin = dynamic_cast<CERT::SVMPlugin *>(*pluginIter);
        if( plugin )
            plugin->postProcess( resultsForCategory( plugin->internalName() ) );
    }
}

/** Clear previous SVM plugin results if no face is found.
 */
void
MP_CERT::clearSVMPlugins()
{
    std::vector<std::string> steps;
    steps.push_back( CERT::PluginSteps::SVM );
    steps.push_back( CERT::PluginSteps::afterFaces );

    // Clear previous results for all SVM and After Face plugins
    std::vector<CERT::PluginBase *> plugins = pc.pluginsForSteps( steps );

    std::for_each( plugins.begin(), plugins.end(), CERT::Plugins::Clear() );
}

bool
MP_CERT::addCategoryFromDirectory( std::string cat, const std::string &dir )
{
    mp_SVMWeights *catWeights = mp_SVMWeights::getSVMWeights( cat );
    catWeights->setDirectory( dir );
    catWeights->loadAllWeights(); // Preload all weights for this category

    bool error = catWeights->getLoadError();
    if( !error )
        gabor->addCategory( cat );

    return !error;
}

std::vector<double>
MP_CERT::resultsForCategory( const std::string &cat )
{
    return gabor->resultsForCategory( cat );
}

void
MP_CERT::setFaceEstimate( int x, int y, int size, int scale )
{
    Square face( size, x, y, scale );
    featureDetector.setTrackedFace( face );
}

bool
MP_CERT::getPatchWithFeatures( RImage<float> &rimage, RImage<float> &lpatch, ObjectWithFeatures &oneObject )
{
    certSquare leftEye;
    certSquare rightEye;

    for( std::vector<VisualObject>::iterator featureIter = oneObject.finalBestFeature.begin(), lastFeature = oneObject.finalBestFeature.end();
         featureIter != lastFeature;
         ++featureIter )
    {
        if( "left_eye" == featureIter->type )
        {
            leftEye.x = featureIter->x;
            leftEye.y = featureIter->y;
        }
        else if( "right_eye" == featureIter->type )
        {
            rightEye.x = featureIter->x;
            rightEye.y = featureIter->y;
        }
    }

    if( (0.0 == leftEye.x && 0.0 == leftEye.y) || (0.0 == rightEye.x && 0.0 == rightEye.y) )
    {
        return false;
    }

    OWFList tmpFaces;
    bool didRotate = false;

    tmpFaces.push_back( oneObject );
    try {
        didRotate = rotator.rotateImageUsingFeatures( &featureDetector, &rimage, tmpFaces, lpatch, alignMethod );
    }
    catch( const char *msg )
    {
        std::cerr << msg << std::endl;
    }
    catch(...)
    {
        std::cerr << "Unable to align face patch." << std::endl;
    }

    return didRotate;
}

ObjectWithFeatures *
MP_CERT::getLargestFace()
{
    return featureDetector.getLargestFace();
}

CFourCoordinates<float>
MP_CERT::getCroppedFaceBox()
{
    return rotator.getFaceBox();
}

mp_FDEyes *
MP_CERT::getFeatureDetector()
{
    return &featureDetector;
}

int
MP_CERT::getMaxTry()
{
    return featureDetector.getMaxTry();
}

void
MP_CERT::setMaxTry( int max )
{
    if( max > 0 )
        featureDetector.setMaxTry( max );
}

void
MP_CERT::setMaxScale( int max )
{
    featureDetector.setMaxScale( max );
}
int
MP_CERT::getMaxScale()
{
    return featureDetector.getMaxScale();
}
void
MP_CERT::setMinScale( int min )
{
    featureDetector.setMinScale( min );
}
int
MP_CERT::getMinScale()
{
    return featureDetector.getMinScale();
}

std::string
MP_CERT::getVersion()
{
    return "4.4.5";
}

std::string
MP_CERT::getBuildString()
{
    std::string version( getMPTRevisionString() );
    version += "::";
    version += getCertRevisionString();
    return version;
}

//ObjectWithFeatures
//MP_CERT::lastFoundFace()
//{
//    return task.currentFace().face;
//}