#include "Behaviors/StateMachine.h"

  $nodeclass TurnAndCheck : VisualRoutinesStateNode {

	$nodeclass MoveArm : DynamicMotionSequenceNode : doStart {
		cout << "moving arm" << endl;
		PostureEngine pe;
		fmat::Column<3> init_pos = kine->getPosition(GripperFrameOffset);
		fmat::Column<3> offset = fmat::pack(0, -100, -30);
		fmat::Column<3> new_pos = init_pos + offset;

		cout << "solving for arm position" << endl;
		cout << "ArmBase = " << pe.getOutputCmd(ArmBaseOffset).value << endl;
		pe.solveLinkPosition(new_pos, GripperFrameOffset, fmat::pack(0, 0, 0));
		cout << "After moving, ArmBase = " << pe.getOutputCmd(ArmBaseOffset).value << endl;
		cout << "After moving, ArmBase weight = " << pe.getOutputCmd(ArmBaseOffset).weight << endl;

		cout << "moving arm" << endl;
		getMC()->advanceTime(500);
		getMC()->setSpeed(0.4);
		getMC()->setPose(pe);
	}

	$nodeclass FindAprilTag : MapBuilderNode(MapBuilderRequest::cameraMap) : doStart {
		mapreq.setAprilTagFamily();
	}

	$nodeclass ConfirmAprilTag : StateNode : doStart {
		NEW_SHAPEVEC(april_tags, AprilTagData, select_type<AprilTagData>(camShS));
		if (april_tags.size() == 0) {
			postStateFailure();
			return;
		}

		Point p(camSkS.getWidth()/2, camSkS.getHeight()/2);
		cout << "center of cam: " << p << endl;
		cout << "center of tag: " << april_tags[0]->getCentroid() << endl;

		if ((p - april_tags[0]->getCentroid()).xyNorm() / camSkS.getWidth() > 0.1)
			postStateFailure();
		else
			postStateSuccess();
		return;
	}
		
	$setupmachine {
		MoveArm =C=>
		LookAtGripper =C=>
		FindAprilTag =C=>
		confirm

		confirm: ConfirmAprilTag

		confirm =S=> SpeechNode("gripping shape")
		confirm =F=> SpeechNode("Failed to grip shape")
	}
  }