/* *-* tab-width: 4; c-basic-offset: 4; -*- */
/*
* Copyright (C) 2007-2008 Anael Orlinski
*
* This file is part of Panomatic.
*
* Panomatic is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2 of the License, or
* (at your option) any later version.
*
* Panomatic is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Panomatic; if not, write to the Free Software
* <http://www.gnu.org/licenses/>.
*/

#include <iostream>

#ifdef _MSC_VER
#define _USE_MATH_DEFINES
#endif
#include <math.h>
#include <vector>
#include <map>
#include <fstream>
#include <cassert>

#include <string.h>

#include "KeyPoint.h"
#include "CircularKeyPointDescriptor.h"
#include "MathStuff.h"
#include "WaveFilter.h"

//#define DEBUG_DESC
//#define DEBUG_ROT

using namespace lfeat;
using namespace std;

LUT<0, 83> Exp1_2(exp, 0.5, -0.08);

CircularKeyPointDescriptor::CircularKeyPointDescriptor(Image& iImage,
        std::vector<int> rings, std::vector<double>ring_radius,
        std::vector<double>ring_gradient_width,
        int ori_nbins, double ori_sample_scale, int ori_gridsize) :
    _image(iImage), _ori_nbins(ori_nbins), _ori_sample_scale(ori_sample_scale),
    _ori_gridsize(ori_gridsize)
{
    // default parameters
    if (rings.size() == 0)
    {
        rings.push_back(1);
        ring_radius.push_back(0);
        ring_gradient_width.push_back(2);
        rings.push_back(6);
        ring_radius.push_back(4.2);
        ring_gradient_width.push_back(1.9);
        /*
        rings.push_back(6);
        ring_radius.push_back(5);
        ring_gradient_width.push_back(3);
        */
        rings.push_back(6);
        ring_radius.push_back(8);
        ring_gradient_width.push_back(3.2);
    }

    assert(rings.size() == ring_radius.size());
    assert(rings.size() == ring_gradient_width.size());

    /*
    // radius of the rings (ring 0 is the center point)
    double ring_radius[3];
    ring_radius[0] =  0;
    ring_radius[1] =  2;
    ring_radius[2] =  5;

    // width of the boxfilter used during the computation (ring 0 is the center point)
    double ring_gradwidth[3];
    ring_gradwidth[0] = 0.5;
    ring_gradwidth[1] = 1.5;
    ring_gradwidth[2] = 2.5;

    // number of entries for each ring
    int nrings[3];
    nrings[0] = 1;
    nrings[1] = 6;
    nrings[2] = 6;
    */
    // compute number of sampling points
    _subRegions = 0;
    for (unsigned int i=0; i < rings.size(); i++)
    {
        _subRegions += rings[i];
    }

    // create a list of sample parameters
    _samples = new SampleSpec[_subRegions];

    // precompute positions of the sampling points
    int j=0;
    for (unsigned int i=0; i < rings.size(); i++)
    {
        // alternate the phi offset of the rings,
        // so that the circles don't overlap too much with the
        // next ring
        double phioffset = i % 2== 0 ? 0 : M_PI/rings[i];
        for (int ri=0; ri < rings[i]; ri++)
        {
            double phi = ri*2*M_PI /rings[i] + phioffset;
            _samples[j].x = ring_radius[i]*cos(phi);
            _samples[j].y = ring_radius[i]*sin(phi);
            _samples[j].size = ring_gradient_width[i];
            j++;
        }
    }

    // compute 4 gradient entries (pos(dx), pos(-dx), pos(dy), pos(-dy))
    // maybe use something else here, for example, just the gradient entries...
    // also use LBP as a 5th feature
    _vecLen = 3;
    _descrLen = _vecLen * _subRegions - 1;

    _ori_hist = new double [_ori_nbins + 2];

}

CircularKeyPointDescriptor::~CircularKeyPointDescriptor()
{
    delete[] _ori_hist;
    delete[] _samples;
}

void CircularKeyPointDescriptor::makeDescriptor(KeyPoint& ioKeyPoint) const
{
    // create a descriptor context
    //KeyPointDescriptorContext aCtx(_subRegions, _vecLen, ioKeyPoint._ori);

    // create the storage in the keypoint
    if (!ioKeyPoint._vec)
    {
        ioKeyPoint.allocVector(getDescriptorLength());
    }

    // create a vector
    createDescriptor(ioKeyPoint);

    // normalize
    Math::Normalize(ioKeyPoint._vec, getDescriptorLength());
}

int CircularKeyPointDescriptor::assignOrientation(KeyPoint& ioKeyPoint, double angles[4]) const
{
    double* hist = _ori_hist+1;
    unsigned int aRX = Math::Round(ioKeyPoint._x);
    unsigned int aRY = Math::Round(ioKeyPoint._y);
    int aStep = (int)(ioKeyPoint._scale + 0.8);

    WaveFilter aWaveFilter(_ori_sample_scale * ioKeyPoint._scale + 1.5, _image);

#ifdef DEBUG_ROT_2
    std::cerr << "ori_scale = " << 2.5 * ioKeyPoint._scale + 1.5 << std::endl;
    std::cerr << "ori= [ ";
#endif

#ifdef _MSC_VER
#pragma message("use LUT after parameter tuning")
#else
#warning use LUT after parameter tuning
#endif
    double coeffadd = 0.5;
    double coeffmul = (0.5 + 6 ) / - (_ori_nbins*_ori_nbins);

    memset(_ori_hist, 0, sizeof(double)*(_ori_nbins + 2));
    // compute haar wavelet responses in a circular neighborhood of _ori_gridsize s
    for (int aYIt = -_ori_gridsize; aYIt <= _ori_gridsize; aYIt++)
    {
        int aSY = aRY + aYIt * aStep;
        for (int aXIt = -_ori_gridsize; aXIt <= _ori_gridsize; aXIt++)
        {
            int aSX = aRX + aXIt * aStep;
            // keep points in a circular region of diameter 6s
            unsigned int aSqDist = aXIt * aXIt + aYIt * aYIt;
            if (aSqDist <= _ori_nbins*_ori_nbins && aWaveFilter.checkBounds(aSX, aSY))
            {
                double aWavX = aWaveFilter.getWx(aSX, aSY);
                // y axis for derivate seems is from bottom to top (typical math coordinate system),
                // not from top to bottom (as in the typical image coordinate system).
                double aWavY = - aWaveFilter.getWy(aSX, aSY);
                double aWavResp = sqrt(aWavX * aWavX + aWavY * aWavY);
                if (aWavResp > 0)
                {
                    // add PI ->  0 .. 2*PI interval
                    double angle = atan2(aWavY, aWavX)+PI;
                    int bin =  angle/(2*PI) * _ori_nbins;
                    // deal with possible rounding problems.
                    bin = (bin+_ori_nbins)%_ori_nbins;
                    // center of bin 0 equals -PI + 16°deg, etc.
                    double weight = exp(coeffmul * (aSqDist+coeffadd));
                    hist[bin] += aWavResp * weight;
                    //hist[bin] += aWavResp * Exp1_2(aSqDist);
#ifdef DEBUG_ROT_2
                    std::cerr << "[ " << aSX << ", " << aSY << ", "
                              << aWavX << ", " << aWavY << ", "
                              << aWavResp << ", " << angle << ", " << bin << ", "
                              << aSqDist << ", " << aWavResp* Exp1_2(aSqDist) << "], " << std::endl;
#endif
                }
            }
        }
    }
#ifdef DEBUG_ROT_2
    std::cerr << "]" << endl;
#endif

#if 0
    // smoothing doesn't seem to work very well with the default grid + scale values for orientation histogram building
    // smooth histogram
    for (int it=0; it < 1; it++)
    {
        double prev = hist[_ori_nbins-1];
        double first = hist[0];
        for (int i=0; i < _ori_nbins-2; i++)
        {
            double hs = (prev + hist[i] + hist[i+1]) / 3.0;
            prev = hist[i];
            hist[i] = hs;
        }
        hist[_ori_nbins-1] = (prev + hist[_ori_nbins-1] + first) / 3.0;
    }
#endif

    // avoid boundary problems, wrap around histogram
    hist[-1] = hist[_ori_nbins-1];
    hist[_ori_nbins] = hist[0];

    // find bin with the maximum response.
    double aMax = hist[0];
    int iMax = 0;
#ifdef DEBUG_ROT
    std::cerr << "rot_hist: [ " << aMax;
#endif
    for (int i=1; i < _ori_nbins; i++)
    {
#ifdef DEBUG_ROT
        std::cerr << ", " << hist[i];
#endif
        if (hist[i] > aMax)
        {
            aMax = hist[i];
            iMax = i;
        }
    }
#ifdef DEBUG_ROT
    std::cerr << " ] " << std::endl;
#endif

    double prev = hist[iMax-1];
    double curr = hist[iMax];
    double next = hist[iMax+1];
    double dsub = -0.5*(next-prev)/(prev+next-2*curr);
    ioKeyPoint._ori = (iMax+0.5+dsub) / _ori_nbins * 2*PI - PI;

    // add more keypoints that are within 0.8 of the maximum strength.
    aMax *= 0.8;
    int nNewOri = 0;
    for (int i=0; i < _ori_nbins; i++)
    {
        double prev = hist[i-1];
        double curr = hist[i];
        double next = hist[i+1];
        if (curr > aMax && prev < curr && next < curr && i != iMax)
        {
            dsub = -0.5*(next-prev)/(prev+next-2*curr);
            angles[nNewOri] = (i+0.5+dsub) / _ori_nbins * 2*PI - PI;
            nNewOri++;
            if (nNewOri == 4)
            {
                break;
            }
        }
    }
    return nNewOri;
}

// gradient and intensity difference
void CircularKeyPointDescriptor::createDescriptor(KeyPoint& ioKeyPoint) const
{
#ifdef DEBUG_DESC
    std::ofstream dlog("descriptor_details.txt", std::ios_base::app);
#endif

    // create the vector of features by analyzing a square patch around the point.
    // for this the current patch (x,y) will be translated in rotated coordinates (u,v)

    double aX = ioKeyPoint._x;
    double aY = ioKeyPoint._y;
    int aS = (int)ioKeyPoint._scale;

    // get the sin/cos of the orientation
    double ori_sin = sin(ioKeyPoint._ori);
    double ori_cos = cos(ioKeyPoint._ori);

    if (aS < 1)
    {
        aS = 1;
    }

    // compute the gradients in x and y for all regions of interest

    // we override the wave filter size later
    WaveFilter aWaveFilter(10, _image);

    // compute features at each position and store in feature vector
    int j=0;
    double middleMean = 0;

    for (int i=0; i < _subRegions; i++)
    {
        // scale radius with aS.
        double xS = _samples[i].x * aS;
        double yS = _samples[i].y * aS;
        // rotate sample point with the orientation
        double aXSample = aX + xS * ori_cos - yS * ori_sin;
        double aYSample = aY + xS * ori_sin + yS * ori_cos;
        // make integer values from double ones
        int aIntXSample = Math::Round(aXSample);
        int aIntYSample = Math::Round(aYSample);
        int aIntSampleSize = Math::Round(_samples[i].size* aS);
        int sampleArea = aIntSampleSize * aIntSampleSize;

        if (!aWaveFilter.checkBounds(aIntXSample, aIntYSample, aIntSampleSize))
        {
            ioKeyPoint._vec[j++] = 0;
            ioKeyPoint._vec[j++] = 0;
            //ioKeyPoint._vec[j++] = 0;
            //ioKeyPoint._vec[j++] = 0;
            if (i > 0)
            {
                ioKeyPoint._vec[j++] = 0;
            }
#ifdef DEBUG_DESC
            dlog << xS << " " << yS << " "
                 << aIntXSample << " " << aIntYSample << " " << aIntSampleSize << " "
                 << 0 << " " << 0 << " " << 0 << " " << 0 << " " << 0 << std::endl;
#endif
            continue;
        }

        double aWavX = aWaveFilter.getWx(aIntXSample, aIntYSample, aIntSampleSize) / sampleArea;
        double aWavY = - aWaveFilter.getWy(aIntXSample, aIntYSample, aIntSampleSize) / sampleArea;

        double meanGray = aWaveFilter.getSum(aIntXSample, aIntYSample, aIntSampleSize) / sampleArea;

        if (i == 0)
        {
            middleMean = meanGray;
        }

        // rotate extracted gradients

        //  need to rotate in the other direction?

        double aWavXR = aWavX * ori_cos + aWavY * ori_sin;
        double aWavYR = -aWavX * ori_sin + aWavY * ori_cos;
        /*
        double aWavXR = aWavX * ori_cos - aWavY * ori_sin;
        double aWavYR = aWavX * ori_sin + aWavY * ori_cos;
        */

#ifdef DEBUG_DESC
        dlog << xS << " " << yS << " "
             << aIntXSample << " " << aIntYSample << " " << aIntSampleSize << " "
             << aWavX << " " << aWavY << " " << meanGray << " " << aWavXR << " " << aWavYR << std::endl;
#endif

        // store descriptor
        ioKeyPoint._vec[j++] = aWavXR;
        ioKeyPoint._vec[j++] = aWavYR;
        /*
        if (aWavXR > 0) {
        	ioKeyPoint._vec[j++] = aWavXR;
        	ioKeyPoint._vec[j++] = 0;
        } else {
        	ioKeyPoint._vec[j++] = 0;
        	ioKeyPoint._vec[j++] = -aWavXR;
        }
        if (aWavYR > 0) {
        	ioKeyPoint._vec[j++] = aWavYR;
        	ioKeyPoint._vec[j++] = 0;
        } else {
        	ioKeyPoint._vec[j++] = 0;
        	ioKeyPoint._vec[j++] = -aWavYR;
        }
        */
        if (i != 0)
        {
            ioKeyPoint._vec[j++] = meanGray - middleMean;
        }
    }
#ifdef DEBUG_DESC
    dlog.close();
#endif

}
