# Connect to a robot and monitor the position from the real robot to RoboDK environment

# More information about the RoboDK API here:
# https://robodk.com/doc/en/RoboDK-API.html

from robolink import *    # API to communicate with RoboDK
from robodk import *      # basic matrix operations
import fileinput

import serial # import serial library for arduino
import time # import time library for waiting

# Start RoboDK API
RDK = Robolink() #connect to robodk and save to vaiable
robot = RDK.Item('ABB IRB 2400/16', ITEM_TYPE_ROBOT) # create a variable of the robot
if not robot.Valid():
    raise Exception('No robot selected or available')



program = RDK.Item('LevelingProgram') # create a variable of the leveling program


if robot.ConnectSafe() != ROBOTCOM_READY:
    raise Exception("Robot not connected")


try:
    ser = serial.Serial('COM3', baudrate = 9600) # define arduino connection via serial
    
except serial.serialutil.SerialException:
    print("No Connection to Arduino possible, quit ...")
    program.Stop()
    RDK.ShowMessage("Arduino not connected to PC, leveling restricted!")
    quit()

time.sleep(3) # wait for the connection to be established

# variable config
tcp_to_trigger = 5.54 - 0.93 # offset in z to the trigger point
ProbeValue = "90000" # value to start searching in file
cmd_down = False # comd down bool to set bltouch down again
arithmetic_sum = 0 # resulting sensor sum
arithmetic_z = 0 # resulting arithmetic value
arithmetic_z_string = 0 # resulting arithemtic value as a string
arithmetic_counter = 0 # arithmetic counter to perform arithmetic calculation
arduino_counter = 0 # arduino counter
output_counter = 0 # output counter
ProbeCounter = 1 # probe counter
previousInstruction = 0 # instruction counter to check if bltouch didnt activate

# BLTouch Connectio Start ---------------------------------

ser.write('down'.encode("utf-8")) # initial down command



# find amount of probe points
ProbePoints = 0
fp = open("C:/BedLevelingData/meshData.txt")
for i, line in enumerate(fp):
    if i == 3:
        ProbePoints = int(line)
        print(ProbePoints)
        break
fp.close()

previousInstruction = 9 # set previous instruction to first probe instruction (fixed value)
# loop through all probe points
while ProbeCounter <= ProbePoints:
    joints = robot.Joints()
    #RoboDK get the TCP Values
    target_ref = robot.Pose()
    pos_ref = target_ref.Pos()
    x,y,z,a,b,c = Pose_2_TxyzRxyz(target_ref)

    #Arduino part
    if (ser.inWaiting() > 0): #only execute the file changer when the sensor triggers
        data = ser.readline().decode('ascii') # output info
        print(data) # output info
        cmd_down = True # set cmd down to true in order to put bltouch back up
        arithmetic_counter += 1
        arithmetic_sum += (z - tcp_to_trigger) # subtract the difference from tcp to trigger point

    #    program.setParam('Start', program.InstructionSelect()+1) # jump to next instruction if sensor activates
    #    previousInstruction =  program.InstructionSelect() + 3 # increase previous instruction by the next 3 (+1) instructions
    #elif (previousInstruction < program.InstructionSelect()): #if no bltouch was actvie increae counter and set z to 0
    #    arithmetic_counter += 1
    #    arithmetic_sum += 0.0001
    #    previousInstruction = program.InstructionSelect() + 2 # jump to next probing
    #    print("no probe")

    
    # check if already third probe
    if (arithmetic_counter == 3):
        arithmetic_z = arithmetic_sum/arithmetic_counter # calc airhtmetic z
        print(arithmetic_z)
        # check the amount of probes already done and change search value accordingly
        if (ProbeCounter < 10):
            ProbeValue = "90000" # base string for z values
        if (ProbeCounter >= 10 and ProbeCounter < 100):
            ProbeValue = "9000"
        if (ProbeCounter >= 100 and ProbeCounter < 1000):
            ProbeValue = "900"
        print("ProbeCounter:", ProbeCounter)
        arithmetic_z_string = str(arithmetic_z)[:len(str(arithmetic_z))-9] # convert arithmetic z to string and shorten it
        with fileinput.FileInput("C:/BedLevelingData/meshValues.txt", inplace=True, backup='.bak') as file: # loop through the mesh file and set probe value
            for line in file:
                print(line.replace(ProbeValue + str(ProbeCounter), arithmetic_z_string), end='')
        with fileinput.FileInput("C:/BedLevelingData/meshVisu.txt", inplace=True, backup='.bak') as file: # loop through visu file and set probe value
            for line in file:
                print(line.replace(ProbeValue + str(ProbeCounter), arithmetic_z_string), end='')
        # increase or reset counters
        RDK.ShowMessage("Probe_Z: " + arithmetic_z_string, False) 
        ProbeCounter += 1 
        arithmetic_z = 0
        arithmetic_counter = 0
        arithmetic_sum = 0
        #previousInstruction = program.InstructionSelect() + 2 # jum to next probing


    # set bltouch back down when the z value is high and cmd_down is active
    if (cmd_down == True and z > 10):
        ser.write('down'.encode("utf-8"))
        arduino_counter = 0
        cmd_down = False

    
    # debugging output
    if (output_counter == 1000):
        #print("X: " + str(x))
        #print("Y: " + str(y))
        #RDK.ShowMessage("Z: " + str(z), False)
        #RDK.ShowMessage("-------", False)
        output_counter = 0

    output_counter += 1

    # always check if there has been a new instruction
    #if (previousInstruction < program.InstructionSelect()):
    #    previousInstruction = program.InstructionSelect()

    RDK.ShowMessage("Current_Z: " + str(z) + "Current Arithmetic" + str(arithmetic_counter), False)

    time.sleep(0.01) # reduce the loop time