//SIN TERMINAR! ! ! !
unit unewton_quad;

(*==============================================================================
  ==============================================================================
 Algoritmo de Newton para resolver el mínimo de una función objetivo N-dimensional.
 Transforma el problema en uno de optimización global sin restricciones a través
 de las condiciones de KKT.

 El algoritmo de máximo descenso:
 Este algoritmo busca el mínimo de una función objetivo N-dimensional
 en la dirección negativa del gradiente:
 -g(x1,...,xN) = -grad_f(x) = -(df/dx1,...,df/dxN)'
 con el tamaño de paso alfa_k (en la iteración k) ajustado de tal forma
 que la función objetivo se minimiza a lo largo de la dirección por un
 método de búsqueda unidimensional (line search).

 -------------------------------------------------------------------------------
 pseudocódigo máximo descenso:

paso 0) sean f   : RN -> R   la función de costo
              g_i : RN -> R   la restricciones de >= 0
              h_i : RN -> R   la restricciones de = 0

         **supongamos que sólo hay >= 0

paso 1) k      <- 0

        elijo lambda_i_k tales que
        si (g_i(x_k) < 0) entonces
           lambda_i_k = -pf(x, g_i(x)) * g_i(x)
        de lo contrario
           lambda_i_k = 0

        construyo el Lagrangeano L = f + sum_i (lambda_i_k * g_i)

        L_xk   <- L(x0)

paso 2.1)  encontrar el tamaño de paso alfa_k a lo largo de la dirección del
           gradiente negativo por el método de aproximación cuadrática:
           GL_k    <- -grad(L)(x_k)
           alfa_k  <- argmin_alfa L(x_k - alfa*GL_k/norm(GL_k))

paso 2.2)  obtener el próximo punto y actualizar L:
            x_ka   <- x_k
            L_ka   <- L_xk
            x_k    <- x_k - alfa*GL_k/norm(GL_k);

            elijo lambda_i_k tales que
            si (g_i(x_k) < 0) entonces
               lambda_i_k = -pf(x, g_i(x)) * g_i(x)
            de lo contrario
               lambda_i_k = 0

            construyo el Lagrangeano L = f + sum_i (lambda_i_k * g_i)


            L_xk   <- L(x_k)

paso 2.3)  si x_ka \approx x_k y L_ka \approx L_xk, entonces declarar
            a x_k el mínimo y terminar. de lo contrario incrementar k y volver
            al paso 2.1.

 -------------------------------------------------------------------------------
 Refs.:
 [1] Luenberger
 [2] Applied Numerical Methods Using MATLAB

================================================================================
================================================================================
*)

{$mode objfpc}{$H+}

interface

uses
  Classes, SysUtils, xMatDefs, matreal,
  ubibfun_onl;

procedure optimizar(f_costo : TFunc_Bloque;
                    g_resDesigualdad,
                    h_resIgualdad : TDAOfTFunc_Bloque;
                    var XOpt : TVectR;
                    x0: TVectR;
					TolX: NReal;
					TolFun: NReal;
					alfa0: NReal;
					MaxIter: NInt);

function linesearch(f: TFunc_Bloque;
                    x_k: TVectR;
                    alfa0: NReal;
                    g_k: TVectR;
                    MaxIter: NInt;
                    TolX: NReal;
                    TolFun: NReal): NReal;


function penaltyFunction(gi_xk : Nreal): NReal;



var
 {$IFDEF DBG}
         fileOut : Text;
 {$ENDIF}
implementation

procedure optimizar(f_costo : TFunc_Bloque;
                    g_resDesigualdad,
                    h_resIgualdad : TDAOfTFunc_Bloque;
                    var XOpt : TVectR;
                    x0: TVectR;
					TolX: NReal;
					TolFun: NReal;
					alfa0: NReal;
					MaxIter: NInt);
var
 alfa_k: NReal;
 k: NInt;
 x_k: TVectR;
 x_ka : TVectR;
 n, nres_des: NInt;
 i: NInt;
 CONVERGIO : Boolean;
 lambdai_k : TVectR;
 L : TFunc_Bloque;
 gi_xk : NReal;
 L_xk, L_xka : NReal;
 GL_k : TVectR;
 GL_k_normEuclid : NReal;
begin

     n := f_costo.nvars;
     nres_des := length(g_resDesigualdad);
     x_k := TVectR.Create_Clone(x0);
     alfa_k:=alfa0;
     k:=0;
     lambdai_k := TvectR.Create_Init(length(g_resDesigualdad));

     for i:= 1 to length(g_resDesigualdad) do begin
         gi_xk := g_resDesigualdad[i-1].Evaluar(x_k);

         if (gi_xk < 0) then
             lambdai_k.pon_e(i,  penaltyFunction(gi_xk) * gi_xk)
         else
             lambdai_k.pon_e(i, 0);
     end;

     {$IFDEF DBG}
          //imprimir en consola
          for i:=1 to length(g_resDesigualdad) do begin
              writeln('lambda ',i,' paso 0 ', ' = ', lambdai_k.e(i):1:6);

          end;
      writeln('=================================================');
     {$ENDIF}

     //construir lagrangreano
     L := TFunc_Bloque.Create;
     L.Agregar_f(f_costo);
     for i:= 1 to length(g_resDesigualdad) do begin
         L.Agregar_f(g_resDesigualdad[i-1], lambdai_k.e(i));
     end;
     L_xk := L.Evaluar(x0);

{$IFDEF DBG}
     //imprimir en consola
{$ENDIF}
     repeat begin
            GL_k := TVectR.Create_Clone(L.Gradiente(x_k));
            GL_k_normEuclid := GL_k.normEuclid;
            if (GL_k_normEuclid <> 0) then
               GL_k.PorReal(1/GL_k_normEuclid);

            //LINE SEARCH : hallar alfa_k = argmin_alfa f(x_k - alfa*g_k)
            alfa_k := linesearch(L, x_k, alfa_k, GL_k,
                                 MaxIter, TolX, TolFun);

            x_ka:=TVectR.Create_Clone(x_k);
            L_xka:=L_xk;
            for i:=1 to n do
                x_k.pon_e(i, x_k.e(i) - alfa_k * GL_k.e(i));

            //recalcular lambda_i y reconstruir L
            for i:= 1 to length(g_resDesigualdad) do begin
                gi_xk := g_resDesigualdad[i-1].Evaluar(x_k);
                if (gi_xk < 0) then
                    lambdai_k.pon_e(i,  penaltyFunction(gi_xk) * gi_xk)
                else
                    lambdai_k.pon_e(i, 0);
            end;
            for i:=1 to length(g_resDesigualdad) do begin
                L.coef[i-1] := lambdai_k.e(i);
            end;
            L_xk:=L.Evaluar(x_k);

            CONVERGIO := (x_ka.distancia(x_k) < TolX) and (abs(L_xka-L_xk) < TolFun); //cambiando "and" por "or" se requiere convergencia mas 'dèbil'

            k:=k+1;
{$IFDEF DBG}

{$ENDIF}
     end until (CONVERGIO);
     xOpt := TVectR.Create_Clone(x_k);

{$IFDEF DBG}

{$ENDIF}

end;

function linesearch(f: TFunc_Bloque;
                    x_k: TVectR;
                    alfa0: NReal;
                    g_k: TVectR;
                    MaxIter: NInt;
                    TolX: NReal;
                    TolFun: NReal): NReal;
var
 num, den : NReal;
 alfa_opt,x0,x1,x2,x3:NReal; //representan valores del parámetro real alfa
 f0,f1,f2,f3:NReal;  //evaluaciones de f(x_k - alfa*g_k), es una función de R->R en alfa
 vaux: TVectR;
 k: NInt;
begin
     k:=MaxIter;

     //generar el initial guess
     x0:=alfa0;
     x1:=alfa0*1.5;
     x2:=alfa0*2.0;

     vaux:=TVectR.Create_Clone(g_k); vaux.PorReal(-x0); vaux.Suma(x_k,vaux);
     f0:=f.Evaluar(vaux);

     vaux:=TVectR.Create_Clone(g_k); vaux.PorReal(-x1); vaux.Suma(x_k,vaux);
     f1:=f.Evaluar(vaux);

     vaux:=TVectR.Create_Clone(g_k); vaux.PorReal(-x2); vaux.Suma(x_k,vaux);
     f2:=f.Evaluar(vaux);

     //iteración
     repeat
           //me calculo los nuevos x3, f3
           num:=f0*(x1*x1-x2*x2)+f1*(x2*x2-x0*x0)+f2*(x0*x0-x1*x1);
           den:=2*(f0*(x1-x2)+f1*(x2-x0)+f2*(x0-x1));
           x3:=num/den;
           vaux:=TVectR.Create_Clone(g_k); vaux.PorReal(-x3); vaux.Suma(x_k, vaux);
           f3:=f.Evaluar(vaux);

           if (k <= 0) or (abs(x3-x1) < TolX) or (abs(f3-f1) < TolFun) then begin
              break;
           end
           else begin
                if (x3 < x1) then
                       if (f3 < f1) then begin
                          f0:=f0; f2:=f1; f1:=f3;
                          x0:=x0; x2:=x1; x1:=x3;
                       end else begin
                          f0:=f3; f1:=f1; f2:=f2;
                          x0:=x3; x1:=x1; x2:=x2;
                       end
                else //x3 > x1
                       if (f3 < f1) then
                       begin
                          f0:=f1; f1:=f3; f2:=f2;
                          x0:=x1; x1:=x3; x2:=x2;
                       end else begin
                          f0:=f0; f1:=f1; f2:=f3;
                          x0:=x0; x1:=x1; x2:=x3;
                       end;
           end;
           k:=k-1;
     until (false);  //el escape está en el if de arriba

     x0:=x3;
     f0:=f3;
     alfa_opt:=x0;
     Result := alfa_opt;
end;

function penaltyFunction(gi_xk : Nreal): NReal;
begin
     Result := 10;
end;




end.
