class stMotionManager;
# include "stMETTargetTracker.h"

class stMotionManager
{
public:
  stMotionManager(char *fileName);
 
  virtual int fileLoader(ifstream &inFile); 
  virtual int collisionChecker(const point &inPoint)const;
  virtual int focusToMaxEscTime(const point &targetPt,
				int          numTrials= 10, 
				float thresDist = 3);
  virtual int drawPostScript(ps_file &psOutFile)const;
  virtual int draw(window &win)const;
protected:
  virtual int robotLoader(char *robotFileName);
  virtual int mapLoader(char *mapFileName);

protected:
  polygon borderPoly_;
  list<polygon> lstMapPoly_;
  list<segment> lstMapSeg_;
  
  double  currMinEscDist_;
  segment currClosestSeg_;

  stVisRobotR2S visRobot_;
  stMEDTargetTracker targetTracker_;
};

#include "stMETTargetTracker.C"

stMotionManager :: stMotionManager(char *fileName)
{
   ifstream inFile(fileName);

   lstMapPoly_.clear();
    if(inFile.fail())
      {
        cout<<"stMotionManager file missing = "<<fileName<<endl;
       
        exit(0);
      }
    if(fileLoader(inFile) != 1)
      {
	cout<<" Error Reading data from# SUB # files of "
	    <<fileName<<endl;
	exit(0);
      }
    inFile.close();
    currMinEscDist_ = -INT_MAX;
    
   
}
int stMotionManager :: fileLoader(ifstream &inFile)
{
  char fileName[40];
  char title[40];
  inFile>>title >>fileName;
  if( mapLoader(fileName) != 1)
    return -1;

  inFile>>title >>fileName;
  
  if( robotLoader(fileName) != 1)
    return -2;

  return 1;
 
}
int stMotionManager :: collisionChecker(const point &inPoint)const
{
  polygon poly;
  
  if(!borderPoly_.contains(inPoint))
    return 1;
  
  forall(poly, lstMapPoly_)
    {
      if(poly.contains(inPoint))
	return 1;
    }
  
  return 0;
}

int stMotionManager :: focusToMaxEscTime(const point &targetPt,
					 int          numTrials,
					 float        thresDist)
{
  stCLCamera   bestCam;
  segment      closestSeg;
  double       MaxMinDist;
  

  targetTracker_.getMETConfig(visRobot_.getCamera(),
			      targetPt,
			      *this,
			      lstMapSeg_,
			      bestCam,
			      closestSeg,
			      MaxMinDist,
			      numTrials) ;
   
  if(MaxMinDist > 0)
    {
    
      currMinEscDist_ = MaxMinDist;
      currClosestSeg_ = closestSeg;

      stConfigR2S bestConfig;
      bestCam.getConfig(bestConfig); 
      visRobot_.setConfig(bestConfig);

    }

  return 1;
}











int stMotionManager :: robotLoader(char *robotFileName)
{
  ifstream inFile(robotFileName);

  if(inFile.fail())
    return -1;
  if(visRobot_.fileLoader(inFile) != 1)
    {
      cout<<robotFileName<<" reading Error \n";
      return -1;
    }
  inFile.close();
  return 1;
}

int stMotionManager :: mapLoader(char *mapFileName)
{

  ifstream inFile(mapFileName);
  if(inFile.fail())
    return -1;

  lstMapPoly_.clear();
  int numPoints;
  char heading[40];

  inFile>>heading;
  cout<<"heading = "<<heading<<endl;
  
  inFile>> numPoints;
  if(numPoints != 4)
    return -1;

  list<point> lstBorderPt;
  for(int i =0; i < numPoints; i++)
    {
      double x,y;
      inFile>>x>>y;
      lstBorderPt.push_back(point(x,y));
      
    }
  borderPoly_ = polygon(lstBorderPt);

  inFile>> numPoints;
  while(!inFile.eof()){
    
    list<point> lstPolyPt;
    for(int i =0; i < numPoints; i++)
      {
	double x,y;
	inFile>>x>>y;
	lstPolyPt.push_back(point(x,y));
	
      }
    
    lstMapPoly_.push_back(polygon(lstPolyPt));
    inFile>> numPoints;
  }

  cout<<"Border \n"<<borderPoly_<<endl;
  lstMapPoly_.print(" list of Plygons \n");
  inFile.close();

  /* Cretaes the lis of segments from the Map polygons */
  lstMapSeg_.clear();
  lstMapSeg_ = borderPoly_.edges();
  segment mapSeg;
  polygon mapPoly;
  forall(mapPoly, lstMapPoly_)
    {
      list<segment> lstSeg = mapPoly.edges();
      lstMapSeg_.conc(lstSeg);
    }
 
  lstMapSeg_.print("\n list of map Segmenst \n");
  return 1;
}
 





int stMotionManager ::  draw(window &win)const
{ 
  list<segment> lstVisSeg   ,lstGapSeg;
  list<point>   lstVisPolyPt;
 
  visRobot_.getCamera().visibilityPolygon( lstMapSeg_, 
					   lstVisSeg,
					   lstGapSeg,
					   lstVisPolyPt);
  
  polygon mapPoly;
  forall(mapPoly, lstMapPoly_)
    {
      win.draw_polygon(mapPoly,red);
    }
  win.draw_polygon(borderPoly_,black);
  win.draw_polygon(visRobot_.getCamera().viewPolygon(), cyan);
  
  
  

  segment vSeg;
  forall(vSeg,lstVisSeg)
    {
      win.draw_arrow(vSeg,violet);
    }
  forall(vSeg,lstGapSeg)
    {
      win.draw_arrow(vSeg,pink);
    }
  
  
  win.draw_text_node(visRobot_.center(),"R",white);
  win.draw_segment(currClosestSeg_, color(150,50,10));
  win.draw_text(currClosestSeg_.source(),"s",blue);
  win.draw_text(currClosestSeg_.target(),"t",blue);
  
 
  return 1;
}


int stMotionManager :: drawPostScript(ps_file &psOutFile)const
{ 
list<segment> lstVisSeg   ,lstGapSeg;
  list<point>   lstVisPolyPt;
 
  visRobot_.getCamera().visibilityPolygon( lstMapSeg_, 
					   lstVisSeg,
					   lstGapSeg,
					   lstVisPolyPt);
  
  polygon mapPoly;
  forall(mapPoly, lstMapPoly_)
    {
      psOutFile.draw_polygon(mapPoly,red);
    }
  psOutFile.draw_polygon(borderPoly_,black);
  psOutFile.draw_polygon(visRobot_.getCamera().viewPolygon(), cyan);
  
  
  

  segment vSeg;
  forall(vSeg,lstVisSeg)
    {
      psOutFile.draw_arrow(vSeg,violet);
    }
  forall(vSeg,lstGapSeg)
    {
      psOutFile.draw_arrow(vSeg,pink);
    }
  
  
  psOutFile.draw_text_node(visRobot_.center(),"R",white);
  psOutFile.draw_segment(currClosestSeg_, color(150,50,10));
  psOutFile.draw_text(currClosestSeg_.source(),"s",blue);
  psOutFile.draw_text(currClosestSeg_.target(),"t",blue);
  
 
  return 1;
}