/* 
 * linprog.c
 *
 * Copyright (c) 1990 Michael E. Hohmeyer,
 *       hohmeyer@miro.berkeley.edu,
 * Permission is granted to modify and re-distribute this code in any manner
 * as long as this notice is preserved.  All standard disclaimers apply.
 *
 */
#include <math.h>
#include "lp.h"


extern void vector_up(double equation[], int ivar, int idim,
		      double low_vector[], double vector[]);


int lp_d_unit(int d, double a[], double b[]);
double lp_eval(double pnt[], int dim, double plane[]);
void plane_down(double elim_eqn[], int ivar, int idim,
	   double old_plane[], double new_plane[]);
int linprog(double halves[], int istart, int m, double n_vec[], double d_vec[],
	int d, double opt[], double *work, int *next, int *prev, int max_size);
int move_to_front(int i, int *next, int *prev);
int lp_no_constraints(int d, double n_vec[], double d_vec[],
		  double opt[]);
void findimax(double plane[], int idim, int *imax);


/* unitize a d+1 dimensional point */
#ifdef GCC
inline
#endif
int lp_d_unit(int d, double a[], double b[])
{
	int i;
	double size;

	size = 0.0;
	for(i=0; i<=d; i++) 
		size += a[i]*a[i];
	if(size < (d+1)*EPS) return(1);
	size = 1.0/sqrt(size);
	for(i=0; i<=d; i++)
		b[i] = a[i]*size;
	return(0);
}
/* determine if a point is in a half space */
#ifdef GCC
inline
#endif
double
lp_eval(double pnt[], int dim, double plane[])
{
	double v;
	int i;

	v = 0.0;
	for(i=0; i<=dim; i++)
		v += plane[i]*pnt[i];
	return(v);
}
#ifdef GCC
inline
#endif
void plane_down(double elim_eqn[], int ivar, int idim,
	   double old_plane[], double new_plane[])
{
	register double crit;
 	register int i;

	crit = old_plane[ivar]/elim_eqn[ivar];
	for(i=0; i<ivar; i++)  {
		new_plane[i] = old_plane[i] - elim_eqn[i]*crit;
	}
	for(i=ivar+1; i<=idim; i++)  {
		new_plane[i-1] = old_plane[i] - elim_eqn[i]*crit;
	}
}
char *malloc();

#include <math.h>
#include <lp_math.h>

int linprog(double halves[], int istart, int m, double n_vec[], double d_vec[],
	int d, double opt[], double *work, int *next, int *prev, int max_size)
/*
** halves  --- half spaces
** istart  --- should be zero unless doing incremental algorithm
** m       --- terminal marker
** n_vec   --- numerator vector
** d_vec   --- denominator vector
** d       --- projective dimension
** opt     --- optimum
** work    --- work space (see below)
** next    --- array of indices into halves
** prev    --- array of indices into halves
** max_size --- size of halves array
**
**
** half-spaces are in the form
** halves[i][0]*x[0] + halves[i][1]*x[1] + 
** ... + halves[i][d-1]*x[d-1] + halves[i][d]*x[d] >= 0
**
** coefficients should be normalized
** half-spaces should be in random order
** the order of the half spaces is 0, next[0] next[next[0]] ...
** and prev[next[i]] = i
**
** halves[max_size][d+1]
**
** the optimum has been computed for the half spaces
** 0 , next[0], next[next[0]] , ... , prev[istart]
** the next plane that needs to be tested is istart
**
** m is the index of the first plane that is NOT on the list
** i.e. m is the terminal marker for the linked list.
**
** the objective function is dot(x,nvec)/dot(x,dvec)
** if you want the program to solve standard d dimensional linear programming
** problems then n_vec = ( x0, x1, x2, ..., xd-1, 0)
** and           d_vec = (  0,  0,  0, ...,    0, 1)
** and halves[0] = (0, 0, ... , 1)
**
** work points to (max_size+3)*(d+2)*(d-1)/2 double space
*/
{
	int status;
	int i, j, imax;
	double *new_opt, *new_n_vec, *new_d_vec,  *new_halves, *new_work;
	double *plane_i;
	double val;

	if(d==1 && m!=0) {
		return(lp_base_case((double (*)[2])halves,m,n_vec,d_vec,opt,
			next,prev));
	} else {
/* find the unconstrained minimum */
		if(!istart) {
			status = lp_no_constraints(d,n_vec,d_vec,opt); 
		} else {
			status = MINIMUM;
		}
		if(m==0) return(status);
/* allocate memory for next level of recursion */
		new_opt = work;
		new_n_vec = new_opt + d;
		new_d_vec = new_n_vec + d;
		new_halves = new_d_vec + d;
		new_work = new_halves + max_size*d;
		for(i = istart; i!=m; i=next[i]) {
#ifdef CHECK
			if(i<0 || i>=max_size) {
				printf("index error\n");
				exit(1);
			}
#endif
/* if the optimum is not in half space i then project the problem
** onto that plane */
			plane_i = halves + i*(d+1);
			if(lp_eval(opt,d,plane_i)<-(d+1)*EPS) {
/* find the largest of the coefficients to eliminate */
			    findimax(plane_i,d,&imax);
/* eliminate that variable */
			    if(i!=0) {
				for(j=0; j!=i; j=next[j]) {
					plane_down(plane_i,imax,d,
					halves+j*(d+1),new_halves+j*d);
				}
			    }
/* project the objective function to lower dimension */
			    plane_down(plane_i,imax,d,n_vec,new_n_vec);
			    plane_down(plane_i,imax,d,d_vec,new_d_vec);
/* solve sub problem */
			    status = linprog(new_halves,0,i,new_n_vec,
			    new_d_vec,d-1,new_opt,new_work,next,prev,max_size);
/* back substitution */
			    if(status!=INFEASIBLE) {
				    vector_up(plane_i,imax,d,new_opt,opt);
				    (void)lp_d_unit(d,opt,opt);
			    } else {
				    return(status);
			    }
/* place this offensive plane in second place */
			    i = move_to_front(i,next,prev);
			    j=0;
			    while(1) {
/* check the validity of the result */
				val = lp_eval(opt,d,halves + j*(d+1));
				if(val <-(d+1)*EPS) {
				    printf("error\n");
				    exit(1);
				}
				if(j==i)break;
				j=next[j];
			    }
			} 
		}
 		return(status);
	}
}
/* returns the index of the plane that is in i's place */
int move_to_front(int i, int *next, int *prev) 
{
	int previ;
	if(i==0 || i == next[0]) return i;
	previ = prev[i];
/* remove i from it's current position */
	next[prev[i]] = next[i];
	prev[next[i]] = prev[i];
/* put i at the front */
	next[i] = next[0];
	prev[i] = 0;
	prev[next[i]] = i;
	next[0] = i;
	return(previ);
}
/* optimize the objective function when there are no contraints */
int lp_no_constraints(int d, double n_vec[], double d_vec[],
		  double opt[])
{
	int i;
	double n_dot_d, d_dot_d;

	n_dot_d = 0.0;
	d_dot_d = 0.0;
	for(i=0; i<=d; i++) {
		n_dot_d += n_vec[i]*d_vec[i];
		d_dot_d += d_vec[i]*d_vec[i];
	}
	if(d_dot_d < EPS*EPS) {
		d_dot_d = 1.0;
		n_dot_d = 0.0;
	}
	for(i=0; i<=d; i++) {
		opt[i] = -n_vec[i] + d_vec[i]*n_dot_d/d_dot_d;
	}
/* normalize the optimal point */
	if(lp_d_unit(d,opt,opt)) {
		opt[d] = 1.0;
		return(AMBIGUOUS);
	} else {
		return(MINIMUM);
	}
}
/* find the largest coefficient in a plane */
void findimax(double plane[], int idim, int *imax)
{
	double rmax;
	int i;

	*imax = 0;
	rmax = fabs(plane[0]);
	for(i=1; i<=idim; i++) {
		if(fabs(plane[i])>rmax) {
			*imax = i;
			rmax = fabs(plane[i]);
		}
	}
}