////////////////////////////////////////////////////////////////
//
// simulator.cc
// Copyright (c) 2002 by Wesley H. Huang.  All rights reserved.
// 
////////////////////////////////////////////////////////////////
#include "simulator.h"
#include "utilities.h"
#include "cgaldefs.h"
#include <iostream>
#include <fstream>
#include <string>
#include <math.h>

// for laser rangefinder
#include <CGAL/Triangle_2_Segment_2_intersection.h>
#include <CGAL/Segment_2_Segment_2_intersection.h>

Simulator::Simulator(const char* fname, bool verbose)
{
  const int bsize = 100;
  char buf[bsize];
  string tmp;
  double x, y;

  // save current floating point output precision and set it to 2 
  int defPrecision = cout.precision(2);

  if (verbose) cout << endl << "Opening file \"" << fname << "\"..." << endl;
  std::ifstream inpFile(fname);

  if (verbose) cout << "Looking for the problem name..." << endl;
  skipComments(inpFile);
  inpFile.getline(buf,bsize-1);
  problemName = string(buf);
  if (verbose) cout << "This is \"" << problemName << "\"" << endl;
  
  string worldFile = getWord("World:", "world file", inpFile, verbose);
  string robotFile = getWord("Robot:", "robot file", inpFile, verbose);

  if (verbose) cout << "Looking for start position..." << endl;
  skipComments(inpFile);
  inpFile >> tmp;
  if (tmp != "start:") {
    cerr << "Didn't find the start position; " <<
      "Instead found \"" << tmp << "\"." << endl;
    exit(1);
  }
  inpFile >> x >> y >> startAng;
  if (verbose) cout << "Start position is (" << x << ", " << y 
		    << "), orientation of " << startAng << " degrees." << endl;
  startPos = Point(x,y);
  startAng *= M_PI/180.0; // convert to radians
  
  getNumbers("NoSonars:", "number of sonars", inpFile, 
	     1, &numSonars, verbose);

  getNumbers("BeamWidth:", "sonar beam width (in degrees)", inpFile, 
	     1, &beamWidth, verbose);
  beamWidth *= M_PI/180.0;  // convert to radians

  getNumbers("SonarRingRadius:", "sonar ring radius", inpFile, 
	     1, &ringRadius, verbose);

  sonarOrientations = new double[numSonars];
  getNumbers("SonarAngles:", "sonar orientations", inpFile,
	     numSonars, sonarOrientations,verbose);
  // convert to radians
  for (int k=0; k < numSonars; k++)
    sonarOrientations[k] *= M_PI/180.0;


  string CommandFile = getWord("Commands:", "command file", inpFile, verbose);

  // reset floating point output precision
  cout.precision(defPrecision);

  // read in the world and robot definitions from files
  w = new World(worldFile.c_str(), verbose);
  r = new SimRobot(robotFile.c_str(), 
		   startPos.x(), startPos.y(), startAng,
		   w, verbose);
  c = new CommandList(CommandFile.c_str(), true, verbose);
}


////////////////////////////////////////////////////////////////

CommandList::CommandList(const char* fname, bool useSensing, 
			 bool verbose)
{
  if (verbose) cout << endl << "Opening file \"" << fname << "\"..." << endl;
  std::ifstream inpFile(fname);

  if (verbose) cout << "Reading commands..." << endl;
  while (true) {
    string n;
    Command *c = new Command();

    skipComments(inpFile);
    if (inpFile.eof()) break;
    inpFile >> n;
    if (n == "move") {
      inpFile >> c->dist >> c->ang;
      if (verbose) cout << "\"Move\" command: distance=" << c->dist 
			<< ", angle=" << c->ang << endl;
      c->ang *= M_PI/180.0;
      c->type = Command::move;
      Cmds.push_back(c);
    }
    else if (n == "sense") {
      if (useSensing) {
	if (verbose) cout << "\"Sense\" command: angle=" << c->ang << endl;
	c->type = Command::sense;
	Cmds.push_back(c);
      }
      else {
	if (verbose) cout << "Ignoring \"sense\" command." << endl;
	delete c;
      }
    }
    else {
      cerr << "Unrecognized command: " << n << endl;
      exit(1);
    }
  }
  if (verbose) cout << "End of file reached." << endl;

  current = Cmds.begin();
}
  
////////////////////////////////////////////////////////////////
  



SimRobot::SimRobot(const char *robotFile, 
		   double Xpos, double Ypos, double startAng,
		   World *W, bool verbose)
  :   Robot(robotFile, verbose, Xpos, Ypos, startAng)
{
  w = W;  // store the pointer to the world
  trueX = Xpos;
  trueY = Ypos;
  trueAng = startAng;
}

void SimRobot::move(double dist, double ang)
{
  // simulate the movement
  trueX += dist*cos(trueAng);
  trueY += dist*sin(trueAng);
  trueAng += ang;

}

void SimRobot::getTruePosition(double *x, double *y, double *ang)
{
  *x = trueX;
  *y = trueY;
  *ang = trueAng;
}

inline double dist(const Point& p, const Point& q) 
{
  double dx = p.x() - q.x();
  double dy = p.y() - q.y();

  return sqrt(dx*dx + dy*dy);
}


////////////////////////////////////////////////////////////////

void updateRange(double contender, bool *init, double *min);
bool obstacleRange(const Polygon& pg, const Triangle& T, double *r,
		   bool verbose=false);
bool noIntersections(const Point& src, const Point& tgt, bool verbose=false);

void Simulator::sonarScan(double *readings, bool verbose)
{
  for (int k=0; k<numSonars; k++) {
    if (verbose) cout << "Simulating SONAR #" << k << endl;
    readings[k] = r->sonar(sonarOrientations[k],
			   ringRadius,
			   beamWidth, verbose);
  }
}

double SimRobot::sonar(double th, double ringRadius, double beamWidth,
		       bool verbose)
{
  const double xRange = w->maxX() - w->minX();
  const double yRange = w->maxY() - w->minY();
  const double largeDistance = xRange > yRange ? xRange*2 : yRange*2;

  // center of the sonar cone
  const double xc = trueX + ringRadius*cos(trueAng+th);
  const double yc = trueY + ringRadius*sin(trueAng+th);
  // left corner of the sonar triangle/arc
  const double xl = xc + largeDistance*cos(trueAng+th+beamWidth/2.0);
  const double yl = yc + largeDistance*sin(trueAng+th+beamWidth/2.0);
  // right corner of the sonar triangle/arc
  const double xr = xc + largeDistance*cos(trueAng+th-beamWidth/2.0);
  const double yr = yc + largeDistance*sin(trueAng+th-beamWidth/2.0);

  Triangle T(Point(xc,yc), Point(xr,yr), Point(xl,yl));

  double range = 10;  // maximum range is 10 meters
  const Polygon& WB = w->boundary->getShape();
  double r;
  if (verbose) cout << "Checking world boundary" << endl;
  if (obstacleRange(WB, T, &r, verbose)) {
    if (r < range)
      range = r;
  }
  for (list<Obstacle*>::iterator ob = w->obstacles.begin();
       ob != w->obstacles.end(); ob++) {
    if (verbose) cout << "Checking obstacle " << (*ob)->name << endl;
    if (obstacleRange((*ob)->getShape(), T, &r, verbose)) {
      if (r < range)
	range = r;
    }
  }

  return range;
}


// return the minimum sonar range to any edge of the polygon
bool obstacleRange(const Polygon& pg, const Triangle& T, double *r,
		   bool verbose)
{
  bool rangeInit = false;
  
  Polygon::Vertex_const_circulator p1, p2, p3;
  p1 = pg.vertices_circulator();
  p2 = pg.vertices_circulator(); p2++;
  p3 = pg.vertices_circulator(); p3++; p3++;

  for (int k=0; k < pg.size(); k++, p1++, p2++, p3++) {
    if (verbose) cout << "Examining edge " << k << " = " 
		      << *p1 << " to " << *p2 << endl;
    // If p2 is inside the cone, it must be part of a corner (interior
    // or exterior).  Check to see whether the corner is visible.
    if (T.bounded_side(*p2) == CGAL::ON_BOUNDED_SIDE) {
      if (verbose) cout << "  p2 is inside the sonar cone" << endl;
      Vector v = *p2 - T.vertex(0);
      Vector t1 = *p1 - *p2;
      Vector t3 = *p3 - *p2;
      if (verbose) cout << "  the vectors are v=" << v << " t1=" << t1
			<< " t3=" << t3 << endl;
      if (verbose) cout << "  the dot products are: " << v*t1 << " and "
			<< v*t3 << endl;
      // both dot products are pos or neg and the line to p2 isn't obstructed
      if ((v*t1)*(v*t3) > 0
	  && noIntersections(T.vertex(0), *p2, false)) { 
	double range = sqrt(v*v);
	if (verbose) cout << "  Range to this vertex is " << range << endl;
	updateRange(range, &rangeInit, r);
      }
      else
	if (verbose) cout << "   not visible or corner has wrong orientation"
			  << endl;
    }
    // Otherwise, check to see whether there's an intersection between
    // the cone and the segment from p1 to p2 (and whether it's
    // visible).
    CGAL::Object intResult = intersection(T, Segment(*p1,*p2));
    Segment sg;
    if (assign(sg, intResult)) {
      if (verbose) cout << "  There is an intersection: " << sg << endl;
      // If the angle of incidence at each end has a different sign,
      // then there must be some point where the ray from the sonar is
      // normal to the line.
      Vector v1 = sg.source() - T.vertex(0);
      Vector v2 = sg.target() - T.vertex(0);
      Vector vS = sg.target() - sg.source();
      float a1 = v1*vS;
      float a2 = v2*vS;
      if (a1*a2 < 0) {
	if (verbose) cout << "  the cone is normal somewhere; searching..."
			  << endl;
	// use bisection to find the normal point on the segment
	// the point is bracketed by the endpoint of v1 and v2
	Vector l = v2 - v1;
	Vector v = v1 + 0.5*l;
	if (verbose) cout << "  trying point " << (CGAL::ORIGIN + v) << endl;
	const float tolerance = 0.01; // bracket normal point to 1cm
	while (l*l > tolerance*tolerance) {
	  float a = v*vS;
	  if (a*a1 > 0)
	    v1 = v;
	  else 
	    v2 = v;
	  l = v2 - v1;
	  v = v1 + 0.5*l;
	  if (verbose) cout << "  trying point " << (CGAL::ORIGIN + v) << endl;
	}
	if (noIntersections(T.vertex(0), T.vertex(0)+v)) {
	  double range = sqrt(v*v);
	  if (verbose) cout << "  Range to this vertex is " << range << endl;
	  updateRange(range, &rangeInit, r);
	}
      }
    }
  }
  return rangeInit;
}	

extern Simulator *P;

// returns true if the vector doesn't intersect any obstacles or the
// world boundary
bool noIntersections(const Point& src, const Point& tgt, bool verbose)
{
  Segment s(src,tgt);
  if (verbose) cout << "    testing visibility of segment " << s << endl;
  if (verbose) cout << "    checking world boundary for intersection" << endl;
  for (Polygon::Edge_const_iterator ei 
	 = P->w->boundary->getShape().edges_begin();
       ei != P->w->boundary->getShape().edges_end(); ei++) {
    if (verbose) cout << "      trying edge " << *ei << endl;
    CGAL::Object intResult = intersection(s, *ei);
    Point pt;
    if (assign(pt, intResult)) {
      if (verbose) cout << "    possible intersection; checking..." << endl;
      Vector r = pt - src;
      if (r*r < 0.9999*s.squared_length()) {
	if (verbose) cout << "    There is a real intersection between "
			  << s << " and " << *ei << endl;
	return false;
      }
    }
  }
  for (list<Obstacle*>::iterator ob = P->w->obstacles.begin();
       ob != P->w->obstacles.end(); ob++) {
    if (verbose) cout << "    checking obstacle " << (*ob)->name
		      << " for intersection" << endl;
    for (Polygon::Edge_const_iterator ei = (*ob)->getShape().edges_begin(); 
	 ei != (*ob)->getShape().edges_end(); ++ei) {
      CGAL::Object intResult = intersection(s, *ei);
      Point pt;
      if (assign(pt, intResult)) {
	Vector r = pt - src;
	if (r*r < 0.9999*s.squared_length()) {
	  if (verbose) cout << "There is an intersection between "
			    << s << " and " << *ei << endl;
	  return false;
	}
      }
    }
  }
  return true;
}


  

//
void updateRange(double contender, bool *init, double *min)
{
  if (*init) {
    if (contender < *min)
      *min = contender;
  }
  else {
    *min = contender;
    *init= true;
  }
}


