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 = 3

# 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.9  # Base throttle for both motors
Kp_sync = 0.05  # Proportional gain for sync correction
Kd_sync = 0.001
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

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



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

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

while True:
    # Find the motor revolutions
    revleft = - enc_left.position / COUNTS_PER_REV
    revright = enc_right.position / COUNTS_PER_REV
    
    # Encoder error due to friction.
    error = enc_left.position - enc_right.position
    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 = (distleft + distright) / 2

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