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

function Assert-Throws([scriptblock]$Action, [string]$Message) {
    try {
        & $Action
    }
    catch {
        return
    }

    throw $Message
}

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

function Get-RequiredProperty($Type, [string]$Name) {
    $property = $Type.GetProperty($Name, [Reflection.BindingFlags]'Instance,Public,NonPublic')
    Assert-True ($null -ne $property) ("Missing property: " + $Name)
    Assert-True (-not $property.CanWrite) ("Snapshot property must be get-only: " + $Name)
    return $property
}

$root = 'MultiWheelC.TrajectoryPlanning.PathSmoothing.'
$algorithms = $root + 'Algorithms.'
$coarsePath = 'MultiWheelC.TrajectoryPlanning.CoarsePath.'
$mapping = 'MultiWheelC.TrajectoryPlanning.Mapping.'

$snapshotType = Get-RequiredType ($algorithms + 'SmoothingOptionsSnapshot')
$inputType = Get-RequiredType ($algorithms + 'SmoothingAlgorithmInput')
$configurationType = Get-RequiredType ($root + 'PathSmoothingConfiguration')
$requestType = Get-RequiredType ($root + 'PathSmoothingRequest')
$preparedPathType = Get-RequiredType ($root + 'Processing.PreparedPath')
$mapType = Get-RequiredType ($mapping + 'PlanningGridMap')
$vehicleType = Get-RequiredType ($coarsePath + 'VehicleParameters')
$coarsePointType = Get-RequiredType ($coarsePath + 'CoarsePathPoint')
$pathSegmentType = Get-RequiredType ($coarsePath + 'PathSegment')

$snapshotConstructor = $snapshotType.GetConstructor([Reflection.BindingFlags]'Instance,NonPublic', $null,
    @($configurationType), $null)
Assert-True ($null -ne $snapshotConstructor) 'SmoothingOptionsSnapshot must be created from PathSmoothingConfiguration.'

$inputConstructor = $inputType.GetConstructor([Reflection.BindingFlags]'Instance,NonPublic', $null,
    @($preparedPathType, $mapType, $vehicleType, [double], [double], $snapshotType), $null)
Assert-True ($null -ne $inputConstructor) 'SmoothingAlgorithmInput must accept the immutable smoothing-options snapshot at its construction boundary.'

$optionNames = @(
    'CubicBSplineEndpointTangentScale',
    'BezierCornerHeadingThresholdRadians',
    'BezierMaximumWindowLengthMeters',
    'BezierHandleLengthRatio',
    'QuinticKnotSpacingMeters',
    'QuinticMinimumKnotSpacingMeters')
$optionProperties = @{}
foreach ($optionName in $optionNames) {
    $optionProperties[$optionName] = Get-RequiredProperty $snapshotType $optionName
}

function New-Configuration {
    return [Activator]::CreateInstance($configurationType)
}

function New-Snapshot($Configuration) {
    return $snapshotConstructor.Invoke(@($Configuration))
}

# Request creation takes a configuration copy. Later mutations to either the source configuration
# or a configuration copy returned by the request must not alter the algorithm snapshot.
$configuration = New-Configuration
$configuration.CubicBSpline.EndpointTangentScale = [double]0.20
$configuration.LocalCubicBezier.CornerHeadingThresholdRadians = [double]0.40
$configuration.LocalCubicBezier.MaximumWindowLengthMeters = [double]0.80
$configuration.LocalCubicBezier.HandleLengthRatio = [double]0.25
$configuration.PiecewiseQuintic.KnotSpacingMeters = [double]0.60
$configuration.PiecewiseQuintic.MinimumKnotSpacingMeters = [double]0.15
$emptyCoarsePath = [Array]::CreateInstance($coarsePointType, 0)
$emptySegments = [Array]::CreateInstance($pathSegmentType, 0)
$request = [Activator]::CreateInstance($requestType, @($emptyCoarsePath, $emptySegments, $null, $null, $configuration))
$configuration.CubicBSpline.EndpointTangentScale = [double]0.90
$requestConfiguration = $request.Configuration
Assert-Near 0.20 $requestConfiguration.CubicBSpline.EndpointTangentScale 'Request configuration must remain independent from source-config mutations.'
$snapshot = New-Snapshot $requestConfiguration
$requestConfiguration.CubicBSpline.EndpointTangentScale = [double]0.70
Assert-Near 0.20 $optionProperties['CubicBSplineEndpointTangentScale'].GetValue($snapshot) 'Algorithm options must remain independent from request-configuration mutations.'
Assert-Near 0.40 $optionProperties['BezierCornerHeadingThresholdRadians'].GetValue($snapshot) 'Snapshot must preserve the request configuration values.'
Assert-Near 0.80 $optionProperties['BezierMaximumWindowLengthMeters'].GetValue($snapshot) 'Snapshot must preserve the request configuration values.'
Assert-Near 0.25 $optionProperties['BezierHandleLengthRatio'].GetValue($snapshot) 'Snapshot must preserve the request configuration values.'
Assert-Near 0.60 $optionProperties['QuinticKnotSpacingMeters'].GetValue($snapshot) 'Snapshot must preserve the request configuration values.'
Assert-Near 0.15 $optionProperties['QuinticMinimumKnotSpacingMeters'].GetValue($snapshot) 'Snapshot must preserve the request configuration values.'

function Assert-InvalidSnapshot([scriptblock]$Mutate, [string]$Message) {
    $invalidConfiguration = New-Configuration
    & $Mutate $invalidConfiguration
    Assert-Throws { New-Snapshot $invalidConfiguration } $Message
}

Assert-InvalidSnapshot { param($c) $c.CubicBSpline.EndpointTangentScale = [double]::NaN } 'Non-finite B-spline tangent scale must be rejected before retry.'
Assert-InvalidSnapshot { param($c) $c.CubicBSpline.EndpointTangentScale = [double]0.0 } 'Non-positive B-spline tangent scale must be rejected before retry.'
Assert-InvalidSnapshot { param($c) $c.LocalCubicBezier.CornerHeadingThresholdRadians = [double]0.0 } 'A zero Bézier heading threshold must be rejected before retry.'
Assert-InvalidSnapshot { param($c) $c.LocalCubicBezier.CornerHeadingThresholdRadians = [Math]::PI + 0.0001 } 'A Bézier heading threshold above pi must be rejected before retry.'
Assert-InvalidSnapshot { param($c) $c.LocalCubicBezier.MaximumWindowLengthMeters = [double]::PositiveInfinity } 'A non-finite Bézier window length must be rejected before retry.'
Assert-InvalidSnapshot { param($c) $c.LocalCubicBezier.HandleLengthRatio = [double]0.0 } 'A non-positive Bézier handle ratio must be rejected before retry.'
Assert-InvalidSnapshot { param($c) $c.PiecewiseQuintic.KnotSpacingMeters = [double]0.0 } 'A non-positive quintic knot spacing must be rejected before retry.'
Assert-InvalidSnapshot { param($c) $c.PiecewiseQuintic.MinimumKnotSpacingMeters = [double]::NaN } 'A non-finite quintic minimum knot spacing must be rejected before retry.'
Assert-InvalidSnapshot { param($c) $c.PiecewiseQuintic.KnotSpacingMeters = [double]0.05; $c.PiecewiseQuintic.MinimumKnotSpacingMeters = [double]0.10 } 'Quintic knot spacing below the configured minimum must be rejected before retry.'

Write-Output 'Path smoothing algorithm-input checks passed.'
