#ifndef __Herding_H_
#define __Herding_H_

#include "Behaviors/BehaviorBase.h"
#include "Events/EventRouter.h"

#include "Events/EventBase.h" 
#include "DualCoding/DualCoding.h"
#include "Behaviors/StateMachine.h"
#include "Motion/XWalkMC.h"
#include "Motion/MotionPtr.h"

#include "BuildBoundary.h"
#include "Manipulation.h"

using namespace DualCoding;

//next way point
extern Point next_pos;
extern float next_ori;

class Herding : public VisualRoutinesStateNode {

public:
	Herding() : VisualRoutinesStateNode("SheepHerding") {}
	virtual void setup() {
        MotionManager::MC_ID walkID = addMotion(MotionPtr<XWalkMC>());
#statemachine
        startnode: StateNode        
        buildlmap: HerdingBuildLMap()	
        buildboundary: BuildBoundary(true)
        lookdown: LookDown()
        resetarm: ArmNode() [getMC()->setJoints(30*M_PI/180, -120*M_PI/180, -80*M_PI/180);]
        dummy0: StateNode
        dummy1: StateNode
        dummy2: StateNode
        reach: ReachBall()
        findobstacles: FindObstacles()
        adjustarm: AdjustArm()
        hit: HitBall()
        reset: ResetPose()

        buildlmapwp: HerdingBuildLMap()	
        buildboundarywp: BuildBoundary(false)
        lookdownwp: LookDown()
        resetarmwp: ArmNode() [getMC()->setJoints(30*M_PI/180, -120*M_PI/180, -80*M_PI/180);]
        dummy0wp: StateNode
        dummy1wp: StateNode
        dummy2wp: StateNode
        reachwp: ReachBall()
        findobstacleswp: FindObstacles()
        adjustarmwp: AdjustArm()
        hitwp: HitBall()
        resetwp: ResetPose()

        move2wp: Move2WayPoint()
        rotatwp: RotAtWayPoint()

        startnode =N=> buildlmap
        
        buildlmap =MAP=> buildboundary            		

        buildboundary =C=> lookdown

        buildboundary =S<bool>=> move2wp

        lookdown =C=> resetarm

    	resetarm =C=> dummy0

        dummy0 =T(3000)=> reach

        reach =C=> dummy1

        dummy1 =T(3000)=> findobstacles

      	findobstacles =MAP=> adjustarm

	    adjustarm =C=> hit

        hit =C=> dummy2

        dummy2 =T(1500)=> reset

        reset =C=> move2wp
        
        move2wp =C=> buildlmapwp

        buildlmapwp =MAP=> buildboundarywp

        buildboundarywp =C=> lookdownwp
        
        buildboundarywp =S<bool>=> rotatwp

        lookdownwp =C=> resetarmwp

    	resetarmwp =C=> dummy0wp

        dummy0wp =T(3000)=> reachwp

        reachwp =C=> dummy1wp

        dummy1wp =T(3000)=> findobstacleswp

      	findobstacleswp =MAP=> adjustarmwp

	    adjustarmwp =C=> hitwp

        hitwp =C=> dummy2wp

        dummy2wp =T(1500)=> resetwp
        
        resetwp =C=> rotatwp

        rotatwp =C=> buildlmap
#endstatemachine
	}
};

#endif