#include "Behaviors/BehaviorBase.h"
#include "DualCoding/DualCoding.h"
#include "Motion/MMAccessor.h"
#include "Motion/MotionManager.h"
#include "Motion/XWalkMC.h"
#include "Events/EventRouter.h"
#include "Behaviors/Nodes/XWalkNode.h"
#include "Shared/WorldState.h"

using namespace std;
using namespace DualCoding;

class RightHandWall : public StateNode {

public:
 
   enum proceedInstruction {
  turnLeft,
  turnRight,
  turnRightCorner,
  turnAround,
  strafeLeft,
  strafeRight
  };

 RightHandWall() : StateNode("RightHandRule"),walker_id(MotionManager::invalid_MC_ID) {} 

     MotionManager::MC_ID walker_id;

	  virtual void DoStart() {

		}    //end Dostart()

           virtual void DoStop() {
                  motman->removeMotion(walker_id);
           }

		class DecideDirection : public StateNode  {
		  public:
		       
		        DecideDirection() : StateNode("DecideDirection") {}
		        

		          virtual void DoStart() {
		          
		            float right, left, front; //sides of the infared
		
	               right = state -> sensors[RightIRDistOffset];
	               left =  state -> sensors[LeftIRDistOffset];
	               front = state -> sensors[CenterIRDistOffset];
	               
	             cout<<"left: "<<left<<" "<<"Right: "<<right<<" "<<"Front:"<< front << endl;
	                              
	               
                 // if close enough to the wall and nothing is around walk straight
	              if( right <= .07  && right >= .009  && left == 0 && front ==0)
	                 postStateCompletion();
	                 
	              else{    
	              // if robot is located too far from the wall or if robot is drifting away from the wall strafe towards the wall 	             
	              if( right < .009 && right > 0 && left == 0 && front ==0)
	                 postStateSignal<proceedInstruction>(strafeRight);
	              
	              else{            
	              // if robot is located too close to the wall or if robot is drifting to close strafe away from the wall  	             
	              if( right > .07  && left == 0 && front ==0)
	                 postStateSignal<proceedInstruction>(strafeLeft);
	              
	              else{        
                 // if nothing is on the right left or infront make a right turn
		           if ( right == 0 && left == 0 && front == 0)
	                 postStateSignal<proceedInstruction>(turnRightCorner);
	              
	              else{       
	              // if there is a wall to the right and the front make a left turn
	              if( front > 0 && right > 0 && left== 0)
	                  postStateSignal<proceedInstruction>(turnLeft);
	              
	              else{       
                 // if there is a wall to the left and in front make a right turn
	              if( front > 0 && left > 0 && right == 0)
	                  postStateSignal<proceedInstruction>(turnRight);
                 
                  else{
                 // if there are walls all around about face  
	                  postStateSignal<proceedInstruction>(turnAround);
	           }   
	         
	          }        
	         }         
	        }           
			 }
		   }
		  }
		};
		
		  virtual void setup() {

	SharedObject<XWalkMC> walk_mc;

   walker_id = motman->addPersistentMotion(walk_mc);
	#statemachine
	
	startnode: DecideDirection()
	startnode =S<proceedInstruction>(turnRightCorner)=> right_c
	right_c: XWalkNode (0,125,0,0,0,0)[setMC(walker_id);
					   setMC(foo)]=>right_c2
	right_c2: XWalkNode (0,0,0,0,0,-M_PI/2)[setMC(walker_id);] =C=> startnode
	
	
	#endstatemachine
	}
};

DATAEVENT_IMPLEMENTATION(RightHandWall::proceedInstruction, unsigned int);