#----------------------------------------------------------------------
#  OpenGFDM - parse GCODE files
#  Author: Andrea Pavan
#  Date: 10/10/2022
#  License: GPLv3-or-later
#----------------------------------------------------------------------


"""
PARSEGCODE reads a gcode file and extracts the tool path
The Marlin firmware is taken as reference: https://marlinfw.org/meta/gcode/
INPUT:
    gcode: vector of strings (one string per line) with the gcode instructions
OPTIONAL INPUTS:
    importoptions: Dict with the following entries:
        "scaleFactor": Float64 that scales the X,Y,Z axis [default: 1e-3]
        "feedrateScaleFactor": Float64 that scales the feedrate (tool speed) [default: 1e-3/60]
        "extrusionScaleFactor": Float64 that scales the E axis [default: 1e-3]
        "temperatureConversionFactors": Float64 array with two factors a,b to convert the T temperatures using the law a+b*T [default: a=273.15,b=1.0]
        "useRelativeCoordinates": Bool true if the gcode is using relative coordinates on the X,Y,Z axis [default: false]
        "useRelativeExtrusion": Bool true if the gcode is using relative positions on the E axis [default: false]
OUTPUT:
    M: Nx6 Float64 matrix with [X,Y,Z,F,E0,T0] as columns (absolute values, machine reference system)
Limitations:
    supported commands: G0,G1,G28,G90,G91,G92,M82,M83,M104,M109
    all other instructions are discarded
"""
function parseGcode(gcode::Vector{String}, importoptions::Dict)
    #parse import options
    scaleFactor = 1e-3;
    feedrateScaleFactor = 1e-3/60;
    extrusionScaleFactor = 1e-3;
    temperatureConversionFactors = [273.15,1.0];
    relativeCoordinates = false;        #whether gcode X,Y,Z coordinates are relative (G91 command)
    relativeExtrusion = false;      #whether gcode extrusion E positions are relative (M83 command)
    if haskey(importoptions,"scaleFactor")
        scaleFactor = importoptions["scaleFactor"];
    end
    if haskey(importoptions,"feedrateScaleFactor")
        feedrateScaleFactor = importoptions["feedrateScaleFactor"];
    end
    if haskey(importoptions,"extrusionScaleFactor")
        extrusionScaleFactor = importoptions["extrusionScaleFactor"];
    end
    if haskey(importoptions,"temperatureConversionFactors")
        temperatureConversionFactors = importoptions["temperatureConversionFactors"];
    end
    if haskey(importoptions,"useRelativeCoordinates")
        relativeCoordinates = importoptions["useRelativeCoordinates"];
    end
    if haskey(importoptions,"useRelativeExtrusion")
        relativeExtrusion = importoptions["useRelativeExtrusion"];
    end
    
    #parsing
    N = length(gcode);
    M = zeros(Float64,N+1,6);       #Nx6 matrix with [X,Y,Z,F,E0,T0] as columns
    xReference = 0;     #coordinate system zero for the X axis
    yReference = 0;     #coordinate system zero for the Y axis
    zReference = 0;     #coordinate system zero for the Z axis
    eReference = 0;     #coordinate system zero for the E axis
    i = 2;      #index to build the output matrix (i=2:end)
    for line in gcode
        #G0,G1: Linear Move
        if startswith(line,"G0 ") || startswith(line,"G1 ")
            line_params = split(split(line,';')[1], ' ', keepempty=false);
            for line_param in line_params
                if line_param[1] == 'X'
                    M[i,1] = parse(Float64,line_param[2:end]) + xReference;
                elseif line_param[1] == 'Y'
                    M[i,2] = parse(Float64,line_param[2:end]) + yReference;
                elseif line_param[1] == 'Z'
                    M[i,3] = parse(Float64,line_param[2:end]) + zReference;
                elseif line_param[1] == 'F'
                    M[i,4] = parse(Float64,line_param[2:end]);
                elseif line_param[1] == 'E'
                    M[i,5] = parse(Float64,line_param[2:end]) + eReference;
                end
            end
            if relativeCoordinates
                M[i,1:3] += M[i-1,1:3];
            end
            if relativeExtrusion
                M[i,5] += M[i-1,5];
            end
            M[i+1,:] = M[i,:];
            i += 1;
        
        #G28: Auto Home
        elseif startswith(line,"G28")
            lineCmd = split(line,';')[1];
            if !contains(lineCmd,'X') && !contains(lineCmd,'Y') && !contains(lineCmd,'Z')
                M[i,1:3] = [0.0, 0.0, 0.0];
            else
                if contains(lineCmd,'X')
                    M[i,1] = 0.0;
                end
                if contains(lineCmd,'Y')
                    M[i,2] = 0.0;
                end
                if contains(lineCmd,'Z')
                    M[i,3] = 0.0;
                end
            end
            M[i+1,:] = M[i,:];
            i += 1;

        #G90: Absolute Positioning
        elseif startswith(line,"G90")
            relativeCoordinates = false;

        #G91: Relative Positioning
        elseif startswith(line,"G91")
            relativeCoordinates = true;

        #G92: Set Position
        elseif startswith(line,"G92")
            line_params = split(split(line,';')[1], ' ', keepempty=false);
            for line_param in line_params
                if line_param[1] == 'X'
                    xReference = M[i,1] - parse(Float64,line_param[2:end]);
                elseif line_param[1] == 'Y'
                    yReference = M[i,2] - parse(Float64,line_param[2:end]);
                elseif line_param[1] == 'Z'
                    zReference = M[i,3] - parse(Float64,line_param[2:end]);
                elseif line_param[1] == 'E'
                    eReference = M[i,5] - parse(Float64,line_param[2:end]);
                end
            end
            #G92.1: Reset workspace to native machine space
            if startswith(line,"G92.1")
                xReference = 0;
                yReference = 0;
                zReference = 0;
                eReference = 0;
            end

        #M82: E Absolute
        elseif startswith(line,"M82")
            relativeExtrusion = false;

        #M83: E Relative
        elseif startswith(line,"M83")
            relativeExtrusion = true;

        #M104: Set Hotend Temperature
        elseif startswith(line,"M104") || startswith(line,"M109")
            line_params = split(split(line,';')[1], ' ', keepempty=false);
            for line_param in line_params
                if line_param[1] == 'S'
                    M[i,6] = parse(Float64,line_param[2:end]);
                end
            end
        end
        
    end

    M[:,1:3] *= scaleFactor;
    M[:,4] *= feedrateScaleFactor;
    M[:,5] *= extrusionScaleFactor;
    M[:,6] = temperatureConversionFactors[1].+temperatureConversionFactors[2].*M[:,6];
    return M[2:i-1,:];
end


"""
PARSEGCODEFROMFILE reads a file and calls the function parseGcode
INPUT:
    filename: String with the gcode file path
OPTIONAL INPUTS as parseGcode
OUTPUT as parseGcode
"""
function parseGcodeFromFile(filename::String, importoptions::Dict=Dict())
    gcode = readlines(filename);
    return parseGcode(gcode, importoptions);
end


"""
CALCULATEGCODEFLOW returns the length of each segment, the time spent and the extrusion flow
INPUT:
    M: Nx6 Float64 matrix with [X,Y,Z,F,E0,T0] as columns (from the function parseGcode)
OUTPUT:
    l: Float64 vector of size N with the length of each segment
    t: Float64 vector of size N with the elapsed time from the start
    v: Float64 vector of size N with the extrusion speed at the end of each segment
    layers: Float64 vector of size N with the layer count
    h: Float64 vector of size N with the layer height on each segment
Limitations:
    the time is calculated from the feedrate F, without accounting for the acceleration/jerk settings
    the flow might lose in accuracy accordingly
    the layers are supposed to be flat for the tracking of the layer number and height
"""
function calculateGcodeFlow(M::Matrix{Float64})
    #length, time, speed info
    N = length(M[:,1]);
    l = zeros(N);       #segments length
    t = zeros(N);       #simulation time
    v = zeros(N);       #extrusion speed
    for i=2:N
        l[i] = sqrt((M[i,1]-M[i-1,1])^2 + (M[i,2]-M[i-1,2])^2 + (M[i,3]-M[i-1,3])^2);
        t[i] = t[i-1] + l[i]/M[i,4];
        v[i] = (M[i,5]-M[i-1,5])/(t[i]-t[i-1]);
    end
    replace!(v, NaN=>0);
    replace!(v, Inf=>0);
    replace!(v, -Inf=>0);

    #counting layers
    layers = zeros(Int,N);      #layer counter
    minZvalue = M[N,3];
    for i=N-1:-1:1
        layers[i] = layers[i+1];
        if M[i,3] < minZvalue && M[i,5] != 0
            layers[i] += 1;
            minZvalue = M[i,3];
        end
    end
    layers *= -1;
    layers .+= abs(layers[1])

    #layers height
    h = zeros(N);       #layer height
    startingLayerRow = 1;       #number of the row of the start of the layer
    layerRows = [];     #list of the current layer extruding rows
    zlayer = 0.0;       #layer z position
    for i=2:N
        if M[i,5] != 0
            push!(layerRows,i);
        end
        if layers[i]>layers[i-1] || i==N
            #the layer instructions are between the rows [startingLayerRow, i-1]
            #the extruding instructions are stored in the layerRows array
            zprevlayer = zlayer;        #previous layer z position
            zlayer = minimum(M[layerRows,3]);
            h[startingLayerRow:i-1] .= zlayer-zprevlayer;
            startingLayerRow = i;
            layerRows = [];
        end
    end

    return l, t, v, layers, h;
end
