import serial
import logging
import time

class PalletProximity:
    def __init__(self) -> None:
        self.log = logging.getLogger("PALLET PROXIMITY SENSOR")
        self.log.setLevel("DEBUG")
        # self.serial_string = '/dev/cu.usbmodem1442401'
        self.serial_string = '/dev/micro'
        self.alive = False
        self.distance_to_pallet = 999
        try:
            self.ser = serial.Serial(self.serial_string, 9600, timeout=1)
            self.alive = True
            self.log.info('Proximity serial open')
        except Exception as e:
            self.log.critical(f"Cannot open serial device, {e}")
            self.log.critical('Exiting')
            exit(0)

    def get_prox_reading(self):
        try:
            self.ser.flushInput()
            # time.sleep(0.05)
            readings = 0
            for _ in range(3):
                raw = str(self.ser.readline())
                raw2 = int(raw.split("|")[1])
                readings += raw2
            self.distance_to_pallet = int(readings / 3)
            return self.distance_to_pallet
        except Exception as e:
            self.log.critical(f"ERROR: {e}, serial input: {raw}")
            return self.distance_to_pallet
            # return False


if __name__ == "__main__":
    print("Testing...")
    a = PalletProximity()
    while True:
        # time.sleep(0.1)
        print(a.get_prox_reading())