#include <stdio.h>
#include "opt.h"

#define Debug	1

/*
	Find one feasible solution
	
	Find x (>=0) s.t. Ax = b
	
	(matrix A and vector b are arbitrary)
	
	extern status LPfeasiblesolution
	(int m, int n, dbl a[], dbl b[], dbl eps, dbl ox[])
	
	inputs: m = number of equations
		n = number of variables
		a = coefficient matrix (m,n)
		b = constants (m,1)
		eps : small value
	outputs: ox : feasible solution (if exists)
	return value
		success : feasible solution is found
		failure : not found
*/

static void buildfeasibilitytableau
(m, n, a, b, aextend, cfeas, dfeas, base)
int	m, n;
dbl	*a, *b, *aextend, *cfeas, *dfeas;
int	*base;
{
	int	i, j;
	
	for (i=0; i<m; i++) {
		if (b[i] >= 0.00) continue;
		matrixrowscalar(m, n, a, i, -1.00);
		b[i] = -b[i];
	}
	
	for (i=0; i<m; i++) {
		for (j=0; j<n; j++) {
			aextend[(n+m)*i+j] = a[n*i+j];
		}
		for (j=n; j<n+m; j++) {
			aextend[(n+m)*i+j] = 0.00;
		}
		aextend[(n+m)*i+(n+i)] = 1.00;
	}
	
	for (j=0; j<n; j++) {
		cfeas[j] = 0.00;
		for (i=0; i<m; i++) {
			cfeas[j] += a[n*i+j];
		}
	}
	for (j=0; j<m; j++) {
		cfeas[n+j] = 0.00;
	}
	
	*dfeas = 0.00;
	for (i=0; i<m; i++) {
		*dfeas -= b[i];
	}
	
	for (i=0; i<m; i++) {
		base[i] = n+i;
	}
}

extern status LPfeasiblesolution
(int m, int n, dbl a[], dbl b[], dbl eps, dbl ox[])
{
	dbl	*aextend, *cfeas, dfeas;
	int	*base;
	status	st;
	
	aextend = allc(dbl,m*(n+m));
	cfeas = allc(dbl,1*(n+m));
	base = allc(int,m);
	
	buildfeasibilitytableau(m, n, a, b, aextend, cfeas, &dfeas, base);
	st = LPsimplextableau(m, n+m, aextend, b, cfeas, &dfeas, eps, base);
	if (st == success && dfeas > -eps) {
		LPsimplexsolution(m, n, b, &dfeas, base, ox, &dfeas);
		return success;
	} else {
		return failure;
	}
}


/*
	Two Phase method
	
	maximize     cx + d
	subject to   Ax = b
		     x >= 0
	
	(matrix A, vector b, vector c, and scalar d are arbitrary)
	
	extern status TwoPhasemethod
	(int m, int n, dbl a[], dbl b[], dbl c[], dbl *d, dbl eps,
	dbl ox[], dbl *ov)
	
	inputs: m = number of equations
		n = number of variables
		a : (m,n) coefficient matrix
		b : (m,1) constants
		c : (1,n) coefficients of objective function
		*d : constant of objective function
		eps : small value
	outputs: ox : (m,1) optimal solution
		*ov : optimal value
	return value
		success : find optimal solution
		failure : diverge
		error   : no feasible solution
*/

static void buildtwophasetableau
(m, n, a, b, c, d, aextend, cextend, cfeas, dfeas, base)
int	m, n;
dbl	*a, *b, *c, *d, *aextend, *cextend, *cfeas, *dfeas;
int	*base;
{
	int	i;
	
	buildfeasibilitytableau(m, n, a, b, aextend, cfeas, dfeas, base);
	for (i=0; i<n; i++) cextend[i] = c[i];
	for (i=n; i<n+m; i++) cextend[i] = 0.00;
}

static void twophasetableaxdump(m, n, aextend, b, cfeas, dfeas, base,
				cextend, d)
int	m, n;
dbl	*aextend, *b, *cfeas, *dfeas, *cextend, *d;
int	*base;
{
	int	i;
	
	matrixfprint(stdout, m, n+m, aextend);
	matrixfprint(stdout, m, 1, b);
	matrixfprint(stdout, 1, n+m, cfeas);
	printf("%lf\n\n", *dfeas);
	for (i=0; i<m; i++) printf("%d\n", base[i]);
	matrixfprint(stdout, 1, n+m, cextend);
	printf("%lf\n", *d);
}

static void reducetwophasetableau(m, n, a, c, aextend, cextend)
int	m, n;
dbl	a[], c[], *aextend, *cextend;
{
	int	i, j;
	
	for (i=0; i<m; i++) {
		for (j=0; j<n; j++) {
			a[n*i+j] = aextend[(n+m)*i+j];
		}
	}
	for (j=0; j<n; j++) {
		c[j] = cextend[j];
	}
}

extern status TwoPhasemethod
(int m, int n, dbl a[], dbl b[], dbl c[], dbl *d, dbl eps, dbl ox[], dbl *ov)
{
	dbl	*aextend, *cextend, *cfeas, dfeas;
	int	*base;
	status	st;
	
	aextend = allc(dbl,m*(n+m));
	cextend = allc(dbl,1*(n+m));
	cfeas = allc(dbl,1*(n+m));
	base = allc(int,m);
	
	/*** 1st phase ***/
	buildtwophasetableau(m, n, a, b, c, d,
			     aextend, cextend, cfeas, &dfeas, base);
	st = TwoPhasemethodtableau(m, n+m, aextend, b, cfeas, &dfeas,
				   cextend, d, eps, base);
	if ( ! (st == success && dfeas > -eps) ) {
		/* no feasible solution */
		return error;
	}
	
	/*** 2nd phase ***/
	reducetwophasetableau(m, n, a, c, aextend, cextend);
	st = LPsimplextableau(m, n, a, b, c, d, eps, base);
	if (st == success) {
		LPsimplexsolution(m, n, b, d, base, ox, ov);
	}
	return st;
}