#include "Shared/RobotInfo.h"

#include "Behaviors/StateMachine.h"

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

$nodeclass SolveOrientation : VisualRoutinesStateNode {

	$nodeclass FindStuff : MapBuilderNode(MapBuilderRequest::localMap) : doStart {
		mapreq.addObjectColor(ellipseDataType, "red");
		mapreq.pursueShapes = true;
	}

	$nodeclass ArmMover : PostureNode : doStart {
		NEW_SHAPEVEC(ellipses_shapes, EllipseData, select_type<EllipseData>(localShS));
		if (ellipses_shapes.size() == 0)
		{
			cout << "No ellipses found" << endl;
		}
		else
		{
			Point center = ellipses_shapes[0]->getCentroid();
			fmat::Column<3> centeroffset = fmat::pack(200, 0, 0);
			fmat::Column<3> target = center.getCoords() - centeroffset;
			fmat::Quaternion oriTgt = fmat::Quaternion::identity();
			fmat::Column<3> noOffset = fmat::pack(0, 0, 0);
			unsigned int link = GripperFrameOffset;
			getMC()->solveLink(target, oriTgt, link, noOffset, oriTgt);
		}
	}

	$nodeclass ArmRotator : PostureNode : doStart {
		SharedObject<HeadPointerMC> head_mc;
    		float angle = head_mc->getJointValue(RobotInfo::PanOffset);
                NEW_SHAPEVEC(ellipses_shapes, EllipseData, select_type<EllipseData>(localShS));
                if (ellipses_shapes.size() == 0)
                {
                        cout << "No ellipses found" << endl;
                }
		else
		{
			Point center = ellipses_shapes[0]->getCentroid();
                        fmat::Column<3> centeroffset = fmat::pack(200*cos(angle), 200*sin(angle), 0);
                        fmat::Column<3> target = center.getCoords() - centeroffset;
                        fmat::Quaternion oriTgt = fmat::Quaternion::aboutZ(angle);
                        fmat::Column<3> noOffset = fmat::pack(0, 0, 0);
			fmat::Quaternion idTgt = fmat::Quaternion::identity();
                        unsigned int link = GripperFrameOffset;
                        getMC()->solveLink(target, oriTgt, link, noOffset, idTgt);

		}
	}

	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{
			initnode: FindStuff =C=> ArmMover =C=> movenode
 			movenode: ArmRotator[setMC(posturemc)] =E(sensorEGID)=> movenode
		}
	}

}

REGISTER_BEHAVIOR(SolveOrientation);