from random import randint
from kuukar.kuukar_config import MOTOR_FL_F, MOTOR_FL_R, MOTOR_FR_F, MOTOR_FR_R, MOTOR_RL_F, MOTOR_RL_R, MOTOR_RR_F, MOTOR_RR_R, TURN_TIME_FS_90DEG_MS
import time
from kuukar.kuukar_leds import leds
from kuukar.kuukar_sensors import sensors
from telemetrix_rpi_pico import telemetrix_rpi_pico
import threading


class motion:

    roaming = False

    def __init__(self, driver_board: telemetrix_rpi_pico.TelemetrixRpiPico, leds: leds, sensors: sensors) -> None:
        self.driver = driver_board
        self.leds = leds
        self.sensors = sensors
        self.roam_thread = threading.Thread(target=self.__roam_looper)
        self.roam_thread.start()

        drive_pins = [MOTOR_FL_F, MOTOR_FL_R, MOTOR_FR_F,
                      MOTOR_FR_R, MOTOR_RL_F, MOTOR_RL_R, MOTOR_RR_F, MOTOR_RR_R]
        for drive_pin in drive_pins:
            self.driver.set_pin_mode_pwm_output(drive_pin)

    def motor_write(self, forward_pin: int, reverse_pin: int, speed: int):
        if speed < 0:
            self.driver.pwm_write(forward_pin, 0)
            self.driver.pwm_write(reverse_pin, -speed)
        elif speed >= 0:
            self.driver.pwm_write(forward_pin, speed)
            self.driver.pwm_write(reverse_pin, 0)

    def drive(self, speed: int):
        self.motor_write(MOTOR_FL_F, MOTOR_FL_R, speed)
        self.motor_write(MOTOR_FR_F, MOTOR_FR_R, speed)
        self.motor_write(MOTOR_RL_F, MOTOR_RL_R, speed)
        self.motor_write(MOTOR_RR_F, MOTOR_RR_R, speed)

    def turn(self, speed: int, duration: float):
        if speed > 0:
            self.leds.set_right_signal_led(True)
        elif speed < 0:
            self.leds.set_left_signal_led(True)
        self.motor_write(MOTOR_FL_F, MOTOR_FL_R, speed)
        self.motor_write(MOTOR_FR_F, MOTOR_FR_R, -speed)
        self.motor_write(MOTOR_RL_F, MOTOR_RL_R, speed)
        self.motor_write(MOTOR_RR_F, MOTOR_RR_R, -speed)
        time.sleep(duration)
        self.stop()

    def turn_cont(self, speed: int):
        if speed > 0:
            self.leds.set_right_signal_led(True)
        elif speed < 0:
            self.leds.set_left_signal_led(True)
        self.motor_write(MOTOR_FL_F, MOTOR_FL_R, speed)
        self.motor_write(MOTOR_FR_F, MOTOR_FR_R, -speed)
        self.motor_write(MOTOR_RL_F, MOTOR_RL_R, speed)
        self.motor_write(MOTOR_RR_F, MOTOR_RR_R, -speed)

    def stop(self):
        self.leds.set_left_signal_led(False)
        self.leds.set_right_signal_led(False)
        self.leds.set_reverse_led(False)
        self.drive(0)

    def roam_start(self):
        self.roaming = True

    def roam_stop(self):
        self.roaming = False

    def __roam_looper(self):
        while True:
            if self.roaming:
                self.__roam_handle()

    def __roam_handle(self):
        self.drive(50)
        f_dist = self.sensors.sonar_get_distance(2)
        if 0 < f_dist < 20:  # Close to collision, turn the vehicle
            l_dist = self.sensors.sonar_get_distance
            r_dist = self.sensors.sonar_get_distance
            if l_dist < 20:  # Left side is blocked
                if r_dist < 20:  # Both side are blocked, do a full 180deg turn
                    self.turn(100, 2*TURN_TIME_FS_90DEG_MS)
                    self.drive(50)
                    time.sleep(3)
                else:
                    # Right Side is Free, turn right
                    self.turn(100, TURN_TIME_FS_90DEG_MS)
                    self.drive(50)
                    time.sleep(3)
            elif r_dist < 20:  # Right side is blocked, left side is free, turn left
                self.turn(-100, TURN_TIME_FS_90DEG_MS)
            else:  # Both Side are free, randomize a direction
                right: bool = randint(0, 1)
                if right:
                    self.turn(100, TURN_TIME_FS_90DEG_MS)
                    self.drive(50)
                    time.sleep(3)
                else:
                    self.turn(-100, TURN_TIME_FS_90DEG_MS)
                    self.drive(50)
                    time.sleep(3)
        time.sleep(0.5)