#!/bin/bash

TARGET="$1"

if [[ "$TARGET" == "-h" || "$TARGET" == "--help" ]]; then
    exit 0
fi

if [[ $# -lt 1 ]]; then
    echo "Error: Expected at least 1 arg"
    exit 1
fi

if [[ $# -ge 2 ]]; then
  SPEED="$2"
fi

source install/setup.bash

case $TARGET in
    "forward") 
        ros2 topic pub --rate 10 /cmd_vel geometry_msgs/msg/TwistStamped "
          header: auto
          twist:
            linear:
              x: ${SPEED:-0.4}
              y: 0.0
              z: 0.0
            angular:
              x: 0.0
              y: 0.0
              z: 0.0"
        ;;
    "backward") 
        ros2 topic pub --rate 10 /cmd_vel geometry_msgs/msg/TwistStamped "
          header: auto
          twist:
            linear:
              x: ${SPEED:--0.8}
              y: 0.0
              z: 0.0
            angular:
              x: 0.0
              y: 0.0
              z: 0.0"
        ;;
    "left") 
        ros2 topic pub --rate 10 /cmd_vel geometry_msgs/msg/TwistStamped "
          header: auto
          twist:
            linear:
              x: 0.0
              y: 0.0
              z: 0.0
            angular:
              x: 0.0
              y: 0.0
              z: ${SPEED:-0.4}"
        ;;
    "right") 
        ros2 topic pub --rate 10 /cmd_vel geometry_msgs/msg/TwistStamped "
          header: auto
          twist:
            linear:
              x: 0.0
              y: 0.0
              z: 0.0
            angular:
              x: 0.0
              y: 0.0
              z: ${SPEED:--0.4}"
        ;;
    "zero")
        ros2 service call /zero std_srvs/srv/Empty
        ;;
    "vel")
        ros2 topic pub --rate 10 /velocity_controller/commands std_msgs/msg/Float64MultiArray "
          data: [$2, $3]"
        ;;
    "switch")
        controllers=$(ros2 control list_controllers | grep -E "velocity|diffdrive")
        to_deactivate=$(echo "$controllers" | awk '$3 == "active" { print $1 }')
        to_activate=$(echo "$controllers" | awk '$3 == "inactive" { print $1 }')
        ros2 control switch_controllers --deactivate $to_deactivate --activate $to_activate --best-effort
        ;;
    *)
        echo "Target not defined"
        exit 1
        ;;
esac
exit 0