import time
from threading import Thread

import serial


class CMPS14:
    def __init__(self, com_port="/dev/cu.SLAB_USBtoUART", baudrate=115200) -> None:
        """
        Other com port: /dev/cu.SLAB_USBtoUART
        """
        self.serial = serial.Serial(com_port, baudrate=baudrate)
        init_message = str(self.serial.readline())
        assert 'I2C begin Failed' not in init_message, "No I2C sensor detected!"
        self.heading = 0
        self.heading_offset = 0
        self.running = False
        time.sleep(1)

    def start(self):
        t = Thread(target=self.update, args=())
        t.daemon = True
        self.running = True
        t.start()
        return self

    def update(self):
        while self.running:
            try:
                ser_bytes = str(self.serial.readline())
                useful = ser_bytes.split('\\')[0].split('\'')[1]
                heading = float(useful.split(':')[1])
                self.heading = heading
            except Exception as e:
                print(f"Error: {e}")

    def read(self):
        corrected_heading = self.heading + self.heading_offset
        if corrected_heading < 0:
            corrected_heading += 360
        if corrected_heading > 360:
            corrected_heading -= 360
        return corrected_heading

    def set_heading_offset(self, offset):
        self.heading_offset = offset

    def update_heading_offset(self, offset):
        self.heading_offset += offset

    def stop(self):
        self.running = False