/*  Robotics 445/600 Spring 2009 - State Machine Example
 *  Michael Ferguson
 *  
 *  Behavior Description: 
 *    Follows a right wall, using a state machine
 *
 *
 */

#ifndef INCLUDED_FollowNode_h_
#define INCLUDED_FollowNode_h_
 
#include "Behaviors/StateMachine.h"

// this follows right walls walls
class FollowNode : public StateNode {
public:
        
   FollowNode(MotionManager::MC_ID id) : StateNode("FollowNode"), start(NULL), leftish(NULL), rightish(NULL), 
	      head_id(MotionManager::invalid_MC_ID), walkid(id) {} //Constructor

   virtual void setup() {
	 // we get our walkId from constructor
     walkid = motman->addPersistentMotion(SharedObject<WalkMC>()); 

     // walk forward, slightly to the left
     leftish=new WalkNode(getName()+"::leftish",100,0,RAD(5))
     addNode(leftish);
	 // walk forward, slightly to the right
     start=addNode(rightish=new WalkNode(getName()+"::rightish",100,0,RAD(-5)));

     // transition to leftish walk if IR is less than FOLLOW_DIST
     rightish->addTransition(new SmoothCompareTrans<float>(leftish,&state->sensors[NearIRDistOffset],CompareTrans<float>::LT,200,EventBase        
	     (EventBase::sensorEGID,SensorSrcID::UpdatedSID,EventBase::statusETID),.7));          
     // transition to rightish walk if IR is greater than FOLLOW_DIST
	 leftish->addTransition(new SmoothCompareTrans<float>(rightish,&state->sensors[NearIRDistOffset],CompareTrans<float>::GT,200,EventBase        
	     (EventBase::sensorEGID,SensorSrcID::UpdatedSID,EventBase::statusETID),.7));          
	 
	 //Set up MC's
     leftish->setMC(walkid);
     rightish->setMC(walkid);
     StateNode::setup();
}

virtual void DoStart() {
    StateNode::DoStart();
	
	head_id = motman->addPersistentMotion(SharedObject<HeadPointerMC>());
	
	//move head to 45 degrees right
	MMAccessor<HeadPointerMC> head(head_id);
	head.mc()->setJoints(0,RAD(-45),RAD(10));
	
    start->DoStart();
    
	//erouter->addListener(this,EventBase::stateMachineEGID,(unsigned int)walk_1); //walking node is cast into an int
    std::cout << getName() << " is starting up." << std::endl;
}

virtual void DoStop() {
    std::cout << getName() << " is shutting down." << std::endl;
    //erouter->removeListener(this);
	motman->removeMotion(head_id);
    StateNode::DoStop();
}

virtual void teardown(){
   StateNode::teardown();
}

virtual void processEvent(const EventBase &event) {
   // std::cout << getName() << " got event: " << event.getDescription()
    //          << std::endl;
}

protected:
    StateNode * start;
    WalkNode * leftish;
    WalkNode * rightish;   
	MotionManager::MC_ID head_id; 
    MotionManager::MC_ID walkid; 
  
private:
    FollowNode(const FollowNode&);
    FollowNode operator=(const FollowNode&);      
};

#endif