unit upenaltyMethod_m3;
(*
maneja restricciones de desigualdad igual que el algoritmo upenaltyMethod_m1 pero
también maneja restricciones de igualdad.

para las restricciones de igualdad, se agregan dos restricciones de desigualdad, ej
f(x) = 0 => f(x) >= 0  y -f(x) >= 0  =====TODO=====

---------------------------------------------------------------------------------------------
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;
                    h : 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;
                                       h : 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; h: TDAOfTFunc_Bloque; c: NReal; x:TVectR): NReal;

function esFactible(g : TDAOfTFunc_Bloque;
                    h: 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, nres_igu: NInt;
 i, j, jj, pos: 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;
 grad_x1, grad_x2: NReal;
begin

     n := f_costo.nvars;
     nres_des := length(g_resDesigualdad);
     nres_igu := length(h_resIgualdad);
     k:=0;
     kr:=0;
     MaxRuns := MaxIter;//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,h_resIgualdad,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);

     //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,h_resIgualdad,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));
     agr2:=TVectR.Create_Init(Gq_xk.n);
     //recorro todas las restricciones. lo importante es ubicar correctamente
     //a la derivada en el vector xa
     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);
            //recorrer las variables
            for j:=1 to g_resDesigualdad[i-1].nvars do begin
                //encontrar dónde las tengo que ubicar en el vector de idvars del gradiente (agr2)
                for jj:=1 to agr2.n do begin
                    if (g_resDesigualdad[i-1].idVars[j-1]=f_costo.idVars[jj-1]) then begin
                       //lo encontré
                       pos:=jj;
                       break;
                    end;
                end;
                agr2.acum_e(pos,agr1.e(j));
            end;
         end;
     end;
     for i:=1 to nres_igu do begin
         ar := h_resIgualdad[i-1].Evaluar(x_k);
         if (ar < 0) then begin //la pongo "del lado de afuera
            agr1:=TVectR.Create_Clone(h_resIgualdad[i-1].Gradiente(x_k));
            agr1.PorReal(2*c_kr*ar);
            //recorrer las variables
            for j:=1 to h_resIgualdad[i-1].nvars do begin
                //encontrar dónde las tengo que ubicar en el vector de idVars del gradiente (agr2)
                for jj:=1 to agr2.n do begin
                    if (h_resIgualdad[i-1].idVars[j-1]=f_costo.idVars[jj-1]) then begin
                       //lo encontré
                       pos:=jj;
                       break;
                    end;
                end;
                agr2.acum_e(pos,agr1.e(j));
            end;
         end;
     end;

     Gq_xk.sum(agr2);

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

      grad_x1:=-Gq_xk.e(1); grad_x2:=-Gq_xk.e(2);  //-gradiente: PARA DEBUG


     writeln('       ' ,'-grad(1) = ',grad_x1:1:6, '  -grad(2) = ', grad_x2:1:6);
     writeln('-----------------------------------------------------------------------------');

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

           //PASO CONSTANTE:
           //   alfa_k := alfa0/10;

           //CRITERIO DE ARMIJO:
             alfa_k := linesearch_criterio_de_armijo(f_costo, g_resDesigualdad,h_resIgualdad,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,h_resIgualdad, 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(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, h_resIgualdad,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;
                    h : 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, h, c, vaux);

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

     vaux:=TVectR.Create_Clone(g_k); vaux.PorReal(-x2); vaux.Suma(x_k,vaux);
     f2:=evaluar_q(f, g, h, 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, h, 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; h: TDAOfTFunc_Bloque; c: NReal; x:TVectR): NReal;
var
 f_xk, q_xk, spenfun : NReal;
 nres_des : NInt;
 ar : Nreal;
 P_xk : NReal;
  i : NInt;
  P_res_des : NReal;
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;
     P_xk:=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:=P_xk+spenfun;

     spenfun:=0;

     for i:=1 to high(h)-low(h)+1 do begin
         ar := h[i-1].Evaluar(x);
         if (ar < 0) then //Calibrar??
            spenfun:=spenfun + ar*ar;
     end;

     P_xk:=P_xk + spenfun;
     q_xk := f_xk + c * P_xk;
     Result := q_xk;
end;

function esFactible(g : TDAOfTFunc_Bloque;
                    h : 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;
     if not (r=FALSE) then begin
        for i:=low(h) to high(h) do begin
            if (h[i].Evaluar(x) < 0) then
               r := FALSE;
               break;
        end;
     end;
     Result := r;
end;

function linesearch_criterio_de_armijo(f : TFunc_Bloque;
                                       g : TDAOfTFunc_Bloque;
                                       h : 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, h, 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, h,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, h, 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.
