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

# 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!!!")
mot_right.throttle = 0.5
mot_left.throttle = 0.5    

while True:
    # Find the motor revolutions
    revleft = enc_left.position / COUNTS_PER_REV
    revright = enc_right.position / COUNTS_PER_REV
    
    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 = 0
        mot_right.throttle = 0
        break