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-False($Actual, [string]$Message) {
    if ($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 Get-RequiredType([string]$Name) {
    return $assembly.GetType($Name, $true)
}

function New-Map([bool]$WithObstacle) {
    $bounds = [Activator]::CreateInstance($boundsType, @([single]0, [single]4000, [single]0, [single]4000))
    $request = [Activator]::CreateInstance($mapRequestType)
    $request.Bounds = $bounds
    $request.ResolutionMm = [single]50
    if ($WithObstacle) {
        $obstacle = [Activator]::CreateInstance($rectangleType, @([single]1900, [single]2100, [single]1800, [single]2200))
        $obstacles = [Array]::CreateInstance($obstacleType, 1)
        $obstacles.SetValue($obstacle, 0)
        $source = [Activator]::CreateInstance($manualSourceType, @('validator-obstacle', [long]1, $true, $obstacles))
        $sources = [Array]::CreateInstance($obstacleSourceType, 1)
        $sources.SetValue($source, 0)
        $request.ObstacleSources = $sources
    } else {
        $request.ObstacleSources = [Array]::CreateInstance($obstacleSourceType, 0)
        $request.AllowExplicitEmptyMap = $true
    }
    $result = [Activator]::CreateInstance($mapFactoryType).Create($request)
    Assert-True $result.Succeeded 'Validation test map must be built.'
    Assert-True $result.Map.PlanningReady 'Validation test map must be ready.'
    return $result.Map
}

function New-SmoothedPoint([double]$X, [double]$Y, [double]$ArcLength, $Direction,
    [double]$VehicleCurvature = 0.0, [bool]$IsGearSwitch = $false, [double]$Clearance = 999.0) {
    return [Activator]::CreateInstance($smoothedPointType, @(
        $X, $Y, [double]0.0, [double]0.0, $ArcLength, $Direction,
        $VehicleCurvature, $VehicleCurvature, $Clearance, $IsGearSwitch, $anchor))
}

function New-PreparedPoint([double]$X, [double]$Y, [double]$ArcLength, [bool]$IsGearSwitch = $false) {
    return [Activator]::CreateInstance($smoothingPointType, @(
        $X, $Y, $ArcLength, [double]0.0, [double]0.0, [double]999.0, $IsGearSwitch, $anchor))
}

function New-OneSegmentCase([double]$X0, [double]$Y0, [double]$X1, [double]$Y1, [double]$VehicleCurvature = 0.0,
    [double]$StartArcLength = 0.0) {
    $candidatePath = [Array]::CreateInstance($smoothedPointType, 2)
    $candidatePath.SetValue((New-SmoothedPoint $X0 $Y0 $StartArcLength $forward), 0)
    $candidatePath.SetValue((New-SmoothedPoint $X1 $Y1 ($StartArcLength + 1.0) $forward $VehicleCurvature), 1)
    $candidateSegments = [Array]::CreateInstance($smoothedSegmentType, 1)
    $candidateSegments.SetValue([Activator]::CreateInstance($smoothedSegmentType, @(0, $forward, 0, 1, $false, $false)), 0)
    $preparedPoints = [Array]::CreateInstance($smoothingPointType, 2)
    $preparedPoints.SetValue((New-PreparedPoint $X0 $Y0 0.0), 0)
    $preparedPoints.SetValue((New-PreparedPoint $X1 $Y1 1.0), 1)
    $preparedSegments = [Array]::CreateInstance($preparedSegmentType, 1)
    $preparedSegments.SetValue([Activator]::CreateInstance($preparedSegmentType, @(0, $forward, $preparedPoints, $false, $false)), 0)
    return [PSCustomObject]@{
        CandidatePath = $candidatePath
        CandidateSegments = $candidateSegments
        Original = [Activator]::CreateInstance($preparedPathType, [object[]]@(,$preparedSegments))
    }
}

function Invoke-Validation($Case, $Map) {
    $arguments = [object[]]@($Case.CandidatePath, $Case.CandidateSegments, $Case.Original, $Map, $vehicle,
        [double]0.05, $null, [double]0.0, $null)
    $accepted = $validateMethod.Invoke($validator, $arguments)
    return [PSCustomObject]@{ Accepted = $accepted; Path = $arguments[6]; MinimumClearance = $arguments[7]; Reason = $arguments[8] }
}

function New-MovedGearSwitchCase {
    $candidatePath = [Array]::CreateInstance($smoothedPointType, 4)
    $candidatePath.SetValue((New-SmoothedPoint 0.5 0.5 0.0 $forward), 0)
    $candidatePath.SetValue((New-SmoothedPoint 1.5 0.5 1.0 $forward), 1)
    $candidatePath.SetValue((New-SmoothedPoint 1.6 0.5 1.0 $reverse 0.0 $true), 2)
    $candidatePath.SetValue((New-SmoothedPoint 0.5 0.5 2.0 $reverse), 3)
    $candidateSegments = [Array]::CreateInstance($smoothedSegmentType, 2)
    $candidateSegments.SetValue([Activator]::CreateInstance($smoothedSegmentType, @(0, $forward, 0, 1, $false, $true)), 0)
    $candidateSegments.SetValue([Activator]::CreateInstance($smoothedSegmentType, @(1, $reverse, 2, 3, $true, $false)), 1)
    $preparedFirst = [Array]::CreateInstance($smoothingPointType, 2)
    $preparedFirst.SetValue((New-PreparedPoint 0.5 0.5 0.0), 0)
    $preparedFirst.SetValue((New-PreparedPoint 1.5 0.5 1.0), 1)
    $preparedSecond = [Array]::CreateInstance($smoothingPointType, 2)
    $preparedSecond.SetValue((New-PreparedPoint 1.5 0.5 0.0 $true), 0)
    $preparedSecond.SetValue((New-PreparedPoint 0.5 0.5 1.0), 1)
    $preparedSegments = [Array]::CreateInstance($preparedSegmentType, 2)
    $preparedSegments.SetValue([Activator]::CreateInstance($preparedSegmentType, @(0, $forward, $preparedFirst, $false, $true)), 0)
    $preparedSegments.SetValue([Activator]::CreateInstance($preparedSegmentType, @(1, $reverse, $preparedSecond, $true, $false)), 1)
    return [PSCustomObject]@{
        CandidatePath = $candidatePath
        CandidateSegments = $candidateSegments
        Original = [Activator]::CreateInstance($preparedPathType, [object[]]@(,$preparedSegments))
    }
}

$root = 'MultiWheelC.TrajectoryPlanning.PathSmoothing.'
$processing = $root + 'Processing.'
$validation = $root + 'Validation.'
$coarsePath = 'MultiWheelC.TrajectoryPlanning.CoarsePath.'
$mapping = 'MultiWheelC.TrajectoryPlanning.Mapping.'

$validatorType = Get-RequiredType ($validation + 'SmoothedPathValidator')
$smoothedPointType = Get-RequiredType ($root + 'SmoothedPathPoint')
$smoothedSegmentType = Get-RequiredType ($root + 'SmoothedPathSegment')
$smoothingPointType = Get-RequiredType ($processing + 'SmoothingPoint2D')
$preparedSegmentType = Get-RequiredType ($processing + 'PreparedDirectionSegment')
$preparedPathType = Get-RequiredType ($processing + 'PreparedPath')
$directionType = Get-RequiredType ($coarsePath + 'TravelDirection')
$sourceType = Get-RequiredType ($root + 'SmoothedPathPointSource')
$vehicleType = Get-RequiredType ($coarsePath + 'VehicleParameters')
$boundsType = Get-RequiredType ($mapping + 'MapBoundsMm')
$obstacleType = Get-RequiredType ($mapping + 'IMapObstacle')
$rectangleType = Get-RequiredType ($mapping + 'AxisAlignedRectangleObstacle')
$obstacleSourceType = Get-RequiredType ($mapping + 'IMapObstacleSource')
$manualSourceType = Get-RequiredType ($mapping + 'ManualObstacleSource')
$mapRequestType = Get-RequiredType ($mapping + 'PlanningMapRequest')
$mapFactoryType = Get-RequiredType ($mapping + 'PlanningMapFactory')

$validator = [Activator]::CreateInstance($validatorType)
$validateMethod = $validatorType.GetMethod('TryValidate')
Assert-True ($null -ne $validateMethod) 'SmoothedPathValidator must expose TryValidate.'
Assert-True ($validateMethod.GetParameters().Length -eq 9) 'TryValidate must accept path, segments, originals, map, vehicle, step, path, clearance, and reason.'
$forward = [Enum]::Parse($directionType, 'Forward')
$reverse = [Enum]::Parse($directionType, 'Reverse')
$anchor = [Enum]::Parse($sourceType, 'Anchor')
$vehicle = [Activator]::CreateInstance($vehicleType)
$vehicle.LengthMeters = 0.20
$vehicle.WidthMeters = 0.20
$vehicle.SafetyMarginMeters = 0.0
$vehicle.MaximumCurvaturePerMeter = 1.0
$vehicle.MinimumTurningRadiusMeters = 1.0

$emptyMap = New-Map $false
$obstacleMap = New-Map $true
$valid = Invoke-Validation (New-OneSegmentCase 0.5 0.5 1.5 0.5) $obstacleMap
Assert-True $valid.Accepted ('A valid straight candidate must pass. Reason=' + $valid.Reason)
Assert-True ($null -ne $valid.Path) 'A valid candidate must return clearance-recomputed points.'
Assert-True ($valid.Path[0].BodyClearance -lt 999.0) 'Validated output must replace an overclaimed candidate clearance.'
Assert-True ($valid.MinimumClearance -ge 0.0) 'A valid candidate must report non-negative conservative clearance.'

$pointCollision = Invoke-Validation (New-OneSegmentCase 2.0 2.0 2.5 2.0) $obstacleMap
Assert-False $pointCollision.Accepted 'A smoothing candidate that touches an obstacle must be rejected.'
$sweptCollision = Invoke-Validation (New-OneSegmentCase 1.5 2.0 2.5 2.0) $obstacleMap
Assert-False $sweptCollision.Accepted 'A smoothing candidate whose sweep cuts through an obstacle must be rejected.'
$outsideBounds = Invoke-Validation (New-OneSegmentCase 0.05 0.5 0.5 0.5) $emptyMap
Assert-False $outsideBounds.Accepted 'A smoothing candidate outside map bounds must be rejected.'
$overCurvature = Invoke-Validation (New-OneSegmentCase 0.5 0.5 1.5 0.5 2.0) $emptyMap
Assert-False $overCurvature.Accepted 'A smoothing candidate above vehicle maximum curvature must be rejected.'
$nonzeroStartArc = Invoke-Validation (New-OneSegmentCase 0.5 0.5 1.5 0.5 0.0 1.0) $emptyMap
Assert-False $nonzeroStartArc.Accepted 'A smoothing candidate must start at global arc length zero.'
$movedSwitch = Invoke-Validation (New-MovedGearSwitchCase) $emptyMap
Assert-False $movedSwitch.Accepted 'A smoothing candidate with a moved gear-switch pose must be rejected.'

Write-Output 'Path smoothing validation checks passed.'