#include "Behaviors/StateMachine.h"
#include "Behaviors/Mon/ArmController.h"
#include "Behaviors/Controller.h"

#include "Motion/MMAccessor.h"
#include "Motion/PIDMC.h"
#include "Shared/RobotInfo.h"
#include "Motion/ArmMC.h"
#include "Behaviors/Nodes/PostureNode.h"
#include "Motion/MotionPtr.h"


$nodeclass SolvingForOrientation : VisualRoutinesStateNode {
	$provide Point pt;
	
	$nodeclass FindEllipse : MapBuilderNode(MapBuilderRequest::worldMap) : doStart {
		mapreq.addObjectColor(ellipseDataType, "red");
		cout << "found ellipses" << endl;
	}
	
	
	
	$nodeclass Extract : VisualRoutinesStateNode : doStart {
		$reference SolvingForOrientation::pt;
		NEW_SHAPEVEC(ellipseHolder, EllipseData, select_type<EllipseData>(worldShS));
		
		if (ellipseHolder.size() < 1) {
			cout << "uh oh, no ellipses" << endl;
			return;
		}
		
		//else ellipses found
		
		pt = ellipseHolder[0]->getCentroid();
		cout << "ellipse: (" << pt << ")" << endl;
		
	}
	
	
	
	
	$nodeclass TrackEllipse : PostureNode : doStart {
		$reference SolvingForOrientation::pt;
		
		float pan = state->outputs[HeadOffset + PanOffset];
		
		fmat::Quaternion originalTarget = fmat::Quaternion::aboutZ(pan * M_PI/2);
		fmat::Column<3> target = fmat::pack(pt.coordX(), pt.coordY(), 16);
		fmat::Column<3> pOffset = fmat::pack(-100,0,0);
		fmat::Quaternion oOffset = fmat::Quaternion::identity();
		bool possible = getMC()->solveLink(target, originalTarget, GripperFrameOffset, pOffset, oOffset);
		
		cout << "end of track ellipse, completed? " << possible << endl;
		
	}
	
		

  $setupmachine{
     	PostureNode("start.pos") =C=> find
     	find : FindEllipse =C=> extractEllipse
     	extractEllipse : Extract =T(2000)=> trackEl
     	trackEl : TrackEllipse =C=> trackEl
  }

}

REGISTER_BEHAVIOR(SolvingForOrientation);