#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"

#include <stdio.h>

using namespace std;

$nodeclass HoldingHands : StateNode {

  $provide fmat::Column<3> globalGripperPos;
  $provide float dist;
  $provide fmat::Column<3> diffVector;
  $provide float SAFETY;

  $nodeclass Handler : PostureNode : doStart {

    $reference HoldingHands::globalGripperPos;
	$reference HoldingHands::dist;
	$reference HoldingHands::diffVector;
    fmat::Transform gripper =
    kine->linkToBase(GripperFrameOffset);
    fmat::Column<3> gripperPos = gripper.translation();
    dist = (globalGripperPos-gripperPos).norm();
    diffVector = (globalGripperPos-gripperPos);
	if(dist >= 15)
		dist = 15;
    cout << "dir: " << diffVector << "mag:" << dist << endl;
  }

  $nodeclass WalkRun : WalkNode : doStart {
	$reference HoldingHands::dist;
	$reference HoldingHands::diffVector;
	$reference HoldingHands::SAFETY;
	SAFETY = 10;
	cout << "speed: " << getMC()->getTargetVelocity() << endl;
	if(dist > 10)
	{
  		fmat::Column<3> newVector = diffVector / dist;
		float diffFor = (newVector[0] - newVector[2]) * 180 / sqrt(2) / 2 / SAFETY;
		float diffAng = newVector[1] * 1.8 / SAFETY;
//		cout << "x: " << diffVector[0] << "y: " << diffVector[1] << "z: " << diffVector[2] << endl;
		getMC()->setTargetVelocity(-diffFor, 0, -diffAng);
		cout << "updated speed: " << getMC()->getTargetVelocity() << endl;
	}
	postStateCompletion();
  }

  $nodeclass StraightArm : PostureNode : doStart {
        getMC()->setOutputCmd(ArmBaseOffset, OutputCmd(-0.1971));
        getMC()->setOutputCmd(ArmShoulderOffset, OutputCmd(0.9341));
        getMC()->setOutputCmd(ArmElbowOffset, OutputCmd(0.0435));
        getMC()->setOutputCmd(ArmWristOffset, OutputCmd(0.1356));
        postStateCompletion();
  }


  $nodeclass MeasurePos : PostureNode : doStart {
        fmat::Transform gripper =
            kine->linkToBase(GripperFrameOffset);
        $reference HoldingHands::globalGripperPos;
        globalGripperPos = gripper.translation();
  }

  $nodeclass ArmMover : PostureNode : doStart {

    fmat::Transform leftFinger =
    kine->linkToBase(LeftFingerFrameOffset);
    fmat::Column<3> leftFingerPos = leftFinger.translation();
    fmat::Transform camerabase =
    kine->linkToBase(CameraFrameOffset);
    fmat::Column<3> camerabasePos = camerabase.translation();
    float dist = (leftFingerPos-camerabasePos).norm();

    if (dist > 200) {
        if (getMC()->getMaxSpeed(ArmOffset) == 0 ) {
            cout << "changed == 0" << endl;
            for (uint i = ArmOffset; i < LeftFingerFrameOffset; i++) {
                getMC()->setMaxSpeed(i, 0.3);
            }
        }
    } else {
        if (getMC()->getMaxSpeed(ArmOffset) != 0) {
            cout << "changed != 0" << endl;
            for (uint i = ArmOffset; i < LeftFingerFrameOffset; i++) {
                getMC()->setMaxSpeed(i, 0);
            }
       }
    }

    fmat::Column<3> targetCam = fmat::pack(0, 0, 100);
    fmat::Column<3> targetBase = kine->linkToBase(CameraFrameOffset)
        * targetCam;
        fmat::Column<3> noOffset = fmat::pack(0, 0, 0);
#ifdef TGT_IS_CALLIOPE5
        unsigned int link = LeftFingerFrameOffset;
#else
        unsigned int link = GripperFrameOffset;
#endif

        //getMC()->solveLinkPosition(targetBase, link, noOffset);
  }

  virtual void setup() {
        // We make the posture motion command here and supply it to the
        // PostureNode via setMC() so that it only has to be registered
        // with the MotionManager once. Registering and deregistering a
        // PostureMC inside a tight sensor loop is inefficient and leads
        // to crashes.
        MotionManager::MC_ID posturemc = addMotion(MotionPtr<PostureMC>());

    $statemachine{
       startnode: StateNode =N=> StraightArm =T(1000)=> MeasurePos =T(1000)=> handler
       handler: Handler[setMC(posturemc)] =E(sensorEGID)=> mover
	   mover: WalkRun =T(1000)=> handler
    }
  }

}

REGISTER_BEHAVIOR(HoldingHands);

#endif