/*
 *  mp_gabor
 *
 *  Author: Andrew Salamon
 *  Date: Wed Mar 29 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_gabor.h"
#include "mp_SVMWeights.h"
#include "mp_GaborKernels.h"

#include <limits>

#ifdef xxDEBUG
#include <iostream>
//#include <ostream>
#endif

#ifdef WIN32
extern "C" {
#ifdef INTELMKL
#include "mkl_cblas.h"
#else
#include "cblas.h"
#endif
}
#endif

#ifdef __APPLE_CC__
#include <vecLib/vBLAS.h>
#endif

#ifdef linux
extern "C" {
#include <gsl/gsl_cblas.h>
    //typedef int integer;
//#include <f2c.h>
//#include <clapack.h>
}
//typedef integer __CLPK_integer;
//typedef float __CLPK_real;
#endif

const unsigned int maxMu = 8;

void printTen( const myComplex *matrix, int width, int height );

/** Transpose a matrix in place (switch column and row order).
 * Equivalent to \<array\>=\<array\>.' in Matlab
 * This is a template that works on double and complex<double> and probably other types as well.
 * This particular implemenatation will only work for square matrixes.
 */
template< typename T >
void genTranspose( T *matrix, int width, int height )
{
    /* transpose in-place */
    T swapf;

    // Can't transpose using this method unless it is a square matrix, we need some way to return an error, though
    if( width != height )
        return;

    for ( int i = height-1; i >= 0; i-- )
        for ( int j = 0; j < i; j++ )
        {
            swapf = matrix[i*width+j];
            matrix[i*width+j] = matrix[j*height+i];
            matrix[j*height+i] = swapf;
        }
}

/** MP_Gabor constructor.
 * The arrays used by FFTW need to be set up during construction.
 * This is so we can generate and store the plans. This also means that we can't
 * reallocate either of these arrays, which is a bit tricky with the RImage pixels because there are
 * a number of methods on that class that will change the array out from under us.
 * It might be better in the long run to copy the input RImage into an internal array.
 */
MP_Gabor::MP_Gabor(  const RImage<float> &_pixels, int width, int height, bool _sub_proc ) : sub_proc(_sub_proc), pixels(_pixels), dblPixels( pixels.width, pixels.height), patchWidth(width), patchHeight(height), normalized(false), fftPlan(0), ifftPlan(0), img_star(0), intermediate(0), absTemp(0), loadError(false), minNi(1), maxNi(9)
{
    const unsigned flags = FFTW_PRESERVE_INPUT|FFTW_MEASURE; // FFTW_ESTIMATE

    if( !sub_proc )
    {
        img_star = (myComplex *)fftw_malloc( sizeof(myComplex) * dblPixels.numpixels );

        double *in = dblPixels.array;
        fftw_complex *outFFTW = (fftw_complex *)img_star;

        fftPlan = fftw_plan_dft_r2c_2d( dblPixels.width,  dblPixels.height, in, outFFTW, flags);
    }

    intermediate = (myComplex *)fftw_malloc( sizeof(myComplex) * dblPixels.numpixels );
    ifftPlan = fftw_plan_dft_2d( patchWidth, patchHeight, (fftw_complex *)intermediate, (fftw_complex *)intermediate, FFTW_BACKWARD, flags);
    // Throw an exception on memory failure, or any failure to create a plan

	clearYhat();
}

MP_Gabor::~MP_Gabor()
{
    if( !sub_proc && img_star )
        fftw_free( img_star );
    if( intermediate )
        fftw_free( intermediate );
    if( absTemp )
        free( absTemp );

    if( fftPlan )
        fftw_destroy_plan( fftPlan );
    if( ifftPlan )
        fftw_destroy_plan( ifftPlan );

//    fftw_cleanup();
}

/** Copies new pixels and resets several variables.
 * Not all intermediate arrays are cleared out but that shouldn't
 * affect the results if each step is done in order.
 * \param[in] newImage Currently this needs to be a 96x96 aligned face patch
 */
void MP_Gabor::setPixels( const RImage<float> &newImage )
{
//    for( int i=0; i < pixels.numpixels; ++i )
//        pixels.setPixel( i, newImage.getPixel(i) );
    pixels = newImage;
    normalized = false;
    clearYhat();
}

RImage<float> &MP_Gabor::getPixels()
{
    return pixels;
}

RImage<double> &MP_Gabor::getDoublePixels()
{
    return dblPixels;
}

myComplex *MP_Gabor::getMatrix()
{
    return intermediate;
}

myComplex *MP_Gabor::getImgStar()
{
    return img_star;
}

double *MP_Gabor::getAbs()
{
    return absTemp;
}

void
MP_Gabor::setCategories( std::vector< std::string > &_categories )
{
	categories = _categories;
}

void
MP_Gabor::addCategory( std::string cat )
{
	categories.push_back( cat );
}

void
MP_Gabor::removeCategory( std::string cat )
{
	std::vector<std::string>::iterator found = std::find( categories.begin(), categories.end(), cat );
	if( categories.end() != found )
		categories.erase( found );
}

std::map< std::string, std::vector<double> >
MP_Gabor::resultsForCategories()
{
	return catResults;
}

std::vector<double>
MP_Gabor::resultsForCategory( const std::string &cat )
{
	return catResults[ cat ];
}

std::map< std::string, mp_AUResults >
MP_Gabor::getFrequencies()
{
    return frequencies;
}

mp_AUResults &
MP_Gabor::frequenciesForCategory( const std::string &cat )
{
    return frequencies[ cat ];
}

void MP_Gabor::clearYhat()
{
	for( std::vector< std::string >::iterator iter = categories.begin(), lastIter = categories.end(); iter != lastIter; ++iter )
	{
		mp_SVMWeights *SVMWeights = mp_SVMWeights::getSVMWeights( *iter );
		int count = SVMWeights->getCount();
		catResults[ *iter ] = std::vector<double>( count, 0.0 );
	}
	frequencies.clear();
}

int MP_Gabor::getWidth()
{
    return patchWidth;
}

int MP_Gabor::getHeight()
{
    return patchHeight;
}

void MP_Gabor::setNi( unsigned int min, unsigned int max )
{
	minNi = min;
	maxNi = max;
}

void MP_Gabor::operator()()
{
	kernelLoop();
}

/** Main AU calculation.
 * This method duplicates the main AU processing in the CERT Matlab scripts.
 *
 * Currently only works on 96x96 aligned face patches.
 *
 * Normalize the image, Fourier transform the image, then call the kernel loop.
 *
 * Returns values for eight AU's: 1, 2, 4, 5, 10, 12, 14, 20
 */
bool MP_Gabor::CERT_Gabor()
{
    loadError = false;
    clearYhat();

	// Don't do anything if we don't have any SVM categories to run.
	if( categories.size() > 0 )
	{
		if( !sub_proc )
		{
			normalize();
			fft(); // includes a transpose
		}

		kernelLoop();

		if( loadError )
		{
			clearYhat();
			return false;
		}
	}

	return true;
}

/* Gabor/SVM kernel loop.
 * Convolve fft with Gabor kernels, then multiply by SVM weights.
 *
 * This was separated from the main CERT_Gabor method so that a threaded version of this class
 *  could run different parts of the loops in different threads.
 */
void MP_Gabor::kernelLoop()
{
    for( unsigned int ni=minNi; (ni<=maxNi) && !loadError; ++ni )
    {
        for( unsigned int mu=1; (mu<=maxMu) && !loadError; ++mu )
        {
#ifdef xxDEBUG
            std::cout << minNi << "/" << maxNi << " is processing Kernel Ni/Mu: " << ni << "/" << mu << std::endl;
            std::cout.flush();
#endif
            kernelMultiply( ni-3, mu-1 );
            ifft();
            fftshift();
            complexConjugateTranspose( intermediate, patchWidth, patchHeight );
            abs();

			// Possible extra normalization step needed for the new weights from Gwen:
			postGaborNormalize();

            genTranspose( absTemp, patchWidth, patchHeight );

			for( std::vector< std::string >::iterator iter = categories.begin(), lastIter = categories.end(); iter != lastIter; ++iter )
			{
				std::vector<double> accum = catResults[ *iter ];
				std::vector<double> step = svmWeight( ni, mu, *iter ); // SVM multiplication step
                transform( accum.begin(), accum.end(), step.begin(), accum.begin(), std::plus<double>() );
				catResults[ *iter ] = accum;

                if( frequencies.find( *iter ) == frequencies.end() )
                {
                    // The results for this category haven't been created yet.
                    mp_SVMWeights *SVMWeights = mp_SVMWeights::getSVMWeights( *iter );
                    unsigned int count = SVMWeights->getCount();
                    mp_AUResults temp( *iter, count );
                    temp.addFrequency( ni, mu, step );
                    frequencies[ *iter ] = temp;
                }
                else
                {
                    frequencies[ *iter ].addFrequency( ni, mu, step );
                }
			}
        }
    }
}


void
MP_Gabor::setImgStar( myComplex *_img_star )
{
    if( sub_proc )
        img_star = _img_star;
}

/** Duplicates Matlab's mat2gray() function.
 * Calc: (pixel - min) * (1 / (max - min))
 * Also fills in the double pixel array (as opposed to the float pixels).
 *
 * Input: pixels (RImage<float>).
 *
 * Output: pixels and dblPixels (RImage<double>).
 */
void MP_Gabor::normalize()
{
    if( !normalized )
    {
        float min = std::numeric_limits<float>::max();
        float max = std::numeric_limits<float>::min();

        for( int index=0; index < pixels.numpixels; ++index )
        {
            float pix = pixels.getPixel( index );
            if( pix < min ) min = pix;
            if( pix > max ) max = pix;
            // min = (pix<min)?pix:min;
        }

        float mult = 1.0 / (max - min);
        for( int index=0; index < pixels.numpixels; ++index )
        {
            float pix = pixels.getPixel( index );
            pix = (pix - min) * mult;
            pixels.setPixel( index, pix );
            // min = (pix<min)?pix:min;
            dblPixels.setPixel( index, pix );
        }

        normalized = true;
    }
}

/** Fast Fourier Transform.
 * Insures that the image is normalized first.
 * FFTW only calculates half of the fft so we fill in the other half then transpose the result.
 * This step may not be necessary if we adjust other data to match.
 *
 * Input: dblPixels (RImage<double>).
 *
 * Output: img_star (complex<double>).
 */
void MP_Gabor::fft()
{
    if( !normalized )
        normalize();

    fftw_execute( fftPlan );

    fftFill( img_star, 49, 96 );
    genTranspose( img_star, patchWidth, patchHeight );
}

/** Inverse Fast Fourier Transfor.
 * After the ifft we need to scale the results to match Matlab: divide each pixel by the width*height.
 *
 * Input: intermediate (complex<double>).
 *
 * Output: intermediate (complex<double>).
 */
void MP_Gabor::ifft()
{
    fftw_execute( ifftPlan );

    // Normalize the result so it matches what Matlab does
    int scale=patchWidth*patchHeight;
//    for( int x=0; x < scale; ++x )
//        intermediate[x] /= scale;

    double alpha = 1.0 / scale;
    cblas_zdscal( scale, alpha, intermediate, 1 );
}

/** Adjust the fftw results to match Matlab's fft2() function.
 * I have no idea why this is so complicated.
 * <ul>
 * <li>Step one: Fill in the left half because the fftw output is only N/2+1 wide, at the same time copy
 *   the right hand column (width-1) to the left hand column (0) and shift everything
 *   else one column to the right.
 * <li>Step two: copy over the origin value [0,0].
 * <li>Step three: copy (and flip) the top row (except the first column) to the right half of the top row.
 * <li>Step four: copy the left half (except the first column and the first row) to the right half, but also flip it
 *   top to bottom.
 * </ul>
 */
void MP_Gabor::fftFill( myComplex *fft, int width, int height )
{
    int max=(width>height)?width:height;
    size_t bytecnt = sizeof(myComplex) * max * max;
    myComplex *cp = (myComplex *)malloc( bytecnt );

    memcpy( cp, fft, bytecnt );
    memset( fft, 0, bytecnt );

    // Step one
    int source = 0;
    for(int r = 0; r < height; ++r)
    {
        for (int c = 0; c < width; ++c)
        {
            if( c == (width-1) )
            {
                if( r < (height-1) )
                    fft[(r+1)*height+0] = cp[++source];
            }
            else
                fft[r*height+c+1] = cp[++source];
        }
    }
    // Step two
    fft[0] = cp[0];

    // Step three
    for (int c = 1; c < width; ++c)
    {
//        myComplex tmp( conj(cp[c]) );
        fft[max-c] = conj(cp[c]);
    }

    // Step four
    for(int r = 1; r < height; ++r)
    {
        for (int c = 1; c < width; ++c)
        {
            fft[(max-r)*height+(max-c)] = conj(fft[r*height+c]);
        }
    }
    free(cp);
}

/** Complex Conjugate Transpose of a matrix, in place.
 * Switch column and row order and invert sign on the complex.
 * Equivalent to \<array\>=\<array\>' in Matlab
 */
void MP_Gabor::complexConjugateTranspose( myComplex *fft, int width, int height )
{
    // Can't transpose unless it is a square matrix, we need some way to return an error, though
    if( width != height )
        return;

//    for( int x=0; x< count; ++x )
//    {
//        fft[x] = std::conj( fft[x] );
//    }

    for ( int i = height-1; i >= 0; i-- )
        for ( int j = 0; j < i; j++ )
        {
            myComplex swapf = fft[i*width+j];
            fft[i*width+j] = conj(fft[j*height+i]);
            fft[j*height+i] = conj(swapf);
        }

    // Now conjugate the diagonal which was skipped in the previous loops
    for ( int i = 0; i < width; ++i )
    {
        fft[i*width+i] = conj(fft[i*width+i]);
    }
}

/** Multiply img_star (fft results) by one Gabor kernel.
 * Put the results in intermediate.
 */
void *MP_Gabor::kernelMultiply( int ni, int mu )
{
	static MP_GaborKernels<double> kernels(96,96);

    int count = patchWidth * patchHeight;
    // Grab a kernel from a cpp file generated from the matlab data.
    const myComplex *psi_star = kernels.getKernel( ni, mu ); // kernels96x96_ker_n1_0;

//    printTen( comp, 96, 96 );
//    printTen( psi_star, 96, 96 );

    for( int x=0; x< count; ++x )
    {
        intermediate[x] = img_star[x] * psi_star[x];
    }

    return NULL;
}

/** Shift the zero frequency to the center of the array.
 * Sometimes called fft normalization.
 */
void MP_Gabor::fftshift()
{
    if( !intermediate )
        return;

    // We are assuming that width == height
    if( patchWidth%2 ) // Odd
    {
        // The odd half hasn't been tested, since all of our current data is even (96x96)
        int half = (patchWidth - 1) / 2;
        for( int row = 0; row < patchHeight; row++ )
        {
            myComplex temp( intermediate[ row*patchWidth + half ] );
            for( int col = 0; col < half; col++ )
            {
                int left = row*patchHeight + col;
                int right = row*patchHeight + col + half;
                intermediate[ right ] = intermediate[ left ];
                intermediate[ left ] = intermediate[ right+1 ];
            }
            intermediate[ row*patchWidth + patchWidth - 1 ] = temp;
        }
        for( int col = 0; col < patchWidth; col++ )
        {
            myComplex temp( intermediate[ half*patchHeight + col ] );
            for( int row = 0; row < half; row++ )
            {
                int top = row*patchHeight + col;
                int bottom = (row+half)*patchHeight + col;
                intermediate[ bottom ] = intermediate[ top ];
                intermediate[ top ] = intermediate[ bottom+1 ];
            }
            intermediate[ (patchHeight-1)*patchWidth + col ] = temp;
        }
    }
    else
    {
        int half = patchWidth / 2;
        for( int row = 0; row < half; row++ )
        {
            for( int col = 0; col < half; col++ )
            {
                int top = row*patchHeight + col;
                int bottom = (row+half)*patchHeight + (col+half);
                myComplex temp( intermediate[ top ] );
                intermediate[ top ] = intermediate[ bottom ];
                intermediate[ bottom ] = temp;
            }
        }
        for( int row = half; row < patchHeight; row++ )
        {
            for( int col = 0; col < half; col++ )
            {
                int bottom = row*patchHeight + col;
                int top = (row-half)*patchHeight + (col+half);
                myComplex temp( intermediate[ top ] );
                intermediate[ top ] = intermediate[ bottom ];
                intermediate[ bottom ] = temp;
            }
        }
    }
}

/** Absolute value of the complex intermediate array.
 * Input: intermediate (complex<double>)
 *
 * Output: abTemp (double)
 */
void MP_Gabor::abs()
{
    int count = patchWidth * patchHeight;

    if( !intermediate )
        return;

    if( !absTemp )
        absTemp = (double *)malloc( sizeof(double) * count );

    for( int x=0; x< count; ++x )
    {
        // |z| = sqrt( real^2 + imag^2 )
		double real = intermediate[x].real();
		double imag = intermediate[x].imag();
        absTemp[x] = sqrt( (real*real) + (imag*imag) );
//		absTemp[x] = hypot( intermediate[x].real(), intermediate[x].imag() );

//		absTemp[x] = std::abs(intermediate[x]); // this works but is much slower than doing it manually.
    }
}

/** Normalize output of Gabor step.
 * From Gwen: gx=gx/(gx*gx') or gx=(inv(gx*gx')*gx')'
 *
 * From Marni: I had a look at the normalization step and it's actually pretty
 * straightforward. In Gwen's notation, gx is a row vector, so the denominator
 * is an inner product which is just a scalar.
 *
 * -# For each of the 72 Gabor filters, compute the sum squared output. So for
 * 96x96 images, ss = gx(1)^2 + ... + gx(9216)^2. (ss is a scalar).
 * -# Divide each element of gx by ss.
 *
 * In essence this is length normalization but she didn't take a square root,
 * so she's dividing by squared length.
 *
 * Input: absTemp
 *
 * Output: absTemp
 */
void MP_Gabor::postGaborNormalize()
{
    int count = patchWidth * patchHeight;
	double ss = 0.0;

	// See http://tuvix.apple.com/documentation/Performance/Conceptual/vDSP/ref_chap/chapter_4.1_section_127.html#//apple_ref/c/func/vDSP_svesq
	// for one replacement for the sum of squares loop.
	for( int index=0; index < count; ++index )
	{
		ss += absTemp[index] * absTemp[index];
	}
    ss = sqrt(ss);

	for( int index=0; index < count; ++index )
	{
		absTemp[index] /= ss;
	}
}

/** Multiply absTemp by one set of SVM weights.
 * Return the results for this step.
 */
std::vector<double> MP_Gabor::svmWeight( int ni, int mu, std::string weightsCategory )
{
    mp_SVMWeights *SVMWeights = mp_SVMWeights::getSVMWeights( weightsCategory );
    const double *weights = SVMWeights->getWeights( ni, mu );
    unsigned int count = SVMWeights->getCount();
	std::vector<double> buf(count,0.0);
    double *tmpYhat = &buf.front();

    if( !weights )
    {
        loadError = true;
        return std::vector<double>( count, 0.0 );
    }

    // If yhat were a simple array, then we could do the sum-ing in place.
    // But that might complicate it if/when we switch to multiple threads

    double alpha = 1.0;
    double beta = 0.0;
    int n = count;
    int m = 96*96;
    int lda = n;
    int incX = 1; // No idea what this is for
    int incY = 1; // No idea what this is for
    // see http://www.netlib.org/clapack/cblas/dgemm.c for details
    cblas_dgemv( CblasRowMajor, CblasTrans, m, n,
                 alpha, weights, lda, absTemp, incX, beta, tmpYhat, incY);

	return buf;
}

void
MP_Gabor::loadAllSVMWeights()
{
    mp_SVMWeights *weights = mp_SVMWeights::getSVMWeights();
	weights->loadAllWeights();
}


void printTen( const myComplex *matrix, int width, int height )
{
    for(int r = 0; r < 10; ++r)
    {
        for (int c = 0; c < 10; ++c)
        {
            std::cout << matrix[r*height+c];
        }
        std::cout << std::endl;
    }
    std::cout << std::endl;
}

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

void printStringVector( std::vector<std::string> &results )
{
    std::cout << std::endl;
    for( unsigned int i=0; i < results.size(); ++i )
    {
        std::cout << results[i] << std::endl;
    }
    std::cout << std::endl;
}