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 {
    $mapRequest = [Activator]::CreateInstance($mapRequestType)
    $mapRequest.Bounds = [Activator]::CreateInstance($boundsType, @([single]0, [single]5000, [single]0, [single]5000))
    $mapRequest.ResolutionMm = [single]50
    $mapRequest.AllowExplicitEmptyMap = $true
    $map = [Activator]::CreateInstance($mapFactoryType).Create($mapRequest).Map
    Assert-True ($null -ne $map) 'Comparison test must create a planning map.'
    return $map
}

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) {
    return [Activator]::CreateInstance($coarsePointType, @(
        $X, $Y, [double]0.0, [double]0.0, $ArcLength, $forward,
        [double]0.0, [double]1.0, $false, $coarseAnchor))
}

function New-SmoothingRequest {
    $points = [Array]::CreateInstance($coarsePointType, 2)
    $points.SetValue((New-CoarsePoint 0.5 0.5 0.0), 0)
    $points.SetValue((New-CoarsePoint 1.5 0.5 1.0), 1)
    $segments = [Array]::CreateInstance($coarseSegmentType, 1)
    $segments.SetValue([Activator]::CreateInstance($coarseSegmentType, @(0, $forward, 0, 1, $false, $false)), 0)
    $configuration = [Activator]::CreateInstance($configurationType)
    return [Activator]::CreateInstance($smoothingRequestType, @($points, $segments, (New-Map), (New-Vehicle), $configuration))
}

function New-CornerSmoothingRequest {
    $points = [Array]::CreateInstance($coarsePointType, 4)
    $points.SetValue((New-CoarsePoint 0.5 0.5 0.0), 0)
    $points.SetValue((New-CoarsePoint 1.0 0.5 0.5), 1)
    $points.SetValue((New-CoarsePoint 1.0 1.0 1.0), 2)
    $points.SetValue((New-CoarsePoint 1.5 1.0 1.5), 3)
    $segments = [Array]::CreateInstance($coarseSegmentType, 1)
    $segments.SetValue([Activator]::CreateInstance($coarseSegmentType, @(0, $forward, 0, 3, $false, $false)), 0)
    $configuration = [Activator]::CreateInstance($configurationType)
    return [Activator]::CreateInstance($smoothingRequestType, @($points, $segments, (New-Map), (New-Vehicle), $configuration))
}

function New-Metrics(
    [double]$VariationEnergy,
    [double]$PeakCurvature,
    [double]$MinimumClearance,
    [double]$LengthChangePercent) {
    return [Activator]::CreateInstance($metricsType, @(
        $true, [double]1.0, $PeakCurvature, [double]0.0, [double]0.0, $VariationEnergy,
        $MinimumClearance, $LengthChangePercent, [double]0.0, [double]0.0, [double]0.0))
}

function New-Timing([double]$MedianMilliseconds) {
    [double[]]$samples = @($MedianMilliseconds, $MedianMilliseconds, $MedianMilliseconds, $MedianMilliseconds, $MedianMilliseconds)
    return [Activator]::CreateInstance($timingType, @($samples, $true, ''))
}

function New-Entry(
    $Method,
    [double]$VariationEnergy,
    [double]$PeakCurvature,
    [double]$MinimumClearance,
    [double]$LengthChangePercent,
    [double]$MedianMilliseconds) {
    [object[]]$arguments = @(
        $Method, $successStatus, (New-Metrics $VariationEnergy $PeakCurvature $MinimumClearance $LengthChangePercent),
        (New-Timing $MedianMilliseconds), ('synthetic-' + $Method.ToString()), '')
    return [Activator]::CreateInstance($entryType, $arguments)
}

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

$comparisonServiceType = Get-RequiredType ($facade + 'PathSmoothingComparisonService')
$comparisonRequestType = Get-RequiredType ($comparison + 'PathSmoothingComparisonRequest')
$comparisonResultType = Get-RequiredType ($comparison + 'PathSmoothingComparisonResult')
$entryType = Get-RequiredType ($comparison + 'PathSmoothingComparisonEntry')
$timingType = Get-RequiredType ($comparison + 'SmoothingTimingSummary')
$rankerType = Get-RequiredType ($comparison + 'SmoothingMethodRanker')
$smoothingRequestType = Get-RequiredType ($root + 'PathSmoothingRequest')
$configurationType = Get-RequiredType ($root + 'PathSmoothingConfiguration')
$metricsType = Get-RequiredType ($root + 'PathQualityMetrics')
$methodType = Get-RequiredType ($root + 'SmoothingMethod')
$statusType = Get-RequiredType ($root + 'PathSmoothingStatus')
$smoothingResultType = Get-RequiredType ($root + 'PathSmoothingResult')
$diagnosticsType = Get-RequiredType ($root + 'PathSmoothingDiagnostics')
$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')
$mapRequestType = Get-RequiredType ($mapping + 'PlanningMapRequest')
$mapFactoryType = Get-RequiredType ($mapping + 'PlanningMapFactory')

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

Assert-True $comparisonServiceType.IsPublic 'Comparison service must be public.'
$compareMethod = $comparisonServiceType.GetMethod('Compare', [Type[]]@($comparisonRequestType, [Threading.CancellationToken]))
Assert-True ($null -ne $compareMethod) 'Comparison service must expose Compare(PathSmoothingComparisonRequest, CancellationToken).'
Assert-Equal $comparisonResultType $compareMethod.ReturnType 'Compare must return PathSmoothingComparisonResult.'

$methods = [Array]::CreateInstance($methodType, 3)
$methods.SetValue($cubicBSpline, 0)
$methods.SetValue($localCubicBezier, 1)
$methods.SetValue($piecewiseQuintic, 2)
$comparisonRequest = [Activator]::CreateInstance($comparisonRequestType, @((New-SmoothingRequest), $methods))
$comparisonService = [Activator]::CreateInstance($comparisonServiceType)
$result = $compareMethod.Invoke($comparisonService, @($comparisonRequest, [Threading.CancellationToken]::None))

Assert-True ($null -ne $result.RawPathBaseline) 'Comparison must publish a separately analyzed raw-path baseline.'
Assert-True $result.RawPathBaseline.IsRawPathBaseline 'Raw baseline must be explicitly marked and excluded from candidates.'
Assert-Equal 3 $result.Entries.Count 'Comparison must contain exactly one entry for every requested method.'
Assert-True ($null -ne $result.RecommendedMethod) 'A comparison with feasible methods must select a recommendation.'
foreach ($entry in $result.Entries) {
    Assert-True (-not $entry.IsRawPathBaseline) 'Candidate entries must not be marked as the raw baseline.'
    Assert-Equal 5 $entry.Timing.MeasuredElapsedMilliseconds.Count 'Warm-up must be excluded and exactly five measurements retained.'
    Assert-True $entry.Timing.IsDeterministic 'Repeated deterministic smoothing geometry must remain eligible for recommendation.'
    Assert-True (-not [string]::IsNullOrWhiteSpace($entry.StableGeometryDigest)) 'Every measured candidate must expose a stable geometry digest.'
}

$fromMeasurementsMethod = $timingType.GetMethod('FromMeasurements')
Assert-True ($null -ne $fromMeasurementsMethod) 'Timing summary must analyze the five measured outputs for deterministic geometry.'
$failureFactory = $smoothingResultType.GetMethod('Failure')
$invalidInputStatus = [Enum]::Parse($statusType, 'InvalidInput')
$infeasibleStatus = [Enum]::Parse($statusType, 'Infeasible')
$inconsistentResults = [Array]::CreateInstance($smoothingResultType, 5)
for ($index = 0; $index -lt 5; $index++) {
    $status = if ($index -eq 4) { $infeasibleStatus } else { $invalidInputStatus }
    [object[]]$failureArguments = New-Object object[] 2
    $failureArguments[0] = $status
    $failureArguments[1] = [Activator]::CreateInstance($diagnosticsType)
    $inconsistentResults.SetValue($failureFactory.Invoke($null, $failureArguments), $index)
}
[object[]]$timingArguments = New-Object object[] 2
$timingArguments[0] = [double[]]@(1.0, 2.0, 3.0, 4.0, 5.0)
$timingArguments[1] = $inconsistentResults
$nonDeterministicTiming = $fromMeasurementsMethod.Invoke($null, $timingArguments)
Assert-True (-not $nonDeterministicTiming.IsDeterministic) 'A status, point-count, segment-count, or digest mismatch must be non-deterministic.'
Assert-True (-not [string]::IsNullOrWhiteSpace($nonDeterministicTiming.Diagnostic)) 'Non-deterministic measurements must publish a stable diagnostic.'

# All published comparison metrics must be normalized against the separately analyzed raw baseline.
$cornerMethods = [Array]::CreateInstance($methodType, 1)
$cornerMethods.SetValue($cubicBSpline, 0)
$cornerRequest = [Activator]::CreateInstance($comparisonRequestType, @((New-CornerSmoothingRequest), $cornerMethods))
$cornerResult = $compareMethod.Invoke($comparisonService, @($cornerRequest, [Threading.CancellationToken]::None))
$cornerEntry = $cornerResult.Entries[0]
Assert-Equal 'Success' $cornerEntry.Status.ToString() 'The unconstrained empty-map corner fixture must produce a B-spline comparison candidate.'
$expectedLengthChange = (($cornerEntry.Path[$cornerEntry.Path.Count - 1].ArcLength - $cornerResult.RawPathBaseline.Metrics.PathLengthMeters) /
    $cornerResult.RawPathBaseline.Metrics.PathLengthMeters) * 100.0
Assert-Near $expectedLengthChange $cornerEntry.Metrics.LengthChangePercent 0.000001 'Candidate length change must be normalized relative to the raw baseline.'

# The ranker must apply every public tie-break in order.  Each pair ties all prior criteria.
$entryListType = [Collections.Generic.IReadOnlyList``1].MakeGenericType(@($entryType))
$rankMethod = $rankerType.GetMethod('Rank', [Type[]]@($entryListType))
Assert-True ($null -ne $rankMethod) 'SmoothingMethodRanker must expose Rank(IReadOnlyList<PathSmoothingComparisonEntry>).'

function Assert-Rank($ExpectedMethod, [object[]]$Entries, [string]$Message) {
    $typedEntries = [Array]::CreateInstance($entryType, $Entries.Count)
    for ($index = 0; $index -lt $Entries.Count; $index++) { $typedEntries.SetValue($Entries[$index], $index) }
    [object[]]$invokeArguments = New-Object object[] 1
    $invokeArguments[0] = $typedEntries
    $actual = $rankMethod.Invoke($null, $invokeArguments)
    Assert-Equal $ExpectedMethod.ToString() $actual.ToString() $Message
}

Assert-Rank $cubicBSpline @(
    (New-Entry $cubicBSpline 1.0 0.5 0.8 5.0 10.0),
    (New-Entry $localCubicBezier 2.0 0.1 1.0 1.0 1.0)) 'Variation-energy tie-break must take priority over later criteria.'
Assert-Rank $cubicBSpline @(
    (New-Entry $cubicBSpline 1.0 0.2 0.8 5.0 10.0),
    (New-Entry $localCubicBezier 1.0 0.3 1.0 1.0 1.0)) 'Peak-curvature tie-break must follow variation energy.'
Assert-Rank $cubicBSpline @(
    (New-Entry $cubicBSpline 1.0 0.2 0.9 5.0 10.0),
    (New-Entry $localCubicBezier 1.0 0.2 0.8 1.0 1.0)) 'Clearance-loss tie-break must follow peak curvature.'
Assert-Rank $cubicBSpline @(
    (New-Entry $cubicBSpline 1.0 0.2 0.9 2.0 10.0),
    (New-Entry $localCubicBezier 1.0 0.2 0.9 3.0 1.0)) 'Length-change tie-break must follow clearance loss.'
Assert-Rank $cubicBSpline @(
    (New-Entry $cubicBSpline 1.0 0.2 0.9 2.0 5.0),
    (New-Entry $localCubicBezier 1.0 0.2 0.9 2.0 6.0)) 'Median elapsed tie-break must be last.'

$cancelSource = [Threading.CancellationTokenSource]::new()
try {
    $cancelSource.Cancel()
    $cancelled = $compareMethod.Invoke($comparisonService, @($comparisonRequest, $cancelSource.Token))
    Assert-True $cancelled.IsCancelled 'Cancellation must stop comparison before subsequent methods start.'
    Assert-Equal $null $cancelled.RecommendedMethod 'Cancelled comparison must not make a recommendation.'
}
finally {
    $cancelSource.Dispose()
}

Write-Output 'Path smoothing comparison checks passed.'