/*
 *  Deception
 *
 *  Author: Andrew Salamon
 *  Date: Thu June 18, 2010
 *
 *  Copyright (c) 2010 Machine Perception Technologies
 *
 * 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 "NRL_Deception.h"

#include <iostream>
#include <sstream>
#include <algorithm>
#include <math.h>

#include "mp_CERT_Public.h"
#include "CERT_Config.h"
#include "CERT_Arguments.h"
#include "CERT_Scheduler.h"
#include "PluginController.h"

#include "Deception.h"

CERT::Config *defaultConfig();

namespace nrl
{

Deception::Deception( const std::string &ay_path, const std::string &vs_path, const std::string &configPath ) : dec(new mpt::Deception(ay_path, vs_path))
{
    CERT::PluginController *pc = CERT::PluginController::getPluginController( CERT::Arguments::configPath() );
    if( configPath.length() == 0 )
        pc->loadPluginsFromConfig( defaultConfig() );

    cert = new MP_CERT_Public(false);
}

Deception::~Deception()
{
    CERT::PluginController::cleanup();
    CERT::Arguments::cleanup();

    delete cert;
    delete dec;

    videos.cleanup();
}

void
Deception::start( unsigned int _startFrame, unsigned int _endFrame )
{
    startFrame = _startFrame;
    endFrame = _endFrame;

    dec->start();
}

bool
Deception::runOneVideo( const std::string &path )
{
    unsigned int count = 0;

    videos.setPath( path );
    if( !videos.isVideo() )
    {
        // should throw an exception instead
//        std::cerr << "Error. Unable to read video file at path: " << path << std::endl;
        return false;
    }

    RImage<float> image;
    while( videos.nextFrame( image ) )
    {
        ++count;
        if( (count >= startFrame) && (count <= endFrame) )
        {
//            if( count >= 445 )
//            {
//                std::cerr << "Frame: " << count << std::endl;
//            }
            std::ostringstream frameStream;
            frameStream << path << ":" << count;
            runOneImage( image, frameStream.str() );
        }
    }

    videos.setPath(""); ///< Frees up memory

    return true;
}

void
Deception::runOneImage( const RImage<float> &image, const std::string &path )
{
    CERT::Task &task( cert->getTask() );
    task.clear();
    task.rimage = image;
    task.path = path;
    cert->processTask();
    CERT::Scheduler::postProcessTask( task ); // Only need this if you want CERT to print out AU values

    if( task.didFindFace )
    {
        std::vector<double> main = task.results["edu.ucsd.mplab.plugins.CERT4_4_Weights"];
        std::vector<double> add1 = task.results["edu.ucsd.mplab.plugins.AdditionalAUs3"];
        std::vector<double> pose = task.results["edu.ucsd.mplab.plugins.PoseDetector"];
        std::vector<double> blink = task.results["edu.ucsd.mplab.plugins.BlinkAU"];
        std::vector<double> aus;

#ifdef xxDEBUG
        CERT::Task::ResultsMap::iterator iter, end;
        for( iter = task.results.begin(), end=task.results.end(); iter != end; ++iter )
        {
            std::cout << iter->first << std::endl;
            mpt::printVec( iter->second );
            std::cout << std::endl;
        }
#endif

        if( main.size() != 11 )
            throw config_exception( "Incorrect number of AUs in CERT 4.4 plugin." );

        if( add1.size() != 8 )
            throw config_exception( "Incorrect number of AUs in Additional AUs 3 plugin." );

        if( pose.size() != 3 )
        {
//            throw config_exception( "Incorrect number of AUs in Pose plugin." );
            // sometimes the pose detector fails even if the others work. Pretend no face was found.
            dec->interpolate( mpt::vectorD1D(), false );
            return;
        }

        if( blink.size() != 1 )
            throw config_exception( "Incorrect number of AUs in Blink plugin." );

        // If the distance between the eyes is too small, treat it like a missed face
        std::vector<double> leftEye = task.results["edu.ucsd.mplab.plugins.features.leftEye"];
        std::vector<double> rightEye = task.results["edu.ucsd.mplab.plugins.features.rightEye"];
        if( (leftEye.size() > 0) && (rightEye.size() > 0) )
        {
            double eyeDist = fabs( leftEye[0] - rightEye[0] );
            if( eyeDist <= 60.0 )
            {
                dec->interpolate( mpt::vectorD1D(), false );
                return;
            }
        }
        else
        {
            throw config_exception( "Unable to calculate eye distance." );
        }

        aus.push_back( main[0] );
        aus.push_back( main[1] );
        aus.push_back( main[2] );
        aus.push_back( main[3] );

        aus.push_back( add1[0] );
        aus.push_back( add1[1] );

        aus.push_back( main[4] );
        aus.push_back( main[5] );
        aus.push_back( main[6] );
        aus.push_back( main[7] );
        aus.push_back( main[8] );
        aus.push_back( main[9] );

        aus.push_back( add1[2] );

        aus.push_back( main[10] );

        aus.push_back( add1[3] );
        aus.push_back( add1[4] );
        aus.push_back( add1[5] );
        aus.push_back( add1[6] );
        aus.push_back( add1[7] );

        aus.push_back( blink[0] );

        aus.push_back( pose[0] );
        aus.push_back( pose[1] );
        aus.push_back( pose[2] );

        /*
        aus: 1, 2, 4, 5, 9, 10, 12, 14, 15, 17, 20
        add3: 6, 7, 18, 23, 24, 25, 26, 28
        blink?: 45
        pose?: 51, 53, 55
            */
//AUnames
//[1 2 4 5 6 7 9 10 12 14 15 17 18 20 23 24 25 26 28 45 51 53 55]
//These are CERT output channels
//[2 3 4 5 45 46 6 7 8 9 10 11 47 12 48 49 50 51 52 53 55 56 57]

        if( aus.size() != 23 )
        {
            throw config_exception( "Incorrect number of total AUs." );
        }
        dec->interpolate( aus, true );
    }
    else
    {
        dec->interpolate( mpt::vectorD1D(), false );
    }

}

double
Deception::currentValue()
{
    return dec->currentValue();
}

} // end namespace nrl

CERT::Config *defaultConfig()
{
    const std::string defaultFD( "./featuredetector/" );
    const std::string defaultSVM( "./SVMWeights" );

    CERT::Config *tmp = new CERT::Config;
    CERT::ConfigItem *item = new CERT::ConfigItem;

    item->setPluginID( "edu.ucsd.mplab.plugins.CERT4_4_Weights" );
    item->setName( "FACS Codes 4.4" );
    item->setInternalName( "CERT4_4_Weights" );
    item->setPluginType( "MPT_PluginStep_SVM" );
    item->setEnabled( true );
    item->setValueForKey( defaultSVM, "dir" );

    std::vector<std::string> labels;
    labels.push_back( "(AU 1) Inner Brow Raise" );
    labels.push_back( "(AU 2) Outer Brow Raise" );
    labels.push_back( "(AU 4) Brow Lower" );
    labels.push_back( "(AU 5) Eye Widen" );
    labels.push_back( "(AU 9) Nose Wrinkle" );
    labels.push_back( "(AU 10) Lip Raise" );
    labels.push_back( "(AU 12) Lip Corner Pull" );
    labels.push_back( "(AU 14) Dimpler" );
    labels.push_back( "(AU 15) Lip Corner Depressor" );
    labels.push_back( "(AU 17) Chin Raise" );
    labels.push_back( "(AU 20) Lip stretch" );
    item->setValueForKey( labels, "labels" );

    tmp->addItem( item );


    item = new CERT::ConfigItem;
    item->setPluginID( "edu.ucsd.mplab.plugins.AdditionalAUs3" );
    item->setName( "Additional AUs 3" );
    item->setInternalName( "AdditionalAUs3" );
    item->setPluginType( "MPT_PluginStep_SVM" );
    item->setEnabled( true );
    item->setValueForKey( defaultSVM, "dir" );

    labels.clear();
    labels.push_back( "(AU 6) Cheek Raise" );
    labels.push_back( "(AU 7) Lids Tight" );
    labels.push_back( "(AU 18) Lip Pucker" );
    labels.push_back( "(AU 23) Lip Tightener" );
    labels.push_back( "(AU 24) Lip Presser" );
    labels.push_back( "(AU 25) Lips Part" );
    labels.push_back( "(AU 26) Jaw Drop" );
    labels.push_back( "(AU 28) Lips Suck" );

    item->setValueForKey( labels, "labels" );

    tmp->addItem( item );


    item = new CERT::ConfigItem;
    item->setPluginID( "edu.ucsd.mplab.plugins.BlinkAU" );
    item->setName( "Blink AU Detector" );
    item->setInternalName( "BlinkAU" );
    item->setPluginType( "MPT_PluginStep_SVM" );
    item->setEnabled( true );
    item->setValueForKey( defaultSVM, "dir" );

    labels.clear();
    labels.push_back( "(AU 45) Blink/Eye Closure" );

    item->setValueForKey( labels, "labels" );

    tmp->addItem( item );


    item = new CERT::ConfigItem;
    item->setPluginID( "edu.ucsd.mplab.plugins.PoseDetector" );
    item->setName( "Pose Detector" );
    item->setInternalName( "posedetector" );
    item->setPluginType( "MPT_PluginStep_AfterFaces" );
    item->setEnabled( true );

    labels.clear();
    labels.push_back( "Yaw" );
    labels.push_back( "Pitch" );
    labels.push_back( "Roll" );
    item->setValueForKey( labels, "labels" );

    tmp->addItem( item );

    CERT::ConfigItem::PropertyType fdproc;

    labels.clear();
    labels.push_back( "left_eye_imp" );
    labels.push_back( "left_eye_nasal_imp" );
    labels.push_back( "left_eye_temporal_imp" );
    fdproc["left_eye"] = labels;

    labels.clear();
    labels.push_back( "right_eye_imp" );
    labels.push_back( "right_eye_nasal_imp" );
    labels.push_back( "right_eye_temporal_imp" );
    fdproc["right_eye"] = labels;

    labels.clear();
    labels.push_back( "nose_imp" );
    fdproc["nose"] = labels;

    labels.clear();
    labels.push_back( "mouth_imp" );
    labels.push_back( "mouth_left_corner" );
    labels.push_back( "mouth_right_corner" );
    fdproc["mouth"] = labels;
    tmp->getGlobalItem().setValueForKey( fdproc, "fdproc" );


    CERT::ConfigItem *feature = new CERT::ConfigItem;
    feature->setPluginID( "edu.ucsd.mplab.plugins.features.leftEye" );
    feature->setName( "Left Eye" );
    feature->setInternalName( "left_eye" );
    feature->setPluginType( "MPT_PluginStep_Features" );
    feature->setEnabled( true );
    feature->setDisplay( false );
    feature->setValueForKey( (defaultFD + "left_eye_CERT3.fdtxml"), "feature" );
    feature->setValueForKey( (defaultFD + "left_eye_CERT3.gpriorxml"), "gprior" );

    tmp->addItem( feature );

    feature = new CERT::ConfigItem;
    feature->setPluginID( "edu.ucsd.mplab.plugins.features.rightEye" );
    feature->setName( "Right Eye" );
    feature->setInternalName( "right_eye" );
    feature->setPluginType( "MPT_PluginStep_Features" );
    feature->setEnabled( true );
    feature->setDisplay( false );
    feature->setValueForKey( (defaultFD + "right_eye_CERT3.fdtxml"), "feature" );
    feature->setValueForKey( (defaultFD + "right_eye_CERT3.gpriorxml"), "gprior" );

    tmp->addItem( feature );

    feature = new CERT::ConfigItem;
    feature->setPluginID( "edu.ucsd.mplab.plugins.features.nose" );
    feature->setName( "Nose" );
    feature->setInternalName( "nose" );
    feature->setPluginType( "MPT_PluginStep_Features" );
    feature->setEnabled( true );
    feature->setDisplay( false );
    feature->setValueForKey( (defaultFD + "nose_CERT3.fdtxml"), "feature" );
    feature->setValueForKey( (defaultFD + "nose_CERT3.gpriorxml"), "gprior" );

    tmp->addItem( feature );

    feature = new CERT::ConfigItem;
    feature->setPluginID( "edu.ucsd.mplab.plugins.features.mouth" );
    feature->setName( "Mouth" );
    feature->setInternalName( "mouth" );
    feature->setPluginType( "MPT_PluginStep_Features" );
    feature->setEnabled( true );
    feature->setDisplay( false );
    feature->setValueForKey( (defaultFD + "mouth_CERT3_1.fdtxml"), "feature" );
    feature->setValueForKey( (defaultFD + "mouth_CERT3_1.gpriorxml"), "gprior" );
    tmp->addItem( feature );

    feature = new CERT::ConfigItem;
    feature->setPluginID( "edu.ucsd.mplab.plugins.features.MPT_LeftEye_Nasal" );
    feature->setName( "left_eye_nasal" );
    feature->setInternalName( "left_eye_nasal" );
    feature->setPluginType( "MPT_PluginStep_Features" );
    feature->setEnabled( true );
    feature->setDisplay( false );
    feature->setValueForKey( (defaultFD + "left_eye_nasal.fdtxml"), "feature" );
    feature->setValueForKey( (defaultFD + "left_eye_nasal.gpriorxml"), "gprior" );
    tmp->addItem( feature );

    feature = new CERT::ConfigItem;
    feature->setPluginID( "edu.ucsd.mplab.plugins.features.MPT_RightEye_Nasal" );
    feature->setName( "right_eye_nasal" );
    feature->setInternalName( "right_eye_nasal" );
    feature->setPluginType( "MPT_PluginStep_Features" );
    feature->setEnabled( true );
    feature->setDisplay( false );
    feature->setValueForKey( (defaultFD + "right_eye_nasal.fdtxml"), "feature" );
    feature->setValueForKey( (defaultFD + "right_eye_nasal.gpriorxml"), "gprior" );
    tmp->addItem( feature );

    feature = new CERT::ConfigItem;
    feature->setPluginID( "edu.ucsd.mplab.plugins.features.MPT_LeftEye_Temporal" );
    feature->setName( "left_eye_temporal" );
    feature->setInternalName( "left_eye_temporal" );
    feature->setPluginType( "MPT_PluginStep_Features" );
    feature->setEnabled( true );
    feature->setDisplay( false );
    feature->setValueForKey( (defaultFD + "left_eye_temporal.fdtxml"), "feature" );
    feature->setValueForKey( (defaultFD + "left_eye_temporal.gpriorxml"), "gprior" );
    tmp->addItem( feature );

    feature = new CERT::ConfigItem;
    feature->setPluginID( "edu.ucsd.mplab.plugins.features.MPT_RightEye_Temporal" );
    feature->setName( "right_eye_temporal" );
    feature->setInternalName( "right_eye_temporal" );
    feature->setPluginType( "MPT_PluginStep_Features" );
    feature->setEnabled( true );
    feature->setDisplay( false );
    feature->setValueForKey( (defaultFD + "right_eye_temporal.fdtxml"), "feature" );
    feature->setValueForKey( (defaultFD + "right_eye_temporal.gpriorxml"), "gprior" );
    tmp->addItem( feature );

    feature = new CERT::ConfigItem;
    feature->setPluginID( "edu.ucsd.mplab.plugins.features.MPT_Mouth_LeftCorner" );
    feature->setName( "mouth_left_corner" );
    feature->setInternalName( "mouth_left_corner" );
    feature->setPluginType( "MPT_PluginStep_Features" );
    feature->setEnabled( true );
    feature->setDisplay( false );
    feature->setValueForKey( (defaultFD + "mouth_left_corner.fdtxml"), "feature" );
    feature->setValueForKey( (defaultFD + "mouth_left_corner.gpriorxml"), "gprior" );
    tmp->addItem( feature );

    feature = new CERT::ConfigItem;
    feature->setPluginID( "edu.ucsd.mplab.plugins.features.MPT_Mouth_RightCorner" );
    feature->setName( "mouth_right_corner" );
    feature->setInternalName( "mouth_right_corner" );
    feature->setPluginType( "MPT_PluginStep_Features" );
    feature->setEnabled( true );
    feature->setDisplay( false );
    feature->setValueForKey( (defaultFD + "mouth_right_corner.fdtxml"), "feature" );
    feature->setValueForKey( (defaultFD + "mouth_right_corner.gpriorxml"), "gprior" );
    tmp->addItem( feature );

    feature = new CERT::ConfigItem;
    feature->setPluginID( "edu.ucsd.mplab.plugins.features.FeatureImprover" );
    feature->setName( "featureimprover" );
    feature->setInternalName( "featureimprover" );
    feature->setPluginType( "MPT_PluginStep_Features" );
    feature->setEnabled( true );
    feature->setDisplay( false );
    feature->setValueForKey( "", "feature" );
    feature->setValueForKey( "", "gprior" );
    labels.clear();
    labels.push_back( "Left Eye Imp" );
    labels.push_back( "Right Eye Imp" );
    labels.push_back( "Left Eye Temporal Imp" );
    labels.push_back( "Left Eye Nasal Imp" );
    labels.push_back( "Right Eye Temporal Imp" );
    labels.push_back( "Right Eye Nasal Imp" );
    labels.push_back( "Nose Imp" );
    labels.push_back( "Mouth Imp" );
    labels.push_back( "Mouth Left Corner Imp" );
    labels.push_back( "Mouth Right Corner Imp" );

    feature->setValueForKey( labels, "labels" );
    tmp->addItem( feature );

    item = new CERT::ConfigItem;
    item->setPluginID( "edu.ucsd.mplab.plugins.features.StandardOutput" );
    item->setName( "Standard Output" );
    item->setInternalName( "standardoutput" );
    item->setPluginType( "MPT_PluginStep_Output" );
#ifdef DEBUG
    item->setEnabled( true );
#else
    item->setEnabled( false ); // endable this if you want CERT AU outputs to be written to file
#endif
    item->setDisplay( true );

    tmp->addItem( item );

#ifdef DEBUG
    // May need to turn this on all the time if we need to filter frames based on face box size
    item = new CERT::ConfigItem;
    item->setPluginID( "edu.ucsd.mplab.plugins.features.FaceFinderStub" );
    item->setName( "Face Detector" );
    item->setInternalName( "facedetector" );
    item->setPluginType( "MPT_PluginStep_NoStep" );
    item->setEnabled( true );
    item->setDisplay( false );

    tmp->addItem( item );
#endif

    return tmp;
}