#include "Behaviors/StateMachine.h"
#include "Motion/MotionPtr.h"
$nodeclass Lab7p4_2 : StateNode {
	$nodeclass ArmMover_first : PostureNode : doStart {
	    fmat::Column<3> targetPos = fmat::pack(300, 0, 300);
	    fmat::Column<3> noOffset = fmat::pack(0, 0, 0);
	    getMC()->solveLinkPosition(targetPos, GripperFrameOffset, noOffset);
	}

	$nodeclass ArmMover_second : DynamicMotionSequenceNode : doStart {
            float step = 10;
    	    float x_start = 300;
    	    float x_target = 600;
    	    float x_cur;
            bool bsolution;
            PostureEngine pe;
    	    for(x_cur = x_start + step; x_cur <= x_target; x_cur += step) {
                    cout << "now x : " << x_cur << endl;
                    
    		bsolution = pe.solveLinkPosition(x_cur, 0, 300, GripperFrameOffset, 0, 0, 0);
            if(bsolution)
            {
              cout << "ok" << endl;
              getMC()->setPose(pe);
            }
            else
            {
              cout << "eee" << endl;
              exit(0);
            }
	    }
	}

    virtual void setup() {
    MotionPtr<PostureMC> pmc;
	    pmc->setMaxSpeed(ArmBaseOffset, M_PI/2);
	    pmc->setMaxSpeed(ArmShoulderOffset, M_PI/2);
	    pmc->setMaxSpeed(ArmElbowOffset, M_PI/2);
	    pmc->setMaxSpeed(ArmWristOffset, M_PI/2);
	    pmc->setMaxSpeed(LeftFingerOffset, M_PI/2);
	    pmc->setMaxSpeed(RightFingerOffset, M_PI/2);
        MotionManager::MC_ID posturemc = addMotion(pmc);

        $statemachine{
            startnode: StateNode
            movearmf: ArmMover_first[setMC(posturemc)]
            movearms: ArmMover_second
            startnode =N=> movearmf =C=> movearms
        }
    }
}

REGISTER_BEHAVIOR(Lab7p4_2);