unit uAjusteFiltroAutoCorrelacion;

{$mode delphi}

interface

uses
  Classes, SysUtils,
  umodelosintcegh,
  uproblema,
  xmatdefs,
  matreal,
  ufxgx;

const
  AJUSTE_MIN = -9;
  AJUSTE_MAX = 9;


type

  { TProblemaAjusteFiltroAutoCorrelacion }

  TProblemaAjusteFiltroAutoCorrelacion = class(TProblema_m01)

    R: TDAofMatR;
    pesos: TVectR;

    A: TMatR;
    NCanales: integer;

    //AutoCorrelaciones = [ R(0), R(1), ...R(N) ]
    // ponderaciones  =  [ p(1) ... p(N) ]
    constructor Create(AutoCorrelaciones: TDAofMatR; ponderaciones: TVectR);

    function ResolverFiltro(var filtroA: TMatR): boolean;

  end;


type

  { T_fObjetivoAjusteFiltroAutoCorrelacion }

  T_fObjetivoAjusteFiltroAutoCorrelacion = class(Tfx)


    //  Buscar la matriz de filtro de autocorrelacion A que minimice la suma de
    //  los errores ponderados por pesos:
    //                                    min_A ( SUMA_{i=1}^n (p_i e_i^2) )

    //  1-  Se crea el ajuste en base a las matrices de correlacion y los pesos
    //  2-  Se calcula el error cuadratico e2 con el A usado
    //    2.1- Se calcula e_k^2
    //      2.1.1-  Calcula A en base a X
    //      2.1.2-  Calcula A^k -> reshape
    //      2.1.3-  Calcula M_k
    //    2.2- Se calcula p_k * e_k^2
    //    2.3- Se calcula e^2 = SUMA_k^Npasos ( p_k * e_k^2 )
    //  3-  Se calcula el gradiente

    //  M : array de matrices de autocorrelaciones  R(n) * R(0)^(-1).
    //    R[n] = r(n) = Sigma_{X,X} (n) = < X_{k+n}, X_{k}^T >.
    //  pesos : pesos de las señales con retardo
    //  A : matriz del filtro.
    M: TDAofMatR;
    pesos: TVectR;
    r0inv: TMatR;
    Ncanales: integer;
    Npasos: integer;

    // auxiliares para evaluaración de f y g
    A: TMatR;

    constructor Create(autoCorrelaciones: TDAofMatR; factoresDecaimiento: TVectR);
    function f(const X: TVectR): NReal; override;
    procedure acum_g(var grad: TVectR; const X: TVectR); override;
    destructor destroy; override;

    //  Calcula la Suma Geometrica:
    //                  SUMA_{h=idxInf}^{h=idxSup} ( ( M1 ) (x) ( M2 ) )^h
    function calcularSumaGeometricaKron( M1, M2: TMatR; idxInf, idxSup: integer)
                                         : TMatR;
  end;

implementation

{ TProblemaAjusteFiltroAutoCorrelacion }

constructor TProblemaAjusteFiltroAutoCorrelacion.Create(AutoCorrelaciones: TDAofMatR;
  ponderaciones: TVectR);
var
  elemento: integer;
begin
  inherited Create_init(1, AutoCorrelaciones[0].nc * AutoCorrelaciones[0].nc + 1,
    0, nil, nil);
  //  .1-  Se cargan las matrices de autocorrelacion y los pesos al problema
  R := AutoCorrelaciones;
  pesos := ponderaciones;

  //  .2-  Se carga el numero de canales y se chequea que el array no este vacio
  if length(R) > 0 then
  begin
    if R[0] <> nil then
      Ncanales := R[0].nc
    else
      raise Exception.Create('La matriz de correlaciones esta vacia');
  end
  else
    raise Exception.Create('El conjunto de matrices de correlaciones esta vacio');

  //  .3-  Se crea la f objetivo con la que se va a hacer la optimizacion
  f := T_fObjetivoAjusteFiltroAutoCorrelacion.Create(R, pesos);

  //  .4-  Se cargan las restricciones de caja para los factores
  for elemento := 1 to Ncanales * Ncanales do
  begin
    cota_inf_set(elemento, AJUSTE_MIN);   //  GFlieller@20190906: No lo tengo determinado
    cota_sup_set(elemento, AJUSTE_MAX);   //  GFlieller@20190906: No lo tengo determinado
  end;

end;

function TProblemaAjusteFiltroAutoCorrelacion.ResolverFiltro(
  var filtroA: TMatR): boolean;

var
  X: TVectR;
  Convergio: boolean;
  cont_Iters: longint;
  fCosto, lcosto: NReal;
  TextoX: TextFile;
  archiX, stringX: string;
  i, j: Integer;
  A_: TMatR;
begin

  //  .1-  Se crea e inicializo el vector X de resolucion del problema de largo Ncanales*Ncanales.
  A_ := TMatR.Create_identidad(Ncanales);
  A_.PorReal(0.95);
  X := A_.vec;

  cont_Iters := 0;
  //  .2-  Se mete en el optimizador
  MinInBox_CPX(X, 0.5, 1e-500, 1000000, cont_Iters, lcosto, fcosto, convergio, False);

  writeln('costo:',lcosto:12:10);
  writeln('cont_Iters:',cont_Iters);


  //  .4-  Se carga en el filtro el resultado de la optimizacion en formato matriz
  filtroA := X.reshape(Ncanales, Ncanales);
//  filtroA.WriteArchiXLT('FiltroA_AjusteAutocorrelacion.xlt');
  //  .5-  Se libera la memoria
  X.Free;

  //  .6-  Se devuelve como resultado la convergencia del metodo
  Result := convergio;

end;

{ T_fObjetivoAjusteFiltroAutoCorrelacion }

constructor T_fObjetivoAjusteFiltroAutoCorrelacion.Create(autoCorrelaciones: TDAofMatR;
  factoresDecaimiento: TVectR);
var
  kPaso: integer;

begin
  inherited Create;

  M := autoCorrelaciones;

  r0inv := TMatR.Create_Clone(M[0]);
  r0inv.Inv;

  for kpaso := 1 to high(M) do
    M[kpaso].Mult(M[kpaso], r0inv);

  pesos := factoresDecaimiento;
  Npasos := length(autoCorrelaciones) - 1;
  if Npasos > 0 then
    Ncanales := autoCorrelaciones[0].nc
  else
    raise Exception.Create('Conjunto de matrices de autocorrelacion vacio');

  A := TMatR.Create_Init(NCanales, NCanales);

end;

function T_fObjetivoAjusteFiltroAutoCorrelacion.f(const X: TVectR): NReal;

var
  e2, e2i: NReal;
  Ai: TMatR;
  i, jFil: integer;

begin
  //  Pasa X a matricial
  X.reshapeOn(A);
  Ai := TMatR.Create_identidad(Ncanales);

  // Calcula el error cuadratico e_i^2 = SUMA_j^Nc SUMA_k^Nc ( A_i[k,j] - M_i[k,j] )^2
  // ec.9
  e2 := 0;

  for i := 1 to Npasos do
  begin
    e2i := 0;
    //  Calculo el A^i, M_i
    Ai.Mult(Ai, A);

    // Calcula la norma cuadrada de Me2 sumando por columnas
    for jFil := 1 to Ncanales do
      e2i := e2i + Ai.Fila(jFil).distancia2(M[i].Fila(jFil));

    // Acumula la norma ponderada por el peso del paso
    e2 := e2 + pesos.e(i) * e2i;
  end;
  Ai.Free;
  Result := e2;
end;

procedure T_fObjetivoAjusteFiltroAutoCorrelacion.acum_g(var grad: TVectR;
  const X: TVectR);

var
  Q_, At, Ati, At_inv, Ai, At_invI, IdNN, IdN, TerminoA, TerminoB,
  TerminoB1, TerminoB2: TMatR;
  g, g_i: TVectR;
  q, p, ipaso, kfila, jcolumna: integer;
  valor: NReal;

begin
  IdN := TMatR.Create_identidad(Ncanales);
  IdNN := TMatR.Create_identidad(Ncanales * Ncanales);
  g := TVectR.Create_InitVal(Ncanales * Ncanales, 0);

  //  .1-  Desvectorializo X en A
  X.reshapeOn(A);


  //  .2-  Transpongo A
  At := TMatR.Create_Clone(A);
  At.Transponer;
  Ati := TMatR.Create_identidad(Ncanales);           // (A^T)^0 = IdN

  //  .3-  Invierto A transpuesta
  At_inv := TMatR.Create_Clone(At);
  At_inv.Inv;

  //  .4-  (((At_invI = A^T)^-1)^i)
  At_invI := TMatR.Create_identidad(Ncanales);

  //  .5-  Hago el producto de Kronecker ((A^T)^-1) (x) A: "( (A transpuesta) a la -1) con A"
  Q_ := TMatR.Create_Kron(At_inv, A);

  //  .6-  Armo la matriz A^{ipaso}
  Ai := TMatR.Create_identidad(Ncanales);           // (A^T)^0 = IdN

  //  .7-  Calculo las derivadas
  //       vec(U_kj)^T * MATRIX * vec(U_pq) = MATRIX[ k*N+j , p*N+q]
  //       donde MATRIX es de dimensiones NxN.

  //       La derivada de la señal con restraso i en el termino k,j respecto
  //       del termino pq
  //       dA^i[k,j]/dA[p,q]  = peso[i] *
  //          vec(U_kj)^T * ( SUMA_{h=1}^i( A^{i-h}^T (x) A^{h-1} ) )* vec(U_pq)

  //       dA[k,j]/dA[p,q]  =  SUMA_{h=1}^i {peso[i] * dA^i[k,j]/dA[p,q]}


  g_i := TVectR.Create_Init(Ncanales * Ncanales);

  for ipaso := 1 to Npasos do
  begin

    //  .7.1-  Calculo el TerminoA
    //  .7.1.1-  Armo la matriz M_i
    Ai.Mult(Ai, A);

    //  .7.1.3-  TerminoA = A^i - M_i
    TerminoA := TMatR.Create_Combinar(1, Ai, -1, M[ipaso]);

    TerminoB := TMatR.Create_Clone(IdNN);
    //   -- Termino 1 --  (A^T)^(i-1) (x) Identidad
    TerminoB1 := TMatR.Create_Kron(Ati, IdN);  // Chequeado
    Ati.Mult(Ati, At);

    //   -- Termino 2 --
    TerminoB2 := calcularSumaGeometricaKron(At_inv,A,0,ipaso-1);

    TerminoB.Mult(TerminoB,TerminoB1);
    TerminoB.Mult(TerminoB,TerminoB2);
    TerminoB1.Free;
    TerminoB2.Free;

    //  .7.6-  Inicializo el gradiente del paso i en 0
    for p := 1 to Ncanales do
    begin
      for q := 1 to Ncanales do
      begin
        begin
          valor := 0;
          for kfila := 1 to Ncanales do
          begin
            for jcolumna := 1 to Ncanales do
            begin
              // Se toma (jCol-1)*N + kFila porque se vectorializa por columnas
              valor := valor + TerminoA.e(kFila, jColumna) * TerminoB.e(
                (jcolumna - 1) * Ncanales + kFila, (q - 1) * Ncanales + p);
            end;
          end;
          g_i.pon_e((q - 1) * Ncanales + p, valor);
        end;
      end;
    end;

    //  Acumulo gradiente = SUMA_i ( pesos[i] *  gradiente[i])
    g.sumRPV(2 * pesos.e(ipaso), g_i);


    //  Libero la memoria del paso
    TerminoA.Free;
    TerminoB.Free;
  end;

  g_i.Free;

  grad.sumRPV(1, g);

  //  Libero la memoria
  IdN.Free;
  IdNN.Free;
  At.Free;
  At_inv.Free;
  Q_.Free;
  Ai.Free;
  At_invI.Free;
end;

destructor T_fObjetivoAjusteFiltroAutoCorrelacion.destroy;
begin

  A.Free;
  r0inv.Free;

end;

function T_fObjetivoAjusteFiltroAutoCorrelacion.calcularSumaGeometricaKron(M1,
  M2: TMatR; idxInf, idxSup: integer): TMatR;
var
  Id_Mkron, M1i, M2i, MkronI, Id_Mkroni, Id_Mkron_inv, Res, Id: TMatR;
  Invertible: Boolean;
  ipaso: Integer;
begin
  //  Calcula la Suma Geometrica:
  //                  SUMA_{h=idxInf}^{h=idxSup} ( ( M1 ) (x) ( M2 ) )^h
  //  Mkron = M1 (x) M2
  //  Si (I - MKron) es invertible:
  //                         SUMA = (I - MKron^idxInf)^-1 (I - Mkron^idxSup+1)
  //  Sino se calcula acumulando.
  Id := TMatR.Create_identidad(M1.nc*M1.nc);
  Res := TMatR.Create_InitVal(M1.nc*M1.nc, M1.nc*M1.nc, 0);
  //  Mkroni = (M1 (x) M2 )^idxSup
  M1i := TMatR.Create_Clone(M1);
  M1i.power_n(idxInf);
  M2i := TMatR.Create_Clone(M2);
  M2i.power_n(idxInf);

  Mkroni := TMatR.Create_Kron(M1i,M2i);
  Id_Mkron := TMatR.Create_Combinar(1, Id, -1, MKroni);
  Mkroni.free;

  Id_Mkron_inv := TMatR.Create_Clone(Id_Mkron);
  Invertible := Id_Mkron_inv.inv;

  if Invertible then
  begin
    //  Mkroni = (M1 (x) M2 )^idxSup
    M1i.Igual(M1);
    M2i.Igual(M2);
    M1i.power_n(idxSup);
    M2i.power_n(idxSup);
    Mkroni := TMatR.Create_Kron(M1i,M2i);
    Id_Mkroni := TMatR.Create_Combinar(1, Id, -1, MKroni);

    Res.Mult(Id_Mkron_inv,Id_Mkroni);
    MkronI.free;
    Id_Mkroni.free;
  end
  else
  begin  // Si no es invertible se calcula "a pedal"
    for ipaso:= idxInf to idxSup do
    begin
      //  Calculo M1^i (x) M2^i
      M1i.Igual(M1);
      M2i.Igual(M2);
      M1i.power_n(ipaso);
      M2i.power_n(ipaso);
      Mkroni := TMatR.Create_Kron(M1i,M2i);
      //  Acumulo en SUMA
      Res.Suma(Res,Mkroni);
      MkronI.free;
    end;
  end;

  // Libero la memoria que no necesito
  Id.free;
  M1i.free;
  M2i.free;
  Id_Mkron_inv.free;
  Id_Mkron.free;

  Result := Res;
end;

end.
