//
// February 23, 2001
// Copyright (C) 2001 by Wesley H. Huang.  All rights reserved.
//
#include "cgal.h"

// when creating the robot, you can specify the standard deviation
// (sigma) of the gaussian distribution used to generate the random
// error.
//
// The dExecSigmaFrac and aExecSigmaFrac parameters are for the errors
// in execution of motion commands.  For example, although you may
// tell the robot to move forward a distance d, it will not move
// exactly that distance.  A sample from a gaussian random variable is
// added to your motion command in order to determine the actual
// distance.  The standard deviation of this gaussian random variable
// is d * dExecSigmaFrac.
//
// The dSigmaFrac and aSigmaFrac parameters are for the error in dead
// reckoning.  A sample from a gaussian random variable is added to
// the true position to determine the dead reckoned position.  The
// standard deviation of this gaussian is d * dSigmaFrac.

class Robot {
private:
  PointC2 realPos;
  float realAng; // stored in degrees

  float distSigmaFrac, angSigmaFrac;
  float distExecSigmaFrac, angExecSigmaFrac;
  float laserSigmaFrac;

  
  list<Polygon*> obstacles;
  float maxWorldDim;		// used in simulating the laser rangefinder
  void readWorld(char* fileName);

public:
  Robot(char *problemFile);

  void getRealPos(PointC2* p, float* th) const {
    *p = realPos; *th = realAng;
  }

  float forward(float d);
  float turn(float ang);

  // given the angle to point the laser, returns the distance from
  // simulating the laser rangefinder from your true position.
  float laser(float ang);

  // 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 laserModel(const PointC2& pos, float theta, float ang,
		  float *z, float* wx, float *wy);
};