#!/bin/bash

# FAST-LIO 开机自启动脚本 (核心版本)
# 只启动必要的核心程序，适用于开机自启动
# 不依赖图形界面，使用后台运行方式

set -e

# 配置
WORKSPACE_DIR="/home/jetson/mid360test"
LOG_DIR="$WORKSPACE_DIR/logs"
STARTUP_LOG="$LOG_DIR/autostart_$(date +%Y%m%d_%H%M%S).log"

# 创建日志目录
mkdir -p "$LOG_DIR"

# 日志函数
log() {
    echo "[$(date '+%Y-%m-%d %H:%M:%S')] $1" | tee -a "$STARTUP_LOG"
}

log "🚀 FAST-LIO 自启动开始"

# 切换到工作目录
cd "$WORKSPACE_DIR" || {
    log "❌ 无法切换到工作目录: $WORKSPACE_DIR"
    exit 1
}

# 清理旧进程
log "🧹 清理旧进程..."
pkill -f "livox_ros_driver2" 2>/dev/null || true
pkill -f "fastlio_mapping" 2>/dev/null || true
pkill -f "distance_calculator" 2>/dev/null || true
sleep 3

# 设置环境
log "⚙️ 设置ROS2环境..."
source /opt/ros/humble/setup.bash 2>/dev/null || {
    log "❌ 无法加载ROS2环境"
    exit 1
}

source "$WORKSPACE_DIR/install/setup.bash" 2>/dev/null || {
    log "❌ 无法加载工作空间环境"
    exit 1
}

# 等待网络稳定
log "🌐 等待网络连接稳定..."
sleep 5

# 验证网络连接
if ping -c 1 192.168.1.194 >/dev/null 2>&1; then
    log "✅ 雷达网络连接正常"
else
    log "⚠️ 雷达网络连接异常，继续启动"
fi

# 启动雷达驱动 (后台运行)
log "📡 启动雷达驱动..."
nohup ros2 launch livox_ros_driver2 msg_MID360_launch.py \
    > "$LOG_DIR/livox_driver.log" 2>&1 &
LIVOX_PID=$!
log "📡 雷达驱动已启动 (PID: $LIVOX_PID)"

# 等待雷达驱动稳定
sleep 8

# 启动FAST-LIO (后台运行)
log "🎯 启动FAST-LIO..."
nohup ros2 launch fast_lio mapping.launch.py \
    > "$LOG_DIR/fastlio.log" 2>&1 &
FASTLIO_PID=$!
log "🎯 FAST-LIO已启动 (PID: $FASTLIO_PID)"

# 等待FAST-LIO稳定
sleep 5

# 启动距离计算器 (后台运行)
if [ -f "$WORKSPACE_DIR/distance_calculator.py" ]; then
    log "📏 启动距离计算器..."
    nohup python3 "$WORKSPACE_DIR/distance_calculator.py" \
        > "$LOG_DIR/distance_calculator.log" 2>&1 &
    CALC_PID=$!
    log "📏 距离计算器已启动 (PID: $CALC_PID)"
else
    log "⚠️ 距离计算器文件不存在"
fi

# 保存PID文件，便于后续管理
echo "$LIVOX_PID" > "$LOG_DIR/livox_driver.pid"
echo "$FASTLIO_PID" > "$LOG_DIR/fastlio.pid"
[ -n "$CALC_PID" ] && echo "$CALC_PID" > "$LOG_DIR/distance_calculator.pid"

log "✅ 所有核心程序启动完成"
log "📊 进程状态:"
log "  - 雷达驱动: PID $LIVOX_PID"
log "  - FAST-LIO: PID $FASTLIO_PID"
[ -n "$CALC_PID" ] && log "  - 距离计算器: PID $CALC_PID"

log "📁 日志文件位置:"
log "  - 启动日志: $STARTUP_LOG"
log "  - 雷达驱动: $LOG_DIR/livox_driver.log"
log "  - FAST-LIO: $LOG_DIR/fastlio.log"
[ -n "$CALC_PID" ] && log "  - 距离计算器: $LOG_DIR/distance_calculator.log"

log "💡 提示:"
log "  - 查看实时状态: tail -f $LOG_DIR/*.log"
log "  - 停止系统: ./stop_system.sh 或 pkill -f 'ros2|python3.*distance_calculator'"
log "  - 系统需要5-10秒完全稳定后才开始UART传输"

log "🎉 自启动脚本执行完成"

# 可选：监控进程状态
sleep 10
log "🔍 检查进程状态..."

check_process() {
    local pid=$1
    local name=$2
    if kill -0 "$pid" 2>/dev/null; then
        log "✅ $name 运行正常 (PID: $pid)"
        return 0
    else
        log "❌ $name 进程异常 (PID: $pid)"
        return 1
    fi
}

check_process "$LIVOX_PID" "雷达驱动"
check_process "$FASTLIO_PID" "FAST-LIO"
[ -n "$CALC_PID" ] && check_process "$CALC_PID" "距离计算器"

log "🏁 初始化完成，系统正在运行中..."