# -*- coding: utf-8 -*-
"""
@author: Patrick Scholle, Technische Universität Braunschweig, Institute of Mechanics and Adaptronics

G code generator for the 4-probe scanner
"""

import numpy as np

path = # your path here
outfile = open(path + '\\test.gcode', 'w')

n_scans_per_position = 1
x_resolution = 80  # Width resolution
y_resolution = 160  # Length resolution
wait_time_scanning = 5000  # time for each scan
z_top_position = 10  # top position for travel moves
z_bottom_position = 2  # bottom position for scanneing

scan_width = 48  # mm
scan_length = 98 # mm  
X0, Y0 = 104, 168  # zero posiion of scanning area is (104,168)
print('Estimated scan Time in minutes: ' + str(x_resolution*y_resolution*(wait_time_scanning+2000)*n_scans_per_position/1000/60))
print(f'length-resolution: {scan_length/y_resolution} mm')
print(f'width-resolution: {scan_width/x_resolution} mm')

# write the parameters as comments into the g-code
gcode = '; Generated with g_code_generatory.py, P.Scholle\n'
gcode += f';n_scans_per_position:{n_scans_per_position}\n'
gcode += f';x_resolution:{x_resolution}\n'
gcode += f';y_resolution:{y_resolution}\n'
gcode += f';wait_time_scanning:{wait_time_scanning}\n'
gcode += f';z_top_position:{z_top_position}\n'
gcode += f';z_bottom_position:{z_bottom_position}\n'
gcode += f';scan_width:{scan_width}\n'
gcode += f';scan_length:{scan_length}\n'
gcode += f';X0:{X0}\n'
gcode += f';Y0:{Y0}\n'

# initial things
gcode += f'G90 \nG0 Z{z_top_position}\nG28 W \n'  # absolute coordinates and homing
gcode += 'G0 F1000\n'  # speed
gcode += f'G0 Z{z_top_position}\n'
gcode += f'G0 X{X0}Y{Y0}\n'

#create a meshgrid with positions
xs = np.linspace(X0, X0+scan_width, x_resolution)
ys = np.linspace(Y0, Y0-scan_length, y_resolution)

for y_current in ys:
    for x_current in xs:
        gcode += f'G0 X{x_current:.3f}Y{y_current:.3f}\n'        
        for i in range(n_scans_per_position): # move up and down
            gcode += f'G0 Z{z_bottom_position}\n'
            gcode += f'G4 P{wait_time_scanning}\n'
            gcode += f'G0 Z{z_top_position}\n'

# It is better if the printer is defined on the part in case I am not
# in time to stop the measurement
gcode += f'G0 Z{z_bottom_position}\n'  


#%% Write to the file
outfile.write(gcode)
outfile.close()