#!/usr/bin/env pybricks-micropython
from pybricks.hubs import EV3Brick
from pybricks.ev3devices import (Motor, TouchSensor, ColorSensor,
                                 InfraredSensor, UltrasonicSensor, GyroSensor)
from pybricks.parameters import Port, Stop, Direction, Button, Color
from pybricks.tools import wait, StopWatch, DataLog
from pybricks.robotics import DriveBase
from pybricks.media.ev3dev import SoundFile, ImageFile
from umqtt.robust import MQTTClient
import time


# This program requires LEGO EV3 MicroPython v2.0 or higher.
# Click "Open user guide" on the EV3 extension tab for more information.

# MQTT setup
MQTT_ClientID = 'bob'
MQTT_Broker = 'marcelus.net'
MQTT_Topic_Status = 'move'
client = MQTTClient(MQTT_ClientID, MQTT_Broker, 1883)

# Objects
ev3 = EV3Brick()
left_motor = Motor(Port.B)
right_motor = Motor(Port.C)
bob = DriveBase(left_motor, right_motor, wheel_diameter = 54, axle_track = 121)
UltraSensor = UltrasonicSensor(Port.S4)
get_tick_rate = 0.1
send_tick_rate = 0.5
sensitivity = 400
safe_distance = 200
ultraSend = [True]


# Functions
def listen(topic, msg) :
    if topic == MQTT_Topic_Status.encode() :
        data = str(msg.decode())
        ev3.screen.print(data)
        if data == "STOP" :
            bob.stop()
        if data == "ULT1" :
            ultraSend[0] = True
        if data == "ULT0" :
            ultraSend[0] = False
        if data[0] == 'T' :
            bob.turn(int(data[1:]))
        if data[0] == 'D' :
            bob.drive(int(data[1:]),0)



def publish(data) :
    client.publish(MQTT_Topic_Status, str(data))
    # debug
    print(data)

def sendUltra() :
    dist = UltraSensor.distance()
    if (dist < sensitivity):
        publish(dist)
    if (dist < safe_distance) :
        bob.stop()
        publish('STOP')


# MQTT connection
client.connect()
time.sleep(0.5)
ev3.screen.print("Connected")
client.set_callback(listen) 
client.subscribe(MQTT_Topic_Status) 
time.sleep(0.5)
ev3.screen.print("Listening...")

# Code
############################################################

# Start is used for the tick_rate to send and get messages
start = time.time()
# ult is used to differentiate the get and send tick rate
ult = None

while True :
    if (time.time() > start + get_tick_rate) :
        client.check_msg()
        if (ultraSend[0]) :
            if (ult == None) :
                ult = start
            if (ult != None and time.time() > ult + send_tick_rate) :
                sendUltra()
                ult = None
        start = time.time()