import RPi.GPIO as GPIO
import pigpio
import time
import sys

TRIGER = 23
ECHO = 24
SERVO = 18
TRIGER2 = 25
ECHO2 = 26
SERVO2 = 19
MAX_DISTANCE = 100.0

data = [[0 for j in range(2)] for i in range(10)]
data2 = [[0 for j in range(2)] for i in range(10)]

GPIO.setmode(GPIO.BCM)
GPIO.setwarnings(False)
GPIO.setup(TRIGER, GPIO.OUT)
GPIO.setup(ECHO, GPIO.IN)
GPIO.setup(TRIGER2, GPIO.OUT)
GPIO.setup(ECHO2, GPIO.IN)

pi = pigpio.pi()
pi.set_mode(SERVO, pigpio.OUTPUT)
pi2 = pigpio.pi()
pi2.set_mode(SERVO2, pigpio.OUTPUT)

x = -90

def read_distance(tn, en):
    GPIO.output(tn, GPIO.HIGH)
    time.sleep(0.00001)
    GPIO.output(tn, GPIO.LOW)
    
    while GPIO.input(en) == GPIO.LOW:
        sig_off = time.time()
    while GPIO.input(en) == GPIO.HIGH:
        sig_on = time.time()
    
    duration = (sig_on - sig_off)
    distance = duration * 34000 / 2
    return distance

def servo_angle(angle):
    duty = 2.5 + (12.0 - 2.5) * (angle + 90) / 180
    pi.hardware_PWM(SERVO, 50, int(duty * 10000))

def servo2_angle(angle):
    duty = 2.5 + (12.0 - 2.5) * (angle + 90) / 180
    pi2.hardware_PWM(SERVO2, 50, int(duty * 10000))
    time.sleep(0.05)

def get_min_data():
    minDt = 1000
    dig = 0
    for i in range(10):
        if data[i][1] != -1 and data[i][1] < minDt:
            minDt = data[i][1]
            dig = data[i][0]
        if data2[i][1] != -1 and data2[i][1] < minDt:
            minDt = data2[i][1]
            dig = data2[i][0]
    if minDt == 1000:
        minDt = -1
    return dig, minDt

try:
    while True:
        for i in range(10):
            servo_angle(x)
            servo2_angle(x+90)
            cm = read_distance(TRIGER, ECHO)
            cm2 = read_distance(TRIGER2, ECHO2)
            if cm >= 2.0 and cm <= MAX_DISTANCE:
                dist = cm
            else:
                dist = -1
            data[i][0] = x
            data[i][1] = dist
            if cm2 >= 2.0 and cm2 <= MAX_DISTANCE:
                dist2 = cm2
            else:
                dist2 = -1
            data2[i][0] = x+90
            data2[i][1] = dist2
            x += 10
        x = -90
        servo_angle(x)
        servo2_angle(x+90)
        time.sleep(0.2)
        dx = get_min_data()
        if dx[1] == -1:
            print('NO DETECT')            
        else:
            print('angle =', dx[0])
            print('distance =', '{:.2f}'.format(dx[1]))
            

except KeyboardInterrupt:
    pi.set_mode(SERVO,pigpio.INPUT)
    pi.stop()
    pi2.set_mode(SERVO2,pigpio.INPUT)
    pi2.stop()
    GPIO.cleanup()
    sys.exit()