#include "mapproblem.h"
#include <math.h>

MapProblem::MapProblem(const char* fname)
{
  string robotFile, sonarFile;

  const int MAX_PROBLEM_NAME_LEN = 100;
  char pn[MAX_PROBLEM_NAME_LEN];

  filereader::FileReader f;
  f.registerCString("ProblemName", pn, MAX_PROBLEM_NAME_LEN);
  f.registerScalar("WorldBoundary", &boundary);
  f.registerScalar("SonarData", &sonarFile);
  f.registerScalar("Robot", &robotFile);
  f.registerScalar("cellSize", &cellSize);
  
  f.readFile(fname);
  if (!f.complete()) {
    cerr << "ERROR: The following parameters were missing from the file..." 
	 << endl;
    list<string> m = f.missingParameters();
    for (list<string>::iterator k = m.begin();
	 k != m.end();
	 k++) 
      cerr << "  " << *k << endl;
    cerr << endl;
    exit(1);
  }

  problemName = string(pn);

  R = new Robot(robotFile.c_str());
  D = new SonarData(sonarFile.c_str());
//   c = new sonar(commandFile.c_str(), verbose);
}

////////////////////////////////////////////////////////////////
// this is sort of a mess because this old code uses pre-"Filereader" stuff...


// Get a list of numbers (space separated) for a declaration.  The
// pointer "results" must have enough memory to store all the numbers.
//
template<class T> void getNumbers(char *parameter, char *description, 
				  ifstream &inpFile,
				  int numNumbers, T *results,
				  bool verbose=false)
{
  string tmp;
  if (verbose) cout << "Looking for " << description << " declaration..." 
		    << endl;
  skipComments(inpFile);
  if (inpFile.eof())
    cerr << "Hey, we've reached the end of the file!" << endl;
  inpFile >> tmp;
  if (tmp != parameter) {
    cerr << "Didn't find the " << description << " declaration;"  <<
      "Instead found \"" << tmp << "\"." << endl;
    exit(1);
  }
  for (int k = 0; k < numNumbers; k++) {
    inpFile >> results[k];
  }
  if (verbose) {
    cout << "The " << description << " is: ";
    for (int k=0; k<numNumbers; k++) 
      cout << results[k] << " ";
    cout << endl;
  }
}

// Skip over any comments (lines which start with '#') and over any
// whitespace (spaces, tabs, and newlines).
//
void skipComments(std::ifstream& f)
{
  char c;
  while (!f.eof()) {
    f.get(c);
    if (c == '#') {
      // skip over a comment line
      while (c != '\n') {
	if (f.eof()) break;
	f.get(c);
      }
    }
    else if (c == '\n' || c == ' ' || c == '\t' || c == '\r') {
      // keep reading characters if it's whitespace
      continue;
    }
    else {
      // if c is the next real character in the file, put it back!
      f.putback(c);
      break;
    }
  }
}


string getWord(char *parameter, char *description, std::ifstream &inpFile,
	       bool verbose=false)
{
  string tmp;

  if (verbose) cout << "Looking for " << description << " declaration..." 
		    << endl;
  skipComments(inpFile);
  inpFile >> tmp;
  if (tmp != parameter) {
    cerr << "Didn't find the " << description << " declaration;"  <<
      "Instead found \"" << tmp << "\"." << endl;
    exit(1);
  }
  inpFile >> tmp;
  if (verbose) cout << "The " << description << " is \"" << tmp
		    << "\"." << endl;
  return tmp;
}

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

SonarData::SonarData(const char* fname, bool verbose) 
{
  F = new ifstream(fname);

  if (!F->is_open()) {
    cerr << "Error opening file " << fname << endl;
    exit(1);
  }

  getNumbers("NoSonars:", "number of sonars", *F, 1, &_numSonars, verbose);
  
  sonarOrientations = new double[_numSonars];
  getNumbers("SonarAngles:", "sonar orientations", *F,
	     _numSonars, sonarOrientations,verbose);
  // convert to radians
  for (int k=0; k < _numSonars; k++)
    sonarOrientations[k] *= M_PI/180.0;

  getNumbers("SonarRingRadius:", "cell size", *F, 
	     1, &sonarRingRadius, verbose);

  getNumbers("SonarConeWidth:", "sonar cone width", *F,
	     1, &sonarConeWidth, verbose);
  sonarConeWidth *= M_PI/180.0;	// convert to radians

  currentReadings = new double[_numSonars];

  doneWithFile = false;
  next();
}

SonarData::~SonarData()
{ 
  delete[] sonarOrientations; 
  delete[] currentReadings; 
  F->close();
}

// get the next configuration and sonar readings from the file
// returns "true" if the end of the file was reached
bool SonarData::next(bool verbose)
{
  double c[3];

  if (doneWithFile)
    return true;

  if (verbose) cout << "Looking for configuration..." << endl;
  getNumbers("position:", "robot configuration", *F, 3, c);
  x = c[0];
  y = c[1];
  th = c[2]*M_PI/180.0;

  if (verbose) cout << "Looking for sonar readings..." << endl;
  getNumbers("sonars:", "sonar readings", *F, _numSonars, currentReadings);

  skipComments(*F);
  if (F->eof()) 
    doneWithFile = true;
  return false;
}



// returns the current position (in meters) and the orientation (in radians)
void SonarData::getConfig(double *_x, double *_y, double *_th)
{
  *_x = x;
  *_y = y;
  *_th = th;
}
		   
// get the current sonar readings
// "readings" must be an array with room for "numSonars" elements
void SonarData::getReadings(double *readings)
{
  for (int k=0; k<_numSonars; k++) {
    readings[k] = currentReadings[k];
  }
}
