#include "GaborTest.h"
#include "rimage_matlab.h"
#include "rimage_ser.h"
#include "mp_SVMWeights.h"
#include "mp_threadedGabor.h"
#include "mpt_AUIterator.h"

#include "mat.h"

#include <fstream>
#include <iostream> // for cout
//#define DISPLAY_AUS
#ifdef DISPLAY_AUS
  #include <iomanip> // for setprecision( int p )
#endif
#include <memory> // For auto_ptr
#include <complex>
#include <numeric> // for std::accumulate

using namespace std;

static const string weightsPrefix("CERT3_3_Weights");

/** Compare two RImages
 * First check dimentions, then compare every pixel. Currently using an arbitrary small
 * value to compare pixels, but something a little more flexible that measured how
 * different images are in some way relevant to CERT would be better.
 */
bool operator==( const RImage<float> &lhs, const RImage<float> &rhs )
{
    if( lhs.width != rhs.width )
        return false;
    if( lhs.height != rhs.height )
        return false;
    if( lhs.numpixels != rhs.numpixels )
        return false;

    float diffSum = 0.0;
    for( int index=0; index < rhs.numpixels; ++index )
    {
        float diff = fabs( lhs.getPixel(index) - rhs.getPixel(index) );
        // These diff values are empirical, based on doing tests in Matlab,
        // with a bit of extra leeway.
        if( diff > 1.0e-07 )
            return false;
    }
    if( diffSum > 2.0e-04)
        return false;

    return true;
}

CPPUNIT_TEST_SUITE_REGISTRATION( GaborTest );

GaborTest::GaborTest() : image(NULL), ffted(NULL), gabor(NULL)
{

}

/** Load an RImage from a file.
 * Currently we load from a Matlab .mat file, but hopefully we will get Boost::Serialization working
 * and switch to using that. At the least we need to load two files, an input and a properly rotated result that we can compare with.
 * The input file must be called "input.mat" and contain a single Matlab array which contains the image. See readImageMat().
 * The known good file is called "rotated.mat".
 */
void GaborTest::setUp()
{
    image = new RImage<float>;
    restoreRImageFromFile( *image, "unit_orig.rimage" );

    if( image )
        gabor = new MP_Gabor( *image, 96, 96 );

    // Set the SVM directory once
    mp_SVMWeights *svm = mp_SVMWeights::getSVMWeights( weightsPrefix );
    svm->setDirectory( "SVMWeights_dir" );

    std::vector< std::string > categories;
    categories.push_back( weightsPrefix );
    gabor->setCategories( categories );
}

/** Delete all test RImages and the gabor object
 */
void GaborTest::tearDown()
{
    if( image ) delete image;
    if( gabor ) delete gabor;
}

/** Rotate an RImage and compare.
 * Rotate the input RImage and compare with one we know is properly rotated.
 */
void GaborTest::testNormalize()
{
    if( !image )
        CPPUNIT_FAIL( "Failed to load the test image from a mat file." );
    if( !gabor )
        CPPUNIT_FAIL( "Failed to create a gabor object." );

    gabor->normalize();
    RImage<float> &test = gabor->getPixels();
    RImage<float> emptyImage;
	RImage<float> normalized;

    restoreRImageFromFile( normalized, "unit_mat2gray.rimage" );

    CPPUNIT_ASSERT( test.numpixels > 0 );
    CPPUNIT_ASSERT( !(test == emptyImage) );
    CPPUNIT_ASSERT( test == normalized );
}

void GaborTest::testFFT()
{
    gabor->fft();
    myComplex *fft = gabor->getImgStar();
    CPPUNIT_ASSERT( fft != NULL );

    mxArray *array = readImageMat( "unit_fft.mat" );
    if( array )
    {
        myComplex *goodFFT = complexFromMatlabArray( array );
        mxDestroyArray(array);

        int cnt = gabor->getWidth() * gabor->getHeight();
        double diffSum = 0.0;
        for( int index=0; index < cnt; ++index )
        {
            double diff = std::abs( fft[index] - goodFFT[index] );
            // Diff values are based on tests done in Matlab
            if( diff > 0.0002 )
                CPPUNIT_FAIL( "FFT data failed to match known good data." );
            diffSum += diff;
        }
        if( diffSum > 0.01 )
            CPPUNIT_FAIL( "FFT diff sums were too large." );
    }
    else
    {
        CPPUNIT_FAIL( "Failed to load the known good fft data." );
    }
}

// We really should try to insure that this one runs last so we don't need to load/reload weights files
void GaborTest::testSVM()
{
    mp_SVMWeights *svm = mp_SVMWeights::getSVMWeights( weightsPrefix );
    const double *weights = svm->getWeights( 1, 2 );
    CPPUNIT_ASSERT_MESSAGE( "Probable missing weights files", (weights != NULL) );
    weights = svm->getWeights( -9999, -9999 );
    CPPUNIT_ASSERT( weights == NULL );

    // Now clear the weights files, set the directory to an unlikely location and try getting a
    // good set of weights.
    svm->clearWeights();
    const std::string saveDirectory = svm->getDirectory();
    svm->setDirectory( "asdf@$%#adsf*&" );
    weights = svm->getWeights( 1, 2 ); // If the clearWeights worked, then this should fail
    CPPUNIT_ASSERT( weights == NULL );
    svm->clearWeights();
    svm->setDirectory( saveDirectory );
}

double AUDiff( const std::vector<double> &res, const std::vector<double> &good )
{
    unsigned int resSize = res.size();
    unsigned int goodSize = good.size();
    unsigned int maxSize = std::max( resSize, goodSize );
    double diff = 0.0;
    for( unsigned int i=0; i<maxSize; ++i )
    {
        if( i >= resSize )
            diff += fabs( good[i] );
        else if( i >= goodSize )
            diff += fabs( res[i] );
        else
            diff += fabs( good[i] - res[i] );
    }
    return diff;
}

static void printResults( std::vector<double> &results )
{
    std::cout.precision(8);
    for( unsigned int i=0; i < results.size(); ++i )
    {
        std::cout << results[i];
        if( i < (results.size() - 1) )
            std::cout << ", ";
    }
    std::cout << std::endl;
}

static const double maxDiff = 1.0e-5;
static const int AUcnt = 10;

/*
    Recalculated the AU outputs on August 31, 2006.
*/
void GaborTest::testCERT()
{
    // Results from processing the same face patch in Matlab
     static const double _matlabRes[] = { -0.97767568, -1.7614114, 0.46247908, -1.0068477, -1.319282, -1.2308271, -0.54711207, -0.69344958 };
    std::vector<double> matlabRes( _matlabRes, _matlabRes+AUcnt );

    gabor->CERT_Gabor();
    vector<double> res = gabor->resultsForCategory( weightsPrefix );

    double diff = AUDiff( res, matlabRes );
#ifdef DEBUG
    std::cout << "AUs for image 1: ";
    printResults( res );
#endif
    CPPUNIT_ASSERT( diff < maxDiff );

    // Test setting a new image
    RImage<float> face2;
    restoreRImageFromFile( face2, "unit_face2.rimage" );

    gabor->setPixels( face2 );

    static const double _matlabRes2[] = { -1.5109611, -0.90098601, -1.800163, -1.6979643, -0.85815242, -1.2595893, -2.2391609, -2.1446403 };
    std::vector<double> matlabRes2( _matlabRes2, _matlabRes2+AUcnt );

    gabor->CERT_Gabor();
    vector<double> res2 = gabor->resultsForCategory( weightsPrefix );

    diff = AUDiff( res2, matlabRes2 );
#ifdef DEBUG
    std::cout << "AUs for image 2: ";
    printResults( res2 );
#endif
    CPPUNIT_ASSERT_MESSAGE( "Failed to calculate second face AU's", (diff < maxDiff) );
}

void GaborTest::testCERTImages()
{
    std::string datapath( "testImages.txt" );
    mpt_AUIterator iter( datapath );
    mp_threadedGabor *gabor1 = new mp_threadedGabor( *image, 96, 96 );
    auto_ptr<mp_threadedGabor> gaborDeleter(gabor1); // Make sure it gets deleted
    std::string fname;
    std::vector<double> AUs;
    unsigned int testCount = 0;

    while( iter.nextDataLine( fname, AUs ) )
    {
        RImage<float> testImage;

        restoreRImageFromFile( testImage, fname );

        gabor1->setPixels( testImage );

        gabor1->CERT_Gabor();
        vector<double> res = gabor1->resultsForCategory( weightsPrefix );

        std::string errorMessage( "Image: " );
        errorMessage += fname;

        double diff = AUDiff( res, AUs );

        ++testCount;

#ifdef DISPLAY_AUS
        std::cout << endl << errorMessage << " AUs: " << setprecision( 8 );
        for( unsigned int ind=0; ind < res.size(); ++ind )
        {
            std::cout << res[ind] << " ";
        }
        std::cout << std::endl;

        std::cout << endl << " Known Good AUs: " << setprecision( 8 );
        for( unsigned int ind=0; ind < AUs.size(); ++ind )
        {
            std::cout << AUs[ind] << " ";
        }
        std::cout << std::endl;

        if( diff >= maxDiff )
            std::cout << endl << "Diff is too great: " << diff << endl;
#else
        CPPUNIT_ASSERT_MESSAGE( errorMessage, diff < maxDiff );
#endif
    }

    CPPUNIT_ASSERT_MESSAGE( "There should have been at least three test images.", testCount >= 3 );
}

/*
    Make sure that the clearYhat method works.
*/
void GaborTest::testClearYhat()
{
    gabor->CERT_Gabor();
    vector<double> res = gabor->resultsForCategory( weightsPrefix );

    // Just make sure we got some kind of non-zero results.
    double sum = std::accumulate( res.begin(), res.end(), 0.0 ); // AUDiff( res, zeros );
    CPPUNIT_ASSERT_MESSAGE( "testClearYhat could not run becuase no results were generated.", sum != 0.0 );

    gabor->clearYhat();
    res = gabor->resultsForCategory( weightsPrefix );
    sum = std::accumulate( res.begin(), res.end(), 0.0 ); // AUDiff( res, zeros );
    CPPUNIT_ASSERT_MESSAGE( "clearYhat did not zero out results.", sum < maxDiff );
}

/*
 *  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.
 */