#ifndef SIMULATOR_H
#define SIMULATOR_H

#include "world.h"
#include "cgaldefs.h"
#include <vector>

////////////////////////////////////////////////////////////////
//
// a single command.  In a "move" command, "ang" represents the angle
// for a turn.  In a "sense" command, "ang" represents the direction
// the range sensor.  In both cases, "ang" is relative to the robot's
// heading.
// 
class Command {
public:
  enum CommandType {move, sense};
  CommandType type;
  double dist, ang; // "ang" is in radians
};

////////////////////////////////////////////////////////////////
//
// a class to manage the list of commands for the robot
// 
class CommandList {
  vector<const Command *> Cmds;
  vector<const Command *>::iterator current;

public:
  // read command file and set current command to be the first
  CommandList(const char* fname, bool useSensing, bool verbose = false);

  // methods to access commands by index (zero indexed)
  int numCommands() const { return Cmds.size();}
  const Command* getCommand(int i) const { return Cmds[i];}

  // methods to access commands sequentially
  const Command* getCommand() const { 
    if (current == Cmds.end())
      return 0;
    else 
      return *current;
  }
  const Command* nextCommand() { return *(current++);}
  const Command* prevCommand() { return *(current--);}
  void restartCommands() { current = Cmds.begin();}
};
  

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

class SimRobot : public Robot {
    
  double trueX, trueY, trueAng;
  double odoDist, odoAng;

  World *w;

public:
  double commandDstSigmaRatio;
  double commandAngSigmaRatio;
  double odometryDstSigmaRatio;
  double odometryAngSigmaRatio;
  double rangeSigmaRatio;

  SimRobot(const char *robotFile, double Xpos, double Ypos, double startAng,
	   World *W, bool verbose=false);

  void move(double dist, double ang);  // commanded motion
  void getOdometry(double *dist, double *ang);  // measured motion

  // this is only so that you can plot the point on the screen to see
  // how far off your estimate is.  
  void getTruePosition(double *x, double *y, double *ang);

  double sonar(double th, double ringRadius, double beamWidth,
	       bool verbose=false);

};

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

class Simulator {

 public:
  string problemName;
  World *w;
  SimRobot *r;
  CommandList *c;
  
  Point startPos;
  double startAng; // in radians

  int numSonars;
  double beamWidth;
  double ringRadius;
  double *sonarOrientations;

  // read in the problem description file
  Simulator(const char* fname, bool verbose = false);

  // compute a sonar scan
  void sonarScan(double *readings, bool verbose=false);
};

#endif
