from machine import Pin
from machine import I2C, Pin
from time import sleep
from pico_i2c_lcd import I2cLcd
from time import sleep_ms, sleep_us

floors = [
  ("1st Floor", Pin(13, Pin.OUT), 0, Pin(21, Pin.IN, Pin.PULL_UP)),
  ("2nd Floor", Pin(12, Pin.OUT), 360, Pin(20, Pin.IN, Pin.PULL_UP)),
  ("3rd Floor", Pin(11, Pin.OUT), 720, Pin(26, Pin.IN, Pin.PULL_UP)),
  ("4th Floor", Pin(10, Pin.OUT), 1080, Pin(22, Pin.IN, Pin.PULL_UP)),
  ("5th Floor", Pin(9, Pin.OUT), 1440, Pin(28, Pin.IN, Pin.PULL_UP)),
  ("6th Floor", Pin(8, Pin.OUT), 1800, Pin(27, Pin.IN, Pin.PULL_UP)),
]

step = Pin(17, Pin.OUT)
direction = Pin(16, Pin.OUT)

i2c = I2C(0, sda=Pin(0), scl=Pin(1), freq=400000)

I2C_ADDR = i2c.scan()[0]
lcd = I2cLcd(i2c, I2C_ADDR, 2, 16)

current_step = 0
 
def stepper (steps, dir='clockwise'):
  
  # elevator going up
  if dir == 'clockwise':
    direction.value(0)
    for i in range(0, steps):
      step.value(1)
      sleep_us(2000)
      step.value(0)
      # current_step = i + 1

  # elevator going down
  if dir == 'counter clockwise':
    direction.value(1)
    for i in range(steps - 1, -1, -1):
      step.value(1)
      sleep_us(2000)
      step.value(0)
      # current_step = i

def going_up(input_floor):
  stepper(input_floor)

def going_down(input_floor):
  stepper(input_floor, 'counter clockwise')

def handle_led(current_step, floor_led, floor_value):
  if current_step == floor_value:
    floor_led.value(1)
  else:
    floor_led.value(0)


while True:
  for floor_desc, floor_led, floor_value, floor_button in floors:
    handle_led(current_step, floor_led, floor_value)
    
    if floor_button.value() == 0:
      lcd.putstr("Going to \n" + floor_desc)
      sleep(0.5)
      lcd.clear()
      
      if current_step < floor_value:
        going_up(floor_value - current_step)
        current_step = floor_value
        lcd.clear()
        lcd.putstr("Arrived at \n" + floor_desc)
        sleep(0.5)
        lcd.clear()
      else:
        going_down(abs(floor_value - current_step))
        current_step = floor_value
      lcd.clear()
      lcd.putstr("Arrived at \n" + floor_desc)
      sleep(0.5)
      lcd.clear()
      
     

  sleep_ms(200)