Compensate.lua

-- Global variables
dbg = false
previouslyWentLeft = false
backlash = 0.05

-- Example usage
local inputFile = "input.tap"











function msg(data)
	if not dbg then return end
	local text = "(" .. debug.getinfo(2, "n").name .. ": " .. data[1] .. ")"
	for id = 2, #data do
		text = string.gsub(text, "%%", tostring(data[id]), 1)
	end
	print(text)
end

-- Helper function to format G-code commands
function formatGCode(command, params)
    local parts = {command}
    local keys = {"X", "Y", "Z", "R", "I", "J", "K", "F"}
    for _, key in ipairs(keys) do
        if params[key] then
            table.insert(parts, string.format("%s%.3f", key, params[key]))
        end
    end
    return table.concat(parts, " ")
end

-- Function to find the center of the arc
function findArcCenter(params, nominalXPos, nominalYPos, clockwise)
    local cx, cy

    -- Calculate center based on I and J parameters
    if params.I and params.J then
        cx = nominalXPos + params.I
        cy = nominalYPos + params.J
    elseif params.I then
        cx = nominalXPos + params.I
        cy = nominalYPos -- Assume J is the same as the current Y position
    elseif params.J then
        cx = nominalXPos
        cy = nominalYPos + params.J -- Assume I is the same as the current X position
    elseif params.R then
        -- Calculate center based on R parameter
        local radius = params.R
        local deltaX, deltaY = params.X - nominalXPos, params.Y - nominalYPos
        local distance = math.sqrt(deltaX * deltaX + deltaY * deltaY)

        if distance > 2 * math.abs(radius) then
            error("The distance between the points is greater than the diameter of the circle defined by R")
        end

        local halfDistance = distance / 2
        local halfChordLength = math.sqrt(radius * radius - halfDistance * halfDistance)
        local midX, midY = (nominalXPos + params.X) / 2, (nominalYPos + params.Y) / 2

        local offsetX = halfChordLength * (deltaY / distance)
        local offsetY = halfChordLength * (-deltaX / distance)

        if clockwise then
            cx = midX + offsetX
            cy = midY + offsetY
        else
            cx = midX - offsetX
            cy = midY - offsetY
        end
    else
        error("Arc must have either I, J, or R parameters")
    end

    return cx, cy
end

function normalizeAngles(startAngle, endAngle, clockwise)
	msg{"Received startAngle %, endAngle % with direction %", startAngle, endAngle, clockwise}

	-- Normalize angles to be within [0, 2*pi)
	local s = (startAngle + 2 * math.pi) % (2 * math.pi)
	local e = (endAngle   + 2 * math.pi) % (2 * math.pi)

	-- Add 2pi to start or end to ensure descending order for clockwise, ascending order for counterclockwise
	if clockwise and e >= s then
		s = s + 2 * math.pi
	elseif not clockwise and e <= s then
		e = e + 2 * math.pi
	end

	msg{"Normalized Start angle: % radians / % degrees", s, math.deg(s)}
	msg{"Normalized End angle:   % radians / % degrees", e, math.deg(e)}

	return s, e
end

function checkCrossings(startAngle, endAngle, clockwise)
    msg{"Checking for crossings for startAngle % to endAngle % with clockwise: %", startAngle, endAngle, clockwise}
    local crosses3Oclock = false
    local crosses9Oclock = false

    if clockwise then
        -- Clockwise (G2) logic
        if startAngle > 2 * math.pi and endAngle < 2 * math.pi then
            crosses3Oclock = true
        end
        if startAngle > math.pi * 3 then
            crosses9Oclock = true
        elseif startAngle > math.pi and endAngle < math.pi then
            crosses9Oclock = true
        end
    else
        -- Counterclockwise (G3) logic
        if startAngle < 2 * math.pi and endAngle > 2 * math.pi then
            crosses3Oclock = true
        end
        if (startAngle < math.pi and endAngle > math.pi) or
           (startAngle > math.pi and endAngle > math.pi * 3) then
            crosses9Oclock = true
        end
    end

    msg{"crosses3Oclock: %   crosses9Oclock: %", crosses3Oclock, crosses9Oclock}
    return crosses3Oclock, crosses9Oclock
end


-- Function to split an arc if it crosses 3 o'clock or 9 o'clock
function splitArc(command, params, nominalPos)
    msg{"Received arc for splitting:   X: %   Y: %   Z: %   I: %   J: %   R: %   F: %   Arc type: %", params.X, params.Y, params.Z, params.I, params.J, params.R, params.F, command}
    msg{"Nominal Position: X%, Y%, Z%", nominalPos.X, nominalPos.Y, nominalPos.Z}
    local result = {}
    local clockwise = command == "G2"
    local cx, cy = findArcCenter(params, nominalPos.X, nominalPos.Y, clockwise)
    msg{"Arc center: X=%, Y=%", cx, cy}

    local startAngle = math.atan2(nominalPos.Y - cy, nominalPos.X - cx)
    local endAngle = math.atan2(params.Y - cy, params.X - cx)

    -- Normalize angles
    startAngle, endAngle = normalizeAngles(startAngle, endAngle, clockwise)

    -- Check for crossings
    local crosses3Oclock, crosses9Oclock = checkCrossings(startAngle, endAngle, clockwise)

    -- Compute the radius if not provided
    local r = params.R
    if not r then
        r = math.sqrt((nominalPos.X - cx) ^ 2 + (nominalPos.Y - cy) ^ 2)
    end
    msg{"Arc Radius: %", r}

    -- Determine if Z changes
    local zChanges = params.Z and params.Z ~= nominalPos.Z

    -- Collect the crossing points
    local crossingPoints = {}

    if crosses3Oclock then
        table.insert(crossingPoints, {angle = math.pi*2, x = cx + r, y = cy, label = "3 o'clock"})
    end
    if crosses9Oclock then
        table.insert(crossingPoints, {angle = math.pi, x = cx - r, y = cy, label = "9 o'clock"})
    end

		-- Determine if start angle is in the top half of the circle
	local startAngle_isTop = (startAngle % (2 * math.pi)) <= math.pi

	msg{"Clockwise is %.  Start position in top half of the circle: %   startAngle: %", clockwise, startAngle_isTop, startAngle % math.pi*2}

	-- Sort the crossing points based on their angles using the booleans
	table.sort(crossingPoints, function(a, b)
		if clockwise then
			-- Clockwise logic
			if startAngle_isTop then
				return a.angle > b.angle  -- 3 o'clock before 9 o'clock
			else
				return a.angle < b.angle  -- 9 o'clock before 3 o'clock
			end
		else
			-- Counterclockwise logic
			if startAngle_isTop then
				return a.angle < b.angle  -- 9 o'clock before 3 o'clock
			else
				return a.angle > b.angle  -- 3 o'clock before 9 o'clock
			end
		end
	end)

	for id, crossing in ipairs(crossingPoints) do
		msg{"Crossing %: %", id, crossing.angle}
	end

    -- Split the arc at each crossing point
    local currentStartAngle = startAngle
    local currentStartX, currentStartY = nominalPos.X, nominalPos.Y
    local currentStartZ = nominalPos.Z

    for _, crossing in ipairs(crossingPoints) do
        local splitAngle = crossing.angle
        local splitX = crossing.x
        local splitY = crossing.y

        -- Calculate the portion of the Z movement up to this split point
        local totalAngleSpan = clockwise and (startAngle - endAngle) or (endAngle - startAngle)
        local angleToSplit = clockwise and (currentStartAngle - splitAngle) % (2 * math.pi) or (splitAngle - currentStartAngle) % (2 * math.pi)
        local angleFraction = math.abs(angleToSplit / totalAngleSpan)
        local splitZ = zChanges and (currentStartZ + (params.Z - nominalPos.Z) * angleFraction) or currentStartZ

        -- Add this arc segment to the result table
        if not (splitX == currentStartX and splitY == currentStartY) then
            local arcParams = {X = splitX, Y = splitY, R = r}
            if zChanges then arcParams.Z = splitZ end
            table.insert(result, {command = command, params = arcParams})
        end

        -- Update the start position for the next segment
        currentStartAngle = splitAngle
        currentStartX = splitX
        currentStartY = splitY
        currentStartZ = splitZ
    end

    -- Add the final arc segment
    if not (currentStartX == params.X and currentStartY == params.Y) then
        local arcParams = {X = params.X, Y = params.Y, R = r}
        if zChanges then arcParams.Z = params.Z end
        table.insert(result, {command = command, params = arcParams})
    end

    msg{"Returning % results", #result}
    return result
end


-- Function to parse a line of G-code
function parseGCode(line)
    if line:match("^%s*$") then return "", "", {} end

    -- Capture leading spaces and the rest of the line
    local leadingSpaces, rest = line:match("^(%s*)(%S+)%s*(.*)")

    -- Calculate number of leading spaces
    local numLeadingSpaces = #leadingSpaces

    -- Skip leading spaces and get the remaining part of the line
    local remainingLine = line:sub(numLeadingSpaces + 1)

    -- Extract command (the first non-space segment)
    local command = remainingLine:match("^(%S+)")

    -- Extract the parameters string (everything after the command)
    local paramsStr = remainingLine:sub(#command + 2)  -- Adjust to remove command and following space

	msg{"paramsStr before double negative replacement: %", paramsStr}
    -- Replace double negatives with positives
    paramsStr = string.gsub(paramsStr, "%-%-", "")
    msg{"paramsStr after double negative replacement: %", paramsStr}


    -- Initialize table for parameters
    local parsedParams = {}

    -- Parse parameters
    for letter, value in paramsStr:gmatch("([XYZRIJKF])%s*([%d%.%+%-]+)") do
        parsedParams[letter] = tonumber(value)
    end

    return leadingSpaces or "", command, parsedParams
end

-- Function to process each move
function processMove(leadingSpaces, command, params, pos, nominalPos)
    local newX = params.X
    local goingLeft

    if newX then
        -- Determine if moving left based on the new position
        goingLeft = newX < nominalPos.X
    else
        -- If no new X is provided, retain the previous direction
        goingLeft = previouslyWentLeft
    end

    msg{"Move: goingLeft: %   newX: %", goingLeft, newX}

    if newX then
        if goingLeft ~= previouslyWentLeft then
            local compensationMove = {X = nominalPos.X + (goingLeft and -backlash or 0)}
            msg{"Adding compensation move"}
            print(leadingSpaces .. formatGCode("G0", compensationMove))
            previouslyWentLeft = goingLeft
        end
        if goingLeft then
            params.X = newX - backlash
        end
        nominalPos.X = newX
        pos.X = params.X
    end
    if params.Y then
        nominalPos.Y = params.Y
        pos.Y = params.Y
    end
    if params.Z then
        nominalPos.Z = params.Z
        pos.Z = params.Z
    end

    -- Print the move
    print(leadingSpaces .. formatGCode(command, params))

    return pos, nominalPos
end

-- Main function to process G-code file
function compensateBacklash(inputFile)
    local file = io.open(inputFile, "r")
    if not file then
        error("Could not open file: " .. inputFile)
    end

    local pos = {X = 0, Y = 0, Z = 0}
    local nominalPos = {X = 0, Y = 0, Z = 0}

    print(string.format("(Warning, Compensated Toolpath!! X Axis Backlash: %s)", backlash))

    for line in file:lines() do
        msg{"Processing line: %", line}
        msg{"Current position: %, %, %    previouslyWentLeft: %", pos.X, pos.Y, pos.Z, previouslyWentLeft}

        local leadingSpaces, command, params = parseGCode(line)

        if command == "G2" or command == "G3" then
            local moves = splitArc(command, params, nominalPos)
            for _, move in ipairs(moves) do
                pos, nominalPos = processMove(leadingSpaces, move.command, move.params, pos, nominalPos)
            end
        elseif command == "G0" or command == "G1" then
            pos, nominalPos = processMove(leadingSpaces, command, params, pos, nominalPos)
        else
            print(line)
        end
    end

    file:close()
end

if not testing then compensateBacklash(inputFile) end