#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 ContinuousTrajectories : StateNode {

   
 $nodeclass IKMvmt : DynamicMotionSequenceNode : doStart {

	PostureEngine posEng;
	int distance = 1;

	fmat::Column<3> change = fmat::pack(distance,0,0);
	fmat::Column<3> zero = fmat::pack(0,0,0);
	fmat::Column<3> pos = kine->getPosition(GripperFrameOffset);

	int i = 0;
	
	cout << "pre mvmt" << endl;

	while(i < 200) {
		pos += change;
		
		posEng.solveLinkPosition(pos,GripperFrameOffset,zero);
		getMC()->setPose(posEng);

		i += distance;
		cout << "pos = " << pos << endl;
	}
	
	cout << "mvmt completed" << endl;
  }



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

		$statemachine{

	  		//simpleMVMT: StateNode =N=> PostureNode("step1.pos") =T(1500)=> PostureNode("step2.pos")

			improvedMVMT: StateNode =N=> PostureNode("step1.pos") =T(2000)=> IKMvmt

		}
	}
	
  
  
  
REGISTER_BEHAVIOR(ContinuousTrajectories);

#endif