#include <iostream>
#include <list>


#include "graphics.h"
#include "drawable.h"
#include "utilities.h"
#include "world.h"
#include "simulator.h"
#include <math.h> // for M_PI

// some globals for use in multiple routines
//
Simulator *P;
list<drawableObject*> wbr;  // world boundary, robot, and robot path
Path *path;
Path *scan = 0;

// storage for sonar readings
double *readings;

bool verbosity = false;

void printHelp();
void myKeyboardHandler(unsigned char key);
void executeRobotCommand();


int main(int argc, char *argv[])
{
  // make sure there are enough arguments
  if (argc < 2) {
    cerr << "Usage: " << argv[0] << " [GLUT options] <problem filename>" <<
      endl;
    exit(1);
  }

  // read in the problem (this must be done before graphics initialization)
  P = new Simulator(argv[argc - 1], false);
  readings = new double[P->numSonars];

  // add the robot's initial location to the path drawn on the screen
  path = new Path;
  path->pts.push_back(P->startPos);

  // initialize the graphics
  const double marginFrac = 0.1;
  double Xmargin = marginFrac * (P->w->maxX() - P->w->minX());
  double Ymargin = marginFrac * (P->w->maxY() - P->w->minY());
  Graphics::init(&argc, argv, 
		 P->w->minX() - Xmargin, P->w->maxX() + Xmargin,
		 P->w->minY() - Ymargin, P->w->maxY() + Ymargin);
  Graphics::addBasicColors();
  //Graphics::verbose=true;

  // add the world boundary and the robot to a list of lists of
  // objects to be drawn in the graphics window
  wbr.push_back(P->w->boundary);
  wbr.push_back(path);
  wbr.push_back(P->r);
  Graphics::objectLists.push_back(&wbr);
  // draw the obstacles on the screen
  Graphics::objectLists.push_front((list<drawableObject*>*) &(P->w->obstacles));

  Graphics::setKeyboardHandler(myKeyboardHandler);
  printHelp();
  Graphics::runEventLoop();
  return 0;
}


void printHelp()
{
  cout 
    << "Press: "
    << "'n' to execute the next command." << endl
    << "       'v' to toggle verbosity." << endl
    << "       'q' to exit." << endl;
}

void myKeyboardHandler(unsigned char key)
{
  switch (key) {
    case 'n': executeRobotCommand(); break;
    case 'v': 
      if (verbosity) 
	verbosity = false;
      else
	verbosity = true;
      cout << "Verbosity is now " << verbosity << endl;
      break;
    case 'q': exit(0);
  }
}

void executeRobotCommand()
{
  const Command *c = P->c->getCommand();

  if (c == 0) {
    cout << "Done with robot commands." << endl << endl;
    return;
  }

  static double x, y, th;	// estimate of the robot's configuration

  if (scan != 0) {
    wbr.remove(scan);
    delete scan;
    scan = 0;
  }

  if (c->type == Command::move) {
//      cout << "Commanding the robot to move: distance=" << c->dist
//  	 << ", angle=" << c->ang*180/M_PI << endl;
    P->r->move(c->dist, c->ang);
    // This example shows the robot at its true position.  For your
    // program, you should always draw the robot at the estimated
    // position.
    P->r->getTruePosition(&x, &y, &th); 
    P->r->setConfig(x, y, th);
    path->pts.push_back(Point(x,y));
//      cout << "The robot is now at (x, y, theta) = (" << x << ", " << y << ", "
//  	 << th*180.0/M_PI << ")" << endl;
    cout << "position: " << x << " " << y << " " << th*180/M_PI << endl;
  }
  else if (c->type == Command::sense) {
    scan= new Path;
    scan->setColor("cyan");
    P->sonarScan(readings, verbosity);
    // draw the sonar scan
    for (int k=0; k<P->numSonars; k++) {
      double sth = th + P->sonarOrientations[k];
      double xs = x + P->ringRadius * cos(sth);
      double ys = y + P->ringRadius * sin(sth);
      scan->pts.push_back(Point(xs, ys));
      scan->pts.push_back(Point(xs + readings[k]*cos(sth - P->beamWidth/2.0),
				ys + readings[k]*sin(sth - P->beamWidth/2.0)));
      scan->pts.push_back(Point(xs + readings[k]*cos(sth + P->beamWidth/2.0),
				ys + readings[k]*sin(sth + P->beamWidth/2.0)));
      scan->pts.push_back(Point(xs, ys));
    }
    // add it to the wbr list
    wbr.push_back(scan);
    cout << "sonars:";
    for (int k=0; k<P->numSonars; k++) 
      cout << " " << readings[k];
    cout << endl;
  }
  else 
    cout << "Unrecognized command." << endl;

//    cout << endl;
  P->c->nextCommand();
}

