
#include <stdlib.h>
#include <iostream>
#include <math.h>
#include "mex.h"

#define PI (3.141592653589793)

using namespace std;

double round(double number)
{
    return number < 0.0 ? ceil(number - 0.5) : floor(number + 0.5);
}

//difference between angles
double anglediff(double a, double b)
{
    double diff = a - b;
    if(diff < -PI)
        diff += 2*PI;
    if(diff > PI)
        diff -= 2*PI;
    return diff;
}

//convert from polar to cartesian coordinates
void pol2cart(double t, double r, int &x, int &y)
{
    y = round(r*cos(t));
    x = round(r*sin(t));
}

//limits the point at feasible coordinates
void crop_coordinates(int &p, int min, int max)
{
    if(p<min)
        p=min;
    if(p>max)
        p=max;
}

//returns the average intensity of the pixels in the neighborhood I_(side*2+1)[x,y]
double evaluateNeighborhood(double *img, int IMG_SIZE, int x, int y, int side)
{
    double totalIntensity = 0.0;
    double sizeWin = pow((float)side*2+1,2);
    for(int cx=x-side;cx<=x+side;cx++)
        for(int cy=y-side;cy<=y+side;cy++)
        {
            totalIntensity += img[cx+IMG_SIZE*cy];
        }
    return totalIntensity/sizeWin;
}

/*
PARAMETERS:
    -image
    -particle
    -MIN
    -MAX
    -MEAN
    -SHIFT
    -penalties
RETURN:
    -fitness
*/
void mexFunction(int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[])
{
    int N_POINTS;
    int IMG_SIZE;
    double *MIN;
    double *MAX;
    double *MEAN;
    int *SHIFT;
    double *PENALTIES;


    
    int i;
    double *orig_coordinates;  //coordinate su cui lavora la DE, comprese tra 0 e 1 (t1 t2 ... r1 r2 ...)
    double *polar_coordinates; //coordinate polari orig_coordinates*(MAX-MIN)+MIN
    int *cart_coordinates;  //coordinate cartesiane (y1 x1 y2 x2 ...)
    int *outer_coordinates;  //coordinate cartesiane dell'outer set(y1 x1 y2 x2 ...)
    double *img;


    
    //parametri: possono essere anche passati?
    const double GAMMA_P = 5.0; //gamma Puncutal Energy
    const double GAMMA_C = 1.0; //gamma Continuous Energy
    const double N_P_IN = 20;   //number points Continuous Energy Inner Set
    const double N_P_OUT = 10;  //number points  Continuous Energy Outer Set
    const double MIN_DIST_DG = 25; //penalty associated with the start and end points of DG
    
    //read input. TODO: put checks
    if(nrhs != 7)
        mexErrMsgTxt("This function takes exactly 6 arguments: image, particle, lb, ub, template, shift, penalties");
    IMG_SIZE = mxGetM(prhs[0]);
    img = (double*) mxGetPr(prhs[0]);
    N_POINTS = mxGetN(prhs[1])/2;
    orig_coordinates = (double*) mxGetPr(prhs[1]);
    MIN = (double*) mxGetPr(prhs[2]);
    MAX = (double*) mxGetPr(prhs[3]);
    MEAN = (double*) mxGetPr(prhs[4]);
    SHIFT = (int*) mxGetPr(prhs[5]);
    PENALTIES = (double*) mxGetPr(prhs[6]);
    
    //instantiates output
    int dims[2] = {1,1};
    plhs[0] = mxCreateNumericArray(2, dims, mxDOUBLE_CLASS, mxREAL );
    double* fitnessPtr = (double*)mxGetData(plhs[0]);
    
    //instantiates coordinates particle
    polar_coordinates = (double*) malloc(sizeof(double)*N_POINTS*2);
    cart_coordinates = (int*) malloc(sizeof(int)*N_POINTS*2);
    outer_coordinates = (int*) malloc(sizeof(int)*N_POINTS*2);
    
    for(i=0;i<N_POINTS*2;i++)
    {
        polar_coordinates[i] = orig_coordinates[i]*(MAX[i]-MIN[i])+MIN[i];
    }
    
    for(i=0;i<N_POINTS;i++)
    {
        pol2cart(polar_coordinates[i],polar_coordinates[i+N_POINTS],cart_coordinates[2*i],cart_coordinates[2*i+1]);
        if(i>0) //the positions are relative: from the second onwards, add
        {
            cart_coordinates[2*i] += cart_coordinates[2*(i-1)];
            cart_coordinates[2*i+1] += cart_coordinates[2*(i-1)+1];
        }
        crop_coordinates(cart_coordinates[2*i],0,IMG_SIZE-1);
        crop_coordinates(cart_coordinates[2*i+1],0,IMG_SIZE-1);
    }
    //Creazione Outer Set Creation
    for(i=0;i<N_POINTS;i++)
    {
        outer_coordinates[2*i] = cart_coordinates[2*i] + SHIFT[2*i];
        outer_coordinates[2*i+1] = cart_coordinates[2*i+1] + SHIFT[2*i+1];
        crop_coordinates(outer_coordinates[2*i],0,IMG_SIZE-1);
        crop_coordinates(outer_coordinates[2*i+1],0,IMG_SIZE-1);
    }
    
    
    double fitness = 0.0;
    int x,y; //point coords
    int xo, yo; //previous point coords
    int xi, yi; //intermediate point coords
    for(i=0;i<N_POINTS;i++) //inner points
    {
        x = cart_coordinates[2*i];
        y = cart_coordinates[2*i+1];
        int px, py;
        
        if(x>0 && x<IMG_SIZE-1 && y>0 && y<IMG_SIZE-1) //on the border: do not compute!
        {
            fitness += GAMMA_P*evaluateNeighborhood(img,IMG_SIZE,x,y,1);
        }
        if(i>0) //CE of inner set
        {
            for(double j=1; j <= N_P_IN; j++)
            {
                xi = xo + (j/(1+N_P_IN))*(x-xo);
                yi = yo + (j/(1+N_P_IN))*(y-yo);
                fitness += GAMMA_C*img[xi+IMG_SIZE*yi];
            }
        }
        xo = x; yo = y;
    }
    for(i=0;i<N_POINTS;i++) //outer points
    {
        x = outer_coordinates[2*i];
        y = outer_coordinates[2*i+1];
        int px, py;
        if(x>0 && x<IMG_SIZE-1 && y>0 && y<IMG_SIZE-1) //on the border: do not compute!
        {
            fitness -= GAMMA_P*evaluateNeighborhood(img,IMG_SIZE,x,y,1);
        }
        if(i>0) //CE of inner set
        {
            for(double j=1; j <= N_P_OUT; j++)
            {
                xi = xo + (j/(1+N_P_OUT))*(x-xo);
                yi = yo + (j/(1+N_P_OUT))*(y-yo);
                fitness -= GAMMA_C*img[xi+IMG_SIZE*yi];
            }
        }
        xo = x; yo = y;
    }
    
    //penalties
    double pen1 = 0.0; //angles
    double pen2 = 0.0; //modules
    double pen3 = 0.0;
    for(i=0;i<N_POINTS-1;i++)
    {
        pen1 += pow(anglediff(MEAN[i],polar_coordinates[i+1]),2);
        pen2 += pow(MEAN[i+N_POINTS] - polar_coordinates[i+1+N_POINTS],2);
    }
    pen1 = sqrt(pen1);
    pen2 = sqrt(pen2);
    if(N_POINTS == 7) //DG
    {
        if( (cart_coordinates[12] - cart_coordinates[0] <= MIN_DIST_DG) || 
            (cart_coordinates[10] - cart_coordinates[0] <= MIN_DIST_DG) || 
            (cart_coordinates[12] - cart_coordinates[2] <= MIN_DIST_DG))
            pen3 = RAND_MAX;
    }
    else //CA
    {
        pen3 = sqrt(pow((double)cart_coordinates[(N_POINTS*2)-1] - cart_coordinates[1],2) + pow((double)cart_coordinates[(N_POINTS*2)-2] - cart_coordinates[0],2));
    }
    fitness -= (PENALTIES[0]*pen1 + PENALTIES[1]*pen2 + PENALTIES[2]*pen3);
    
    fitnessPtr[0] = (double) fitness;
    
    free(polar_coordinates);
    free(cart_coordinates);
    free(outer_coordinates);

}
