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

function Assert-Rejected($Result, [string]$Message) {
    if ($Result.Accepted) { throw $Message }
    Assert-True (-not [string]::IsNullOrWhiteSpace($Result.Reason)) 'Rejected curves must provide a stable reason.'
}

function Assert-Throws([scriptblock]$Action, [string]$Message) {
    try {
        & $Action
    }
    catch [ArgumentOutOfRangeException] {
        return
    }
    catch [System.Reflection.TargetInvocationException] {
        Assert-True ($_.Exception.InnerException -is [ArgumentOutOfRangeException]) $Message
        return
    }
    throw $Message
}

$curveType = $assembly.GetType('MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2.QuinticHermiteCurve2D', $true)
$tryCreate = $curveType.GetMethod('TryCreate', [Reflection.BindingFlags]'Static,NonPublic')
$evaluate = $curveType.GetMethod('Evaluate', [Reflection.BindingFlags]'Instance,NonPublic')
Assert-True ($null -ne $tryCreate) 'QuinticHermiteCurve2D must expose its internal TryCreate factory.'
Assert-True ($null -ne $evaluate) 'QuinticHermiteCurve2D must expose its internal Evaluate method.'

function Invoke-Curve(
    [double]$X0, [double]$Y0, [double]$Dx0, [double]$Dy0, [double]$Ddx0, [double]$Ddy0,
    [double]$X1, [double]$Y1, [double]$Dx1, [double]$Dy1, [double]$Ddx1, [double]$Ddy1) {
    $arguments = [object[]]@($X0, $Y0, $Dx0, $Dy0, $Ddx0, $Ddy0, $X1, $Y1, $Dx1, $Dy1, $Ddx1, $Ddy1, $null, $null)
    $accepted = $tryCreate.Invoke($null, $arguments)
    Assert-True $accepted "Quintic Hermite curve creation must succeed. Reason=$($arguments[13])"
    return $arguments[12]
}

function Invoke-InvalidCurve([string]$CaseName) {
    $arguments = switch ($CaseName) {
        'ZeroFirstDerivative' { [object[]]@(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.0, 0.0, 1.0, 0.0, 0.0, 0.0, $null, $null) }
        'NonFiniteInput' { [object[]]@(0.0, 0.0, [double]::NaN, 0.0, 0.0, 0.0, 2.0, 0.0, 1.0, 0.0, 0.0, 0.0, $null, $null) }
        default { throw "Unknown invalid curve case: $CaseName" }
    }
    return [PSCustomObject]@{
        Accepted = $tryCreate.Invoke($null, $arguments)
        Reason = [string]$arguments[13]
    }
}

function Invoke-Evaluate($Curve, [double]$U) {
    $arguments = [object[]]@($U, $null, $null, $null, $null, $null, $null)
    $evaluate.Invoke($Curve, $arguments)
    return [PSCustomObject]@{
        X = [double]$arguments[1]; Y = [double]$arguments[2]
        Dx = [double]$arguments[3]; Dy = [double]$arguments[4]
        Ddx = [double]$arguments[5]; Ddy = [double]$arguments[6]
    }
}

function Get-Curvature($Sample) {
    return ($Sample.Dx * $Sample.Ddy - $Sample.Dy * $Sample.Ddx) /
        [Math]::Pow($Sample.Dx * $Sample.Dx + $Sample.Dy * $Sample.Dy, 1.5)
}

function Assert-Boundary($Expected, $Actual, [string]$Name) {
    Assert-Near $Expected.X $Actual.X 1e-10 "$Name X must match."
    Assert-Near $Expected.Y $Actual.Y 1e-10 "$Name Y must match."
    Assert-Near $Expected.Dx $Actual.Dx 1e-10 "$Name dX must match."
    Assert-Near $Expected.Dy $Actual.Dy 1e-10 "$Name dY must match."
    Assert-Near $Expected.Ddx $Actual.Ddx 1e-10 "$Name ddX must match."
    Assert-Near $Expected.Ddy $Actual.Ddy 1e-10 "$Name ddY must match."
}

$straight = Invoke-Curve 0 0 1 0 0 0  2 0 1 0 0 0
$straightStart = Invoke-Evaluate $straight 0.0
$straightEnd = Invoke-Evaluate $straight 1.0
$straightMid = Invoke-Evaluate $straight 0.5
Assert-Boundary ([PSCustomObject]@{ X = 0.0; Y = 0.0; Dx = 1.0; Dy = 0.0; Ddx = 0.0; Ddy = 0.0 }) $straightStart 'Straight start'
Assert-Boundary ([PSCustomObject]@{ X = 2.0; Y = 0.0; Dx = 1.0; Dy = 0.0; Ddx = 0.0; Ddy = 0.0 }) $straightEnd 'Straight end'
Assert-Near 0.0 $straightMid.Y 1e-12 'A straight Hermite curve must remain on the axis.'

$curved = Invoke-Curve 0 0 1 0 0 0.4  1 1 0 1 -0.4 0
$curvedStart = Invoke-Evaluate $curved 0.0
$curvedEnd = Invoke-Evaluate $curved 1.0
Assert-Boundary ([PSCustomObject]@{ X = 0.0; Y = 0.0; Dx = 1.0; Dy = 0.0; Ddx = 0.0; Ddy = 0.4 }) $curvedStart 'Curved start'
Assert-Boundary ([PSCustomObject]@{ X = 1.0; Y = 1.0; Dx = 0.0; Dy = 1.0; Ddx = -0.4; Ddy = 0.0 }) $curvedEnd 'Curved end'
Assert-Near 0.4 (Get-Curvature $curvedStart) 1e-10 'Start curvature must match boundary derivatives.'
Assert-Near 0.4 (Get-Curvature $curvedEnd) 1e-10 'End curvature must match boundary derivatives.'

$allFinite = $true
foreach ($u in @(0.0, 0.25, 0.5, 0.75, 1.0)) {
    $sample = Invoke-Evaluate $curved $u
    foreach ($value in @($sample.X, $sample.Y, $sample.Dx, $sample.Dy, $sample.Ddx, $sample.Ddy)) {
        $allFinite = $allFinite -and (-not [double]::IsNaN($value)) -and (-not [double]::IsInfinity($value))
    }
}
Assert-True $allFinite 'All sampled values must be finite.'

Assert-Rejected (Invoke-InvalidCurve 'ZeroFirstDerivative') `
    'A curve endpoint with zero first derivative must be rejected.'
Assert-Rejected (Invoke-InvalidCurve 'NonFiniteInput') `
    'A curve with a non-finite boundary value must be rejected.'
Assert-Throws { Invoke-Evaluate $curved -0.001 } 'Evaluate must reject parameters below zero.'
Assert-Throws { Invoke-Evaluate $curved 1.001 } 'Evaluate must reject parameters above one.'

Write-Output 'Local G2 quintic curve checks passed.'
