//
// February 23, 2001
// Copyright (C) 2001 by Wesley H. Huang.  All rights reserved.
//
// This file contains routines that implement the robot class which
// simulates a mobile robot.

#include <math.h>
#include <fstream>
#include "robot.h"
#include "rand.h"

#include <stdio.h>
#define sqr(x) ((x)*(x))
#define approx(a,b) (fabs((a)-(b)) < 1e-6)

//#define IM_USING_STUPID_WINDOWS

bool readPolygon(ifstream& inpFile, char** polygonName, Polygon** p);
bool laserIntersection(const PointC2& pos, const Segment& beam, 
		       const Polygon& p, float *dist, Segment *s);
bool stupidWindowsIntersect(const Segment& edge, const Segment& beam, float *d);

// maximum length for character strings for filenames, polygon names, etc.
const int MAXLEN = 100;


// move the robot forward a distance d along the current heading
//
float Robot::forward(float d) 
{
  float actDist = gaussian(d, d*distExecSigmaFrac);
  float drDist = gaussian(actDist, d*distSigmaFrac);

  realPos = realPos + VectorC2(actDist*cos(realAng*M_PI/180.0), 
		      actDist*sin(realAng*M_PI/180.0));

  return drDist;
}

// turn the robot an angle ang (in degrees)
//
float Robot::turn(float ang)
{
  float actAng = gaussian(ang, ang*angExecSigmaFrac);
  float drAng = gaussian(actAng, ang*angSigmaFrac);

  realAng += actAng;

  return drAng;
}

Robot::Robot(char *problemFile) 
{
  char line[MAXLEN];
  char worldfile[MAXLEN];
  long randomSeed;
  float x,y;

  ifstream infile(problemFile);
  infile>>line;
  while(!infile.eof()) {
      if(!strcmp("seed=",line)) {
	infile>>randomSeed;
	myRandSeed(randomSeed);
      } 
      if(!strcmp("world=",line)) {
	infile>>worldfile;
	readWorld(worldfile);
      } 
      else if(!strcmp("start=",line)) {
	infile>>x;
	infile>>y;
	infile>>realAng;
	realPos = PointC2(x,y);
      }
      if(!strcmp("dSigmaFrac=",line)) {
	infile>>distSigmaFrac;
      } 
      if(!strcmp("aSigmaFrac=",line)) {
	infile>>angSigmaFrac;
      } 
      if(!strcmp("dExecSigmaFrac=",line)) {
	infile>>distExecSigmaFrac;
      } 
      if(!strcmp("aExecSigmaFrac=",line)) {
	infile>>angExecSigmaFrac;
      } 
      if(!strcmp("lSigmaFrac=",line)) {
	infile>>laserSigmaFrac;
      } 
      infile>>line;
    }
}

// returns true if a polygon has successfully been read.
// it creates storage for the polygon name and list of points.
bool readPolygon(ifstream& inpFile, char** polygonName, Polygon** p) 
{
  *p = new Polygon();
  *polygonName = new char[MAXLEN];

  char lineBuffer[MAXLEN];
  char *tokenA;
  char *tokenB;

  //this works for the sample file... not very robust but it does the trick
  while (true) {
    inpFile.getline(lineBuffer, MAXLEN);
    if (inpFile.eof())
      return false;

    //we break up each line into 1 or 2 pieces
    //we look at the pieces to figure out what the line is doing
    float x=0,y=0;
    tokenA=strtok(lineBuffer," ");
    if(tokenA) {
      tokenB=strtok(NULL," ");
    } 
    if(!tokenA) {

    } else if(tokenA[0]=='#') {
      //comment
    } else if(tokenA[0]=='}') {
      //we are done with this object
      break;
    } else if(tokenB[0]=='{') {
      //get the name from tokenA
      strcpy(*polygonName,tokenA);
    } else {
      //get the x and y coordinates
      x=atoi(tokenA);
      y=atoi(tokenB);
      //push onto pointlist
      (**p).push_back(PointC2(x,y));
    }
  }

  cout << "The polygon "<< *polygonName <<" has the points:\n";
  for (VertexIterator::iterator k = (**p).vertices_begin(); 
       k != (**p).vertices_end(); k++) {
    cout << "  (" << (*k).x() << ", " << (*k).y() << ")\n";
  }
  return true;
}


void Robot::readWorld(char* fileName)
{
  char* polygonName;
  Polygon* poly;
  int k = 0;

  ifstream worldFile(fileName);

  if (worldFile == NULL) {
    cout << "Error opening the world file: " << fileName << "\n";
    exit(-1);
  }

  cout << "\nReading the world boundary...\n";
  if (!readPolygon( worldFile, &polygonName, &poly)) {
    // error reading the robot
    cout << "Error reading the boundary from the world file: " 
	 << fileName << "\n";
    exit(-1);
  }
  obstacles.push_back(poly);	// add the world boundary as an obstacle
  delete polygonName;		// throw away the name
  Bbox worldBbox = poly->bbox();
  float xDim = worldBbox.xmax() - worldBbox.xmin();
  float yDim = worldBbox.ymax() - worldBbox.ymin();
  maxWorldDim = xDim > yDim ? xDim : yDim;

  cout << "\nReading the obstacles...\n";
  while (readPolygon( worldFile, &polygonName, &poly)) {
    obstacles.push_back(poly);	// add the obstacle
    delete polygonName;		// throw away the name
    k++;
  }

  cout << "\nRead " << k << " obstacles from the world file: "
       << fileName << "\n";
}

// This function simulates the laser rangefinder.  It does so simply
// by calling the laserModel function with the real position and
// orientation and then adding some noise to the returned reading.
float Robot::laser(float ang)
{
  float z, wx, wy;

  laserModel(realPos, realAng, ang, &z, &wx, &wy);

  return gaussian(z, laserSigmaFrac * z);
}

// This function is your model of the laser rangfinder which you will
// use for the kalman filtering.
// 
// given a position and orientation (i.e. your state estimate) and the
// angle to point the laser rangefinder, this function returns the
// distance according to your map and the slope of the wall where the
// laser beam hits.
void Robot::laserModel(const PointC2& pos, float theta, float ang,
		       float *z, float* wx, float *wy)
{
  Segment laserBeam(pos, 
		    pos + VectorC2(maxWorldDim*cos((theta+ang)*M_PI/180.0),
				   maxWorldDim*sin((theta+ang)*M_PI/180.0)));

  Segment closestSegment;
  float closestDist = 2 * maxWorldDim;
  float d;

  for (list<Polygon*>::iterator p = obstacles.begin();
       p != obstacles.end(); p++) {
    Segment s;

    if (laserIntersection(pos, laserBeam, **p, &d, &s))
      if (d < closestDist) {
	closestDist = d;
	closestSegment = s;
      }
  }

  *z = d;
  *wx = closestSegment.end().x() - closestSegment.start().x();
  *wy = closestSegment.end().y() - closestSegment.start().y();
  float mag = sqrt(sqr(*wx)+sqr(*wy));
  *wx /= mag;
  *wy /= mag;
}


bool laserIntersection(const PointC2& pos, const Segment& beam, 
		       const Polygon& p, float *dist, Segment *s)
{
  bool intersected = false;
  float minDist;

  for (EdgeIterator ed = p.edges_begin(); ed != p.edges_end(); ++ed) {
    float d;
    bool didIntersect = false;
#ifndef IM_USING_STUPID_WINDOWS
    Object result = CGAL::intersection(*ed, beam);
    PointC2 pt;
    if (CGAL::assign(pt, result)) {
      d = sqrt(sqr(pt.x() - pos.x()) + sqr(pt.y() - pos.y()));
      didIntersect = true;
    }
#else
    didIntersect = stupidWindowsIntersect(*ed, beam, &d);
#endif
    
    if (didIntersect) {
      if (!intersected) {
	intersected = true;
	minDist = d;
	*s = *ed;
      }
      else if (d < minDist) {
	minDist = d;
	*s = *ed;
      }
    }
  }	
  if (intersected)
    *dist = minDist;
  return intersected;
}

// do a line segment intersection for stupid windows
// return true if there is an intersection and also the distance d
bool stupidWindowsIntersect(const Segment& edge, const Segment& beam, float *d)
{
  float edgeDx = edge.end().x() - edge.start().x();
  float edgeDy = edge.end().y() - edge.start().y();
  float beamDx = beam.end().x() - beam.start().x();
  float beamDy = beam.end().y() - beam.start().y();
  
  // if the segments are parallel, we proclaim there is no intersection
  float det = beamDx*edgeDy - edgeDx*beamDy;
  if (approx(det, 0.0))
    return false;

  float t = (edgeDx*(beam.start().y() - edge.start().y())
	     - edgeDy*(beam.start().x() - edge.start().x())) / det;
  float s = (beamDx*(edge.start().y() - beam.start().y()) 
	     - beamDy*(edge.start().x() - beam.start().x())) / - det;

  if (t > 0 && t < 1.0 && s >= 0 && s <= 1.0) {
    *d = t * sqrt(sqr(beamDx)+sqr(beamDy));
//      printf("Intersected (%.0f, %.0f)-(%.0f, %.0f)\n",
//  	   beam.start().x(), beam.start().y(), beam.end().x(), beam.end().y());
//      printf("with (%.0f, %.0f)-(%.0f, %.0f)",
//  	   edge.start().x(), edge.start().y(), edge.end().x(), edge.end().y());
//      printf(" t=%f d=%f\n",
//  	   t,*d);
    return true;
  }
  return false;
}