#include "Shared/RobotInfo.h"
#include "Behaviors/Nodes/PostureNode.h"
#include "Behaviors/Transitions/EventTrans.h"
#include "Motion/MotionPtr.h"
#include "Behaviors/StateMachine.h"

$nodeclass SmoothTrajectory {

    $nodeclass Prepare : PostureNode {
	virtual void doStart() {
	    unsigned int link = LeftFingerFrameOffset;
	    fmat::Column<3> target = fmat::pack(200,0,200);
	    getMC()->solveLinkPosition(target, link, fmat::pack(0,0,0));
	}
    }

    $nodeclass MoveArm : DynamicMotionSequenceNode {
	virtual void doStart() {
	    PostureEngine pe = PostureEngine();
	    unsigned int link = LeftFingerFrameOffset;
            float startX = 200;
	    float finalX = 400;
	    float nextX = startX;
	    fmat::Column<3> target;
	    MMAccessor<DynamicMotionSequence> mseq_acc = getMC();
	    for(float i=0.0; i<100; i++) {
		nextX = (((finalX-startX)*i)/100) + startX;

		target = fmat::pack(nextX,0,200);
		pe.solveLinkPosition(target, link, fmat::pack(0,0,0));
		mseq_acc->advanceTime(30);
		mseq_acc->setPose(pe);
	    }
	}
    }

    $setupmachine{
        p : Prepare
        m : MoveArm
	p =C=> StateNode =T(2000)=> m
    }
}

REGISTER_BEHAVIOR(SmoothTrajectory);