#include "MPT_GaborPyramid.h"

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


namespace mpt
{

GaborPyramid::GaborPyramid() : 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( 0.5 * dOmega );
	nDir = 180.0 / dTheta;
}

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

}

/* Fills pixels and dblPixels with a padded version of the input,
 * then calls fft, which fills in img_star
 */
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();
    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) );
//			rawFilter=spGabor(omega, F, Ka, Kb, szx, szy);
//			rawFilters[f][d]=imresize(rawFilter, 0.5^(f-1));
		}
	}
}

void
GaborPyramid::spGabor( double omega, double F )
{
//// 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);]
//    C = sqrt(log(2)/pi);
//    a = Ka*F/C;
//	b = Kb*F/C;
//	f = F*[cosd(omega); sind(omega)];
//
//// rotation, scale
//	mscale = diag([a b]);
//	mrot = [cosd(omega) sind(omega); -sind(omega) cosd(omega)];
//	mtran =mscale*mrot; 
//
////rasterize
//	nDeg= 4; //FoV measured in degree, here we assume face is about 4 deg viewed in regular distance.
////dx = 1/24;
////dy = 1/24;
//	dx = nDeg/szx;
//	dy = nDeg/szy;
//
//	xtick = [(-nDeg/2+dx):dx:nDeg/2];
//	ytick = [(-nDeg/2+dx):dy:nDeg/2];
//
//	frot = inv(mtran)'*f;
//	c = exp(-pi * frot' * frot);		//zero mean constant
//	for ix = 1:length(xtick);
//		for iy =1:length(ytick)
//			pos = [xtick(ix);
//            ytick(iy)];
//			h(iy, ix) = exp( j * 2 * pi * ( pos' * f )) - c;
//
//			xrot = mtran*pos;
//			w(iy, ix) = exp(-pi * xrot' * xrot);
//		end
//	end
//
//	g =  h.*w;
//	g = g/norm(g);
//    return g;
}

} // end namespace mpt