#!/bin/bash

# NowYouSeeMe Holodeck Startup Script
# Starts all components of the holodeck environment

set -e

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

# Configuration
PROJECT_ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
PYTHON_VENV="$PROJECT_ROOT/venv"
BUILD_DIR="$PROJECT_ROOT/build"
CONFIG_DIR="$PROJECT_ROOT/configs"

# Default settings
START_CAMERA=true
START_CSI=true
START_SYNC=true
START_SLAM=true
START_FUSION=true
START_RENDERING=true
DEBUG_MODE=false

# Parse command line arguments
while [[ $# -gt 0 ]]; do
    case $1 in
        --no-camera)
            START_CAMERA=false
            shift
            ;;
        --no-csi)
            START_CSI=false
            shift
            ;;
        --no-sync)
            START_SYNC=false
            shift
            ;;
        --no-slam)
            START_SLAM=false
            shift
            ;;
        --no-fusion)
            START_FUSION=false
            shift
            ;;
        --no-rendering)
            START_RENDERING=false
            shift
            ;;
        --debug)
            DEBUG_MODE=true
            shift
            ;;
        --help)
            echo "Usage: $0 [options]"
            echo "Options:"
            echo "  --no-camera     Skip camera capture"
            echo "  --no-csi        Skip CSI acquisition"
            echo "  --no-sync       Skip time synchronization"
            echo "  --no-slam       Skip SLAM processing"
            echo "  --no-fusion     Skip sensor fusion"
            echo "  --no-rendering  Skip rendering engine"
            echo "  --debug         Enable debug mode"
            echo "  --help          Show this help message"
            exit 0
            ;;
        *)
            echo "Unknown option: $1"
            exit 1
            ;;
    esac
done

# Function to print colored output
print_status() {
    echo -e "${GREEN}[INFO]${NC} $1"
}

print_warning() {
    echo -e "${YELLOW}[WARNING]${NC} $1"
}

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

# Check if virtual environment exists
check_environment() {
    if [ ! -d "$PYTHON_VENV" ]; then
        print_error "Virtual environment not found. Please run ./tools/build.sh --deps first."
        exit 1
    fi
    
    # Activate virtual environment
    source "$PYTHON_VENV/bin/activate"
    
    # Check if required packages are installed
    python -c "import cv2, numpy, scipy" 2>/dev/null || {
        print_error "Required Python packages not found. Please run ./tools/build.sh --deps first."
        exit 1
    }
}

# Start time synchronization service
start_sync_service() {
    if [ "$START_SYNC" = true ]; then
        print_status "Starting time synchronization service..."
        
        if [ -f "$BUILD_DIR/bin/sync_service" ]; then
            "$BUILD_DIR/bin/sync_service" &
            SYNC_PID=$!
            echo $SYNC_PID > /tmp/nowyouseeme_sync.pid
            print_status "Time sync service started (PID: $SYNC_PID)"
        else
            print_warning "Time sync service not found, skipping..."
        fi
    fi
}

# Start camera capture
start_camera() {
    if [ "$START_CAMERA" = true ]; then
        print_status "Starting camera capture..."
        
        # Check if camera config exists
        if [ -f "$CONFIG_DIR/camera_config.json" ]; then
            python "$PROJECT_ROOT/src/ingestion/capture.py" \
                --config "$CONFIG_DIR/camera_config.json" &
            CAMERA_PID=$!
            echo $CAMERA_PID > /tmp/nowyouseeme_camera.pid
            print_status "Camera capture started (PID: $CAMERA_PID)"
        else
            print_warning "Camera config not found, using defaults..."
            python "$PROJECT_ROOT/src/ingestion/capture.py" &
            CAMERA_PID=$!
            echo $CAMERA_PID > /tmp/nowyouseeme_camera.pid
            print_status "Camera capture started (PID: $CAMERA_PID)"
        fi
    fi
}

# Start CSI acquisition
start_csi() {
    if [ "$START_CSI" = true ]; then
        print_status "Starting CSI acquisition..."
        
        # Check if CSI config exists
        if [ -f "$CONFIG_DIR/csi_config.json" ]; then
            python "$PROJECT_ROOT/src/ingestion/csi_acquirer.py" \
                --config "$CONFIG_DIR/csi_config.json" &
            CSI_PID=$!
            echo $CSI_PID > /tmp/nowyouseeme_csi.pid
            print_status "CSI acquisition started (PID: $CSI_PID)"
        else
            print_warning "CSI config not found, using defaults..."
            python "$PROJECT_ROOT/src/ingestion/csi_acquirer.py" &
            CSI_PID=$!
            echo $CSI_PID > /tmp/nowyouseeme_csi.pid
            print_status "CSI acquisition started (PID: $CSI_PID)"
        fi
    fi
}

# Start RF SLAM
start_rf_slam() {
    if [ "$START_SLAM" = true ]; then
        print_status "Starting RF SLAM..."
        
        if [ -f "$BUILD_DIR/bin/rf_slam" ]; then
            "$BUILD_DIR/bin/rf_slam" &
            RF_SLAM_PID=$!
            echo $RF_SLAM_PID > /tmp/nowyouseeme_rf_slam.pid
            print_status "RF SLAM started (PID: $RF_SLAM_PID)"
        else
            print_warning "RF SLAM not found, skipping..."
        fi
    fi
}

# Start vision SLAM
start_vision_slam() {
    if [ "$START_SLAM" = true ]; then
        print_status "Starting vision SLAM..."
        
        if [ -f "$BUILD_DIR/bin/orb_slam" ]; then
            "$BUILD_DIR/bin/orb_slam" &
            VISION_SLAM_PID=$!
            echo $VISION_SLAM_PID > /tmp/nowyouseeme_vision_slam.pid
            print_status "Vision SLAM started (PID: $VISION_SLAM_PID)"
        else
            print_warning "Vision SLAM not found, skipping..."
        fi
    fi
}

# Start sensor fusion
start_fusion() {
    if [ "$START_FUSION" = true ]; then
        print_status "Starting sensor fusion..."
        
        if [ -f "$BUILD_DIR/bin/ekf_fusion" ]; then
            "$BUILD_DIR/bin/ekf_fusion" &
            FUSION_PID=$!
            echo $FUSION_PID > /tmp/nowyouseeme_fusion.pid
            print_status "Sensor fusion started (PID: $FUSION_PID)"
        else
            print_warning "Sensor fusion not found, skipping..."
        fi
    fi
}

# Start rendering engine
start_rendering() {
    if [ "$START_RENDERING" = true ]; then
        print_status "Starting rendering engine..."
        
        # Start Unity viewer if available
        if command -v unity &> /dev/null; then
            unity --project "$PROJECT_ROOT/src/engine/UnityPlugin" &
            UNITY_PID=$!
            echo $UNITY_PID > /tmp/nowyouseeme_unity.pid
            print_status "Unity viewer started (PID: $UNITY_PID)"
        else
            print_warning "Unity not found, skipping rendering..."
        fi
    fi
}

# Start web dashboard
start_dashboard() {
    print_status "Starting web dashboard..."
    
    # Start simple HTTP server for dashboard
    python -m http.server 8080 --directory "$PROJECT_ROOT/docs/dashboard" &
    DASHBOARD_PID=$!
    echo $DASHBOARD_PID > /tmp/nowyouseeme_dashboard.pid
    print_status "Web dashboard started at http://localhost:8080 (PID: $DASHBOARD_PID)"
}

# Cleanup function
cleanup() {
    print_status "Shutting down holodeck environment..."
    
    # Kill all background processes
    for pid_file in /tmp/nowyouseeme_*.pid; do
        if [ -f "$pid_file" ]; then
            PID=$(cat "$pid_file")
            if kill -0 "$PID" 2>/dev/null; then
                kill "$PID"
                print_status "Killed process $PID"
            fi
            rm "$pid_file"
        fi
    done
    
    print_status "Holodeck environment stopped"
}

# Set up signal handlers
trap cleanup EXIT INT TERM

# Main startup sequence
main() {
    echo -e "${BLUE}================================${NC}"
    echo -e "${BLUE}  Starting NowYouSeeMe Holodeck${NC}"
    echo -e "${BLUE}================================${NC}"
    echo ""
    
    # Check environment
    check_environment
    
    # Start components
    start_sync_service
    sleep 1
    
    start_camera
    sleep 1
    
    start_csi
    sleep 1
    
    start_rf_slam
    sleep 1
    
    start_vision_slam
    sleep 1
    
    start_fusion
    sleep 1
    
    start_rendering
    sleep 1
    
    start_dashboard
    
    echo ""
    print_status "Holodeck environment started successfully!"
    echo ""
    echo -e "${GREEN}Services running:${NC}"
    echo "  - Time sync: $(cat /tmp/nowyouseeme_sync.pid 2>/dev/null || echo 'Not running')"
    echo "  - Camera: $(cat /tmp/nowyouseeme_camera.pid 2>/dev/null || echo 'Not running')"
    echo "  - CSI: $(cat /tmp/nowyouseeme_csi.pid 2>/dev/null || echo 'Not running')"
    echo "  - RF SLAM: $(cat /tmp/nowyouseeme_rf_slam.pid 2>/dev/null || echo 'Not running')"
    echo "  - Vision SLAM: $(cat /tmp/nowyouseeme_vision_slam.pid 2>/dev/null || echo 'Not running')"
    echo "  - Fusion: $(cat /tmp/nowyouseeme_fusion.pid 2>/dev/null || echo 'Not running')"
    echo "  - Unity: $(cat /tmp/nowyouseeme_unity.pid 2>/dev/null || echo 'Not running')"
    echo "  - Dashboard: $(cat /tmp/nowyouseeme_dashboard.pid 2>/dev/null || echo 'Not running')"
    echo ""
    echo -e "${GREEN}Access points:${NC}"
    echo "  - Web dashboard: http://localhost:8080"
    echo "  - Unity viewer: Check Unity window"
    echo ""
    echo "Press Ctrl+C to stop all services"
    
    # Wait for user interrupt
    wait
}

# Run main function
main "$@"