/** @(#) numericalap/finitehesianfunction.cpp */

// #include "finitehesianfunction.h"
/** @(#) finitehesianfunction.h */
#ifndef FINITEHESSIANFUNCTION_H
#define FINITEHESSIANFUNCTION_H
//----------------------------------------------------------------------------
#include "../matrixdouble/squaredmatrix.h"
#include "../matrixdouble/vectordouble.h"
#include "../functionnd/functionnd_1d.h"
#include "../mathpos/mathpos.h"
#include "../utilpos/interfacepos.h"

#include <algorithm>
#include <cmath>
#include <iomanip>
#include <iostream>
#include <string>
#include <stdexcept>	// runtime_error
using namespace std;

/**
* Calcula una aproximacion hacia adelante mediante diferencias finitas hacia
* la hessiana (grad^2(f(xc)) usando solo los valores de f(xc).
* Uso:
*	fc = func.evaluate(xc); // debe ser parametro, modularidad libro
*	finiteHessianFunction(xc, fc, Sx, func, H, 1e-8);
*        H.upperTriangularToSymmetric();  // si desea matriz rellena
* Solo la diagonal y el triangulo superior de H fueron ocupados,
* debido a que estas son las unicas
* porciones de H que son referenciadas por el resto del
* sistema de algoritmos.
* No necesita cambios, si se desea economizar el almacenaminto.
* ei es el i-esimo vector unitario.
* @param n Taman~o de vectores y matrices. No se checan dimensiones.
* @param xc Vector con punto de prueba inicial. De aqui se toma la dimension
* @param fc Evaluacion de la funcion en xc.
* @param Sx Escala. Valores usuales.
* @param theFunction Nombre de la funcion. F:R^n->R. FN.
* @param H return. Aproximacion a la matriz hessiana simetrica
* ( equiv grad^2(f(xc)) ). No resize.
* @param eta Precision de la maquina. Omision: mathpos::MACHEPS.
* @author Omar Posada Villarreal, Daniel Alba Cuellar
* @version 1.0, 04/03/2002
* @see Algoritmo A5.6.2 (FDHESSF). Dennis. Pagina 321.
* Probar con ejemplo 4.1.6. pag 72.
*/
void finiteHessianFunction(int n, VectorDouble &xc, double fc,
		FunctionND_1D &theFunction, VectorDouble &Sx,
		SquaredMatrix &H, double eta = MACHEPS)
		throw(invalid_argument) {
// En depuracion usar checkMethod, al final del archivo
        // { logic
        xc.setLogic(1);
        Sx.setLogic(1);
        H.setLogic(1);
        // } logic

	VectorDouble stepsize(n, 1);
	VectorDouble fneighbor(n, 1);

	double absXc, invSx;
        double fii, fij;
        double tempi, tempj;
	int i, j;
	double cuberteta = pow(eta, 1.0/3.0);		//1.

	// 2.
	for (i = 1; i <= n; ++i) {

		// Calula stepsize[i] y f(xc + stepsize[i] * ei).
		// Aqui se cambia la regla del taman~o de paso.
		absXc = fabs(xc[i]);
		invSx = 1.0 / Sx[i];
		stepsize[i] = cuberteta * max(absXc, invSx) * sign(xc[i]);//2.1
         	//cout << "\nstepsize: " << stepsize;
		tempi = xc[i];				// 2.2
		xc[i] += stepsize[i];			// 2.3

		// Reduce ligeramente los errores por precision finita.
		// ver Section 5.4
		stepsize[i] = xc[i] - tempi;		// 2.4

		// fneighbor[i] = fneighbor[xc + stepsize[i]*ei]
		fneighbor[i] = theFunction.evaluate(xc); // 2.5
                //cout <<"\nfneighbor: " << fneighbor;

		// Restaurar el valor de xc[i]
		xc[i] = tempi;				// 2.6
	}

	// 3.
	for (i = 1; i <= n; ++i) {

		// Calcula el i-esimo renglon de la matriz hessiana (H).
		tempi = xc[i];				// 3.1

		// Alternativamente, xc[i] -= stepsize[i]
		xc[i] += 2 * stepsize[i];		// 3.2

		// fii = f(xc + 2 * stepsize[i] * ei)
		fii = theFunction.evaluate(xc);		// 3.3

        	// 3.4
                // fc es escalar
		H[i][i] = ((fc - fneighbor[i]) + (fii - fneighbor[i]))
		  		/ (stepsize[i] * stepsize[i]);	// 3.4
		xc[i] = tempi + stepsize[i];		// 3.5

		// 3.6
		for (j = (i+1); j <= n; ++j) {

			// Calcula H[i,j]
			tempj = xc[j];			// 3.6.1
			xc[j] += stepsize[j];

			// fij = f(xc + stepsize[i] *ei +stepsize[j] * ej)
			fij = theFunction.evaluate(xc);	// 3.6.3
			H[i][j] = ((fc - fneighbor[i]) + (fij - fneighbor[j]))
			  		/ (stepsize[i] * stepsize[j]);//3.6.4
			// Restaura xc[j]
			xc[j] = tempj;			// 3.6.5
		}
		// Restaura xc[j]
		xc[i] = tempi;				//3.7
                //cout << "\nH:\n"<<H;
	} // for i
}
/** Checa parametros: dimensiones, rangos, divisiones entre cero */
void checkFiniteHessianFunction(int n, VectorDouble &xc, double fc,
		FunctionND_1D &theFunction, VectorDouble &Sx,
		SquaredMatrix &H, double eta = MACHEPS)
                throw(invalid_argument) {
	// Condiciones invalidas
        if ( (n < 1)
        	|| (xc.getSize() != n)
        	|| (Sx.getSize() != n)
        	|| (H.getSize() != n)
                || (eta < 0.0)
        	) {
        	throw invalid_argument("\nfiniteHessFunc: arg invalidos.");
        }
        // Prevee division entre cero
        if (Sx.hasAlmostZeros()) {
        	throw invalid_argument("finiteHessFunc: Division entre cero.");
        }

	finiteHessianFunction(n, xc, fc, theFunction, Sx, H, eta);
}

//----------------------------------------------------------------------------

void finiteHessianFunctionSquare(int n, VectorDouble &xc, double fc,
		FunctionND_1D &theFunction, VectorDouble &Sx,
		SquaredMatrix &H, double eta = MACHEPS)
		throw(invalid_argument) {
// En depuracion usar checkMethod, al final del archivo
        // { logic
        xc.setLogic(1);
        Sx.setLogic(1);
        H.setLogic(1);
        // } logic

	VectorDouble stepsize(n, 1);
	VectorDouble fneighbor(n, 1);

	double absXc, invSx;
        double fii, fij;
        double tempi, tempj;
	int i, j;
	double cuberteta = pow(eta, 1.0/3.0);		//1.

	// 2.
	for (i = 1; i <= n; ++i) {

		// Calula stepsize[i] y f(xc + stepsize[i] * ei).
		// Aqui se cambia la regla del taman~o de paso.
		// Calula stepsize[i] y f(xc + stepsize[i] * ei).
		// Aqui se cambia la regla del taman~o de paso.
		/* { antes
                * absXc = fabs(xc[i]);
		* invSx = 1.0 / Sx[i];
		* stepsize[i] = cuberteta * max(absXc, invSx) * sign(xc[i]);//2.1
                */


                // Raiz cuadrada de MACHEPS
		stepsize[i] = SQRT_MACHEPS;//2.1


         	//cout << "\nstepsize: " << stepsize;
		tempi = xc[i];				// 2.2
		xc[i] += stepsize[i];			// 2.3

		// Reduce ligeramente los errores por precision finita.
		// ver Section 5.4
		stepsize[i] = xc[i] - tempi;		// 2.4

		// fneighbor[i] = fneighbor[xc + stepsize[i]*ei]
		fneighbor[i] = theFunction.evaluate(xc); // 2.5
                //cout <<"\nfneighbor: " << fneighbor;

		// Restaurar el valor de xc[i]
		xc[i] = tempi;				// 2.6
	}

	// 3.
	for (i = 1; i <= n; ++i) {

		// Calcula el i-esimo renglon de la matriz hessiana (H).
		tempi = xc[i];				// 3.1

		// Alternativamente, xc[i] -= stepsize[i]
		xc[i] += 2 * stepsize[i];		// 3.2

		// fii = f(xc + 2 * stepsize[i] * ei)
		fii = theFunction.evaluate(xc);		// 3.3

        	// 3.4
                // fc es escalar
		H[i][i] = ((fc - fneighbor[i]) + (fii - fneighbor[i]))
		  		/ (stepsize[i] * stepsize[i]);	// 3.4
		xc[i] = tempi + stepsize[i];		// 3.5

		// 3.6
		for (j = (i+1); j <= n; ++j) {

			// Calcula H[i,j]
			tempj = xc[j];			// 3.6.1
			xc[j] += stepsize[j];

			// fij = f(xc + stepsize[i] *ei +stepsize[j] * ej)
			fij = theFunction.evaluate(xc);	// 3.6.3
			H[i][j] = ((fc - fneighbor[i]) + (fij - fneighbor[j]))
			  		/ (stepsize[i] * stepsize[j]);//3.6.4
			// Restaura xc[j]
			xc[j] = tempj;			// 3.6.5
		}
		// Restaura xc[j]
		xc[i] = tempi;				//3.7
                //cout << "\nH:\n"<<H;
	} // for i
}

void finiteHessianFunctionCubic(int n, VectorDouble &xc, double fc,
		FunctionND_1D &theFunction, VectorDouble &Sx,
		SquaredMatrix &H, double eta = MACHEPS)
		throw(invalid_argument) {
// En depuracion usar checkMethod, al final del archivo
        // { logic
        xc.setLogic(1);
        Sx.setLogic(1);
        H.setLogic(1);
        // } logic

	VectorDouble stepsize(n, 1);
	VectorDouble fneighbor(n, 1);

	double absXc, invSx;
        double fii, fij;
        double tempi, tempj;
	int i, j;
	double cuberteta = pow(eta, 1.0/3.0);		//1.

	// 2.
	for (i = 1; i <= n; ++i) {

		// Calula stepsize[i] y f(xc + stepsize[i] * ei).
		// Aqui se cambia la regla del taman~o de paso.
		/* { antes
                * absXc = fabs(xc[i]);
		* invSx = 1.0 / Sx[i];
		* stepsize[i] = cuberteta * max(absXc, invSx) * sign(xc[i]);//2.1
                */


                // Raiz cubica de MACHEPS
		stepsize[i] = pow( MACHEPS, 1.0/3.0); //2.1


         	//cout << "\nstepsize: " << stepsize;
		tempi = xc[i];				// 2.2
		xc[i] += stepsize[i];			// 2.3

		// Reduce ligeramente los errores por precision finita.
		// ver Section 5.4
		stepsize[i] = xc[i] - tempi;		// 2.4

		// fneighbor[i] = fneighbor[xc + stepsize[i]*ei]
		fneighbor[i] = theFunction.evaluate(xc); // 2.5
                //cout <<"\nfneighbor: " << fneighbor;

		// Restaurar el valor de xc[i]
		xc[i] = tempi;				// 2.6
	}

	// 3.
	for (i = 1; i <= n; ++i) {

		// Calcula el i-esimo renglon de la matriz hessiana (H).
		tempi = xc[i];				// 3.1

		// Alternativamente, xc[i] -= stepsize[i]
		xc[i] += 2 * stepsize[i];		// 3.2

		// fii = f(xc + 2 * stepsize[i] * ei)
		fii = theFunction.evaluate(xc);		// 3.3

        	// 3.4
                // fc es escalar
		H[i][i] = ((fc - fneighbor[i]) + (fii - fneighbor[i]))
		  		/ (stepsize[i] * stepsize[i]);	// 3.4
		xc[i] = tempi + stepsize[i];		// 3.5

		// 3.6
		for (j = (i+1); j <= n; ++j) {

			// Calcula H[i,j]
			tempj = xc[j];			// 3.6.1
			xc[j] += stepsize[j];

			// fij = f(xc + stepsize[i] *ei +stepsize[j] * ej)
			fij = theFunction.evaluate(xc);	// 3.6.3
			H[i][j] = ((fc - fneighbor[i]) + (fij - fneighbor[j]))
			  		/ (stepsize[i] * stepsize[j]);//3.6.4
			// Restaura xc[j]
			xc[j] = tempj;			// 3.6.5
		}
		// Restaura xc[j]
		xc[i] = tempi;				//3.7
                //cout << "\nH:\n"<<H;
	} // for i
}
#endif
// Fin------------------------------------------------------------------------