#include "Behaviors/StateMachine.h"

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

#define NUM_STEPS 16

$nodeclass Lab7Q4b : VisualRoutinesStateNode {
    $nodeclass Punch : DynamicMotionSequenceNode : doStart {
        PostureEngine pe;
        pe.loadFile("Lab7-start.pos");
        
        // get the position of the gripper
        fmat::Transform gripper = pe.linkToBase(GripperFrameOffset);
        fmat::Column<3> firstPos = gripper.translation();
        
        // move forward 15 cm
        float dx = 150;
        
        // shift the start position by 15 cm
        firstPos[0] -= dx;
        
        fmat::Column<3> currPos = firstPos;
        fmat::Column<3> zeroes = fmat::pack(0, 0, 0);
        
        getMC()->clear();
        getMC()->advanceTime(2000);
        
        for (int i = 0; i < NUM_STEPS; i++) {
            // tiny steps
            fmat::Column<3> nextPos = currPos;
            // move along x axis
            nextPos[0] += dx/NUM_STEPS;
            // try to get the gripper offset to our desired point
            pe.solveLinkPosition(nextPos, GripperFrameOffset, zeroes);
            currPos = nextPos;
            
            getMC()->setPose(pe);
            getMC()->advanceTime(500);
            
        }
    }
    

    $setupmachine {
        startnode : StateNode
        move : Punch()
        speak : SpeechNode("Done.")
        
        startnode =N=> move =C=> speak
    }
}

REGISTER_BEHAVIOR(Lab7Q4b);