/*-----------------------------------------------------------------------
qpcsolver.c: Algorithm solving QPC task.
 
 QPC task to solve is
  
   min 0.5*alpha'*H*alpha + c'*alpha

   subject to  sum(alpha) = 1,  alpha(i) >= 0
  
 H [dim x dim] is symmetric positive definite matrix.
 c [dim x 1] is an arbitrary vector.

 The precision of the found solution is given by
 the parameters tmax, tolabs and tolrel which
 define the stopping conditions:
 
 UB-LB <= tolabs      ->  exit_flag = 1   Abs. tolerance.
 UB-LB <= UB*tolrel   ->  exit_flag = 2   Relative tolerance.
 t >= tmax            ->  exit_flag = 0   Number of iterations.

 UB ... Upper bound on the optimal solution.
 LB ... Lower bound on the optimal solution.
 t  ... Number of iterations.
 History ... Value of LB and UB wrt. number of iterations.


 The following algorithms are implemented:
 ..............................................

 - QPC solver based on MDM algorithm.
   exitflag = qpc_mdm( &get_col, diag_H, vector_c, dim,  
                 tmax, tolabs, tolrel, &alpha, &t, &History, verb  );

 - QPC solver based on improved MDM algorithm 1.
    exitflag = qpc_imdm( &get_col, diag_H, vector_c, dim,  
                 tmax, tolabs, tolrel, &alpha, &t, &History, verb  );

 - QPC solver based on improved MDM algorithm 2.
    exitflag = qpc_iimdm( &get_col, diag_H, vector_c, dim,  
                 tmax, tolabs, tolrel, &alpha, &t, &History, verb  );

 - QPC solver based on the Kowalczyk's algorithm.
    exitflag = qpc_kowalczyk( &get_col, diag_H, vector_c, dim,  
                  tmax, tolabs, tolrel, &alpha, &t, &History, verb  ); 

 - QPC solver based on the Keerthis's algorithm.
    exitflag = qpc_keerthi( &get_col, diag_H, vector_c, dim, 
                  tmax, tolabs, tolrel, &alpha, &t, &History, verb );

 - QPC solver based on the Kozinec (alis Gilbert's) algorithm.
    exitflag = qpc_kozinec( &get_col, diag_H, vector_c, dim, 
                  tmax, tolabs, tolrel, &alpha, &t, &History, verb );

 Modifications:
 26-nov-2004, VF
 25-nov-2004, VF
 21-nov-2004, VF
 20-nov-2004, VF
 31-may-2004, VF
 23-Jan-2004, VF

-------------------------------------------------------------------- */

#include "mex.h"
#include "matrix.h"
#include <math.h>
#include <stdlib.h>
#include <string.h>
#include <limits.h>

#define HISTORY_BUF 1000000

#define MINUS_INF INT_MIN
#define PLUS_INF  INT_MAX

#define ABS(A) ((A >= 0) ? A : -A)
#define MIN(A,B) ((A < B) ? A : B)
#define INDEX(ROW,COL,DIM) ((COL*DIM)+ROW)


/* --------------------------------------------------------------
 QPC solver based on MDM algorithm.

 Usage: exitflag = qpc_mdm( &get_col, diag_H, vector_c, dim,  
                  tmax, tolabs, tolrel, &alpha, &t, &History );
-------------------------------------------------------------- */
int qpc_mdm(const void* (*get_col)(long,long),
            double *diag_H,
            double *vector_c,
            long dim, 
            long tmax,
            double tolabs,
            double tolrel,
            double *alpha,
            long  *ptr_t,
            double **ptr_History,
            long verb)
{
  double LB;
  double UB;
  double aHa, ac;
  double tmp, tmp1;
  double Huu, Huv, Hvv;
  double min_beta, max_beta, beta;
  double lambda;
  double *History;
  double *Ha;
  double *tmp_ptr;
  double *col_u, *col_v;
  long u;
  long v;
  long new_u;
  long new_v;
  long i;
  long t;
  long History_size;
  int exitflag;

  /* ------------------------------------------------------------ */
  /* Initialization                                               */
  /* ------------------------------------------------------------ */

  Ha = mxCalloc(dim, sizeof(double));
  if( Ha == NULL ) mexErrMsgTxt("Not enough memory.");

  History_size = (tmax < HISTORY_BUF ) ? tmax+1 : HISTORY_BUF;
  History = mxCalloc(History_size*2,sizeof(double));
  if( History == NULL ) mexErrMsgTxt("Not enough memory.");

  /* inx = argmin(0.5*diag_H + vector_c ); */
  for( tmp1 =  PLUS_INF, i = 0; i < dim; i++ ) {
    tmp = 0.5*diag_H[i] + vector_c[i];
    if( tmp1 > tmp) {
      tmp1 = tmp;
      v = i;
    }
  }
  col_v = (double*)get_col(v,-1);

  for( min_beta = PLUS_INF, i = 0; i < dim; i++ ) 
  {
    alpha[i] = 0;
    Ha[i] = col_v[i];

    beta = Ha[i] + vector_c[i];
    if( beta < min_beta ) {
      min_beta = beta;
      u = i;
    }
  }

  alpha[v] = 1;
  aHa = diag_H[v];
  ac = vector_c[v];

  UB = 0.5*aHa + ac;
  LB = min_beta - 0.5*aHa;
  t = 0;
  History[INDEX(0,0,2)] = LB;
  History[INDEX(1,0,2)] = UB;

  if( verb ) {
    mexPrintf("Init: UB=%f, LB=%f, UB-LB=%f, (UB-LB)/|UB|=%f \n",
      UB, LB, UB-LB,(UB-LB)/UB);
  }  

  /* Stopping conditions */
  if( UB-LB <= tolabs ) exitflag = 1;
  else if(UB-LB <= ABS(UB)*tolrel ) exitflag = 2;
  else exitflag = -1;

  /* ------------------------------------------------------------ */
  /* Main optimization loop                                       */
  /* ------------------------------------------------------------ */

  while( exitflag == -1 ) 
  {
    t++;     

    col_u = (double*)get_col(u,-1);
    col_v = (double*)get_col(v,u);

    /* Adaptation rule and update */
    Huu = diag_H[u];
    Hvv = diag_H[v];
    Huv = col_u[v];

    lambda = (Ha[v]-Ha[u]+vector_c[v]-vector_c[u])/(alpha[v]*(Huu-2*Huv+Hvv));
    if( lambda < 0 ) lambda = 0; else if (lambda > 1) lambda = 1;

    aHa = aHa + 2*alpha[v]*lambda*(Ha[u]-Ha[v])+
                lambda*lambda*alpha[v]*alpha[v]*(Huu-2*Huv+Hvv);

    ac = ac + lambda*alpha[v]*(vector_c[u]-vector_c[v]);

    tmp = alpha[v];
    alpha[u]=alpha[u]+lambda*alpha[v];
    alpha[v]=alpha[v]-lambda*alpha[v];

    UB = 0.5*aHa + ac;

    min_beta = PLUS_INF; 
    max_beta = MINUS_INF;
    for( i = 0; i < dim; i++ ) 
    {
       Ha[i] = Ha[i] + lambda*tmp*(col_u[i] - col_v[i]);

       beta = Ha[i]+ vector_c[i];

       if( alpha[i] !=0 && max_beta < beta ) 
       {
         new_v = i;
         max_beta = beta;
       }

       if( beta < min_beta )
       { 
         new_u = i;
         min_beta = beta;
       }
    }    

    LB = min_beta - 0.5*aHa; 
    u = new_u;    
    v = new_v;

    /* Stopping conditions */
    if( UB-LB <= tolabs ) exitflag = 1; 
    else if( UB-LB <= ABS(UB)*tolrel ) exitflag = 2;
    else if(t >= tmax) exitflag = 0; 

    if( verb && (t % verb) == 0) {
      mexPrintf("%d: UB=%f, LB=%f, UB-LB=%f, (UB-LB)/|UB|=%f \n",
        t, UB, LB, UB-LB,(UB-LB)/UB);
    }  

    /* Store selected values */
    if( t < History_size ) {
      History[INDEX(0,t,2)] = LB;
      History[INDEX(1,t,2)] = UB;
    }
    else {
      tmp_ptr = mxCalloc((History_size+HISTORY_BUF)*2,sizeof(double));
      if( tmp_ptr == NULL ) mexErrMsgTxt("Not enough memory.");
      for( i = 0; i < History_size; i++ ) {
        tmp_ptr[INDEX(0,i,2)] = History[INDEX(0,i,2)];
        tmp_ptr[INDEX(1,i,2)] = History[INDEX(1,i,2)];
      }
      tmp_ptr[INDEX(0,t,2)] = LB;
      tmp_ptr[INDEX(1,t,2)] = UB;
      
      History_size += HISTORY_BUF;
      mxFree( History );
      History = tmp_ptr;
    }
  }

  /* print info about last iteration*/
  if(verb && (t % verb) ) {
    mexPrintf("%d: UB=%f, LB=%f, UB-LB=%f, (UB-LB)/|UB|=%f \n",
      t, UB, LB, UB-LB,(UB-LB)/UB);
  }  


  /*------------------------------------------------------- */
  /* Set outputs                                            */
  /*------------------------------------------------------- */
  (*ptr_t) = t;
  (*ptr_History) = History;

  /* Free memory */
  mxFree( Ha );
  
  return( exitflag ); 
}


/* --------------------------------------------------------------
 QPC solver based on improved MDM algorithm 1.

 Search strategy: u determined by common rule and v is 
 optimized.

 Usage: exitflag = qpc_imdm( &get_col, diag_H, vector_c, dim,  
                  tmax, tolabs, tolrel, &alpha, &t, &History );
-------------------------------------------------------------- */

int qpc_imdm(const void* (*get_col)(long,long),
            double *diag_H,
            double *vector_c,
            long dim, 
            long tmax,
            double tolabs,
            double tolrel,
            double *alpha,
            long  *ptr_t,
            double **ptr_History,
            long verb)
{
  double LB;
  double UB;
  double aHa, ac;
  double tmp, tmp1;
  double Huu, Huv, Hvv;
  double min_beta, max_beta, beta;
  double max_improv, improv;
  double lambda;
  double *History;
  double *Ha;
  double *tmp_ptr;
  double *col_u, *col_v;
  long u;
  long v;
  long new_u;
  long new_v;
  long i;
  long t;
  long History_size;
  int exitflag;

  /* ------------------------------------------------------------ */
  /* Initialization                                               */
  /* ------------------------------------------------------------ */

  Ha = mxCalloc(dim, sizeof(double));
  if( Ha == NULL ) mexErrMsgTxt("Not enough memory.");

  History_size = (tmax < HISTORY_BUF ) ? tmax+1 : HISTORY_BUF;
  History = mxCalloc(History_size*2,sizeof(double));
  if( History == NULL ) mexErrMsgTxt("Not enough memory.");

  /* inx = argmin(0.5*diag_H + vector_c ); */
  for( tmp1 =  PLUS_INF, i = 0; i < dim; i++ ) {
    tmp = 0.5*diag_H[i] + vector_c[i];
    if( tmp1 > tmp) {
      tmp1 = tmp;
      v = i;
    }
  }

  col_v = (double*)get_col(v,-1);

  for( min_beta = PLUS_INF, i = 0; i < dim; i++ ) 
  {
    alpha[i] = 0;
    Ha[i] = col_v[i];

    beta = Ha[i] + vector_c[i];
    if( beta < min_beta ) {
      min_beta = beta;
      u = i;
    }
  }

  alpha[v] = 1;
  aHa = diag_H[v];
  ac = vector_c[v];

  UB = 0.5*aHa + ac;
  LB = min_beta - 0.5*aHa;

  t = 0;
  History[INDEX(0,0,2)] = LB;
  History[INDEX(1,0,2)] = UB;

  if( verb ) {
    mexPrintf("Init: UB=%f, LB=%f, UB-LB=%f, (UB-LB)/|UB|=%f \n",
      UB, LB, UB-LB,(UB-LB)/UB);
  }  

  /* Stopping conditions */
  if( UB-LB <= tolabs ) exitflag = 1;
  else if(UB-LB <= ABS(UB)*tolrel ) exitflag = 2;
  else exitflag = -1;

  /* ------------------------------------------------------------ */
  /* Main optimization loop                                       */
  /* ------------------------------------------------------------ */

  col_u = (double*)get_col(u,-1);
  while( exitflag == -1 ) 
  {
    t++;     

    col_v = (double*)get_col(v,u);

    /* Adaptation rule and update */
    Huu = diag_H[u];
    Hvv = diag_H[v];
    Huv = col_u[v];

    lambda = (Ha[v]-Ha[u]+vector_c[v]-vector_c[u])/(alpha[v]*(Huu-2*Huv+Hvv));
    if( lambda < 0 ) lambda = 0; else if (lambda > 1) lambda = 1;

    aHa = aHa + 2*alpha[v]*lambda*(Ha[u]-Ha[v])+
                lambda*lambda*alpha[v]*alpha[v]*(Huu-2*Huv+Hvv);

    ac = ac + lambda*alpha[v]*(vector_c[u]-vector_c[v]);

    tmp = alpha[v];
    alpha[u]=alpha[u]+lambda*alpha[v];
    alpha[v]=alpha[v]-lambda*alpha[v];

    UB = 0.5*aHa + ac;
    
/*    max_beta = MINUS_INF;*/
    for( min_beta = PLUS_INF, i = 0; i < dim; i++ ) 
    {
       Ha[i] = Ha[i] + lambda*tmp*(col_u[i] - col_v[i]);

       beta = Ha[i]+ vector_c[i];

       if( beta < min_beta )
       { 
         new_u = i;
         min_beta = beta;
       }
    }    

    LB = min_beta - 0.5*aHa; 
    u = new_u;    
    col_u = (double*)get_col(u,-1);

    /* search for optimal v while u is fixed */
    for( max_improv =  MINUS_INF, i = 0; i < dim; i++ ) {

      if( alpha[i] != 0 ) {
        beta = Ha[i] + vector_c[i];

        if( beta >= min_beta ) {

          tmp = diag_H[u] - 2*col_u[i] + diag_H[i];
          if( tmp != 0 ) {
            improv = (0.5*(beta-min_beta)*(beta-min_beta))/tmp;

            if( improv > max_improv ) {
              max_improv = improv;
              v = i;
            }
          }
        }
      }
    }

    /* Stopping conditions */
    if( UB-LB <= tolabs ) exitflag = 1; 
    else if( UB-LB <= ABS(UB)*tolrel ) exitflag = 2;
    else if(t >= tmax) exitflag = 0; 

    /* print info */
    if(verb && (t % verb) == 0 ) {
      mexPrintf("%d: UB=%f, LB=%f, UB-LB=%f, (UB-LB)/|UB|=%f \n",
        t, UB, LB, UB-LB,(UB-LB)/UB);
    }  

    /* Store selected values */
    if( t < History_size ) {
      History[INDEX(0,t,2)] = LB;
      History[INDEX(1,t,2)] = UB;
    }
    else {
      tmp_ptr = mxCalloc((History_size+HISTORY_BUF)*2,sizeof(double));
      if( tmp_ptr == NULL ) mexErrMsgTxt("Not enough memory.");
      for( i = 0; i < History_size; i++ ) {
        tmp_ptr[INDEX(0,i,2)] = History[INDEX(0,i,2)];
        tmp_ptr[INDEX(1,i,2)] = History[INDEX(1,i,2)];
      }
      tmp_ptr[INDEX(0,t,2)] = LB;
      tmp_ptr[INDEX(1,t,2)] = UB;
      
      History_size += HISTORY_BUF;
      mxFree( History );
      History = tmp_ptr;
    }
  }

  /* print info about last iteration*/
  if(verb && (t % verb) ) {
    mexPrintf("%d: UB=%f, LB=%f, UB-LB=%f, (UB-LB)/|UB|=%f \n",
      t, UB, LB, UB-LB,(UB-LB)/UB);
  }  


  /*------------------------------------------------------- */
  /* Set outputs                                            */
  /*------------------------------------------------------- */
  (*ptr_t) = t;
  (*ptr_History) = History;

  /* Free memory */
  mxFree( Ha );
  
  return( exitflag ); 
}

/* --------------------------------------------------------------
 QPC solver based on improved MDM algorithm 2.

 Search strategy: u fix and v optimzed plus v fixed and u 
 optimized. 

 Usage: exitflag = qpc_iimdm( &get_col, diag_H, vector_c, dim,  
                  tmax, tolabs, tolrel, &alpha, &t, &History );
-------------------------------------------------------------- */

int qpc_iimdm(const void* (*get_col)(long,long),
            double *diag_H,
            double *vector_c,
            long dim, 
            long tmax,
            double tolabs,
            double tolrel,
            double *alpha,
            long  *ptr_t,
            double **ptr_History,
            long verb)