#include "Behaviors/StateMachine.h"
#include "Behaviors/Demos/Navigation/VeeTags.h"
#include "Crew/PilotRequest.h"

$nodeclass VeeNavigation : VeeTags {

  $nodeclass WalkBackward250 : PilotNode(PilotTypes::walk) : doStart {
    pilotreq.dx = -250;
  }

  $nodeclass WalkForward500 : PilotNode(PilotTypes::walk) : doStart {
    pilotreq.dx = 500;
  }

  $nodeclass WalkForward1500 : PilotNode(PilotTypes::walk) : doStart {
    pilotreq.dx = 1500;
  }

  $nodeclass WalkForward3000 : PilotNode(PilotTypes::walk) : doStart {
    pilotreq.dx = 3000;
  }

  $nodeclass TurnLeft : PilotNode(PilotTypes::walk) : doStart {
    pilotreq.da = 1.571;
  }

  $nodeclass TurnRight : PilotNode(PilotTypes::walk) : doStart {
    pilotreq.da = -1.571;
  }

  $nodeclass FinalPose : StateNode : doStart {
    Point currPos = theAgent->getCentroid();
    AngTwoPi currHead = theAgent->getOrientation();
    cout << "Robot stops at " << currPos << endl;
    cout << "Robot stops facing " << currHead << endl;
  }

  $setupmachine {

    rundemo: WalkBackward250 //placeholder

    rundemo =C=>
     WalkBackward250 =C=>
      TurnLeft =C=>
       WalkForward1500 =C=>
         TurnRight =C=>
	  WalkForward1500 =C=> 
	   TurnRight =C=>
	    WalkForward3000 =C=> 
	     TurnRight =C=>
	      WalkForward1500 =C=>
	       TurnRight =C=>
	        WalkForward1500 =C=> 
		 TurnRight =C=>
		  WalkForward500 =C=>
		   FinalPose =N=>
		    PilotNode(PilotTypes::localize);		     

  }
}

REGISTER_BEHAVIOR(VeeNavigation);