'''
v2 - if sensor all 0, car backs up to hopefully intersect line
v2.2 - if sensor all 1, move car ahead 3.5" and rotate left
v2.3 - if sensor all 1, check forward (at intersection), then check left, and right.
'''
import machine
from time import sleep_ms, ticks_ms
import pca9685
import motor
from MPU6050 import MPU6050
from Car_lib import accel_calibration, motor_move, GyroSensor

# set up i2c and pca9685
sdaPIN=machine.Pin(13)
sclPIN=machine.Pin(14)
i2c=machine.I2C(0,sda=sdaPIN, scl=sclPIN)

# Initialize the PCA9685 using the default address (0x40).
pwm = pca9685.PCA9685(address=0x5f, i2c = i2c)
# Set the PWM frequency to 60Hz, good for servos.
pwm.freq(60)
motors = motor.DCMotors(address=0x5f, i2c = i2c)
# initial accelerometer and gyro
mpu = MPU6050()


def car_rotate(gyro_sensor, mode, angle, angle_bias=0):
    angZ = gyro_sensor.get_gyro(reset=True)
    if mode == 'left':
        motor_move(motors, m1_speed=-2000, m2_speed=-2000, m3_speed=2000, m4_speed=2000, scale=1.0)
        while True:
            angZ = gyro_sensor.get_gyro()
#             if angZ > angle - angle_bias:     # sensor mounted upright
            if angZ < -angle + angle_bias:      # sensor upside down
                break
    elif mode == 'right':
        motor_move(motors, m1_speed=2000, m2_speed=2000, m3_speed=-2000, m4_speed=-2000, scale=1.0)
        while True:
            angZ = gyro_sensor.get_gyro()
#             if angZ < -angle + angle_bias:    # sensor mounted upright
            if angZ > angle - angle_bias:      # sensor upside down
                break

def move3():
    # scale the motor speed
    motor_speed = 2000
    scale_speed = 0.4
    sleep_time =237
    sleep_time =320

    # move forward and brake
    scale = scale_speed
    motor_move(motors,
               m1_speed=motor_speed,
               m2_speed=motor_speed,
               m3_speed=motor_speed,
               m4_speed=motor_speed,
               scale=scale)
    sleep_ms(sleep_time)
    motor_move(motors)
    sleep_ms(500)
    
# delay to unplug car
sleep_ms(1000)

#line tracking

# define speeds
SPEED_LV4 = 4000
SPEED_LV3 = 3000
SPEED_LV2 = 2500
SPEED_LV1 = 1500
scale = 0.25
# set bias for  turn (the car usually overshoots the desired angle)
angle_bias = 15

# find calibration values
axc, ayc, axsd, aysd, gzc = accel_calibration(mpu)
gyro_sensor = GyroSensor(mpu, gzc)

while True:
# for n in range(30):
    # read leds
    data = i2c.readfrom(0x20,1)
    test1 = int(data[0] - 248)  #convert to int 0-7
    
    if (test1 == 2 or test1 == 5):
#         print('test1 = 2 or 5')
        motor_move(motors, SPEED_LV1, SPEED_LV1, SPEED_LV1, SPEED_LV1, scale=scale)    #Move Forward
   
    elif (test1 == 7):                                 # all black
#         print('test1 = 0 or 7')
        motor_move(motors)
        sleep_ms(1000)
        
        # move forward to check intersection
        motor_move(motors, SPEED_LV1, SPEED_LV1, SPEED_LV1, SPEED_LV1, scale=scale)
        sleep_ms(250)
        data = i2c.readfrom(0x20,1)
        test2 = int(data[0] - 248)  #convert to int 0-7
        if test2 != 0:
            continue    # line detected, keep going
        
        # move back, then check left
        motor_move(motors)  # stop
        sleep_ms(250)
        motor_move(motors, -SPEED_LV1, -SPEED_LV1, -SPEED_LV1, -SPEED_LV1, scale=scale)  # move back
        sleep_ms(300)
        motor_move(motors)
        # move 3.5 inches
        move3()
        # rotate left
        car_rotate(gyro_sensor, 'left', angle=90, angle_bias=angle_bias)
        # move forward to check intersection
        motor_move(motors, SPEED_LV1, SPEED_LV1, SPEED_LV1, SPEED_LV1, scale=scale)
        sleep_ms(250)
        data = i2c.readfrom(0x20,1)
        test2 = int(data[0] - 248)  #convert to int 0-7
        if test2 != 0:
            continue    # line detected, keep going
        
        # move back, then check right
        motor_move(motors)  # stop
        sleep_ms(250)
        # rotate right to turn around
        car_rotate(gyro_sensor, 'right', angle=175, angle_bias=angle_bias)
        # move forward to check intersection
        motor_move(motors, SPEED_LV1, SPEED_LV1, SPEED_LV1, SPEED_LV1, scale=scale)
        sleep_ms(250)
        data = i2c.readfrom(0x20,1)
        test2 = int(data[0] - 248)  #convert to int 0-7
        if test2 != 0:
            continue    # line detected, keep going
        
        # if not successful, stop (maybe inside final 'circle'
        motor_move(motors)
        break  # end the course
        
    elif (test1 == 0):                                 # off track or between
        motor_move(motors,-SPEED_LV1, -SPEED_LV1, -SPEED_LV1, -SPEED_LV1, scale=scale)   # backup

    elif (test1 == 4 or test1 == 6):
#         print('test1 = 4 or 6')
        motor_move(motors,SPEED_LV4, SPEED_LV4 , - SPEED_LV3, -SPEED_LV3, scale=scale)  #Turn Right

    elif (test1 == 1 or test1 == 3):
#         print('test1 = 1 or 3')
        motor_move(motors, -SPEED_LV3, -SPEED_LV3, SPEED_LV4, SPEED_LV4, scale=scale)  #Turn Left