//-*-c++-*-

/* HeadBehavior.h.fsm
 * Code for HW1 question 2. Makes the robot nod its head "yes".
 * @author David Klionsky (dklionsk)
 * @author Carl Doersch (cdoersch)
 */

#ifndef INCLUDED_HeadBehavior_h_
#define INCLUDED_HeadBehavior_h_
 
#include "Behaviors/StateMachine.h"
#include "Events/EventRouter.h"

class HeadBehavior : public StateNode {

public:
  HeadBehavior() : StateNode("HeadBehavior") {}
 
  virtual void setup() {
#statemachine

startnode: StateNode =N=> {noblink, neutral}

noblink: LedNode [setPriority(MotionManager::kBackgroundPriority);
	          getMC()->set(RobotInfo::FaceLEDMask,0.0);]

wait: StateNode
  =E(buttonEGID, ChiaraInfo::GreenButOffset, activateETID)=>
     blink: LedNode [getMC()->cycle(RobotInfo::GreenLEDMask, 1500, 1.0);] =T(1000)=> wait

neutral: HeadPointerNode [getMC()->setMaxSpeed(0,0.2); getMC()->setMaxSpeed(1,0.2); 
			  getMC()->lookAtPoint(5000,0,400);] =T(2000)=> lookDown

lookDown: HeadPointerNode [ getMC()->setMaxSpeed(0,0.2); getMC()->setMaxSpeed(1,0.2); 
			    getMC()->lookAtPoint(500,0,-100); ] =T(2000)=> neutral

#endstatemachine
  }       // end of setup()

};

#endif