import time
import math
import board
import adafruit_hcsr04
import pwmio
import digitalio
import rotaryio
from adafruit_motor import motor

# Mathematics
diameter = 0.047 # 47mm wheel diameter
circumference = diameter * math.pi
target_dist = 5

# Encoder constants
GEAR_RATIO = 50                     # The gear ratio of the motor
COUNTS_PER_REV = 12 * GEAR_RATIO    # The counts per revolution of the motor's output shaft
ENCODER_NAMES = ["A", "D"]

# Motor constants
FREQUENCY = 25000               
DECAY_MODE = motor.SLOW_DECAY

UPDATE_RATE = 100  # Hz
SLEEP_TIME = 1 / UPDATE_RATE
BASE_THROTTLE = 0.85  # Base throttle for both motors
Kp_sync = 1.5  # Proportional gain for sync correction
Kd_sync = 0.04
prev_error = 0

# Create a digitalinout object for the user switch
user_sw = digitalio.DigitalInOut(board.USER_SW)
user_sw.direction = digitalio.Direction.INPUT
user_sw.pull = digitalio.Pull.UP

# Set the pwm and motor pins
pwm_a_p = pwmio.PWMOut(board.MOTOR_A_P, frequency=FREQUENCY)
pwm_a_n = pwmio.PWMOut(board.MOTOR_A_N, frequency=FREQUENCY)

pwm_d_p = pwmio.PWMOut(board.MOTOR_D_P, frequency=FREQUENCY)
pwm_d_n = pwmio.PWMOut(board.MOTOR_D_N, frequency=FREQUENCY)

mot_right = motor.DCMotor(pwm_a_p, pwm_a_n)
mot_left = motor.DCMotor(pwm_d_p, pwm_d_n)

mot_right.decay_mode = DECAY_MODE
mot_left.decay_mode = DECAY_MODE

# Create the encoder objects
enc_right = rotaryio.IncrementalEncoder(board.ENCODER_A_B, board.ENCODER_A_A, divisor=1)
enc_left = rotaryio.IncrementalEncoder(board.ENCODER_D_B, board.ENCODER_D_A, divisor=1)

enc_right.position = 0
enc_left.position = 0
lastrevleft = 0
lastrevright = 0
revleft = 0
revright = 0

# Set the sensor pins
sonar = adafruit_hcsr04.HCSR04(trigger_pin=board.TRIG, echo_pin=board.ECHO)

def to_revs(position):
    return position / COUNTS_PER_REV
    
def vel(rev, last_rev):
    return (rev - last_rev) / UPDATE_RATE

while sonar.distance < 20:
    print("I am waiting...", sonar.distance)
    
    if sonar.distance >= 20:
        break

print(">>>>> Starting movement!!!")

while True:
    # Find the motor revolutions
    lastrevleft = revleft
    lastrevright = revright
    revleft = to_revs(enc_left.position)
    revright = to_revs(enc_right.position)
    
    vel_left = vel(revleft, lastrevleft)
    vel_right = vel(revright, lastrevright)
    
    # Encoder error due to friction.
    error = vel_left - vel_right
    derivative = (error - prev_error) / UPDATE_RATE
    prev_error = error
    correction = (Kp_sync * error) #+ (Kd_sync * derivative)
    
    left_throttle = BASE_THROTTLE - correction
    right_throttle = BASE_THROTTLE + correction

    left_throttle = max(min(left_throttle, 1.0), -1.0)
    right_throttle = max(min(right_throttle, 1.0), -1.0)

    
    distleft = revleft * circumference
    distright = revright * circumference
    
    avg = (abs(distleft) + abs(distright)) / 2

    print(f"Distance traveled: {avg:.3f} m")
    
        
    if avg >= target_dist:
        print("Target reached!!!")
        mot_left.throttle = 0
        mot_right.throttle = 0
        break
    
    
    mot_right.throttle = right_throttle
    mot_left.throttle = - left_throttle
        
    print(right_throttle, left_throttle)
    
    time.sleep(0.05)
print("Finished!!!")