// script.cc

#include <iostream.h>
#include <fstream.h>
#include <strstream.h>
#include <assert.h>
#include <vector.h>
#include <strings.h>

#include "general.hh"
#include "Timer.hh"
#include "Timer.icc"
#include "function.hh"
#include "function.icc"
#include "script.hh"
#include "script.icc"
#include "State.hh"
#include "State.icc"
#include "P_Motion.hh"
#include "P_Motion.icc"
#include "O_ProximityScan.hh"
#include "O_ProximityScan.icc"
#include "O_SonarScan.hh"
#include "O_SonarScan.icc"
#include "O_LaserScan.hh"
#include "O_LaserScan.icc"

State referencePosition;

////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////
//
// Opens the script file and gets initial information.
//
////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////
script::script( string& fileName,
		string& refPosFile,
		bool useSonar = true,
		bool useLaser = false,
		float timeFactor = 1.0,
		float skipDistance = 20.0,
		int skipTime = 0,
		int numberOfLaserScanners = 1,
		int scriptType = SCRIPT_TYPE_NOT_SET) :
  _laser(numberOfLaserScanners * 180),
#ifndef PIONEER_II
  _sonar(24),
#else
  _sonar(8),
#endif  
  _timeFactor(timeFactor),
  _skipDistance( skipDistance),
  _skipTime( skipTime),
  _useSonar( useSonar),
  _useLaser( useLaser),
  _stdin(NULL),
  _realTimeMode(true),
  _referencePositions(0)
{
  Timer currentTime;
  
  if ( fileName == (string) "stdin") {
    _stdin = stdin;
    cerr << "# Get data from stdin.\n";
  }
#ifdef USE_ZLIB
  else if (( _zFile = gzopen( fileName.c_str(), "r")) == NULL) {
    cerr << "Cannot open " << fileName << "\n";
    exit(1);
  }
#else
  else if (( _file = fopen( fileName.c_str(), "r")) == NULL) {
    cerr << "Cannot open " << fileName << "\n";
    exit(1);
  }
#endif
  else
    cerr << "# Get data from " << fileName << ".\n";

  bool timeRead = false;
  char line[maxStringLength];

  _scriptType = scriptType;

  // Try to determine the script type.
  if ( _scriptType == SCRIPT_TYPE_NOT_SET)
    while ( ( _scriptType == SCRIPT_TYPE_NOT_SET) && getNextLine( line)) {
      if (readingLine(line, origScriptOpenMark)){
	_scriptType = OLD_SCRIPT_TYPE;
      }
      else if (readingLine(line, newScriptOpenMark)){
	_scriptType = NEW_SCRIPT_TYPE;
      }
    }
  
  if ( _scriptType == OLD_SCRIPT_TYPE) {
    positionMark = origScriptPosMark;
    timeMark = origScriptTimeMark;
    laserMark = origScriptLaserMark;
    sonarMark = origScriptSonarMark;
    imageMark = origScriptImageMark;
    timeOffset = origScriptTimeOffset;
    cerr << "# Old script" << endl;
  }
  else if ( _scriptType == NEW_SCRIPT_TYPE){
    positionMark = newScriptPosMark;
    timeMark = newScriptTimeMark;
    laserMark = newScriptLaserMark;
    sonarMark = newScriptSonarMark;
    imageMark = newScriptImageMark;
    timeOffset = newScriptTimeOffset;
    cerr << "# New script" << endl;
  }
  else {
    cerr << "# Error: Cannot determine script type.\n";
    exit(0);
  }

  if ( skipTime >= 0) {

    // First look for the start time of the script.
    while ( getNextLine( line) && !timeRead){

      if ( readingLine(line, timeMark)){
	
	_startTime = getTime(line, timeMark, timeOffset);
	
	cout << "# Found script start time: " << _startTime << endl;
	  
	// Now skip the next skipTime seconds.
	if ( skipTime > 0) {
	  
	  Timer timeToBeReached = _startTime;
	  timeToBeReached.secs() += skipTime;
	  
	  while ( getNextLine( line) && !timeRead){
	    if (readingLine(line, timeMark)){
	      currentTime = getTime(line, timeMark, timeOffset);
	      if ( currentTime >= timeToBeReached)
		timeRead = true;
	    }
	  }
	}
	else {
	  timeRead = true;
	  currentTime = _startTime;
	}	
      }
    }
    
    assert( timeRead);
  }
  
  // Initialize the start time of the script and the real time of day.
  _realTime.reset();

  _elapsedTimeSinceLastMeasurement = currentTime - _startTime;
  _elapsedTime = currentTime - _startTime;
  _previousTime = currentTime;

  readReferencePositions( refPosFile);
}

//----------------------------------------------------------------------
// Reads the reference positions.
//----------------------------------------------------------------------
void
script::readReferencePositions( string& fileName)
{
  char line[1024];
  
  // Open the file.
  FILE *fp;
  if ((fp = fopen(fileName.c_str(),"r")) == NULL) {
    cerr << "# Warning: cannot open reference position file " << fileName
<< endl;
    _referencePositions.resize( 1);
    _referencePositions[0].timeStamp = 0.0;
    State defaultRefPos(0,0,0);
    _referencePositions[0].position = defaultRefPos;
    return;
  }

  fprintf( stderr, "# Read reference positions from %s.\n",
fileName.c_str());

  int numberOfPositions = 0;
  while ( (fgets(line, MAX_STRING_LENGTH,fp) != NULL)) {
    numberOfPositions++;
  }

  rewind( fp);
  
  _referencePositions.resize( numberOfPositions);

  int posNum = 0;
  
  while ( (fgets(line, MAX_STRING_LENGTH,fp) != NULL) &&
	  (posNum < _referencePositions.size())){
    if (sscanf( line, "%f %f %f %f",
		&(_referencePositions[posNum].timeStamp),
		&(_referencePositions[posNum].position[X_DIM]),
		&(_referencePositions[posNum].position[Y_DIM]),
		&(_referencePositions[posNum].position[ROT_DIM]))
	== 4) {
      _referencePositions[posNum].position[ROT_DIM] = deg2Rad(
_referencePositions[posNum].position[ROT_DIM]);
      posNum++;
    }
    else {
      cerr << "#Error: Cannot read " << line;
      break;
    }
  }
  
  fprintf( stderr, "# Read %d positions.\n", _referencePositions.size());
}


//------------------------------------------------------------
// Gets the next sensor value from the script and sleeps until
// the time mark in the script is reached.
//------------------------------------------------------------
bool
script::getNextItem()
{
  char line[maxStringLength];
  float sensorSkipDistance = 0.0;
  Timer currentTime;
  
  // If "_skipIfRobotStands" is set, we search in the script for the next changed
  // robot position.
  bool positionHasChanged = false;
  
  do {

    if ( getNextLine( line)){
      
      if ( readingLine(line, sonarMark) ){
	if ( positionHasChanged && useSonar()) {
	  if ( _sonar.update(line)) {
	    _sonar.setPosition( _scriptPosition);
	    sensorSkipDistance = _sonar.distanceSinceLastIntegration();
	    PolarMotion motion = _sonar.motionSinceLastIntegration();
	  }
	}
      }
      else if (readingLine(line, laserMark)){
	if ( positionHasChanged && useLaser()) {
	  if ( _laser.update(line)) {
	    _laser.setPosition( _scriptPosition);
	    sensorSkipDistance = _laser.distanceSinceLastIntegration();
	  }
	}
      }
      
      // Check whether the position has changed.
      else if (readingLine(line, positionMark)){
	
	State nextPos = extractPosition(line, positionMark);
	
	if ( nextPos != _scriptPosition) {
	  _scriptPosition = nextPos;
	  positionHasChanged = true;
	}
      }
      else if ( readingLine(line, timeMark)){
	currentTime = getTime(line, timeMark, timeOffset);
	_elapsedTimeSinceLastMeasurement = currentTime - _previousTime;
      }
    }
    else
      return false;
  }
  while ( sensorSkipDistance < _skipDistance
	  ||
	  _realTimeMode &&
	  _elapsedTimeSinceLastMeasurement < _realTime.expiredTime());

  _elapsedTime = currentTime - _startTime;
  
  if ( _useLaser)
    _laser._referencePosition = getRefPos();
  else if ( _useSonar)
    _sonar._referencePosition = getRefPos();

  _previousTime = currentTime;
  _realTime.reset();
  return true;
}