/***********************************************************************/
/*                                                                     */
/*   svm_hideo.c                                                       */
/*                                                                     */
/*   The Hildreth and D'Espo solver specialized for SVMs.              */
/*                                                                     */
/*   Author: Thorsten Joachims                                         */
/*   Date: 02.07.02                                                    */
/*                                                                     */
/*   Copyright (c) 2002  Thorsten Joachims - All rights reserved       */
/*                                                                     */
/*   This software is available for non-commercial use only. It must   */
/*   not be modified and distributed without prior permission of the   */
/*   author. The author is not responsible for implications from the   */
/*   use of this software.                                             */
/*                                                                     */
/***********************************************************************/

# include <math.h>
# include "svm_common.h"

/* Common Block Declarations */


# define PRIMAL_OPTIMAL      1
# define DUAL_OPTIMAL        2
# define MAXITER_EXCEEDED    3
# define NAN_SOLUTION        4
# define ONLY_ONE_VARIABLE   5

# define LARGEROUND          0
# define SMALLROUND          1

/* /////////////////////////////////////////////////////////////// */

# define DEF_PRECISION          1E-5
# define DEF_MAX_ITERATIONS     200
# define DEF_LINDEP_SENSITIVITY 1E-8
# define EPSILON_HIDEO          1E-20
# define EPSILON_EQ             1E-5

double *optimize_qp(QP *, double *, long, double *, LEARN_PARM *);
double *primal=0,*dual=0;
long   precision_violations=0;
double opt_precision=DEF_PRECISION;
long   maxiter=DEF_MAX_ITERATIONS;
double lindep_sensitivity=DEF_LINDEP_SENSITIVITY;
double *buffer;
long   *nonoptimal;

long  smallroundcount=0;

/* /////////////////////////////////////////////////////////////// */

void *my_malloc();

int optimize_hildreth_despo(long,long,double,double,double,long,long,long,double,double *,
			    double *,double *,double *,double *,double *,
			    double *,double *,double *,long *,double *);
int solve_dual(long,long,double,double,long,double *,double *,double *,
	       double *,double *,double *,double *,double *,double *,
	       double *,double *,double *,double *,long);

void linvert_matrix(double *, long, double *, double, long *);
void lprint_matrix(double *, long);
void ladd_matrix(double *, long, double);
void lcopy_matrix(double *, long, double *);
void lswitch_rows_matrix(double *, long, long, long);
void lswitchrk_matrix(double *, long, long, long);

double calculate_qp_objective(long, double *, double *, double *);



double *optimize_qp(qp,epsilon_crit,nx,threshold,learn_parm)
QP *qp;
double *epsilon_crit;
long nx; /* Maximum number of variables in QP */
double *threshold; 
LEARN_PARM *learn_parm;
/* start the optimizer and return the optimal values */
/* The HIDEO optimizer does not necessarily fully solve the problem. */
/* Since it requires a strictly positive definite hessian, the solution */
/* is restricted to a linear independent subset in case the matrix is */
/* only semi-definite. */
{
  long i,j;
  int result;
  double eq;

  if(!primal) { /* allocate memory at first call */
    primal=(double *)my_malloc(sizeof(double)*nx);
    dual=(double *)my_malloc(sizeof(double)*((nx+1)*2));
    nonoptimal=(long *)my_malloc(sizeof(long)*(nx));
    buffer=(double *)my_malloc(sizeof(double)*((nx+1)*2*(nx+1)*2+
					       nx*nx+2*(nx+1)*2+2*nx+1+2*nx+
					       nx+nx+nx*nx));
    (*threshold)=0;
    for(i=0;i<nx;i++) {
      primal[i]=0;
    }
  }

  if(verbosity>=4) { /* really verbose */
    printf("\n\n");
    eq=qp->opt_ce0[0];
    for(i=0;i<qp->opt_n;i++) {
      eq+=qp->opt_xinit[i]*qp->opt_ce[i];
      printf("%f: ",qp->opt_g0[i]);
      for(j=0;j<qp->opt_n;j++) {
	printf("%f ",qp->opt_g[i*qp->opt_n+j]);
      }
      printf(": a=%.30f",qp->opt_xinit[i]);
      printf(": y=%f\n",qp->opt_ce[i]);
    }
    printf("eq-constraint=%.30f opt_ce0=%f\n",eq,qp->opt_ce0[0]);
    printf("upper_bound=%.30f\n",qp->opt_up[0]);
  }

  result=optimize_hildreth_despo(qp->opt_n,qp->opt_m,
				 opt_precision,(*epsilon_crit),
				 learn_parm->epsilon_a,maxiter,
				 /* (long)PRIMAL_OPTIMAL, */
				 (long)0, (long)0,
				 lindep_sensitivity,
				 qp->opt_g,qp->opt_g0,qp->opt_ce,qp->opt_ce0,
				 qp->opt_low,qp->opt_up,primal,qp->opt_xinit,
				 dual,nonoptimal,buffer);
  if(verbosity>=3) { 
    printf("return(%d)...",result);
  }

  if(learn_parm->totwords < learn_parm->svm_maxqpsize) { 
    /* larger working sets will be linear dependent anyway */
    learn_parm->svm_maxqpsize=maxl(learn_parm->totwords,(long)2);
  }

  if(result == NAN_SOLUTION) {
    lindep_sensitivity*=2;  /* throw out linear dependent examples more */
                            /* generously */
    if(learn_parm->svm_maxqpsize>2) {
      learn_parm->svm_maxqpsize--;  /* decrease size of qp-subproblems */
    }
    precision_violations++;
  }

  /* take one round of only two variable to get unstuck */
  if(result != PRIMAL_OPTIMAL) {

    smallroundcount++;

    result=optimize_hildreth_despo(qp->opt_n,qp->opt_m,
				   opt_precision,(*epsilon_crit),
				   learn_parm->epsilon_a,(long)maxiter,
				   (long)PRIMAL_OPTIMAL,(long)SMALLROUND,
				   lindep_sensitivity,
				   qp->opt_g,qp->opt_g0,qp->opt_ce,qp->opt_ce0,
				   qp->opt_low,qp->opt_up,primal,qp->opt_xinit,
				   dual,nonoptimal,buffer);
    if(verbosity>=3) { 
      printf("return_srd(%d)...",result);
    }

    if(result != PRIMAL_OPTIMAL) {
      if(result != ONLY_ONE_VARIABLE) 
	precision_violations++;
      if(result == MAXITER_EXCEEDED) 
	maxiter+=100;
      if(result == NAN_SOLUTION) {
	lindep_sensitivity*=2;  /* throw out linear dependent examples more */
	                        /* generously */
	/* results not valid, so return inital values */
	for(i=0;i<qp->opt_n;i++) {
	  primal[i]=qp->opt_xinit[i];
	}
      }
    }
  }


  if(precision_violations > 50) {
    precision_violations=0;
    (*epsilon_crit)*=10.0; 
    if(verbosity>=1) {
      printf("\nWARNING: Relaxing epsilon on KT-Conditions (%f).\n",
	     (*epsilon_crit));
    }
  }	  

  if((qp->opt_m>0) && (result != NAN_SOLUTION))
    (*threshold)=dual[1]-dual[0];
  else
    (*threshold)=0;

  if(verbosity>=4) { /* really verbose */
    printf("\n\n");
    eq=qp->opt_ce0[0];
    for(i=0;i<qp->opt_n;i++) {
      eq+=primal[i]*qp->opt_ce[i];
      printf("%f: ",qp->opt_g0[i]);
      for(j=0;j<qp->opt_n;j++) {
	printf("%f ",qp->opt_g[i*qp->opt_n+j]);
      }
      printf(": a=%.30f",primal[i]);
      printf(": nonopti=%ld",nonoptimal[i]);
      printf(": y=%f\n",qp->opt_ce[i]);
    }
    printf("eq-constraint=%.30f\n",eq);
    printf("b=%f\n",(*threshold));
    printf(" smallroundcount=%ld ",smallroundcount);
  }

  return(primal);
}



int optimize_hildreth_despo(n,m,precision,epsilon_crit,epsilon_a,maxiter,goal,
			    smallround,lindep_sensitivity,g,g0,ce,ce0,low,up,
			    primal,init,dual,lin_dependent,buffer)
     long   n;            /* number of variables */
     long   m;            /* number of linear equality constraints [0,1] */
     double precision;    /* solve at least to this dual precision */
     double epsilon_crit; /* stop, if KT-Conditions approx fulfilled */
     double epsilon_a;    /* precision of alphas at bounds */
     long   maxiter;      /* stop after this many iterations */
     long   goal;         /* keep going until goal fulfilled */
     long   smallround;   /* use only two variables of steepest descent */
     double lindep_sensitivity; /* epsilon for detecting linear dependent ex */
     double *g;           /* hessian of objective */
     double *g0;          /* linear part of objective */
     double *ce,*ce0;     /* linear equality constraints */
     double *low,*up;     /* box constraints */
     double *primal;      /* primal variables */
     double *init;        /* initial values of primal */
     double *dual;        /* dual variables */
     long   *lin_dependent;
     double *buffer;
{
  long i,j,k,from,to,n_indep,changed;
  double sum,bmin=0,bmax=0;
  double *d,*d0,*ig,*dual_old,*temp,*start;       
  double *g0_new,*g_new,*ce_new,*ce0_new,*low_new,*up_new;
  double add,t;
  int result;
  double obj_before,obj_after; 
  long b1,b2;

  g0_new=&(buffer[0]);    /* claim regions of buffer */
  d=&(buffer[n]);
  d0=&(buffer[n+(n+m)*2*(n+m)*2]);
  ce_new=&(buffer[n+(n+m)*2*(n+m)*2+(n+m)*2]);
  ce0_new=&(buffer[n+(n+m)*2*(n+m)*2+(n+m)*2+n]);
  ig=&(buffer[n+(n+m)*2*(n+m)*2+(n+m)*2+n+m]);
  dual_old=&(buffer[n+(n+m)*2*(n+m)*2+(n+m)*2+n+m+n*n]);
  low_new=&(buffer[n+(n+m)*2*(n+m)*2+(n+m)*2+n+m+n*n+(n+m)*2]);
  up_new=&(buffer[n+(n+m)*2*(n+m)*2+(n+m)*2+n+m+n*n+(n+m)*2+n]);
  start=&(buffer[n+(n+m)*2*(n+m)*2+(n+m)*2+n+m+n*n+(n+m)*2+n+n]);
  g_new=&(buffer[n+(n+m)*2*(n+m)*2+(n+m)*2+n+m+n*n+(n+m)*2+n+n+n]);
  temp=&(buffer[n+(n+m)*2*(n+m)*2+(n+m)*2+n+m+n*n+(n+m)*2+n+n+n+n*n]);

  b1=-1;
  b2=-1;
  for(i=0;i<n;i++) {   /* get variables with steepest feasible descent */
    sum=g0[i];         
    for(j=0;j<n;j++) 
      sum+=init[j]*g[i*n+j];
    sum=sum*ce[i];
    if(((b1==-1) || (sum<bmin)) 
       && (!((init[i]<=(low[i]+epsilon_a)) && (ce[i]<0.0)))
       && (!((init[i]>=( up[i]-epsilon_a)) && (ce[i]>0.0)))
       ) {
      bmin=sum;
      b1=i;
    }
    if(((b2==-1) || (sum>bmax)) 
       && (!((init[i]<=(low[i]+epsilon_a)) && (ce[i]>0.0)))
       && (!((init[i]>=( up[i]-epsilon_a)) && (ce[i]<0.0)))
       ) {
      bmax=sum;
      b2=i;
    }
  }
  /* in case of unbiased hyperplane, the previous projection on */
  /* equality constraint can lead to b1 or b2 being -1. */
  if((b1 == -1) || (b2 == -1)) {
    b1=0;
    b2=n-1;
  }

  for(i=0;i<n;i++) {
    start[i]=init[i];
  }

  /* in case both examples are equal */
  add=0;
  changed=0;
  if((g[b1*n+b2] == g[b1*n+b1]) && (g[b1*n+b2] == g[b2*n+b2])) {
    if(ce[b1] == ce[b2]) { /* distribute evenly */
      changed=1;
      start[b1]=(init[b1]+init[b2])/2.0;
      start[b2]=(init[b1]+init[b2])/2.0;
      if(start[b2] > up[b2]) {
	t=start[b2]-up[b2];
	start[b2]=up[b2];
	start[b1]+=t;
      }
      if(start[b1] > up[b1]) {
	t=start[b1]-up[b1];
	start[b1]=up[b1];
	start[b2]+=t;
      }
    }
  }
  if((-g[b1*n+b2] == g[b1*n+b1]) && (-g[b1*n+b2] == g[b2*n+b2])) {
    if(ce[b1] != ce[b2]) { /* set to upper bound */
      changed=1;
      t=up[b1]-init[b1];
      if((up[b2]-init[b2]) < t) {
	t=up[b2]-init[b2];
      }
      start[b1]=init[b1]+t;
      start[b2]=init[b2]+t;
    }
  }
  /* if we have a biased hyperplane, then adding a constant to the */
  /* hessian does not change the solution. So that is done for examples */
  /* with zero diagonal entry, since HIDEO cannot handle them. */
  else if((fabs(g[b1*n+b1]) < lindep_sensitivity) 
	  || (fabs(g[b2*n+b2]) < lindep_sensitivity)) {
    add+=0.093274;
  }    
  /* in case both examples are linear dependent */
  else if(g[b1*n+b2] != 0 && g[b2*n+b2] != 0 &&
	  (fabs(g[b1*n+b1]/g[b1*n+b2] - g[b1*n+b2]/g[b2*n+b2])
	   < lindep_sensitivity)) { 
    add+=0.078274;
  }


/* printf("b1=%ld,b2=%ld\n",b1,b2); */

  lcopy_matrix(g,n,d);
  if((m==1) && (add>0.0)) {
    for(j=0;j<n;j++) {
      for(k=0;k<n;k++) {
	d[j*n+k]+=add*ce[j]*ce[k];
      }
    }
  }
  else {
    add=0.0;
  }

  if(n>2) {                    /* switch, so that variables are better mixed */
    lswitchrk_matrix(d,n,b1,(long)0); 
    if(b2 == 0) 
      lswitchrk_matrix(d,n,b1,(long)1); 
    else
      lswitchrk_matrix(d,n,b2,(long)1); 
  }
  if(smallround == SMALLROUND) {
    for(i=2;i<n;i++) {
      lin_dependent[i]=1;
    }
    lin_dependent[0]=0;
    lin_dependent[1]=0;
  }
  else {
    for(i=0;i<n;i++) {
      lin_dependent[i]=0;
    }
  }
  linvert_matrix(d,n,ig,lindep_sensitivity,lin_dependent);
  if(n>2) {                    /* now switch back */
    if(b2 == 0) {
      lswitchrk_matrix(ig,n,b1,(long)1); 
      i=lin_dependent[1];  
      lin_dependent[1]=lin_dependent[b1];
      lin_dependent[b1]=i;
    }
    else {
      lswitchrk_matrix(ig,n,b2,(long)1); 
      i=lin_dependent[1];  
      lin_dependent[1]=lin_dependent[b2];
      lin_dependent[b2]=i;
    }
    lswitchrk_matrix(ig,n,b1,(long)0); 
    i=lin_dependent[0];  
    lin_dependent[0]=lin_dependent[b1];
    lin_dependent[b1]=i;
  }
  /* lprint_matrix(d,n); */
  /* lprint_matrix(ig,n); */

  lcopy_matrix(g,n,g_new);   /* restore g_new matrix */
  if(add>0)
    for(j=0;j<n;j++) {
      for(k=0;k<n;k++) {
	g_new[j*n+k]+=add*ce[j]*ce[k];
      }
    }

  for(i=0;i<n;i++) {  /* fix linear dependent vectors */
    g0_new[i]=g0[i]+add*ce0[0]*ce[i];
  }
  if(m>0) ce0_new[0]=-ce0[0];
  for(i=0;i<n;i++) {  /* fix linear dependent vectors */
    if(lin_dependent[i]) {
      for(j=0;j<n;j++) {
	if(!lin_dependent[j]) {
	  g0_new[j]+=start[i]*g_new[i*n+j];
	}
      }
      if(m>0) ce0_new[0]-=(start[i]*ce[i]);
    }
  }
  from=0;   /* remove linear dependent vectors */
  to=0;
  n_indep=0;
  for(i=0;i<n;i++) {
    if(!lin_dependent[i]) {
      g0_new[n_indep]=g0_new[i];
      ce_new[n_indep]=ce[i]; 
      low_new[n_indep]=low[i];
      up_new[n_indep]=up[i];
      primal[n_indep]=start[i];
      n_indep++;
    }
    for(j=0;j<n;j++) {
      if((!lin_dependent[i]) && (!lin_dependent[j])) {
        ig[to]=ig[from];
        g_new[to]=g_new[from];
	to++;
      }
      from++;
    }
  }

  if(verbosity>=3) {
    printf("real_qp_size(%ld)...",n_indep);
  }
  
  /* cannot optimize with only one variable */
  if((n_indep<=1) && (m>0) && (!changed)) { 
    for(i=n-1;i>=0;i--) {
      primal[i]=init[i];
    }
    return((int)ONLY_ONE_VARIABLE);
  }

  result=solve_dual(n_indep,m,precision,epsilon_crit,maxiter,g_new,g0_new,
		    ce_new,ce0_new,low_new,up_new,primal,d,d0,ig,
		    dual,dual_old,temp,goal);
  
  j=n_indep;
  for(i=n-1;i>=0;i--) {
    if(!lin_dependent[i]) {
      j--;
      primal[i]=primal[j];
    }
    else if((m==0) && (g[i*n+i]==0)) {
      /* if we use a biased hyperplane, each example with a zero diagonal */
      /* entry must have an alpha at the upper bound. Doing this */
      /* is essential for the HIDEO optimizer, since it cannot handle zero */
      /* diagonal entries in the hessian for the unbiased hyperplane case. */
      primal[i]=up[i];  
    }
    else {
      primal[i]=start[i];  /* leave as is */
    }
    temp[i]=primal[i];
  }
   
  if(verbosity>=3) {
    obj_before=calculate_qp_objective(n,g,g0,init);
    obj_after=calculate_qp_objective(n,g,g0,primal);
    printf("before(%.30f)...after(%.30f)...result_sd(%d)...",
	   obj_before,obj_after,result); 
  }

  return((int)result);
}


int solve_dual(n,m,precision,epsilon_crit,maxiter,g,g0,ce,ce0,low,up,primal,
	       d,d0,ig,dual,dual_old,temp,goal)
     /* Solves the dual using the method of Hildreth and D'Espo. */
     /* Can only handle problems with zero or exactly one */
     /* equality constraints. */

     long   n;            /* number of variables */
     long   m;            /* number of linear equality constraints */
     double precision;    /* solve at least to this dual precision */
     double epsilon_crit; /* stop, if KT-Conditions approx fulfilled */
     long   maxiter;      /* stop after that many iterations */
     double *g;
     double *g0;          /* linear part of objective */
     double *ce,*ce0;     /* linear equality constraints */
     double *low,*up;     /* box constraints */
     double *primal;      /* variables (with initial values) */
     double *d,*d0,*ig,*dual,*dual_old,*temp;       /* buffer  */
     long goal;
{
  long i,j,k,iter;
  double sum,w,maxviol,viol,temp1,temp2,isnantest;
  double model_b,dist;
  long retrain,maxfaktor,primal_optimal=0,at_bound,scalemaxiter;
  double epsilon_a=1E-15,epsilon_hideo;
  double eq; 

  if((m<0) || (m>1)) 
    perror("SOLVE DUAL: inappropriate number of eq-constrains!");

  /*  
  printf("\n");
  for(i=0;i<n;i++) {
    printf("%f: ",g0[i]);
    for(j=0;j<n;j++) {
      printf("%f ",g[i*n+j]);
    }
    printf(": a=%.30f",primal[i]);
    printf(": y=%f\n",ce[i]);
  }
  */

  for(i=0;i<2*(n+m);i++) {
    dual[i]=0;
    dual_old[i]=0;
  }
  for(i=0;i<n;i++) {   
    for(j=0;j<n;j++) {   /* dual hessian for box constraints */
      d[i*2*(n+m)+j]=ig[i*n+j];
      d[(i+n)*2*(n+m)+j]=-ig[i*n+j];
      d[i*2*(n+m)+j+n]=-ig[i*n+j];
      d[(i+n)*2*(n+m)+j+n]=ig[i*n+j];
    }
    if(m>0) {
      sum=0;              /* dual hessian for eq constraints */
      for(j=0;j<n;j++) {
	sum+=(ce[j]*ig[i*n+j]);
      }
      d[i*2*(n+m)+2*n]=sum;
      d[i*2*(n+m)+2*n+1]=-sum;
      d[(n+i)*2*(n+m)+2*n]=-sum;
      d[(n+i)*2*(n+m)+2*n+1]=sum;
      d[(n+n)*2*(n+m)+i]=sum;
      d[(n+n+1)*2*(n+m)+i]=-sum;
      d[(n+n)*2*(n+m)+(n+i)]=-sum;
      d[(n+n+1)*2*(n+m)+(n+i)]=sum;
      
      sum=0;
      for(j=0;j<n;j++) {
	for(k=0;k<n;k++) {
	  sum+=(ce[k]*ce[j]*ig[j*n+k]);
	}
      }
      d[(n+n)*2*(n+m)+2*n]=sum;
      d[(n+n)*2*(n+m)+2*n+1]=-sum;
      d[(n+n+1)*2*(n+m)+2*n]=-sum;
      d[(n+n+1)*2*(n+m)+2*n+1]=sum;
    } 
  }

  for(i=0;i<n;i++) {   /* dual linear component for the box constraints */
    w=0;
    for(j=0;j<n;j++) {
      w+=(ig[i*n+j]*g0[j]); 
    }
    d0[i]=up[i]+w;
    d0[i+n]=-low[i]-w;
  }

  if(m>0) {  
    sum=0;             /* dual linear component for eq constraints */
    for(j=0;j<n;j++) {
      for(k=0;k<n;k++) {
	sum+=(ce[k]*ig[k*n+j]*g0[j]); 
      }
    }
    d0[2*n]=ce0[0]+sum;
    d0[2*n+1]=-ce0[0]-sum;
  }

  maxviol=999999;
  iter=0;
  retrain=1;
  maxfaktor=1;
  scalemaxiter=maxiter/5;
  while((retrain) && (maxviol > 0) && (iter < (scalemaxiter*maxfaktor))) {
    iter++;
    
    while((maxviol > precision) && (iter < (scalemaxiter*maxfaktor))) {
      iter++;
      maxviol=0;
      for(i=0;i<2*(n+m);i++) {
	sum=d0[i];
	for(j=0;j<2*(n+m);j++) {
	  sum+=d[i*2*(n+m)+j]*dual_old[j];
	}
	sum-=d[i*2*(n+m)+i]*dual_old[i];
	dual[i]=-sum/d[i*2*(n+m)+i];
	if(dual[i]<0) dual[i]=0;
	
	viol=fabs(dual[i]-dual_old[i]);
	if(viol>maxviol) 
	  maxviol=viol;
	dual_old[i]=dual[i];
      }
      /*
      printf("%d) maxviol=%20f precision=%f\n",iter,maxviol,precision); 
      */
    }
  
    if(m>0) {
      for(i=0;i<n;i++) {
	temp[i]=dual[i]-dual[i+n]+ce[i]*(dual[n+n]-dual[n+n+1])+g0[i];
      }
    } 
    else {
      for(i=0;i<n;i++) {
	temp[i]=dual[i]-dual[i+n]+g0[i];
      }
    }
    for(i=0;i<n;i++) {
      primal[i]=0;             /* calc value of primal variables */
      for(j=0;j<n;j++) {
	primal[i]+=ig[i*n+j]*temp[j];
      }
      primal[i]*=-1.0;
      if(primal[i]<=(low[i])) {  /* clip conservatively */
	primal[i]=low[i];
      }
      else if(primal[i]>=(up[i])) {
	primal[i]=up[i];
      }
    }

    if(m>0) 
      model_b=dual[n+n+1]-dual[n+n];
    else
      model_b=0;

    epsilon_hideo=EPSILON_HIDEO;
    for(i=0;i<n;i++) {           /* check precision of alphas */
      dist=-model_b*ce[i]; 
      dist+=(g0[i]+1.0);
      for(j=0;j<i;j++) {
	dist+=(primal[j]*g[j*n+i]);
      }
      for(j=i;j<n;j++) {
	dist+=(primal[j]*g[i*n+j]);
      }
      if((primal[i]<(up[i]-epsilon_hideo)) && (dist < (1.0-epsilon_crit))) {
	epsilon_hideo=(up[i]-primal[i])*2.0;
      }
      else if((primal[i]>(low[i]+epsilon_hideo)) &&(dist>(1.0+epsilon_crit))) {
	epsilon_hideo=(primal[i]-low[i])*2.0;
      }
    }
    /* printf("\nEPSILON_HIDEO=%.30f\n",epsilon_hideo); */

    for(i=0;i<n;i++) {           /* clip alphas to bounds */
      if(primal[i]<=(low[i]+epsilon_hideo)) {
	primal[i]=low[i];
      }
      else if(primal[i]>=(up[i]-epsilon_hideo)) {
	primal[i]=up[i];
      }
    }

    retrain=0;
    primal_optimal=1;
    at_bound=0;
    for(i=0;(i<n);i++) {  /* check primal KT-Conditions */
      dist=-model_b*ce[i]; 
      dist+=(g0[i]+1.0);
      for(j=0;j<i;j++) {
	dist+=(primal[j]*g[j*n+i]);
      }
      for(j=i;j<n;j++) {
	dist+=(primal[j]*g[i*n+j]);
      }
      if((primal[i]<(up[i]-epsilon_a)) && (dist < (1.0-epsilon_crit))) {
	retrain=1;
	primal_optimal=0;
      }
      else if((primal[i]>(low[i]+epsilon_a)) && (dist > (1.0+epsilon_crit))) {
	retrain=1;
	primal_optimal=0;
      }
      if((primal[i]<=(low[i]+epsilon_a)) || (primal[i]>=(up[i]-epsilon_a))) {
	at_bound++;
      }
      /*    printf("HIDEOtemp: a[%ld]=%.30f, dist=%.6f, b=%f, at_bound=%ld\n",i,primal[i],dist,model_b,at_bound);  */
    }
    if(m>0) {
      eq=-ce0[0];               /* check precision of eq-constraint */
      for(i=0;i<n;i++) { 
	eq+=(ce[i]*primal[i]);
      }
      if((EPSILON_EQ < fabs(eq)) 
	 /*
	 && !((goal==PRIMAL_OPTIMAL) 
	       && (at_bound==n)) */
	 ) {
	retrain=1;
	primal_optimal=0;
      }
      /* printf("\n eq=%.30f ce0=%f at-bound=%ld\n",eq,ce0[0],at_bound);  */
    }

    if(retrain) {
      precision/=10;
      if(((goal == PRIMAL_OPTIMAL) && (maxfaktor < 50000))
	 || (maxfaktor < 5)) {
	maxfaktor++;
      }
    }
  }

  if(!primal_optimal) {
    for(i=0;i<n;i++) {
      primal[i]=0;             /* calc value of primal variables */
      for(j=0;j<n;j++) {
	primal[i]+=ig[i*n+j]*temp[j];
      }
      primal[i]*=-1.0;
      if(primal[i]<=(low[i]+epsilon_a)) {  /* clip conservatively */
	primal[i]=low[i];
      }
      else if(primal[i]>=(up[i]-epsilon_a)) {
	primal[i]=up[i];
      }
    }
  }

  isnantest=0;
  for(i=0;i<n;i++) {           /* check for isnan */
    isnantest+=primal[i];
  }

  if(m>0) {
    temp1=dual[n+n+1];   /* copy the dual variables for the eq */
    temp2=dual[n+n];     /* constraints to a handier location */
    for(i=n+n+1;i>=2;i--) {
      dual[i]=dual[i-2];
    }
    dual[0]=temp2;
    dual[1]=temp1;
    isnantest+=temp1+temp2;
  }

  if(isnan(isnantest)) {
    return((int)NAN_SOLUTION);
  }
  else if(primal_optimal) {
    return((int)PRIMAL_OPTIMAL);
  }
  else if(maxviol == 0.0) {
    return((int)DUAL_OPTIMAL);
  }
  else {
    return((int)MAXITER_EXCEEDED);
  }
}


void linvert_matrix(matrix,depth,inverse,lindep_sensitivity,lin_dependent)
double *matrix;
long depth;
double *inverse,lindep_sensitivity;
long *lin_dependent;  /* indicates the active parts of matrix on 
			 input and output*/
{
  long i,j,k;
  double factor;

  for(i=0;i<depth;i++) {
    /*    lin_dependent[i]=0; */
    for(j=0;j<depth;j++) {
      inverse[i*depth+j]=0.0;
    }
    inverse[i*depth+i]=1.0;
  }
  for(i=0;i<depth;i++) {
    if(lin_dependent[i] || (fabs(matrix[i*depth+i])<lindep_sensitivity)) {
      lin_dependent[i]=1;
    }
    else {
      for(j=i+1;j<depth;j++) {
	factor=matrix[j*depth+i]/matrix[i*depth+i];
	for(k=i;k<depth;k++) {
	  matrix[j*depth+k]-=(factor*matrix[i*depth+k]);
	}
	for(k=0;k<depth;k++) {
	  inverse[j*depth+k]-=(factor*inverse[i*depth+k]);
	}
      }
    }
  }
  for(i=depth-1;i>=0;i--) {
    if(!lin_dependent[i]) {
      factor=1/matrix[i*depth+i];
      for(k=0;k<depth;k++) {
	inverse[i*depth+k]*=factor;
      }
      matrix[i*depth+i]=1;
      for(j=i-1;j>=0;j--) {
	factor=matrix[j*depth+i];
	matrix[j*depth+i]=0;
	for(k=0;k<depth;k++) {
	  inverse[j*depth+k]-=(factor*inverse[i*depth+k]);
	}
      }
    }
  }
}

void lprint_matrix(matrix,depth)
double *matrix;
long depth;
{
  long i,j;
  for(i=0;i<depth;i++) {
    for(j=0;j<depth;j++) {
      printf("%5.2f ",(double)(matrix[i*depth+j]));
    }
    printf("\n");
  }
  printf("\n");
}

void ladd_matrix(matrix,depth,scalar)
double *matrix;
long depth;
double scalar;
{
  long i,j;
  for(i=0;i<depth;i++) {
    for(j=0;j<depth;j++) {
      matrix[i*depth+j]+=scalar;
    }
  }
}

void lcopy_matrix(matrix,depth,matrix2) 
double *matrix;
long depth;
double *matrix2;
{
  long i;
  
  for(i=0;i<(depth)*(depth);i++) {
    matrix2[i]=matrix[i];
  }
}

void lswitch_rows_matrix(matrix,depth,r1,r2) 
double *matrix;
long depth,r1,r2;
{
  long i;
  double temp;

  for(i=0;i<depth;i++) {
    temp=matrix[r1*depth+i];
    matrix[r1*depth+i]=matrix[r2*depth+i];
    matrix[r2*depth+i]=temp;
  }
}

void lswitchrk_matrix(matrix,depth,rk1,rk2) 
double *matrix;
long depth,rk1,rk2;
{
  long i;
  double temp;

  for(i=0;i<depth;i++) {
    temp=matrix[rk1*depth+i];
    matrix[rk1*depth+i]=matrix[rk2*depth+i];
    matrix[rk2*depth+i]=temp;
  }
  for(i=0;i<depth;i++) {
    temp=matrix[i*depth+rk1];
    matrix[i*depth+rk1]=matrix[i*depth+rk2];
    matrix[i*depth+rk2]=temp;
  }
}

double calculate_qp_objective(opt_n,opt_g,opt_g0,alpha)
long opt_n;
double *opt_g,*opt_g0,*alpha;
{
  double obj;
  long i,j;
  obj=0;  /* calculate objective  */
  for(i=0;i<opt_n;i++) {
    obj+=(opt_g0[i]*alpha[i]);
    obj+=(0.5*alpha[i]*alpha[i]*opt_g[i*opt_n+i]);
    for(j=0;j<i;j++) {
      obj+=(alpha[j]*alpha[i]*opt_g[j*opt_n+i]);
    }
  }
  return(obj);
}