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

#define GRIP GripperFrameOffset
#define MAX_SPEED 0.3f

$nodeclass ContinuousTrajectories : StateNode {

$nodeclass Trajectory : DynamicMotionSequenceNode : doStart {

	PostureEngine pe;

	/* Calculate trajectory */
	int dist = 1;
	fmat::Column<3> delta = fmat::pack(dist,0,0);
	fmat::Column<3> zero = fmat::pack(0,0,0);
	fmat::Column<3> pos = kine->getPosition(GRIP);
	int i = 0;
	while(i < 200) {

		pos += delta;
		cout << "Current pos: " << pos << endl;	
		pe.solveLinkPosition(pos,GRIP,zero);
		getMC()->setPose(pe);

		i += dist;
	}
	cout << "DONE!!" << endl;
}

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

		$statemachine{

			interpolate: StateNode=N=> PostureNode("7411.pos") =T(1000)=> Trajectory 

			// The first, jerky part
			crappymovement: StateNode =N=> PostureNode("7411.pos") =T(2000)=> PostureNode("7412.pos")

		}
	}
}

REGISTER_BEHAVIOR(ContinuousTrajectories);