#!/usr/bin/env python3
import paho.mqtt.client as mqtt
from ev3dev2.motor import LargeMotor, OUTPUT_A, OUTPUT_B, SpeedPercent, MoveTank
from ev3dev2.sensor import INPUT_1
from ev3dev2.sensor.lego import TouchSensor
from ev3dev2.sensor.lego import ColorSensor
from ev3dev2.sensor.lego import LightSensor
from ev3dev2.led import Leds
from ev3dev2.sound import Sound
import time

def sprava(optsenz, otacl, otacp):
   return "Sprava %i %i %i" % (optsenz, otacl, otacp)

print("ZACIATOK")

client = mqtt.Client()
client.connect("192.168.1.111", 1883,60) # adresa HP pocitaca
svetlo = ColorSensor('in4')
tendruhy = LightSensor('in3')

# drive in a turn for 5 rotations of the outer motor
# the first two parameters can be unit classes or percentages.

# drive in a different turn for 3 seconds
#tank_drive.on_for_seconds(SpeedPercent(60), SpeedPercent(30), 0.1)

Lavy = LargeMotor('outA')
Pravy = LargeMotor('outB')
time.sleep(5)
for i in range(0,1000):
   #print(svetlo.ambient_light_intensity)
   lf = svetlo.reflected_light_intensity
   td = tendruhy.ambient_light_intensity
   #print("%i Line Follower %i TenDruhy %i" % (i,lf,td))
   while svetlo.reflected_light_intensity < 5:
      Lavy.run_timed(time_sp = 20, speed_sp=1000)
      time.sleep(0.2)
   while svetlo.reflected_light_intensity > 4:
      Pravy.run_timed(time_sp = 20, speed_sp=1000)
      time.sleep(0.2)
   sp = sprava(td, Lavy.position, Pravy.position)
   client.publish("topic/test", sp)
   print(sp)