/** Author: vfremont
 
    inspired by leonidas/dt_hogs6

    This heuristic calculates corners strength of an image in the Harris-Stephens way 
    over 3x3 pixels blocks
 
    feature = sum(Ix^2*Iy^2) - sum((Ix*Iy)^2) - 0.04*(sum(Ix^2) + sum(Iy^2))^2
 
    where Ix and Iy are spatial gradients of the image
 
    ref : C.Harris, M.Stephens. “A Combined Corner and Edge Detector”. 1988
  
*/

#include <mash/heuristic.h>

using namespace Mash;


//------------------------------------------------------------------------------
// Declaration of the heuristic class
//------------------------------------------------------------------------------
class corner: public Heuristic
{
    //_____ Construction / Destruction __________
public:
    corner();
    virtual ~corner();


    //_____ Implementation of Heuristic __________
public:
    //--------------------------------------------------------------------------
    /// @brief  Called once at the creation of the heuristic
    ///
    /// Pre-computes all the data that will never change during the life-time of
    /// the heuristic
    ///
    /// When this method is called, the 'roi_extent' attribute is initialized
    ///
    /// @remark The implementation of this method is optional
    //--------------------------------------------------------------------------
    virtual void init();

    //--------------------------------------------------------------------------
    /// @brief  Called once when the heuristic is destroyed
    ///
    /// Frees the memory allocated by the init() method
    ///
    /// @remark This method must be implemented if init() is used and allocated
    ///         some memory
    //--------------------------------------------------------------------------
    virtual void terminate();

    //--------------------------------------------------------------------------
    // Returns the number of features this heuristic computes
    //
    // When this method is called, the 'roi_extent' attribute is initialized
    //--------------------------------------------------------------------------
    virtual unsigned int dim();

    //--------------------------------------------------------------------------
    // Called once per image, before any computation 
    //
    // Pre-computes from a full image the data the heuristic will need to compute
    // features at any coordinates in the image
    //
    // When this method is called, the following attributes are initialized:
    //     - roi_extent
    //     - image
    //--------------------------------------------------------------------------
    virtual void prepareForImage();

    //--------------------------------------------------------------------------
    // Called once per image, after any computation 
    //
    // Frees the memory allocated by the prepareForImage() method
    //--------------------------------------------------------------------------
    virtual void finishForImage();

    //--------------------------------------------------------------------------
    // Called once per coordinates, before any computation
    //
    // Pre-computes the data the heuristic will need to compute features at the
    // given coordinates
    //
    // When this method is called, the following attributes are initialized:
    //     - roi_extent
    //     - image
    //     - coordinates
    //--------------------------------------------------------------------------
    virtual void prepareForCoordinates();
    
    //--------------------------------------------------------------------------
    // Called once per coordinates, after any computation 
    //
    // Frees the memory allocated by the prepareForCoordinates() method
    //--------------------------------------------------------------------------
    virtual void finishForCoordinates();

    //--------------------------------------------------------------------------
    // Computes the specified feature
    //
    // When this method is called, the following attributes are initialized:
    //     - roi_extent
    //     - image
    //     - coordinates
    //--------------------------------------------------------------------------
    virtual scalar_t computeFeature(unsigned int feature_index);


    //_____ Attributes __________
protected:
    scalar_t* _corner;
};


//------------------------------------------------------------------------------
// Creation function of the heuristic
//------------------------------------------------------------------------------
extern "C" Heuristic* new_heuristic()
{
    return new corner();
}



/************************* CONSTRUCTION / DESTRUCTION *************************/

corner::corner()
{
    // TODO: Initialization of the attributes that doesn't depend of anything
}


corner::~corner()
{
    // TODO: Cleanup of the allocated memory still remaining
}


/************************* IMPLEMENTATION OF Heuristic ************************/

void corner::init()
{
    // TODO: Initialization of the attributes that depend on the size of the
    // region of interest
}


void corner::terminate()
{
    // TODO: Cleanup of the memory allocated by the init() method
}


unsigned int corner::dim()
{
    unsigned int roi_size = roi_extent * 2 + 1;
    return (roi_size-2) * (roi_size-2);
}


void corner::prepareForImage()
{
    // TODO: Initialization of the attributes that depend of the whole image
}


void corner::finishForImage()
{
    // TODO: Frees the memory allocated by the prepareForImage() method
}


void corner::prepareForCoordinates()
{
    scalar_t a,b,c,d,e,f,g,h,i;
    
    
    // Compute the coordinates of the top-left pixel of the region of interest
    unsigned int x0 = coordinates.x - roi_extent;
    unsigned int y0 = coordinates.y - roi_extent;
    
    unsigned int roi_size = roi_extent * 2 + 1;
    byte_t** pixels = image->grayLines();
    scalar_t dx,dy;
    
    _corner=new scalar_t[dim()];
    
    // spatial gradients
    scalar_t Ix[image->height()][image->width()];
    scalar_t Iy[image->height()][image->width()];   
    
    
    //Calculate gradients (zero padding)
    for(unsigned int y=0; y<roi_size; y++) {
        for(unsigned int x=0; x<roi_size; x++) {
            if(x==0){
                dx = pixels[y0+y][x0+x+1];
            }
            else{
                if (x==roi_size-1){
                    dx = -pixels[y0+y][x0+x-1];
                }
                else{
                    dx = pixels[y0+y][x0+x+1] - pixels[y0+y][x0+x-1];
                }
            }
            if(y==0){
                dy = -pixels[y0+y+1][x0+x];
            }
            else{
                if (y==roi_size-1){
                    dy = pixels[y0+y-1][x0+x];
                }
                else{
                    dy = -pixels[y0+y+1][x0+x] + pixels[y0+y-1][x0+x];
                }
            }
            dx=dx/255;
            dy=dy/255;
            
            
            Ix[y0+y][x0+x] = dx;
            Iy[y0+y][x0+x] = dy;
            
        }
    }

    unsigned int count=0;
    scalar_t sumIx2=0,sumIy2=0,sumIxIy=0;
    
    for (unsigned int y = 0; y < roi_size-2; ++y)
    {
        for (unsigned int x = 0; x < roi_size-2; ++x){
                                    
            a = Ix[y0+y][x0+x];b = Ix[y0+y][x0+x+1];c = Ix[y0+y][x0+x+2];
            d = Ix[y0+y+1][x0+x];e = Ix[y0+y+1][x0+x+1];f = Ix[y0+y+1][x0+x+2];
            g = Ix[y0+y+2][x0+x];h = Ix[y0+y+2][x0+x+1];i = Ix[y0+y+2][x0+x+2];
            sumIx2 = a*a+b*b+c*c+d*d+e*e+f*f+g*g+i*i;
            a = Iy[y0+y][x0+x];b = Iy[y0+y][x0+x+1];c = Iy[y0+y][x0+x+2];
            d = Iy[y0+y+1][x0+x];e = Iy[y0+y+1][x0+x+1];f = Iy[y0+y+1][x0+x+2];
            g = Iy[y0+y+2][x0+x];h = Iy[y0+y+2][x0+x+1];i = Iy[y0+y+2][x0+x+2];
            sumIy2 = a*a+b*b+c*c+d*d+e*e+f*f+g*g+i*i;
            a = Ix[y0+y][x0+x]*Iy[y0+y][x0+x];
            b = Ix[y0+y][x0+x+1]*Iy[y0+y][x0+x+1];
            c = Ix[y0+y][x0+x+2]*Iy[y0+y][x0+x+2];
            d = Ix[y0+y+1][x0+x]*Iy[y0+y+1][x0+x];
            e = Ix[y0+y+1][x0+x+1]*Iy[y0+y+1][x0+x+1];
            f = Ix[y0+y+1][x0+x+2]*Iy[y0+y+1][x0+x+2];
            g = Ix[y0+y+2][x0+x]*Iy[y0+y+2][x0+x];
            h = Ix[y0+y+2][x0+x+1]*Iy[y0+y+2][x0+x+1];
            i = Ix[y0+y+2][x0+x+2]*Iy[y0+y+2][x0+x+2];
            sumIxIy = a+b+c+d+e+f+g+i;
            
            // Harris corner strength response
            _corner[count] = (sumIx2*sumIy2 - sumIxIy*sumIxIy) - 0.04*(sumIx2 + sumIy2)*(sumIx2 + sumIy2);
            
            count++;
            
        }
    }
    
}


void corner::finishForCoordinates()
{
    delete[] _corner;
}


scalar_t corner::computeFeature(unsigned int feature_index)
{
    return _corner[feature_index];
}