#!/usr/bin/env python3
import hal, time, serial

# --- Helper Functions ---
def encode_nibbles(val):
    """Splits a byte into two nibbles and applies the pendant offset"""
    low = (val & 15) + 224
    high = (val >> 4) + 224
    return low, high

def decode_nibbles(low, high):
    """Combines two nibbles with offset back into a byte"""
    return (low - 224) + ((high - 224) << 4)

def calculate_checksum(data_bytes):
    """Calculates the XOR checksum as required by the Morbidelli PLC"""
    chk = 0
    for b in data_bytes:
        chk ^= b
    chk ^= 68
    return encode_nibbles(chk)

def verify_checksum(data_bytes, chk_low, chk_high):
    """Verifies if the received checksum matches the calculated one"""
    calc_low, calc_high = calculate_checksum(data_bytes)
    return (chk_low == calc_low) and (chk_high == calc_high)

# --- HAL Initialization ---
h = hal.component("remote")

# IN pins (LEDs)
for pin in ["led_start", "led_stop", "led_lock", "led_f1", "led_f3", "led_f4"]:
    h.newpin(pin, hal.HAL_BIT, hal.HAL_IN)

# OUT pins (Buttons, Axes, Modes)
out_pins = [
    "start", "stop", "plus", "minus", "lock", "cuffia", 
    "f1", "f2", "f3", "f4", "f5", "f6", "f7", "f8",
    "x", "y", "z", "a", "b", "c", "mode"
]
for pin in out_pins:
    h.newpin(pin, hal.HAL_BIT, hal.HAL_OUT)

# OUT pins (Potentiometers)
h.newpin("pot_work", hal.HAL_FLOAT, hal.HAL_OUT)
h.newpin("pot_rapid", hal.HAL_FLOAT, hal.HAL_OUT)

h.ready()

# --- Serial Port Setup ---
# timeout=0.05 gives the device 50ms to respond to our request.
ser = serial.Serial(  
    port='/dev/ttyS0',
    baudrate=57600,
    parity=serial.PARITY_NONE,
    stopbits=serial.STOPBITS_ONE,
    bytesize=serial.EIGHTBITS,
    timeout=0.05 
)

# --- Main Loop ---
try:
    while True:
        # Give LinuxCNC some breathing room (runs at ~50Hz)
        time.sleep(0.02) 
        
        # =========================================================
        # 1. SEND LEDs (Message 40)
        # =========================================================
        led_val = (h['led_start']       | 
                  (h['led_stop'] << 1)  | 
                  (h['led_lock'] << 2)  | 
                  (h['led_f1']   << 3)  | 
                  (h['led_f3']   << 4)  | 
                  (h['led_f4']   << 5))
        
        cc4, cc5 = encode_nibbles(led_val)
        cc6, cc7 = calculate_checksum([cc4, cc5])
        
        ser.write(bytearray([2, 40, cc4, cc5, cc6, cc7, 3]))
        ser.read_until(b'\x03')  # Consume the ACK from the pendant
        
        # =========================================================
        # 2. READ MAIN BUTTONS (Message 33)
        # =========================================================
        ser.write(bytearray([2, 33, 3]))
        rm = ser.read_until(b'\x03') # Read until ETX
        
        if len(rm) == 9 and rm[0] == 2 and rm[1] == 33:
            data = rm[2:6]
            chk_l, chk_h = rm[6], rm[7]
            
            if verify_checksum(data, chk_l, chk_h):
                key1, key2, key3 = data[0] - 224, data[1] - 224, data[2] - 224
                
                # Reset all button pins
                for p in ["start", "stop", "plus", "minus", "lock", "cuffia", "f1", "f2", "f3", "f4", "f5", "f6"]:
                    h[p] = 0

                # Assign buttons
                if key1 == 1: h['start'] = 1  
                elif key1 == 8: h['minus'] = 1
                elif key1 == 2: h['lock'] = 1
                elif key1 == 4: h['f3'] = 1
                
                if key2 == 1: h['stop'] = 1
                elif key2 == 2: h['f1'] = 1
                elif key2 == 8: h['cuffia'] = 1   
                elif key2 == 4: h['f4'] = 1

                if key3 == 1: h['plus'] = 1
                elif key3 == 2: h['f2'] = 1
                elif key3 == 4: h['f5'] = 1
                elif key3 == 8: h['f6'] = 1
            else:
                print("Error 33: Checksum failed")

        # =========================================================
        # 3. READ AXES & MODES (Message 34)
        # =========================================================
        ser.write(bytearray([2, 34, 3]))
        rm = ser.read_until(b'\x03')
        
        if len(rm) == 11 and rm[0] == 2 and rm[1] == 34:
            data = rm[2:8]
            chk_l, chk_h = rm[8], rm[9]
            
            if verify_checksum(data, chk_l, chk_h):
                key = data[0] - 224
                mode = data[2] - 224
                f78 = data[4] - 224
                
                # Reset axes pins
                for p in ["x", "y", "z", "a", "b", "c", "f7", "f8"]:
                    h[p] = 0

                # Assign axes/modes
                if key == 1: h['x'] = 1
                elif key == 2: h['a'] = 1
                elif key == 3: h['b'] = 1
                elif key == 4: h['c'] = 1
                elif key == 8: h['z'] = 1
                elif key in (9, 12): h['y'] = 1  
                           
                if mode == 3: h['mode'] = 1
                elif mode == 2: h['mode'] = 0
                    
                if f78 == 1: h['f7'] = 1
                elif f78 == 2: h['f8'] = 1
            else:
                print("Error 34: Checksum failed")

        # =========================================================
        # 4. READ POTENTIOMETERS (Message 32)
        # =========================================================
        ser.write(bytearray([2, 32, 3]))
        rm = ser.read_until(b'\x03')
        
        if len(rm) == 13 and rm[0] == 2 and rm[1] == 32:
            data = rm[2:10]
            chk_l, chk_h = rm[10], rm[11]
            
            if verify_checksum(data, chk_l, chk_h):
                pot_rapid = decode_nibbles(data[0], data[1])
                pot_work  = decode_nibbles(data[4], data[5])
                
                h['pot_work'] = pot_work
                h['pot_rapid'] = pot_rapid
            else:
                print("Error 32: Checksum failed")

except KeyboardInterrupt:
    raise SystemExit
finally:
    if ser.is_open:
        ser.close()