/* lptest.c : exercise mike hohmeyer's implementation of raimund
    seidel's randomized linear programming algorithm */

/* compile as:

    cc -c lptest.c
    cc -o lptest lptest.o -L<libdir> -llp -lm
*/

#include <stdio.h>
#include <math.h>
#include <malloc.h>
#include "lp.h"

#define MAX(a,b) ((a) >= (b)) ? (a) : (b)
#define MIN(a,b) ((a) <= (b)) ? (a) : (b)

static int debug, verbose;

void
usage(char *prog)
{
    printf ("usage: %s [-i # iterations, default 10] -d[ebug] -v[erbose]\n", prog);
    exit (0);
}

float *
falloc(int nfloats)
{
float *storage;

    if ((storage = (float *)malloc (nfloats * sizeof (float))) == NULL)
	exit (fprintf (stderr, "can't malloc %d floats!\n", nfloats));
    return (storage);
}

int *
ialloc(int nints)
{
int *storage;

    if ((storage = (int *) malloc (nints * sizeof (int))) == NULL)
	exit (fprintf (stderr, "can't malloc %d ints!\n", nints));
    return (storage);
}

void
randperm(
    int n,
    int perm[])
{
int i, j, t;

    for(i=0; i<n; i++)
	perm[i] = i;
    for(i=0; i<n; i++) {
	j = rand()%(n-i)+i;
	t = perm[j];
	perm[j] = perm[i];
	perm[i] = t;
	}
}

/* set up next, prev lists for lp instance */
/* randomization's handled entirely within */
void
generate_lp_params (
    int *next, 
    int *prev, 
    int *perm,
    int nhalves)
{
int k;

/* 
   from lp doc;
	the order of the half spaces is 0, next[0], next[next[0]] ...
	and prev[next[i]] = i

	the optimum has been computed for the half spaces
	0 , next[0], next[next[0]] , ... , prev[istart]

   so, a trivially correct, although non-randomized, solution is:

   for (k = 0; k < nhalves; k++)   {
	next[k] = k+1;
	prev[k+1] = k;
	}

   we want to randomize the halfspace order, however.
*/
    /* scramble constraints */
    randperm (nhalves-1, perm);		/* in 0..nhalves-2 */

    next[perm[0] + 1] = nhalves;		/* n[j], 0 < j <= nhalves-1 */
    prev[nhalves] = perm[0] + 1;

    for (k = 1; k < nhalves-1; k++)  {
	next[perm[k] + 1] = perm[k-1] + 1;
	prev[perm[k-1] + 1] = perm[k] + 1;
	}

    next[0] = perm[nhalves-2] + 1;
    prev[next[0]] = 0;
}

int
main(
    int argc,
    char **argv)
{
extern char *optarg;
int c, d, j, k;
int nhs, ndim, nits, status;
float *halves, *opt, *numer, *denom, *work;
int *next, *prev, *perm;
float mindist, maxdist, dist;

    nits = 10;
    debug = 0;

    while((c = getopt(argc,argv,"di:v")) != EOF) {
	switch(c) {
	    case 'd':
		debug = 1;
		fprintf (stderr, "debugging on\n");
		break;

	    case 'i':
		nits = atoi (optarg);
		if (debug)
		    fprintf (stderr, "will do %d iterations\n", nits);
		break;

	    case 'v':
		verbose = 1;
		fprintf (stderr, "verbose = %d\n", verbose);
		break;

	    default :
		usage (argv[0]);
		break;
	    }
	}

    /* read number of halfspaces, dimensions */
    if (scanf ("%d %d\n", &nhs, &ndim) != 2)	{
	fprintf (stderr, "expecting <# halves> <# dims>\n");
	exit (-1);
	}

    /* alloc halfspace storage (d+1 coeffs each) */
    halves = falloc (nhs * (ndim + 1));

    /* alloc optimum, obj fn numerator, obj fn denominator (d+1 coeffs) */
    opt = falloc (ndim + 1);
    numer = falloc (ndim + 1);
    denom = falloc (ndim + 1);

    /* alloc work area: (max_size+3)*(d+2)*(d-1)/2 float space */
    work = falloc ((nhs + 3) * (ndim + 2) * (ndim - 1) / 2);

    /* alloc linked list storage, permutation array */
    next = ialloc (nhs + 1);
    prev = ialloc (nhs + 1);
    perm = ialloc (nhs + 1);

    /* read the points */
    for (k = 0; k < nhs; k++)
	for (d = 0; d < ndim + 1; d++)
	    if (scanf (" %f ", &halves[k * (ndim+1) + d]) != 1)   {
		fprintf (stderr, "expecting float halves for halfspace %d coeff %d\n", k, d);
		exit (-1);
		}

    /* assign rational linear objective function */
    for (d = 0; d <= ndim; d++)	{
	numer[d] = (d == ndim) ? 1.0 : 0.0;
	denom[d] = 0.0;
	}

    /* call linprog multiple times, printing the result */
    fprintf (stderr, "\t%12s%12s%12s%12s\n", "iteration", "return val", "min dist", "max dist");

    for (k = 1; k <= nits; k++)	{
	/* randomize */
	generate_lp_params (next, prev, perm, nhs);

	/* call linprog */
	status = linprog ((float *)halves, 0, nhs, numer, denom, ndim, opt, work, next, prev, nhs);
    
	if (status != INFEASIBLE)   {
	    /* normalize */
	    for (d = 0; d < ndim; d++)
		opt[d] /= opt[ndim];
	    opt[ndim] = 1.0;

	    mindist = 1E10;
	    maxdist = 0.0;
	    for (j = 0; j < nhs; j++)	{
		dist = halves[j * (ndim + 1) + ndim];
		for (d = 0; d < ndim; d++)
		    dist += halves[j * (ndim + 1) + d] * opt[d];

		/* dist should be >= 0.0 */
		if (dist < 0.0)	{   
		    fprintf (stderr, "feasible optimum violates halfspace %d\n", j);
		    fprintf (stderr, "\toptimum\t:");
		    for (d = 0; d <= ndim; d++)
			fprintf (stderr, "%12.4f", opt[d]);
		    fprintf (stderr, "\n");
		    }

		mindist = MIN (dist, mindist);
		maxdist = MAX (dist, mindist);
		}
	    if (verbose || k == 1 || k == nits)
		fprintf (stderr, "\t%12d%12d%12.4f%12.4f\n", k, status, mindist, maxdist);
	    }
	else	{
	    if (verbose || k == 1 || k == nits)
	        fprintf (stderr, "\t%12d%12s\n", k, "INFEASIBLE");
	    }
	}
}