#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{
	$provide MotionManager::MC_ID headmc;
	$provide MotionManager::MC_ID armmc;	
	//general command handler
	$nodeclass MyAction : StateNode{
		enum choice {headUp, headDown, headLeft, headRight, parkArm, extendArm, closeGripper, openGripper, turnWrist};
		$provide float rad;
		$provide std::string myCommandLine;
				
		//$provide std::string myCmd;
		//command parser
		$nodeclass MyParse : StateNode: doStart{
			//const TextMsgEvent *txtev = dynamic_cast<const TextMsgEvent*>(event);
			
				//std::string myCommandLine = txtev->getText();
				$reference MyAction::rad;
				$reference MyAction::myCommandLine;
				myCommandLine = getAncestor<PilotDemo>()->commandLine;
				rad = 0.0;
				std::istringstream is(myCommandLine);
				string cmd;
				is >> cmd >> rad;
				//cout<< cmd<<rad<<endl;
				if(cmd == "headup"){
					postStateSignal<choice>(headUp);
				}else if(cmd == "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();
				}

			
			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);
		}
		
		//say the command out
		$nodeclass CmdSpeaker : SpeechNode : doStart{
			std::string &commandLine = getAncestor<PilotDemo>()->commandLine;
			setText(commandLine);
			setShowText(true);
			preStart();
		}	
		//setup
		virtual void setup(){
			$reference MyPilotDemo::headmc;
			$reference MyPilotDemo::armmc;
			//MotionManager::MC_ID armmc = addMotion(MotionPtr<ArmMC>());			
		$statemachine{
			
			parse: MyParse
			say: CmdSpeaker
			parse=F=>SpeechNode("invalid action")=C=>PostMachineCompletion
			//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=>say=C=>PostMachineCompletion
			parse=S<choice>(headDown)=>headdown=C=>say=C=>PostMachineCompletion
			parse=S<choice>(headLeft)=>headleft=C=>say=C=>PostMachineCompletion
			parse=S<choice>(headRight)=>headright=C=>say=C=>PostMachineCompletion
			parse=S<choice>(turnWrist)=>turnwrist=C=>say=C=>PostMachineCompletion
			parse=S<choice>(closeGripper)=>closegripper=C=>say=C=>PostMachineCompletion
			parse=S<choice>(openGripper)=>opengripper=C=>say=C=>PostMachineCompletion
			parse=S<choice>(parkArm)=>parkarm=C=>say=C=>PostMachineCompletion
			parse=S<choice>(extendArm)=>extendarm=C=>say=C=>PostMachineCompletion

		}
		}
	}
	
	//
	$nodeclass MySpeaker: StateNode{
		$nodeclass SayCommand: SpeechNode: doStart{
			std::string &commandLine = getAncestor<PilotDemo>()->commandLine;
			//cout << "command: "<<commandLine << endl;
			std::string msg = commandLine.substr(4);
			setText(msg);
			setShowText(true);
			preStart();
		}
		$setupmachine{
			myStart: SayCommand=C=>PostMachineCompletion
		}
	}
	

	$nodeclass MyMotion : StateNode{
		$nodeclass SayCommand: SpeechNode: doStart{
			std::string &commandLine = getAncestor<PilotDemo>()->commandLine;
			setText(commandLine);
			setShowText(true);
			preStart();
		}
		$setupmachine{
			SayCommand=C=>Motion=C=>PostMachineCompletion
		}
	}

	virtual void registerUserCommands() {
		headmc = addMotion(MotionPtr<HeadPointerMC>());
		armmc = addMotion(MotionPtr<ArmMC>());	
		//cout<<this->commandLine<<endl;
		$statemachine{
			myAction : MyAction
			myMotion : MyMotion
		}
   		addCommand("f", myMotion);
		addCommand("b", myMotion);
    		addCommand("F", myMotion);
    		addCommand("B", myMotion);
    		addCommand("fwd", myMotion);

    		addCommand("l", myMotion);
   		 addCommand("r", myMotion);
    		addCommand("L", myMotion);
    		addCommand("R", myMotion);
    		addCommand("turn", myMotion);

		addCommand("headup", myAction, "headup <rad>	tilt head up for <rad>");
		addCommand("headdown", myAction, "headdown <rad>	tilt head down for <rad>");
		addCommand("headleft", myAction, "headleft <rad>	tilt head left for <rad>");
		addCommand("headright", myAction, "headright <rad>	tilt head right for <rad>");
		addCommand("opengripper", myAction, "opengripper	open the gripper");
		addCommand("closegripper", myAction, "closegripper	close the gripper");
		addCommand("parkarm", myAction, "parkarm	fold the arm to its parking position");
		addCommand("extendarm", myAction, "extendarm	stretch the arm out");
		addCommand("turnwrist", myAction, "turnwrist <rad>	rotate the wrist for <rad>, positive for clockwise");
	}
}

REGISTER_BEHAVIOR(MyPilotDemo);