#include "Shared/RobotInfo.h"

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

#include "Behaviors/Nodes/PostureNode.h"
#include "Behaviors/Transitions/EventTrans.h"
#include "Motion/MotionPtr.h"
#include <cmath>
using namespace std;

$nodeclass GripperTrackCamera2 : StateNode {

  $nodeclass FindObjects : MapBuilderNode(MapBuilderRequest::localMap) : doStart {
	mapreq.addObjectColor(ellipseDataType, "red");
  }

  $nodeclass ArmMover : PostureNode : doStart {
	NEW_SHAPEVEC(ellipses, EllipseData, select_type<EllipseData>(camShS));
	if (ellipses.empty()) return;
	Point center = ellipses[0]->getCentroid();

    fmat::Column<3> targetCam = fmat::pack(0, 0, 300);
    fmat::Column<3> targetBase = kine->linkToBase(CameraFrameOffset) * targetCam;
	fmat::Column<3> noOffset = fmat::pack(0, 0, 0);
	unsigned int link = LeftFingerFrameOffset;

	fmat::Column<3> diff = targetBase - (kine->linkToBase(link).translation());
	int dist = sqrt(pow(diff[0], 2) + pow(diff[1], 2) + pow(diff[2], 2));

	cout << "dist is " << dist << endl;

	if (dist > 150)
		for (uint i = ArmBaseOffset; i < ArmBaseOffset + NumArmJoints; i++)
			getMC()->setMaxSpeed(i, .3);

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

	if (dist > 150)
		for (uint i = ArmBaseOffset; i < ArmBaseOffset + NumArmJoints; i++)
			getMC()->setMaxSpeed(i, 1);
  }

  virtual void setup() {
		MotionManager::MC_ID posturemc = addMotion(MotionPtr<PostureMC>());

    $statemachine{
  		startnode: ArmMover[setMC(posturemc)] =E(sensorEGID)=> startnode
    }
  }

}

REGISTER_BEHAVIOR_MENU(GripperTrackCamera2,DEFAULT_TK_MENU"/Kinematics Demos");

#endif