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

$nodeclass TrajectoryB : 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 : DynamicMotionSequenceNode : doStart {
	    PostureEngine instance;
	    float x;
	    float step = 10;
	    float startX = 310;
	    float goal = 550;	
	    bool success;
	    fmat::Column<3> noOffset = fmat::pack(0, 0, 0);
	    for(x = startX; x<=goal; x+=step) {
		fmat::Column<3> targetPos = fmat::pack(x, -80, 250);
		success = instance.solveLinkPosition(targetPos,GripperFrameOffset,noOffset);
		if(success) {
			getMC()->setPose(instance);
		}
	    }
	}

        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
       			startnode =N=> firstMove =C=> secondMove
            }
        }

}

REGISTER_BEHAVIOR(TrajectoryB);