#include "Shared/RobotInfo.h"

#if defined(TGT_HAS_ARMS) && defined(TGT_HAS_HEAD)

#include "Behaviors/Nodes/PostureNode.h"
#include "Behaviors/Transitions/EventTrans.h"
#include "Motion/MotionPtr.h"
#include "Behaviors/StateMachine.h"

$nodeclass WalkRobot : StateNode {

	/////////////////////////////////////////////////////////////////////////////

	$nodeclass StickArmOut : PostureNode : doStart {
		getMC()->setOutputCmd(ArmBaseOffset,OutputCmd(-0.08));
		getMC()->setOutputCmd(ArmShoulderOffset,OutputCmd(0.74));
		getMC()->setOutputCmd(ArmElbowOffset,OutputCmd(0.36));
		getMC()->setOutputCmd(ArmWristOffset,OutputCmd(-0.98));
		getMC()->setOutputCmd(WristRotateOffset,OutputCmd(1.44));
		postStateCompletion();
	}

	/////////////////////////////////////////////////////////////////////////////

	$nodeclass RobotWalk : WalkNode {
		virtual void doStart() {
			erouter->addListener(	this,
					  	EventBase::sensorEGID);
		}

		virtual void doEvent() {
			float armBase = state->outputs[ArmBaseOffset];
			float armShoulder = state->outputs[ArmShoulderOffset];
			float val = armShoulder - 0.74;
			float maxDisp = 0.5;
			float maxVelocity = 700;

			//Turn left if  offset is greater than threshold
			if((armBase - (-0.08)) > 0.15) setDisplacement(0,0,1.57);
		
			//Turn right if off set is greater than threshold
			else if (armBase - (-0.08) < -0.15) setDisplacement(0,0,-1.57);
			
			//Walk forward proportional to displacement of shoulder
			else if (val < -0.05) {
				val = -val;
				float velocity = (val/maxDisp) * maxVelocity;
				setVelocity(velocity,0,0);
			}

			//walk backward proportional to displacement of shoulder
			else if (val > 0.05) {
				float velocity = (val/maxDisp) * maxVelocity;
				setVelocity(-velocity,0,0);
			}
		}
	}

	/////////////////////////////////////////////////////////////////////////////

	virtual void setup() {

		MotionManager::MC_ID posturemc = addMotion(MotionPtr<PostureMC>());
		$statemachine{
		  	startnode: StickArmOut[setMC(posturemc)] =T(5000)=> RobotWalk

		}
	}
}
REGISTER_BEHAVIOR(WalkRobot);
#endif