#include "Shared/RobotInfo.h"


#if defined(TGT_HAS_ARMS) && defined(TGT_HAS_HEAD)
#include "Behaviors/StateMachine.h"

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


#endif

fmat::Column<3> expectedPos;
float xThresh;
float yThresh;

$nodeclass HoldHands : StateNode {

	$nodeclass ExtendArm : PostureNode : doStart {

		//set arm initially to be extend
		
 		getMC()->setMaxSpeed(ArmOffset, 0.25);
                getMC()->setMaxSpeed(ArmElbowOffset, 0.25);
                getMC()->setMaxSpeed(ArmShoulderOffset, 0.25);
                getMC()->setMaxSpeed(ArmWristOffset, 0.25);
		getMC()->setOutputCmd(ArmWristOffset, OutputCmd(1.128 ));
		getMC()->setOutputCmd(ArmElbowOffset, OutputCmd(-2.126 ));
		getMC()->setOutputCmd(ArmBaseOffset, OutputCmd(-.0161));
		getMC()->setOutputCmd(WristRotateOffset , OutputCmd (-1.625));
		getMC()->setOutputCmd(ArmShoulderOffset , OutputCmd(2.177));

	}


	$nodeclass SetExpected : StateNode : doStart {
		
		expectedPos = kine->linkToBase(ArmWristOffset).translation();
		postStateCompletion();		
	}

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

		}

		virtual void doEvent() {
		
		//get the actual postition
		
		fmat::Column<3> actualPos = kine->linkToBase(ArmWristOffset).translation();

		//get the difference and calculate a vector to that position
		
		if ( fabsf(actualPos[0] - expectedPos[0])  > xThresh || fabsf(actualPos[1] - expectedPos[1]) > yThresh) { 

			//tell the robot to move towards that vector
			float xDist = expectedPos[0] - actualPos[0] ;
			float aDist = -((expectedPos[1] - actualPos[1])/3.14);

			if (aDist > 1.0/3.14 )
				aDist = 1.0/3.14;

			if(aDist < -(1.0/3.14))
				aDist = -1.0/3.14;
			
			float yDist = 0;
			getMC()->setTargetVelocity(xDist, yDist, aDist);
			cout << expectedPos[0] - actualPos[0] << endl;
			cout << expectedPos[1] - actualPos[1] << endl;
			
		}

		else {
			getMC()->setTargetVelocity(0,0,0);
		}

	}

	}

		 virtual void setup() {

                MotionManager::MC_ID posturemc = addMotion(MotionPtr<PostureMC>());
		xThresh = 1;
		yThresh = 10;
                $statemachine {

                        startnode : ExtendArm[setMC(posturemc)] =C=> SetExpected =C=>  follow : FollowDrag

                }

        }

}

REGISTER_BEHAVIOR(HoldHands);