/** Author: Luis Fco.
 Ispired in: Francois Fleuret implementation of Zk
             Philip Abbet examples/meanThreshold
             Luis Francisco /ThresholdGradient
             Luis Francisco /SymmetriAX
 
 
 
 This heuristic threshold the ROI refered to its mean value and then
 extracts the edges using the calculation of gradient as in luis/Gradient.
 
 Afterwards, it analice the axial symmetry of the edges extracted using the score:
 
                  sum (abs(pixel_value_reference_side - picture_value_symmetric_side))
        Score  =  --------------------------------------------------------------------
                                Number of symmetric pixels considered
 
 By default, _numfeatures = 180 features. 
 The range of degrees where symmetry will be evaluated goes from 0º to 180º
 
 */

#include <iostream>
#include <mash/heuristic.h>
#include <cmath>
#include <stdlib.h>

using namespace Mash;
const scalar_t PI = 3.14159265359;

#define PIXEL_DELTA(a, b) ((a) >= (b) ? (a) - (b) : (b) - (a))

bool edge(           byte_t v0, byte_t v1,
          byte_t v2, byte_t v3, byte_t v4, byte_t v5,
          byte_t v6, byte_t v7) {
    byte_t g = PIXEL_DELTA(v3, v4);
    
    return
    g > PIXEL_DELTA(v0, v3) && g > PIXEL_DELTA(v1, v4) &&
    g > PIXEL_DELTA(v2, v3) && g > PIXEL_DELTA(v4, v5) &&
    g > PIXEL_DELTA(v3, v6) && g > PIXEL_DELTA(v4, v7);
}

//------------------------------------------------------------------------------
// Declaration of the heuristic class
//------------------------------------------------------------------------------
class EdgesSymmetryAX: public Heuristic
{
    //_____ Construction / Destruction __________
public:
    EdgesSymmetryAX();
    virtual ~EdgesSymmetryAX();
    
    //_____ 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 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:
    // number of direcctions to evaluate the gradient
    static const int nb_edge_types = 8;
    
    // Vector for the storage of 8 gradient images, one for every direction
    int *_edge_count;
    
    // Vector for the storage of the addition of 8 gradient images
    int *_global_edge_count;
    
    // Image dimmensions
    int _image_width, _image_height;
    
    // Mean value of the ROI
    float _mean;
    
    // Here we will store all the degrees for axial symmetry evaluation
    scalar_t* _degrees;
    
    // Numer of features, by default 180, can be modified in EdgesSymmetryAX()
    int _numfeatures;
    
    // Vector to store the features
    scalar_t* featuresMatrix;
};


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



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

EdgesSymmetryAX::EdgesSymmetryAX(): _mean(0.0f)
{
    // TODO: Initialization of the attributes that doesn't depend of anything
    
    // The number of features is also the number of degrees for axial symmetry evaluation
    _numfeatures=10;
    
    // Filling up the_degrees attribute 
    _degrees = new scalar_t[_numfeatures];
    scalar_t step=180.0/(_numfeatures-1);
    for (int i=0; i<_numfeatures; i++) {
        _degrees[i]=i*step;
    }
    _degrees[_numfeatures-1]=180; // not needed, for fixing aproximations issues
}


EdgesSymmetryAX::~EdgesSymmetryAX()
{
    delete _degrees;
}


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

void EdgesSymmetryAX::init()
{
}

unsigned int EdgesSymmetryAX::dim()
{
    // We have has many features as degrées considered in the symmetry
    return _numfeatures;
}


void EdgesSymmetryAX::prepareForImage()
{
    // Until here, there is nothing to do with the ROI.
    // All processing concerning the ROI must be done in prepare for coordinates
}


void EdgesSymmetryAX::prepareForCoordinates()
{
    // In this Method:
    //  1. Evaluate the ROI mean value
    //  2. Binarize the image acording that mean value
    //  3. Evaluate the 8 gradients and store in _edge_count
    //  4. Evaluate the addition for 8 gradient and store in _global_edge_count
    
    unsigned int x0 = coordinates.x - roi_extent;
    unsigned int y0 = coordinates.y - roi_extent;
    unsigned int roi_size = roi_extent * 2 + 1;
    
    byte_t** pLines = image->grayLines();
   
    // Evaluate the mean value from the ROI area (i.e. 127x127 in /testheuristic)
    // ==========================================================================
    
    _mean = 0.0f;
    for (unsigned int y = 0; y < roi_extent * 2 + 1; ++y)
    {
        for (unsigned int x = 0; x < roi_extent * 2 + 1; ++x)
            _mean += pLines[y0 + y][x0 + x];
    }
    _mean /= roi_size*roi_size;
    
    // Binarize the whole Image (i.e. 128x128 coil-100) thresholding using 
    // the mean value of the ROI (i.e. 127x127 in /testheuristic)
    // ===================================================================
    
    _image_width = image->width();
    _image_height = image->height();
    
    // Storing the image binarized
    byte_t pixels[_image_height][_image_width];
    for(int y = 0; y < _image_height; y++) {
        for(int x = 0; x < _image_width; x++) {
            if (pLines[y][x] >= _mean)
                pixels[y][x]=1;
            else 
                pixels[y][x]=0;
        }
    }
    
    
    // Evaluate the 8 directions gradients for the whole image (i.e. 128x128 coil-100) 
    // binarized using the mean value of the ROI (i.e. 127x127 in /testheuristic)
    // The size of the gradients is the same as the image size (i.e. 128x128 coil-100)
    // ===============================================================================
    
    _edge_count = new int [_image_width * _image_height * nb_edge_types];
    
    int d00, d01, d02, d03, d04, d05, d06, d07;
    int d08, d09, d10, d11, d12, d13, d14, d15;
    
    for(int y = 0; y < _image_height; y++) {
        for(int x = 0; x < _image_width; x++) {
            
            for(int e = 0; e < nb_edge_types; e++)
                _edge_count[e + nb_edge_types * (x + _image_width * y)] = 0;
            
            if(x > 1 && x < _image_width - 3 && y > 1 && y < _image_height - 3) {
                d00 = pixels[(x - 1)][(y - 1)];
                d01 = pixels[(x + 0)][(y - 1)];
                d02 = pixels[(x + 1)][(y - 1)];
                d03 = pixels[(x + 2)][(y - 1)];
                
                d04 = pixels[(x - 1)][(y + 0)];
                d05 = pixels[(x + 0)][(y + 0)];
                d06 = pixels[(x + 1)][(y + 0)];
                d07 = pixels[(x + 2)][(y + 0)];
                
                d08 = pixels[(x - 1)][(y + 1)];
                d09 = pixels[(x + 0)][(y + 1)];
                d10 = pixels[(x + 1)][(y + 1)];
                d11 = pixels[(x + 2)][(y + 1)];
                
                d12 = pixels[(x - 1)][(y + 2)];
                d13 = pixels[(x + 0)][(y + 2)];
                d14 = pixels[(x + 1)][(y + 2)];
                d15 = pixels[(x + 2)][(y + 2)];
                
                /*
                 
                 XXXXXX     .XXXXX     ...XXX     .....X
                 XXXXXX     ..XXXX     ...XXX     ....XX
                 ......     ...XXX     ...XXX     ...XXX
                 ......     ....XX     ...XXX     ..XXXX
                 
                 #0         #1         #2         #3
                 
                 ......     X.....     XXX...     XXXXX.
                 ......     XX....     XXX...     XXXX..
                 XXXXXX     XXX...     XXX...     XXX...
                 XXXXXX     XXXX..     XXX...     XX....
                 
                 #4         #5         #6         #7
                 
                 */
                
                if(edge(d04, d08, d01, d05, d09, d13, d06, d10)) {
                    if(d05 < d09)
                        _edge_count[0 + nb_edge_types * (x + _image_width * y)]++;
                    else
                        _edge_count[4 + nb_edge_types * (x + _image_width * y)]++;
                }
                
                if(edge(d02, d07, d00, d05, d10, d15, d08, d13)) {
                    if(d05 < d10)
                        _edge_count[7 + nb_edge_types * (x + _image_width * y)]++;
                    else
                        _edge_count[3 + nb_edge_types * (x + _image_width * y)]++;
                }
                
                if(edge(d01, d02, d04, d05, d06, d07, d09, d10)) {
                    if(d05 < d06)
                        _edge_count[6 + nb_edge_types * (x + _image_width * y)]++;
                    else
                        _edge_count[2 + nb_edge_types * (x + _image_width * y)]++;
                }
                
                if(edge(d01, d04, d03, d06, d09, d12, d11, d14)) {
                    if(d06 < d09)
                        _edge_count[1 + nb_edge_types * (x + _image_width * y)]++;
                    else
                        _edge_count[5 + nb_edge_types * (x + _image_width * y)]++;
                }
            }
        }
    }

    
    // Adition of the 8 gradient images and storage in _global_edge_count
    // The size of the gradients addition is the same as the image size (i.e. 128x128 coil-100) 
    // ========================================================================================
    
    _global_edge_count = new int [_image_width * _image_height];
    
    for(int y = 0; y < _image_height; y++) {
        for(int x = 0; x < _image_width; x++) {
            _global_edge_count[(x + _image_width * y)]=0;
            for(int e = 0; e < nb_edge_types; e++)
                _global_edge_count[(x + _image_width * y)] += _edge_count[e + nb_edge_types * (x + _image_width * y)];
        }
    }
    
    // Calculation of the symmetryAX 
    // =============================
    
    // Evaluation of the axial symmetry of the ROI (i.e. 127x127 in /testheuristic)
    // from a gradient image (i.e. 128x128 coil-100)
    
    // In the _edge_count variable, we can find the gradients for every 8 direction
    // In the _global_edge_count, we can find the addition of the 8 gradients
    
    // In this method we are calculating the axial symmetry for the addition 
    // of those 8 gradient images. Modifying this method to provide the axial
    // symmetry for every direction is not complicated
    
    
    featuresMatrix = new scalar_t[dim()];
    scalar_t axe_degrees =0.0;
    scalar_t axe_radians =0.0;
    scalar_t score = 0.0;
    scalar_t cont_pix_sym =0.0;
    
    for (unsigned int l=0; l<dim() ; ++l) {
        
        cont_pix_sym =0.0;
        score = 0.0;
        axe_degrees=_degrees[l];
        axe_radians=axe_degrees* PI / 180;
        
        // Covering all the ROI with coordinates x and y refered to the top-lef pixel of the ROI
        
        for (int y=0; y<roi_size; y++) {
            for (int x=0; x<roi_size; x++) {
                
                // Coordinates refered to the ROI center
                int x_o = -(roi_extent-x);
                int y_o = roi_extent-y;
                
                // Module and argument calculation for vector (x,y) refered to the ROI center
                scalar_t point_phase_radians_o;
                if (x_o!=0) {
                    point_phase_radians_o=std::atan(double(y_o)/double(x_o));
                    // Implement atan() sign correction
                    if (((point_phase_radians_o < 0) && (x_o < 0)) || ((point_phase_radians_o > 0) && (x_o < 0)))  { 
                        point_phase_radians_o += PI;
                    }
                    if ((point_phase_radians_o==0) && (x_o < 0)) {
                        point_phase_radians_o = PI;
                    }
                } else {
                    // Avoid zero division
                    point_phase_radians_o = (y_o >=0 ? PI/2 : 3*PI/2);          
                }
                //scalar_t point_phase_degrees_o = point_phase_radians_o *180 / PI;
                scalar_t point_module = std::sqrt(double((y_o*y_o) + (x_o*x_o)));
                
                // Coordinates refered to the rotated cartesian axis with the origin in the ROI center
                int x_turned = round(point_module*std::cos(point_phase_radians_o-axe_radians));
                int y_turned = round(point_module*std::sin(point_phase_radians_o-axe_radians));
                
                // Coordinates for the symmetric pixel refered to the rotated cartesian axis 
                // with the origin in the ROI center
                int x_sym_turned = x_turned;
                int y_sym_turned = -y_turned;
                
                // Module and argument calculation for symmetric pixel (x_sym_turned,y_sym_turned)
                // Refered to the rotated cartesian axis with the origin in the ROI center
                scalar_t point_sym_phase_radians_turned; 
                if (x_sym_turned!=0) {
                    point_sym_phase_radians_turned=std::atan(double(y_sym_turned)/double(x_sym_turned));
                    // Implement atan() sign correction
                    if (((point_sym_phase_radians_turned < 0) && (x_sym_turned < 0)) || ((point_sym_phase_radians_turned > 0) && (x_sym_turned < 0)))  { 
                        point_sym_phase_radians_turned += PI;
                    }
                    if ((point_sym_phase_radians_turned==0) && (x_sym_turned < 0)) {
                        point_phase_radians_o = PI;
                    }
                } else {
                    // Avoid zero division
                    point_sym_phase_radians_turned = (y_sym_turned >=0 ? PI/2 : 3*PI/2);            
                }
                //scalar_t point_sym_phase_degrees_turned = point_sym_phase_radians_turned *180 / PI;
                scalar_t point_sym_module_turned = point_module;
                
                // Coordinates of the symmetric pixel refered to the non rotated axis
                // with the origin in the ROI center
                int x_sym_o = round(point_sym_module_turned*std::cos(point_sym_phase_radians_turned+axe_radians));
                int y_sym_o = round(point_sym_module_turned*std::sin(point_sym_phase_radians_turned+axe_radians));
                
                // Coordinates of the symmetric pixel refered to the non rotated axis 
                // with the origin in the top-lef pixel of the ROI
                int x_sym = x_sym_o + roi_extent;
                int y_sym = roi_extent - y_sym_o;
                
                
                // Calculation of the score
                if (((x_sym>=0) && (x_sym<roi_size) && (y_sym>=0) && (y_sym<roi_size))&&(y_turned!=0)&&(y_turned>0)) {
                    // Conditions (x_sym>=0) && (x_sym<roi_size) && (y_sym>=0) && (y_sym<roi_size)
                    // assure that the symetric point falls in the ROI
                    
                    // Condition (y_turned!=0)
                    // assure that all the points in the symmetric axe are not considered
                    
                    // Condition (y_turned>0)
                    // assure that every cuple of symmetric pixels are computed only once
                    
                    scalar_t _pixelValue = (scalar_t) _global_edge_count[((x0 + x) + _image_width * (y0 + y))];
                    scalar_t _pixelValueSym = (scalar_t) _global_edge_count[((x0 + x_sym) + _image_width * (y0 + y_sym))];
                    score += scalar_t(std::abs(_pixelValue-_pixelValueSym));
                    cont_pix_sym++;
                    
                } // Points out of ROI, in the symmetry axe or that have already been computed are ignored
                
            } //end for
            
        } //end for
        featuresMatrix[l]=(score/cont_pix_sym);
    } // end for features
}

void EdgesSymmetryAX::finishForCoordinates()
{
    delete[] _edge_count;
    delete[] _global_edge_count;
    delete featuresMatrix;
}


scalar_t EdgesSymmetryAX::computeFeature(unsigned int feature_index)
{   
    return featuresMatrix[feature_index];   
}