param([string]$AssemblyPath = (Join-Path $PSScriptRoot '..\bin\Debug\netstandard2.0\ClumsyPilot.dll'))

$ErrorActionPreference = 'Stop'
$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.000001) { throw "$Message Expected=$Expected Actual=$Actual" }
}
function Get-RequiredType([string]$Name) { return $assembly.GetType($Name, $true) }

function Assert-CoarsePathGeometry($ExpectedPath, $ActualPath, $Map, [string]$ScenarioName) {
    Assert-Equal $ExpectedPath.Count $ActualPath.Count "Raw comparison request $ScenarioName must preserve every coarse-path point."
    for ($index = 0; $index -lt $ExpectedPath.Count; $index++) {
        $expected = $ExpectedPath[$index]
        $actual = $ActualPath[$index]
        Assert-Near $expected.X $actual.X "Raw comparison request $ScenarioName point $index must preserve X."
        Assert-Near $expected.Y $actual.Y "Raw comparison request $ScenarioName point $index must preserve Y."
        Assert-Near $expected.Heading $actual.Heading "Raw comparison request $ScenarioName point $index must preserve heading."
        Assert-Near $expected.UnwrappedHeading $actual.UnwrappedHeading "Raw comparison request $ScenarioName point $index must preserve unwrapped heading."
        Assert-Near $expected.ArcLength $actual.ArcLength "Raw comparison request $ScenarioName point $index must preserve arc length."
        Assert-Equal $expected.Direction $actual.Direction "Raw comparison request $ScenarioName point $index must preserve travel direction."
        Assert-Near $expected.VehicleCurvature $actual.VehicleCurvature "Raw comparison request $ScenarioName point $index must preserve vehicle curvature."
        $expectedClearance = $expected.BodyClearance
        if ([double]::IsPositiveInfinity($expectedClearance)) {
            $widthMeters = ($Map.Bounds.XMax - $Map.Bounds.XMin) / 1000.0
            $heightMeters = ($Map.Bounds.YMax - $Map.Bounds.YMin) / 1000.0
            $expectedClearance = [Math]::Sqrt($widthMeters * $widthMeters + $heightMeters * $heightMeters)
        }
        Assert-Near $expectedClearance $actual.BodyClearance "Raw comparison request $ScenarioName point $index must preserve or normalize clearance for smoothing."
        Assert-Equal $expected.IsGearSwitchPoint $actual.IsGearSwitchPoint "Raw comparison request $ScenarioName point $index must preserve gear-switch flag."
        Assert-Equal $expected.Source $actual.Source "Raw comparison request $ScenarioName point $index must preserve point source."
    }
}

function Assert-SegmentTopology($ExpectedSegments, $ActualPath, $ActualSegments, [string]$Description) {
    Assert-Equal $ExpectedSegments.Count $ActualSegments.Count "$Description must preserve segment count."
    $expectedStartIndex = 0
    for ($index = 0; $index -lt $ActualSegments.Count; $index++) {
        $expected = $ExpectedSegments[$index]
        $actual = $ActualSegments[$index]
        Assert-Equal $index $actual.SegmentIndex "$Description segment $index must retain its stable index."
        Assert-Equal $expected.Direction $actual.Direction "$Description segment $index must preserve travel direction."
        Assert-Equal $expected.StartsAtGearSwitch $actual.StartsAtGearSwitch "$Description segment $index must preserve start gear-switch topology."
        Assert-Equal $expected.EndsAtGearSwitch $actual.EndsAtGearSwitch "$Description segment $index must preserve end gear-switch topology."
        Assert-Equal $expectedStartIndex $actual.StartIndex "$Description segment $index must start directly after the prior segment."
        Assert-True ($actual.EndIndex -ge $actual.StartIndex -and $actual.EndIndex -lt $ActualPath.Count) "$Description segment $index must cover valid path indices."
        Assert-Equal $actual.StartsAtGearSwitch $ActualPath[$actual.StartIndex].IsGearSwitchPoint "$Description segment $index start flag must match its path point."
        for ($pointIndex = $actual.StartIndex; $pointIndex -le $actual.EndIndex; $pointIndex++) {
            Assert-Equal $actual.Direction $ActualPath[$pointIndex].Direction "$Description segment $index may not contain mixed directions."
        }
        $hasNext = $index + 1 -lt $ActualSegments.Count
        $expectedEndSwitch = $hasNext -and $ActualPath[$actual.EndIndex + 1].IsGearSwitchPoint
        Assert-Equal $expectedEndSwitch $actual.EndsAtGearSwitch "$Description segment $index end flag must match the next gear switch."
        $expectedStartIndex = $actual.EndIndex + 1
    }
    Assert-Equal $ActualPath.Count $expectedStartIndex "$Description segments must cover every path point."
}

function Assert-GearSwitchGeometry($ExpectedPath, $ActualPath, [string]$Description) {
    $expectedSwitches = @($ExpectedPath | Where-Object { $_.IsGearSwitchPoint })
    $actualSwitches = @($ActualPath | Where-Object { $_.IsGearSwitchPoint })
    Assert-Equal $expectedSwitches.Count $actualSwitches.Count "$Description must preserve gear-switch count."
    for ($index = 0; $index -lt $expectedSwitches.Count; $index++) {
        Assert-Near $expectedSwitches[$index].X $actualSwitches[$index].X "$Description gear switch $index must preserve X."
        Assert-Near $expectedSwitches[$index].Y $actualSwitches[$index].Y "$Description gear switch $index must preserve Y."
        Assert-Near $expectedSwitches[$index].Heading $actualSwitches[$index].Heading "$Description gear switch $index must preserve heading."
        Assert-Equal $expectedSwitches[$index].Direction $actualSwitches[$index].Direction "$Description gear switch $index must preserve direction."
    }
}

function Assert-RawBaselinePath($ExpectedPath, $ActualPath, [string]$ScenarioName) {
    Assert-Equal $ExpectedPath.Count $ActualPath.Count "Raw baseline $ScenarioName must preserve every coarse-path point."
    for ($index = 0; $index -lt $ExpectedPath.Count; $index++) {
        $expected = $ExpectedPath[$index]
        $actual = $ActualPath[$index]
        Assert-Near $expected.X $actual.X "Raw baseline $ScenarioName point $index must preserve X."
        Assert-Near $expected.Y $actual.Y "Raw baseline $ScenarioName point $index must preserve Y."
        Assert-Near $expected.Heading $actual.Heading "Raw baseline $ScenarioName point $index must preserve heading."
        Assert-Near $expected.UnwrappedHeading $actual.UnwrappedHeading "Raw baseline $ScenarioName point $index must preserve unwrapped heading."
        Assert-Near $expected.ArcLength $actual.ArcLength "Raw baseline $ScenarioName point $index must preserve arc length."
        Assert-Equal $expected.Direction $actual.Direction "Raw baseline $ScenarioName point $index must preserve travel direction."
        Assert-Near $expected.VehicleCurvature $actual.VehicleCurvature "Raw baseline $ScenarioName point $index must preserve vehicle curvature."
        Assert-Equal $expected.IsGearSwitchPoint $actual.IsGearSwitchPoint "Raw baseline $ScenarioName point $index must preserve gear-switch topology."
    }
}

$coarse = 'MultiWheelC.TrajectoryPlanning.CoarsePath.'
$coarseFacade = $coarse + 'Facade.'
$coarseTest = $coarse + 'Test.'
$smoothingTest = 'MultiWheelC.TrajectoryPlanning.PathSmoothing.Test.'
$smoothingFacade = 'MultiWheelC.TrajectoryPlanning.PathSmoothing.Facade.'
$comparison = 'MultiWheelC.TrajectoryPlanning.PathSmoothing.Comparison.'

$coarseServiceType = Get-RequiredType ($coarseFacade + 'CoarsePathPlanningService')
$coarseJobType = Get-RequiredType ($coarseFacade + 'CoarsePathPlanningJob')
$coarseResultType = Get-RequiredType ($coarseFacade + 'CoarsePathPlanningJobResult')
$scenarioType = Get-RequiredType ($coarseTest + 'CoarsePathTestScenario')
$coarseFactoryType = Get-RequiredType ($coarseTest + 'CoarsePathScenarioFactory')
$scenarioFactoryType = Get-RequiredType ($smoothingTest + 'SmoothingScenarioFactory')
$comparisonServiceType = Get-RequiredType ($smoothingFacade + 'PathSmoothingComparisonService')
$comparisonRequestType = Get-RequiredType ($comparison + 'PathSmoothingComparisonRequest')

$coarseCreate = $coarseFactoryType.GetMethod('Create', [Type[]]@($scenarioType))
$coarsePlan = $coarseServiceType.GetMethod('Plan', [Type[]]@($coarseJobType, [Threading.CancellationToken]))
$createComparisonRequest = $scenarioFactoryType.GetMethod('CreateEndToEndRequest', [Type[]]@($coarseJobType, $coarseResultType))
$compare = $comparisonServiceType.GetMethod('Compare', [Type[]]@($comparisonRequestType, [Threading.CancellationToken]))
Assert-True ($null -ne $createComparisonRequest) 'Smoothing scenario factory must convert a successful coarse planning job into a comparison request.'

$planner = [Activator]::CreateInstance($coarseServiceType)
$comparisonService = [Activator]::CreateInstance($comparisonServiceType)
foreach ($scenarioName in @('ExplicitEmpty', 'RectangleDetour', 'ManualAndTwoLeg', 'ReverseGearSwitch')) {
    $scenario = [Enum]::Parse($scenarioType, $scenarioName)
    $job = $coarseCreate.Invoke($null, @($scenario))
    $planningTimer = [Diagnostics.Stopwatch]::StartNew()
    $planned = $coarsePlan.Invoke($planner, @($job, [Threading.CancellationToken]::None))
    $planningTimer.Stop()
    Assert-Equal 'Success' $planned.PlanningResult.Status.ToString() "Coarse scenario $scenarioName must succeed before smoothing comparison."
    $request = $createComparisonRequest.Invoke($null, @($job, $planned))
    Assert-CoarsePathGeometry $planned.PlanningResult.Path $request.SmoothingRequest.CoarsePath $planned.MapResult.Map $scenarioName
    $comparisonTimer = [Diagnostics.Stopwatch]::StartNew()
    $comparisonResult = $compare.Invoke($comparisonService, @($request, [Threading.CancellationToken]::None))
    $comparisonTimer.Stop()
    Assert-True (-not $comparisonResult.IsCancelled) "Comparison $scenarioName must not be cancelled."
    Assert-Equal 'Success' $comparisonResult.RawPathBaseline.Status.ToString() "Raw baseline $scenarioName must remain a feasible, verified copy of the coarse path."
    Assert-RawBaselinePath $planned.PlanningResult.Path $comparisonResult.RawPathBaseline.Path $scenarioName
    Assert-SegmentTopology $planned.PlanningResult.Segments $comparisonResult.RawPathBaseline.Path $comparisonResult.RawPathBaseline.Segments "Raw baseline $scenarioName"
    Assert-GearSwitchGeometry $planned.PlanningResult.Path $comparisonResult.RawPathBaseline.Path "Raw baseline $scenarioName"
    foreach ($entry in $comparisonResult.Entries | Where-Object { $_.Status.ToString() -eq 'Success' }) {
        Assert-SegmentTopology $comparisonResult.RawPathBaseline.Segments $entry.Path $entry.Segments "Successful $($entry.Method) $scenarioName"
        Assert-GearSwitchGeometry $comparisonResult.RawPathBaseline.Path $entry.Path "Successful $($entry.Method) $scenarioName"
    }
    $successfulEntryCount = @($comparisonResult.Entries | Where-Object { $_.Status.ToString() -eq 'Success' }).Count
    Write-Output ("$scenarioName diagnostics: planning=$([Math]::Round($planningTimer.Elapsed.TotalSeconds, 3))s; comparison=$([Math]::Round($comparisonTimer.Elapsed.TotalSeconds, 3))s; successfulMethods=$successfulEntryCount")
}

Write-Output 'Path smoothing end-to-end integration checks passed.'