#!/bin/bash

# Run script for Unitree GO2 Custom Controller
# This script handles common startup scenarios

set -e

# Default values
CONFIG_FILE="config/robot_config.json"
INTERFACE="eth0"
LOG_LEVEL="INFO"
LOG_FILE=""
BACKGROUND=false

# Colors for output
RED='\033[0;31m'
GREEN='\033[0;32m'
YELLOW='\033[1;33m'
NC='\033[0m' # No Color

print_usage() {
    echo "Usage: $0 [OPTIONS]"
    echo ""
    echo "Options:"
    echo "  -c, --config FILE       Configuration file (default: config/robot_config.json)"
    echo "  -i, --interface IFACE   Network interface (default: eth0)"
    echo "  -l, --log-level LEVEL   Log level: DEBUG, INFO, WARN, ERROR (default: INFO)"
    echo "  -f, --log-file FILE     Log to file"
    echo "  -b, --background        Run in background"
    echo "  -h, --help              Show this help"
    echo ""
    echo "Examples:"
    echo "  $0                                    # Run with defaults"
    echo "  $0 -i wlan0 -l DEBUG                # Use WiFi interface with debug logging"
    echo "  $0 -c prod_config.json -f robot.log # Use production config and log to file"
    echo "  $0 -b                                # Run in background"
}

log_info() {
    echo -e "${GREEN}[INFO]${NC} $1"
}

log_warn() {
    echo -e "${YELLOW}[WARN]${NC} $1"
}

log_error() {
    echo -e "${RED}[ERROR]${NC} $1"
}

# Parse command line arguments
while [[ $# -gt 0 ]]; do
    case $1 in
        -c|--config)
            CONFIG_FILE="$2"
            shift 2
            ;;
        -i|--interface)
            INTERFACE="$2"
            shift 2
            ;;
        -l|--log-level)
            LOG_LEVEL="$2"
            shift 2
            ;;
        -f|--log-file)
            LOG_FILE="$2"
            shift 2
            ;;
        -b|--background)
            BACKGROUND=true
            shift
            ;;
        -h|--help)
            print_usage
            exit 0
            ;;
        *)
            log_error "Unknown option: $1"
            print_usage
            exit 1
            ;;
    esac
done

# Check if executable exists
EXECUTABLE="./bin/unitree_go2_custom"
if [ ! -f "$EXECUTABLE" ]; then
    EXECUTABLE="./build/bin/unitree_go2_custom"
    if [ ! -f "$EXECUTABLE" ]; then
        log_error "Executable not found. Please build the project first:"
        echo "  mkdir build && cd build && cmake .. && make"
        exit 1
    fi
fi

# Check if configuration file exists
if [ ! -f "$CONFIG_FILE" ]; then
    log_warn "Configuration file not found: $CONFIG_FILE"
    log_info "Creating default configuration..."
    mkdir -p "$(dirname "$CONFIG_FILE")"
    cat > "$CONFIG_FILE" << 'EOF'
{
  "network": {
    "interface": "eth0"
  },
  "mqtt": {
    "broker": "localhost",
    "port": 1883,
    "client_id": "unitree_go2_client"
  },
  "topics": {
    "prefix": "unitree/go2",
    "cmd": "cmd",
    "state": "state"
  },
  "robot": {
    "control_frequency": 200.0,
    "state_publish_frequency": 50.0
  },
  "safety": {
    "max_linear_velocity": 1.5,
    "max_angular_velocity": 2.0,
    "emergency_stop_timeout": 5.0
  }
}
EOF
    log_info "Default configuration created at: $CONFIG_FILE"
fi

# Check network interface
if ! ip link show "$INTERFACE" >/dev/null 2>&1; then
    log_warn "Network interface '$INTERFACE' not found"
    log_info "Available interfaces:"
    ip link show | grep -E '^[0-9]+:' | cut -d: -f2 | tr -d ' '
    log_info "You can specify a different interface with -i option"
fi

# Build command
CMD="$EXECUTABLE"
CMD="$CMD --config $CONFIG_FILE"
CMD="$CMD --interface $INTERFACE"
CMD="$CMD --log-level $LOG_LEVEL"

if [ -n "$LOG_FILE" ]; then
    CMD="$CMD --log-file $LOG_FILE"
    # Create log directory if needed
    mkdir -p "$(dirname "$LOG_FILE")"
fi

# Display startup information
log_info "Starting Unitree GO2 Custom Controller"
log_info "Executable: $EXECUTABLE"
log_info "Configuration: $CONFIG_FILE"
log_info "Network Interface: $INTERFACE"
log_info "Log Level: $LOG_LEVEL"
if [ -n "$LOG_FILE" ]; then
    log_info "Log File: $LOG_FILE"
fi

# Setup signal handlers for graceful shutdown
cleanup() {
    log_info "Shutting down robot controller..."
    if [ -n "$ROBOT_PID" ]; then
        kill -TERM "$ROBOT_PID" 2>/dev/null || true
        wait "$ROBOT_PID" 2>/dev/null || true
    fi
    log_info "Shutdown complete"
    exit 0
}

trap cleanup SIGINT SIGTERM

# Run the robot controller
if [ "$BACKGROUND" = true ]; then
    log_info "Starting in background mode..."
    nohup $CMD > robot_output.log 2>&1 &
    ROBOT_PID=$!
    log_info "Robot controller started with PID: $ROBOT_PID"
    log_info "Output is being logged to: robot_output.log"
    log_info "Use 'kill $ROBOT_PID' to stop the robot"
else
    log_info "Starting robot controller..."
    log_info "Press Ctrl+C to stop"
    $CMD &
    ROBOT_PID=$!
    wait "$ROBOT_PID"
fi