#!/bin/bash

# --- Cleanup function runs on script exit ---
function cleanup {
    echo -e "\nCleaning up and stopping HAL..."
    halrun -U
    echo "HAL stopped."
}

# Trap the EXIT signal to run the cleanup function automatically
trap cleanup EXIT

HAL_FILE="ethercat-home.hal"
DELAY_SECONDS=3 # Reduced delay slightly, adjust if needed

echo "Starting HAL configuration: $HAL_FILE"

# Run halrun in the background, keeping HAL alive after the file is loaded
halcmd -kf "$HAL_FILE"

# Give HAL a moment to initialize
sleep $DELAY_SECONDS

echo "Waiting until all 3 motors are enabled..."
while true; do
    # Get the status of all three motors in one go
    motor1_status=$(halcmd getp cia402.0.stat-op-enabled)
    motor2_status=$(halcmd getp cia402.1.stat-op-enabled)
    motor3_status=$(halcmd getp cia402.2.stat-op-enabled)

    if [[ "$motor1_status" == "TRUE" && "$motor2_status" == "TRUE" && "$motor3_status" == "TRUE" ]]; then
        echo "All motors are ready."
        break
    else
        # Provide more specific feedback
        echo "Waiting... [Motor1: $motor1_status, Motor2: $motor2_status, Motor3: $motor3_status]"
        sleep 1
    fi
done

# Prompt user to begin
printf "Start homing? (y/N): "
read user_response

# Check the user's response
if [[ "$user_response" != "y" && "$user_response" != "Y" ]]; then
  echo "Homing aborted by user. Exiting."
  exit 0 # The trap will handle cleanup
fi

# --- NEW: Functions for Homing and Status Check ---

# Function to send the homing command to all drives
function trigger_homing {
    echo "Sending homing command to drives..."
    # The command is sent multiple times, preserving original logic
    halcmd setp cia402.0.home 1
    halcmd setp cia402.1.home 1
    halcmd setp cia402.2.home 1
    echo "Homing command sent."
}

# Function to check the homing status of the drives
function check_homing_status {
    echo "--------------------------"
    echo "Current Homing Status:"
    # The 'is-homed' pin is the standard output for this status
    status1=$(halcmd getp cia402.0.homed)
    status2=$(halcmd getp cia402.1.homed)
    status3=$(halcmd getp cia402.2.homed)
    echo "  Motor 1: $status1"
    echo "  Motor 2: $status2"
    echo "  Motor 3: $status3"
    if [[ "$status1" == "TRUE" && "$status2" == "TRUE" && "$status3" == "TRUE" ]]; then
        echo "SUCCESS: All motors are homed."
    else
        echo "INFO: Not all motors are homed yet."
    fi
    echo "--------------------------"
}

# --- Main interactive loop ---

# Trigger homing for the first time
trigger_homing

# Enter a loop to allow for retries or status checks
while true; do
    echo
    echo "Homing Control Menu:"
    echo "  (h) Trigger Homing Again"
    echo "  (s) Check Homing Status"
    echo "  (q) Quit"
    printf "Enter your choice: "
    read -n 1 -s choice # Read 1 character silently
    echo # Move to a new line

    case "$choice" in
        h|H)
            trigger_homing
            ;;
        s|S)
            check_homing_status
            ;;
        q|Q)
            echo "Exiting."
            exit 0
            ;;
        *)
            echo "Invalid option. Please try again."
            ;;
    esac
done