param(
    [Parameter(Mandatory = $true)][string[]]$BatchNames,
    [Parameter(Mandatory = $true)][string[]]$Pairs,
    [Parameter(Mandatory = $true)][string[]]$GroundPlanes,
    [Parameter(Mandatory = $true)][string]$OutputRoot,
    [Parameter(Mandatory = $true)][double]$RtkReferenceHeightAboveGroundM,
    [int]$Bootstrap = 200,
    [double]$MaxBatchTranslationDifferenceM = 0.25,
    [double]$MaxBatchRotationDifferenceDeg = 5.0
)

$ErrorActionPreference = "Stop"
$Repo = Split-Path -Parent $PSScriptRoot

if ($BatchNames.Count -lt 2) {
    throw "At least two independent batches are required"
}
if (($Pairs.Count -ne $BatchNames.Count) -or ($GroundPlanes.Count -ne $BatchNames.Count)) {
    throw "BatchNames, Pairs, and GroundPlanes must have the same number of entries"
}
if ($RtkReferenceHeightAboveGroundM -le 0.0) {
    throw "RtkReferenceHeightAboveGroundM must be greater than zero"
}
foreach ($Path in @($Pairs + $GroundPlanes)) {
    if (-not (Test-Path -LiteralPath $Path)) {
        throw "Input does not exist: $Path"
    }
}

$PreflightRoot = Join-Path $OutputRoot "preflight"
New-Item -ItemType Directory -Force -Path $PreflightRoot | Out-Null
$BatchExtrinsics = @()
for ($Index = 0; $Index -lt $BatchNames.Count; $Index++) {
    $SafeName = $BatchNames[$Index] -replace '[^A-Za-z0-9_.-]', '_'
    $BatchExtrinsic = Join-Path $PreflightRoot "$SafeName.json"
    Write-Host "[preflight batch: $($BatchNames[$Index])]"
    & python (Join-Path $Repo "code\rigorous_calibration.py") calibrate `
        --pairs $Pairs[$Index] `
        --ground-planes $GroundPlanes[$Index] `
        --reference-height $RtkReferenceHeightAboveGroundM `
        --bootstrap 0 `
        --output $BatchExtrinsic | Out-Null
    if ($LASTEXITCODE -ne 0) { throw "Batch preflight failed: $($BatchNames[$Index])" }
    $BatchExtrinsics += $BatchExtrinsic
}

for ($Index = 1; $Index -lt $BatchNames.Count; $Index++) {
    $SafeName = $BatchNames[$Index] -replace '[^A-Za-z0-9_.-]', '_'
    $ComparisonPath = Join-Path $PreflightRoot "$SafeName-vs-batch0.json"
    & python (Join-Path $Repo "code\compare_extrinsics.py") `
        --reference $BatchExtrinsics[0] `
        --candidate $BatchExtrinsics[$Index] `
        --output $ComparisonPath | Out-Null
    if ($LASTEXITCODE -ne 0) { throw "Batch comparison failed: $($BatchNames[$Index])" }

    $Comparison = Get-Content -LiteralPath $ComparisonPath -Raw | ConvertFrom-Json
    $TranslationDifference = [double]$Comparison.relative_translation_norm_m
    $RotationDifference = [double]$Comparison.relative_rotation_deg
    Write-Host ("[preflight consistency] {0} vs {1}: {2:F4} m / {3:F3} deg" -f `
        $BatchNames[$Index], $BatchNames[0], $TranslationDifference, $RotationDifference)
    if (($TranslationDifference -gt $MaxBatchTranslationDifferenceM) -or
        ($RotationDifference -gt $MaxBatchRotationDifferenceDeg)) {
        throw ("Batch extrinsics are inconsistent: {0} vs {1} = {2:F4} m / {3:F3} deg; " +
            "check RTK heading/frame convention and sensor installation") -f `
            $BatchNames[$Index], $BatchNames[0], $TranslationDifference, $RotationDifference
    }
}

$InputRoot = Join-Path $OutputRoot "inputs"
$JointPairs = Join-Path $InputRoot "joint_consensus_pairs.npz"
$JointGroundPlanes = Join-Path $InputRoot "joint_ground_planes.csv"
$InputSummary = Join-Path $InputRoot "joint_input_summary.json"
$Extrinsic = Join-Path $OutputRoot "shared_T_RTK_lidar.json"

$BuildArgs = @((Join-Path $Repo "code\build_joint_rtk_lidar_inputs.py"))
for ($Index = 0; $Index -lt $BatchNames.Count; $Index++) {
    $BuildArgs += @(
        "--batch-name", $BatchNames[$Index],
        "--pairs", $Pairs[$Index],
        "--ground-planes", $GroundPlanes[$Index]
    )
}
$BuildArgs += @(
    "--output-pairs", $JointPairs,
    "--output-ground-planes", $JointGroundPlanes,
    "--summary", $InputSummary
)

Write-Host "[combine independent batches]"
& python @BuildArgs
if ($LASTEXITCODE -ne 0) { throw "Combining independent batches failed" }

Write-Host "[solve shared T_RTK_lidar]"
& python (Join-Path $Repo "code\rigorous_calibration.py") calibrate `
    --pairs $JointPairs `
    --ground-planes $JointGroundPlanes `
    --reference-height $RtkReferenceHeightAboveGroundM `
    --bootstrap $Bootstrap `
    --output $Extrinsic
if ($LASTEXITCODE -ne 0) { throw "Shared RTK-LiDAR calibration failed" }

Write-Host "Shared T_RTK_lidar: $Extrinsic"
Write-Host "Joint input summary: $InputSummary"