#include "WaypointSide.h"
#include "FloatAttribute.h"
#include "AngleAttribute.h"
#include "BooleanAttribute.h"
#include "Syslog.h"
#include "WorkSiteIF.h"
#include "NavUtils.h"

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

#define NotSpecified 0.0
#define AngleNotSpecified ((NotSpecified) / Math::RadsPerDeg)

WaypointSide::WaypointSide()
  : Waypoint(), _nothing(-1.0), _waypointDone(False)
{
  // The Waypoint will load it's attributes first, then
  //
  attributes.add(new FloatAttribute("nothing", 
            "Nrthing specific",
            &_nothing, NotSpecified));

}



WaypointSide::~WaypointSide()
{
}


void WaypointSide::execute( void )
{
  Waypoint::execute();
  // Check values in Waypoint, State, etc.
  // May have to setState(Active) to do more stuff
  //
  if (!_waypointDone && state() == Behavior::Finished)
  {
    _waypointDone = True;  // The waypoint is done, but there may be more to do
  }

  // See if I'm really done
  //
  if (_nothing < 0.0 && _waypointDone)  
    setState(Finished);
}


Boolean WaypointSide::shouldBehaviorStart()
{
  return Waypoint::bothSequence();
}


Boolean WaypointSide::validInput()
{
  return (Waypoint::validInput() && _nothing >= 0.0);
}