#include "Shared/RobotInfo.h"

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

$nodeclass Part4SimpleMotion : StateNode {

  // keeping only x as variable results in diagonal motion for some reason	
  $nodeclass Position1 : PostureNode : doStart {
    fmat::Column<3> target = fmat::pack(50,-100,350);
    fmat::Column<3> noOffset = fmat::pack(0,0,0);

    // otherwise the arm shakes the robot by moving too fast
    float speed = 0.5;
    getMC()->setMaxSpeed(ArmBaseOffset,speed);
    getMC()->setMaxSpeed(ArmShoulderOffset,speed);
    getMC()->setMaxSpeed(ArmElbowOffset,speed);
    getMC()->setMaxSpeed(ArmWristOffset,speed);
    getMC()->setMaxSpeed(WristRotateOffset,speed);
    getMC()->setMaxSpeed(LeftFingerOffset,speed);
    getMC()->setMaxSpeed(RightFingerOffset,speed);


    getMC()->solveLinkPosition(target,LeftFingerFrameOffset,noOffset);
  }

  $nodeclass Position2 : PostureNode : doStart {
    fmat::Column<3> target = fmat::pack(250,-100,350);
    fmat::Column<3> noOffset = fmat::pack(0,0,0);

    // otherwise the arm shakes the robot by moving too fast
    float speed = 0.5;
    getMC()->setMaxSpeed(ArmBaseOffset,speed);
    getMC()->setMaxSpeed(ArmShoulderOffset,speed);
    getMC()->setMaxSpeed(ArmElbowOffset,speed);
    getMC()->setMaxSpeed(ArmWristOffset,speed);
    getMC()->setMaxSpeed(WristRotateOffset,speed);
    getMC()->setMaxSpeed(LeftFingerOffset,speed);
    getMC()->setMaxSpeed(RightFingerOffset,speed);


    getMC()->solveLinkPosition(target,LeftFingerFrameOffset,noOffset);
  }

  virtual void setup() {
    $statemachine{
      Position1 =T(2500)=>  Position2
    }
  }

}

REGISTER_BEHAVIOR(Part4SimpleMotion);