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

$nodeclass Lab7p5 {

	$provide Point p;

	//Tilt Camera Down
	$nodeclass OrientHead : PostureNode : doStart {
		float startPos = -1;
		getMC()->setOutputCmd(HeadTiltOffset, startPos);

	}

	//Find ellipses on map
	$nodeclass FindEllipses : MapBuilderNode : doStart {
		mapreq.addAllObjectColors(ellipseDataType);
	}

	//sets the center location of the ellipse
	$nodeclass GetCenter : StateNode : doStart {
		$reference Lab7p5::p;
		NEW_SHAPEVEC(ellipse_shapes, EllipseData, select_type<EllipseData>(camShS));
		int const numEllipses = ellipse_shapes.size();
		if (numEllipses == 0 ) {
			cout << "No ellipse found." << endl;
			exit(1);
		}
		else if (numEllipses > 1) {
			cout << "Only one ellipse can be in view." << endl; 
			exit(1);		
		}
		p =  ellipse_shapes[0]->getCentroid();
		cout << "Set ellipse center point." << endl;
		postStateCompletion();
	}
		

	$nodeclass PreMovement : PostureNode : doStart {
		$reference Lab7p5::p;
		fmat::Column<3> noOffset = fmat::pack(0,0,0);
		fmat::Quaternion oriTarget = fmat::Quaternion::identity();
		//first travel to right of cyllinder to avoid hitting it
		fmat::Column<3> rightOffset = fmat::pack(0,100,0);
		fmat::Column<3> tempPos = p.getCoords() - rightOffset;
		cout << "Making temp move" << endl;
		getMC()->solveLink(tempPos,oriTarget,GripperFrameOffset,noOffset,oriTarget);	

	}
	$nodeclass InitialMovement : PostureNode : doStart {
		$reference Lab7p5::p;
		fmat::Column<3> initialOffset = fmat::pack(100,0,0);
		fmat::Column<3> targetPos = p.getCoords() - initialOffset;
		fmat::Column<3> noOffset = fmat::pack(0,0,0);
		fmat::Quaternion oriTarget = fmat::Quaternion::identity();
		cout << "Making initial Movement" << endl;
		getMC()->solveLink(targetPos,oriTarget,GripperFrameOffset,noOffset,oriTarget);

	}

	$nodeclass MakeMovement : PostureNode {
		//subscribe to pan tilt events
		virtual void doStart() {
			cout << "Listening to head movements." << endl;
			erouter->addListener(this, EventBase::sensorEGID);
		}

		virtual void doEvent() {
			cout << "Adjusting for head movements." << endl;
			$reference Lab7p5::p;
			getMC()->takeSnapshot();
			float angle = (getMC()->getOutputCmd(HeadPanOffset)).value;
			fmat::Column<3> cOffset = fmat::pack(200*cos(angle), 200*sin(angle), 0);
			fmat::Column<3> target = p.getCoords() - cOffset;
			fmat::Quaternion oriTarget = fmat::Quaternion::aboutZ(angle);
			fmat::Column<3> noOffset = fmat::pack(0,0,0);
			fmat::Quaternion identityTarget = fmat::Quaternion::identity();
			getMC()->solveLink(target, oriTarget, GripperFrameOffset, noOffset, identityTarget);
			

		}
	}


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

		$statemachine {
			startNode : StateNode
			orientHead : OrientHead
			findEllipses : FindEllipses
			setEllipse : GetCenter
			makeMovement : MakeMovement[setMC(posturemc)]
			initialMovement : InitialMovement
			preMovement : PreMovement

			startNode =N=> 
			//	orientHead =C=> 
					findEllipses =C=>
						setEllipse
			//after finding the ellipse run behavior to control movement
			setEllipse =C=> preMovement =C=> initialMovement =C=> makeMovement
		}
	}
}

REGISTER_BEHAVIOR(Lab7p5);