<#
.SYNOPSIS
    Removes gcode lines where X and Y coordinates differ from the previous
    kept line by <= Threshold (default 0.0006), which causes Komo CRC Fault errors.

.DESCRIPTION
    Parses a gcode .OUT file and removes movement lines where both the X and Y
    differences from the last kept position are within the threshold AND there
    is no significant Z change. Lines with Z moves are always kept.
    This eliminates near-zero XY moves that trigger Komo ERROR: CRC Fault.

    Only cutting moves (G1, G2, G3) are candidates for removal. All other lines
    including G0 (rapid), G28 (home), G52 (offset), M-codes, and comments are
    always kept unchanged.

.PARAMETER InputFile
    Path to the input gcode file.

.PARAMETER OutputFile
    Path for the cleaned output file. Defaults to input filename with "_cleaned"
    appended before the extension.

.PARAMETER Threshold
    Maximum X or Y difference to consider a line for removal. Default 0.0006.

.EXAMPLE
    .\Remove-SmallMoves.ps1 "C:\path\to\file.OUT"
    .\Remove-SmallMoves.ps1 -InputFile "file.OUT" -OutputFile "file_fixed.OUT" -Threshold 0.001
#>

param(
    [Parameter(Mandatory = $true)]
    [string]$InputFile,

    [string]$OutputFile,

    [double]$Threshold = 0.0006
)

if (-not $OutputFile) {
    $OutputFile = [System.IO.Path]::Combine(
        [System.IO.Path]::GetDirectoryName($InputFile),
        [System.IO.Path]::GetFileNameWithoutExtension($InputFile) + '_cleaned' + [System.IO.Path]::GetExtension($InputFile)
    )
}

$lines = Get-Content $InputFile

$lastX = $null
$lastY = $null
$lastZ = $null
$removedLines = [System.Collections.ArrayList]::new()
$result = [System.Collections.ArrayList]::new()
$removedCount = 0
$lineNum = 0

foreach ($line in $lines) {
    $lineNum++

    $xMatch = [regex]::Match($line, 'X(-?\d+\.?\d*)')
    $yMatch = [regex]::Match($line, 'Y(-?\d+\.?\d*)')
    $zMatch = [regex]::Match($line, 'Z(-?\d+\.?\d*)')

    $hasX = $xMatch.Success
    $hasY = $yMatch.Success
    $hasZ = $zMatch.Success

    if (-not ($hasX -or $hasY -or $hasZ)) {
        [void]$result.Add($line)
        continue
    }

    $isCuttingMove = $line -match '\bG[123]\b'

    if (-not $isCuttingMove) {
        if ($hasX) { $lastX = $x }
        if ($hasY) { $lastY = $y }
        if ($hasZ) { $lastZ = $z }
        [void]$result.Add($line)
        continue
    }

    $x = if ($hasX) { [double]::Parse($xMatch.Groups[1].Value, [System.Globalization.CultureInfo]::InvariantCulture) } else { $lastX }
    $y = if ($hasY) { [double]::Parse($yMatch.Groups[1].Value, [System.Globalization.CultureInfo]::InvariantCulture) } else { $lastY }
    $z = if ($hasZ) { [double]::Parse($zMatch.Groups[1].Value, [System.Globalization.CultureInfo]::InvariantCulture) } else { $lastZ }

    if ($null -ne $lastZ -and $null -ne $z) {
        $diffZ = [math]::Abs($z - $lastZ)
    } else {
        $diffZ = [double]::MaxValue
    }

    if ($diffZ -lt 0.0001) {
        if ($null -ne $lastX -and $null -ne $lastY -and $null -ne $x -and $null -ne $y) {
            $diffX = [math]::Abs($x - $lastX)
            $diffY = [math]::Abs($y - $lastY)

            if ($diffX -le $Threshold -and $diffY -le $Threshold) {
                [void]$removedLines.Add("Line $lineNum removed: X=${x} Y=${y} Z=${z} (dX=$diffX, dY=$diffY, dZ=$diffZ)")
                $removedCount++
                continue
            }
        }
    }

    if ($hasX) { $lastX = $x }
    if ($hasY) { $lastY = $y }
    if ($hasZ) { $lastZ = $z }
    [void]$result.Add($line)
}

$result | Set-Content $OutputFile

Write-Host ""
Write-Host "GCode Small-Move Remover"
Write-Host "========================"
Write-Host "Threshold: <= $Threshold in both X and Y"
Write-Host "Input:      $InputFile ($($lines.Count) lines)"
Write-Host "Output:     $OutputFile ($($result.Count) lines)"
Write-Host "Removed:    $removedCount lines"
Write-Host ""

if ($removedLines.Count -gt 0) {
    Write-Host "Sample removed lines (first 20):"
    $sample = if ($removedLines.Count -gt 20) { $removedLines[0..19] } else { $removedLines }
    foreach ($r in $sample) {
        Write-Host "  $r"
    }
    if ($removedLines.Count -gt 20) {
        Write-Host "  ... and $($removedLines.Count - 20) more"
    }
}