import time
import sys
import signal

from PyMata.pymata import PyMata

def signal_handler(sig, frame):
    print('You pressed Ctrl+C')
    if board is not None:
        board.reset()
        board.close()
    sys.exit(0)


def loupiote(board, distance):
    mini = 3
    maxi = 20
    if distance > maxi:
        distance = maxi
    if distance < mini:
        distance = mini
    frac = (distance - mini) / (maxi - mini)
    board.analog_write(3, 255)
    board.analog_write(5, int(255 * (1- frac)))
    board.analog_write(6, int(255 * frac))


board = PyMata("/dev/ttyUSB0", verbose=True, baud_rate=115200)
signal.signal(signal.SIGINT, signal_handler)

board.sonar_config(12, 12)
board.set_pin_mode(3, PyMata.PWM, PyMata.DIGITAL)
board.set_pin_mode(5, PyMata.PWM, PyMata.DIGITAL)
board.set_pin_mode(6, PyMata.PWM, PyMata.DIGITAL)

time.sleep(1)

while True:
    data = board.get_sonar_data()
    distance = data[12][1]
    print(distance)
    loupiote(board, distance)
    time.sleep(.2)