#include "Shared/RobotInfo.h"
#include "Behaviors/StateMachine.h"
#include <math.h>

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

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

$nodeclass GripperPointEllipse : VisualRoutinesStateNode {

	//x and y value of the ellipse
	$provide float xOrigin;
	$provide float yOrigin;

	//Get ellipse from mapbuildernode
	$nodeclass FindEllipse : MapBuilderNode(MapBuilderRequest::localMap) : doStart {
		mapreq.addObjectColor(ellipseDataType, "red");
	}

	//Map out position of the ellipse and store values in $provide
	$nodeclass ExamineResult : VisualRoutinesStateNode : doStart {

		//Get the ellipse shape in cam
		NEW_SHAPEVEC(ellipse_shapes, EllipseData, select_type<EllipseData>(localShS));
		if(ellipse_shapes.size() > 0) {
			EllipseData myEllipse = ellipse_shapes[0];
			$reference GripperPointEllipse::xOrigin;
			$reference GripperPointEllipse::yOrigin;
			xOrigin = myEllipse.getCentroid().coordX();
			yOrigin = myEllipse.getCentroid().coordY();
			cout << xOrigin << endl;
			cout << yOrigin << endl;
		} else {
			cout << "No ellipse is found in view!" << endl;
		}
		postStateCompletion();
	}

	//Make the arm track the camera
	$nodeclass ArmMover : PostureNode : doStart {

		//Calculate the angles for the arm of the robot & position
		$reference GripperPointEllipse::xOrigin;
		$reference GripperPointEllipse::yOrigin;
		float curPan = state->outputs[HeadOffset+PanOffset];
		float maxTurn = 0.5;
		float fractionTurn = curPan/maxTurn;
		float radTurn = (M_PI/2) * fractionTurn;
		if (radTurn > M_PI/2) radTurn = M_PI/2;
		else if (radTurn < -M_PI/2) radTurn = -M_PI/2;
		float xPos = (xOrigin-100) + 100*(1 - cos(radTurn));
		float yPos = (yOrigin) + 100 * (sin(radTurn));

		//Get the arm to move to the correct position in field
	 	fmat::Column<3> targetBase = fmat::pack(xPos,yPos,0);
		fmat::Quaternion oriTgt = fmat::Quaternion::aboutZ(-radTurn);
		unsigned int link = GripperFrameOffset;
		fmat::Quaternion oriOffset = fmat::Quaternion::identity();
		fmat::Column<3> noOffset = fmat::pack(0, 0, 0);
		getMC()->solveLink(targetBase, oriTgt, link, noOffset, oriOffset);
	}

	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: PostureNode("beginPos.pos") =C=> FindEllipse =C=> ExamineResult =C=> armmove: ArmMover[setMC(posturemc)] =E(sensorEGID)=> armmove
		}
	}
}
REGISTER_BEHAVIOR(GripperPointEllipse);
#endif