param(
    [string]$AssemblyPath = (Join-Path $PSScriptRoot '..\bin\Debug\netstandard2.0\ClumsyPilot.dll'),
    [string]$FixturePath = (Join-Path $PSScriptRoot '..\ParkrobTrajplanner\PathSmoothing\Test\Fixtures\path-smoothing-fixtures.json')
)

$ErrorActionPreference = 'Stop'
$newtonsoft = Join-Path $env:USERPROFILE '.nuget\packages\newtonsoft.json\13.0.4\lib\netstandard2.0\Newtonsoft.Json.dll'
if (Test-Path $newtonsoft) { $null = [Reflection.Assembly]::LoadFrom($newtonsoft) }
$assembly = [Reflection.Assembly]::LoadFrom((Resolve-Path $AssemblyPath))

function Assert-True($Actual, [string]$Message) { if (-not $Actual) { throw $Message } }
function Assert-Equal($Expected, $Actual, [string]$Message) {
    if ($Expected -ne $Actual) { throw "$Message Expected=$Expected Actual=$Actual" }
}
function Assert-Near([double]$Expected, [double]$Actual, [string]$Message) {
    if ([Math]::Abs($Expected - $Actual) -gt 0.000000001) {
        throw "$Message Expected=$Expected Actual=$Actual"
    }
}
function Get-RequiredType([string]$Name) { return $assembly.GetType($Name, $true) }

$root = 'MultiWheelC.TrajectoryPlanning.PathSmoothing.'
$facade = $root + 'Facade.'
$test = $root + 'Test.'
$serviceType = Get-RequiredType ($facade + 'PathSmoothingService')
$requestType = Get-RequiredType ($root + 'PathSmoothingRequest')
$configurationType = Get-RequiredType ($root + 'PathSmoothingConfiguration')
$methodType = Get-RequiredType ($root + 'SmoothingMethod')
$factoryType = Get-RequiredType ($test + 'SmoothingScenarioFactory')
$localG2 = [Enum]::Parse($methodType, 'LocalG2Quintic')
$service = [Activator]::CreateInstance($serviceType)
$smooth = $serviceType.GetMethod('Smooth', [Type[]]@($requestType, [Threading.CancellationToken]))
$createFixtures = $factoryType.GetMethod('CreateFixtureRequests', [Type[]]@([string]))
Assert-True ($null -ne $smooth) 'Local G2 must use the public service entrypoint.'

function New-LocalG2Request($BaseRequest, [scriptblock]$Configure = $null) {
    $configuration = [Activator]::CreateInstance($configurationType)
    $configuration.Method = $localG2
    $configuration.AllowFallbackToCoarsePath = $true
    if ($null -ne $Configure) { & $Configure $configuration }
    return [Activator]::CreateInstance($requestType, @(
        $BaseRequest.CoarsePath, $BaseRequest.Segments, $BaseRequest.Map,
        $BaseRequest.Vehicle, $configuration))
}

function Invoke-LocalG2($Request, [Threading.CancellationToken]$CancellationToken = [Threading.CancellationToken]::None) {
    return $smooth.Invoke($service, @($Request, $CancellationToken))
}

function Assert-PublishedPath($Result, [string]$Name) {
    Assert-True ($Result.Path.Count -gt 0) "$Name must publish a complete path."
    Assert-Equal 0 $Result.Segments[0].StartIndex "$Name segments must start at zero."
    Assert-Equal ($Result.Path.Count - 1) $Result.Segments[-1].EndIndex "$Name segments must cover the path."
    Assert-True $Result.Diagnostics.Metrics.IsFeasible "$Name diagnostics must be feasible."
    Assert-True ($Result.Diagnostics.Metrics.MinimumBodyClearanceMeters -ge 0.02) "$Name must retain the configured clearance reserve."
}

function Assert-Equivalent($First, $Second, [string]$Name) {
    Assert-Equal $First.Status $Second.Status "$Name must retain deterministic status."
    Assert-Equal $First.Path.Count $Second.Path.Count "$Name must retain deterministic point count."
    Assert-Equal $First.RegionReports.Count $Second.RegionReports.Count "$Name must retain deterministic report count."
    for ($index = 0; $index -lt $First.Path.Count; $index++) {
        Assert-Near $First.Path[$index].X $Second.Path[$index].X "$Name point $index must retain X."
        Assert-Near $First.Path[$index].Y $Second.Path[$index].Y "$Name point $index must retain Y."
    }
    for ($index = 0; $index -lt $First.RegionReports.Count; $index++) {
        Assert-Equal $First.RegionReports[$index].SelectedCandidateIndex $Second.RegionReports[$index].SelectedCandidateIndex "$Name report $index must retain candidate selection."
        Assert-Equal $First.RegionReports[$index].Status $Second.RegionReports[$index].Status "$Name report $index must retain status."
        Assert-Near $First.RegionReports[$index].StartArcLengthMeters $Second.RegionReports[$index].StartArcLengthMeters "$Name report $index must retain order."
    }
}

$fixtures = $createFixtures.Invoke($null, @((Resolve-Path $FixturePath).Path))
$expected = @{
    'straight' = @('NotNeeded')
    'single-turn' = @('Complete')
    'large-heading-change' = @('Complete')
    's-bend' = @('Complete', 'PartialImprovement')
    'rectangle-detour' = @('Complete', 'PartialImprovement')
    'multi-obstacle-detour' = @('Complete', 'PartialImprovement')
    'forward-reverse-switch' = @('Complete', 'PartialImprovement', 'NotNeeded')
}

$results = @{}
foreach ($fixture in $fixtures) {
    $request = New-LocalG2Request $fixture.SmoothingRequest
    $first = Invoke-LocalG2 $request
    $second = Invoke-LocalG2 $request
    $scenario = $fixture.SmoothingRequest.CoarsePath[0].Source.ToString() # fixture index is resolved below by stable input order
    $results[$results.Count] = $first
    Write-Output ("Fixture $($results.Count - 1): status=$($first.Status); reports=$($first.RegionReports.Count); reason=$($first.Diagnostics.Reason)")
    foreach ($report in $first.RegionReports) {
        Write-Output ("  region=$($report.Status)/$($report.FailureReason); candidates=$($report.CandidateCount); selected=$($report.SelectedCandidateIndex)")
    }
    Assert-Equivalent $first $second "Fixture $($results.Count - 1)"
}

$fixtureIds = @('straight', 'single-turn', 's-bend', 'large-heading-change', 'rectangle-detour', 'multi-obstacle-detour', 'narrow-corridor', 'forward-reverse-switch')
for ($index = 0; $index -lt $fixtureIds.Count; $index++) {
    $id = $fixtureIds[$index]
    if (-not $expected.ContainsKey($id)) { continue }
    $result = $results[$index]
    Assert-True ($expected[$id] -contains $result.Status.ToString()) "$id must publish its required Local G2 status."
    Assert-PublishedPath $result $id
    if ($result.Status.ToString() -in @('Complete', 'PartialImprovement')) {
        Assert-True (@($result.RegionReports | Where-Object { $_.Status.ToString() -eq 'Improved' }).Count -gt 0) "$id must report an accepted region."
    }
    if ($result.Status.ToString() -eq 'PartialImprovement') {
        Assert-True (@($result.RegionReports | Where-Object { $_.Status.ToString() -eq 'RetainedOriginal' }).Count -gt 0) "$id must report retained regions."
    }
}

# The multi-obstacle fixture is a same-direction two-window integration case.  Its two replacements
# change length, so processing it front-to-back would invalidate the later original arc window.
$twoRegion = $results[5]
Assert-True ($twoRegion.RegionReports.Count -ge 2) 'Two-region integration fixture must publish both detector regions.'
Assert-Equal 'Improved' $twoRegion.RegionReports[0].Status.ToString() 'First detector-order two-region report must improve.'
Assert-Equal 'Improved' $twoRegion.RegionReports[1].Status.ToString() 'Second detector-order two-region report must improve.'
Assert-True ($twoRegion.RegionReports[0].StartArcLengthMeters -lt $twoRegion.RegionReports[1].StartArcLengthMeters) 'Reports must retain detector ascending arc order.'
Assert-True (@($twoRegion.Path | Where-Object { $_.Source.ToString() -eq 'LocalG2Transition' }).Count -gt 2) 'Both Local G2 replacements must remain in the final path.'

$strictRequest = New-LocalG2Request $fixtures[1].SmoothingRequest {
    param($configuration)
    $configuration.LocalG2Quintic.MinimumPeakGradientImprovementRatio = 0.99
}
$unchanged = Invoke-LocalG2 $strictRequest
Assert-Equal 'Unchanged' $unchanged.Status.ToString() 'Detected events with no accepted region must publish a verified raw path as Unchanged.'
Assert-PublishedPath $unchanged 'Unchanged Local G2'

$cancelSource = [Threading.CancellationTokenSource]::new()
$cancelSource.Cancel()
$cancelled = Invoke-LocalG2 (New-LocalG2Request $fixtures[1].SmoothingRequest) $cancelSource.Token
Assert-Equal 'Cancelled' $cancelled.Status.ToString() 'Cancellation must propagate through Local G2.'
Assert-Equal 0 $cancelled.Path.Count 'A cancelled run must not publish a partial path.'

Write-Output 'Local G2 service integration checks passed.'
