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, mcu: telemetrix_rpi_pico.TelemetrixRpiPico, leds: leds, sensors: sensors) -> None:
        self.mcu = mcu
        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.mcu.set_pin_mode_pwm_output(drive_pin)

    def motor_write(self, forward_pin: int, reverse_pin: int, speed: int):
        if speed < 0:
            self.mcu.pwm_write(forward_pin, 0)
            self.mcu.pwm_write(reverse_pin, -int(speed))
        elif speed >= 0:
            self.mcu.pwm_write(forward_pin, int(speed))
            self.mcu.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_left_signal_led(False)
            self.leds.set_right_signal_led(True)
        elif speed < 0:
            self.leds.set_left_signal_led(True)
            self.leds.set_right_signal_led(False)
        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/1000.0)
        self.stop()

    def turn_cont(self, speed: int):
        if speed > 0:
            self.leds.set_right_signal_led(True)
            self.leds.set_left_signal_led(False)
        elif speed < 0:
            self.leds.set_right_signal_led(False)
            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 drive_skew(self, speed: float, dir: float):
        if dir > 0:
            self.leds.set_right_signal_led(True)
            self.leds.set_left_signal_led(False)
        elif dir < 0:
            self.leds.set_right_signal_led(False)
            self.leds.set_left_signal_led(True)


        lm_speed = (100+dir/2)*speed/100
        if lm_speed>99:
            lm_speed = 99
        elif lm_speed<-99:
            lm_speed = -99
        rm_speed = (100-dir/2)*speed/100
        if rm_speed > 99:
            rm_speed = 99
        elif rm_speed < -99:
            rm_speed = -99;

        self.motor_write(MOTOR_FL_F, MOTOR_FL_R, lm_speed)
        self.motor_write(MOTOR_FR_F, MOTOR_FR_R, rm_speed)
        self.motor_write(MOTOR_RL_F, MOTOR_RL_R, lm_speed)
        self.motor_write(MOTOR_RR_F, MOTOR_RR_R, rm_speed)

    '''    
    def skt_drive(self, speed: float, turn: float, t=0):
        speed *=100
        turn *=100
        leftSpeed = speed - turn
        rightSpeed = speed + turn
        if leftSpeed>99: leftSpeed=99
        elif leftSpeed<-99: leftSpeed= -99
        if rightSpeed>99: rightSpeed=99
        elif rightSpeed<-99: rightSpeed= -99
        if speed>99: speed=99
        elif speed<-99: speed= -99

        if (rightSpeed - leftSpeed) > 0:     
            ##self.motor_write(MOTOR_FL_F, MOTOR_FL_R, -1*abs(leftSpeed))
            self.motor_write(MOTOR_FR_F, MOTOR_FR_R, abs(rightSpeed))
            ##self.motor_write(MOTOR_RL_F, MOTOR_RL_R, speed)
            self.motor_write(MOTOR_RR_F, MOTOR_RR_R, speed)
        elif (rightSpeed - leftSpeed) < 0:
            self.motor_write(MOTOR_FL_F, MOTOR_FL_R, abs(leftSpeed))
            ##self.motor_write(MOTOR_FR_F, MOTOR_FR_R, -1*abs(rightSpeed))
            self.motor_write(MOTOR_RL_F, MOTOR_RL_R, speed)
            ##self.motor_write(MOTOR_RR_F, MOTOR_RR_R, speed)
        else:
            self.motor_write(MOTOR_FL_F, MOTOR_FL_R, abs(leftSpeed))
            self.motor_write(MOTOR_FR_F, MOTOR_FR_R, abs(rightSpeed))
            self.motor_write(MOTOR_RL_F, MOTOR_RL_R, speed)
            self.motor_write(MOTOR_RR_F, MOTOR_RR_R, speed)
    '''

    def skt_drive(self, speed: float, turn: float, t=0):
        speed *=100
        turn *=100
        leftSpeed = speed - turn
        rightSpeed = speed + turn
        if leftSpeed>99: leftSpeed=99
        elif leftSpeed<-99: leftSpeed= -99
        if rightSpeed>99: rightSpeed=99
        elif rightSpeed<-99: rightSpeed= -99
        if speed>99: speed=99
        elif speed<-99: speed= -99

        self.motor_write(MOTOR_FL_F, MOTOR_FL_R, abs(leftSpeed))
        self.motor_write(MOTOR_FR_F, MOTOR_FR_R, abs(rightSpeed))
        #self.motor_write(MOTOR_RL_F, MOTOR_RL_R, speed)
        #self.motor_write(MOTOR_RR_F, MOTOR_RR_R, speed)
        time.sleep(t)


    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.drive(25)
        self.roaming = True

    def roam_stop(self):
        self.roaming = False
        time.sleep(0.5)
        self.stop()

    def is_roaming(self) -> bool:
        return self.roaming

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

    def __roam_handle(self):
        if self.sensors.sonar_get_distance()<500:
            self.turn(50,2000)
            self.drive(25)
            time.sleep(0.5)
        if not self.roaming:
            self.stop()