#include "Shared/RobotInfo.h"
#include "Behaviors/StateMachine.h"

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

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

$nodeclass HoldingHands2 : StateNode {
	$provide int numWest = 0;
	$provide int numEast = 0;

	//extends the arm for guiding
	$nodeclass ExtendArm : PostureNode : doStart {
		getMC()->setOutputCmd(ArmBaseOffset,-0.104924);
		getMC()->setOutputCmd(ArmShoulderOffset,2.4235);
		getMC()->setOutputCmd(ArmElbowOffset,-2.35696);
		getMC()->setOutputCmd(ArmWristOffset, 0.0998061);
		getMC()->setOutputCmd(WristRotateOffset,-1.55339);
		getMC()->setOutputCmd(LeftFingerOffset, -0.125398);
		getMC()->setOutputCmd(RightFingerOffset, 0.284064);
	}
	
	$nodeclass WalkRobot : WalkNode {
		float maxSpeed; //the highest allowable speed in degrees/s
		float xSpeed; // the current walking speed of the robot in x
		float ySpeed; // the current walking speed of the robot in y
		float startX; //the x before displacement
		float startY; //the y before displacement
		float xThresh; //the most possible displacement in x direction
		float yThresh; //the most possible displacement in y direction
		float maxXVel;

		virtual void doStart() {
			maxSpeed=45;
			startX = 389.856;
			startY = -106.545;
			xThresh= 5; //any change more than this results in full speed
			yThresh= 8;
			maxXVel = 200; 
			erouter->addListener(this, EventBase::sensorEGID);
		}

		virtual void doEvent() {
			fmat::Transform targetGripper = kine->linkToBase(GripperFrameOffset);
			fmat::Column<3> gripperPos = targetGripper.translation();
			float xDisplacement = abs(startX-gripperPos[0]);
			float yDisplacement = abs(startY-gripperPos[1]);
			float xPercentage = (xDisplacement/xThresh);
			float yPercentage = (yDisplacement/yThresh);
			float xVel = xPercentage*maxXVel;//xPercentage*maxSpeed;
			float yVel = deg2rad(yPercentage * maxSpeed);

			//set yVel to max if displacement greater than threshold
			if (yVel > deg2rad(maxSpeed)) {
				yVel = deg2rad(maxSpeed);			
			}
			//same with xVel
			if ((xVel > maxXVel)) {
				xVel = maxXVel;			
			}

			//make neg as necessary
			if (gripperPos[1]<startY) {
				yVel *= -1;
			}
			if (gripperPos[0]<startX) {
				xVel *= -1;
			}
			std::cout << "X Percent: " << xPercentage << "Y Percent: " << yPercentage; 
			//Set speeed to 0 if they have barely moved			
			if (xPercentage < .2) {
				xVel = 0;
			}
			if (yPercentage < .2) {
				yVel = 0;
			}

			std::cout << "MovingX: " << xVel  << "MovingY" << yVel << endl; 
			//determine what type of move the user wanted by whether x or y were 
			// relatively higher
			if (xPercentage > yPercentage) { //move in x
				getMC()->setTargetVelocity(xVel,0,0);
			}
			else if (yPercentage > xPercentage){ //move in y
		       		getMC()->setTargetVelocity(0,0,yVel);
			}
		}

	}

	virtual void setup() {
		//set max joint speed for arm movement (when extending to user)
		float armMaxSpeed = deg2rad((float)45); 
		MotionPtr<PostureMC>()->setMaxSpeed(ArmOffset,armMaxSpeed);
		MotionManager::MC_ID posturemc = addMotion(MotionPtr<PostureMC>());

		$statemachine {
			startNode : StateNode
			walkRobot : WalkRobot
			extendArm : ExtendArm[setMC(posturemc)]
			extendArmAgain : ExtendArm[setMC(posturemc)]
			startNode =N=> 
				extendArm =C=> 
					{walkRobot, extendArmAgain}
		}
	}
}

REGISTER_BEHAVIOR(HoldingHands2);
#endif