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

#This code is used to test the motors by generating an array of postive and negative angles for the robot to attain

#INPUT parameters:
n = 5 # This is the number of iterations of this code you want performed.
maxang = 180 # Maximum magnitude of an angle the robot achieves out of 360
fullrot = 2085 #this variable represents the posi value that corresponds with a full 360 rotation. Might need to be altered depending on model
maxtim = 10 # This is the maximum amount of time each iterations are allowed to run. 
err = 5 #This is the maximum error allowed. The function moverobot will accept values within plus or minus err of the desired position
ma = 20000 # ma is the maximum power delivered to the robot out of 65535. 20000 is a good starting point for reliable performance


# Generation of a list of random directions between -maxang and maxang
maxang = (maxang/360) * fullrot
direc = [random.uniform(-maxang, maxang) for _ in range(n)]
direc = np.array(direc)
direc = np.array([fullrot])# code for full rotation

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

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

# Setting 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)

#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)


#Parameters needed for PID controller
posi = 0 #this is the variable use to track the position of the wheels
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)

# Function to set motor direction
def set_motor(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()

# Main function to move the robot to match desired position
def moverobot(d):
    global posi, prevT, eprev, eintegral
    
    #PID controller variables
    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 limit
            print(f"Skipping iteration {d+1} due to timeout.")
            break # Skip this iteration if time exceeds 15 seconds
        
        if posi > (int(direc[d]) + err) or posi < (int(direc[d]) - err):
            e = posi - direc[d]
            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_motor(dir, pwr)
            eprev = e
            
            print("direc[d]:", direc[d], "Position:", posi,"iteration", d+1,)
        else:
            m1pwmpin.duty_u16(0)
            m2pwmpin.duty_u16(0)
            print('correct angle')
            break
        sleep_ms(10)

    print('Final position', posi, "Desired position", direc[d])
    accuracy = abs(((abs(posi-direc[d]))/direc[d]))*100 #accuracy here is actually error
    print('Error', accuracy)
    posi = 0
    prevT = ticks_us()
    eprev = 0
    eintegral = 0

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

# Lists for time taken, as well as accuracy
time_taken_list = []
direc_time_pairs = []
accuracy1 = []

for d in range(len(direc)):
    time_taken, accuracy = moverobot(d)
    time_taken_list.append(time_taken)
    direc_time_pairs.append((time_taken, direc[d]))
    accuracy1.append(100 - accuracy)#converting error to accuracy

#converting accuracy1 to numpy array so that median can be found
accuracy1 = np.array(accuracy1)


# Calculating the average time and average error
average_time_taken = sum(time_taken_list) / len(time_taken_list)
time_taken_list.append(average_time_taken)
average_accuracy = sum(accuracy1)/len(accuracy1)


# Sort the direc_time_pairs list based on time taken in descending order
sorted_pairs = sorted(direc_time_pairs, key=lambda x: x[0], reverse=True)


# Print the 5 longest times and their corresponding posi values to ensure that PID controller is working as desired and not stalling
print("\n \n \n5 Longest Times and Corresponding Direction Values:")
for i in range(min(5, len(sorted_pairs))):
    print(f"Time: {sorted_pairs[i][0]:.2f}s, Direction: {sorted_pairs[i][1]:.2f} degrees")


# Print average time
if average_time_taken is not None:
    print("Average time taken:", average_time_taken)
    print("Average Accuracy:", average_accuracy)
    print("Median Accuracy:", np.median(accuracy1))