#!/usr/bin/env python

def g1_from_point(point):
    return "G1 X{} Y{} Z{}\n".format(round(point[0], 5), round(point[1], 5), round(point[2], 5))

def g1_from_speed(speed):
    return "G1 F{}\n".format(speed)

if __name__ == '__main__':

    guess = 65 # Your best guess of the focus height
    guess_padding = 20 # The test will cover your guessed height +/- this value
    line_count = 2 # Number of test lines to burn. Minimum is 2.
    line_spacing = 0.6 # Spacing between lines

    first_line_x = 2.0 + ((line_count + 1) * line_spacing + 1) * 0
    prime_x = 0
    prime_line_count = 0
    do_home = True

    starting_z = guess - guess_padding
    line_depth = guess_padding

    line_start_y = -23 # Y start position of lines
    line_end_y = 20 # Y end position of lines
    travel_speed = 10000
    cut_speed = 140
    travel_power = 0 # Laser off
    cut_power = 255 # Laser full power
    power_off_delay = 5
    power_on_delay = 0
    zig_zag = True

    reverse_z = False
    if reverse_z:
        starting_z = starting_z + line_count * line_depth
        line_depth = -line_depth

    gcode = ""
    gcode += "M107\n"
    gcode += "G90\n"
    if do_home:
        gcode += "G28\n"
    gcode += "G21\n"
    gcode += "\n"
    
    for k in range(0, prime_line_count):
        gcode += "; Prime {}\n".format(k)
        gcode += g1_from_speed(travel_speed)
        gcode += g1_from_point((prime_x, line_start_y, starting_z))
        gcode += g1_from_speed(cut_speed)
        gcode += "M106 S{}\n".format(cut_power)
        if power_on_delay > 0:
            gcode += "G4 P{}\n".format(power_on_delay)
        gcode += g1_from_point((prime_x, line_end_y, starting_z + line_depth / 2.))
        gcode += "M106 S{}\n".format(travel_power)
        gcode += "G4 P{}\n".format(power_off_delay)
        gcode += "\n"

    position = (first_line_x, line_start_y, starting_z)
    for k in range(0, line_count):
        ending_z = starting_z + (k + 1) * line_depth
        if zig_zag and k % 2:
            ending_z = starting_z + k * line_depth

        gcode += "; Line {} Z={} to Z={}\n".format(k, position[2], ending_z)
        gcode += g1_from_speed(travel_speed)
        gcode += g1_from_point(position)

        # calculate next position and set cut speed
        position = (position[0], line_end_y, ending_z)
        gcode += g1_from_speed(cut_speed)

        # laser on and delay
        gcode += "M106 S{}\n".format(cut_power)
        if power_on_delay > 0:
            gcode += "G4 P{}\n".format(power_on_delay)
    
        # cut line
        gcode += g1_from_point(position)
       
        # laser off and delay
        gcode += "M106 S{}\n".format(travel_power)
        gcode += "G4 P{}\n".format(power_off_delay)
       
        # calculate next line start position and move at travel speed
        next_starting_z = position[2]
        if zig_zag and not k % 2:
            next_starting_z = starting_z + (k + 2) * line_depth

        position = (position[0] + line_spacing, line_start_y, next_starting_z)

        gcode += "\n"
        
    gcode += g1_from_speed(travel_speed)
    gcode += "G1 X140 Y91\n"

    gcode += "M106 S0\n"
    gcode += "M107\n"
    gcode += "M18\n"

    print gcode