param(
    [Parameter(Mandatory = $true)][string]$Batch1Prepared,
    [Parameter(Mandatory = $true)][string]$Batch2Prepared,
    [string]$OutputRoot = ""
)

$ErrorActionPreference = "Stop"
$Repo = Split-Path -Parent $PSScriptRoot
if ([string]::IsNullOrWhiteSpace($OutputRoot)) {
    $OutputRoot = Join-Path $Repo "results\01_previous_two_batches"
}
$Code = Join-Path $Repo "code\rigorous_calibration.py"
$Refine = Join-Path $Repo "code\refine_pairs.py"
$Summary = Join-Path $Repo "code\summarize_results.py"
$B1Frames = Join-Path $Batch1Prepared "frames_all"
$B2Frames = Join-Path $Batch2Prepared "frames_all"
$B1Body = Join-Path $Batch1Prepared "body_poses_rear_gga_raw_rear_to_front.csv"
$B2Body = Join-Path $Batch2Prepared "body_poses_rear_gga_raw_rear_to_front.csv"

function Run-Python {
    param([string]$Stage, [string[]]$Arguments)
    Write-Host "[$Stage]"
    & python @Arguments
    if ($LASTEXITCODE -ne 0) { throw "$Stage failed with Python exit code $LASTEXITCODE" }
}

foreach ($Path in @($B1Frames, $B2Frames, $B1Body, $B2Body)) {
    if (-not (Test-Path -LiteralPath $Path)) { throw "Input does not exist: $Path" }
}
$Common = Join-Path $OutputRoot "common"
$Small = Join-Path $OutputRoot "small_gicp"
$Open = Join-Path $OutputRoot "open3d_gicp"
New-Item -ItemType Directory -Force -Path $Common,$Small,$Open | Out-Null

Run-Python "batch2 ground planes" @($Code, "ground", "--frames", $B2Frames, "--output", (Join-Path $Common "ground_planes_batch2.csv"))

foreach ($Backend in @("small_gicp", "open3d")) {
    $Directory = if ($Backend -eq "small_gicp") { $Small } else { $Open }
    $Extra = @()
    if ($Backend -eq "open3d") { $Extra = @("--max-gap", "3", "--multistart", "1", "--iterations", "40") }
    $B2Raw = Join-Path $Directory "B_batch2_estimation.npz"
    $B2QualityJson = Join-Path $Directory "B_batch2_quality.json"
    $B2QualityCsv = Join-Path $Directory "B_batch2_quality.csv"
    Run-Python "$Backend batch2 pairs" (@($Code, "pairs", "--backend", $Backend, "--frames", $B2Frames, "--body", $B2Body,
        "--output", $B2Raw, "--quality-json", $B2QualityJson, "--quality-csv", $B2QualityCsv) + $Extra)
    $B2Refined = Join-Path $Directory "B_batch2_refined.npz"
    Run-Python "$Backend batch2 refinement" @($Refine, "--pairs", $B2Raw, "--quality-json", $B2QualityJson, "--output", $B2Refined)
    $Extrinsic = Join-Path $Directory "extrinsic_batch2_refined.json"
    Run-Python "$Backend batch2 calibration" @($Code, "calibrate", "--pairs", $B2Refined,
        "--ground-planes", (Join-Path $Common "ground_planes_batch2.csv"), "--output", $Extrinsic)

    $B1Raw = Join-Path $Directory "B_batch1_auxiliary.npz"
    $B1QualityJson = Join-Path $Directory "B_batch1_quality.json"
    $B1QualityCsv = Join-Path $Directory "B_batch1_quality.csv"
    Run-Python "$Backend batch1 auxiliary pairs" @($Code, "pairs", "--backend", $Backend, "--frames", $B1Frames, "--body", $B1Body,
        "--output", $B1Raw, "--quality-json", $B1QualityJson, "--quality-csv", $B1QualityCsv,
        "--max-gap", "3", "--multistart", "1", "--iterations", "40")
    $B1Refined = Join-Path $Directory "B_batch1_auxiliary_refined.npz"
    Run-Python "$Backend batch1 refinement" @($Refine, "--pairs", $B1Raw, "--quality-json", $B1QualityJson, "--output", $B1Refined)
    Run-Python "$Backend batch1 auxiliary check" @($Code, "validate", "--pairs", $B1Refined,
        "--extrinsic", $Extrinsic, "--output", (Join-Path $Directory "batch1_auxiliary_check.json"))
}

Run-Python "backend summary" @($Summary,
    "--open3d", (Join-Path $Open "extrinsic_batch2_refined.json"),
    "--small", (Join-Path $Small "extrinsic_batch2_refined.json"),
    "--open3d-quality", (Join-Path $Open "B_batch2_quality.json"),
    "--small-quality", (Join-Path $Small "B_batch2_quality.json"),
    "--open3d-check", (Join-Path $Open "batch1_auxiliary_check.json"),
    "--small-check", (Join-Path $Small "batch1_auxiliary_check.json"),
    "--output", (Join-Path $OutputRoot "comparison_summary.json"),
    "--recommended-output", (Join-Path $OutputRoot "final_extrinsic_recommended.json"))

Write-Host "Two-batch results: $OutputRoot"