#!/usr/bin/env python3

import socket
import sys
import time
import threading
import evdev
import ev3dev.auto as ev3
import time
from ev3dev.ev3 import *
from ev3dev2.motor import LargeMotor
from ev3dev2.motor import SpeedDPS, SpeedRPM, SpeedRPS, SpeedDPM


def main():
    lm = LargeMotor('outA')  # inicializuj motor
    lm.stop(stop_action="hold")
    us = UltrasonicSensor()  # inicializuj senzor
    us.mode = 'US-DIST-CM'
    d = 0
    while True:
        if abs(us.value()-d) < 5:  # ak sa vzdialenost nezmenila
            continue    		   # nemusis hladat figurku
        mini = 2000			# ake je minimum
        mpos = lm.position  # kde je minimum
        rel0 = lm.position  # odcitaj momentalnu poziciu
        lm.on_for_seconds(speed=-10, seconds=0.4)  # pohni sa dolava
        lm.wait_while('running')
        rel1 = lm.position
        lm.run_forever(speed_sp=100)
        while lm.position < rel0+(rel0-rel1)-2:  # najdi minimum v intervali
            uval = us.value()                    # a zapamataj si kde si ho
            print(uval)                          # nasiel
            if mini > uval:
                mini = uval
                mpos = lm.position
        lm.stop(stop_action="hold")
        print(lm.position, rel1, rel0, rel0+(rel0-rel1))
        lm.run_forever(speed_sp=-100)  # ideme na poziciu s
        while lm.position > mpos:        # minimalnou vzdialenostou
            pass
        lm.stop(stop_action="hold")
        print(mpos, rel0, us.value())
        print("	", mini)
        d = mini
        time.sleep(0.1)

if __name__ == '__main__':
    main()