from machine import Pin, SoftI2C
import ssd1306
from hcsr04 import HCSR04
from time import sleep, sleep_us

i2c = SoftI2C(scl=Pin(22), sda=Pin(21))
oled = ssd1306.SSD1306_I2C(128, 64, i2c)

sensor1 = HCSR04(trigger_pin=27, echo_pin=26)
sensor2 = HCSR04(trigger_pin=13, echo_pin=12)

ir_sensor1 = Pin(35, Pin.IN)
ir_sensor2 = Pin(34, Pin.IN)

dir_pin = Pin(5, Pin.OUT)
step_pin = Pin(18, Pin.OUT)

LED1 = Pin(4, Pin.OUT)
LED2 = Pin(2, Pin.OUT)

def read_distance(sensor):
    return sensor.distance_cm()

def display_info(distance1, distance2, volume1, volume2):
    oled.fill(0)
    if distance1 < 100:
        oled.text("Tank 1: {}L".format(volume1), 0, 20)
    else:
        oled.text("Tank 1: EMPTY", 0, 20)
    
    if distance2 < 100:
        oled.text("Tank 2: {}L".format(volume2), 0, 30)
    else:
        oled.text("Tank 2: EMPTY", 0, 30)
    oled.show()

def control_pump(distance1, distance2):
    dist1 = int(distance1)
    dist2 = int(distance2)
    if dist1 > dist2 and dist1 > 3:
        move_stepper(1,200,2000,5)
        LED1.on()
    
    elif dist2 > dist1 and dist2 > 3:
        move_stepper(0,200,2000,5)
        LED2.on()
    
    elif dist2 == dist1 or dist1 == dist2:
        stop_stepper()
        LED1.off()
        LED2.off()

def move_stepper(direct,steps, delay, accel):
	dir_pin.value(direct)
	steps = abs(steps)
	for i in range(steps):
		step_pin.value(1)
		sleep_us(delay)
		step_pin.value(0)
        sleep_us(delay)

def stop_stepper():
    step_pin.value(0)

def cal_vol(distance):
    Vol = ((100 - distance)*40*40)/1000
    Vol1 = round(Vol, 2)
    return Vol1

while True:
    distance1 = read_distance(sensor1)
    distance2 = read_distance(sensor2)
    volume1 = cal_vol(distance1)
    volume2 = cal_vol(distance2)
    control_pump(distance1, distance2)
    display_info(distance1, distance2, volume1, volume2)