#include "Shared/RobotInfo.h"

#include "Behaviors/StateMachine.h"

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

#define SLOW_SPEED M_PI*1/8
#define NUM_STEPS 20

$nodeclass Lab7p4 : StateNode {

    $nodeclass MyTrajectory : DynamicMotionSequenceNode : doStart {

        PostureEngine p;

        p.loadFile("arm1.pos");
        fmat::Column<3> startPos = p.linkToBase(GripperFrameOffset).translation();
        cout << "StartPos: " << startPos << endl;
        
        p.loadFile("arm2.pos");
        fmat::Column<3> endPos = p.linkToBase(GripperFrameOffset).translation();
        cout << "EndPos: " << endPos << endl;
        
        fmat::Column<3> deltaPos = (endPos - startPos);
        cout << "DeltaPos: " << deltaPos << endl;

        fmat::Column<3> deltaStep = deltaPos / NUM_STEPS;
        cout << "DeltaStep: " << deltaStep << endl;

        fmat::Column<3> pos = startPos;

        for(int i = 0; i < NUM_STEPS; i++) {
            cout << "Position: " << pos << endl;

            pos += deltaStep;

            p.solveLinkPosition(pos, GripperFrameOffset, fmat::pack(0,0,0));
            getMC()->setPose(p);
            getMC()->advanceTime(400);
        }

    }


  virtual void setup() {
    $statemachine{

        // part IV.1 : non-linear
//        PostureNode("arm1.pos") =T(2000)=> PostureNode("arm2.pos")


        PostureNode("arm1.pos") =T(1000)=> MyTrajectory() =C=> SpeechNode("done")

    }
  }

}

REGISTER_BEHAVIOR(Lab7p4);