#include <stdlib.h>

#include "problema.h"
#include "method.h"
#include "logger.h"
#include "utilities.h"

#define STEP0    100

#define CUSTOM   1

#define TAU      0.01
#define OMEGA    0.0001

#define MAX_ITER 100000

static double homeRandom(double a, double b)
{
    return ( rand()/(double)RAND_MAX ) * (b-a) + a;
}

static double ESwithRo(
    double    x[],
    Problema* pb,
    double    ro)
{
    int i;
    double  copy[pb->_dimension];
    double* calculGradient;

    // On copie le tableau car on ne veut pas le modifier
    memcpy(copy,x,pb->_dimension * sizeof(double));
    
    // On calcule le gradient car utile au calcul
    if( pb->_derivees[0] )
    {
        calculGradient = pb->_derivees[0](x);
    }
    // sinon
    else
    {
        calculGradient = approximateGradient(
                            pb->_function,
                            x,
                            pb->_dimension,
                            0.001//FIXME
                        );
    }

    // Calcul du vecteur interm�diaire
    for(i = 0; i < 3; i++)
        copy[i] = x[i] - ro * calculGradient[i];

    free(calculGradient);

    return pb->_function(copy,pb->_dimension)[0];
}

/**
 * ici dk = - grad( ES( x_{k} ) )
 * le calcul devient donc :
 * OMEGA_{1} * RO_{k}^{i} * - norme( grad( ES( w_{k} ) ) )
 */
static double EScomplement(
    double x[],
    Problema* pb,
    double step)
{
    double* calculGradient;
    double result;

    // On calcule le gradient car utile au calcul
    if( pb->_derivees[0] )
    {
        calculGradient = pb->_derivees[0](x);
    }
    // sinon
    else
    {
        calculGradient = approximateGradient(
                            pb->_function,
                            x,
                            pb->_dimension,
                            0.001//FIXME
                        );
    }

    result = OMEGA * step * - normeEuclidienne( calculGradient,pb->_dimension );

    free(calculGradient);
    
    return result;
}

static double computeLinearStep(
    double x[],
    Problema* pb)
{
    // Initialize - ARBITRARY VALUE
    double step = STEP0;            // Pas initial - volontairement "grand"

    //Debug
    logMessage(DEBUG,"newline","");
    logMessage(DEBUG,"méthode linear","");

    step = x[0] / pb->_derivees[0](x)[0];

    logMessage(INFO,"liner step",doubleToString(step));

    return step;
}

static double computeArmijoStep(
    double x[],
    Problema* pb)
{
    // Initialize - ARBITRARY VALUE
    double step = STEP0;            // Pas initial - volontairement "grand"
    int    counter = 0;           // Compteur de boucle
    double eswro, escomp, es;     // Calcul interm�diaire pour la condition de boucle

    //Debug
    logMessage(DEBUG,"newline","");
    logMessage(DEBUG,"méthode armijo : pas initial",doubleToString(step));

    // Calcul de la premi�re it�ration
    eswro  = ESwithRo(x, pb, step);
    es     = pb->_function(x,pb->_dimension)[0];
    escomp = EScomplement(x,pb,step);

    logMessage(DEBUG,"condition : J( x_{k} + ro_{k}^{i} * d_{k})",doubleToString(eswro));
    logMessage(DEBUG,"condition : J( x_{k} )",doubleToString(es));
    logMessage(DEBUG,"condition : - omega_{1} * ro_{k}^{i} * d_{k} * d_{k}",doubleToString(escomp));
    logMessage(DEBUG,"condition : eswro > es + escomp",printBoolean( eswro > es + escomp ));

    while( ( eswro > es + escomp ) && ( ++counter < MAX_ITER ) )
    {
        //new step - RANDOMLY
        do
        {
            step = ((double)homeRandom(TAU,1 - TAU)) * step;
        }
        while(step == 0);

        logMessage(DEBUG,"actualisation pas",doubleToString(step));

        eswro  = ESwithRo(x, pb, step);
        es     = pb->_function(x,pb->_dimension)[0];
        escomp = EScomplement(x,pb,step);
    
        logMessage(DEBUG,"condition : J( x_{k} + ro_{k}^{i} * d_{k})",doubleToString(eswro));
        logMessage(DEBUG,"condition : J( x_{k} )",doubleToString(es));
        logMessage(DEBUG,"condition : - omega_{1} * ro_{k}^{i} * d_{k} * d_{k}",doubleToString(escomp));
        logMessage(DEBUG,"condition : eswro > es + escomp",printBoolean( eswro > es + escomp ));

    }

    logMessage(DEBUG,"counter value",integerToString(counter,10));
    logMessage(DEBUG,"counter limit",integerToString(MAX_ITER,10));
    logMessage(DEBUG,"resultat optimisation pas",doubleToString(step));
    
    return step;
}

void resolveByGradientOptimalStep(
    Problema*    pb,
    Method*      m,
    double       init_vector[],
    void*        args[])
{
    // iteration
    double current_iteration[pb->_dimension];
    double old_iteration    [pb->_dimension];

    //
    double norme;
    double diff[pb->_dimension];
    double* derivee_buffer = NULL;
    double step;
    double old_error = -1, current_error;
    int counter = 0,i;
    boolean toContinue = 1;

    logMessage(DEBUG,"newline","");
    logMessage(DEBUG,"function name","resolveByGradientOptimalStep");
    logMessage(DEBUG,"parameters : problem name",pb->_name);
    logMessage(DEBUG,"parameters : method name",m->_name);
    logMessage(DEBUG,"parameters : vector",printVector(init_vector,pb->_dimension));
    logMessage(DEBUG,"parameters : epsilon in",doubleToString(m->_epsilon_in));
    logMessage(INFO,"parameters : epsilon",doubleToString(m->_epsilon_in));
    logMessage(DEBUG,"parameters : epsilon out",doubleToString(m->_epsilon_out));
    logMessage(DEBUG,"parameters : patience",integerToString(m->_max_iteration,10));

    // Initialize old_iteration <- init_vector
    memcpy(
            old_iteration,
            init_vector,
            sizeof(double) * pb->_dimension);

    if(pb->_solution)
    {
        logMessage(DEBUG,"solution","bundled with the problem");
        logMessage(DEBUG,"solution value",printVector(pb->_solution,pb->_dimension));
    }
    if( pb->_derivees[0] )
        logMessage(DEBUG,"derivee acquisition","we got the first derivee");
    else
        logMessage(DEBUG,"derivee acquisition","dynamically calculated");

    do
    {
        // Recherche pas optimal
        if(CUSTOM)
            step = computeLinearStep(old_iteration,pb);
        else
            step = computeArmijoStep(old_iteration,pb);

        printf("- <linear step> '%.5lf'\n",doubleToString(step));
        printf("- <armijo step> '%.5lf'\n",doubleToString(computeArmijoStep(old_iteration,pb)));

        free(derivee_buffer); //FIXME due to approximate gradient
        // Si on a la dérivée première autant l'utiliser...
        if( pb->_derivees[0] )
        {
            derivee_buffer = pb->_derivees[0](old_iteration);
        }
        // sinon
        else
        {
            derivee_buffer = approximateGradient(
                                pb->_function,
                                old_iteration,
                                pb->_dimension,
                                0.001//FIXME
                            );
        }
        // Calcul it�ration
        for(i=0; i<pb->_dimension; i++)
            current_iteration[i] =
                    old_iteration[i]
                    - (
                        step
                        * derivee_buffer[i]
                    );

        logMessage(DEBUG,"derivee value",printVector(derivee_buffer,pb->_dimension));
        logMessage(DEBUG,"current iteration",printVector(current_iteration,pb->_dimension));
        logMessage(DEBUG,"old iteration",printVector(old_iteration,pb->_dimension));

        if(counter < 10)
        {
            logMessage(INFO,"iteration",printVector(current_iteration,pb->_dimension));
        }


        // Calcul de la diff�rence entre les deux vecteurs
        for(i=0; i<pb->_dimension; i++)
            diff[i] = current_iteration[i] - old_iteration[i];

        // Calcul de la norme pour la condition d'arr�t
        norme = normeEuclidienne(diff,pb->_dimension);

        logMessage(DEBUG,"difference's norme value",doubleToString(norme));

        //Calcul de l'erreur, si on la possède
        if(pb->_solution){
            current_error = error(pb->_dimension,current_iteration,pb->_solution);

            logMessage(DEBUG,"error",doubleToString(current_error));
            if(old_error != -1)
                logMessage(DEBUG,"error difference between iteration",doubleToString(fabs(current_error - old_error)));

            // Actualisation statistique sur l'erreur
            old_error = current_error;
        }

        // Pr�paration prochaine it�ration
        memcpy(old_iteration,current_iteration,pb->_dimension * sizeof(double));


        toContinue = toContinue && ( norme > m->_epsilon_in );
        // TODO
        //toContinue = toContinue && ( fabs(pb->_function(old_iteration,pb->_dimension) - pb->_function(current_iteration,pb->_dimension)) > m->_epsilon_out );
        toContinue = toContinue && ( ++counter < m->_max_iteration );
        // TODO
        //toContinue = toContinue && (pb->_solution && old_error != -1) ? fabs(current_error - old_error) < m->_epsilon_in : 1)

        logMessage(DEBUG,"counter",integerToString(counter,10));

        logMessage(DEBUG,"stop condition 1 : norme > m->_epsilon_in",printBoolean(norme > m->_epsilon_in));
        logMessage(DEBUG,"stop condition 2 : counter < m->_max_iteration",printBoolean((counter+1) < m->_max_iteration));
        logMessage(DEBUG,"should we continue",printBoolean(toContinue));
    }
    while( toContinue );

    // TODO : fill, FIXME : make it a define
    m->_last_complexity            = counter;
    m->_last_result_function_value = pb->_function(current_iteration,pb->_dimension)[0];
    if(pb->_solution)
    {// FIXME
        m->_last_absolute_error        = normeEuclidienne(diff,pb->_dimension);
        m->_last_relative_error        = normeEuclidienne(diff,pb->_dimension)/normeEuclidienne(pb->_solution,pb->_dimension);
    }
    // S'il y avait une précédente valeur, on libère le précédent espace utilisé
    if(m->_last_result) free(m->_last_result);
    m->_last_result = (double*) malloc(sizeof(double) * pb->_dimension);
    memcpy(m->_last_result, current_iteration, sizeof(double) * pb->_dimension);
}