#include "Movie.h"
#include "MovieObject.h"
#include "List.h"
#include <iostream.h>
#include "movieoutput.h"
#include "cell.h"
#include "line.h"
#include "assert.h"
#include "pursuer.h"
#include "vispoly.h"
#include "pgraph.h"

void MV_movietest(void);

void MakeMovie(char **argv, int argc, PInfo *pInfo)
{
  // potential problem: is cout going to the right place?


  PursuerMovie M;
  M.movieDimension() = MD_2D;
  M.infoBit() = 1;
  M.SetOutput(argv, argc);
  M.resolution = M.CalculateWidth(pInfo->wInfo->border) * RESOLUTION;
  if( M.resolution < 1 ) M.resolution = 1;

  M.AddObstacles(pInfo->wInfo);
  if( M.bDrawCells ) M.AddCells(pInfo);
  M.AddRobot(pInfo->wInfo, pInfo);
  if( M.bDrawPolys ) M.AddVPolys(pInfo->cellInfo);
  else M.AddFrames(pInfo);

  cout << M;
}

void MakeMultiMovie(char **argv, int argc, PInfo *pInfo1, PInfo *pInfo2)
{
  MultiMovie M;
  M.movieDimension() = MD_2D;
  M.infoBit() = 1;
  M.SetOutput(argv, argc);
  M.resolution = M.CalculateWidth(pInfo1->wInfo->border) * RESOLUTION;
  if( M.resolution < 1 ) M.resolution = 1;

  M.AddObstacles(pInfo1->wInfo); // pInfo2 has same obstacles
  if( M.bDrawCells ) M.AddCells(pInfo2);
  M.AddRobot(pInfo1->wInfo, pInfo1);
  M.AddRobot(pInfo2->wInfo, pInfo2);
  M.AddFrames(pInfo1, pInfo2);

  cout << M;
}

void PursuerMovie::SetOutput(char **argv, int argc)
{
  int i;

  bDrawCells = FALSE;
  bDrawPolys = FALSE;
  bDrawGaps = FALSE;
  bFast = FALSE;
  for( i = 2; i < argc; i++ ) {
    if( argv[i][0] == 'c' ) bDrawCells = TRUE;
    if( argv[i][0] == 'p' ) bDrawPolys = TRUE;
    if( argv[i][0] == 'v' ) bDrawPolys = TRUE;
    if( argv[i][0] == 'e') bDrawGaps = TRUE;
    if( argv[i][0] == 'f') bFast = TRUE;
  }
  if( bDrawCells ) printf("Drawing cell borders\n");
  if( bDrawPolys ) printf("Drawing vispolys\n");
  if( bDrawGaps ) printf("Drawing gap edges\n");

}

/* add border & obstacles to the movie as static components */
void PursuerMovie::AddObstacles(WorldInfo *wInfo)
{
  List<MC_MovieObject> staticList;
  MC_Polygon *ob;
  MC_MovieObject mOb;
  MC_Coord x[MOVIE_MAXPTS], y[MOVIE_MAXPTS];
  int i,j;
  double width;

  width = CalculateWidth(wInfo->border) * BORDER_WIDTH;

  /* draw border as a bunch of lines */
  for( i = 0; i < wInfo->border->nPts; i++ ) {
    DumpLine(&staticList, width, MC_Obstacle, 0,
	     wInfo->border->pts[i].x, wInfo->border->pts[i].y,
	     wInfo->border->pts[(i + 1)%wInfo->border->nPts].x,
	     wInfo->border->pts[(i + 1)%wInfo->border->nPts].y);
  }

  /* create a static poly for each other obstacle */
  for( i = 0; i < wInfo->nObs; i++ ) {
    for( j = 0; j < wInfo->obs[i]->nPts; j++ ) {
      x[j] = wInfo->obs[i]->pts[j].x;
      y[j] = wInfo->obs[i]->pts[j].y;
    }
    ob = new MC_Polygon;
    ob->setXY(j, x, y);
    mOb.setObject(ob);
    mOb.nature() = MC_Obstacle;
    staticList.add2Top(mOb);
  }

  staticObjects() = staticList;

}

void PursuerMovie::AddCells(PInfo *pInfo)
{
  int i, j;
  CellInfo *cInfo;
  Cell *c;
  double width;
  List<MC_MovieObject> list;

  cInfo = pInfo->cellInfo;
  list = staticObjects();
  width = CalculateWidth(pInfo->wInfo->border) * PATH_WIDTH;

  for( i = 0; i < cInfo->nCells; i++ ) {
    c = cInfo->cells[i];
    /*      printf("\n\nCell\n----\n");  */
    for( j = 0; j < c->nEdges; j++ ) {
      /*      printf(" (%f, %f), (%f, %f)\n", c->edges[j]->A->x,
	     c->edges[j]->A->y,
	     c->edges[j]->B->x, c->edges[j]->B->y);*/
      DumpLine(&list, width, MC_Decomposition, 0, c->edges[j]->A->x,
	       c->edges[j]->A->y, c->edges[j]->B->x, c->edges[j]->B->y);
    }
    /* now output cell's center */
    MC_MovieObject mOb;
    MC_Ball *ob;
    ob = new MC_Ball;
    ob->setCenter(c->pt->x, c->pt->y);
    ob->setRadius(width / 2);
    
    mOb.setObject(ob);
    mOb.nature() = MC_Path;
    list.add2Bottom(mOb);
  }

  printf("Cells: %d\n\n", cInfo->nCells);
  staticObjects() = list;
}

/* add the frames illustrating a solution to the p/e problem */
void PursuerMovie::AddFrames(PInfo *pinfo)
{
  MC_Frame *frame; // make array to pass in multiple frames to movie
  List<MC_MovieObject> *animated;
  List<MC_MovieObject> *stationary;
  List<MC_MovieObject> *gList;
  int i, frameCount;
  double width, currdist, dist;
  double lastx, lasty;
  Point newP;
  VPoly newVPoly;

  MC_Transform *transform; // make an array if we have multiple moving obs
  List <MC_Transform> *transformList;
  frame = new MC_Frame[pinfo->nSteps * 20];

/* add the frames we've built to the movie */

  animated = &movingObjects();
  stationary = &staticObjects();

  lastx = pinfo->sln[0]->NodeCell()->pt->x;
  lasty = pinfo->sln[0]->NodeCell()->pt->y;

  frameCount = 0;
  for( i = 0; i < pinfo->nSteps; i++ ) {

    // new: interpolate between cell points in steps of size resolution.
    if( i < pinfo->nSteps - 1 && !bFast && !bDrawGaps) {
      dist = LineDistance(pinfo->sln[i]->NodeCell()->pt,
			  pinfo->sln[i + 1]->NodeCell()->pt);
    } else {
      dist = 0.1; // force one step only
    }

    for( currdist = 0; currdist < dist; currdist += resolution ) {

      if( i < pinfo->nSteps - 1 && !bFast && !bDrawGaps ) {
	newP.x = pinfo->sln[i]->NodeCell()->pt->x + currdist/dist *
	  ( pinfo->sln[i + 1]->NodeCell()->pt->x - 
	    pinfo->sln[i]->NodeCell()->pt->x );
	newP.y = pinfo->sln[i]->NodeCell()->pt->y + currdist/dist *
	  ( pinfo->sln[i + 1]->NodeCell()->pt->y -
	    pinfo->sln[i]->NodeCell()->pt->y );
	newP.w = 1;
	assert(MakeVisPoly(pinfo->edgeInfo, &newP, &newVPoly) );
      } else {
	newP.x = pinfo->sln[i]->NodeCell()->pt->x;
	newP.y = pinfo->sln[i]->NodeCell()->pt->y;
	newVPoly = *pinfo->sln[i]->NodeCell()->vPoly;
      }

      gList = new List<MC_MovieObject>;
      if( bDrawGaps ) {
	width = CalculateWidth(pinfo->wInfo->border) * BORDER_WIDTH;
	DumpGaps(gList, width, MC_Goal, MC_Landmark, pinfo->sln[i], 
		 &newVPoly);
      } else {
	DumpVisPoly(gList, MC_Goal, &newVPoly);
      }
      /* now mark the cell center so we can see where we start from */
      
      // use robot moving object
      transformList = new List<MC_Transform>;
      transform = new MC_Transform;

      transform->shift() = MC_Point(newP.x - lastx, newP.y - lasty, 1);
      transformList->add2Top(*transform);
      
      //lastx = pinfo->sln[i]->NodeCell()->pt->x;
      //lasty = pinfo->sln[i]->NodeCell()->pt->y;
      
      frame[frameCount++].setFrame(stationary,
				   animated, *transformList, *gList);
    }
  }
  setFrames(frameCount, frame);
  assert(checkIntegrity(1));
}


void PursuerMovie::AddVPolys(CellInfo *cInfo)
{
  MC_Frame *frame; // make array to pass in multiple frames to movie
  List<MC_MovieObject> *animated;
  List<MC_MovieObject> *stationary;
  List<MC_MovieObject> *gList;
  int i;

  MC_Transform *transform; // make an array if we have multiple moving obs
  List <MC_Transform> *transformList;
  frame = new MC_Frame[cInfo->nCells];

/* add the frames we've built to the movie */


  animated = &movingObjects();
  stationary = &staticObjects();

  for( i = 0; i < cInfo->nCells; i++ ) {
    transformList = new List<MC_Transform>;
    transform = new MC_Transform; // default transformation
    transformList->add2Top(*transform);

    gList = new List<MC_MovieObject>;
    /* now mark the cell center so we can see where we start from */

    MC_MovieObject mOb;
    MC_Ball *ob;
    ob = new MC_Ball;
    ob->setCenter(cInfo->cells[i]->pt->x, cInfo->cells[i]->pt->y);
    ob->setRadius(2);
    mOb.setObject(ob);
    mOb.nature() = MC_Goal;
    gList->add2Top(mOb);
    DumpVisPoly(gList, MC_Path, cInfo->cells[i]->vPoly);

    frame[i].setFrame(stationary, animated, *transformList, *gList);

  }
  setFrames(cInfo->nCells, frame);
  assert(checkIntegrity(1));
}

void PursuerMovie::AddRobot(WorldInfo *wInfo, PInfo *pInfo)
{
  /* todo: create a small ball that will be the pursuer robot. */

  List<MC_MovieObject> movingList;
  MC_MovieObject mOb;
  MC_Ball *ob;
  ob = new MC_Ball;

  if( wInfo->robot != NULL ) {
    //ob->setCenter(wInfo->robot->pts[0].x, wInfo->robot->pts[0].y);
    ob->setCenter(pInfo->sln[0]->NodeCell()->pt->x,
		  pInfo->sln[0]->NodeCell()->pt->y);
    ob->setRadius(CalculateWidth(wInfo->border) * ROBOT_WIDTH);
  } else {
    //ob->setCenter(0, 0);
    ob->setCenter(pInfo->sln[0]->NodeCell()->pt->x,
		  pInfo->sln[0]->NodeCell()->pt->y);
    ob->setRadius(CalculateWidth(wInfo->border) * ROBOT_WIDTH);
  }
    
  movingList = movingObjects(); // so we don't lose others
  mOb.setObject(ob);
  mOb.nature() = MC_Robot;
  movingList.add2Top(mOb);

  movingObjects() = movingList;
}

double PursuerMovie::CalculateWidth(Obstacle *border)
{
  float minX, maxX, minY, maxY, widY, widX;
  int i;
  minX = minY = INFINITY;
  maxX = maxY = -INFINITY;

  for( i = 0; i < border->nPts; i++ ) {
    if( border->pts[i].x < minX ) minX = border->pts[i].x;
    if( border->pts[i].x > maxX ) maxX = border->pts[i].x;
    if( border->pts[i].y < minY ) minY = border->pts[i].y;
    if( border->pts[i].y > maxY ) maxY = border->pts[i].y;
  }
  
  widY = maxY - minY;
  widX = maxX - minX;

  if( widY > widX ) return(widY);
  return(widX);
}

void PursuerMovie::DumpVisPoly(List<MC_MovieObject> *addList,
	MC_ObjectNature nat, VPoly *vpoly)
{
  int i;
  MC_Polygon *ob;
  MC_MovieObject mOb;
  MC_Coord x[MOVIE_MAXPTS], y[MOVIE_MAXPTS];

  /*  printf("Poly edges: %d\n\n", vpoly->nEdges);*/
  for( i = 0; i < vpoly->nEdges; i++ ) {
    x[i] = vpoly->edges[i]->A->x;
    y[i] = vpoly->edges[i]->A->y;
  }

  ob = new MC_Polygon;
  ob->setXY(i, x, y);
  mOb.setObject(ob);
  mOb.nature() = nat;
  addList->add2Bottom(mOb);
}

/* draw the edges of the current visibility polygon. */
void PursuerMovie::DumpGaps(List<MC_MovieObject> *addList, double width,
        MC_ObjectNature dirty, MC_ObjectNature clean, PGNode *node,
			    VPoly *vpoly)
{
  int i, nGaps, tag;
  double x1, x2, y1, y2;
    
  //VPoly *vpoly;
  MC_ObjectNature use;
  Bitmap state;

  nGaps = 0;
  state = node->NodeState();
  //vpoly = node->NodeCell()->vPoly;
  for( i = 0; i < vpoly->nEdges; i++ ) {
    if( vpoly->edges[i]->bGap ) {
      if( state & ( 1 << nGaps )) {
	use = dirty;
	tag = 0;
      } else {
	use = clean;
	tag = TAG2;
      }

      x1 = vpoly->edges[i]->A->x;
      x2 = vpoly->edges[i]->B->x;      y1 = vpoly->edges[i]->A->y;
      y2 = vpoly->edges[i]->B->y;
      DumpLine(addList, width, use, tag, x1, y1, x2, y2);
      nGaps++;
    }
  }
}

void PursuerMovie::DumpLine(List<MC_MovieObject> *addList, double width,
			    MC_ObjectNature nat, int tag,
			    double x1, double y1, double x2, double y2)
{
  MC_Polygon *ob;
  MC_MovieObject mOb;
  MC_Coord x[5], y[5];
  int i;

  x[0] = x1 - width;
  y[0] = y1 - width;
  x[1] = x2 - width;
  y[1] = y2 - width;
  x[2] = x2 + width;
  y[2] = y2 + width;
  x[3] = x1 + width;
  y[3] = y1 + width;
  
  ob = new MC_Polygon;
  ob->setXY(4, x, y);
  mOb.setObject(ob);
  if( tag != 0 ) {
    mOb.tagNumber() = tag;
  }
  mOb.nature() = nat;
  addList->add2Bottom(mOb);
}


/* add the frames illustrating a solution that involved 2 robots */
void MultiMovie::AddFrames(PInfo *pinfo1, PInfo *pinfo2)
{
  MC_Frame *frame; // make array to pass in multiple frames to movie
  List<MC_MovieObject> *animated;
  List<MC_MovieObject> *stationary;
  List<MC_MovieObject> *gList;
  int i, frameCount;
  double width, currdist, dist;
  double r1x, r1y, r2x, r2y;
  Point newP;
  VPoly newVPoly, r1poly;

  MC_Transform *transform1, *transform2;
  List <MC_Transform> *transformList;
  frame = new MC_Frame[pinfo1->nSteps * 20 + pinfo2->nSteps * 20];

  animated = &movingObjects();
  stationary = &staticObjects();

  r1x = pinfo1->sln[0]->NodeCell()->pt->x;
  r1y = pinfo1->sln[0]->NodeCell()->pt->y;
  r2x = pinfo2->sln[0]->NodeCell()->pt->x;
  r2y = pinfo2->sln[0]->NodeCell()->pt->y;

  frameCount = 0;
  for( i = 0; i < pinfo1->nSteps; i++ ) {

    // new: interpolate between cell points in steps of size resolution.
    if( i < pinfo1->nSteps - 1 && !bFast && !bDrawGaps) {
      dist = LineDistance(pinfo1->sln[i]->NodeCell()->pt,
			  pinfo1->sln[i + 1]->NodeCell()->pt);
    } else {
      dist = 0.1; // force one step only
    }

    for( currdist = 0; currdist < dist; currdist += resolution ) {

      if( i < pinfo1->nSteps - 1 && !bFast && !bDrawGaps ) {
	newP.x = pinfo1->sln[i]->NodeCell()->pt->x + currdist/dist *
	  ( pinfo1->sln[i + 1]->NodeCell()->pt->x - 
	    pinfo1->sln[i]->NodeCell()->pt->x );
	newP.y = pinfo1->sln[i]->NodeCell()->pt->y + currdist/dist *
	  ( pinfo1->sln[i + 1]->NodeCell()->pt->y -
	    pinfo1->sln[i]->NodeCell()->pt->y );
	newP.w = 1;
	assert(MakeVisPoly(pinfo1->edgeInfo, &newP, &newVPoly) );
      } else {
	newP.x = pinfo1->sln[i]->NodeCell()->pt->x;
	newP.y = pinfo1->sln[i]->NodeCell()->pt->y;
	newVPoly = *pinfo1->sln[i]->NodeCell()->vPoly;
      }

      gList = new List<MC_MovieObject>;
      if( bDrawGaps ) {
	width = CalculateWidth(pinfo1->wInfo->border) * BORDER_WIDTH;
	DumpGaps(gList, width, MC_Goal, MC_Landmark, pinfo1->sln[i], 
		 &newVPoly);
      } else {
	DumpVisPoly(gList, MC_Goal, &newVPoly);
      }
      /* now mark the cell center so we can see where we start from */
      
      // use robot moving object
      transformList = new List<MC_Transform>;

      transform1 = new MC_Transform;
      transform1->shift() = MC_Point(newP.x - r1x, newP.y - r1y, 1);
      transformList->add2Top(*transform1);

      transform2 = new MC_Transform;
      transformList->add2Top(*transform2);
      
      frame[frameCount++].setFrame(stationary,
				   animated, *transformList, *gList);
    }
  }

  r1poly = newVPoly;

  /* now do the same thing for the second robot with the first stationary */
  for( i = 0; i < pinfo2->nSteps; i++ ) {

    // new: interpolate between cell points in steps of size resolution.
    if( i < pinfo2->nSteps - 1 && !bFast && !bDrawGaps) {
      dist = LineDistance(pinfo2->sln[i]->NodeCell()->pt,
			  pinfo2->sln[i + 1]->NodeCell()->pt);
    } else {
      dist = 0.1; // force one step only
    }

    for( currdist = 0; currdist < dist; currdist += resolution ) {

      if( i < pinfo2->nSteps - 1 && !bFast && !bDrawGaps ) {
	newP.x = pinfo2->sln[i]->NodeCell()->pt->x + currdist/dist *
	  ( pinfo2->sln[i + 1]->NodeCell()->pt->x - 
	    pinfo2->sln[i]->NodeCell()->pt->x );
	newP.y = pinfo2->sln[i]->NodeCell()->pt->y + currdist/dist *
	  ( pinfo2->sln[i + 1]->NodeCell()->pt->y -
	    pinfo2->sln[i]->NodeCell()->pt->y );
	newP.w = 1;
	assert(MakeVisPoly(pinfo2->edgeInfo, &newP, &newVPoly) );
      } else {
	newP.x = pinfo2->sln[i]->NodeCell()->pt->x;
	newP.y = pinfo2->sln[i]->NodeCell()->pt->y;
	newVPoly = *pinfo2->sln[i]->NodeCell()->vPoly;
      }

      gList = new List<MC_MovieObject>;
      if( bDrawGaps ) {
	width = CalculateWidth(pinfo2->wInfo->border) * BORDER_WIDTH;
	DumpGaps(gList, width, MC_Goal, MC_Landmark, pinfo2->sln[i], 
		 &newVPoly);
      } else {
	DumpVisPoly(gList, MC_Goal, &newVPoly);
	DumpVisPoly(gList, MC_Goal, &r1poly);
      }
      /* now mark the cell center so we can see where we start from */
      
      // use robot moving object
      transformList = new List<MC_Transform>;

      transform1 = new MC_Transform;
      transformList->add2Top(*transform1);

      transform2 = new MC_Transform;
      transform2->shift() = MC_Point(newP.x - r2x, newP.y - r2y, 1);
      transformList->add2Top(*transform2);
      
      frame[frameCount++].setFrame(stationary,
				   animated, *transformList, *gList);
    }
  }

  setFrames(frameCount, frame);
  assert(checkIntegrity(1));
}