param(
    [Parameter(Mandatory = $true)][string]$Prepared,
    [Parameter(Mandatory = $true)][string]$OutputRoot,
    [Parameter(Mandatory = $true)][double]$ReferenceHeight,
    [string]$ReferencePoseFile = "reference_poses_rtk_gga_raw_heading.csv",
    [int]$MinStations = 20,
    [int]$MinPairs = 20,
    [int]$Bootstrap = 100,
    # Roof-mounted H32 (~2 m): ground near z≈-2. Old [-1.4,-0.4] fits walls on this vehicle.
    [double]$GroundZMin = -2.5,
    [double]$GroundZMax = -1.5,
    [int]$SmallGicpMaxGap = 26,
    [int]$Open3DMaxGap = 26,
    [double]$MaxReferenceTranslationM = 8.0,
    [string]$SolverInitialExtrinsic = "",
    [double]$RefineMinInlierRatio = 0.63,
    [double]$RefineMaxInlierRmseM = 0.14
)

$ErrorActionPreference = "Stop"
$Repo = Split-Path -Parent $PSScriptRoot
$Code = Join-Path $Repo "code\rigorous_calibration.py"
$Refine = Join-Path $Repo "code\refine_pairs.py"
$Consensus = Join-Path $Repo "code\cross_backend_filter.py"
$Frames = Join-Path $Prepared "frames_all"
$ReferencePoses = Join-Path $Prepared $ReferencePoseFile
$Common = Join-Path $OutputRoot "common"
$Open = Join-Path $OutputRoot "open3d_gicp"
$Small = Join-Path $OutputRoot "small_gicp"
$ConsensusOut = Join-Path $OutputRoot "consensus"

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 @($Frames, $ReferencePoses)) {
    if (-not (Test-Path -LiteralPath $Path)) { throw "Input does not exist: $Path" }
}
New-Item -ItemType Directory -Force -Path $Common,$Open,$Small,$ConsensusOut | Out-Null

$Ground = Join-Path $Common "ground_planes.csv"
Run-Python "ground planes" @($Code, "ground", "--frames", $Frames, "--output", $Ground,
    "--z-min", "$GroundZMin", "--z-max", "$GroundZMax")

foreach ($Backend in @("small_gicp", "open3d")) {
    $Directory = if ($Backend -eq "small_gicp") { $Small } else { $Open }
    $Raw = Join-Path $Directory "B_estimation.npz"
    $QualityJson = Join-Path $Directory "B_quality.json"
    $QualityCsv = Join-Path $Directory "B_quality.csv"
    $MaxGap = if ($Backend -eq "open3d") { $Open3DMaxGap } else { $SmallGicpMaxGap }
    $PairArgs = @($Code, "pairs", "--backend", $Backend, "--frames", $Frames, "--reference-poses", $ReferencePoses,
                  "--output", $Raw, "--quality-json", $QualityJson, "--quality-csv", $QualityCsv,
                  "--min-stations", "$MinStations", "--min-pairs", "$MinPairs", "--max-gap", "$MaxGap")
    if ($MaxReferenceTranslationM -gt 0) {
        $PairArgs += @("--max-reference-translation", "$MaxReferenceTranslationM")
    }
    if ($Backend -eq "open3d") { $PairArgs += @("--multistart", "1", "--iterations", "40") }
    Run-Python "$Backend pairs" $PairArgs
    Run-Python "$Backend X-independent refinement" @(
        $Refine, "--pairs", $Raw, "--quality-json", $QualityJson,
        "--output", (Join-Path $Directory "B_refined.npz"), "--min-pairs", "$MinPairs",
        "--min-inlier-ratio", "$RefineMinInlierRatio",
        "--max-inlier-rmse", "$RefineMaxInlierRmseM"
    )
    $CalibrationArgs = @(
        $Code, "calibrate", "--pairs", (Join-Path $Directory "B_refined.npz"),
        "--ground-planes", $Ground, "--reference-height", "$ReferenceHeight",
        "--bootstrap", "$Bootstrap", "--output", (Join-Path $Directory "extrinsic.json")
    )
    if (-not [string]::IsNullOrWhiteSpace($SolverInitialExtrinsic)) {
        $CalibrationArgs += @("--initial-extrinsic", $SolverInitialExtrinsic)
    }
    Run-Python "$Backend calibration" $CalibrationArgs
}

$ConsensusPairs = Join-Path $ConsensusOut "B_consensus.npz"
Run-Python "cross-backend consensus" @(
    $Consensus, "--open3d-pairs", (Join-Path $Open "B_refined.npz"),
    "--small-pairs", (Join-Path $Small "B_refined.npz"),
    "--output", $ConsensusPairs, "--min-pairs", "$MinPairs"
)
$ConsensusCalibrationArgs = @(
    $Code, "calibrate", "--pairs", $ConsensusPairs, "--ground-planes", $Ground,
    "--reference-height", "$ReferenceHeight", "--bootstrap", "$Bootstrap",
    "--output", (Join-Path $ConsensusOut "extrinsic.json")
)
if (-not [string]::IsNullOrWhiteSpace($SolverInitialExtrinsic)) {
    $ConsensusCalibrationArgs += @("--initial-extrinsic", $SolverInitialExtrinsic)
}
Run-Python "consensus calibration" $ConsensusCalibrationArgs

Write-Host "Calibration results: $OutputRoot"