# designed by nohim 
# copyright - all rights reserved.
# start-date - 20/04/2025
# revision - 1
# geenie's code

# libraries
from machine import Pin as pin
from machine import PWM
from machine import time_pulse_us sleep_us
from time import sleep


#------------- MOTOR CONTROLS ---------------
# Setup pins
mrb = pin(0, pin.OUT)
mrf = pin(1, pin.OUT)
mlb = pin(2, pin.OUT)
mlf = pin(3, pin.OUT)
ena = PWM(pin(4))
enb = PWM(pin(5))

# Set PWM frequency
ena.freq(1000)  # 1 kHz is a good start
enb.freq(1000)  # 1 kHz is a good start

#------------- ULTRASONIC CONTROLS ---------------
trigger = pin(14, pin.OUT)
echo = pin(15, pin.IN)

class ultrasonic:
    def __init__(self):
            pass
    def get_distance(self):
         # Send 10us pulse to trigger
        trigger.low()
        sleep_us(2)
        trigger.high()
        sleep_us(10)
        trigger.low()
 
        # Measure the duration of the echo pulse
        duration = time_pulse_us(echo, 1, 30000)  # timeout after 30ms
        
        if duration < 0:
            return None  # Timeout occurred

        # Speed of sound = 34300 cm/s => Distance = (duration / 2) * 0.0343
        distance_cm = (duration / 2) * 0.0343
        return distance_cm


class Geenie:
    def __init__(self):
        # geenie's different states
        self.motor_state = ""
        self.turn = ""
        # distance for the geenie
        self.T1 = 100
        self.T2 = 80
        self.T3 = 20

        self.S1 = 100
        self.S2 = 80
        self.S3 = 20
        # speed
        self.speed = ""
        self.off_sec = 2
        self.reverse_sec = 2
        self.turn_sec = 0.5
        self.turn_after_sec = 0

        self.reverse_speed = 40

    def forward(self):
        self.motor_state = "F"
        # move the right motor forward
        mrf.value(1)
        mrb.value(0)
        # move the left motor forward
        mlf.value(1)
        mlb.value(0)
    
    def reverse(self):
        self.motor_state  = "B"
        # move the right motor backward
        mrf.value(0)
        mrb.value(1)
        # move the left motor backward
        mlf.value(0)
        mlb.value(1)

    def turn_right(self):
        self.turn = "R"
        # turn the right motor forward
        mrf.value(1)
        mrb.value(0)
        # turn the left motor backward
        mlf.value(0)
        mlb.value(1)
        
    def turn_left(self):
        self.turn = "L"
        # turn the right motor backward
        mrf.value(0)
        mrb.value(1)
        # turn the left motor forward
        mlf.value(1)
        mlb.value(0)
    
    def off(self):
        self.motor_state = False
        self.turn = False
        for x in [mrf,mrb,mlf,mlb]:
            x.value(0)

    def current_motor_state(self):
        return self.motor_state

    def set_speed(self,duty_percent):
        self.speed = duty_percent
        duty = int(duty_percent * 65535 / 100)  # Convert to 16-bit value
        ena.duty_u16(duty)
        enb.duty_u16(duty)

    def read_distance(self):
         ult = ultrasonic()
        # get the distance to the farthest object
         return ult.get_distance()
    
    def perform_T3(self):
            self.off() # stop the geenie
            sleep(self.off_sec)
            self.set_speed(self.reverse_speed)
            self.reverse()
            sleep(self.reverse_sec)

            # turn left ================================================================
            self.turn_left()
            # turn left for a specific sec
            sleep(self.turn_sec)
            sleep(self.turn_after_sec)
            # pause for reading the distance on the left side
            left_distance = self.read_distance()
            sleep(3)

            # get back to original position ==============================================
            self.turn_right()
            sleep(self.turn_sec)

            # turn right =================================================================
            self.turn_right()
            # turn right for a specific sec
            sleep(self.turn_sec)
            sleep(self.turn_after_sec)
            # pause for reading the distance on the right side
            right_distance = self.read_distance()
            sleep(3)

            # get back to original position ==============================================
            self.turn_left()
            sleep(self.turn_sec)

            return (left_distance,right_distance)
        

    def RUN(self):
        # get the distance to the farthest object
        distance = self.read_distance()
        
        # check if the distance is between T1 and T2
        if distance <= self.T1 and distance > self.T2:
            # get the current motor state
            # check if it is running
            if self.current_motor_state() == "F":
                self.set_speed(self.S1)
            # if the geenie is not running
            else:   
                sleep(2)
                self.set_speed(self.S1) # set the speed
                self.forward()          # go forward

        elif distance <= self.T2 and distance > self.T3:
            # get the current motor state
            # check if it is running
            if self.current_motor_state() == "F":
                self.set_speed(self.S2)
            # if the geenie is not running
            else:   
                sleep(2)
                self.set_speed(self.S2) # set the speed
                self.forward()          # go forward
        elif distance <= self.T3:
            (left,right) = self.perform_T3()
            sleep(3)
            if left > right:
                self.turn_left()
                sleep(self.turn_sec)
                sleep(self.turn_after_sec)
            else:
                self.turn_right()
                sleep(self.turn_sec)
                sleep(self.turn_after_sec)

        elif distance > self.T1:
            self.set_speed(self.S1)
            self.forward()
            


geenie = Geenie()

while True:
    geenie.RUN()