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,
    [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) 'B-spline test must create an explicit empty planning map.'
    return $map
}

function New-AlgorithmInput([object[]]$Segments, [double]$ReserveMeters) {
    $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
    return $inputConstructor.Invoke(@($preparedPath, (New-EmptyMap), $vehicle, [double]0.05, $ReserveMeters))
}

function Invoke-Smoothing([object[]]$Segments, [double]$ReserveMeters, [double]$Strength = 1.0) {
    $candidate = $smoothMethod.Invoke($smoother, @(
        (New-AlgorithmInput $Segments $ReserveMeters), $Strength, [Threading.CancellationToken]::None))
    Assert-True (Get-PropertyValue $candidate 'Succeeded') 'B-spline 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-DistanceToSegment($Point, $Left, $Right) {
    $deltaX = $Right.X - $Left.X
    $deltaY = $Right.Y - $Left.Y
    $lengthSquared = $deltaX * $deltaX + $deltaY * $deltaY
    if ($lengthSquared -le 0.0) { return Get-PointDistance $Point $Left }
    $projection = (($Point.X - $Left.X) * $deltaX + ($Point.Y - $Left.Y) * $deltaY) / $lengthSquared
    $projection = [Math]::Max(0.0, [Math]::Min(1.0, $projection))
    $closest = New-Object PSObject -Property @{
        X = $Left.X + $projection * $deltaX
        Y = $Left.Y + $projection * $deltaY
    }
    return Get-PointDistance $Point $closest
}

function Get-DistanceToPolyline($Point, [object[]]$SourcePoints) {
    $minimum = [double]::PositiveInfinity
    for ($index = 1; $index -lt $SourcePoints.Count; $index++) {
        $minimum = [Math]::Min($minimum, (Get-DistanceToSegment $Point $SourcePoints[$index - 1] $SourcePoints[$index]))
    }
    return $minimum
}

function Get-TravelAngle($Left, $Right) {
    return [Math]::Atan2($Right.Y - $Left.Y, $Right.X - $Left.X)
}

function Get-AngleDifference([double]$Left, [double]$Right) {
    $difference = $Left - $Right
    while ($difference -gt [Math]::PI) { $difference -= 2.0 * [Math]::PI }
    while ($difference -lt -[Math]::PI) { $difference += 2.0 * [Math]::PI }
    return [Math]::Abs($difference)
}

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

$smootherType = Get-RequiredType ($algorithms + 'CubicBSplineSmoother')
$pointType = Get-RequiredType ($processing + 'SmoothingPoint2D')
$segmentType = Get-RequiredType ($processing + 'PreparedDirectionSegment')
$preparedPathType = Get-RequiredType ($processing + 'PreparedPath')
$inputType = Get-RequiredType ($algorithms + 'SmoothingAlgorithmInput')
$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]), $null)
Assert-True ($null -ne $inputConstructor) 'Algorithm input must carry the minimum clearance reserve for per-anchor movement limits.'
$smoother = [Activator]::CreateInstance($smootherType, $true)
$smoothMethod = $smootherType.GetMethod('Smooth', [Reflection.BindingFlags]'Instance,Public')
Assert-True ($null -ne $smoothMethod) 'CubicBSplineSmoother must implement the internal smoother contract.'
Assert-Equal 'CubicBSpline' $smoother.Method.ToString() 'B-spline smoother must identify its public smoothing method.'

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

# Straight samples are returned exactly, so a straight is never distorted or densified.
$straightSource = @(
    (New-Point 0.0 0.0 0.0 0.0 0.03),
    (New-Point 1.0 0.0 1.0 0.0 0.03),
    (New-Point 2.0 0.0 2.0 0.0 0.03),
    (New-Point 3.0 0.0 3.0 0.0 0.03))
$straightResult = Invoke-Smoothing @((New-DirectionSegment 0 $forward $straightSource)) 0.02
Assert-Equal 1 $straightResult.Count 'A single direction segment must produce exactly one candidate segment.'
Assert-Equal $straightSource.Count $straightResult[0].Points.Count 'A straight must retain its original samples.'
for ($index = 0; $index -lt $straightSource.Count; $index++) {
    Assert-Near $straightSource[$index].X $straightResult[0].Points[$index].X 0.0 'Straight X coordinates must remain exact.'
    Assert-Near $straightSource[$index].Y $straightResult[0].Points[$index].Y 0.0 'Straight Y coordinates must remain exact.'
}

# A five-anchor corner uses the preprocessor's 0.05 m sampling scale. It must retain exact endpoint poses,
# follow endpoint travel tangents, turn continuously, and stay within the per-anchor clearance reserve radius.
$cornerSource = @(
    (New-Point 0.0 0.0 0.0 0.0 0.08),
    (New-Point 0.05 0.0 0.05 0.0 0.08),
    (New-Point 0.10 0.0 0.10 0.0 0.08),
    (New-Point 0.10 0.05 0.15 ([Math]::PI / 2.0) 0.08),
    (New-Point 0.10 0.10 0.20 ([Math]::PI / 2.0) 0.08))
$cornerResult = Invoke-Smoothing @((New-DirectionSegment 0 $forward $cornerSource)) 0.02
$cornerPoints = @($cornerResult[0].Points)
Assert-True ($cornerPoints.Count -gt $cornerSource.Count) 'A non-straight B-spline candidate must provide sampled curve geometry.'
$cornerStart = $cornerPoints[0]
$cornerEnd = $cornerPoints[$cornerPoints.Count - 1]
Assert-Near $cornerSource[0].X $cornerStart.X 0.0 'B-spline start X must be exact.'
Assert-Near $cornerSource[0].Y $cornerStart.Y 0.0 'B-spline start Y must be exact.'
Assert-Near $cornerSource[$cornerSource.Count - 1].X $cornerEnd.X 0.0 'B-spline end X must be exact.'
Assert-Near $cornerSource[$cornerSource.Count - 1].Y $cornerEnd.Y 0.0 'B-spline end Y must be exact.'
Assert-Near 0.0 (Get-AngleDifference (Get-TravelAngle $cornerPoints[0] $cornerPoints[1]) 0.0) 0.02 'B-spline start travel tangent must follow the supplied forward heading.'
Assert-Near 0.0 (Get-AngleDifference (Get-TravelAngle $cornerPoints[$cornerPoints.Count - 2] $cornerPoints[$cornerPoints.Count - 1]) ([Math]::PI / 2.0)) 0.02 'B-spline end travel tangent must follow the supplied forward heading.'
for ($index = 2; $index -lt $cornerPoints.Count; $index++) {
    $previousAngle = Get-TravelAngle $cornerPoints[$index - 2] $cornerPoints[$index - 1]
    $currentAngle = Get-TravelAngle $cornerPoints[$index - 1] $cornerPoints[$index]
    Assert-True ((Get-AngleDifference $previousAngle $currentAngle) -lt 0.08) 'B-spline corner samples must turn without a tangent discontinuity.'
}

# Every evaluated point may deviate only by BodyClearance - reserve, never by raw BodyClearance.
$allowedRadius = 0.06
foreach ($point in $cornerPoints) {
    Assert-True ((Get-DistanceToPolyline $point $cornerSource) -le ($allowedRadius + 0.000000001)) 'Every B-spline displacement must stay inside the per-anchor clearance reserve radius.'
}

# Adjacent direction segments retain their duplicated switch pose and independent topology; no fit may cross the switch.
$reverseSource = @(
    (New-Point 0.10 0.10 0.0 ([Math]::PI / 2.0) 0.08 $true),
    (New-Point 0.10 0.05 0.05 ([Math]::PI / 2.0) 0.08),
    (New-Point 0.10 0.0 0.10 ([Math]::PI / 2.0) 0.08))
$switchResult = Invoke-Smoothing @(
    (New-DirectionSegment 0 $forward $cornerSource $false $true),
    (New-DirectionSegment 1 $reverse $reverseSource $true $false)) 0.02
Assert-Equal 2 $switchResult.Count 'B-spline smoothing must preserve each direction segment boundary.'
Assert-True $switchResult[0].EndsAtGearSwitch 'The forward segment must retain its gear-switch boundary flag.'
Assert-True $switchResult[1].StartsAtGearSwitch 'The reverse segment must retain its gear-switch boundary flag.'
$switchLeft = $switchResult[0].Points[$switchResult[0].Points.Count - 1]
$switchRight = $switchResult[1].Points[0]
Assert-Near $switchLeft.X $switchRight.X 0.0 'B-spline smoothing must retain the duplicated gear-switch X pose.'
Assert-Near $switchLeft.Y $switchRight.Y 0.0 'B-spline smoothing must retain the duplicated gear-switch Y pose.'

Write-Output 'Path smoothing cubic B-spline checks passed.'