from serial import Serial


class KITT:
    def __init__(self, port, baudrate=115200):
        # Initialize the serial connection
        # self.serial = Serial(port, baudrate, rtscts=True)
        self.serial = Serial(port, baudrate,rtscts=True)
        # Initialize beacon parameters here using send_command
        # Set carrier frequency, bit frequency, repetition count, and code pattern
        carrier_frequency_value = 10000
        carrier_frequency = carrier_frequency_value.to_bytes(2, byteorder='big')
        self.send_command(b"F"+carrier_frequency+b"\n")


        bit_frequency_value = 5000
        bit_frequency = bit_frequency_value.to_bytes(2, byteorder='big')
        self.send_command(b"B"+bit_frequency+b"\n")


        desired_repetition_frequency = 2  # Replace with your value
        repetition_count_value = bit_frequency_value // desired_repetition_frequency
        repetition_count = repetition_count_value.to_bytes(2, byteorder='big')
        # Ensure repetition_count_value is at least 32
        repetition_count_value = max(repetition_count_value, 32)
        repetition_count = repetition_count_value.to_bytes(2, byteorder='big')
        self.send_command(b"R"+repetition_count+b"\n")

        code_value = 0x67676767 # Replace with your code
        code = code_value.to_bytes(4, byteorder='big')
        self.send_command(b"C"+code+b"\n")

    def send_command(self, command):
        # Send the command string over the serial connection
        self.serial.write(command)
        print(command)

    def set_speed(self, speed):
        # Send the motor speed command using send_command
        # Use the format 'M<speed>\n'
        self.send_command(f"M{speed}\n".encode())

    def set_angle(self, angle):
        # Send the steering angle command using send_command
        # Use the format 'D<angle>\n'
        self.send_command(f"D{angle}\n".encode())

    def stop(self):
        # Stop the car by setting speed and angle to neutral (150)
        # Call set_speed and set_angle with 150
        self.set_speed(150)
        self.set_angle(150)


    def start_beacon(self):
        # Send commands to start the beacon
        # Use the command 'A1\n'
        self.send_command(b"A1\n")

    def stop_beacon(self):
        # Send commands to stop the beacon
        # Use the command 'A0\n'
        self.send_command(b"A0\n")

    def get_distance_report(self):
        self.send_command(b"Sd\n")
        #self.serial.read_until(b'\x04').decode('utf-8')
        return self.serial.read_until(b'\x04').decode('utf-8')


    def close(self):
        # Close the serial connection
        # self.serial.close()
        self.serial.close()