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 Get-PropertyValue($Instance, [string]$Name) {
    $property = $Instance.GetType().GetProperty($Name, [Reflection.BindingFlags]'Instance,Public,NonPublic')
    Assert-True ($null -ne $property) ("Missing property: " + $Name)
    return $property.GetValue($Instance)
}

function New-Point(
    [double]$X,
    [double]$Y,
    [double]$ArcLength,
    [double]$Heading,
    [double]$BodyClearance = 1.0,
    [bool]$IsGearSwitch = $false) {
    return [Activator]::CreateInstance($pointType, @(
        $X, $Y, $ArcLength, $Heading, $Heading, $BodyClearance, $IsGearSwitch, $anchor))
}

function New-DirectionSegment(
    [int]$Index,
    $Direction,
    [object[]]$Points,
    [bool]$StartsAtGearSwitch = $false,
    [bool]$EndsAtGearSwitch = $false) {
    $typedPoints = [Array]::CreateInstance($pointType, $Points.Count)
    for ($pointIndex = 0; $pointIndex -lt $Points.Count; $pointIndex++) {
        $typedPoints.SetValue($Points[$pointIndex], $pointIndex)
    }
    return [Activator]::CreateInstance($segmentType, @(
        $Index, $Direction, $typedPoints, $StartsAtGearSwitch, $EndsAtGearSwitch))
}

function New-EmptyMap {
    $request = [Activator]::CreateInstance($mapRequestType)
    $request.Bounds = [Activator]::CreateInstance($boundsType, @([single]0, [single]5000, [single]0, [single]5000))
    $request.ResolutionMm = [single]50
    $request.AllowExplicitEmptyMap = $true
    $map = [Activator]::CreateInstance($mapFactoryType).Create($request).Map
    Assert-True ($null -ne $map) 'Quintic test must create an explicit empty planning map.'
    return $map
}

function New-AlgorithmInput(
    [object[]]$Segments,
    [double]$ReserveMeters,
    [double]$KnotSpacingMeters = 1.0,
    [double]$MinimumKnotSpacingMeters = 0.10) {
    $typedSegments = [Array]::CreateInstance($segmentType, $Segments.Count)
    for ($index = 0; $index -lt $Segments.Count; $index++) {
        $typedSegments.SetValue($Segments[$index], $index)
    }
    $preparedPath = [Activator]::CreateInstance($preparedPathType, [object[]]@(,$typedSegments))
    $vehicle = [Activator]::CreateInstance($vehicleType)
    $vehicle.LengthMeters = [double]0.20
    $vehicle.WidthMeters = [double]0.20
    $vehicle.SafetyMarginMeters = [double]0.0
    $vehicle.MaximumCurvaturePerMeter = [double]100.0
    $vehicle.MinimumTurningRadiusMeters = [double]0.01
    $configuration = [Activator]::CreateInstance($configurationType)
    $configuration.PiecewiseQuintic.KnotSpacingMeters = $KnotSpacingMeters
    $configuration.PiecewiseQuintic.MinimumKnotSpacingMeters = $MinimumKnotSpacingMeters
    $options = $optionsConstructor.Invoke(@($configuration))
    return $inputConstructor.Invoke(@($preparedPath, (New-EmptyMap), $vehicle, [double]0.05, $ReserveMeters, $options))
}

function Invoke-Candidate(
    [object[]]$Segments,
    [double]$ReserveMeters = 0.0,
    [double]$KnotSpacingMeters = 1.0,
    [double]$MinimumKnotSpacingMeters = 0.10) {
    return $smoothMethod.Invoke($smoother, @(
        (New-AlgorithmInput $Segments $ReserveMeters $KnotSpacingMeters $MinimumKnotSpacingMeters),
        [double]1.0, [Threading.CancellationToken]::None))
}

function Invoke-Smoothing(
    [object[]]$Segments,
    [double]$ReserveMeters = 0.0,
    [double]$KnotSpacingMeters = 1.0,
    [double]$MinimumKnotSpacingMeters = 0.10) {
    $candidate = Invoke-Candidate $Segments $ReserveMeters $KnotSpacingMeters $MinimumKnotSpacingMeters
    Assert-True (Get-PropertyValue $candidate 'Succeeded') 'Quintic smoothing must produce a candidate for the deterministic fixture.'
    return @(Get-PropertyValue $candidate 'Segments')
}

function Get-PointDistance($Left, $Right) {
    $deltaX = $Left.X - $Right.X
    $deltaY = $Left.Y - $Right.Y
    return [Math]::Sqrt($deltaX * $deltaX + $deltaY * $deltaY)
}

function Get-Reference([object[]]$Source, [double]$ArcLength) {
    $typedPoints = [Array]::CreateInstance($pointType, $Source.Count)
    for ($index = 0; $index -lt $Source.Count; $index++) { $typedPoints.SetValue($Source[$index], $index) }
    $arguments = [object[]]@($typedPoints, $ArcLength, $null, $null)
    Assert-True $interpolateMethod.Invoke($null, $arguments) 'Quintic test must resolve every sampled local-arc reference.'
    return $arguments[2]
}

function Get-PointAtArcLength([object[]]$Points, [double]$ArcLength) {
    foreach ($point in $Points) {
        if ([Math]::Abs($point.ArcLength - $ArcLength) -lt 0.000000000001) { return $point }
    }
    throw "Missing quintic sample at arc length $ArcLength"
}

function Get-EndpointDerivative([object[]]$Samples, [double]$StepMeters, [bool]$AtStart) {
    $firstCoefficients = @((-137.0 / 60.0), 5.0, -5.0, (10.0 / 3.0), (-5.0 / 4.0), (1.0 / 5.0))
    $x = 0.0
    $y = 0.0
    for ($index = 0; $index -lt 6; $index++) {
        $sampleIndex = if ($AtStart) { $index } else { 5 - $index }
        $sign = if ($AtStart) { 1.0 } else { -1.0 }
        $x += $firstCoefficients[$index] * $Samples[$sampleIndex].X
        $y += $firstCoefficients[$index] * $Samples[$sampleIndex].Y
    }
    return [PSCustomObject]@{ X = $sign * $x / $StepMeters; Y = $sign * $y / $StepMeters }
}

function Get-EndpointSecondDerivative([object[]]$Samples, [double]$StepMeters, [bool]$AtStart) {
    $coefficients = @((15.0 / 4.0), (-77.0 / 6.0), (107.0 / 6.0), -13.0, (61.0 / 12.0), (-5.0 / 6.0))
    $x = 0.0
    $y = 0.0
    for ($index = 0; $index -lt 6; $index++) {
        $sampleIndex = if ($AtStart) { $index } else { 5 - $index }
        $x += $coefficients[$index] * $Samples[$sampleIndex].X
        $y += $coefficients[$index] * $Samples[$sampleIndex].Y
    }
    return [PSCustomObject]@{ X = $x / ($StepMeters * $StepMeters); Y = $y / ($StepMeters * $StepMeters) }
}

function Get-IntervalSamples([object[]]$Points, [double]$StartArcLength, [double]$EndArcLength, [bool]$FromStart) {
    $intervalLength = $EndArcLength - $StartArcLength
    $result = @()
    for ($index = 0; $index -lt 6; $index++) {
        $parameter = if ($FromStart) { $index / 8.0 } else { (3.0 + $index) / 8.0 }
        $result += Get-PointAtArcLength $Points ($StartArcLength + $parameter * $intervalLength)
    }
    return $result
}

function Get-QuinticPositionFromInteriorSamples(
    [object[]]$Points,
    [double]$StartArcLength,
    [double]$EndArcLength,
    [double[]]$Parameters,
    [double]$TargetParameter) {
    $intervalLength = $EndArcLength - $StartArcLength
    $x = 0.0
    $y = 0.0
    for ($index = 0; $index -lt $Parameters.Count; $index++) {
        $weight = 1.0
        for ($otherIndex = 0; $otherIndex -lt $Parameters.Count; $otherIndex++) {
            if ($index -ne $otherIndex) {
                $weight *= ($TargetParameter - $Parameters[$otherIndex]) / ($Parameters[$index] - $Parameters[$otherIndex])
            }
        }
        $sample = Get-PointAtArcLength $Points ($StartArcLength + $Parameters[$index] * $intervalLength)
        $x += $weight * $sample.X
        $y += $weight * $sample.Y
    }
    return [PSCustomObject]@{ X = $x; Y = $y }
}

$root = 'MultiWheelC.TrajectoryPlanning.PathSmoothing.'
$processing = $root + 'Processing.'
$algorithms = $root + 'Algorithms.'
$coarsePath = 'MultiWheelC.TrajectoryPlanning.CoarsePath.'

$smootherType = Get-RequiredType ($algorithms + 'PiecewiseQuinticSmoother')
$pointType = Get-RequiredType ($processing + 'SmoothingPoint2D')
$segmentType = Get-RequiredType ($processing + 'PreparedDirectionSegment')
$preparedPathType = Get-RequiredType ($processing + 'PreparedPath')
$inputType = Get-RequiredType ($algorithms + 'SmoothingAlgorithmInput')
$optionsType = Get-RequiredType ($algorithms + 'SmoothingOptionsSnapshot')
$configurationType = Get-RequiredType ($root + 'PathSmoothingConfiguration')
$interpolatorType = Get-RequiredType ($processing + 'PathReferenceInterpolator')
$vehicleType = Get-RequiredType ($coarsePath + 'VehicleParameters')
$directionType = Get-RequiredType ($coarsePath + 'TravelDirection')
$sourceType = Get-RequiredType ($root + 'SmoothedPathPointSource')
$boundsType = Get-RequiredType 'MultiWheelC.TrajectoryPlanning.Mapping.MapBoundsMm'
$mapType = Get-RequiredType 'MultiWheelC.TrajectoryPlanning.Mapping.PlanningGridMap'
$mapRequestType = Get-RequiredType 'MultiWheelC.TrajectoryPlanning.Mapping.PlanningMapRequest'
$mapFactoryType = Get-RequiredType 'MultiWheelC.TrajectoryPlanning.Mapping.PlanningMapFactory'

$inputConstructor = $inputType.GetConstructor([Reflection.BindingFlags]'Instance,NonPublic', $null,
    @($preparedPathType, $mapType, $vehicleType, [double], [double], $optionsType), $null)
Assert-True ($null -ne $inputConstructor) 'Algorithm input must carry immutable quintic options and clearance reserve.'
$optionsConstructor = $optionsType.GetConstructor([Reflection.BindingFlags]'Instance,NonPublic', $null, @($configurationType), $null)
Assert-True ($null -ne $optionsConstructor) 'Quintic tests must create immutable options snapshots.'
$interpolateMethod = $interpolatorType.GetMethod('TryInterpolateByArcLength', [Reflection.BindingFlags]'Static,Public,NonPublic')
Assert-True ($null -ne $interpolateMethod) 'PathReferenceInterpolator must expose local-arc interpolation.'
$smoother = [Activator]::CreateInstance($smootherType, $true)
$smoothMethod = $smootherType.GetMethod('Smooth', [Reflection.BindingFlags]'Instance,Public')
Assert-True ($null -ne $smoothMethod) 'PiecewiseQuinticSmoother must implement the internal smoother contract.'
Assert-Equal 'PiecewiseQuintic' $smoother.Method.ToString() 'Quintic smoother must identify its public smoothing method.'

$forward = [Enum]::Parse($directionType, 'Forward')
$reverse = [Enum]::Parse($directionType, 'Reverse')
$anchor = [Enum]::Parse($sourceType, 'Anchor')

# The 1.0 m local-arc knot spacing creates shared knots at s=1 and s=2.
# The generated 1/8 samples allow exact one-sided quintic derivative reconstruction.
$continuitySource = @(
    (New-Point 0.00 0.00 0.00 0.00),
    (New-Point 0.50 0.00 0.50 0.00),
    (New-Point 1.00 0.00 1.00 0.00),
    (New-Point 1.00 0.50 1.50 ([Math]::PI / 2.0)),
    (New-Point 1.00 1.00 2.00 ([Math]::PI / 2.0)),
    (New-Point 1.30 1.30 2.40 ([Math]::PI / 4.0)))
$continuityOutput = @(Invoke-Smoothing @((New-DirectionSegment 0 $forward $continuitySource)))[0].Points
foreach ($sharedArcLength in @(1.0, 2.0)) {
    $leftSamples = Get-IntervalSamples $continuityOutput ($sharedArcLength - 1.0) $sharedArcLength $false
    $rightEnd = if ($sharedArcLength -eq 2.0) { 2.4 } else { $sharedArcLength + 1.0 }
    $rightSamples = Get-IntervalSamples $continuityOutput $sharedArcLength $rightEnd $true
    $leftPosition = Get-QuinticPositionFromInteriorSamples $continuityOutput ($sharedArcLength - 1.0) $sharedArcLength `
        @((2.0 / 8.0), (3.0 / 8.0), (4.0 / 8.0), (5.0 / 8.0), (6.0 / 8.0), (7.0 / 8.0)) 1.0
    $rightPosition = Get-QuinticPositionFromInteriorSamples $continuityOutput $sharedArcLength $rightEnd `
        @((1.0 / 8.0), (2.0 / 8.0), (3.0 / 8.0), (4.0 / 8.0), (5.0 / 8.0), (6.0 / 8.0)) 0.0
    $leftFirst = Get-EndpointDerivative $leftSamples (1.0 / 8.0) $false
    $rightFirst = Get-EndpointDerivative $rightSamples (($rightEnd - $sharedArcLength) / 8.0) $true
    $leftSecond = Get-EndpointSecondDerivative $leftSamples (1.0 / 8.0) $false
    $rightSecond = Get-EndpointSecondDerivative $rightSamples (($rightEnd - $sharedArcLength) / 8.0) $true
    Assert-Near $leftPosition.X $rightPosition.X 0.000001 'Shared knot X position must match from both quintic intervals.'
    Assert-Near $leftPosition.Y $rightPosition.Y 0.000001 'Shared knot Y position must match from both quintic intervals.'
    Assert-Near $leftFirst.X $rightFirst.X 0.000001 'Shared knot X first derivative must be C1.'
    Assert-Near $leftFirst.Y $rightFirst.Y 0.000001 'Shared knot Y first derivative must be C1.'
    Assert-Near $leftSecond.X $rightSecond.X 0.000001 'Shared knot X second derivative must be C2.'
    Assert-Near $leftSecond.Y $rightSecond.Y 0.000001 'Shared knot Y second derivative must be C2.'
}

$firstOutput = $continuityOutput[0]
$lastOutput = $continuityOutput[$continuityOutput.Count - 1]
Assert-Near $continuitySource[0].X $firstOutput.X 0.0 'Quintic start X must remain exact.'
Assert-Near $continuitySource[0].Y $firstOutput.Y 0.0 'Quintic start Y must remain exact.'
Assert-Near $continuitySource[$continuitySource.Count - 1].X $lastOutput.X 0.0 'Quintic end X must remain exact.'
Assert-Near $continuitySource[$continuitySource.Count - 1].Y $lastOutput.Y 0.0 'Quintic end Y must remain exact.'
foreach ($point in $continuityOutput) {
    $reference = Get-Reference $continuitySource $point.ArcLength
    Assert-True ((Get-PointDistance $point $reference) -le ($reference.BodyClearance + 0.000000000001)) 'Every quintic sample must remain inside its local reference movement bound.'
}

# Separate prepared direction segments must retain their exact duplicated switch pose and topology.
$reverseSource = @(
    (New-Point 1.30 1.30 0.00 ([Math]::PI / 4.0) 1.0 $true),
    (New-Point 1.30 0.80 0.50 ([Math]::PI / 2.0)),
    (New-Point 1.30 0.30 1.00 ([Math]::PI / 2.0)))
$switchOutput = @(Invoke-Smoothing @(
    (New-DirectionSegment 0 $forward $continuitySource $false $true),
    (New-DirectionSegment 1 $reverse $reverseSource $true $false)))
Assert-Equal 2 $switchOutput.Count 'Quintic smoothing must retain separate direction segments.'
Assert-True $switchOutput[0].EndsAtGearSwitch 'Forward quintic segment must retain its gear-switch boundary flag.'
Assert-True $switchOutput[1].StartsAtGearSwitch 'Reverse quintic segment must retain its gear-switch boundary flag.'
$leftSwitch = $switchOutput[0].Points[$switchOutput[0].Points.Count - 1]
$rightSwitch = $switchOutput[1].Points[0]
Assert-Near $leftSwitch.X $rightSwitch.X 0.0 'Quintic smoothing must preserve switch X exactly.'
Assert-Near $leftSwitch.Y $rightSwitch.Y 0.0 'Quintic smoothing must preserve switch Y exactly.'

# Spacing selects local-arc knots, while a valid minimum spacing does not change that selection.
$oneMeterOutput = @(Invoke-Smoothing @((New-DirectionSegment 0 $forward $continuitySource)) 0.0 1.0 0.10)[0].Points
$halfMeterOutput = @(Invoke-Smoothing @((New-DirectionSegment 0 $forward $continuitySource)) 0.0 0.50 0.10)[0].Points
$largeMinimumOutput = @(Invoke-Smoothing @((New-DirectionSegment 0 $forward $continuitySource)) 0.0 1.0 0.30)[0].Points
Assert-True ($halfMeterOutput.Count -gt $oneMeterOutput.Count) 'Custom knot spacing must create additional local-arc knot intervals.'
Assert-Equal $oneMeterOutput.Count $largeMinimumOutput.Count 'Valid minimum knot spacing must not change knot selection.'
for ($index = 0; $index -lt $oneMeterOutput.Count; $index++) {
    Assert-Near $oneMeterOutput[$index].X $largeMinimumOutput[$index].X 0.000000000001 'Minimum knot spacing must not change valid quintic X geometry.'
    Assert-Near $oneMeterOutput[$index].Y $largeMinimumOutput[$index].Y 0.000000000001 'Minimum knot spacing must not change valid quintic Y geometry.'
}

$shortSource = @(
    (New-Point 0.00 0.00 0.00 0.00),
    (New-Point 0.05 0.00 0.05 0.00))
$shortCandidate = Invoke-Candidate @((New-DirectionSegment 0 $forward $shortSource)) 0.0 1.0 0.10
Assert-Equal 'Failed' (Get-PropertyValue $shortCandidate 'Status').ToString() 'A segment shorter than the configured minimum knot spacing must fail terminally.'
Assert-True (-not (Get-PropertyValue $shortCandidate 'Succeeded')) 'A degenerate short quintic segment must not be executable.'
Assert-Equal 0 (Get-PropertyValue $shortCandidate 'Segments').Count 'A terminal quintic degeneracy must publish no geometry.'

# Repeated decimal addition must not create a spurious 1e-16 m terminal residual when the
# requested spacing divides the local length exactly. A real 0.005 m remainder remains below
# the 0.01 m minimum and must still fail terminally.
$decimalMultipleSource = @(
    (New-Point 0.0 0.0 0.0 0.0),
    (New-Point 1.0 0.0 1.0 0.0))
$decimalMultipleCandidate = Invoke-Candidate @((New-DirectionSegment 0 $forward $decimalMultipleSource)) 0.0 0.10 0.01
Assert-Equal 'Success' (Get-PropertyValue $decimalMultipleCandidate 'Status').ToString() 'Decimal-exact knot multiples must not become a terminal under-minimum residual failure.'
$decimalMultipleOutput = @(Get-PropertyValue $decimalMultipleCandidate 'Segments')[0].Points
Assert-Equal 81 $decimalMultipleOutput.Count 'A 1.0 m segment with 0.1 m spacing must create exactly ten valid quintic intervals.'
Assert-Near 1.0 $decimalMultipleOutput[$decimalMultipleOutput.Count - 1].ArcLength 0.0 'Decimal-multiple knot normalization must retain the exact terminal arc length.'

$meaningfulShortResidualSource = @(
    (New-Point 0.0 0.0 0.0 0.0),
    (New-Point 1.005 0.0 1.005 0.0))
$meaningfulShortResidualCandidate = Invoke-Candidate @((New-DirectionSegment 0 $forward $meaningfulShortResidualSource)) 0.0 0.10 0.01
Assert-Equal 'Failed' (Get-PropertyValue $meaningfulShortResidualCandidate 'Status').ToString() 'A genuine 0.005 m terminal residual must remain a terminal spacing failure.'
Assert-Equal 0 (Get-PropertyValue $meaningfulShortResidualCandidate 'Segments').Count 'A genuine under-minimum residual must publish no geometry.'

$smallScaleResidualSource = @(
    (New-Point 0.0 0.0 0.0 0.0),
    (New-Point 0.000000000000105 0.0 0.000000000000105 0.0))
$smallScaleResidualCandidate = Invoke-Candidate @((New-DirectionSegment 0 $forward $smallScaleResidualSource)) 0.0 0.00000000000001 0.000000000000006
Assert-Equal 'Failed' (Get-PropertyValue $smallScaleResidualCandidate 'Status').ToString() 'Endpoint normalization tolerance must scale with local arc magnitude and preserve a genuine small residual failure.'

# Local-arc reference mapping, rather than sample index/global distance, must reject this unsafe nonuniform path.
$nonuniformUnsafeSource = @(
    (New-Point 0.0 0.0 0.0 0.0 0.50),
    (New-Point 1.0 0.0 4.0 0.0 0.50),
    (New-Point 2.0 0.0 5.0 0.0 0.50),
    (New-Point 3.0 0.0 6.0 0.0 0.50),
    (New-Point 3.0 1.0 9.0 ([Math]::PI / 2.0) 0.50),
    (New-Point 3.0 2.0 20.0 ([Math]::PI / 2.0) 0.50))
$nonuniformUnsafeCandidate = Invoke-Candidate @((New-DirectionSegment 0 $forward $nonuniformUnsafeSource)) 0.0 5.0 0.10
Assert-Equal 'RetryableInfeasible' (Get-PropertyValue $nonuniformUnsafeCandidate 'Status').ToString() 'Unsafe nonuniform local-arc quintic movement must be retryable.'
Assert-True (-not (Get-PropertyValue $nonuniformUnsafeCandidate 'Succeeded')) 'Unsafe nonuniform quintic geometry must not be executable.'
Assert-Equal 0 (Get-PropertyValue $nonuniformUnsafeCandidate 'Segments').Count 'Retryable quintic infeasibility must publish no geometry.'

Write-Output 'Path smoothing piecewise quintic checks passed.'