param(
    [Parameter(Mandatory = $true)][string]$CombinedRoot,
    [Parameter(Mandatory = $true)][double]$RtkReferenceHeightAboveGroundM,
    [string]$OutputRoot = "",
    [string]$WorkRoot = "",
    [int]$ExpectedStations = 27,
    [int]$MinStations = 20,
    [int]$MinPairs = 20,
    [int]$Bootstrap = 200,
    # Roof-mounted H32 (~2 m): ground points are near z≈-2 in the LiDAR frame (Z-up).
    # The old [-1.4, -0.4] window fits walls on this vehicle and must not be reused.
    [double]$GroundZMin = -2.5,
    [double]$GroundZMax = -1.5,
    [int]$SmallGicpMaxGap = 26,
    [int]$Open3DMaxGap = 26,
    [double]$MaxReferenceTranslationM = 8.0,
    # Baseline frame: rawHeading as RTK X. Default vehicle-forward for this car: -90
    # (master/slave swapped, baseline points vehicle-right).
    [double]$HeadingOffsetDeg = -90.0,
    [string]$SolverInitialExtrinsic = "",
    [double]$RefineMinInlierRatio = 0.63,
    [double]$RefineMaxInlierRmseM = 0.14
)

$ErrorActionPreference = "Stop"
$Repo = Split-Path -Parent $PSScriptRoot
if ([string]::IsNullOrWhiteSpace($OutputRoot)) { $OutputRoot = Join-Path $Repo "outputs\rtk_lidar_calibration" }
if ([string]::IsNullOrWhiteSpace($WorkRoot)) { $WorkRoot = Join-Path $Repo "work\prepared_rtk_direct" }
if ([string]::IsNullOrWhiteSpace($SolverInitialExtrinsic)) {
    $SolverInitialExtrinsic = Join-Path $PSScriptRoot "rtk_lidar_mechanical_initial.json"
}

$PoseName = if ([math]::Abs($HeadingOffsetDeg) -le 1e-12) {
    "rtk_gga_raw_heading"
} else {
    "rtk_vehicle_heading"
}
$ReferencePoseFile = "reference_poses_${PoseName}.csv"
$Prepared = $WorkRoot

& (Join-Path $Repo "run\prepare_multisensor_dataset.ps1") `
    -CombinedRoot $CombinedRoot -Output $Prepared -HeadingOffsetDeg $HeadingOffsetDeg `
    -AntennaLever @(0.0,0.0,0.0) -PoseName $PoseName -MinStations $MinStations `
    -ExpectedStations $ExpectedStations -Overwrite
if ($LASTEXITCODE -ne 0) { throw "RTK-direct dataset preparation failed" }

# Pair registration intentionally has no --initial-extrinsic (B must stay X-independent).
# SolverInitialExtrinsic is applied only in the final AX=XB calibrate stage.
& (Join-Path $Repo "run\run_single_dataset.ps1") `
    -Prepared $Prepared -OutputRoot $OutputRoot `
    -ReferencePoseFile $ReferencePoseFile `
    -ReferenceHeight $RtkReferenceHeightAboveGroundM `
    -MinStations $MinStations -MinPairs $MinPairs -Bootstrap $Bootstrap `
    -GroundZMin $GroundZMin -GroundZMax $GroundZMax `
    -SmallGicpMaxGap $SmallGicpMaxGap -Open3DMaxGap $Open3DMaxGap `
    -MaxReferenceTranslationM $MaxReferenceTranslationM `
    -SolverInitialExtrinsic $SolverInitialExtrinsic `
    -RefineMinInlierRatio $RefineMinInlierRatio `
    -RefineMaxInlierRmseM $RefineMaxInlierRmseM
if ($LASTEXITCODE -ne 0) { throw "RTK-direct calibration failed" }

$Finalize = @(
    (Join-Path $Repo "code\finalize_direct_rtk_lidar.py"),
    "--result-root", $OutputRoot,
    "--reference-height", "$RtkReferenceHeightAboveGroundM",
    "--heading-offset-deg", "$HeadingOffsetDeg"
)
& python @Finalize
if ($LASTEXITCODE -ne 0) { throw "Final result packaging failed" }

Write-Host "Final T_RTK_lidar: $(Join-Path $OutputRoot 'final_T_RTK_lidar.json')"