/**
* @file FastFilter.cpp
* @brief color gradient
* @author Erik Rodner
* @date 07/22/2008

*/
#include <iostream>
#include <math.h>

#include "core/basics/FastMath.h"
#include "FastFilter.h"

using namespace std;
using namespace NICE;
using namespace OBJREC;

/** global lookup tables */

template <class SrcValueType, class DstValueType>
void FastFilter::calcGradientX ( const SrcValueType *img, int xsize, int ysize, DstValueType *d )
{
    int k = 0;
    for ( int y = 0 ; y < ysize ; y++ )
    {
	d[k] = 0.0;
	k++;
	for ( int x = 1 ; x < xsize-1 ; x++,k++ )
            d[k] = - img[k-1] + img[k+1];
	d[k] = 0.0;
	k++;
    }
}

template <class SrcValueType, class DstValueType>
void FastFilter::calcGradientY ( const SrcValueType *img, int xsize, int ysize, DstValueType *d )
{
    int k = xsize;
    for ( int x = 0 ; x < xsize ; x++ )
	d[x] = 0.0;
    for ( int x = 0 ; x < xsize ; x++ )
	d[x+xsize*(ysize-1)] = 0.0;

    for ( int y = 1 ; y < ysize-1; y++ )
	for ( int x = 0 ; x < xsize ; x++,k++ )
            d[k] = - img[k-xsize] + img[k+xsize];
}

template <class GrayValueType, class GradientMagnitudeType, class GradientDirectionType>
void FastFilter::calcColorGradient (
			  const GrayValueType *r,
			  const GrayValueType *g,
			  const GrayValueType *b,
			  int xsize, int ysize,
			  GradientMagnitudeType *gradient,
			  GradientDirectionType *dir,
			  int numBins,
			  bool usesigned )
{
    double *atan2Table = FastMath::getSingleton().atan2Table;
    double *sqrtTable = FastMath::getSingleton().sqrtTable;

    double *rgx = new double[xsize*ysize];
    calcGradientX ( r, xsize, ysize, rgx );
    double *rgy = new double[xsize*ysize];
    calcGradientY ( r, xsize, ysize, rgy );

    double *ggx = new double[xsize*ysize];
    calcGradientX ( g, xsize, ysize, ggx );
    double *ggy = new double[xsize*ysize];
    calcGradientY ( g, xsize, ysize, ggy );

    double *bgx = new double[xsize*ysize];
    calcGradientX ( b, xsize, ysize, bgx );
    double *bgy = new double[xsize*ysize];
    calcGradientY ( b, xsize, ysize, bgy );

    double binq = usesigned ? 2*M_PI / numBins : M_PI / numBins;

    int k = 0;
    for ( int y = 0 ; y < ysize ; y++ )
	for ( int x = 0 ; x < xsize ; x++,k++ )
	{
	    double rx = rgx[k];
	    double ry = rgy[k];
	    //GradientMagnitudeType g  = (GradientMagnitudeType)sqrt ( rx*rx + ry*ry );
	    GradientMagnitudeType g  = (GradientMagnitudeType)sqrtTable [ (int)(rx*rx + ry*ry) ];
	    double max = g;
	    double mrx = rx;
	    double mry = ry;

	    rx = ggx[k];
	    ry = ggy[k];
	    g  = sqrt ( rx*rx + ry*ry );
	    if ( g > max ) { max = g; mrx = rx ; mry = ry; };

	    rx = bgx[k];
	    ry = bgy[k];

	    g  = sqrt ( rx*rx + ry*ry );
	    if ( g > max ) { max = g; mrx = rx ; mry = ry; };

	    gradient[k] = max;
	    int a_index = ((int)ry+255)*(255*2+1) + ((int)rx+255);
	    double angle = atan2Table[ a_index ];
	    if ( usesigned ) {
		if ( angle < 0 )
		    angle = 2*M_PI + angle;
	    } else {
		if ( angle < 0 )
		    angle = M_PI + angle;
	    }

	    GradientDirectionType bin = (GradientDirectionType)(angle / binq);

	    dir[k] = bin < numBins ? bin : numBins - 1;
	}

    delete [] rgx;
    delete [] rgy;
    delete [] ggx;
    delete [] ggy;
    delete [] bgx;
    delete [] bgy;
}

template <class GrayValueType, class GradientMagnitudeType, class GradientDirectionType>
void FastFilter::calcGradient ( const GrayValueType *gray, 
				  int xsize, int ysize,
				  GradientMagnitudeType *gradient, 
				  GradientDirectionType *dir, 
				  int numBins, 
				  bool usesigned )
{
    double *gx = new double[xsize*ysize];
    calcGradientX ( gray, xsize, ysize, gx );
    double *gy = new double[xsize*ysize];
    calcGradientY ( gray, xsize, ysize, gy );
    double *atan2Table = FastMath::getSingleton().atan2Table;
    double *sqrtTable = FastMath::getSingleton().sqrtTable;

    double binq = usesigned ? 2*M_PI / numBins : M_PI / numBins;

    int k = 0;
    for ( int y = 0 ; y < ysize ; y++ )
	for ( int x = 0 ; x < xsize ; x++,k++ )
	{
	    double rx = gx[k];
	    double ry = gy[k];
	    //GradientMagnitudeType g  = (GradientMagnitudeType)sqrt ( rx*rx + ry*ry );
	    GradientMagnitudeType g  = (GradientMagnitudeType)sqrtTable [ (int)(rx*rx + ry*ry) ];
	    gradient[k] = g;

	    int a_index = ((int)ry+255)*(255*2+1) + ((int)rx+255);
	    double angle = atan2Table[ a_index ];

	    if ( usesigned ) {
		if ( angle < 0 )
		    angle = 2*M_PI + angle;
	    } else {
		if ( angle < 0 )
		    angle = M_PI + angle;
	    }

	    GradientDirectionType bin = (GradientDirectionType)(angle / binq);

	    dir[k] = bin < numBins ? bin : numBins - 1;
	}

    delete [] gx;
    delete [] gy;
}


//lazy attempt for realizing fast histogram of oriented gradients
//since (neg.) double values are allowed, fast filtering based on look-up tables is no longer possible
template <class GrayValueType, class OrientedGradientHistogramType>
void FastFilter::calcOrientedGradientHistogram ( const GrayValueType *gray, 
						 int xsize, int ysize,
						 OrientedGradientHistogramType *hog, 
						 int numBins, 
						 bool usesigned )
{
    double *gx = new double[xsize*ysize];
    calcGradientX ( gray, xsize, ysize, gx );
    double *gy = new double[xsize*ysize];
    calcGradientY ( gray, xsize, ysize, gy );
    
    double binq = usesigned ? 2*M_PI / numBins : M_PI / numBins;

    for(int i=0; i<numBins; i++) hog[i] = (OrientedGradientHistogramType) 0.0;

    int k = 0;
    for ( int y = 0 ; y < ysize ; y++ )
	for ( int x = 0 ; x < xsize ; x++,k++ )
	{
	    double rx = gx[k];
	    double ry = gy[k];
	    //GradientMagnitudeType g  = (GradientMagnitudeType)sqrt ( rx*rx + ry*ry );
	    GrayValueType g  = (GrayValueType) sqrt(rx*rx + ry*ry);

	    double angle = atan2(ry,rx);

	    if ( usesigned ) {
		if ( angle < 0 )
		    angle = 2*M_PI + angle;
	    } else {
		if ( angle < 0 )
		    angle = M_PI + angle;
	    }

	    uint bin = (uint)(angle / binq);
            bin = bin < numBins ? bin : numBins - 1;
	    hog[bin]+= (OrientedGradientHistogramType) g;
	    
	}

    delete [] gx;
    delete [] gy;
}