/*
 *  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 "Deception.h"

#include <iostream>
#include <fstream>
#include <algorithm>
#include <limits>

#ifdef WIN32
#define _USE_MATH_DEFINES
#include <math.h>
#endif

#include <cmath>

// might need to use this to eliminate differences between compilers and OSes.
static const double my_pi = 3.1415926535897;

namespace mpt
{

Deception::Deception( const std::string &w2_path, const std::string &path2 ) : count(0), interpolating(false), lastCount(0), svm(w2_path), firstFrame(-1)
{
}

Deception::~Deception()
{
}

void
Deception::start()
{
    count = 0;
    lastCount = 0;
    interpolating = false;
    auSeries.clear();
	lastFrame.clear();
}

void
Deception::interpolate( const vectorD1D &aus, bool faceFound )
{
    ++count;

    if( faceFound )
    {
        if( interpolating && (lastCount > 0) )
        {
            double numSteps = count - lastCount;
            std::vector<double> austeps;

            // calculate the step value for each AU
            for( unsigned int ind = 0; ind < aus.size(); ++ind )
            {
                double step = (aus[ind] - lastFrame[ind]) / numSteps;
                austeps.push_back( step );
            }

            // now increment the lastFrame AU values by austeps, for each missing frame
            for( unsigned int cnt = lastCount + 1; cnt < count; ++cnt )
            {
                std::transform( lastFrame.begin(), lastFrame.end(), austeps.begin(), lastFrame.begin(), std::plus<double>() );
                accumulate( lastFrame );
            }
        }

        if( firstFrame < 0 )
            firstFrame = count;

        interpolating = false;
        accumulate( aus );

        // store these two for when we stop finding faces and need to interpolate
        lastFrame = aus;
        lastCount = count;
    }
    else
    {
        if( count > 0 )
        {
//            std::cout << "missing face: " << count << std::endl;
            interpolating = true;
        }
    }
}

void
Deception::accumulate( const vectorD1D &aus )
{
    auSeries.push_back( aus );
}

double
Deception::currentValue()
{
    // values from w1p.mat
//    const double d = 1.26;
//    const int b = 4;

    // constants for the smoothed histograms
	const double d0 = 1;
	const double dx = 2.2;
	const double w = d0 * 0.7;
    const int BB = 4;

    if( 0 == auSeries.size() )
    {
        std::cerr << "No frames to process in Deception::currentValue()" << std::endl;
        return std::numeric_limits<double>::quiet_NaN();
    }

#ifdef DEBUG
    debugWriteAUSeries( "auSeries.txt" );
#endif

    vectorD3D AI = ai_jw();

#ifdef DEBUG
    debugWriteGabors( "gabor.txt", AI );
#endif

//    vectorD1D old_histograms = newhist( AI, d, b );
    vectorD1D histograms = smoothHistograms( AI, BB, d0, dx, w );

    double val = svm.decision( histograms );

#ifdef DEBUG
    debugWriteHistogram( "out.txt", histograms, val );
#endif

    return val;
}

vectorD3D
Deception::ai_jw()
{
    vectorD3D AI;

    // may need to skip the beginning, when we don't have enough frames collected
    // also may need to start removing from the beginning once we reach the window size

    for( int i8=1; i8 <= 12; ++i8 )
    {
        static double sqrt2 = sqrt(2.0);
        static double sqrt_pi = sqrt(M_PI);

        double F0 = pow( 2.0, (-4.0-(i8/2.0)) );
        double a = (F0/sqrt2) * sqrt_pi;

        std::vector<int> T;
        vectorD1D s1 = makeCosineFilters( F0, a, T ); // should be cached
        vectorD2D filt = integratedGaborFilters( auSeries, s1, T );

        for( unsigned int i = 0; i < filt.size(); ++i )
        {
            std::transform( filt[i].begin(), filt[i].end(), filt[i].begin(), std::bind1st( std::multiplies<double>(), F0 ) );
        }
        AI.push_back( filt );

        // [ai]=IntegratedGaborFilters(x1,F0,a);
//            n=size(ai);
//            AI(i,i8,k,1:n(1),1:n(2))=ai*F0;%frequency adjustment
    }

    return AI;
}


class isInBin
{
public:
    isInBin( double _bot, double _top ) : bot(_bot), top(_top) { }
    bool operator()( double val ) { return ((val > bot) && (val <= top)); }

    double bot;
    double top;
};

// I could not get MSVC to recognize my call to log in the transform, so I had to misdirect
double myLog( double val ) { return log(val); }

vectorD1D
Deception::newhist( const vectorD3D &AI, double d, int b )
{
    vectorD3D h;

    std::vector<double> binEdges;
    std::vector<isInBin> binPreds;

    for( int i = 1; i <= b; ++i )
    {
        binEdges.push_back( (static_cast<double>(i)*d) + 2.5 - (d*static_cast<double>(b)) );
    }
    binEdges.push_back( 5.0 );

    for( int i = 0; i < b; ++i )
    {
        isInBin abin( binEdges[i], binEdges[i+1] );
        binPreds.push_back( abin );
    }

//    printVec( std::cout, binEdges );

    double sumOfSquares = 0.0;

    for( unsigned int j=0; j < AI.size(); ++j )
    {
        double r = pow( 2, (static_cast<double>(j)/2.0) );
        std::vector< std::vector<double> > outer;
        for( unsigned int k=0; k < AI[j].size(); ++k )
        {
            std::vector<double>::iterator iter;
            std::vector<double> x = AI[j][k];

            iter = std::remove_if( x.begin(), x.end(), std::bind2nd( std::less_equal<double>(), 0.0) );
            x.erase( iter, x.end() );

            std::transform( x.begin(), x.end(), x.begin(), myLog );

            std::vector<double> counts(binPreds.size());
            for( unsigned int i=0; i < binPreds.size(); ++i )
            {
                double cnt = std::count_if( x.begin(), x.end(), binPreds[i] );
                cnt *= r;
                sumOfSquares += cnt * cnt;
                counts[i] = cnt;
//                h[j][k].push_back( cnt );
            }
            outer.push_back( counts );
        }
        h.push_back( outer );
    }

    // I need to flatten h, somehow.
    std::vector<double> flatH;
    sumOfSquares = sqrt( sumOfSquares );

    for( unsigned int i=0; i < binPreds.size(); ++i )
        for( unsigned int k=0; k < AI[0].size(); ++k )
            for( unsigned int j=0; j < AI.size(); ++j )
                flatH.push_back( h[j][k][i] / sumOfSquares );
//    h=h(:)';
//    h=h/sqrt(h*h'); %normalized h

    return flatH;
}

// This specifically matches how Matlab's sign function works.
int
Deception::sign( double val )
{
    if( val < 0.0 ) return -1;
    if( val > 0.0 ) return 1;
    return 0;
}

vectorD1D
Deception::getAUvector( const vectorD2D &aus, unsigned int index )
{
    std::vector<double> series;
    for( unsigned int i = 0; i < aus.size(); ++i )
        series.push_back( aus[i][index] );
    return series;
}

vectorD2D
Deception::integratedGaborFilters( const vectorD2D &aus, const vectorD1D &s1, const std::vector<int> &T )
{
    std::vector< std::vector<double> > ai;
    unsigned int nt = aus.size(); // number of time steps, i.e. frames
    unsigned int na = aus[0].size(); // number of AUs

    // j is only being used as an index into our au vectors, so no need to worry about 0 vs 1 indexing.
//    na = 1; // for testing only
    for( unsigned int j = 0; j < na; ++j )
    {
        std::vector<double> y = getAUvector( aus, j );
        std::vector<double> V;

        for( unsigned int v = 0; v < nt; ++v )
        {
            std::vector<int> ty = T;
//            std::vector<int> ty_f; // for the matlab: ty=ty(f)
//            std::vector<unsigned int> f; // indices into ty and s1

            std::transform( ty.begin(), ty.end(), ty.begin(), std::bind1st( std::plus<int>(), v ) );

            double sum = 0.0;

//            std::vector<double> d_y;
//            std::vector<double> d_s1;

            for( unsigned int i = 0; i < ty.size(); ++i )
            {
                if( (ty[i] > 0) && (ty[i] <= static_cast<int>(nt)) )
                {
                    double y_val = y[ ty[i]-1 ];
                    double s1_val = s1[ i ];
                    sum += y_val * s1_val;
                }
            }

            V.push_back( sum );
        }

        double vsum = V[0];
        std::vector<double> zeroCrossings;

        for( unsigned int i = 1; i < V.size(); ++i )
        {
            int vsign1 = sign( V[i-1] );
            int vsign2 = sign( V[i] );
            if( vsign1 != vsign2 )
            {
                zeroCrossings.push_back( vsum );
                vsum = 0.0;
            }
            vsum += V[i];
        }
        zeroCrossings.push_back( vsum );
        ai.push_back( zeroCrossings );

//        if( 0 == j )
//            printVec( zeroCrossings );

//        f=find( sign(V(1:(nt-1))) ~= sign(V(2:nt)) ); %find zero crossings
//        f=unique([f(:);0;nt]);
//
//        for i1=1:(size(f,1)-1)
//            tt=(f(i1)+1):f(i1+1);
//            vsum=sum(V(tt));
//            ai(j,i1)=vsum;
//      end
    }

    return ai;
}


vectorD1D
Deception::makeCosineFilters( double F0, double a, std::vector<int> &T )
{
    int t0 = static_cast<int>( ceil(3.0/a) );

    T.clear();

    // possibly use iota to fill this in
    for( int i = -t0; i <= t0; ++i )
        T.push_back(i);

    // .^ = element by element power
    // .* = element by element multiplication

    const double pia2 = -M_PI * pow( a, 2 );
    const double twopiF0 = 2 * M_PI * F0;
    std::vector<double> s1( T.size() );

//    s1=exp(-pi*(a^2)*(T.^2)).*cos(2*pi*F0*T);
    double mean = 0.0;
    for( unsigned int i = 0; i < s1.size(); ++i )
    {
        s1[i] = exp( pia2 * pow( static_cast<double>(T[i]), 2 ) ) * cos( twopiF0 * T[i] );
        mean += s1[i];
    }

    mean /= static_cast<double>(s1.size());

    double sumabs = 0.0;
//    s1=s1-mean(s1(:));%remove mean
    for( unsigned int i = 0; i < s1.size(); ++i )
    {
        s1[i] -= mean;
        sumabs += fabs( s1[i] );
    }

//    s1=s1/sum(abs(s1(:)));%1-norm
    for( unsigned int i = 0; i < s1.size(); ++i )
    {
        s1[i] /= sumabs;
    }

    return s1;
}

double
Deception::decision( std::vector<double> histogram )
{
    return svm.decision( histogram );
}

vectorD1D
Deception::loadHistogram( const std::string &path )
{
    vectorD1D hist;
    std::ifstream ifs( path.c_str() );
    if( ifs.good() )
    {
        double val;
        while( ifs >> val )
            hist.push_back( val );
    }

    if( mpt::svm_rbf::feature_vector_length != hist.size() )
    {
        std::cerr << "Wrong size in histogram loaded from " << path << ". " << hist.size() << std::endl;
    }

    return hist;
}


// ai is a three dimensional matrix:
//   1st dim = Gabor frequencies (1-12)
//   2nd dim = AUs
//   3rd dim = time
// d0 and dx are Gwen's parameters
// BB is the number of bins in the histogram
vectorD1D
Deception::smoothHistograms( const vectorD3D &ai, int BB, double d0, double dx, double w )
{
//	double const d0 = 1;
//	double const dx = 2.2;
//	const double w = d0 * 0.7;

//    int n1 = ai.size();
//    int n2 = ai[0].size();
//    int n3 = ai[0][0].size();

    vectorD3D histogram( ai.size() ); // pre-allocate

//	double *histogram = (double *) malloc(sizeof(double) * n1 * n2 * BB);
//	double *x = (double *) malloc(sizeof(double) * n3);
	std::vector<double> v(BB);

	/* Initialize v */
	for ( int i = 0; i < BB; i++) {
		v[i] = dx - d0 * i;
		v[i] /= w;
	}

    double sumOfSquares = 0.0;

	for ( unsigned int i = 0; i < ai.size(); i++)
    {
		double r = pow(2, (i+1)/2.0);
        histogram[i].resize( ai[i].size() );

		for ( unsigned int j = 0; j < ai[i].size(); j++)
        {
            vectorD1D x;
			for ( unsigned int k = 0; k < ai[i][j].size(); k++) {
				if (ai[i][j][k] > 0) {
					x.push_back( log(ai[i][j][k]) );
				}
			}

            histogram[i][j].resize( BB );
			for (int k = 0; k < BB; k++)
            {
				double sum = 0;
				for (unsigned int l = 0; l < x.size(); l++) {
					double xk = 1 - (x[l]/w - v[k]) * (x[l]/w - v[k]);
					if (xk > 0) {
						sum += xk * r;
					}
				}

                sumOfSquares += sum * sum;
				histogram[i][j][k] = sum;
			}
		}
	}

    // To match Gwen's reshaping, we have to unpack a 3-dimensional histogram into
    // a vector. Note that, in Matlab, the array indexes start with *least* significant index,
    // and become more significant to the right.
    // histogram[k * (n1 * n2) + j * n1 + i] = sum;
    std::vector<double> flatH;

    sumOfSquares = sqrt( sumOfSquares );

    for( int i=0; i < BB; ++i )
        for( unsigned int k=0; k < ai[0].size(); ++k )
            for( unsigned int j=0; j < ai.size(); ++j )
                flatH.push_back( histogram[j][k][i] / sumOfSquares );

	return flatH;
}

#ifdef DEBUG
void
Deception::debugWriteAUSeries( const std::string &file )
{
    std::ofstream ofs1( file.c_str() );
    for( unsigned int i = 0; i < auSeries.size(); ++i )
    {
        printVec( auSeries[i], ofs1 );
    }
}

void
Deception::debugWriteGabors( const std::string &file, const vectorD3D &AI )
{
    std::ofstream ofs1( file.c_str() );
    unsigned int maxlen = 0;

    for( unsigned int i = 0; i < AI.size(); ++i )
        for( unsigned int j = 0; j < AI[i].size(); ++j )
            if( AI[i][j].size() > maxlen )
                maxlen = AI[i][j].size();

    for( unsigned int i = 0; i < AI.size(); ++i )
    {
        for( unsigned int j = 0; j < AI[i].size(); ++j )
        {
            std::vector<double> tmp( AI[i][j] );
            if( tmp.size() < maxlen )
                tmp.resize( maxlen );
            printVec( tmp, ofs1 );
        }
    }
}

void
Deception::debugWriteHistogram( const std::string &file, const vectorD1D &histograms, double val )
{
    std::ofstream ofs( file.c_str() );

    ofs << "First Frame: " << firstFrame << std::endl;
    ofs << "Num Frames: " << auSeries.size() << std::endl << std::endl;
    printVec( histograms, ofs );

    ofs << std::endl << "Deception Value: " << val << std::endl;
}
#endif
} // end namespace mpt