#!/usr/bin/env bash
# setup_can.sh — Bring up CANable 2.0 (slcan/ttyACM0) as slcan0 at 500 kbps
# Issue: https://gitea.vayrette.com/seb/saltylab-firmware/issues/674
#
# This script uses slcand to expose the CANable 2.0 (USB CDC / slcan firmware)
# as a SocketCAN interface.  It is NOT used for the gs_usb (native SocketCAN)
# firmware variant — use jetson/scripts/can_setup.sh for that.
#
# Usage:
#   sudo ./setup_can.sh           # bring up slcan0
#   sudo ./setup_can.sh down      # bring down slcan0 and kill slcand
#   sudo ./setup_can.sh verify    # candump one-shot check (Ctrl-C to stop)
#
# Environment overrides:
#   CAN_DEVICE   — serial device  (default: /dev/ttyACM0)
#   CAN_IFACE    — SocketCAN name (default: slcan0)
#   CAN_BITRATE  — bit rate       (default: 500000)
#
# Mamba CAN ID: 1 (0x001)
# VESC left:    56 (0x038)
# VESC right:   68 (0x044)

set -euo pipefail

DEVICE="${CAN_DEVICE:-/dev/ttyACM0}"
IFACE="${CAN_IFACE:-slcan0}"
BITRATE="${CAN_BITRATE:-500000}"

log()  { echo "[setup_can] $*"; }
warn() { echo "[setup_can] WARNING: $*" >&2; }
die()  { echo "[setup_can] ERROR: $*" >&2; exit 1; }

# Map numeric bitrate to slcand -s flag (0-8 map to standard CAN speeds)
bitrate_flag() {
    case "$1" in
        10000)   echo "0" ;;
        20000)   echo "1" ;;
        50000)   echo "2" ;;
        100000)  echo "3" ;;
        125000)  echo "4" ;;
        250000)  echo "5" ;;
        500000)  echo "6" ;;
        800000)  echo "7" ;;
        1000000) echo "8" ;;
        *) die "Unsupported bitrate: $1 (choose 10k–1M)" ;;
    esac
}

# ── up ─────────────────────────────────────────────────────────────────────
cmd_up() {
    # Verify serial device is present
    if [[ ! -c "$DEVICE" ]]; then
        die "$DEVICE not found — is CANable 2.0 plugged in?"
    fi

    # If interface already exists, leave it alone
    if ip link show "$IFACE" &>/dev/null; then
        log "$IFACE is already up — nothing to do."
        ip -details link show "$IFACE"
        return 0
    fi

    local sflag
    sflag=$(bitrate_flag "$BITRATE")

    log "Starting slcand on $DEVICE → $IFACE at ${BITRATE} bps (flag -s${sflag}) …"
    # -o open device, -c close on exit, -f forced, -s<N> speed, -S<baud> serial baud
    slcand -o -c -f -s"${sflag}" -S 3000000 "$DEVICE" "$IFACE" \
        || die "slcand failed to start"

    # Give slcand a moment to create the netdev
    local retries=0
    while ! ip link show "$IFACE" &>/dev/null; do
        retries=$((retries + 1))
        if [[ $retries -ge 10 ]]; then
            die "Timed out waiting for $IFACE to appear after slcand start"
        fi
        sleep 0.2
    done

    log "Bringing up $IFACE …"
    ip link set "$IFACE" up \
        || die "ip link set $IFACE up failed"

    log "$IFACE is up."
    ip -details link show "$IFACE"
}

# ── down ───────────────────────────────────────────────────────────────────
cmd_down() {
    log "Bringing down $IFACE …"
    if ip link show "$IFACE" &>/dev/null; then
        ip link set "$IFACE" down || warn "Could not set $IFACE down"
    else
        warn "$IFACE not found — already down?"
    fi

    # Kill any running slcand instances bound to our device
    if pgrep -f "slcand.*${DEVICE}" &>/dev/null; then
        log "Stopping slcand for $DEVICE …"
        pkill -f "slcand.*${DEVICE}" || warn "pkill returned non-zero"
    fi
    log "Done."
}

# ── verify ─────────────────────────────────────────────────────────────────
cmd_verify() {
    if ! ip link show "$IFACE" &>/dev/null; then
        die "$IFACE is not up — run '$0 up' first"
    fi
    log "Listening on $IFACE — expecting frames from Mamba (0x001), VESC left (0x038), VESC right (0x044)"
    log "Press Ctrl-C to stop."
    exec candump "$IFACE"
}

# ── main ───────────────────────────────────────────────────────────────────
CMD="${1:-up}"
case "$CMD" in
    up)     cmd_up     ;;
    down)   cmd_down   ;;
    verify) cmd_verify ;;
    *)
        echo "Usage: $0 [up|down|verify]"
        exit 1
        ;;
esac