unit upen_barr_mixed_Method_m2;
(*
Maneja restricciones de igualdad y de desigualdad. Se define la función logarítmica-cuadrática:
q(c_kr, x_k) = f(x_k) - 1/c_kr*sum_i (log(abs(g_i(x_k)))) + c_k*sum_i (h_i**2)
---------------------------------------------------------------------------------------------
descripción del algoritmo:
paso 0)  x_k <- x0
         c_kr <- c0
         f_xk <- f(x_k)
         q_xk <- q(c_kr, x_k) = f_xk + c_kr*P(x) , siendo P(x) = sum_{i=1}^{i=m} min(0, g_i(x))^2

paso 1)  x_ka <- x_k
         q_xka <- q_xk
         resolver el problema de optimización SIN restricciones:
         q_xka <- q_xk
         x_k <- min q(c_kr, x) , condicion inicial x_k
         q_xk <- q(c_kr, x_k)

paso 3)  si x_k \approx x_ka y q_ka \approx q_xk entonces
                declarar a x_k el óptimo y terminar.
         de lo contrario
                incrementar k
                c_kr <- c_k*(1+sqrt(5))/2
                y volver al paso 1.
---------------------------------------------------------------------------------------------
*)
{$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);

//linesearch quadratic fitness
function linesearch(f : TFunc_Bloque;
                    g : TDAOfTFunc_Bloque;
                    x_k: TVectR;
                    c : NReal;
                    alfa0: NReal;
                    g_k: TVectR;
                    MaxIter: NInt;
                    TolX: NReal;
                    TolFun: NReal): NReal;

//linesearch aproximado : criterio de Armijo
function linesearch_criterio_de_armijo(f : TFunc_Bloque;
                                       g : TDAOfTFunc_Bloque;
                                       c : Nreal;
                                       x_k : TVectR;
                                       d_k : TVectR;  //dirección de descenso escogida
                                       alfa0 : NReal;
                                       MaxIter : NInt;
                                       TolFun : NReal
                                       ): NReal;

function evaluar_q(f: TFunc_Bloque; g: TDAOfTFunc_Bloque; c: NReal; x:TVectR): NReal;

function esFactible(g : TDAOfTFunc_Bloque;
                    x : TVectR) : Boolean;

const
  GOLDEN_NUMBER = 1.6180339887;
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
 f_xk : NReal;
 alfa_k: NReal;
 k: NInt;
 x_k: TVectR;
 x_ka : TVectR;
 n, nres_des: NInt;
 i: NInt;
 Gq_xk_normEuclid: NReal;
 CONVERGIO : Boolean;
 c_kr : NReal;
 q_xk, q_xka : NReal;
 Gq_xk : TVectR;
 ar : NReal;
 c0 : NReal;
 agr1, agr2 : TVectR;
 kr, MaxRuns : NInt;
 gq1, gq2 : NReal;
 xx1, xx2 : NReal;
 MinRuns : NInt;
 ES_FACTIBLE : Boolean;
begin

     n := f_costo.nvars;
     nres_des := length(g_resDesigualdad);
     k:=0;
     kr:=0;
     MaxRuns := MaxIter;
     MinRuns := -1;

     x_k := TVectR.Create_Clone(x0);
     x_ka := TVectR.Create_Clone(x_k);

     c0 := 1;
     c_kr := c0;

     f_xk := f_costo.Evaluar(x_k);
     q_xk := evaluar_q(f_costo, g_resDesigualdad,c_kr, x_k);

     {$IFDEF DBG}
     //imprimir en consola
     writeln('--------------------------------------------------------------------------------');
     writeln('paso = ', k,'  x_k(1) = ',x_k.e(1):1:6, '  x_k(2) = ', x_k.e(2):1:6,'  q_xk = ', q_xk:1:6,'  alfa_k = ',alfa0:1:4);
     writeln('-----------------------------------------------------------------------------');
     //tambien imprimir en archivo
     Assign (fileOut, 'a.txt');
     Rewrite (fileOut);
     writeln(fileOut,'kr ','paso    ','x_k(1)    ', 'x_k(2)    ','q_xk    ','alfa_k     ');
     writeln(fileOut,kr, '    ',k,'    ',x_k.e(1):1:6,'    ',x_k.e(2):1:6,'    ',q_xk:1:6,'    ',alfa0:1:6);
{$ENDIF}


     repeat begin       //bucle exterior : ir "levantando" las paredes con la sucesión c_kt
            k:=0;
            x_ka := TVectR.Create_Clone(x_k);
            alfa_k:=alfa0;  //para el control del paso del algoritmo de minimización sin restricciones

            f_xk := f_costo.Evaluar(x_k);
            q_xk := evaluar_q(f_costo, g_resDesigualdad,c_kr, x_k);

     repeat begin       //bucle interior: resolver problema de minimización SIN restricciones para q(c_k, x);

            Gq_xk := TVectR.Create_Clone(f_costo.Gradiente(x_k));

            gq1:=Gq_xk.e(1); gq2:=Gq_xk.e(2);    //debug

            agr2:=TVectR.Create_Init(Gq_xk.n);
            for i:=1 to nres_des do begin
                ar := g_resDesigualdad[i-1].Evaluar(x_k);
                if (ar < 0) then begin
                   agr1 := TVectR.Create_Clone(g_resDesigualdad[i-1].Gradiente(x_k));
                   agr1.PorReal(2*c_kr*ar);
                   agr2.sum(agr1);
                end;
            end;
            Gq_xk.sum(agr2);

            gq1 := Gq_xk.e(1); gq2:=Gq_xk.e(2);     //debug

            Gq_xk_normEuclid := Gq_xk.normEuclid;
            if (Gq_xk_normEuclid <> 0) then
               Gq_xk.PorReal(1/Gq_xk_normEuclid);

            //LINE SEARCH : hallar alfa_k = argmin_alfa q(x_k - alfa*Gq_k)

            //LINESEARCH CUADRÁTICO:
            //alfa_k := linesearch(f_costo, g_resDesigualdad, x_k, c_kr, alfa_k, Gq_xk, MaxIter, TolX, TolFun);

           //REDUCCIÓN CONSTANTE:
           // alfa_k := alfa_k / GOLDEN_NUMBER;

           //CRITERIO DE ARMIJO:
             alfa_k := linesearch_criterio_de_armijo(f_costo, g_resDesigualdad,c_kr, x_k, Gq_xk, alfa_k, 10, TolFun);


            x_ka:=TVectR.Create_Clone(x_k);
            q_xka:=q_xk;
            for i:=1 to x_k.n do                     //dar el paso
                x_k.pon_e(i, x_k.e(i) - alfa_k * Gq_xk.e(i));

            xx1:=x_k.e(1); xx2:=x_k.e(2);

            q_xk := evaluar_q(f_costo, g_resDesigualdad, c_kr, x_k);

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

            if not (CONVERGIO) then begin
               k:=k+1;
            end;

            {$IFDEF DBG}
        writeln('run = ' ,kr, ' paso = ', k,'  x_k(1) = ',x_k.e(1):1:6, '  x_k(2) = ', x_k.e(2):1:6,'  q_xk = ', q_xk:1:6,'  alfa_k = ',alfa_k:1:6);
        writeln('-----------------------------------------------------------------------------');
        writeln(fileOut,kr, '    ',k,'    ',x_k.e(1):1:6,'    ',x_k.e(2):1:6,'    ',q_xk:1:6,'    ',alfa_k:1:6);
{$ENDIF}


     end until ((CONVERGIO) or (k > MaxIter));



     xx1:=x_k.e(1); xx2 := x_k.e(2);

     if not (esFactible(g_resDesigualdad, x_k)) then begin
        ES_FACTIBLE := FALSE;
        c_kr := c_kr * GOLDEN_NUMBER * GOLDEN_NUMBER * GOLDEN_NUMBER ;
        kr:=kr+1;
     end else
        ES_FACTIBLE := TRUE;


     end until (((ES_FACTIBLE) or ( kr > MaxRuns)) and (kr > MinRuns));

{$IFDEF DBG}
Close(fileOut);
{$ENDIF}

     xOpt := TVectR.Create_Clone(x_k);

     f_xk:=f_costo.Evaluar(xOpt);
     xx1:=xOpt.e(1); xx2:=xOpt.e(2);

end;

function linesearch(f : TFunc_Bloque;
                    g : TDAOfTFunc_Bloque;
                    x_k: TVectR;
                    c : Nreal;
                    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:=evaluar_q(f, g, c, vaux);

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

     vaux:=TVectR.Create_Clone(g_k); vaux.PorReal(-x2); vaux.Suma(x_k,vaux);
     f2:=evaluar_q(f, g, c, 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));

           if (den <> 0) then
              x3:=num/den;

           vaux:=TVectR.Create_Clone(g_k); vaux.PorReal(-x3); vaux.Suma(x_k, vaux);
           f3:=evaluar_q(f, g, c, 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 evaluar_q(f: TFunc_Bloque; g: TDAOfTFunc_Bloque; c: NReal; x:TVectR): NReal;
var
 f_xk, q_xk, spenfun : NReal;
 nres_des : NInt;
 ar : Nreal;
 P_xk : NReal;
  i : NInt;
begin
     f_xk := f.Evaluar(x);
     q_xk := f_xk;   //armar q(c, x) : no utilizo una TFunc porque en este caso parecería que es màs eficiente evaluarla directamente de acuerdo a la elección de la función de penalización adoptada
     spenfun := 0;
     nres_des := high(g) - low(g) + 1;
     for i:=1 to nres_des do begin
         ar := g[i-1].Evaluar(x);
         if (ar < 0) then
            spenfun := spenfun + ar*ar;
     end;
     P_xk:=spenfun;
     q_xk := f_xk + c * P_xk;
     Result := q_xk;
end;

function esFactible(g : TDAOfTFunc_Bloque;
                    x : TVectR) : Boolean;
var
 r : Boolean; i: NInt;
begin
     r := TRUE;
     for i:=low(g) to high(g) do begin
         if (g[i].Evaluar(x) < 0) then begin
            r := FALSE;
            break;
         end;
     end;
     Result := r;
end;

function linesearch_criterio_de_armijo(f : TFunc_Bloque;
                                       g : TDAOfTFunc_Bloque;
                                       c : Nreal;
                                       x_k : TVectR;
                                       d_k : TVectR;  //dirección de descenso escogida
                                       alfa0 : NReal;
                                       MaxIter : NInt;
                                       TolFun : NReal
                                       ): NReal;
var
  alfa, alfaOpt : NReal;
  eta, epsilon : Nreal;
  SATISFACE : Boolean;
  phi_0, phi_alfa, phi_eta_alfa, phi_prima_0 : NReal;
  x_ks : TVectR;  // x_k siguiente ( = x_{k+1} )
  grad : TVectR;
  k : NInt;
begin
     //parámetros del criterio de armijo
     eta := 2;
     epsilon := 0.001;
     alfa := alfa0;
     phi_0 := evaluar_q(f, g, c, x_k);
     k:=0;
     //calcular phi'(0) : utilizo el parámetro d_k que es el gradiente
     grad := TVectR.Create_Clone(d_k);
     grad.PorReal(-1);
     phi_prima_0 := grad.PEV(d_k);

     repeat begin
            x_ks := TVectR.Create_Clone(d_k);
            x_ks.PorReal(-alfa);
            x_ks.sum(x_k);
            phi_alfa := evaluar_q(f, g, c, x_ks);

            SATISFACE := (phi_0 + epsilon * phi_prima_0 * alfa - phi_alfa >= TolFun);
            if not SATISFACE then
               alfa := alfa/eta;
            k := k+1;
     end until ((SATISFACE) or (k > MaxIter));

     k:=0;
     repeat begin
            x_ks := TVectR.Create_Clone(d_k);
            x_ks.PorReal(-alfa*eta);
            x_ks.sum(x_k);
            phi_eta_alfa := evaluar_q(f, g, c, x_ks);

            SATISFACE := (phi_0 + epsilon * phi_prima_0 * eta * alfa - phi_eta_alfa >= TolFun);

            if SATISFACE then
               alfa := alfa * eta;
            k := k+1;
     end until ( (not (SATISFACE)) or (k > MaxIter));

     alfaOpt := alfa;
     Result := alfaOpt;
end;


end.
