#include <stdlib.h>

#include <string.h>
#include <math.h>

#include "problema.h"
#include "method.h"

#include "logger.h"
#include "utilities.h"

#define STEP0    100

#define CUSTOM   0

#define TAU      0.01
#define OMEGA    0.0001

#define MAX_ITER 100

#define LOG_LEVEL INFO

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

	if( ! ( pb->_derivees[0]) )
		free(calculGradient);

	logMessage(DEBUG3,"ex with ro vector",printVector(copy,pb->_dimension));

	return pb->compute(pb, 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
				);
	}

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

	if( ! ( pb->_derivees[0]) )
			free(calculGradient);

	logMessage(DEBUG3,"ex omplement result",doubleToString(result));

	return result;
}

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

	//Debug
	logMessage(DEBUG3, "newline", "");
	logMessage(DEBUG3, "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(DEBUG2, "newline", "");
	logMessage(DEBUG2, "méthode armijo : pas initial", doubleToString(step));

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

	logMessage(DEBUG2, "condition : J( x_{k} + ro_{k}^{i} * d_{k})",
			doubleToString(eswro));
	logMessage(DEBUG2, "condition : J( x_{k} )", doubleToString(es));
	logMessage(DEBUG2, "condition : - omega_{1} * ro_{k}^{i} * d_{k} * d_{k}",
			doubleToString(escomp));
	logMessage(DEBUG2, "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 ( fabs(step) <= 0.00001 );

		logMessage(DEBUG3, "step counter", integerToString(counter));
		logMessage(DEBUG2, "actualisation pas", doubleToString(step));

		eswro = ESwithRo(x, pb, step);
		es = pb->compute(pb, x, pb->_dimension)[0];
		escomp = EScomplement(x, pb, step);

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

	}

	logMessage(DEBUG2, "counter value", integerToString(counter));
	logMessage(DEBUG2, "counter limit", integerToString(MAX_ITER));
	logMessage(DEBUG2, "resultat optimisation pas", doubleToString(step));

	return step;
}

void resolveByGradientOptimalStep(Problema* pb, Method* m,
		double init_vector[], MethodParameters 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;
	double epsilon = *((double *) (args.parameters[INDEX_EPSILON]));
	int max_iteration = *((int *) (args.parameters[INDEX_PATIENCE]));
	boolean toContinue = 1;

    activateLogger();
    setLoggerLevel(LOG_LEVEL);

	logMessage(DEBUG1, "entering method", "resolveByGradientOptimalStep");
	logMessage(DEBUG2, "parameters : problem name", pb->name);
	logMessage(DEBUG2, "parameters : method name", m->_name);
	logMessage(DEBUG2, "parameters : vector", printVector(init_vector,
			pb->_dimension));
	logMessage(DEBUG2, "parameters : epsilon in", doubleToString(epsilon));
	logMessage(DEBUG2, "parameters : patience", integerToString(max_iteration));

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

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

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

		logMessage(DEBUG2, "armijo step", doubleToString(step));

    	if( ! ( pb->_derivees[0] ) )
    		free(derivee_buffer);


		// 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);
		}
		// Calcul it�ration
		for (i = 0; i < pb->_dimension; i++)
			current_iteration[i] = old_iteration[i]
					- (step * derivee_buffer[i]);

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

		if (counter < 10) {
			logMessage(DEBUG1, "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(DEBUG2, "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(DEBUG2, "error", doubleToString(current_error));
			if (old_error != -1)
				logMessage(DEBUG2, "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 > epsilon);
		toContinue = toContinue && (++counter < max_iteration);

		logMessage(DEBUG2, "counter", integerToString(counter));

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

	saveAlgorithmData(m,pb,
            args,counter,current_iteration,pb->compute(pb,current_iteration,pb->_dimension)[0],current_error);

}