package optimizacion;

import operaciones.Matrices;
/**
 * <p>Title: Metodo de Levenberg Marquart</p>
 * <p>Description: Minimiza una funcion utilizando el
 * metodo de LM</p>
 * <p>Copyright: Copyright (c) 2006</p>
 * <p>Company: UMSNH</p>
 * @author Dr. Felix Calderon Solorio
 * @version 1.0
 */


public class LM {
    public double lambda; // Lambda del metodo de LM
    public double epsilon = 1e-5; // criterio de paro
    public double lam0 = 1e-03; // Lamda inicial
    public int MAX_ITER = 500; // numero maximo de iteraciones
    public int Num_Par = 2; // numero de parametros
    public double Lambda_MAX = 1e40; // maximo valor de Lambda

    public LM(int numero_parametros, double Tolerancia)
    {
        Num_Par = numero_parametros;
        epsilon = Tolerancia;
    }

    // **************************************************************
    //
    //     Metodo de Levenbert Marquart
    //
    // **************************************************************

    public double Levenbert_Marquart(double x[]) {
        int i, j, M = Num_Par, iter = 0;
        double g[] = new double[M];
        double H[][] = new double[M][M];
        double e0, e1;
        String salida;

        lambda = lam0;

        e1 = funcion(x);

        do {
            iter++;

            for (i = 0; i < M; i++) {
                g[i] = 0;
                for (j = 0; j < M; j++) {
                    H[i][j] = 0;
                }
            }

            Gradiente(g, x);
            Hessiano(H, x);

            e0 = e1;

            e1 = IteraLM(H, g, x, e0);
            imprime(iter, e1, x);

        } while ((e1 < e0) && (e0 - e1) > epsilon && iter < MAX_ITER);

        return e1;
    }
    
    /**
     * funcion que imprime 
     * @param iter
     * @param e1
     * @param x
     */

    public void imprime(int iter, double e1, double x[]) {
        String salida;
        salida = "Solucion LM en ";
        salida += "Iteracion " + iter + " error = \t " + e1;
        System.out.print(salida);
        System.out.print(" x = {");
        for (int i = 0; i < Num_Par; i++) System.out.print(x[i] + ", ");
        System.out.println("}");
    }

    // *************************************************************************

    public double IteraLM(double H0[][], double g0[], double x0[],
                                 double e0) {
        int M = Num_Par, i, j;
        double x1[] = new double[M];
        double e1, lambda = lam0;
        double dT[] = new double[M];
        double H[][] = new double[M][M];
        double g[] = new double[M];

        do {
            for (i = 0; i < M; i++) {
                g[i] = g0[i];
                for (j = 0; j < M; j++) {
                    H[i][j] = H0[i][j];
                    if (i == j)
                        H[i][j] += lambda;
                }
            }

            Matrices.SolucionSL(H, dT, g);

            for (i = 0; i < M; i++)
                x1[i] = x0[i] - dT[i];

            e1 = funcion(x1);
            if (e1 > e0) lambda *= 10.0;

        } while (e1 > e0 && lambda < Lambda_MAX);

        lambda = lambda > lam0 ? lambda / 10 : lambda;

        if (e1 < e0) {
            for (i = 0; i < M; i++)
                x0[i] = x1[i];
            return e1;
        }
        return e0;
    }

    public double funcion(double x[]) {
        double f, c = 2.0 * Math.PI;

        f = 20.0 +
            x[0] * x[0] - 10.0 * Math.cos(c * x[0]) +
            x[1] * x[1] - 10.0 * Math.cos(c * x[1]);
        return f;
    }

    public void Gradiente(double g[], double x[]) {
        double c = 2.0 * Math.PI;

        g[0] = 2.0 * x[0] + 10.0 * c * Math.sin(c * x[0]);
        g[1] = 2.0 * x[1] + 10.0 * c * Math.sin(c * x[1]); ;
    }

    public void Hessiano(double H[][], double x[]) {
        double c = 2.0 * Math.PI;

        H[0][0] = 2.0 + 10.0 * c * c * Math.cos(c * x[0]);
        H[0][1] = 0.0;
        H[1][0] = 0.0;
        H[1][1] = 2.0 + 10.0 * c * c * Math.cos(c * x[0]);
    }

    static public void main(String args[]) {
        double x[] = {0.7, 0.7};

        LM aplica = new LM(2, 1e-05);
        aplica.Levenbert_Marquart(x);
    }
}