#include "Behaviors/Demos/Navigation/PilotDemo.h"
#include "Behaviors/Nodes/HeadPointerNode.h"
#include "Motion/MotionPtr.h"
#include "Motion/Kinematics.h"
#include "Shared/RobotInfo.h"
$nodeclass MyPilotDemo : PilotDemo{
	//general command handler
	$nodeclass MyAction : StateNode{
		enum choice {headUp, headDown, headLeft, headRight, parkArm, extendArm, closeGripper, openGripper, turnWrist};
		$provide float rad;
		//command parser
		$nodeclass MyParse : StateNode: doStart{
			const TextMsgEvent *txtev = dynamic_cast<const TextMsgEvent*>(event);
			if(txtev!=NULL){
				std::string myCommandLine = txtev->getText();
				$reference MyAction::rad;
				rad = 0.0;
				std::istringstream is(myCommandLine);
				string cmd;
				is >> cmd >> rad;
				//cout<< cmd<<rad<<endl;
				if(cmd == "headup"){
					//headup
					cout<<"choose head up"<<endl;
					//postStateSignal<int>(1);					
					postStateSignal<choice>(headUp);
				}else if(cmd == "headdown"){
					//headdown
					postStateSignal<choice>(headDown);
				}else if(cmd == "headleft"){
					postStateSignal<choice>(headLeft);
				}else if(cmd == "headright"){
					postStateSignal<choice>(headRight);
				}else if(cmd == "parkarm"){
					postStateSignal<choice>(parkArm);
				}else if(cmd == "extendarm"){
					postStateSignal<choice>(extendArm);
				}else if(cmd == "closegripper"){
					postStateSignal<choice>(closeGripper);
				}else if(cmd == "opengripper"){
					postStateSignal<choice>(openGripper);
				}else if(cmd == "turnwrist"){
					postStateSignal<choice>(turnWrist);
				}else{
					postStateFailure();
				}

			}else{
				cout<<"nnnuuullll"<<endl;
			}
			postStateCompletion();
		}
		//****head actions
		//move head up action-radian
		$nodeclass HeadUpMover : HeadPointerNode : doStart{
			$reference MyAction::rad;
			unsigned int jointInd = NodOffset;
			cout<<"do head up"<<endl;
			float curRad = getMC()->getJointValue(NodOffset);
			cout<<"curRad="<<curRad<<endl;
			//getMC()->lookInDirection(x, y, z);			
			getMC()->setJointValue(jointInd, curRad+rad);

		}
		//move head down action
		$nodeclass HeadDownMover : HeadPointerNode : doStart{
			$reference MyAction::rad;
			unsigned int jointInd = NodOffset;
			float curRad = getMC()->getJointValue(jointInd);
			cout<<"curRad="<<curRad<<endl;
			getMC()->setJointValue(jointInd, curRad-rad);
		}
		//move head left action
		$nodeclass HeadLeftMover : HeadPointerNode : doStart{
			$reference MyAction::rad;
			unsigned int jointInd = PanOffset;
			float curRad = getMC()->getJointValue(jointInd);
			cout<<"curRad="<<curRad<<endl;
			getMC()->setJointValue(jointInd, curRad+rad);
		}
		//move head right action
		$nodeclass HeadRightMover : HeadPointerNode : doStart{
			$reference MyAction::rad;
			unsigned int jointInd = PanOffset;
			float curRad = getMC()->getJointValue(jointInd);
			cout<<"curRad="<<curRad<<endl;
			getMC()->setJointValue(jointInd, curRad-rad);
		}
		//****arm actions
		//turn wrist
		$nodeclass TurnWristMover : ArmNode : doStart{
			$reference MyAction::rad;
			unsigned int jointInd = WristRotateOffset;
			float curRad = getMC()->getJointValue(jointInd);
			cout<<"curRad="<<curRad<<endl;
			getMC()->setJointValue(jointInd, curRad+rad);
		}
		//close gripper
		$nodeclass CloseGripperMover : ArmNode : doStart{
			getMC()->setJointValue(LeftFingerOffset, 0.44);
			getMC()->setJointValue(RightFingerOffset, -0.44);
		}	
		//open gripper
		$nodeclass OpenGripperMover : ArmNode : doStart{
			getMC()->setJointValue(LeftFingerOffset, -1.22);
			getMC()->setJointValue(RightFingerOffset, 1.22);
		}
		//park arm
		$nodeclass ParkArmMover : ArmNode : doStart{
			getMC()->setMaxSpeed(0.5);
			getMC()->setJointValue(0, 1.4);
			getMC()->setJointValue(1, 2.45);
			getMC()->setJointValue(2, -2.36);
			getMC()->setJointValue(3, 1.96);
			getMC()->setJointValue(WristRotateOffset,0);
			getMC()->setJointValue(LeftFingerOffset, 0.44);
			getMC()->setJointValue(RightFingerOffset, -0.44);
		}	
		//extend arm
		$nodeclass ExtendArmMover : ArmNode : doStart{
			getMC()->setMaxSpeed(0.5);
			getMC()->setJointValue(0, 0);
			getMC()->setJointValue(1, 0);
			getMC()->setJointValue(2, 0);
			getMC()->setJointValue(3, 0);
			getMC()->setJointValue(WristRotateOffset,0);
			getMC()->setJointValue(LeftFingerOffset, 0.44);
			getMC()->setJointValue(RightFingerOffset, -0.44);
		}	
		//setup
		virtual void setup(){
			MotionManager::MC_ID headmc = addMotion(MotionPtr<HeadPointerMC>());
			MotionManager::MC_ID armmc = addMotion(MotionPtr<ArmMC>());			
		$statemachine{
			myStart: SpeechNode("start my actions")=C=>wait
			wait: StateNode
			parse: MyParse
			wait=TM("end")=>SpeechNode("end of my command")=C=>PostMachineCompletion
			wait=TM=>parse
			parse=F=>SpeechNode("failed action")=C=>wait
			//define action nodes
			//****head actions
			headup: HeadUpMover[setMC(headmc)]
			headdown: HeadDownMover[setMC(headmc)]
			headleft: HeadLeftMover[setMC(headmc)]			
			headright: HeadRightMover[setMC(headmc)]
			//****arm actions
			turnwrist: TurnWristMover[setMC(armmc)]
			closegripper: CloseGripperMover[setMC(armmc)]
			opengripper: OpenGripperMover[setMC(armmc)]
			parkarm: ParkArmMover[setMC(armmc)]
			extendarm: ExtendArmMover[setMC(armmc)]
			//N-way choice
			parse=S<choice>(headUp)=>headup=C=>wait
			parse=S<choice>(headDown)=>headdown=C=>wait
			parse=S<choice>(headLeft)=>headleft=C=>wait
			parse=S<choice>(headRight)=>headright=C=>wait
			parse=S<choice>(turnWrist)=>turnwrist=C=>wait
			parse=S<choice>(closeGripper)=>closegripper=C=>wait
			parse=S<choice>(openGripper)=>opengripper=C=>wait
			parse=S<choice>(parkArm)=>parkarm=C=>wait
			parse=S<choice>(extendArm)=>extendarm=C=>wait
			//wait=TM("head")=>SpeechNode("hhheaduppp")=C=>wait			
		}
		}
	}

	virtual void registerUserCommands() {
		//cout<<this->commandLine<<endl;
		$statemachine{
			myAction : MyAction
		}
		addCommand("my", myAction, "listen for my command");
	}
}

REGISTER_BEHAVIOR(MyPilotDemo);