#include <stdlib.h>
#include <stdarg.h>
#include <stream.h>
#include <AutoArray.H>
#include "Hedron.H"

static char rcsid_Hedron[] = "$Id: Hedron.C,v 1.1 90/12/03 03:15:11 marc Exp $";

Hedron::Hedron(const Matrix& points, int nedges, int first ...)
:vertices(points)
{
   int i;
   va_list ap;

   edges = (int *) malloc(sizeof(int)*2*nedges);
   num_edges = nedges;
   
   va_start(ap, first);
   edges[0] = first;
   for (i=1; i < 2*nedges; i += 1)
      edges[i] = va_arg(ap, int);
   va_end(ap);
}

Hedron::Hedron(const Matrix& points, int nedges, const int *edgesp)
:vertices(points)
{
   num_edges = nedges;
   edges = (int *) malloc(sizeof(int)*2*nedges);
   if (edgesp)
      bcopy(edgesp, edges, 2*num_edges*sizeof(int));
   else
      bzero(edges,2*num_edges*sizeof(int));
}

Hedron::Hedron(const Hedron& h)
:vertices(h.vertices)
{
   edges = (int *) malloc(sizeof(int)*2*h.num_edges);
   num_edges = h.num_edges;
   bcopy(h.edges, edges, 2*num_edges*sizeof(int));
}

Hedron::~Hedron()
{
   free(edges);
}

Hedron operator+(const Hedron& h1, const Hedron& h2)
{
   int i;

   Matrix m(h1.vertices & h2.vertices);
   AutoArray edges_aa(sizeof(int), (h1.num_edges+h2.num_edges)*2);
   int *edges = (int *) edges_aa.Data();

   for (i=0;i<h1.num_edges*2;i++)
      edges[i] = h1.edges[i];
   for (i=0;i<h2.num_edges*2;i++)
      edges[i+h1.num_edges*2] = h2.edges[i]+h1.vertices.Rows();

   return(Hedron(m,h1.num_edges+h2.num_edges,edges));
}

Hedron operator*(const Hedron& h, const Matrix& m)
{
   Hedron h1(h.vertices * m, h.num_edges, h.edges);

   return h1;
}

Hedron& Hedron::operator=(const Hedron& h)
{
     free(edges);

     vertices = h.vertices;
     edges = (int *) malloc(sizeof(int)*2*h.num_edges);
     num_edges = h.num_edges;
     bcopy(h.edges, edges, 2*num_edges*sizeof(int));

     return(*this);
}

ostream& operator<<(ostream& s, const Hedron& h)
{
     int i;
     
     s << h.vertices << "\n";
     for (i=0;i<h.num_edges;i++)
#ifdef __GNUG__
	  s.form("%6d  %6d\n",h.edges[i*2],h.edges[i*2+1]);
#else
          s << form("%6d  %6d\n",h.edges[i*2],h.edges[i*2+1]);
#endif
     return(s);
}

void Hedron::Print(void) const
{
   cout << *this;
}

static int outcode(int dim, double *lower, double *upper, double *point)
{
   int i;
   int code=0;

   for (i=0;i<dim;i++) {
      if (point[i] < lower[i]) code += 1<<(i*2);
      if (point[i] > upper[i]) code += 1<<(i*2+1);
   }

   return(code);
}

Hedron Hedron::Clip(const Matrix& c) const
{
   if ((vertices.Cols() != c.Cols()+1) ||
       (c.Rows() != 2))
      abort();

   Matrix clip(num_edges*2,vertices.Cols());
   int i,j,k,p;
   double *pa,*pb;
   int oa,ob;
   double *pt;
   int ot;
   double param,t;

   p=0;
   for (i=0; i<num_edges; i++) {
      // Copy unclipped points into new matrix
      for (j=0; j<vertices.Cols(); j++) {
	 clip(p*2+1,j+1) = VertexCoord(i*2,j+1);
	 clip(p*2+2,j+1) = VertexCoord(i*2+1,j+1);
      }

      pa = clip.Row(p*2+1);
      pb = clip.Row(p*2+2);
      oa = outcode(c.Cols(),c.Row(1),c.Row(2),pa);
      ob = outcode(c.Cols(),c.Row(1),c.Row(2),pb);

      while(1) {
	 if ((oa == 0) && (ob == 0)) {
	    // trivially accept
	    // advance pointer to next position
	    p+=1;
	    break;
	 } else if (oa&ob) {
	    // trivially reject
	    break;
	 } else {
	    if (oa == 0) {
#define SWAP(x,y,t) t=x;x=y;y=t
	       SWAP(pa,pb,pt);
	       SWAP(oa,ob,ot);
	    }

	    /* a is invisible.  clip it */
	    param = 1.0;

	    for (j=0; j<c.Cols(); j++) {
	       // if min along this axis
	       if (oa&(1<<(j*2))) {
		  oa ^= 1<<(j*2);
		  // compute parameter
		  t = (c(1,j+1)-pb[j])/(pa[j]-pb[j]);
	       } else if (oa&(1<<(j*2+1))) {
		  oa ^= 1<<(j*2+1);
		  // compute parameter
		  t = (c(2,j+1)-pb[j])/(pa[j]-pb[j]);
	       } else {
		  continue;
	       }
	       // if this parameter clips more than the previous, keep it
	       if (t<param) param = t;
	    }

	    if (param < 0)
	       break;
	    // clip the points
	    for (k=0; k<clip.Cols(); k++)
	       pa[k] = pb[k]+param*(pa[k]-pb[k]);
	 }
      }
   }

   AutoArray clip_edges_aa(sizeof(int), p*2);
   int *clip_edges = (int *) clip_edges_aa.Data();
   for (i=0;i<p*2;i++)
      clip_edges[i] = i+1;
   
   return(Hedron(Matrix(clip,1,1,p*2,clip.Cols()),
		 p,
		 clip_edges));
}

Hedron& Hedron::AddEdge(int from, int to)
{
   num_edges++;
   edges = (int *) realloc(edges,num_edges*2*sizeof(int));
   edges[num_edges*2-2] = from;
   edges[num_edges*2-1] = to;

   return(*this);
}