package optimizacion;
import operaciones.Matriz;

/**
 * <p>Title: Programa/Clase: QP.java</p>
 * <p>Description:
 * Objetivo: Calcular, empleando el metodo de conjunto activo, el minimo de
 * una funcion objetivo de segundo grado (Programacion Cuadratica),
 * sujeta a restricciones de igualdad y  de desigualdad especificadas.
 *
 * Entradas: La aproximacion inicial a la solucion, las restricciones a que
 * se debe someter la busqueda, el numero de restricciones de igualdad y
 * el numero de desigualdades.
 *
 * Salidas: El vector solucion que minimiza el problema planteado.
 * Notas.-
 * (a) Se espera que a traves de la herencia de esta clase (QP), se redefinan
 * los metodos:
 *          double funcion( Matriz )
 *          Matriz Gradiente( Matriz )
 *          Matriz Hessiano( Matriz )
 *       para personalizar el problema de optimizacion que se quiera resolver.
 *
 * (b) La matriz de restricciones se define como la matriz ampliada del
 * sistema de ecuaciones de restriccion, esto es, solamente los coeficientes
 * de las incognitas y los terminos independientes.
 *        Por ejemplo, para las ecuaciones de restriccion:
 *                               x - 2y >= -2
 *           -x - 2y + 6 >= 0 => -x - 2y >= -6
 *                               -x + 2y >= -2
 *                                x      >=  0
 *                                     y >=  0
 *
 *        le corresponde el arreglo:
 *           double[][] constraints = { {  1.0, -2.0, -2.0 },
 *                                      { -1.0, -2.0, -6.0 },
 *                                      { -1.0,  2.0, -2.0 },
 *                                      {  1.0,  0.0,  0.0 },
 *                                      {  0.0,  1.0,  0.0 } };
 *
 * (c) Por norma del lenguaje Java los subindices de los vectores xk y pk
 * (incognitas mas lambdas o multiplicadores de Lagrange), inician en 0.
 * </p>
 * <p>Copyright: Copyright (c) 2005</p>
 * <p>Company: UMSNH</p>
 * @author Sergio Rogelio Tinoco Martinez.
 * @version 1.0
 */
public class QP
{
   final double UMBRALZERO = 1e-10; // Umbral "cero" (para evitar errores de redondeo)

   /**
    * Calcular, empleando el metodo de conjunto activo, el minimo de una funcion
    *  objetivo de segundo grado (Programacion Cuadratica), sujeta a restricciones
    * de igualdad y de desigualdad especificadas.
    * @param x0 Matriz Aproximacion inicial a la solucion.
    * @param Constraints Matriz Restricciones a que se debe someter la busqueda.
    * @param nEq int Numero de restricciones de igualdad.
    * @param nIneq int Numero de restricciones de desigualdad tipo >=.
    * @return Matriz El vector solucion calculado (como instancia de la clase Matriz).
    */

   public Matriz
   activeSetSolution( Matriz x0, Matriz Constraints, int nEq, int nIneq )
   {
      double alfak = 0.0; // Tamanno de paso
      int k = -1;         // Numero de iteracion

      Matriz xk = Matriz.igual_a( x0 );

      int m = Constraints.nren; // Numero de restricciones
      int n = xk.nren;          // Numero de incognitas

      if ( m != (nEq + nIneq) )
         error( "El # de restricciones DIFIERE de (Igualdades + Desigualdades)" );

      boolean[] Wk = calcActiveSet( xk, Constraints, nEq, nIneq );

      // Armar la matriz con G, A, A transpuesta y 0s
      Matriz Q = armaSistema( xk, Constraints, n, m );

      // Matriz de terminos independientes del sistema total
      Matriz Qb = new Matriz( n+m, 1 );

      for(;;) // Ciclar hasta que se encuentre una solucion
      {
         System.out.println( "\n******************** Iteracion: " + ( ++k ) );

         // Armar la matriz de terminos independientes del sistema total
         Matriz gk = Gradiente( xk );

         for ( int i = 0; i < gk.nren; ++i )
            Qb.inserta( i, 0, gk.obten(i, 0) );

         Matriz pk = SolucionSL( Matriz.igual_a(Q), Matriz.igual_a(Qb), Wk);

         // Se resuelve para -p => invertir signo (solo para las p's)
         for ( int i = 0; i < n; ++i )
            pk.inserta( i, 0, pk.obten(i, 0) * -1.0 );

         // Mostrar resultados
         System.out.println( "\npk: " ); pk.imprime();
         System.out.println( "\nxk: " ); xk.imprime();

         if ( isZeros( pk, n ) )
         {
            int lambdaNegativa = findNegative( pk, n + nEq, Wk );

            if ( lambdaNegativa < 0 ) // No hay lambdas negativas => Terminamos
               return ( xk );

            else // Hay lambda negativa => "Desactivarla"
            {
               System.out.println( "\nSiguiente iteracion saco lambda[ " +
                                   (lambdaNegativa - n) + " ] de Wk." );
               Wk[ lambdaNegativa ] = false;
            }

         } // if pk == 0s
         else // pk != 0s
         {
            double[] resultados = calcAlfa( xk, pk, Constraints, Wk );

            alfak = resultados[0];

            // Simular la eliminacion de las lambdas del vector pk
            int pkFilas = pk.nren;
            pk.nren = n;

            xk = xk.mas( pk.por( alfak ) );
            System.out.println( "\nXk+1: " ); xk.imprime();

            System.out.println( "\nAlfa: " + alfak );

            // Restablecer valor
            pk.nren = pkFilas;

            // Si hay alguna restriccion bloqueando agregarla
            // al conjunto activo, si no, mantener mismo Wk
            if ( alfak < 1.0 )
            {
               System.out.println( "\nSiguiente iteracion activo lambda[ " +
                                   ( (int) resultados[1] - n ) + " ] en Wk." );
               Wk[ (int) resultados[1] ] = true;
            }
         }
      }
   }

   /**
    * Calcular el conjunto de restricciones que para el vector solucion dado "x",
    * se convierten en igualdad, segun las ecuaciones especificadas en la matriz "Constraints".
    * @param x Matriz Vector solucion aproximado
    * @param Constraints Matriz Ecuaciones de restricciones a verificar.
    * @param nEq int Numero de restricciones de igualdad.
    * @param nIneq int Numero de restricciones de desigualdad tipo >=.
    * @return boolean[] Un vector de valores booleanos (como instancia de la
    * clase Matriz). Cada valor "true" indica que la ecuacion de restriccion
    * correspondiente se convierte en igualdad para las condiciones especificadas
    * (se "activa").
    */

   private boolean[]
   calcActiveSet( Matriz x, Matriz Constraints, int nEq, int nIneq )
   {
      int m = Constraints.nren; // Numero de restricciones
      int n = x.nren;           // Numero de incognitas
      int dimTotal = n + m;     // Restricciones + incognitas

      boolean[] W = new boolean[ dimTotal ];

      // Activar restricciones de igualdad y las filas
      // correspondientes a las incognitas
      for ( int i = 0; i < (n + nEq); ++i ) W[i] = true;

      // Activar restricciones de desigualdad que se cumplen
      for ( int i = nEq; i < m; ++i )
      {
         double suma = 0.0;
         for ( int j = 0; j < n; ++j )
            suma += Constraints.obten( i, j ) * x.obten( j, 0 );

         if ( Math.abs( suma - Constraints.obten(i,n) ) <= UMBRALZERO )
            W[ i + (n+nEq) ] = true;

      } // for i

      return ( W );

   } // Metodo calcActiveSet

   /**
    * Arma la matriz simetrica del sistema de ecuaciones simultaneas que se
    * resuelve en el algoritmo de la Programacion Cuadratica (con las matrices
    * G, A, A transpuesta y 0s).
    * @param x Matriz Aproximacion al vector solucion.
    * @param A Matriz Coeficientes de las ecuaciones de restriccion del problema
    * @param n int Numero de incognitas
    * @param m int Numero total de restricciones
    * @return Matriz  La matriz simetrica "armada" (n+m x n+m), con el formato:
    *                      [ G  A^t ]
    *                      [ A   0  ]
    */

   private Matriz
   armaSistema( Matriz x, Matriz A, int n, int m )
   {
      Matriz G = Hessiano( x );

      int dimTotal = n + m; // Numero de incognitas + numero de restricciones

      Matriz Q = new Matriz( dimTotal, dimTotal ); // Q se crea inicializada con 0s

      // Copiar G
      for ( int i = 0; i < n; ++i )
         for ( int j = 0; j < n; ++j )
            Q.inserta( i, j, G.obten(i, j) );

      // Copiar A
      for ( int i = n; i < dimTotal; ++i )
         for ( int j = 0; j < n; ++j )
            Q.inserta( i, j, A.obten(i-n, j) );

      // Copiar A transpuesta
      for ( int i = 0; i < n; ++i )
         for ( int j = n; j < dimTotal; ++j )
            Q.inserta( i, j, A.obten(j-n, i) );

      return ( Q );

   } // Metodo armaSistema

   /**
    * Calcular el tamanno de paso alfa, a fin de que al avanzar en una direccion,
    * no se infrinja alguna de las ecuaciones de restriccion del sistema
    * @param xk Matriz Aproximacion al vector solucion
    * @param pk Matriz "Direccion" en la cual avanzamos para minimizar la funcion
    * objetivo
    * @param Constraints Matriz Coeficientes de las ecuaciones de restriccion
    * del problema planteado. (incluye terminos independientes)
    * @param W boolean[] Indicadores booleanos de las restricciones actualmente
    * "activas"
    * @return double[] Un vector de 2 valores con el significado:
    *   1) Alfa minima calculada, en el intervalo semiabierto ( -Inf , 1 ].
    *   2) Indice correspondiente a esa alfa (en W).
    */

   private double[]
   calcAlfa( Matriz xk, Matriz pk, Matriz Constraints, boolean[] W )
   {
      int m = Constraints.nren; // Numero de restricciones
      int n = xk.nren; // Numero de incognitas

      // Simular la eliminacion de terminos independientes
      --Constraints.ncol;

      // Simular la eliminacion de las lambdas del vector pk
      int pkFilas = pk.nren;
      pk.nren = n;

      Matriz Ap = Constraints.por( pk ); // Queda de m x 1
      Matriz Ax = Constraints.por( xk ); // Queda de m x 1

      // Restablecer valores
      ++Constraints.ncol;
      pk.nren = pkFilas;

      double[] alfas = new double[ m ]; // 1 alfa por cada lambda (por cada restriccion)

      // Inicializar con su valor maximo (para seleccionar el minimo correcto)
      for ( int i = 0; i < m; ++i ) alfas[i] = 1.0;

      for ( int i = 0; i < m; ++i )
      {
         if ( !W[n+i] && (Ap.obten(i,0) < 0.0) )
            alfas[i] = ( Constraints.obten(i,n) - Ax.obten(i,0) ) / Ap.obten( i,0 );
      }

      int idxMinimo = 0;
      double minimo = alfas[0];

      for ( int i = 0; i < m; ++i )
         if ( alfas[i] < minimo )
         {
            minimo = alfas[i];
            idxMinimo = i;
         }

      double[] resultados = new double[2];
      resultados[0] = ( minimo < 1.0 ) ? minimo : 1.0;

      // Devolver el indice con respecto a W
      // ( -1.0 indica que la alfa no se calculo, se asigno como 1.0 )
      resultados[1] = ( minimo < 1.0 ) ? idxMinimo + n : -1.0;

      return ( resultados );

   }

   /**
    * Verificar si los primeros n valores en un vector son cero
    * @param m Matriz Vector a verificar
    * @param n int Numero de valores iniciales que se deben verificar en el vector
    * @return boolean true si los n primeros valores son cero y false en
    * caso contrario.
    */

   private boolean
   isZeros( Matriz m, int n )
   {
      for ( int i = 0; i < n; ++i )
         if ( Math.abs( m.obten(i,0) ) > UMBRALZERO ) return ( false );

      return ( true );

   } // Metodo isZeros

   /**
    * Buscar el valor del multiplicador de Lagrange (lambda) mas negativo del
    * vector de "direcciones" de minimizacion, considerando que no esten en el
    * conjunto activo actual.
    * @param pk Matriz Vector de "direcciones" de minimizacion
    * @param inicio int Posicion en pk a partir de la cual se encuentran los
    * multiplicadores de Lagrange (lambdas)
    * @param W boolean[] Conjunto activo actual
    * @return int
    * (-1) si no existen lambdas negativas en pk de entre las que no pertenecen
    * al conjunto activo actual. El subindice correspondiente en W, en caso
    * contrario.
    */

   private int
   findNegative( Matriz pk, int inicio, boolean[] W )
   {
      double lambdaMinima = 0.0; // Para buscar lambdas negativas
      int idxLambda = -1;

      for ( int i = inicio; i < pk.nren; ++i )
      {
         double lambda = pk.obten(i,0);

         if ( W[i] && (lambda < lambdaMinima ) )
         {
            lambdaMinima = lambda;
            idxLambda = i;
         }

      } // for i

      return ( idxLambda );

   } // Metodo findNegative

   /**
    * Soluciona el sistema de ecuaciones ax=b, solo donde Wi es igual a true
    * @param a Matriz
    * @param b Matriz
    * @param W boolean[]
    * @return Matriz
    */

   Matriz SolucionSL( Matriz a, Matriz b, boolean[] W )
   {
       int n = a.nren; // Numero de incognitas
       int i, j, k;
       double suma;

       Matriz x = new Matriz(n, 1); // Vector solucion, inicializado en 0s

       // Eliminacion Gaussiana
       for (k = 0; k <= (n - 2); ++k) {
           if (W[k]) {
               for (i = k + 1; i <= (n - 1); ++i) {
                   b.datos[i][0] -= a.datos[i][k] * b.datos[k][0] /
                           a.datos[k][k];
                   for (j = n - 1; j >= k; --j)
                       a.datos[i][j] -= a.datos[i][k] * a.datos[k][j] /
                               a.datos[k][k];
               }
           }
       }
       // Substitucion hacia atras
       for (k = n - 1; k >= 0; --k) {
           if (W[k]) {
               suma = 0;
               for (j = k + 1; j < n; ++j)
                   suma += a.datos[k][j] * x.datos[j][0];
               x.datos[k][0] = (b.datos[k][0] - suma) / a.datos[k][k];
           }
       }
       return (x);
   }

   /**
    * Funcion objetivo
    * @param x Matriz
    * @return double
    */

   public double funcion(Matriz x)
   {
      return 0;

   }

   /**
    * Gradiente de la funci�n objetivo
    * @param x Matriz
    * @return Matriz
    */

   public Matriz Gradiente(Matriz x)
   {
      return null;

   }

   /**
    * Hessiano de la funcion Objetivo
    * @param x Matriz
    * @return Matriz
    */

   public Matriz Hessiano(Matriz x)
   {
      return null;
   }

   /**
    * Indicar un error localizado durante el proceso y finalizar
    * la ejecucion del programa.
    * @param texto_error String
    */

   private void
   error( String texto_error )
   {
      System.err.println( "\n\nError en Clase QP.java: " + texto_error + "." );
      System.exit( 1 );
   }
}