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, [double]$Tolerance, [string]$Message) {
    if ([Math]::Abs($Expected - $Actual) -gt $Tolerance) {
        throw "$Message Expected=$Expected Actual=$Actual Tolerance=$Tolerance"
    }
}

function Get-RequiredType([string]$Name) {
    return $assembly.GetType($Name, $true)
}

function New-Map([bool]$WithObstacle) {
    $mapRequest = [Activator]::CreateInstance($mapRequestType)
    $mapRequest.Bounds = [Activator]::CreateInstance($boundsType, @([single]0, [single]5000, [single]0, [single]5000))
    $mapRequest.ResolutionMm = [single]50
    if ($WithObstacle) {
        $obstacle = [Activator]::CreateInstance($rectangleType, @([single]900, [single]1100, [single]400, [single]600))
        $obstacles = [Array]::CreateInstance($obstacleType, 1)
        $obstacles.SetValue($obstacle, 0)
        $source = [Activator]::CreateInstance($manualSourceType, @('service-safety-obstacle', [long]1, $true, $obstacles))
        $sources = [Array]::CreateInstance($obstacleSourceType, 1)
        $sources.SetValue($source, 0)
        $mapRequest.ObstacleSources = $sources
    }
    else {
        $mapRequest.AllowExplicitEmptyMap = $true
    }
    $map = [Activator]::CreateInstance($mapFactoryType).Create($mapRequest).Map
    Assert-True ($null -ne $map) 'Service test must create a planning map.'
    return $map
}

function New-EmptyMap { return New-Map $false }

function New-CollidingMap { return New-Map $true }

function New-Vehicle {
    $vehicle = [Activator]::CreateInstance($vehicleType)
    $vehicle.LengthMeters = [double]0.20
    $vehicle.WidthMeters = [double]0.20
    $vehicle.SafetyMarginMeters = [double]0.0
    $vehicle.MaximumCurvaturePerMeter = [double]100.0
    return $vehicle
}

function New-CoarsePoint(
    [double]$X,
    [double]$Y,
    [double]$ArcLength,
    $Direction,
    [double]$BodyClearance = 1.0,
    [bool]$IsGearSwitch = $false) {
    return [Activator]::CreateInstance($coarsePointType, @(
        $X, $Y, [double]0.0, [double]0.0, $ArcLength, $Direction,
        [double]0.0, $BodyClearance, $IsGearSwitch, $coarseAnchor))
}

function New-Configuration($Method = $cubicBSpline) {
    $configuration = [Activator]::CreateInstance($configurationType)
    $configuration.Method = $Method
    return $configuration
}

function New-Request([object[]]$Points, $Configuration, $Map = $null) {
    if ($null -eq $Map) { $Map = New-EmptyMap }
    $typedPoints = [Array]::CreateInstance($coarsePointType, $Points.Count)
    for ($index = 0; $index -lt $Points.Count; $index++) {
        $typedPoints.SetValue($Points[$index], $index)
    }

    $segments = [Array]::CreateInstance($coarseSegmentType, 1)
    $segments.SetValue([Activator]::CreateInstance($coarseSegmentType, @(
        0, $forward, 0, ($Points.Count - 1), $false, $false)), 0)
    return [Activator]::CreateInstance($requestType, @($typedPoints, $segments, $Map, (New-Vehicle), $Configuration))
}

function New-StraightRequest($Configuration) {
    return New-Request @(
        (New-CoarsePoint 0.5 0.5 0.0 $forward),
        (New-CoarsePoint 1.5 0.5 1.0 $forward)) $Configuration
}

function New-InfeasibleRequest($Configuration) {
    # Zero declared movement clearance makes every non-linear B-spline displacement retryably infeasible,
    # while the empty map still permits the independently revalidated coarse-path fallback.
    return New-Request @(
        (New-CoarsePoint 0.5 0.5 0.0 $forward 0.0),
        (New-CoarsePoint 1.0 0.5 0.5 $forward 0.0),
        (New-CoarsePoint 1.0 1.0 1.0 $forward 0.0),
        (New-CoarsePoint 1.5 1.0 1.5 $forward 0.0)) $Configuration
}

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

function Assert-NoGeometry($Result, [string]$Message) {
    Assert-Equal 0 $Result.Path.Count "$Message A non-published result must not expose a path."
    Assert-Equal 0 $Result.Segments.Count "$Message A non-published result must not expose segments."
}

function Assert-InvalidInputBeforeRetry($Configuration, [string]$CaseName) {
    $result = Invoke-Smooth (New-StraightRequest $Configuration)
    Assert-Equal 'InvalidInput' $result.Status.ToString() "$CaseName must be rejected as invalid input."
    Assert-NoGeometry $result $CaseName
    Assert-Equal 0 $result.Diagnostics.RetryCount "$CaseName must be rejected before any smoothing retry."
    Assert-Near 0.0 $result.Diagnostics.AcceptedStrength 0.0 "$CaseName must not accept a smoothing strength."
}

$root = 'MultiWheelC.TrajectoryPlanning.PathSmoothing.'
$facade = $root + 'Facade.'
$coarsePath = 'MultiWheelC.TrajectoryPlanning.CoarsePath.'
$mapping = 'MultiWheelC.TrajectoryPlanning.Mapping.'

$serviceType = Get-RequiredType ($facade + 'PathSmoothingService')
$requestType = Get-RequiredType ($root + 'PathSmoothingRequest')
$resultType = Get-RequiredType ($root + 'PathSmoothingResult')
$configurationType = Get-RequiredType ($root + 'PathSmoothingConfiguration')
$methodType = Get-RequiredType ($root + 'SmoothingMethod')
$coarsePointType = Get-RequiredType ($coarsePath + 'CoarsePathPoint')
$coarseSegmentType = Get-RequiredType ($coarsePath + 'PathSegment')
$directionType = Get-RequiredType ($coarsePath + 'TravelDirection')
$coarsePointSourceType = Get-RequiredType ($coarsePath + 'CoarsePathPointSource')
$vehicleType = Get-RequiredType ($coarsePath + 'VehicleParameters')
$boundsType = Get-RequiredType ($mapping + 'MapBoundsMm')
$obstacleType = Get-RequiredType ($mapping + 'IMapObstacle')
$rectangleType = Get-RequiredType ($mapping + 'AxisAlignedRectangleObstacle')
$obstacleSourceType = Get-RequiredType ($mapping + 'IMapObstacleSource')
$manualSourceType = Get-RequiredType ($mapping + 'ManualObstacleSource')
$mapRequestType = Get-RequiredType ($mapping + 'PlanningMapRequest')
$mapFactoryType = Get-RequiredType ($mapping + 'PlanningMapFactory')

Assert-True $serviceType.IsPublic 'PathSmoothingService must be public.'
$service = [Activator]::CreateInstance($serviceType)
$smoothMethod = $serviceType.GetMethod('Smooth', [Type[]]@($requestType, [Threading.CancellationToken]))
Assert-True ($null -ne $smoothMethod) 'PathSmoothingService must expose Smooth(PathSmoothingRequest, CancellationToken).'
Assert-Equal $resultType $smoothMethod.ReturnType 'PathSmoothingService Smooth must return PathSmoothingResult.'

$forward = [Enum]::Parse($directionType, 'Forward')
$cubicBSpline = [Enum]::Parse($methodType, 'CubicBSpline')
$localCubicBezier = [Enum]::Parse($methodType, 'LocalCubicBezier')
$piecewiseQuintic = [Enum]::Parse($methodType, 'PiecewiseQuintic')
$coarseAnchor = [Enum]::Parse($coarsePointSourceType, 'Start')

# The public method registry must retain every stable enum-to-algorithm mapping.
foreach ($method in @($cubicBSpline, $localCubicBezier, $piecewiseQuintic)) {
    $result = Invoke-Smooth (New-StraightRequest (New-Configuration $method))
    Assert-Equal 'Success' $result.Status.ToString() "A valid straight path must succeed for $method."
    Assert-Equal $method.ToString() $result.Method.ToString() "The result must retain the selected $method method."
    Assert-True ($result.Path.Count -gt 0) "A successful $method result must publish geometry."
    Assert-True $result.Diagnostics.Metrics.IsFeasible "A successful $method result must publish feasible diagnostics."
}

# Every configuration scalar is checked before the options snapshot or retry runner starts.
$invalidConfigurationCases = @(
    [PSCustomObject]@{ Name = 'NaN output spacing'; Mutate = { param($c) $c.OutputSpacingMeters = [double]::NaN } },
    [PSCustomObject]@{ Name = 'zero output spacing'; Mutate = { param($c) $c.OutputSpacingMeters = [double]0.0 } },
    [PSCustomObject]@{ Name = 'infinite collision step'; Mutate = { param($c) $c.MaximumCollisionCheckStepMeters = [double]::PositiveInfinity } },
    [PSCustomObject]@{ Name = 'zero collision step'; Mutate = { param($c) $c.MaximumCollisionCheckStepMeters = [double]0.0 } },
    [PSCustomObject]@{ Name = 'NaN clearance reserve'; Mutate = { param($c) $c.MinimumClearanceReserveMeters = [double]::NaN } },
    [PSCustomObject]@{ Name = 'negative clearance reserve'; Mutate = { param($c) $c.MinimumClearanceReserveMeters = [double]-0.01 } },
    [PSCustomObject]@{ Name = 'NaN smoothing strength'; Mutate = { param($c) $c.SmoothingStrength = [double]::NaN } },
    [PSCustomObject]@{ Name = 'zero smoothing strength'; Mutate = { param($c) $c.SmoothingStrength = [double]0.0 } },
    [PSCustomObject]@{ Name = 'NaN B-spline scale'; Mutate = { param($c) $c.CubicBSpline.EndpointTangentScale = [double]::NaN } },
    [PSCustomObject]@{ Name = 'zero B-spline scale'; Mutate = { param($c) $c.CubicBSpline.EndpointTangentScale = [double]0.0 } },
    [PSCustomObject]@{ Name = 'zero Bezier threshold'; Mutate = { param($c) $c.LocalCubicBezier.CornerHeadingThresholdRadians = [double]0.0 } },
    [PSCustomObject]@{ Name = 'over-pi Bezier threshold'; Mutate = { param($c) $c.LocalCubicBezier.CornerHeadingThresholdRadians = [Math]::PI + 0.01 } },
    [PSCustomObject]@{ Name = 'NaN Bezier window'; Mutate = { param($c) $c.LocalCubicBezier.MaximumWindowLengthMeters = [double]::NaN } },
    [PSCustomObject]@{ Name = 'zero Bezier window'; Mutate = { param($c) $c.LocalCubicBezier.MaximumWindowLengthMeters = [double]0.0 } },
    [PSCustomObject]@{ Name = 'infinite Bezier handle scale'; Mutate = { param($c) $c.LocalCubicBezier.HandleLengthRatio = [double]::PositiveInfinity } },
    [PSCustomObject]@{ Name = 'zero Bezier handle scale'; Mutate = { param($c) $c.LocalCubicBezier.HandleLengthRatio = [double]0.0 } },
    [PSCustomObject]@{ Name = 'NaN quintic knot spacing'; Mutate = { param($c) $c.PiecewiseQuintic.KnotSpacingMeters = [double]::NaN } },
    [PSCustomObject]@{ Name = 'zero quintic knot spacing'; Mutate = { param($c) $c.PiecewiseQuintic.KnotSpacingMeters = [double]0.0 } },
    [PSCustomObject]@{ Name = 'infinite minimum quintic knot spacing'; Mutate = { param($c) $c.PiecewiseQuintic.MinimumKnotSpacingMeters = [double]::PositiveInfinity } },
    [PSCustomObject]@{ Name = 'zero minimum quintic knot spacing'; Mutate = { param($c) $c.PiecewiseQuintic.MinimumKnotSpacingMeters = [double]0.0 } },
    [PSCustomObject]@{ Name = 'quintic knot spacing below minimum'; Mutate = { param($c) $c.PiecewiseQuintic.KnotSpacingMeters = [double]0.05; $c.PiecewiseQuintic.MinimumKnotSpacingMeters = [double]0.10 } }
)
foreach ($case in $invalidConfigurationCases) {
    $configuration = New-Configuration
    & $case.Mutate $configuration
    Assert-InvalidInputBeforeRetry $configuration $case.Name
}

$unknownMethodConfiguration = New-Configuration ([Enum]::ToObject($methodType, 99))
Assert-InvalidInputBeforeRetry $unknownMethodConfiguration 'unknown smoothing method'

$invalidCoarseConfiguration = New-Configuration
$invalidCoarseRequest = New-Request @(
    (New-CoarsePoint ([double]::NaN) 0.5 0.0 $forward),
    (New-CoarsePoint 1.5 0.5 1.0 $forward)) $invalidCoarseConfiguration
$invalidCoarseResult = Invoke-Smooth $invalidCoarseRequest
Assert-Equal 'InvalidInput' $invalidCoarseResult.Status.ToString() 'A non-finite coarse path coordinate must be invalid input.'
Assert-NoGeometry $invalidCoarseResult 'Invalid coarse path'
Assert-Equal 0 $invalidCoarseResult.Diagnostics.RetryCount 'Invalid coarse input must be rejected before retries.'

# A finite, structurally valid coarse path may still be unsafe for the requested map and vehicle.
# It must be rejected before method selection/retries and may never use fallback to publish the unsafe geometry.
$unsafeCoarseResult = Invoke-Smooth (New-Request @(
    (New-CoarsePoint 0.5 0.5 0.0 $forward),
    (New-CoarsePoint 1.5 0.5 1.0 $forward)) (New-Configuration) (New-CollidingMap))
Assert-Equal 'InvalidInput' $unsafeCoarseResult.Status.ToString() 'A colliding coarse path must be invalid before smoothing starts.'
Assert-NoGeometry $unsafeCoarseResult 'Unsafe coarse path'
Assert-Equal 0 $unsafeCoarseResult.Diagnostics.RetryCount 'Unsafe coarse geometry must be rejected before retry execution.'

$cancelledConfiguration = New-Configuration
$cancellationSource = [Threading.CancellationTokenSource]::new()
$cancellationSource.Cancel()
try {
    $cancelledResult = Invoke-Smooth (New-StraightRequest $cancelledConfiguration) $cancellationSource.Token
    Assert-Equal 'Cancelled' $cancelledResult.Status.ToString() 'Pre-cancelled smoothing must return the explicit cancellation result.'
    Assert-NoGeometry $cancelledResult 'Cancelled smoothing'
    Assert-Equal 0 $cancelledResult.Diagnostics.RetryCount 'Cancellation before execution must not start retries.'
}
finally {
    $cancellationSource.Dispose()
}

$withoutFallbackConfiguration = New-Configuration
$withoutFallbackConfiguration.AllowFallbackToCoarsePath = $false
$withoutFallbackResult = Invoke-Smooth (New-InfeasibleRequest $withoutFallbackConfiguration)
Assert-Equal 'Infeasible' $withoutFallbackResult.Status.ToString() 'A retryably infeasible candidate without fallback must remain infeasible.'
Assert-NoGeometry $withoutFallbackResult 'Infeasible smoothing without fallback'
Assert-Equal 3 $withoutFallbackResult.Diagnostics.RetryCount 'Infeasible smoothing must exhaust the four configured strengths.'
Assert-Near 0.0 $withoutFallbackResult.Diagnostics.AcceptedStrength 0.0 'Infeasible smoothing must not accept a strength.'

$withFallbackConfiguration = New-Configuration
$withFallbackConfiguration.AllowFallbackToCoarsePath = $true
$withFallbackResult = Invoke-Smooth (New-InfeasibleRequest $withFallbackConfiguration)
Assert-Equal 'FallbackToCoarsePath' $withFallbackResult.Status.ToString() 'A verified coarse path must return explicit fallback status.'
Assert-Equal 'CubicBSpline' $withFallbackResult.Method.ToString() 'Fallback must retain the originally selected method.'
Assert-True ($withFallbackResult.Path.Count -gt 0) 'A verified fallback must publish the revalidated coarse geometry.'
Assert-True $withFallbackResult.Diagnostics.Metrics.IsFeasible 'Fallback must publish feasible shared-geometry diagnostics.'
foreach ($point in $withFallbackResult.Path) {
    Assert-Equal 'CoarsePathFallback' $point.Source.ToString() 'Every fallback point must be explicitly labeled as coarse-path fallback.'
}
Assert-Equal $withoutFallbackResult.Diagnostics.RetryCount $withFallbackResult.Diagnostics.RetryCount 'Fallback must preserve retry diagnostics from the failed method.'
Assert-Near $withoutFallbackResult.Diagnostics.AcceptedStrength $withFallbackResult.Diagnostics.AcceptedStrength 0.0 'Fallback must preserve the failed method accepted-strength diagnostic.'
Assert-Equal $withoutFallbackResult.Diagnostics.TerminationReason $withFallbackResult.Diagnostics.TerminationReason 'Fallback must preserve the failed method termination reason.'

Write-Output 'Path smoothing service checks passed.'