/*! 
*   \file  CFST.cpp
*   \brief Defines class to calculate FST features.
*    
*   \details  
*   Based on 2001 paper by Oliva and Torralba.
*
*   \date      March 16, 2012
*   \copyright eBay Research Labs.
*/

// Include standard C++ headers.  
//#include <algorithm>

// Include openCV-specific headers.
#include <highgui.h>
#include <opencv2/contrib/contrib.hpp> // For Timer.

#include "CFST.h"
#include "Fourier.h"

// Namespaces.
using namespace std;
using namespace cv;


/*!
*   \brief Constructor for CFST.
*
*   \param [in]  dimBlocks  Dimensions of image blocks. dimBlocks[0] = #rows, dimBlocks[1] = #columns.
*   \param [in]  numScales  #scales to extract features from. Default = 4.
*   \param [in]  numAngles  #orientations to extract features from. Default = 8
*/
CFST::CFST(const int dimBlocks[], int numScales, int numAngles)
{
    m_DimBlocks[0] = dimBlocks[0]; // #Rows
    m_DimBlocks[1] = dimBlocks[1]; // #Columns
    m_numScales    = numScales;
    m_numAngles    = numAngles;
}


/*!
*   \brief Destructor for CFST.
*/
CFST::~CFST()
{
}


/*!
*   \brief Extract features for the given block of image.
*
*   \param [in]  gray       Single channel image. Type is CV_32F.
*   \param [in]  paddedDim  Dimension at which Fourier Transform will be taken.
*   \return Features for the given block of image.
*/
Mat CFST::FeaturesForBlock(const Mat &gray, int paddedDim)
{
    // Initialize.
    Mat ftrsMatrix = Mat::zeros(m_numScales, m_numAngles, CV_32F);
    Mat binArea    = Mat::zeros(m_numScales, m_numAngles, CV_32F); // Area of bins in Fourier space.

    // Get dimensions.
    int height = gray.rows;
    int width  = gray.cols;
    int n      = (height > width) ? height : width;

    CV_Assert( n <= paddedDim ); // Make sure size does not exceed paddedDim.

    n = paddedDim;
    //n += (n - 2 * (n/2)); // Make even.

    // Zero-pad to make nxn. TODO: Allow non-square matrix.
    Mat img;
    copyMakeBorder(gray, img, 0, n-height, 0, n-width, BORDER_CONSTANT, Scalar(0));

    // Apply Fourier Transform and then logarithm map on the magnitude.
    Mat fourier;
    log( 1 + Fourier::FourierMagnitude(img), fourier );
    
    // Define Top-left coordinates of quadrants. 
    // Top-left, top-right, bottom-left, bottom-right, respectively.
    int       nc   = n/2;
    const int qx[] = {0, nc,  0, nc}; 
    const int qy[] = {0,  0, nc, nc};

    // Quadrants (in the usual sense) are swapped 1<->3 and 2<->4.
    // Define start and end indices accordingly.
    const int x1[] = {0, -nc,   0, -nc};
    const int x2[] = {nc,  0,  nc,   0};
    const int y1[] = {0,   0, -nc, -nc};
    const int y2[] = {nc, nc,   0,   0};

    // Radial and angular widths.
    const float dr     = (float) nc / (float) m_numScales;
    const float dtheta = (float) CV_PI / (float) m_numAngles;

    vector < float > rupper;
    rupper.resize(m_numScales);
    vector < float >::reverse_iterator iter = rupper.rbegin();
    float s = 1.0;
    for (; iter!=rupper.rend(); iter++)
    {
        *iter = s;
        s    /= 2.0;
    }

    // Treat one quadrant at a time since the quadrants are swapped. .
    // TODO: Use look up table for speed up to calculate r-theta (especially for square image).
    for (int qi=0; qi<4; qi++)
    {
        // Current quadrant.
        Mat imgQuadrant(fourier(Rect(qx[qi], qy[qi], nc, nc)));

        MatIterator_<float> it = imgQuadrant.begin<float>();
        for (int i=y1[qi]; i<y2[qi]; i++)
        {
            for (int j=x1[qi]; j<x2[qi]; j++)
            {
                int ri = (int) floor(sqrt( (float) i * (float) i + (float) j * (float) j ) / dr);

                //float r  = sqrt( (float) i * (float) i + (float) j * (float) j ) / nc;
                //int   ri = m_numScales;
                //for (int k=0; k<m_numScales; k++)
                //{
                //    if (r <= rupper[k]) 
                //    {
                //        ri = k;
                //        break;
                //    }
                //}

                if (ri >= m_numScales) continue; // This is necessary since ri can be greater than m_numScales.

                int ti = (int) floor((0.5+atan2((float) i, (float) j)) / dtheta); // atan2 returns values in [-pi, pi]. Add 0.5 for offset of dtheta/2.
                if (ti < 0) continue;
                if (ti >= m_numAngles) ti = m_numAngles-1; // Border case - happens sometimes, especially since we are using float version of CV_PI.

                binArea.at<float>(ri, ti)++;            // Increment count.
                ftrsMatrix.at<float>(ri, ti) += *it++;  // Acccumulate magnitude of spectrum.
            }
        }
    }

    // Get average value in each polar bin.
    divide(ftrsMatrix, binArea, ftrsMatrix); // Note that binArea has non-zero values;

    // Copy to a row vector.
    Mat ftrs = Mat::zeros(1, m_numScales*m_numAngles, CV_32F); // Order: Angles first, then repeat for various scales.
    MatIterator_<float> it = ftrs.begin<float>();
    for (int i=0; i<m_numScales; i++)
    {
        for (int j=0; j<m_numAngles; j++)
        {
            *it++ = ftrsMatrix.at<float>(i,j);
            //cerr << i << ", " << j << ":: " << *it << endl;
            //it++;
        }
    }

    return ftrs;
}


/*!
*   \brief Extract features for the given block of image.
*
*   \param [in]  lum       Single channel image.
*   \return Preprocessed image. Type = CV_32F.
*/
Mat CFST::Preprocess(const Mat &lum)
{
    Mat_<float> img(lum);
    log((1+img), img); // Apply logarithm to enhance low contrast regions.
    normalize(img, img, 0, 1, CV_MINMAX); // Normalize image to have range in [0, 1]
    return img; 

/*
// The following is the implementation of prefilter as in GIST Matlab code.
TickMeter timer;
timer.start();
    int w    = 5;
    float fc = 4;
    float s1 = fc / sqrt((float) std::log(2.0));
    s1 *= s1;

    Mat_<float> img;
    normalize(lum, img, 0, 1, CV_MINMAX, CV_32F); 

    log((1+img), img);

    // Pad image to reduce boundary artifacts.
    Mat padded;
    copyMakeBorder(img, padded, w, w, w, w, BORDER_REFLECT);

    // Pad image to make it square, as well as so that #rows and #cols are even.
    int height = padded.rows;
    int width  = padded.cols;
    int n      = (height > width) ? height : width;
    n          = n + (n - 2 * (n/2));

    copyMakeBorder(padded, img, 0, n-height, 0, n-width, BORDER_REFLECT);

    Mat gaussFourier = FourierOfGaussian(n, s1);
    Mat lowPass      = ApplyFilter(img, gaussFourier, false);
    Mat highPass     = img - lowPass;
    Mat highPass2    = highPass.mul(highPass);

    Mat denominator;
    sqrt(ApplyFilter(highPass2, gaussFourier, true), denominator);
    denominator += 0.2;

    Mat result;
    divide(highPass, denominator, result);

    Rect valid(w, w, width-2*w-1, height-2*w-1);
    normalize(result(valid), result, 0, 1, CV_MINMAX); 


timer.stop();
cerr << "Time to preprocess (" << lum.rows << " x " << lum.cols << " --> " << result.rows << " x " << result.cols << "): " << timer.getTimeMilli() << " ms" << endl;
cerr << "Mean of preprocessed image = " << mean(result)[0] << endl;
//imshow("Preprocessed", result);
//waitKey(0);

    return result;
    */
}


/*!
*   \brief Extract features for the given image.
*
*   \param [in]  lum   Single channel image.
*   \param [out] ftrs  Features extracted from the image.
*/
void CFST::CalculateFeatures(const Mat &img, Mat &ftrs)
{
    //TickMeter timer;
    //timer.start();

    // Convert to single channel luminance image.
    Mat gray;
    if (img.channels()==3)
    {
        cvtColor(img, gray, CV_RGB2GRAY);    
    }
    else
    {
        gray = img;
    }

    // Preprocess image to enhance contrast.
    Mat lum = Preprocess(gray);

    // Create and initialize container for features.
    int numBlocks           = m_DimBlocks[0] * m_DimBlocks[1];
    int numfeaturesPerBlock = m_numScales    * m_numAngles;
    ftrs = Mat::zeros(numBlocks, numfeaturesPerBlock, CV_32F);
    
    // Get dimensions of image.
    int height = lum.rows;
    int width  = lum.cols;

    // Get dimensions of each block image.
    int h = height / m_DimBlocks[0];
    int w = width  / m_DimBlocks[1];

    // Scan blocks of image row-wise, and extract features from each block. 
    int blockID   = 0;
    int paddedDim = 128 / MIN(m_DimBlocks[0], m_DimBlocks[1]); // Please use the same maxDim in the numerator (here, 128), as used by CFeatureExtraction.
    paddedDim    += (paddedDim - 2 * (paddedDim/2)); // Make even.

    for (int ri=0; ri<m_DimBlocks[0]; ri++)
    {
        // Last block can have different value than the rest.
        int hi = h; //(ri==m_DimBlocks[0]-1) ? (h + height - m_DimBlocks[0]*h) : h;
        for (int ci=0; ci<m_DimBlocks[1]; ci++)
        {
            // Last block can have different value than the rest.
            int wi = w; //(ci==m_DimBlocks[1]-1) ? (w + width - m_DimBlocks[1]*w) : w;

            // Define rectangular block.
            Rect rect(ci*w, ri*h, wi, hi);

            // Extract features from current block.
            Mat currFtrs = FeaturesForBlock(lum(rect), paddedDim);
            currFtrs.copyTo(ftrs.row(blockID++));
        }
    }


    //timer.stop();
    //cerr << "Total time (" << lum.rows << " x " << lum.cols << "): " << timer.getTimeMilli() << " ms" << endl;
    //cerr << sum(ftrs)(0) << endl;

    //// For debugging: Print values.
    //blockID = 0;
    //for (int ri=0; ri<m_DimBlocks[0]; ri++)
    //{
    //    for (int ci=0; ci<m_DimBlocks[1]; ci++)
    //    {
    //        Mat currFtrs = ftrs.row(blockID++);
    //        for (int i=0; i<numfeaturesPerBlock; i++)
    //            cerr << blockID << "," << i << ": " << currFtrs.at<float>(i) << endl;
    //        cerr << endl;
    //    }
    //}
    return;
}