#include "Motion/MotionPtr.h"
#include "Behaviors/StateMachine.h"
#include <cmath>

using namespace std;

$nodeclass ContinuousTrajectories1 : StateNode {

  $nodeclass InitPos : PostureNode : doStart {
	fmat::Column<3> target = fmat::pack(300, -120, 400);

	for (uint i = ArmBaseOffset; i < ArmBaseOffset + NumArmJoints; i++)
		getMC()->setMaxSpeed(i, 0.6);
	getMC()->solveLinkPosition(target, GripperFrameOffset, fmat::pack(0, 0, 0));
  }

  $nodeclass EndPos : PostureNode : doStart {
	fmat::Column<3> target = fmat::pack(500, -120, 400);

	for (uint i = ArmBaseOffset; i < ArmBaseOffset + NumArmJoints; i++)
		getMC()->setMaxSpeed(i, 0.6);
	getMC()->solveLinkPosition(target, GripperFrameOffset, fmat::pack(0, 0, 0));
  }

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

    $statemachine{
		startNode : InitPos[setMC(posturemc)]
		finishNode : EndPos[setMC(posturemc)]
  		startNode =C=> finishNode
    }
  }

}

REGISTER_BEHAVIOR(ContinuousTrajectories1);