from machine import Pin, ADC, PWM
from time import ticks_us, ticks_diff, sleep_ms
import time
import random
from ulab import numpy as np
from ulab import scipy as spy
import math
from math import sin, fabs


#THe code below executes an algorithm, that detects when the everage value from the front IR sensor drops,
#rotates the robot to find the maximum signal and continues moving in that direction


#INPUT parameters:
fullrot = 2085
maxtim=5
err = 5
ma = 20000
x = 50
pwm_val = 20000 #slow rotations for stable values

#Motor Vcc pins set up
m1vcc = 0
m2vcc = 7
m1vccpin = Pin(m1vcc, Pin.OUT)
m2vccpin = Pin(m2vcc, Pin.OUT)
m1vccpin.value(1)
m2vccpin.value(1)

#Vcc pin set up for IR sensors. Cannot set this to max 65535 or sensors misbehave
vcc = PWM(Pin(21), 10000)
vcc.duty_u16(65000)
vcc2 = PWM(Pin(22), 10000)
vcc2.duty_u16(65000)

# Motor pins
m1dir = 10
m1pwm = 11
m2dir = 13
m2pwm = 12

# Encoder pins
enca = 8 # Encoder A
encb = 9# Encoder B
enc2a =8
enc2b =9

# Set up pins
enca_pin = Pin(enca, Pin.IN)
encb_pin = Pin(encb, Pin.IN)
freqq = 10000
m1pwmpin = PWM(Pin(m1pwm), freq=freqq)
m2pwmpin = PWM(Pin(m2pwm), freq=freqq)
m1dirpin = Pin(m1dir, Pin.OUT)
m2dirpin = Pin(m2dir, Pin.OUT)

# Setting up IRsensor pins
ir_sensor1 = ADC(Pin(28))  # Analog input for IR sensor 1
ir_sensor2 = ADC(Pin(26))  # Analog input for IR sensor 2


# Setting up lists
ir_values1 = []
ir_values1old = []
ir_values2 = []
history_length = 10  # Number of readings to store for averaging


#PID parameters
posi = 0
prevT = ticks_us()
eprev = 0
eintegral = 0


# Function to read encoder and add or subtract from posi
def read_encoder(pin):
    global posi
    if encb_pin.value() > 0:
        posi += 1
    else:
        posi -= 1
# Attach interrupt
enca_pin.irq(trigger=Pin.IRQ_RISING, handler=read_encoder)


#Choosing desired color
black = 1
white = 0
color = black

# Motor control functions
def move_forward():
    m1pwmpin.duty_u16(pwm_val)
    m2pwmpin.duty_u16(pwm_val)
    m1dirpin.value(1)
    m2dirpin.value(1)

def move_backward():
    m1pwmpin.duty_u16(pwm_val)
    m2pwmpin.duty_u16(pwm_val)
    m1dirpin.value(0)
    m2dirpin.value(0)

def stop_motors():
    m1pwmpin.duty_u16(0)
    m2pwmpin.duty_u16(0)


#This function is for recalibrating and finding the optimal direction to move in
def rotate_robot():
    global posi, dominant, prevT, eprev, eintegral
    dominant_sensor_readings = [] 
    posi_list = [] 
    
    posi = 0
    while abs(posi) < fullrot:
        #get the sensor readings
        ir_value1 = ir_sensor1.read_u16()
        ir_value1 = ((ir_value1 - minir1)/ir1_diff)*scale
        dominant_sensor_readings.append(ir_value1)
        posi_list.append(posi)
        #rotate
        m1pwmpin.duty_u16(pwm_val)
        m2pwmpin.duty_u16(pwm_val)
        m1dirpin.value(0)
        m2dirpin.value(1)
        time.sleep(0.01)  # Small delay to reduce noise in readings
        
    stop_motors()
    max_value, max_posi = find_maximum_position(dominant_sensor_readings, posi_list)
    #posi = 0
    prevT = ticks_us()
    eprev = 0
    eintegral = 0
    print("max. posi", max_posi)
    return max_value, max_posi

def find_maximum_position(ir_readings, posi_readings):
    max_value = max(ir_readings)
    max_index = ir_readings.index(max_value)
    max_posi = posi_readings[max_index]
    return max_value, max_posi


def average_last_n_values(values, n):
    return sum(values[-n:]) / n

def set_motor2(dir, pwm_val):
    m1pwmpin.duty_u16(pwm_val)
    m2pwmpin.duty_u16(pwm_val)
    if dir == 1:
        m1dirpin.value(0)
        m2dirpin.value(1)
    elif dir == -1:
        m1dirpin.value(1)
        m2dirpin.value(0)
    else:
        m1pwmpin.off()
        m2pwmpin.off()
        

#Function calibrates the IR sensors of the robot
def calibrate_robot():
    ir_values1 = []
    ir_values2 = []
    
    global posi
    posi = 0
    while abs(posi) < fullrot:
        #get the sensor readings
        ir_value1 = ir_sensor1.read_u16()
        ir_value2 = ir_sensor2.read_u16()
        ir_values1.append(ir_value1)
        ir_values2.append(ir_value2) 
        #rotate
        m1pwmpin.duty_u16(pwm_val)
        m2pwmpin.duty_u16(pwm_val)
        m1dirpin.value(0)
        m2dirpin.value(1)
        sleep_ms(100)
        
    stop_motors()
    maxir1 = max(ir_values1)
    minir1 = min(ir_values1)
    maxir2 = max(ir_values2)
    minir2 = min(ir_values1)
    ir1_diff = maxir1 - minir1
    ir2_diff = maxir2 - minir2
    
    if color == white:
        scale = -1000
    elif color == black:
        scale = 1000
        
    ir_values1 = []
    ir_values2 = []
    return maxir1, minir1, maxir2, minir2, ir1_diff, ir2_diff, scale

maxir1, minir1, maxir2, minir2, ir1_diff, ir2_diff, scale = calibrate_robot()

        
#Code that moves robot to desired posi
def moverobot(max_posi):
    global posi, prevT, eprev, eintegral
    
    kp = 3000
    kd = 0.1
    ki = 300
    currT = ticks_us()
    deltaT = ticks_diff(currT, prevT) / 1e6
    prevT = currT

    start_time = ticks_us()  # Start time for this iteration

    while True:
        elapsed_time = ticks_diff(ticks_us(), start_time) / 1e6  # Calculate elapsed time
        if elapsed_time > maxtim:  # Check if the time exceeds 15 seconds
            print(f"Skipping iteration due to timeout.")
            break # Skip this iteration if time exceeds 15 seconds
        
        if posi > (max_posi + err) or posi < (max_posi - err):
            e = posi - max_posi
            dedt = (e - eprev) / deltaT
            eintegral += e * deltaT
            u = kp * e + kd * dedt + ki * eintegral
            pwr = int(fabs(u))
            if pwr > ma:
                pwr = ma
            dir = 1 if u > 0 else -1
            set_motor2(dir, pwr)
            eprev = e
            
            print("Desired", max_posi, "Position:", posi, "U")
        else:
            m1pwmpin.duty_u16(0)
            m2pwmpin.duty_u16(0)
            print('correct angle')
            break
        sleep_ms(10)

    print('Final position', posi, "Desired position", max_posi)
    if max_posi == 0:
        max_posi = 1
    accuracy = abs(((abs(posi-max_posi))/max_posi))*100
    print('accuracy', accuracy)
    posi = 0
    prevT = ticks_us()
    eprev = 0
    eintegral = 0

    return elapsed_time, accuracy  # Return the time taken for this iteration


#Initial movement, 
max_value, max_posi = rotate_robot() 
moverobot(max_posi)
move_forward()

# Main loop
while True:
    # Read the analog values from both IR sensors
    ir_value1 = ir_sensor1.read_u16()
    ir_value1 = ((ir_value1 - minir1)/ir1_diff)*scale

    # Append the current values to the list
    ir_values1.append(ir_value1)
    
    
    if len(ir_values1)>= history_length:
 
        # Keep only the last `history_length` values
        if len(ir_values1) > history_length:
            popval = ir_values1.pop(0)
            ir_values1old.append(popval)
        if len(ir_values1old) > history_length:
            ir_values1old.pop(0)
        

        # Calculate the average of the last `history_length` readings
        avg_ir_value1 = average_last_n_values(ir_values1, history_length)
        avg_ir_value1old = average_last_n_values(ir_values1old, history_length)
        

        print("Avg IR1:", avg_ir_value1,"Avg old", avg_ir_value1old)  # For debugging

        # Check if the current average value is greater than the previous average value
        if avg_ir_value1 < avg_ir_value1old - x:
           max_value, max_posi = rotate_robot()
           moverobot(max_posi)
           move_forward()
           ir_values1 = []
           ir_values1old = []
           
        
        time.sleep(0.01)  # Small delay to reduce noise in readings
    else:
        continue