#!/bin/bash

TARGET="$1"

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

if [[ $# -lt 1 ]]; then
    echo "Error: Expected at least 1 arg"
    echo "Available commands: napred, rotiraj, na"
    exit 1
fi

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

if [[ $# -ge 3 ]]; then
  MIN_ROT="$(echo "$3*a(1)/45" | bc -l)"
fi

source install/setup.bash

case $TARGET in
    "na") 
        ros2 action send_goal /moveCoords toid_msgs/action/SimpleMoveCoords "
          x: 0.0
          y: 0.0
          theta: 0.0
          backwards: ${SMER:-0}"
        ;;
    "pravo") 
        ros2 action send_goal /translateX toid_msgs/action/SimpleTranslateX  "distance: ${SMER:-0.3}"
        ;;
    "rotiraj") 
        SMER="$(echo "$SMER*a(1)/45" | bc -l)"
        echo $SMER
        ros2 action send_goal /rotate toid_msgs/action/SimpleRotate  "
          angle: ${SMER:-0.3}
          min_angle: ${MIN_ROT:-0.0}"
        ;;
    "stani") 
        ros2 service call /moveCoords/_action/cancel_goal action_msgs/srv/CancelGoal &
        ros2 service call /translateX/_action/cancel_goal action_msgs/srv/CancelGoal &
        ros2 service call /rotate/_action/cancel_goal action_msgs/srv/CancelGoal &
        wait
        ;;
    *)
        echo "Target not defined"
        exit 1
        ;;
esac
exit 0