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

using namespace std;

$nodeclass ContinuousTrajectories2 : StateNode {

  $nodeclass IncrementPos : DynamicMotionSequenceNode : doStart {
	PostureEngine pe = PostureEngine();

	fmat::Column<3> init_pos = fmat::pack(300, -120, 400);
	fmat::Column<3> end_target = fmat::pack(500, -120, 400);
	fmat::Column<3> cur_target;

	float num_steps = 20000;
	for (float i = 0; i < num_steps; i++) {
		cur_target = fmat::pack(300 + (200.0 / num_steps * i), -120, 400);
		pe.solveLinkPosition(cur_target, GripperFrameOffset, fmat::pack(0, 0, 0));
		getMC()->setSpeed(0.1);
		getMC()->setPose(pe);
	}
  }

  $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));
  }

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

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

}

REGISTER_BEHAVIOR(ContinuousTrajectories2);