#include "WaypointWall.h"
#include "WaypointWallLog.h"
#include "FloatAttribute.h"
#include "AngleAttribute.h"
#include "BooleanAttribute.h"
//#include "StringAttribute.h"
#include "IntegerAttribute.h"
#include "Syslog.h"
#include "WorkSiteIF.h"
#include "DvlSideIF.h"
#include "MultibeamerIF.h"
#include "NavUtils.h"
#include "TimeP.h"

#ifndef PI
#define PI     3.14159265358979323846
#endif
#define R2D(r) (r*180.0/PI)

#define NotSpecified -100000.0
#define AngleNotSpecified ((NotSpecified) / Math::RadsPerDeg)
//#define DEFAULT_STRING_ATTR "\"\""
#define DEFAULT_STRING_ATTR "DefaultString"

WaypointWallLog WaypointWall::log = WaypointWallLog(DataLog::BinaryFormat);

WaypointWall::WaypointWall()
   : Behavior(WaypointWallBehaviorName, Sequential), 
     cos30(cos(PI/6.))
{
  attributes.add(new FloatAttribute("northing", 
				    "Northing (or specify latitude/longitude)",
				    &_northing, NotSpecified));

  attributes.add(new FloatAttribute("easting", 
				    "Easting (or specify latitude/longitude)",
				    &_easting, NotSpecified));

  attributes.add(new AngleAttribute("latitude", 
				    "Latitude (or specify northing/easting)",
				    &_latitude, AngleNotSpecified));

  attributes.add(new AngleAttribute("longitude", 
				    "Longitude (or specify northing/easting)",
				    &_longitude, AngleNotSpecified));


  attributes.add(new FloatAttribute("speed", "Speed", &_speed));

  attributes.add(new FloatAttribute("depth", "Depth", &_depth, NotSpecified));

  attributes.add(new FloatAttribute("standOff", "StandOff", &_standOff, NotSpecified));

  attributes.add(new FloatAttribute("sonarTimeOut", "SonarTimeOut", 
                                     &_sonarTimeOut, NotSpecified));

  attributes.add(new FloatAttribute("maxCrossTrackError",  "Max Cross Track Error", 
				    &_maxXte, NotSpecified));

  attributes.add(new FloatAttribute("simWallInter",  "Sim Wall Intercept", 
				    &_wallInter, NotSpecified));

  attributes.add(new FloatAttribute("simWallSlope",  "Sim Wall Slope", 
				    &_wallSlope, NotSpecified));
//
// Dvl=1; Reson=2; EchoSounder=3.
  attributes.add(new IntegerAttribute("rangeInst",  "Range Instrument", 
                                      &_rangeInst, NotSpecified));
//
//Beware that these attributes are not parsed until after this constructor
//finishes - so you can't use it in a calculation here.
//
//  attributes.add(new StringAttribute("rangeInst", "Range Instrument",));


  for( int i=0; i<NUM_BEAMS; i++ )
  {
     _consecGoodHits[i] = 0;
  }

  _first = True;

  _hMode = DynamicControlIF::WaypointWall;
  _sonarIsAlive = True;
  _abortOnTimeOut = True;

  //_log = new WaypointWallLog(this, DataLog::BinaryFormat);
}



WaypointWall::~WaypointWall()
{
   //
   //Don't delete _dvlSide because subsequent behaviors may use it, assuming
   //this destructor runs when this behavior ends.  Or does it not run until
   //the whole stack is done ??

   //
   //if( _dvlSide ) delete _dvlSide;
}


void WaypointWall::execute( void )
{
  double dn, de;
  double dtw, goal;
  double ye;
  double heading;
  Boolean debug = True;
  Boolean gotReturn = False;

  NavigationIF::Position position;
  NavigationIF::Attitude attitude;
  // Get current position
  _navigation->state(&position, &attitude);

  heading = attitude.yaw;

  if(_first)
  {
     //
     // This is Steve R's psi_ref, that is, _bearing = psi_ref.
     //
    _bearing = PI + Math::modPi( atan2(_easting - position.y,
				 _northing - position.x) - PI);
    //
    // Please DO NOT CHANGE the write statement below.  It is automatically
    // read out of syslog by a shell script, and read into the plotting 
    // routines.  Any changes will disrupt the plotting routines.  Contact
    // Rob McEwen if you need to change this.
    //
    Syslog::write( "WaypointWall Initialization: \n"
		   "  Begin waypoint control at t= %-15.2f"
		   "                   (wplog)\n"
		   "  The current location (N,E) = %-15.1f, %-15.1f  (wplog)\n"
		   "  The next waypoint          = %-15.1f, %-15.1f  (wplog)\n"
		   "  The bearing to the next w.p. is %.1f Degrees.\n", 
                   _missionClock->seconds(),
		   position.x, position.y, 
		   _northing, _easting, R2D(_bearing) );

    _rangeToWall = _standOff;
    _first = False;
    Syslog::write( "WaypointWall:: standOff = %5.2f", _standOff);

    WaypointWall::log.setFields("Waypoint", position.x, position.y,
				_northing, _easting, R2D(_bearing));
  }

  /* distance to waypoint in N, E coords */
  dn = position.x - _northing;
  de = position.y - _easting;

  /* transform into dtw, xte coordinates */
  dtw = -de*sin(_bearing) - dn*cos(_bearing);

  switch( _rangeInst )
  {
     case Dvl:
	processDvl( &_sonarIsAlive, &_sonarHasNewData, &_rangeToWall );
	break;
     case Multibeamer:
	processMultibeamer( &_sonarIsAlive, &_sonarHasNewData, &_rangeToWall );
	break;
     case EchoSounder:
	processEchoSounder( &_sonarIsAlive, &_sonarHasNewData, &_rangeToWall );
	break;
     default:
	Syslog::write("WaypointWall::Error in _rangeInst switch statment.");
	break;
  }
  //
  // The sonar has stopped responding. Either abort or return to line.
  if( !_sonarIsAlive ) 
  {
     if( _abortOnTimeOut )
     {
	Syslog::write("WaypointWall - Side-looking Sonar has not responded "
	              "for too long.  Aborting.");
	abortMission();
     }
     else
     {
	Syslog::write("WaypointWall - Side-looking Sonar has not responded "
	              "for too long.\n  Fly down the center of the corridor.");
	_xTrkOffset = 0.;
	_hMode = DynamicControlIF::Waypoint;
     }
  }
  else
  {
     _hMode = DynamicControlIF::WaypointWall;
  }

  double xteWall, xteWay;
  if( _sonarIsAlive && _sonarHasNewData )
  {
     //
     // Compute cross-track error from the line.  
     xteWay = dn*sin(_bearing) - de*cos(_bearing);
     //
     // Compute the position error in the waypoint frame.  Assume the
     // Dvl is mounted on the port side. Otherwise, flip the sign.
     //
     xteWall = (_standOff - _rangeToWall)*cos(heading-_bearing);
     //
     // Now check to see if the vehicle will be commanded out of the
     // lane. "Captain, you are out of the lane!"
     if( xteWay - xteWall >= _maxXte )
     {
	_xTrkOffset = xteWay - _maxXte;
     }
     else if( xteWay - xteWall <= -_maxXte )
     {
	_xTrkOffset = xteWay + _maxXte;
     }
     else
     {
	_xTrkOffset = xteWall;
     }
  }

  //
  //Since here dtw is the projection along the line of bearing, only
  //require that the vehicle pass the waypoint, which is equivalent to
  //dtw<0.
  //
  goal = 0.;

  /* load outputs */
  if ( dtw <= goal ) 
  {

    setState(Finished);
    //
    // Please to not modify the following write.
    //
    dprintf(" WaypointWall has set the state to finished.\n");

    Syslog::write( "WaypointWall (%.1f, %.1f), \n"
		   "  reached at t = %-15.2f"
		   "                                 (wplog)\n", 
                    _northing, _easting, _missionClock->seconds());
    Syslog::write( "Vehicle Position is error is (%.1f, %.1f)\n",
                    _northing-position.x, _easting-position.y);
  }
  else 
  {
    /* load command vector */

    if (_depth != NotSpecified)
      setVertical(DynamicControlIF::Depth, _depth);

    setSpeed(DynamicControlIF::Speed, _speed);

    double newBearing=0., newNorthing = 0., newEasting = 0.;

    setHorizontal(_hMode, _bearing, _northing, _easting,
		       newBearing, newNorthing, newEasting, 
		       _first, _xTrkOffset);
  }
}


Boolean WaypointWall::shouldBehaviorStart()
{
  return bothSequence();
}

void WaypointWall::processDvl( Boolean *sonarIsAlive, 
                               Boolean *sonarHasNewData, 
                               double *rangeToWall )
{
  //
  // Compute range to the wall from the side-looking Dvl:
  DeviceIF::Status status = _dvlSide->get(&_dvlData, &_dvlHasNewData);
  
  double ctheta, stheta, cphi, sphi;
  int numValidBeams=0;
  double beamRange[NUM_BEAMS], h[NUM_BEAMS];
  double aveAlt = 0., minRange = 500.;
  Boolean useMinBeam = False;
  int minIndx = -1;
  //
  TimeIF::TimeSpec now;
  double nowUnix;
  Time::gettime(&now);
  nowUnix = Time::seconds(&now);
  //
  // Ignore the _dvlHasNewData passed by get() and flag new data here.  Use a !=
  // rather than an < in the "if" below so that a Dvl clock rollover doesn't
  // set _dvlHasNewData to False.
  //
  *sonarHasNewData = False;
  if( _lastPingTime != _dvlData.pingTime )
  {
    *sonarHasNewData  = True;
    *sonarIsAlive     = True;
    _lastPingTime     = _dvlData.pingTime;
    _lastUnixPingTime = nowUnix;
  }
  //
  // Ensure that the Dvl is still working.
  //
  if( nowUnix > _lastUnixPingTime + _sonarTimeOut ) *sonarIsAlive = False;

  if( *sonarIsAlive && *sonarHasNewData )
  {
#if 0
     ctheta = cos(attitude.pitch);
     stheta = sin(attitude.pitch);
     cphi   = cos(attitude.roll);
     sphi   = sin(attitude.roll);
#endif
     //
     // Unfortunately the beams are not declared as an array in the DvlSideIF.idl.

     // keep running count of number of consecutive good hits for filtering
     (_dvlData.beam1 != BAD_BEAM_RANGE) ? _consecGoodHits[0]++:_consecGoodHits[0]=0;
     (_dvlData.beam2 != BAD_BEAM_RANGE) ? _consecGoodHits[1]++:_consecGoodHits[1]=0;
     (_dvlData.beam3 != BAD_BEAM_RANGE) ? _consecGoodHits[2]++:_consecGoodHits[2]=0;
     (_dvlData.beam4 != BAD_BEAM_RANGE) ? _consecGoodHits[3]++:_consecGoodHits[3]=0;

     // Prevent rollover
     if (_consecGoodHits[0] >= MIN_GOOD_HITS_WALL) 
	_consecGoodHits[0] = MIN_GOOD_HITS_WALL;
     if (_consecGoodHits[1] >= MIN_GOOD_HITS_WALL) 
	_consecGoodHits[1] = MIN_GOOD_HITS_WALL;
     if (_consecGoodHits[2] >= MIN_GOOD_HITS_WALL) 
	_consecGoodHits[2] = MIN_GOOD_HITS_WALL;
     if (_consecGoodHits[3] >= MIN_GOOD_HITS_WALL) 
	_consecGoodHits[3] = MIN_GOOD_HITS_WALL;
 
     beamRange[0] = _dvlData.beam1;
     beamRange[1] = _dvlData.beam2;
     beamRange[2] = _dvlData.beam3;
     beamRange[3] = _dvlData.beam4;

     //mark beam range as bad if it hasn't tracked the bottom for more than
     //MIN_GOOD_HITS number of bottom detects
     if (_consecGoodHits[0] < MIN_GOOD_HITS_WALL)  beamRange[0] = BAD_BEAM_RANGE;
     if (_consecGoodHits[1] < MIN_GOOD_HITS_WALL)  beamRange[1] = BAD_BEAM_RANGE;
     if (_consecGoodHits[2] < MIN_GOOD_HITS_WALL)  beamRange[2] = BAD_BEAM_RANGE;
     if (_consecGoodHits[3] < MIN_GOOD_HITS_WALL)  beamRange[3] = BAD_BEAM_RANGE;
     //
     //
     // Beam numbering convention, looking through Dvl from behind.  x and z are
     // the vehicle axes.
     //
     //                x
     //                ^ 
     //                |
     //              1 | 3
     //                |------> z
     //              4   2
     //
     // h[i] is the the z component of each beam's unit vector (in N).
     // Multiply by the measured range to get the altitude of the i^th beam
     // assuming a planar level bottom.
     //
     //h[0] = ( -stheta*c3s2  -  ctheta*sphi*s3s2  +  ctheta*cphi*c2 )*_dvlData.beam1;
     //h[1] = (  stheta*c3s2  +  ctheta*sphi*s3s2  +  ctheta*cphi*c2 )*_dvlData.beam2;
     //h[2] = ( -stheta*c3s2  +  ctheta*sphi*s3s2  +  ctheta*cphi*c2 )*_dvlData.beam3;
     //h[3] = (  stheta*c3s2  -  ctheta*sphi*s3s2  +  ctheta*cphi*c2 )*_dvlData.beam4;
     //
     h[0] = _dvlData.beam1*cos30;
     h[1] = _dvlData.beam2*cos30;
     h[2] = _dvlData.beam3*cos30;
     h[3] = _dvlData.beam4*cos30;

     for( int i=0; i<NUM_BEAMS; i++ ) 
     {
	/*
	** Get range from beam i
	*/
	if (beamRange[i] != (double) BAD_BEAM_RANGE) 
	{
	   numValidBeams++;
	   /*
	   ** Sum up all ranges, or take the minimum range of the four beams
	   */
	   if( useMinBeam ) 
	   {
	      if( beamRange[i] < minRange ) 
	      {
		 minIndx = i;
		 minRange = beamRange[i];
	      }
	   }
	   else 
	   {
	      aveAlt += h[i];
	   }  // if( useMinBeam ) 
	}  // if (beamRange[i] != (double) BAD_BEAM_RANGE) 
     }  // for( i=0; i<NUM_BEAMS; i++ ) 
     //
     // Select either the minimum altitude or the average of the altitudes
     // computed from the good (nonzero) beam ranges. 
     //
     if(numValidBeams)
     {
	//
	// Compute the range to the wall in vehicle coordinates.
	if( useMinBeam )  *rangeToWall =  h[minIndx];
	else              *rangeToWall =  aveAlt / ( (double) numValidBeams );
     }
     else
     {
	//
	// There wasn't a return.  Interpret this as the wall being out of
	// range.  Add code later to determine if instead the wall is too
	// close.  Another cause for no return is the wall is angled >20
	// degrees from the Dvl centerline.

	*rangeToWall = MAX_BEAM_RANGE * cos30;
     }
  }//  if( *sonarIsAlive && *sonarHasNewData )
}

void WaypointWall::processMultibeamer( Boolean *sonarIsAlive, 
                                       Boolean *sonarHasNewData, 
                                       double  *rangeToWall )
{
   TimeIF::TimeSpec updateTime;
   Boolean ready;
   double pingTime;

   _multibeamer->get_status( rangeToWall, &updateTime, sonarIsAlive, &ready);
   
   pingTime = Time::seconds(&updateTime);

   if( _lastPingTime != pingTime )
   {
      *sonarHasNewData   = True;
      _lastPingTime      = pingTime;
   }
   else
   {
      *sonarHasNewData   = False;
   }
}

void WaypointWall::processEchoSounder( Boolean *sonarIsAlive, 
                                       Boolean *sonarHasNewData, 
                                       double  *rangeToWall )
{
}



Boolean WaypointWall::validInput()
{
  Boolean debug = False;
  Boolean valid = True;

  dprintf("WaypointWall::validInput() - _northing: %.2f, _easting: %.2f",
	  _northing, _easting);

  dprintf("WaypointWall::validInput() - _latitude: %.2f, _longitude: %.2f",
	  _latitude, _longitude);

  Boolean _utmSpecified = False;
  Boolean _geographicSpecified = False;

  if (_northing != NotSpecified || _easting != NotSpecified)
    _utmSpecified = True;

  if (_latitude != NotSpecified || _longitude != NotSpecified)
    _geographicSpecified = True;

  if (_utmSpecified && _geographicSpecified) {
    printError("Can't specify both UTM and geographic coords");
    valid = False;
  }

  // Waypoint location must be specified either in UTM or geographic coords
  Boolean coordsSpecified = False;
  if (_northing != NotSpecified && _easting != NotSpecified) {
    coordsSpecified = True;
  }

  if (_latitude != NotSpecified && _longitude != NotSpecified) {
    if (coordsSpecified) {
      printError("Use either UTM or geographic to specify coords");
      valid = False;
    }
    else {
      // Convert lat/lon to UTM
      WorkSiteIF workSite("workSite");
      NavUtils::geoToUtm(_latitude, _longitude, workSite.utmZone(), 
			 &_northing, &_easting);

      coordsSpecified = True;
    }
  }

  if (!coordsSpecified) {
    printError("Use either UTM or geographic to specify coords");
    valid = False;
  }

  if (_speed < 0.) {
    printError("Invalid speed");
    valid = False;
  }

  if (_depth != NotSpecified && _depth < 0.) {
    printError("Invalid depth");
    valid = False;
  }

  if (_maxXte == NotSpecified || _maxXte < 0.)
  {
    printError("maxCrossTrackError not specified.");
    valid = False;
  }

  if (_sonarTimeOut == NotSpecified || _sonarTimeOut < 0.)
  {
    printError("sonarTimeOut not specified.");
    valid = False;
  }

  if (_standOff == NotSpecified || _standOff < 0.)
  {
    printError("standOff not specified.");
    valid = False;
  }

  if(_wallInter == NotSpecified && _wallSlope != NotSpecified ||
     _wallInter != NotSpecified && _wallSlope == NotSpecified)
  {
     printError("You must specify both simWallInter and simWallSlope.");
     valid = False;
  }

  Syslog::write("WaypointWall::validInput() - Opening IF to rangeInst = %d", 
                _rangeInst);

  Boolean IFopen = False;
  
  switch( _rangeInst )
  {
     case Dvl:
	try 
	{
	   if( !_dvlSide ) _dvlSide = new DvlSideIF("dvlSide");
	   IFopen = True;
	} 
	catch (Exception e) 
	{
	   Syslog::write("WaypointWall--Caught exception on creation "
	   "of DvlIF:%s\n", e.msg);
	   _dvlSide = NULL;
	}
	catch(...)
	{
	   Syslog::write("WaypointWall -- Failed to initialize connection "
	   "to DvlSideIF");
	   _dvlSide = NULL;
	}
	break;
     case Multibeamer:
	try 
	{
	   if( !_multibeamer ) _multibeamer = new MultibeamerIF("Multibeamer");
	   IFopen = True;
	} 
	catch (Exception e) 
	{
	   Syslog::write("WaypointWall--Caught exception on creation "
	   "of MultibeamerIF:%s\n", e.msg);
	   _multibeamer = NULL;
	}
	catch(...)
	{
	   Syslog::write("WaypointWall -- Failed to initialize connection "
	   "to MultibeamerIF");
	   _multibeamer = NULL;
	}
	break;
#if 0
     case EchoSounder:
	try 
	{
	   if( !_echoSounder ) _echoSounder = new EchoSounderIF("EchoSounder");
	   IFopen = True;
	} 
	catch (Exception e) 
	{
	   Syslog::write("WaypointWall--Caught exception on creation "
	   "of EchoSounderIF:%s\n", e.msg);
	   _echoSounder = NULL;
	}
	catch(...)
	{
	   Syslog::write("WaypointWall -- Failed to initialize connection "
	   "to EchoSounderIF");
	   _echoSounder = NULL;
	}
	break;
#endif
     default:
     {
	Syslog::write("WaypointWall:: Error.  You must specify the range sensor.");
	//abortMission();
	valid = False;
     }
     break;
  }

  if( !IFopen )
  {
     Syslog::write("WaypointWall:: Error.  The interface to %d failed to open. "
                   "Aborting.", _rangeInst);
     //abortMission();
     valid = False;
  }
  else
  {
     Syslog::write("Using range finder %d.", _rangeInst);
  }


  return valid;
}