// system includes
#include <iostream.h>
#include <math.h>
#include <stdio.h>

// lp includes
#define CPP
#include "lp/lp.h"

// local includes
#include "assert.h"
#include "vec4.h"
#include "bounds3d.h"
#include "fincident.h"

#define LP_DIM 4
#define LP_EPS (LP_DIM * EPS)	    /* EPS = 1.0E-7F */

  //this n should match whatever n you're going to call halfspaces_incident_axialboxes with.
LPMem::LPMem (int n) {
    vecs = new Plane[n+6];

    //contiguous float version of vecs
    fvecs = new float [4 * (n+6)];
    
    // lp workspace; (n+6)+3
    work = (float *) malloc((unsigned)sizeof(float) * (((n + 6) + 3) * 5));
    
    // permuted constraint indices
    perm = (int *) malloc((unsigned)sizeof(int) * (n+6));
    
    // next, previous constraints
    next = (int *) malloc((unsigned)sizeof(int) * ((n+6) + 1));
    prev = (int *) malloc((unsigned)sizeof(int) * ((n+6) + 1));
}
LPMem::~LPMem () {
  delete vecs;
  delete fvecs;
  free (work);
  free (perm);
  free (next);
  free (prev);
}

// generate permuted list of indices 0..n-1
static void
randperm(
    int n, 
    int *perm)
{
int i, j, t;

    assert (n > 0);

	// unpermuted list
    for(i = 0; i < n; i++)
	  perm[i] = i;

	// permute
    for(i = 0; i < n; i++) {
    	j = rand() % (n-i) + i;	    // mike's soln
    	// j = rand() % (i + 1);    // this is ok too
		// j = rand() % n;  -- definitely gives non-random distribution!
		t = perm[j];
		perm[j] = perm[i];
		perm[i] = t;
	}
}

/* given:
   n 4-coefficient plane equations
   an axial bounding box
   return
     1 iff intersection of positive halfspaces incident on bounding box
	   in this case, if witness non-null, place feasible point in witness
	 0 otherwise
*/
int
halfspaces_incident_axialbox (
    const Plane* peqn, int n,
    const Bounds3d& box,
    Point3d *witness, LPMem *lpmem )
{
float numer[4], denom[4], opt4[4];
int k, tk, status, ninside;

    // no constraints to satisfy
    if (n < 1)	{
	  if (witness)
	    *witness = box[0];
	  return (1);
	}

#define LPTRIVIALACCEPT
#ifdef LPTRIVIALACCEPT
    // compute midpoint of axial bounding box
	Point3d mid = 0.5 * (box[0] + box[1]);
	mid[3] = 1.;

    // is midpoint inside all planes?
	for (k = ninside = 0; k < n; k++) {
	  if ( peqn[k].PointPlaneDist (mid) >= -1.0E-5)
		ninside++;
	  else
		break;
	}

    if (ninside == n)	{
	  if (witness)
	    *witness = mid;
	  return (1);
	}
#endif

	// constraint coefficients; n + 6
    //XXX this ain't MP-safe!! alloc all these guys per-render-worker.
    Plane *vecs = lpmem->vecs;
    
	// lp workspace; (n+6)+3
    float* work = lpmem->work;

    // permuted constraint indices
    int* perm = lpmem->perm;

    // next, previous constraints
    int* next = lpmem->next;
    int* prev = lpmem->prev;

	// convert axial bounds into six oriented halfspaces
    box.FillHalfspaces (vecs);

    // generate permuted list of indices
    randperm ( n, perm );

    // fill in constraints for given half spaces
    for (k = 0; k < n; k++) {
      tk = 6 + perm[k];
      vecs[tk]=peqn[k];
    }

    // set up objective function numer.p / denom.p
    numer[0] = 1.0;	   /* minimize x + y + z */
    numer[1] = 1.0;
    numer[2] = 1.0;
    numer[3] = 0.0;

    denom[0] = 0.0;
    denom[1] = 0.0;
    denom[2] = 0.0;
    denom[3] = 0.0; //XXX we sort of think this might fix LP.

    for(k=0; k < n+6; k++) {
	  // set up linked list for linear program
	  next[k] = k+1;
	  prev[k+1] = k;

	  /* unitize halfspaces */
	  vecs[k].FastMakeUnit4();
	}

    // set up initial optimum for linear program
    opt4[0] = box[0][0];	    /* initial optimum is minimum */
    opt4[1] = box[0][1];
    opt4[2] = box[0][2];
    opt4[3] = 1.0;

    float *fvecs = lpmem->fvecs;
    //convert the vecs into contiguous floats which linprog requires
    for (k = 0; k < n+6; k++) {
      fvecs[k*4 + 0] = vecs[k][0]; 
      fvecs[k*4 + 1] = vecs[k][1]; 
      fvecs[k*4 + 2] = vecs[k][2]; 
      fvecs[k*4 + 3] = vecs[k][3]; 
    }
	// check feasibility of linear program
    //    status = linprog (fvecs, 6, n+6, numer, denom, 3, opt4, work, next, prev, n+6);
    //XXX testing; don't give it an incremental optimimum pt to start with.
    status = linprog (fvecs, 0, n+6, numer, denom, 3, opt4, work, next, prev, n+6);

#define LPSANITYCHECK 1
#ifdef LPSANITYCHECK
    if (status == INFEASIBLE)	{   /* sanity check: check vertices of extent */
	  int xi, yi, zi;

	  for (xi = 0; xi < 1; xi++)
		for (yi = 0; yi < 1; yi++)
		  for (zi = 0; zi < 1; zi++)	{
		    Vec4 cornerpt (box[xi][0], box[yi][1], box[zi][2], 1.);
		    Vec4 peqnnormalized;
	    	for (k = ninside = 0; k < n; k++)
		  {
		    peqnnormalized = peqn[k]; peqnnormalized.PlaneNormalize();
		    ninside += (peqnnormalized.PointPlaneDist (cornerpt) >= 1.0E-5);
		  }

	    	if (ninside == n)   {
			  fprintf (stderr, "linprog failed! extent vtx %6.3f %6.3f %6.3f inside %d halfspaces!\n", 
					   cornerpt[0], cornerpt[1], cornerpt[2], n);
			  for (k = 0; k < n; k++)
			    {
			      peqnnormalized = peqn[k]; peqnnormalized.PlaneNormalize();
			      fprintf (stderr, "p . h[%2d] (%9.6f %9.6f %9.6f %9.6f) is %6.3f\n",
						 k, peqnnormalized[0],  peqnnormalized[1],  peqnnormalized[2],  peqnnormalized[3],
						 peqnnormalized.PointPlaneDist (cornerpt));
			    }
		    }
		  }   /* zi */
	}
    else    {	// feasible; is optimum really inside?
	  // project optimum from 3DH to 3D by division by w
	  Vec4 optpt (opt4[0] / opt4[3], opt4[1] / opt4[3], opt4[2] / opt4[3], 1.);
	  // check that optimum is really on or above every constraint plane
	  Vec4 peqnnormalized;
	  for (k = ninside = 0; k < n; k++)
	    {
	      peqnnormalized = peqn[k]; peqnnormalized.PlaneNormalize();
	    ninside += (peqnnormalized.PointPlaneDist (optpt) >= -1.0E-4);
	    }
	  if (ninside < n)    {
	    fprintf (stderr, "linprog failed! optimum %6.3f %6.3f %6.3f %6.3f not inside %d of %d halfspaces!\n", 
				 optpt[0], optpt[1], optpt[2], optpt[3], n - ninside, n);
	    cerr << box << endl;
	    for (k = 0; k < n; k++)
	      {
		peqnnormalized = peqn[k]; peqnnormalized.PlaneNormalize();
		  fprintf (stderr, "p . h[%2d] (%9.6f %9.6f %9.6f %9.6f) is %6.7f\n",
				   k, peqnnormalized[0],  peqnnormalized[1],  peqnnormalized[2],  peqnnormalized[3],
				   peqnnormalized.PointPlaneDist (optpt));
	      }
	  }
	}
#endif

    if (status != INFEASIBLE)	{
	  if (witness)
	    *witness = Point3d (opt4[0] / opt4[3], opt4[1] / opt4[3], opt4[2] / opt4[3], 1. );
	  return (1);
	}
    else
	  return (0);
}

#if 0
/* 1 iff extent incident on the intersection of halfspaces in hsarray */
/* NOT PARALLELIZED */
int
peqn_array_incident_extent(
    PLANE  *peqn, int n,
    EXTENT  *extent)
{
    return (peqn_array_incident_extent_witness(peqn, n, extent, NULL));
}

/* 1 iff point incident on all halfspaces */
int
peqn_array_incident_point (
    PLANE  *peqns, int npeqns,
    POINT  *p)
{
int k;

    assert (npeqns >= 0);

    if (npeqns == 0)
	return 1;

    for (k = 0; k < npeqns; k++)
	if (!point_onorabove_plane (p, peqns + k))
	    return 0;
    return 1;
}

#endif