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')
$factoryType = Get-RequiredType ($test + 'SmoothingScenarioFactory')
$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.'

$publishedStatuses = @('Complete', 'PartialImprovement', 'NotNeeded', 'Unchanged')

function New-LocalG2Request($BaseRequest, [scriptblock]$Configure = $null) {
    $configuration = [Activator]::CreateInstance($configurationType)
    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.0) "$Name must remain collision-free at the configured 0 m 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."
    }
}

$fixtures = $createFixtures.Invoke($null, @((Resolve-Path $FixturePath).Path))
for ($index = 0; $index -lt $fixtures.Count; $index++) {
    $request = New-LocalG2Request $fixtures[$index].SmoothingRequest
    $first = Invoke-LocalG2 $request
    $second = Invoke-LocalG2 $request
    Assert-Equivalent $first $second "Fixture $index"
    $status = $first.Status.ToString()
    Assert-True ($status -in @('Complete', 'PartialImprovement', 'NotNeeded', 'Unchanged', 'Failed', 'InvalidInput', 'Infeasible')) "Fixture $index must return a defined Local G2 result status."
    if ($publishedStatuses -contains $status) {
        Assert-PublishedPath $first "Fixture $index"
    }
    else {
        Assert-Equal 0 $first.Path.Count "Unpublished fixture $index must not expose a path."
        Assert-Equal 0 $first.Segments.Count "Unpublished fixture $index must not expose segments."
    }
}

$cancelSource = [Threading.CancellationTokenSource]::new()
try {
    $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.'
}
finally {
    $cancelSource.Dispose()
}

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