#include "MPT_GaborPyramid.h"

#include <string>
#include <iomanip>
#include <fstream>
#include <sstream>
#include <cmath>
#include <limits>

#include "matlab_norm.h"
#include "matlab_imresize.h"

#include "rimage_ser.h"

static const double my_pi = 3.1415926535897;

static inline float deg2rad( float degrees )
{
    return (degrees / 360.0) * (2 * my_pi);
}

namespace mpt
{


GaborPyramid::GaborPyramid( const RImage<float> &_pixels, int width, int height, bool _sub_proc ) : MP_Gabor(_pixels, width, height), Fmax(8.0), dF(1.4), dOmega(40), dTheta(22.5), szx(96), szy(96), nLayers(5)
{
	Ka = ( pow(2.0, dF) - 1) / ( pow( 2.0, dF) + 1);
	Kb = tan( deg2rad( 0.5 * dOmega ) );
	nDir = 180.0 / dTheta;

    baseLayer();
}

bool
GaborPyramid::importPaddedFilters( const std::string &file )
{
    std::ifstream ifs( file.c_str() );
    if( ifs.good() )
    {
        int layers, freqs;
        if( !getSize( ifs, layers, freqs ) )
        {
            std::cerr << "Unable to load Gabor Pyramid size." << std::endl;
            return false;
        }
        if( (layers != nLayers) || (freqs != Fmax) )
        {
            std::cerr << "Gabor Pyramid size does not match default." << std::endl;
            return false;
        }

        paddedFilters.resize( nLayers );
        for( int layeri = 0; layeri < nLayers; ++layeri )
        {
            paddedFilters[layeri].resize( Fmax );
            for( int fi = 0; fi < Fmax; ++fi )
            {
                int fWidth;
                int fHeight;
                if( !getSize( ifs, fWidth, fHeight ) )
                {
                    std::cerr << "Unable to load Gabor Pyramid filter size. Filter " << layeri << "x" << fi << std::endl;
                    return false;
                }

                std::vector<double> oneFilter;
                if( !getOneFilter( ifs, fWidth, fHeight, oneFilter ) )
                {
                    std::cerr << "Unable to load Gabor Pyramid filter. Filter " << layeri << "x" << fi << std::endl;
                    return false;
                }
                // plug oneFilter into the right place in our data structure.
                paddedFilters[layeri][fi] = oneFilter;
            }
        }
    }

    return true;
}

Feature
GaborPyramid::featureFromImage( RImage<float> &patch )
{
/*
    int imgW = patch.width;
    int imgH = patch.height;

    setPixels( patch );
    padFFTImage( patch );

function F=FeatFromImage(X , gb)
			%padding to 2x-1 size, similar to what we did for rawFilter->padFilter
			[imgH  imgW] = size(X);
			padXFFTbase = padFFTImage(X);

			F=[]; %the size of Feature is actuall KNOWN ahead of time, but I'm lazy...
			[nLevel nDir]=size(gb);
			for pyLevel=1:nLevel
				ratio= 2^(pyLevel-1);
				%crop the four corners
				edgeH  = imgH/ratio-1;
                edgeW  = imgW/ratio-1;
				padXFFT = padXFFTbase([1:edgeH end-edgeH:end], [1:edgeW,end-edgeW:end]) / (ratio^2);

				for pyOri=1:nDir
					%convolution filtering and remove the borders
					Fx = ifft2(padXFFT.* gb(pyLevel,pyOri).padFilter);
					Fx = cropCenter(Fx);
					Fmag = abs(Fx(:)');

					%normalize
					subF = Fmag./norm(Fmag,2);

					%accumulate feature
					F=[F subF];

				end %for-orientation
			end %for-level
			F= [F 1];
return
*/
    return Feature();
}

/* Fills pixels and dblPixels with a padded version of the input,
 * then calls fft, which fills in img_star
 * This is partially copied from mp_gabor, but it's probably going to have to be re-written so we can
 *   just pass in a rimage to work on, and return another rimage as output.
 */
void
GaborPyramid::padFFTImage( RImage<float> &in )
{
    int width = in.width;
    int height = in.height;

    width = (width * 2) - 1;
    height = (height * 2) - 1;

    pixels.setSize( width, height );
    pixels.setConst( 0.0 );
    dblPixels.setSize( width, height );
    dblPixels.setConst( 0.0 );

    for( int x = 0; x < in.width; ++x )
    {
        for( int y = 0; y < in.height; ++y )
        {
            float pix = in.getPixel( x, y );
            pixels.setPixel( x, y , pix );
            dblPixels.setPixel( x, y , pix );
        }
    }

    bool oldNorm = normalized;
    normalize();
    normalized = true;
    fft();
    normalized = oldNorm;
}

bool
GaborPyramid::getSize( std::istream &is, int &width, int &height )
{
    std::string line;
    std::getline( is, line );
    std::stringstream numStr( line );
    if( (numStr >> width) && (numStr >> height) )
        return true;

    return false;
}

/*
  We may instead want to use a matrix rather than a vector.
  But I'm not sure how they're going to be used, yet.
*/
bool
GaborPyramid::getOneFilter( std::istream &is, int width, int height, std::vector<double> &oneFilter )
{
    std::string line;
    std::getline( is, line );
    std::stringstream numStr( line );

    int count = width * height;

    for( int ind = 0; ind < count; ++ind )
    {
        double val;
        if( !(numStr >> val) )
            return false;
        oneFilter.push_back( val );
    }

    return true;
}

void
GaborPyramid::baseLayer()
{
	for( int f = 1; f <= nLayers; ++f )
    {
		for( int d = 1; d <= nDir ; ++d )
        {
			double omega = dTheta*(d-1);
			double F = Fmax / pow( 2, (f-1) );
            RImage<myComplex> rawFilter;
//			rawFilter=spGabor(omega, F, Ka, Kb, szx, szy);
			spGabor( omega, F, rawFilter );
#ifdef DEBUG
            if( (1==f) && (1==d) )
            {
                RImage<float> realIM( rawFilter.width, rawFilter.height );
                RImage<float> imagIM( rawFilter.width, rawFilter.height );

                for( int x=0; x < rawFilter.width; ++x )
                {
                    for( int y=0; y < rawFilter.width; ++y )
                    {
                        realIM[x][y] = rawFilter[x][y].real();
                        imagIM[x][y] = rawFilter[x][y].imag();
                    }
                }
                saveRImageToFile( realIM, "rawFilter_real_1_1.rimage" );
                saveRImageToFile( imagIM, "rawFilter_imag_1_1.rimage" );

//                SimpleMatrixOps::display( rawFilter );
            }
#endif
            // We don't actually need to raw filters, so after resizing, run padFFTImage
            RImage<myComplex> resized;
            matlab_imresize( rawFilter, resized, pow(0.5,f-1) );
//			rawFilters[f][d]=imresize(rawFilter, 0.5^(f-1));
		}
	}
}

void
diagonal( std::vector<double> &input, Matrix2d &output )
{
    // resize output
    output.resize( boost::extents[input.size()][input.size()] );
    // set all of output to zero
    SimpleMatrixOps::setAllZero( output );
    // set the diagonal of output to the input vector
    for( unsigned int i = 0; i < input.size(); ++i )
        output[i][i] = input[i];
}

template<class T>
T deg2rad( T degrees )
{
    return (degrees / 360.0) * (2.0 * my_pi);
}

template<class T>
T cosd( T angle )
{
    T radians = deg2rad( angle );
    return cos(radians);
}

template<class T>
T sind( T angle )
{
    T radians = deg2rad( angle );
    return sin(radians);
}


void
GaborPyramid::spGabor( double omega, double F, RImage<myComplex> &rawFilter )
{
//// omega = normal orientation
//// F = frequency
//// Ka = Bandwidth // normal
//// Kb = Bandwidth \perp normal
//// szx = width of the output in pixel
//// szy = height of the output in pixel
//// Gabor Half Magnitude Range
//// Freq: [F-F*Ka F+F*Ka];
//// Orientation bandwidth: [omega-tan-1(Kb) omega+tan-1(Kb);]
    double C = sqrt(log(2)/my_pi);
    double a = (Ka*F)/C;
    double b = (Kb*F)/C;
    Matrix2d f( boost::extents[2][1] );
//	std::vector<double> f(2,0.0);
    f[0][0] = F*cosd(omega); // cosine in degrees rather than radians
    f[1][0] = F*sind(omega);

    Matrix2d mtran;
    {
        // Just localized a couple of temporary variables. Might want to move this to a separate method.
    //// rotation, scale
    //	mscale = diag([a b]);
        Matrix2d mscale;
        std::vector<double> vec(2);
        vec[0] = a;
        vec[1] = b;
        diagonal( vec, mscale );

    //	mrot = [cosd(omega) sind(omega); -sind(omega) cosd(omega)];
        Matrix2d mrot( boost::extents[2][2] );
        mrot[0][0] = cosd(omega);
        mrot[0][1] = sind(omega);
        mrot[1][0] = -sind(omega);
        mrot[1][1] = cosd(omega);

    //	mtran =mscale*mrot;
        SimpleMatrixOps::multiply( mscale, mrot, mtran );
    }

//
////rasterize
	double nDeg = 4.0; //FoV measured in degree, here we assume face is about 4 deg viewed in regular distance.
	double dx = nDeg/szx;
	double dy = nDeg/szy;

	std::vector<double> xtick;
	std::vector<double> ytick;
//	xtick = [(-nDeg/2+dx):dx:nDeg/2];
//	ytick = [(-nDeg/2+dx):dy:nDeg/2];

    for( double val = -nDeg/2+dx; val < nDeg/2; val += dx )
        xtick.push_back( val );
    xtick.push_back( nDeg/2.0 );

    // in the matlab code, ytick starts out using dx, not dy. Not sure if that's a bug or not.
    // doesn't matter right now as dx and dy should be the same
    for( double val = -nDeg/2+dx; val < nDeg/2; val += dy )
        ytick.push_back( val );
    ytick.push_back( nDeg/2.0 );

//  see this link for the inverse of a 2x2 matrix: http://people.richland.edu/james/lecture/m116/matrices/inverses.html
//	frot = inv(mtran)'*f;
    double c;
    {
        Matrix2d frot, frot_tmp, frot2;
        SimpleMatrixOps::inverse( mtran, frot );
        SimpleMatrixOps::transpose( frot, frot_tmp );
        SimpleMatrixOps::multiply( frot_tmp, f, frot );

        SimpleMatrixOps::transpose( frot, frot_tmp );
        SimpleMatrixOps::multiply( frot_tmp, frot, frot2 );
        // check to make sure frot2 is 1x1?
        double tmp = frot2[0][0];
        c = exp( -my_pi * tmp );
//	c = exp(-pi * frot' * frot);		//zero mean constant
    }

//    Matrix2d h( boost::extents[ytick.size()][xtick.size()] );
    rawFilter.setSize( ytick.size(), xtick.size() );
    Matrix2d w( boost::extents[ytick.size()][xtick.size()] );

    Matrix2d pos( boost::extents[2][1] );
    Matrix2d pos_trans( boost::extents[1][2] );
    Matrix2d tmp;
    Matrix2d xrot, xrot_trans;

    myComplex j(0.0,1.0);

	for( unsigned int ix = 0; ix < xtick.size(); ++ix )
    {
		for( unsigned int iy = 0; iy < ytick.size(); ++iy )
        {
            pos[0][0] = xtick[ix];
            pos[1][0] = ytick[iy];
            pos_trans[0][0] = xtick[ix];
            pos_trans[0][1] = ytick[iy];

//			h(iy, ix) = exp( j * 2 * pi * ( pos' * f )) - c;
            SimpleMatrixOps::multiply( pos_trans, f, tmp );
            double tmp_d = tmp[0][0];
            rawFilter[iy][ix] = exp( j * 2.0 * my_pi * tmp_d) - c;

//			xrot = mtran*pos;
            SimpleMatrixOps::multiply( mtran, pos, xrot );

            SimpleMatrixOps::transpose( xrot, xrot_trans );
            SimpleMatrixOps::multiply( xrot_trans, xrot, tmp );
            // check that tmp is 1x1
            w[iy][ix] = exp( -my_pi * tmp[0][0] );
//			w(iy, ix) = exp(-pi * xrot' * xrot);
		}
	}

//	g =  h.*w;
//	g = g/norm(g);
	for( unsigned int ix = 0; ix < xtick.size(); ++ix )
    {
		for( unsigned int iy = 0; iy < ytick.size(); ++iy )
        {
            rawFilter[iy][ix] = rawFilter[iy][ix] * static_cast<double>(w[iy][ix]);
        }
    }

//    Matrix2d U, S, V;
//    SimpleMatrixOps::svd( h, U, S, V );
//    double norm = std::numeric_limits<double>::min();
//    for( unsigned int i = 0; i < S.shape()[1]; ++i )
//    {
//        double tmp = S[0][i];
//        if( tmp > norm ) norm = tmp;
//    }
    double norm = matlab_norm( rawFilter );

//    rawFilter.resize( boost::extents[ytick.size()][xtick.size()] );
	for( unsigned int ix = 0; ix < xtick.size(); ++ix )
    {
		for( unsigned int iy = 0; iy < ytick.size(); ++iy )
        {
            rawFilter[iy][ix] = rawFilter[iy][ix] / norm;
        }
    }
}

} // end namespace mpt