/**
 *  Auteur : MACHIZAUD Andr�a
 *  TD 01 - Heuristique
 *  Sujet : Gradient � pas fixe
 *  Fonction objectif pour l'optimisation d�terministe : x_{k+1} = x_{k} - \ro_{k} \grad ES(x_{k})
 *  Fonction test : ES(x) = x_{1}^{2} + x_{2}^{2} + x_{3}^{2}
 *  Erreur : e_{k} = ||x_{k} - x^{*}||
 *
 */
#include <stdio.h>
#include <string.h>
#include <stdlib.h>

#include <math.h>
//lib aleatoire
#include <time.h>

#include "gradient.h"
#include "logger.h"

float solutionES[3] = {0.0, 0.0, 0.0};

char* itoa(int val, int base){

	static char buf[32] = {0};

	int i = 30;

	for(; val && i ; --i, val /= base)

		buf[i] = "0123456789abcdef"[val % base];

	return &buf[i+1];

}

char* printVector(const double vector[],const size_t dimension)
{
    char* value;
    char buffer[20];
    char debug_buffer[40];
    unsigned int counter = 0;

    value =(char *)malloc(40*sizeof(char));
    memset(value,'\0',40);

    if(DEBUG)printf("<init>Value : '%s'\n",value);
    discreteLog(DEBUG,"init",(char*)itoa(value,10));

    strcat(value,"{");
    if(DEBUG)printf("<1st delimiter>Value : '%s'\n",value);
    while(counter < dimension)
    {
        if(DEBUG)printf("<counter>Value : '%d'\n",counter);
        if(DEBUG)printf("<dimension>Value : '%d'\n",dimension);

        memset(buffer,'\0',20);

        if(DEBUG)printf("<clean buffer>Value : '%s'\n",buffer);

        sprintf(buffer,"%.3lf",vector[counter]);
        if( counter + 1 < dimension ) strcat(buffer,", ");

        if(DEBUG)printf("<fill buffer> Value : '%s'\n",buffer);

        strcat(value,buffer);

        if(DEBUG)printf("<concat value> Value : '%s'\n",value);
        counter++;
    }
    strcat(value,"}");
    if(DEBUG)printf("<completed> Value : '%s'\n",value);
    return value;
}

/*
double resolveByGradientPasFixe(
    const double (*function)(double vector[],size_t dimension), /* objective function* /
    const double vector0[],                                     /* initial vector * /
    const size_t vector_dimension,                              /* vecteur dimension * /
    double pas,                                                 /* step * /
    double epsilon,                                             /* epsilon * /
    int    max_iter )
{
    double current_iteration[vector_dimension], old_iteration[vector_dimension];
    float norme, diff[3];
    float old_error = -1, current_error;
    int counter = 0,i;

    if(DEBUG)
        printf("Vecteur initial x : %s\n",printVector(vector0,vector_dimension));

    for(i=0; i<3; i++)
        old_iteration[i] = x0[i];

    do
    {
        for(i=0; i<3; i++)
            current_iteration[i] = old_iteration[i] - (pas0 * gradientES(old_iteration).x[i]);


        for(i=0; i<3; i++)
            diff[i] = current_iteration[i] - old_iteration[i];

        norme = normeVR3(diff);


        //Calcul de l'erreur
        current_error = errorES(current_iteration);

        // Info debug
//        printf("\n [Current iteration] : ");printVector3(current_iteration);
//        printf("\n [Old iteration] : ");printVector3(old_iteration);
        printf("\n Erreur : %f",current_error);
        if(old_error != -1)
            printf(" [ difference ( %f ) ]",(current_error - old_error));

        // Pr�paration prochaine it�ration
        for(i=0; i<3; i++)
            old_iteration[i] = current_iteration[i];

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

    }
    while( norme > epsilon && ++counter < max_iter );

    // Info Debug
    printf("\nCounter value : %d",counter);
    printf("\nNorme de la difference : %f",normeVR3(diff));
    printf("\nVecteur optimum : ");printVector3(current_iteration);
    printf("\nFunction value for x : %f",ES(current_iteration));
}
*/

float errorES(float x[])
{
    float buffer[3];
    int i;

    for(i=0; i<3; i++)
        buffer[i] = x[i] - solutionES[i];

    return normeVR3(buffer);
}

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

double* approximateGradient(
    double (*function)(const double vector[],const size_t dimension),
    double x[],
    size_t dimension,
    double delta)
{
    double buffer[dimension];
    double* result;
    int counter;

    result = (double *)malloc(dimension * sizeof(double));

    if(DEBUG)printf("\n\n<prensentation>    Value : 'approximateGradient'\n");
    if(DEBUG)printf("<init vector>    Value : '%s'\n",printVector(x,dimension));
    if(DEBUG)printf("<init dimension> Value : '%d'\n",dimension);

    for(counter = 0; counter < dimension;counter++)
    {
        memcpy(buffer,x,dimension * sizeof(double)); //copie du vecteur
        if(DEBUG)printf("<init buffer> Value : '%s'\n",printVector(buffer,dimension));
        buffer[counter] += delta;
        if(DEBUG)printf("<delta buffer> Value : '%s'\n",printVector(buffer,dimension));

        result[counter] = ( (*function)(buffer,dimension) - (*function)(x,dimension) ) / delta;
    }

    if(DEBUG)printf("<result> Value : '%s'\n",printVector(result,dimension));

    return result;
}

/*
VR3 gradientGP(float x[])
{
    VR3 v;
    v.x[0] = 2 * x[0];
    v.x[1] = 2 * x[1];
    v.x[2] = 2 * x[2];

    return v;
}*/

VR3 gradientES(float x[])
{
    VR3 v;
    v.x[0] = 2 * x[0];
    v.x[1] = 2 * x[1];
    v.x[2] = 2 * x[2];

    return v;
}

/*
 * Calcul de la norme
 * Prends en param�tres un vecteur � trois composantes
 */
float normeVR3(float x[])
{
    int i;
    double temp = 0;
    for(i=0; i<3; i++)
        temp += x[i] * x[i];

//    //Info debug
//    printf("\nVector received : ");
//    printVector3(x);
//    printf("\nNorme value : %f", sqrt(temp));

    return (float) sqrt( temp );
}

float ESwithRo(float x[], float ro)
{
    int i;
    float copy[3];
    VR3 calculGradient;

    // On copie le tableau car on ne veut pas le modifier
    memcpy(copy,x,3 * sizeof(float));
/*
    copyArray3(copy,x);
*/
    // On calcule le gradient car utile au calcul
    calculGradient=gradientES(x);

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

    // Info debug
//    printf("\n\n ES WTIH RO");
//    printf("\n\t - Vecteur originel x : ");printVector3(x);
//    printf("\n\t - Gradient : ");printVector3(calculGradient.x);
//    printf("\n\t - Pas courant : %f",ro);
//    printf("\n\t - Vecteur interm�diaire x :");printVector3(copy);
//    printf("\n\t - Valeur de ES pour ce vecteur : %f",ES(copy));

    return ES(copy);
}

/**
 * ici dk = - grad( ES( x_{k} ) )
 * le calcul devient donc :
 * OMEGA_{1} * RO_{k}^{i} * - norme( grad( ES( w_{k} ) ) )
 */
double EScomplement(float x[], double step)
{
    VR3 temp;
    temp = gradientES(x);

    //Info debug
//    printf("\n\n ES COMPLEMENT");
//    printf("\n\t * Omega : %f",OMEGA);
//    printf("\n\t * Actual step : %f",step);
//    printf("\n\t * Original vector : ");printVector3(x);
//    printf("\n\t * Gradient : ");printVector3(temp.x);
//    printf("\n\t * Compute result : %f * %f = %f",OMEGA*step,- normeVR3(temp.x),OMEGA * step * - normeVR3( temp.x ));

    return OMEGA * step * - normeVR3( temp.x );
}

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

    //Debug
    printf("\n Pas initial : %f",step);

    // Calcul de la premi�re it�ration
    eswro = ESwithRo(x, step);
    es = ES(x);
    escomp = EScomplement(x ,step);

//    printf("\nCondition : %f <= %f + %f (%f)?",eswro,es,escomp,es + escomp);
    // Test condition
//    while( (ESwithRo(x, step) <= ES(x) + EScomplement(x ,step)) && (++counter < MAX_ITER) )
    while( ( eswro > es + escomp ) && ( ++counter < MAX_ITER ) )
    {
        //new step - RANDOMLY
        do
        {
            step = ((float)homeRandom (TAU,1 - TAU)) * step;
        }
        while(step == 0);
        printf("\n\n-->Actualisation pas : %f",step);

        eswro = ESwithRo(x, step);
        es = ES(x);
        escomp = EScomplement(x ,step);
//        printf("\nCondition : %f <= %f + %f (%f)?",eswro,es,escomp,es + escomp);
    }

//    printf("\nEnded with counter = %d\n\n",counter);
    return step;
}

void resolveGradientOptimal(float x0[], float espilon, int max_iter)
{
    float current_iteration[3], old_iteration[3];
    float norme, diff[3], step;
    float old_error = -1, current_error;
    int counter = 0,i;

    //Info Debug
//    printf("\nVecteur initial x : ");printVector3(x0);

    // Initialize
    for(i=0; i<3; i++)
        old_iteration[i] = x0[i];

    do
    {
        // Recherche pas optimal
        step = computeArmijoStep(old_iteration);

//        printf("\n Nouveau pas : %f\n",step);

        // Calcul it�ration
        for(i=0; i<3; i++)
            current_iteration[i] = old_iteration[i] - (step * gradientES(old_iteration).x[i]);

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

        // Calcul de la norme pour la condition d'arr�t
        norme = normeVR3(diff);

        //Calcul de l'erreur
        current_error = errorES(current_iteration);

        // Info debug
        printf("\n [Current iteration] : ");printVector3(current_iteration);
        printf("\n [Old iteration] : ");printVector3(old_iteration);
        printf("\n Erreur : %f",current_error);
        if(old_error != -1)
            printf(" [ difference ( %f ) ]",(current_error - old_error));

        // Pr�paration prochaine it�ration
        for(i=0; i<3; i++)
            old_iteration[i] = current_iteration[i];

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

        // Condition d'arr�t
    }
    while( norme > espilon && ++counter < max_iter );

    // Info Debug
    printf("\nCounter value : %d",counter);
    printf("\nNorme de la difference : %f",normeVR3(diff));
    printf("\nVecteur optimum : ");printVector3(current_iteration);
    printf("\nFunction value for x : %f",ES(current_iteration));
}