#include "WaypointBottom.h"
#include "FloatAttribute.h"
#include "AngleAttribute.h"
#include "BooleanAttribute.h"
#include "StringAttribute.h"
#include "Syslog.h"
#include "WorkSiteIF.h"
#include "NavUtils.h"
#include "TimeP.h"
#include "VehicleConfigurationServer.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 "\"\""

WaypointBottom::WaypointBottom()
  : Behavior(WaypointBottomBehaviorName, Sequential)
{
  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("altitude", "Altitude", &_altitude, 
  NotSpecified));

  attributes.add(new FloatAttribute("wavelength", "Wavelength", 
  &_lambda, NotSpecified));

  attributes.add(new FloatAttribute("amplitude", "Amplitude", &_amp, NotSpecified));

  attributes.add(new FloatAttribute("stime", "Settling Time", &_stime, 0.));

  attributes.add(new BooleanAttribute("feedThetaForward", "Use Feedfoward", 
  &_feedThetaForward, True));

  attributes.add(new BooleanAttribute("quarterWave", "Quarter-wave step", 
  &_quarterWave, False));

  attributes.add(new StringAttribute("altFileName", "Altitude & Theta FF File", 
  &_baseName, DEFAULT_STRING_ATTR));

  _first = True;

  _thetaFF  = 0.;

  _latched = False;
  _run     = False;

  _indxTab = 0;

  _altitudeCmd = 50.;

  _lenTab = 0;

  _initProfile = True;

  for( int i=0; i< NTAB; i++ )
  {
     _altTab[i][0] = 0.;
     _altTab[i][1] = 0.;
     _altTab[i][2] = 50.;
  }
  
}



WaypointBottom::~WaypointBottom()
{
   if( _fpAsc) fclose(_fpAsc);
}


void WaypointBottom::execute( void )
{
  double dn, de;
  double dtw, goal;
  Boolean debug = True;
  TimeIF::TimeSpec now;

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

  if(_first)
  {
    _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( "WaypointBottom 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) );

    TimeIF::TimeSpec now;
    Time::gettime(&now);
    _stime0 = Time::seconds(&now);

    _first = False;
    _depthCmd = _depth;

    if( _depth != NotSpecified && _amp != NotSpecified && 
    _lambda != NotSpecified )
    {
       Syslog::write("WaypointBottom:: _feedThetaForward = %d",_feedThetaForward);
       Syslog::write("WaypointBottom:: _quarterWave = %d",_quarterWave);
    }
    if ( _depth != NotSpecified )
       Syslog::write("WaypointBottom:: Depth control.");
    if( _altitude != NotSpecified ) 
       Syslog::write("WaypointBottom:: Altitude control.");
  }

  Time::gettime(&now);
  if( !_latched && ( Time::seconds(&now) >= (_stime0 + _stime)) )
  {
     
     _x0 = position.x;
     _y0 = position.y;
     _run = True;
     _latched = True;
  }


  /* 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);


  double h_tg;
  double cmdAlt, dist, dn0, de0;

  if( _run && _depth != NotSpecified && _amp != NotSpecified && 
      _lambda != NotSpecified )
  {

     dn0 = position.x - _x0;
     de0 = position.y - _y0;

     //dist = dn0*sin(_bearing) + de0*cos(_bearing);
     dist = sqrt( dn0*dn0 + de0*de0 );

     if( !_quarterWave || dist<_lambda/2. )
     {
	_depthCmd = _depth - _amp*(1.-cos(2.*PI*dist/_lambda));
	if( _feedThetaForward )
	   _thetaFF  = 2.*PI*_amp/_lambda*sin(2.*PI*dist/_lambda);
	else
	   _thetaFF = 0.;
     }
     else
     {
	_thetaFF = 0.;
     }
     if( _initProfile )
     {
	Syslog::write("WaypointBottom::  Latching position and starting "
	"command generator at t = %.2f.",_missionClock->seconds());
	_initProfile = False;
     }
  }
  else
  {
     _depthCmd = _depth;
     _thetaFF  = 0.;
  }
  //
  if( _altitude != NotSpecified )
  {
     _thetaFF = 0.;
     _altitudeCmd = _altitude;

     // If a look-up table exists, use it for altitude and thetaFF.
     // _x0 and _y0 aren't valid until _run == True.
     if( _run && _fpAsc )
     {
	//
	// Distance from start:
	dn0 = position.x - _x0;
	de0 = position.y - _y0;
	dist = sqrt( dn0*dn0 + de0*de0 );

	while( _altTab[_indxTab+1][0] < dist && _indxTab < _lenTab-2 )  _indxTab++;

	_thetaFF     = (double) _altTab[_indxTab][1];
	_altitudeCmd = (double) _altTab[_indxTab][2];

	if( dist >= _altTab[_lenTab-1][0] ) 
	{
	   _altitudeCmd = (double) _altTab[_lenTab-1][2];
	   _thetaFF = 0.;
	}

	if( _initProfile )
	{
	   Syslog::write("WaypointBottom::  Latching position and starting "
	   "profile at t = %.2f.",_missionClock->seconds());
	   _initProfile = False;
	}

	Syslog::write("_indxTab = %d, _altTab[][0]=%7.2f, dist=%7.2f, "
	"_thetaFF=%7.2f, _altitudeCmd = %7.2f",
	_indxTab, _altTab[_indxTab][0], dist, _thetaFF, _altitudeCmd);
     }
  } 
  //
  //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(" Waypoint has set the state to finished.\n");

    Syslog::write( "Waypoint (%.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, _depthCmd, PI/2., _thetaFF);

    if (_altitude != NotSpecified)
      setVertical(DynamicControlIF::Altitude, _altitudeCmd, PI/2., _thetaFF);

    setSpeed(DynamicControlIF::Speed, _speed);

    setHorizontal(DynamicControlIF::Waypoint, _bearing, _northing, _easting);
  }
}


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


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

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

  dprintf("WaypointBottom::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 (_depth == NotSpecified && _altitude == NotSpecified ) 
  {
    printError("WaypointBottom::Error - You must specifiy either "
    "depth or altitude.");
    valid = False;
  }

  if (_depth != NotSpecified && _altitude != NotSpecified ) 
  {
    printError("WaypointBottom::Error - You cannot specifiy both "
    "depth and altitude.");
    valid = False;
  }

  if (_lambda != NotSpecified && _lambda == 0.) {
    printError("Wavelengh cannot be zero.");
    valid = False;
  }

  char *auvConfigDir = getenv(AuvConfigDirName);
  if (auvConfigDir == 0)
  {
    Syslog::write("WaypointBottom::WaypointBottom() - environment variable %s not set\n",
                  AuvConfigDirName);
  }
  //
  // FullFileName contains the full path and file name.
  //
  sprintf(_fullFileName, "%s/%s", auvConfigDir, _baseName);

  _fpAsc = fopen(_fullFileName,"r");

  if( _fpAsc == NULL )
  {
    Syslog::write( " WaypointBottom::Waypointbottom() - "
                   "Could not open:\n   %s.\n   ThetaFF = 0.\n", _fullFileName );
  }
  else
  {
     int i;
     char buf[128];
     for( i=0; fgets(buf, 128, _fpAsc) && i<NTAB; i++ )
     {
	sscanf(buf, "%f %f %f", &_altTab[i][0], &_altTab[i][1], &_altTab[i][2]);
	Syslog::write("WaypointBottom:: Distance=%7.2f, ThetaFF=%7.2f, "
	"Altitude=%7.2f\n",
	_altTab[i][0], _altTab[i][1], _altTab[i][2]);
     }
     _lenTab = i;
     Syslog::write("WaypointBottom:: Read %d lines from file %s.",
     _lenTab, _fullFileName);
  }
  //_tergde = new TerrainGuidance(_mapFile, initConditions);
  //Syslog::write("WaypointBottom:: 21 Distance=%7.2f, ThetaFF=%7.2f, "
  //"Altitude=%7.2f\n",
  //_altTab[21][0], _altTab[21][1], _altTab[21][2]);

  return valid;
}