# Unity-MoveIt2 系统快速部署脚本 (Windows PowerShell)
# 此脚本自动化部署Unity-MoveIt2系统的所有组件

param(
    [string]$Mode = "full",  # full, ros2-only, unity-only
    [string]$RobotName = "niryo_one",
    [switch]$SkipDocker = $false,
    [switch]$Development = $false,
    [switch]$Verbose = $false
)

# 设置错误处理
$ErrorActionPreference = "Stop"

# 颜色输出函数
function Write-ColorOutput {
    param(
        [string]$Message,
        [string]$Color = "White"
    )
    
    $colors = @{
        "Red" = [ConsoleColor]::Red
        "Green" = [ConsoleColor]::Green
        "Yellow" = [ConsoleColor]::Yellow
        "Blue" = [ConsoleColor]::Blue
        "Cyan" = [ConsoleColor]::Cyan
        "White" = [ConsoleColor]::White
    }
    
    Write-Host $Message -ForegroundColor $colors[$Color]
}

function Write-Step {
    param([string]$Message)
    Write-ColorOutput "🔄 $Message" "Cyan"
}

function Write-Success {
    param([string]$Message)
    Write-ColorOutput "✅ $Message" "Green"
}

function Write-Warning {
    param([string]$Message)
    Write-ColorOutput "⚠️  $Message" "Yellow"
}

function Write-Error {
    param([string]$Message)
    Write-ColorOutput "❌ $Message" "Red"
}

# 检查系统要求
function Test-SystemRequirements {
    Write-Step "检查系统要求..."
    
    # 检查PowerShell版本
    if ($PSVersionTable.PSVersion.Major -lt 5) {
        throw "需要PowerShell 5.0或更高版本"
    }
    
    # 检查操作系统
    if (-not $IsWindows -and $PSVersionTable.PSVersion.Major -ge 6) {
        Write-Warning "此脚本主要为Windows设计"
    }
    
    # 检查Docker（如果需要）
    if (-not $SkipDocker) {
        try {
            $dockerVersion = docker --version
            Write-Success "Docker已安装: $dockerVersion"
        }
        catch {
            Write-Warning "Docker未安装或不可用，将跳过Docker部署"
            $script:SkipDocker = $true
        }
    }
    
    # 检查Git
    try {
        $gitVersion = git --version
        Write-Success "Git已安装: $gitVersion"
    }
    catch {
        throw "Git未安装，请先安装Git"
    }
    
    # 检查Python（用于ROS2）
    try {
        $pythonVersion = python --version
        Write-Success "Python已安装: $pythonVersion"
    }
    catch {
        Write-Warning "Python未安装，ROS2功能可能不可用"
    }
    
    Write-Success "系统要求检查完成"
}

# 创建目录结构
function New-ProjectStructure {
    Write-Step "创建项目目录结构..."
    
    $directories = @(
        "logs",
        "cache",
        "temp",
        "backup",
        "models",
        "unity-project/Assets/Scripts",
        "unity-project/Assets/Prefabs",
        "unity-project/Assets/Materials",
        "ros2-workspace/src",
        "ros2-workspace/build",
        "ros2-workspace/install",
        "analysis-service/src",
        "analysis-service/config"
    )
    
    foreach ($dir in $directories) {
        $fullPath = Join-Path $PWD $dir
        if (-not (Test-Path $fullPath)) {
            New-Item -ItemType Directory -Path $fullPath -Force | Out-Null
            if ($Verbose) {
                Write-Host "  创建目录: $dir"
            }
        }
    }
    
    Write-Success "目录结构创建完成"
}

# 安装ROS2依赖
function Install-ROS2Dependencies {
    Write-Step "安装ROS2依赖..."
    
    # 检查是否在WSL环境中
    if ($env:WSL_DISTRO_NAME) {
        Write-ColorOutput "检测到WSL环境，使用Linux安装方式" "Blue"
        
        # 在WSL中安装ROS2
        $wslCommands = @(
            "sudo apt update",
            "sudo apt install -y curl gnupg2 lsb-release",
            "curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg",
            "echo 'deb [arch=`$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu `$(. /etc/os-release && echo `$UBUNTU_CODENAME) main' | sudo tee /etc/apt/sources.list.d/ros2.list > /dev/null",
            "sudo apt update",
            "sudo apt install -y ros-humble-desktop",
            "sudo apt install -y ros-humble-moveit",
            "sudo apt install -y python3-colcon-common-extensions"
        )
        
        foreach ($cmd in $wslCommands) {
            Write-Host "执行: $cmd"
            wsl bash -c $cmd
        }
    }
    else {
        Write-ColorOutput "Windows环境，请手动安装ROS2 Humble" "Yellow"
        Write-Host "下载地址: https://github.com/ros2/ros2/releases"
    }
    
    Write-Success "ROS2依赖安装完成"
}

# 构建ROS2工作空间
function Build-ROS2Workspace {
    Write-Step "构建ROS2工作空间..."
    
    $workspacePath = Join-Path $PWD "ros2-workspace"
    
    # 创建package.xml模板
    $packageXml = @"
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
  <name>unity_moveit_bridge</name>
  <version>1.0.0</version>
  <description>Unity-MoveIt2 Bridge Package</description>
  <maintainer email="dev@unity-moveit.com">Unity-MoveIt2 Team</maintainer>
  <license>MIT</license>

  <buildtool_depend>ament_cmake</buildtool_depend>
  <depend>rclcpp</depend>
  <depend>rclpy</depend>
  <depend>sensor_msgs</depend>
  <depend>geometry_msgs</depend>
  <depend>trajectory_msgs</depend>
  <depend>moveit_msgs</depend>
  <depend>moveit_core</depend>
  <depend>moveit_ros_planning_interface</depend>
  <depend>ros_tcp_endpoint</depend>

  <test_depend>ament_lint_auto</test_depend>
  <test_depend>ament_lint_common</test_depend>

  <export>
    <build_type>ament_cmake</build_type>
  </export>
</package>
"@
    
    $packagePath = Join-Path $workspacePath "src/unity_moveit_bridge/package.xml"
    New-Item -ItemType Directory -Path (Split-Path $packagePath) -Force | Out-Null
    Set-Content -Path $packagePath -Value $packageXml -Encoding UTF8
    
    # 创建CMakeLists.txt模板
    $cmakeContent = @"
cmake_minimum_required(VERSION 3.8)
project(unity_moveit_bridge)

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

# find dependencies
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rclpy REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(trajectory_msgs REQUIRED)
find_package(moveit_msgs REQUIRED)
find_package(moveit_core REQUIRED)
find_package(moveit_ros_planning_interface REQUIRED)

# Install Python modules
ament_python_install_package(`${PROJECT_NAME})

# Install Python scripts
install(PROGRAMS
  scripts/bridge_node.py
  scripts/planning_node.py
  DESTINATION lib/`${PROJECT_NAME}
)

# Install launch files
install(DIRECTORY launch
  DESTINATION share/`${PROJECT_NAME}/
)

# Install config files
install(DIRECTORY config
  DESTINATION share/`${PROJECT_NAME}/
)

if(BUILD_TESTING)
  find_package(ament_lint_auto REQUIRED)
  ament_lint_auto_find_test_dependencies()
endif()

ament_package()
"@
    
    $cmakePath = Join-Path $workspacePath "src/unity_moveit_bridge/CMakeLists.txt"
    Set-Content -Path $cmakePath -Value $cmakeContent -Encoding UTF8
    
    # 构建工作空间
    if ($env:WSL_DISTRO_NAME) {
        Push-Location $workspacePath
        try {
            wsl bash -c "source /opt/ros/humble/setup.bash && colcon build --symlink-install"
        }
        finally {
            Pop-Location
        }
    }
    
    Write-Success "ROS2工作空间构建完成"
}

# 配置Unity项目
function Set-UnityProject {
    Write-Step "配置Unity项目..."
    
    $unityProjectPath = Join-Path $PWD "unity-project"
    
    # 创建Unity项目配置文件
    $projectSettings = @{
        "companyName" = "Unity-MoveIt2"
        "productName" = "RobotSimulation"
        "version" = "1.0.0"
        "bundleVersion" = "1.0"
    }
    
    $settingsJson = $projectSettings | ConvertTo-Json -Depth 3
    $settingsPath = Join-Path $unityProjectPath "ProjectSettings/ProjectSettings.json"
    
    New-Item -ItemType Directory -Path (Split-Path $settingsPath) -Force | Out-Null
    Set-Content -Path $settingsPath -Value $settingsJson -Encoding UTF8
    
    # 创建基本的C#脚本模板
    $robotControllerScript = @"
using UnityEngine;
using Unity.Robotics.ROSTCPConnector;
using RosMessageTypes.Sensor;
using RosMessageTypes.Geometry;

namespace UnityMoveIt2
{
    public class RobotController : MonoBehaviour
    {
        [Header("ROS Configuration")]
        public string rosIPAddress = "127.0.0.1";
        public int rosPort = 10000;
        
        [Header("Robot Configuration")]
        public string robotName = "niryo_one";
        public Transform[] jointTransforms;
        
        private ROSConnection ros;
        
        void Start()
        {
            // 初始化ROS连接
            ROSConnection.GetOrCreateInstance().Connect(rosIPAddress, rosPort);
            ros = ROSConnection.GetOrCreateInstance();
            
            // 订阅关节状态
            ros.Subscribe<JointStateMsg>("/joint_states", OnJointStateReceived);
            
            Debug.Log("Unity机器人控制器初始化完成");
        }
        
        void OnJointStateReceived(JointStateMsg jointState)
        {
            // 更新关节位置
            UpdateJointPositions(jointState.position);
        }
        
        void UpdateJointPositions(double[] positions)
        {
            for (int i = 0; i < Mathf.Min(positions.Length, jointTransforms.Length); i++)
            {
                if (jointTransforms[i] != null)
                {
                    // 根据关节类型更新旋转
                    jointTransforms[i].localRotation = Quaternion.Euler(0, 0, (float)(positions[i] * Mathf.Rad2Deg));
                }
            }
        }
        
        public void SendJointCommand(float[] targetPositions)
        {
            // 发送关节命令到ROS2
            // 实现细节...
        }
    }
}
"@
    
    $scriptPath = Join-Path $unityProjectPath "Assets/Scripts/RobotController.cs"
    New-Item -ItemType Directory -Path (Split-Path $scriptPath) -Force | Out-Null
    Set-Content -Path $scriptPath -Value $robotControllerScript -Encoding UTF8
    
    Write-Success "Unity项目配置完成"
}

# 部署Docker服务
function Deploy-DockerServices {
    if ($SkipDocker) {
        Write-Warning "跳过Docker部署"
        return
    }
    
    Write-Step "部署Docker服务..."
    
    $dockerComposePath = Join-Path $PWD "configs/docker/docker-compose.yml"
    
    if (-not (Test-Path $dockerComposePath)) {
        Write-Error "Docker Compose文件不存在: $dockerComposePath"
        return
    }
    
    try {
        # 构建镜像
        Write-Host "构建Docker镜像..."
        docker-compose -f $dockerComposePath build
        
        # 启动服务
        Write-Host "启动Docker服务..."
        docker-compose -f $dockerComposePath up -d
        
        # 等待服务启动
        Start-Sleep -Seconds 10
        
        # 检查服务状态
        $services = docker-compose -f $dockerComposePath ps --format json | ConvertFrom-Json
        
        foreach ($service in $services) {
            if ($service.State -eq "running") {
                Write-Success "服务 $($service.Service) 运行正常"
            }
            else {
                Write-Warning "服务 $($service.Service) 状态: $($service.State)"
            }
        }
    }
    catch {
        Write-Error "Docker部署失败: $_"
    }
    
    Write-Success "Docker服务部署完成"
}

# 验证部署
function Test-Deployment {
    Write-Step "验证部署..."
    
    $tests = @()
    
    # 测试ROS2服务
    if (-not $SkipDocker) {
        try {
            $rosServices = docker exec unity-ros2-bridge ros2 service list 2>$null
            if ($rosServices -match "plan_kinematic_path") {
                $tests += @{ Name = "ROS2规划服务"; Status = "✅" }
            }
            else {
                $tests += @{ Name = "ROS2规划服务"; Status = "❌" }
            }
        }
        catch {
            $tests += @{ Name = "ROS2规划服务"; Status = "❌" }
        }
    }
    
    # 测试网络连接
    try {
        $tcpTest = Test-NetConnection -ComputerName "localhost" -Port 10000 -WarningAction SilentlyContinue
        if ($tcpTest.TcpTestSucceeded) {
            $tests += @{ Name = "TCP连接(10000)"; Status = "✅" }
        }
        else {
            $tests += @{ Name = "TCP连接(10000)"; Status = "❌" }
        }
    }
    catch {
        $tests += @{ Name = "TCP连接(10000)"; Status = "❌" }
    }
    
    # 测试文件结构
    $requiredFiles = @(
        "configs/robots/niryo_one.yaml",
        "configs/unity/unity_settings.json",
        "configs/ros2/bridge_config.yaml",
        "README.md"
    )
    
    foreach ($file in $requiredFiles) {
        if (Test-Path $file) {
            $tests += @{ Name = "文件: $file"; Status = "✅" }
        }
        else {
            $tests += @{ Name = "文件: $file"; Status = "❌" }
        }
    }
    
    # 显示测试结果
    Write-Host "`n部署验证结果:" -ForegroundColor Cyan
    Write-Host "=" * 50
    
    foreach ($test in $tests) {
        Write-Host "$($test.Status) $($test.Name)"
    }
    
    $successCount = ($tests | Where-Object { $_.Status -eq "✅" }).Count
    $totalCount = $tests.Count
    
    Write-Host "=" * 50
    Write-Host "通过: $successCount/$totalCount" -ForegroundColor $(if ($successCount -eq $totalCount) { "Green" } else { "Yellow" })
    
    if ($successCount -eq $totalCount) {
        Write-Success "所有验证测试通过！"
    }
    else {
        Write-Warning "部分验证测试失败，请检查配置"
    }
}

# 生成启动脚本
function New-StartupScripts {
    Write-Step "生成启动脚本..."
    
    # Windows启动脚本
    $startScript = @"
@echo off
echo 启动Unity-MoveIt2系统...

REM 检查Docker服务
docker-compose -f configs/docker/docker-compose.yml ps >nul 2>&1
if %errorlevel% neq 0 (
    echo 启动Docker服务...
    docker-compose -f configs/docker/docker-compose.yml up -d
    timeout /t 10 /nobreak >nul
)

REM 检查服务状态
echo 检查服务状态...
docker-compose -f configs/docker/docker-compose.yml ps

echo.
echo Unity-MoveIt2系统已启动
echo 请在Unity中连接到 localhost:10000
echo.
pause
"@
    
    Set-Content -Path "start-system.bat" -Value $startScript -Encoding UTF8
    
    # PowerShell启动脚本
    $psStartScript = @"
# Unity-MoveIt2 系统启动脚本
Write-Host "启动Unity-MoveIt2系统..." -ForegroundColor Green

# 启动Docker服务
if (Get-Command docker-compose -ErrorAction SilentlyContinue) {
    Write-Host "启动Docker服务..."
    docker-compose -f configs/docker/docker-compose.yml up -d
    Start-Sleep -Seconds 10
    
    Write-Host "服务状态:"
    docker-compose -f configs/docker/docker-compose.yml ps
}
else {
    Write-Warning "Docker Compose未安装，请手动启动ROS2服务"
}

Write-Host "`nUnity-MoveIt2系统已启动" -ForegroundColor Green
Write-Host "请在Unity中连接到 localhost:10000" -ForegroundColor Cyan
"@
    
    Set-Content -Path "start-system.ps1" -Value $psStartScript -Encoding UTF8
    
    Write-Success "启动脚本生成完成"
}

# 主部署函数
function Start-Deployment {
    Write-ColorOutput "`n🚀 Unity-MoveIt2 系统快速部署" "Blue"
    Write-ColorOutput "=" * 50 "Blue"
    
    Write-Host "部署模式: $Mode"
    Write-Host "机器人: $RobotName"
    Write-Host "跳过Docker: $SkipDocker"
    Write-Host "开发模式: $Development"
    Write-Host ""
    
    try {
        # 执行部署步骤
        Test-SystemRequirements
        New-ProjectStructure
        
        if ($Mode -eq "full" -or $Mode -eq "ros2-only") {
            Install-ROS2Dependencies
            Build-ROS2Workspace
        }
        
        if ($Mode -eq "full" -or $Mode -eq "unity-only") {
            Set-UnityProject
        }
        
        if ($Mode -eq "full" -and -not $SkipDocker) {
            Deploy-DockerServices
        }
        
        New-StartupScripts
        Test-Deployment
        
        Write-ColorOutput "`n🎉 部署完成！" "Green"
        Write-ColorOutput "=" * 50 "Green"
        
        Write-Host "`n下一步操作:"
        Write-Host "1. 运行 start-system.ps1 启动系统"
        Write-Host "2. 在Unity中打开 unity-project 项目"
        Write-Host "3. 配置ROS连接到 localhost:10000"
        Write-Host "4. 开始使用Unity-MoveIt2系统！"
        
    }
    catch {
        Write-Error "部署失败: $_"
        Write-Host "`n请检查错误信息并重试"
        exit 1
    }
}

# 显示帮助信息
function Show-Help {
    Write-Host @"
Unity-MoveIt2 快速部署脚本

用法:
    .\quick-deploy.ps1 [参数]

参数:
    -Mode <string>        部署模式 (full|ros2-only|unity-only) [默认: full]
    -RobotName <string>   机器人名称 [默认: niryo_one]
    -SkipDocker          跳过Docker部署
    -Development         开发模式（包含调试工具）
    -Verbose             详细输出
    -Help                显示此帮助信息

示例:
    .\quick-deploy.ps1                          # 完整部署
    .\quick-deploy.ps1 -Mode ros2-only          # 仅部署ROS2
    .\quick-deploy.ps1 -SkipDocker -Verbose     # 跳过Docker，详细输出
    .\quick-deploy.ps1 -Development             # 开发模式部署

"@
}

# 脚本入口点
if ($args -contains "-Help" -or $args -contains "--help" -or $args -contains "-h") {
    Show-Help
    exit 0
}

# 开始部署
Start-Deployment