from machine import Pin, PWM, ADC
from time import sleep_ms, sleep_us, ticks_us, ticks_diff
from neopixel import NeoPixel

#Speed variable (tweak this to the lowest possible, that still starts the motors)
speed=50000
hunting_speed = 60000 #Used when robot detects light

# safety distance för ultrasonic sensor (roughly cm)
us_safety_distance = 10
max_us_time = us_safety_distance * 2 / 0.03432

light_threshold = 15000 #

### MOTORS
#Pin Setup (left motor)
pwm1 = PWM(Pin(22))
in1  = Pin(21, Pin.OUT)
in2  = Pin(20, Pin.OUT)

#Pin Setup (right motor)
in3  = Pin(19, Pin.OUT)
in4  = Pin(18, Pin.OUT)
pwm2 = PWM(Pin(17))

### SENSORS
#IR Sensors
irSensorL=Pin(0, Pin.IN, Pin.PULL_UP)
irSensorR=Pin(1, Pin.IN, Pin.PULL_UP)

#UltraSonic Sensor
usTrigger = Pin(2, Pin.OUT) #Triggerpin
usEcho = Pin(3, Pin.IN) #Echopin

#Light Sensors
lightSensorL = ADC(27)
lightSensorR = ADC(26)

# NEOPOIXEL PIN
ledPin = Pin(28, Pin.OUT)

# NEOPIXEL SETUP
led = NeoPixel(ledPin, 12)
red=(100,0,0)
green=(0,100,0)
blue=(0,0,100)
white=(10,10,10)

#Set PWM freqency for PWM-Pins
pwm1.freq(100)
pwm2.freq(100)

#Helper
def set_motors(v1, v2, v3, v4, speed1, speed2):
    in1.value(v1)
    in2.value(v2)
    in3.value(v3)
    in4.value(v4)
    pwm1.duty_u16(speed1)
    pwm2.duty_u16(speed2)

def move_forward(s = speed):
    set_motors(0, 1, 0, 1, s, s)

def move_left(s = speed):
    set_motors(0, 1, 0, 0, s, s)

def move_right(s = speed):
    set_motors(0, 0, 0, 1, s, s)

def stop():
    set_motors(0, 0, 0, 0, 0, 0)
    
def turn_around(s = speed):
    print('turning around')
    set_motors(1, 0, 0, 1, s, s)
    sleep_ms(800)
    
def us_measurement():
    # starts ultrasonic measurement
    usTrigger.low()
    sleep_us(2)
    usTrigger.high()
    sleep_us(10)
    usTrigger.low()
    
def echo_callback(pin):
    global start_time, object_is_near
    
    if pin.value() == 1:
        start_time = ticks_us()
    else:
        if start_time > 0:
            # Calculate duration
            duration = ticks_diff(ticks_us(), start_time)
            #print("US measurement: ", duration)
            
            # If duration is shorter than tresshhold = Object is near!
            if duration > 0 and duration <= max_us_time:
                object_is_near = True
            else:
                object_is_near = False
                
            start_time = 0

def ir_left_callback(pin):
    global line_detected
    line_detected = True

def ir_right_callback(pin):
    global line_detected
    line_detected = True
    
def change_led(c):
    global red, green, blue, white
    led.fill(c)
    led.write()

    
# Global flags
start_time = 0
object_is_near = False
line_detected = False

# Register interrupts
irSensorL.irq(trigger=Pin.IRQ_RISING, handler=ir_left_callback)
irSensorR.irq(trigger=Pin.IRQ_RISING, handler=ir_right_callback)
usEcho.irq(trigger=Pin.IRQ_RISING | Pin.IRQ_FALLING, handler=echo_callback)

#Main Loop
while True:
    # ultrasonic sensor logic
    us_measurement() #Start ultrasonic measurement
    
    if object_is_near: # Turn around if object is detected
        change_led(blue)
        turn_around()
        object_is_near = False
        continue

    # IR Sensor logic
    if line_detected: #
        change_led(green)
        ir_values = (irSensorL.value(), irSensorR.value()) #Read IR sensors
        while ir_values != (0,0):
            if ir_values == (1,1):
                stop() #wait for help
            elif ir_values == (1,0):
                move_left() 
            elif ir_values == (0,1):
                move_right()
            sleep_ms(100)
            ir_values = (irSensorL.value(), irSensorR.value())

        line_detected = False
    else:
        #Read Lightsensors
        lsR = lightSensorR.read_u16()
        lsL = lightSensorL.read_u16()
        #print("Light measurement: R:", lsR , " L:" , lsL)

        # change direction based on light sensors
        if (lsR + lsL) < (light_threshold * 2): # checks threshold
            change_led(red)
            if (abs(lsR - lsL) < 100): # moving forward
                move_forward(hunting_speed)
            elif lsR < lsL: #moving right
                move_right(hunting_speed)
            else: #moving left
                move_left(hunting_speed)
        else:
            change_led(white)
            move_forward(speed)
        
    for _ in range(10):
        sleep_ms(10)
        if line_detected or object_is_near:
            break  # Interrupt sleep