#include "world.h"
#include <iostream>
#include <fstream>
#include "filereader.h"

using namespace std;
using namespace dolt;

istream& operator>> (istream& in, Obstacle& obst)
{
  in >> obst.name >> *dynamic_cast<Polygon*>(&obst);
  return in;
}
  
World::World(const char* fname, bool verbose)
{
  ifstream f(fname);

  if (!f.is_open()) {
    cerr << "ERROR: could not open the file \"" << fname << "\"" << endl;
    exit(1);
  }

  filereader::skipWhiteComment(f);
  f >> name;
  if (!f.good()) {
    cerr << "ERROR: while reading the world name" << endl;
    exit(1);
  }

  filereader::skipWhiteComment(f);
  boundary = new drawablePolygon(SOLID, false);
  f >> *boundary;
  if (!f.good()) {
    cerr << "ERROR: while reading the world boundary" << endl;
    exit(1);
  }
  boundary->setBorderColor(dolt::Color("mediumblue"));
  boundary->setLineWidth(1.0f);

  Obstacle *obst;
  while (true) {
    filereader::skipWhiteComment(f);
    if (f.eof())
      break;
    obst = new Obstacle();
    f >> *obst;
    obstacles.push_back(obst);
  }
}

Robot::Robot(const char* fname, bool verbose, 	
	     double X, double Y, double Ang)
{
  ifstream f(fname);

  if (!f.is_open()) {
    cerr << "ERROR: could not open the file \"" << fname << "\"" << endl;
    exit(1);
  }

  filereader::skipWhiteComment(f);
  f >> name;
  if (!f.good()) {
    cerr << "ERROR: while reading the robot name" << endl;
    exit(1);
  }

  filereader::skipWhiteComment(f);
  f >> shape;
  if (!f.good()) {
    cerr << "ERROR: while reading the world boundary" << endl;
    exit(1);
  }

  // set the body color of the robot
  setColor( Color("blue") );

  // create the points of the polygon
  for (int k=0; k<shape.size(); k++)
    this->push_back(Point(0.0, 0.0));
  setConfig(X, Y, Ang);
}

void Robot::setConfig(double X, double Y, double Ang)
{
  x=X;
  y=Y;
  ang=Ang;

  // set the points of the drawablePolygon so that the robot displays
  // at the proper position and orientation on the screen
  const double c = cos(ang);
  const double s = sin(ang);
  for (Polygon::Vertex_iterator k = shape.vertices_begin(),
	 j = this->vertices_begin();
       k != shape.vertices_end(); k++, j++) {
    const Point p(x + c*k->x() - s*k->y(), 
		  y + s*k->x() + c*k->y());
    this->set(j, p);
  }
}

void Robot::getConfig(double *X, double *Y, double *Ang)
{
  *X=x;
  *Y=y;
  *Ang=ang;
}

  
		   
