#ifndef _MOTION_NODES_H_
#define _MOTION_NODES_H_

#ifdef TGT_HAS_ARMS

//************

#include "Behaviors/StateNode.h"
#include "Behaviors/Nodes/ArmNode.h"
#include "Behaviors/Nodes/HeadPointerNode.h"
#include "Behaviors/Nodes/DynamicMotionSequenceNode.h"
#include "Behaviors/Nodes/PostureNode.h"
#include "Behaviors/Nodes/SoundNode.h"
#include "Behaviors/Nodes/SpeechNode.h"

#if defined(TGT_HAS_LEGS) || defined(TGT_HAS_WHEELS)
#include "Behaviors/Nodes/WalkNode.h"
#endif

#include "DualCoding/DualCoding.h"
#include "DualCoding/VisualRoutinesStateNode.h"

#include "Behaviors/Transition.h"
#include "Behaviors/Transitions/CompareTrans.h"
#include "Behaviors/Transitions/CompletionTrans.h"
#include "Behaviors/Transitions/EventTrans.h"
#include "Behaviors/Transitions/NullTrans.h"
#include "Behaviors/Transitions/RandomTrans.h"
#include "Behaviors/Transitions/SignalTrans.h"
#include "Behaviors/Transitions/SmoothCompareTrans.h"
#include "Behaviors/Transitions/TextMsgTrans.h"
#include "Behaviors/Transitions/TimeOutTrans.h"
#include "Behaviors/Transitions/VisualTargetTrans.h"
#include <Behaviors/Transitions/TimeOutTrans.h>

#ifdef TARGET_HAS_IR
#include "Behaviors/Transitions/VisualTargetCloseTrans.h"
#endif


//************

#include <Motion/MotionManager.h>

#include <IPC/DriverMessaging.h>
#include <Motion/PlanarThreeLinkArm.h>
#include <Shared/fmatCore.h>

#include <string>
using namespace std;

typedef fmat::Row<RobotInfo::NumArmJoints> ArmConfiguration;

/*
Nodes Defined:

SetWrist
SetArm
SetJoint
SetGround
Twist
Sway
Rock
Turn
WalkSideways
WalkForwards
*/

extern MotionManager::MC_ID motionNodesWalkMC;

#shortnodeclass ResetInitialPlacement : WalkNode : doStart
	setMC(motionNodesWalkMC);
	motman->setPriority(motionNodesWalkMC, priority);
	getMC()->start();
	postStateCompletion();

#nodeclass SetWrist(float pitch=0, float roll=0, float grip=0): ArmNode
	#nodemethod doStart
		if(RobotInfo::NumArmJoints <= 3) {
			postStateCompletion();
			return;
		}
		cout<<"Setting wrist to ("<<pitch<<", "<<roll<<", "<<grip<<")"<<endl;
//		getMC()->takeSnapshot();
		getMC()->setWrist(pitch, roll, grip);
	#endnodemethod
#endnodeclass

#nodeclass SetArm(float shoulder=0, float elbow=0, float wrist=0): ArmNode
	void move(float s, float e, float w) {
		cout<<"Setting arm to ("<<s<<", "<<e<<", "<<w<<")"<<endl;
//		getMC()->takeSnapshot();
		getMC()->setJoints(s, e, w);
	}

	#nodemethod doStart
		if(const DataEvent<ArmConfiguration>* dev = dynamic_cast<const DataEvent<ArmConfiguration>*>(event)) {
			ArmConfiguration value = dev->getData();
			move(value[0], value[1], value[2]);
		} else {
			move(shoulder, elbow, wrist);
		}
	#endnodemethod
#endnodeclass

#nodeclass SetJoint(string jointName, float val = 0, float speed = 0.4): ArmNode
	void moveJoint(float value) {
		cout<<"Setting "<<jointName<<" to "<<val<<endl;
		unsigned int offset = 10;
		if(jointName == "ARM:shldr")
			offset = 0;
		else if(jointName == "ARM:elbow")
			offset = 1;
		else if(jointName == "ARM:wristYaw")
			offset = 2;
		else if(RobotInfo::NumArmJoints > 3) {
			if(jointName == "ARM:wristPitch")
				offset = 3;
			else if(jointName == "ARM:wristRoll" || jointName == "ARM:gripperLeft")
				offset = 4;
			else if(jointName == "ARM:gripper" || jointName == "ARM:gripperRight")
				offset = 5;
			else {
				postStateCompletion();
				return;
			}
		}
		else {
			postStateCompletion();
			return;
		}
		cout<<jointName<<" moving"<<endl;
		getMC()->takeSnapshot();
		getMC()->setMaxSpeed(offset, speed); 	 
		getMC()->setJointValue(offset, val);
	} 
	
	#nodemethod doStart
		if(const DataEvent<float>* dev = dynamic_cast<const DataEvent<float>*>(event)) {
			float value = dev->getData();
			moveJoint(value);
		} else {
			moveJoint(val);
		}
	#endnodemethod
#endnodeclass

#nodeclass SetServoPower(string jointName, bool val) : StateNode : doStart
/*	using namespace DriverMessaging;
	if(hasListener(ToggleServoPower::NAME)) {
		ToggleServoPower msg;
		msg.jointName = jointName;
		msg.holdPower = true;
		postMessage(msg);
	}
*/
#endnodeclass

#if TGT_HAS_LEGS

#nodeclass SetGround(float groundOffset = -10): WalkNode
	void setID() {
		if(motionNodesWalkMC == MotionManager::invalid_MC_ID){
			postStateCompletion();
			return;
		}
		setMC(motionNodesWalkMC);
		motman->setPriority(motionNodesWalkMC, priority);
	}
	
	void move(float offset) {
		cout<<"Setting ground plane to "<<offset<<endl;
		getMC()->groundPlane[3] = offset;
	}

	#nodemethod doStart
		setID();
		if(const DataEvent<float>* dev = dynamic_cast<const DataEvent<float>*>(event)) {
			float off = dev->getData();
			move(off);
		} else {
			move(groundOffset);
		}
	#endnodemethod
#endnodeclass

#nodeclass Twist(float angle = 0): WalkNode
	void setID() {
		if(motionNodesWalkMC == MotionManager::invalid_MC_ID){
			postStateCompletion();
			return;
		}
		setMC(motionNodesWalkMC);
		motman->setPriority(motionNodesWalkMC, priority);
	}
	
	void move(float howmuch) {
		cout<<"Twisting to "<<howmuch<<endl;
		getMC()->offsetA = howmuch;
	}

	#nodemethod doStart
		setID();
		if(const DataEvent<float>* dev = dynamic_cast<const DataEvent<float>*>(event)) {
			float off = dev->getData();
			move(off);
		} else {
			move(angle);
		}
	#endnodemethod
#endnodeclass

#nodeclass Sway(float offset = 0): WalkNode
	void setID() {
		if(motionNodesWalkMC == MotionManager::invalid_MC_ID){
			postStateCompletion();
			return;
		}
		setMC(motionNodesWalkMC);
		motman->setPriority(motionNodesWalkMC, priority);
	}
	
	void move(float howmuch) {
		cout<<"Swaying to "<<howmuch<<endl;
		getMC()->offsetY = howmuch;
	}

	#nodemethod doStart
		setID();
		if(const DataEvent<float>* dev = dynamic_cast<const DataEvent<float>*>(event)) {
			float off = dev->getData();
			move(off);
		} else {
			move(offset);
		}
	#endnodemethod
#endnodeclass

#nodeclass Rock(float offset = 0): WalkNode
	void setID() {
		if(motionNodesWalkMC == MotionManager::invalid_MC_ID){
			postStateCompletion();
			return;
		}
		setMC(motionNodesWalkMC);
		motman->setPriority(motionNodesWalkMC, priority);
	}
	
	void move(float howmuch) {
		cout<<"Rocking to "<<howmuch<<endl;
		getMC()->offsetX = howmuch;
	}

	#nodemethod doStart
		setID();
		if(const DataEvent<float>* dev = dynamic_cast<const DataEvent<float>*>(event)) {
			float off = dev->getData();
			move(off);
		} else {
			move(offset);
		}
	#endnodemethod
#endnodeclass

#nodeclass Turn(float myangle = 0): WalkNode
	void setID() {
		if(motionNodesWalkMC == MotionManager::invalid_MC_ID){
			postStateCompletion();
			return;
		}
		setMC(motionNodesWalkMC);
		motman->setPriority(motionNodesWalkMC, priority);
	}
	
	void move(float ang) {
		cout<<"Turning by "<<ang<<endl;
		getMC()->setTargetVelocity(0, 0, 3*M_PI/180);
		getMC()->setTargetDisplacement(0,0, ang);
	}

	#nodemethod doStart
		setID();
		if(const DataEvent<float>* dev = dynamic_cast<const DataEvent<float>*>(event)) {
			float val = dev->getData();
			move(val);
		} else {
			move(myangle);
		}
	#endnodemethod

#endnodeclass

#nodeclass WalkSideways(float distance = 0) : WalkNode
	void setID() {
		if(motionNodesWalkMC == MotionManager::invalid_MC_ID){
			postStateCompletion();
			return;
		}
		setMC(motionNodesWalkMC);
		motman->setPriority(motionNodesWalkMC, priority);
	}
	
	void move(float dist) {
		cout<<"Walking sideways "<<dist<<endl;
		getMC()->setTargetVelocity(0, 10, 0);
		getMC()->setTargetDisplacement(0, dist, 0);
	}

	#nodemethod doStart
		setID();
		if(const DataEvent<float>* dev = dynamic_cast<const DataEvent<float>*>(event)) {
			float dist = dev->getData();
			move(dist);
		} else {
			move(distance);
		}
	#endnodemethod
#endnodeclass

#nodeclass WalkForward(float distance = 0) : WalkNode
	void setID() {
		if(motionNodesWalkMC == MotionManager::invalid_MC_ID){
			postStateCompletion();
			return;
		}
		setMC(motionNodesWalkMC);
		motman->setPriority(motionNodesWalkMC, priority);
	}
	
	void move(float dist) {
		cout<<"Walking forward by "<<dist<<endl;
		getMC()->setTargetVelocity(10, 0, 0);
		getMC()->setTargetDisplacement(dist, 0, 0);
	}

	#nodemethod doStart
		setID();
		if(const DataEvent<float>* dev = dynamic_cast<const DataEvent<float>*>(event)) {
			float dist = dev->getData();
			move(dist);
		} else {
			move(distance);
		}
	#endnodemethod
#endnodeclass

#else	//TGT_HAS_LEGS

#shortnodeclass SetGround(float a=-10) : StateNode : doStart
	postStateCompletion();
	
#shortnodeclass Twist(float a=0) : StateNode : doStart
	postStateCompletion();
	
#shortnodeclass Sway(float a=0) : StateNode : doStart
	postStateCompletion();
	
#shortnodeclass Rock(float a=0) : StateNode : doStart
	postStateCompletion();
	
#shortnodeclass Turn(float a=0) : StateNode : doStart
	postStateCompletion();
	
#shortnodeclass WalkSideways(float a=0) : StateNode : doStart
	postStateCompletion();
	
#shortnodeclass WalkForwards(float a=0) : StateNode : doStart
	postStateCompletion();
	
#endif //TGT_HAS_LEGS

#endif	//TGT_HAS_ARMS

#endif