from machine import Pin, I2C, PWM, ADC
from mpu6050 import MPU6050
import time
import utime

i2c = I2C(0, scl=Pin(1), sda=Pin(0))
irr = Pin(14, Pin.IN)

mpu = MPU6050(i2c)
mpu.wake()

relay = Pin(18, Pin.OUT)
pwm = PWM(Pin(26, Pin.OUT))
pwm.freq(50)

outA = Pin(4, mode=Pin.IN) # Pin CLK of encoder
outB = Pin(3, mode=Pin.IN) # Pin DT of encoder

counter = 0

outA_last = 0
outA_current = 0
outA_last = outA.value()

x_pin = ADC(Pin(27))
y_pin = ADC(Pin(28))

def set_servo_angle(angle):
    min_duty = 1600
    max_duty = 8000
    duty = int((angle / 180) * (max_duty - min_duty) + min_duty)
    pwm.duty_u16(duty)

def detect_mpu6050():
    data = mpu.read_accel_data()
    return {"X": data[0],
            "Y": data[1],
            "Z": data[2]}

def read_encoder():
    global counter
    global direction
    global outA_last
    global outA_current

    outA_current = outA.value()

    if outA_current != outA_last:
        if outB.value() != outA_current:
            counter += 1
        else:
            counter -= 1

    outA_last = outA_current
    utime.sleep_ms(1)

    return counter

def read_joystick():
    x = int(x_pin.read_u16() * 100 / 65535)
    y = int(y_pin.read_u16() * 100 / 65535)
    return (x, y)

encoder_turn = 0
joystick_direction = "up"
acceleration = 1.5

directions = {
                "up": 100,
                "down": 0
             }

encoder_turn = int(input("Enter encoder turning value for lock (multiple of 2): "))
joystick_direction = input("What direction should the joystick face to open the lock (up/down)?: ")
acceleration = float(input("Minimum Y Acceleration for lock to open: "))

ir_val = 0
while True:
    print("Current encoder value: " + str(read_encoder()))

    last_state = ir_val
    ir_val = irr.value()

    # Turn lock system on
    if ir_val == 0 and last_state == 1:
        print("Lock system on!")
        relay.value(1)

    # To open lock
    encoder_correct = read_encoder() == encoder_turn
    joystick_correct = read_joystick()[1] == directions[joystick_direction]
    object_picked_up = detect_mpu6050()["Y"] > acceleration

    if encoder_correct and joystick_correct and object_picked_up:
        set_servo_angle(180)
    else:
        set_servo_angle(90)
    
    time.sleep(0.001)