M98 P"0:/sys/led/resetstatus.g"
M98 P"0:/macros/System/Calibration/Testing/Disable Probe"


M291 R"Servo Motor Test" P"Servo motor will rotate to 0 degree and then to a set degree" S4 K{"Test","Skip","Cancel"}
if input == 0
    while iterations < 30
        M98 P"0:/macros/System/Calibration/Z Probe/Rotate holder to a set degree"
        G4 S2
        M98 P"0:/macros/System/Calibration/Z Probe/Rotate holder to 0 degree"
        G4 S1

    M291 R"Servo Motor Test" P"Was the servo motor able to rotate smoothly?" S4 K{"Yes","No"}
    if input == 1
        abort "Error: Servo Failed"
elif input == 1
    ; Skipping
elif input == 2
    abort "Test cancelled by user"

M564 H0
M291 R"Mode build plate close to the nozzle" P"Move the build plate close to the nozzle" Z1 S3
M564 H1

if sensors.probes[0].value[0] != 1000
    M98 P"0:/sys/led/fault.g"
    abort "Error: S0 Dissconnect Z Probe or move the build plate away from the nozzle"
else
    M98 P"0:/sys/led/start_cold.g"



; Enable probe
M98 P"0:/macros/System/Calibration/Testing/Enable Probe"
G4 S1

var max_iter = 1200 ; 120 seconds
var iter = 0



; 1 - T0 -> Build Plate ============================================================================
; check if z-probe value is 1000
if sensors.probes[0].value[0] == 1000
    M291 R"Touch the nozzle" P"Touch Left Nozzle to the Bed, Z - Probe value should change to 0" S1 T60
else
    M98 P"0:/sys/led/fault.g"
    abort "Error: S1 Z-Probe value is not 1000"

; wait for z-probe value to change to 0
while sensors.probes[0].value[0] != 0
    G4 P100
    set var.iter = var.iter + 1

if var.iter <= var.max_iter
    M292
    M98 P"0:/sys/led/end.g"
else
    M98 P"0:/sys/led/fault.g"
    abort "Error: S1 Z-Probe value did not change to 0"



M98 P"0:/sys/led/start_cold.g"
G4 S1



; 2 - T1 -> Build Plate ============================================================================
; check if z-probe value is 1000
if sensors.probes[0].value[0] == 1000
    M291 R"Touch the nozzle" P"Touch Right Nozzle to the Bed, Z - Probe value should change to 0" S1 T60
else
    M98 P"0:/sys/led/fault.g"
    abort "Error: S2 Z-Probe value is not 1000"

; wait for z-probe value to change to 0
while sensors.probes[0].value[0] != 0
    G4 P100
    set var.iter = var.iter + 1

if var.iter <= var.max_iter
    M292
    M98 P"0:/sys/led/end.g"
else
    M98 P"0:/sys/led/fault.g"
    abort "Error: S2 Z-Probe value did not change to 0"



M98 P"0:/sys/led/start_cold.g"
G4 S1



; 3 - Attach the Z-Probe to the holder ============================================================================
; check if z-probe value is 1000
if sensors.probes[0].value[0] == 1000
    M291 R"Attach the Z-Probe to the holder" P"Attach the Z-Probe to the holder, Z - Probe value should change to 0" S1 T60
else
    M98 P"0:/sys/led/fault.g"
    abort "Error: S3 Z-Probe value is not 1000"

; wait for z-probe value to change to 0
while sensors.probes[0].value[0] != 0
    G4 P100
    set var.iter = var.iter + 1

if var.iter <= var.max_iter
    M292
    M98 P"0:/sys/led/end.g"
else
    M98 P"0:/sys/led/fault.g"
    abort "Error: S3 Probe was not recognized"



M98 P"0:/sys/led/start_cold.g"
G4 S1



; 4 - Click the Z-Probe ============================================================================
; check if z-probe value is 0
if sensors.probes[0].value[0] == 0
    M291 R"Click the Z-Probe" P"Click the Z-Probe, Z - Probe value should change to 1000" S1 T60
else
    M98 P"0:/sys/led/fault.g"
    abort "Error: S4 Probe was not attached"

; wait for z-probe value to change to 1000
while sensors.probes[0].value[0] != 1000
    G4 P100
    set var.iter = var.iter + 1

if var.iter <= var.max_iter
    M292
    M98 P"0:/sys/led/end.g"
else
    M98 P"0:/sys/led/fault.g"
    abort "Error: S4 Probe might be damaged"


M98 P"0:/macros/System/Calibration/Testing/Disable Probe" ; Disable probe

M98 P"0:/sys/led/resetstatus.g"