#!/usr/bin/env python3
from ev3dev2.motor import MediumMotor,OUTPUT_B,LargeMotor,OUTPUT_D,OUTPUT_C,MoveTank
from ev3dev2.sensor.lego import InfraredSensor,TouchSensor
from ev3dev2.display import Display
from time import sleep
from threading import Thread

ts = TouchSensor()
lcd = Display()
tank_pair = MoveTank(OUTPUT_D,OUTPUT_C)
medium_motor = MediumMotor(OUTPUT_B)
ir = InfraredSensor()
pravy_motor = LargeMotor(OUTPUT_D)
lavy_motor = LargeMotor(OUTPUT_C)
premena = 0

def aky_pohyb():
    global premena
    global vyberanie
    while ts.is_pressed == False:
        lcd.update()
        sleep(1)
        lcd.clear()
        lcd.text_pixels(str(premena),False,45,25,font = 'courB24')
        if premena < 2:
            sleep(1)
            premena += 1


def meranie():
    while True:
        distance = ir.distance()
        if distance:
            medium_motor.on(speed=2*(ir.heading()-0))
        else:
            medium_motor.off()
def meranie_2():
    while True:
        distance = ir.distance()
        if distance:
            medium_motor.on(speed=5*(ir.heading()-0))
        else:
            medium_motor.off()
def pohyb():
    while True:     
        tank_pair.on_for_degrees(-5,5,180)
        tank_pair.on_for_degrees(5,-5,180)
def pohyb_1():
    while True:     
        tank_pair.on_for_degrees(-15,15,180)
        tank_pair.on_for_degrees(15,-15,180)
def pohyb_2():
    while True:     
        tank_pair.on_for_degrees(-5,5,90)
        tank_pair.on_for_degrees(5,-5,90)
        tank_pair.on_for_degrees(-15,15,180)
        tank_pair.on_for_degrees(15,-15,180)


aky_pohyb()

lcd.update()
sleep(1)
lcd.clear()
lcd.text_pixels(str(premena),False,45,25,font = 'courB24')

if premena == 0:
    if __name__ == '__main__':
        Thread(target = meranie).start()
        Thread(target = pohyb).start()
elif premena == 1:
    if __name__ == '__main__':
        Thread(target = meranie_2).start()
        Thread(target = pohyb_1).start()
elif premena == 2:
    if __name__ == '__main__':
        Thread(target = meranie_2).start()
        Thread(target = pohyb_2).start()