#include "Behaviors/StateMachine.h"
#include "Motion/MotionPtr.h"

$nodeclass TrajectoryA : StateNode {
	$nodeclass FirstMove : PostureNode : doStart {
	    fmat::Column<3> targetPos = fmat::pack(300, -80, 250);
	    fmat::Column<3> noOffset = fmat::pack(0, 0, 0);
	    getMC()->solveLinkPosition(targetPos, GripperFrameOffset, noOffset);
	}

	$nodeclass SecondMove : PostureNode : doStart {
            fmat::Column<3> targetPos = fmat::pack(546, -80, 250);
            fmat::Column<3> noOffset = fmat::pack(0, 0, 0);
            getMC()->solveLinkPosition(targetPos, GripperFrameOffset, noOffset);
	}

	
        virtual void setup() {
    	    	float maxSpeed = deg2rad((float)45);
		MotionPtr<PostureMC>()->setMaxSpeed(ArmOffset,maxSpeed);
		
		MotionManager::MC_ID posturemc = addMotion(MotionPtr<PostureMC>());
        	$statemachine{
        		startnode: StateNode
        		firstMove: FirstMove[setMC(posturemc)]
        		secondMove: SecondMove[setMC(posturemc)]
       			startnode =N=> firstMove =C=> secondMove
            }
        }

}

REGISTER_BEHAVIOR(TrajectoryA);