/**
 *  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}
 *
 */
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
//lib aleatoire
#include <time.h>
#include <conio.h>

#define TAU      0.01
#define OMEGA    0.0001

#define EPSILON  0.000001
#define MAX_ITER 100000

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

typedef struct _vr3
{
    float x[3];
} VR3;

typedef struct _vr2
{
    float x[2];
} VR2;

void printVector3(float x[])
{
    printf("[%.3f,%.3f,%.3f]",x[0],x[1],x[2]);
}

void copyArray3(float copy[],float x[])
{
    int i;
    for(i=0;i<3;i++)
        copy[i] = x[i];
}

/**
 * Fonction à étudier
 * * [1 + (x1 + x2 + 1)2 . (19 - 14x1 + 3x12 - 14x2 + 6x1x2 + 3x22)].[30 + (2x1 - 3x2)2 . (18 - 32x1 + 12x12 + 48x2 - 36x1x2 + 27x22)]
 * Domaine de recherche :
 * * -2 < xj < 2 ; j := [1,2]
 * * 4 minima locaux
 * * 1 minimum global :
 *      (x1,x2)* = (0,-1);
 *      GP((x1,x2)*) = 3;
 */
float GP1(float x[])
{
    return (
            1
            + pow(
                  x[0]
                  + x[1]
                  + 1
                ,2)
            *
                (
                 19
                 - 14 * x[0]
                 + 3 * pow(x[0],2)
                 - 14 * x[1]
                 + 6 * x[0] * x[1] +
                 3 * pow(x[0],2)
                 )
            ) *
            (
             30
             + pow(
                2 * x[0]
                - 3 * x[1]
               ,2)
             *
                (
                 18
                 - 32 * x[0]
                 + 12 * pow(x[0],2)
                 + 48 * x[1]
                 - 36 * x[0] * x[1]
                 + 27 * pow(x[1],2)
                 )
             );
}

float ES(float x[])
{
    return x[0]*x[0] + x[1]*x[1] + x[2]*x[2];
}

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
 */
double 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 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
    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 );
}

double computeArmijoStep(float x[])
{
    // Initialize - ARBITRARY VALUE
    double step = 10;          // Pas initial
    float randomValue;
    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 = random (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], norme, diff[3], step;
    int counter = 0,i;

    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);

        // Info debug
//        printf("\n [Current iteration] : ");printVector3(current_iteration);
//        printf("\n [Old iteration] : ");printVector3(old_iteration);

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

    // 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));
}

void resolveGradientPasFixe(float x0[], float pas0, float espilon, int max_iter)
{
    float current_iteration[3], old_iteration[3], norme, diff[3];
    int counter = 0,i;

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

    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);

//        printf("\n Current : ");
//        printVector3(current_iteration);
//        printf("\n Old : ");
//        printVector3(old_iteration);

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

    }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));
}


int main()
{
    int choix = 2;
    float x0[3] = {3,3,3}, pas;

    // Init the random seed
    srand (time (NULL));

    printf("\nVecteur initial x : ");
    /*
     * saisir le vecteur en donnant chaque composante séparée par une virgule.
     * e.g : 5.0,3.0,4.0
     */
//    scanf("%f,%f,%f",&x0[0],&x0[1],&x0[2]);
    printVector3(x0);
    printf("\nFunction value for x : %f",ES(x0));

    printf("\nChoix de la méthode :");
    printf("\n1 - Gradient à pas fixe");
    printf("\n2 - Gradient à pas optimal");
    printf("\n3 - Benchmark\n");

//    scanf("%d",&choix);
    printf("%d\n",choix);

    switch(choix)
    {
        case 1 :
            printf("\nPas : ");
            /*
             * saisir un pas assez faible, pour que le prochain point soit dans le voisinage du premier vecteur.
             * e.g : 0.1
             */
            scanf("%f",&pas);
            printf("5\n");

            resolveGradientPasFixe(x0,pas,EPSILON,MAX_ITER);
            break;
        case 2 :
            resolveGradientOptimal(x0,EPSILON,MAX_ITER);
            break;
        case 3 :
            printf("\nPas : ");
            /*
             * saisir un pas assez faible, pour que le prochain point soit dans le voisinage du premier vecteur.
             * e.g : 0.1
             */
//            scanf("%f",&pas);
            pas = 0.1;
            printf("%f\n",pas);

            printf("\nGRADIENT PAS FIXE :");
            resolveGradientPasFixe(x0,pas,EPSILON,MAX_ITER);
            printf("\n\nGRADIENT PAS OPTIMAL :");
            resolveGradientOptimal(x0,EPSILON,MAX_ITER);
            break;
    }

    return EXIT_SUCCESS;
}