import SerialHandler
import threading
import time

serialHandler = SerialHandler.SerialHandler("/dev/ttyACM0")
serialHandler.sendPidActivation(True)

ev1 = threading.Event()
serialHandler.readThread.addWaiter("MCTL", ev1)
ev2 = threading.Event()
serialHandler.readThread.addWaiter("BRAK", ev2, print)

sent = serialHandler.sendMove(-0.5,5.0)
if sent:
    isConfirmed = ev1.wait(timeout=1.0)
    ev1.clear()
    if(isConfirmed):
        print("\tMoving was confirmed!")
    else:
        try:
            raise ConnectionError('1', 'Response was not received!')
        except Exception as e:
            print (e)

else:
    print("Sending problem")
time.sleep(3.0)
sent = serialHandler.sendBrake(0.0)
if sent:
    isConfirmed = ev2.wait(timeout=1.0)
    ev2.clear()
    if(isConfirmed):
        print("\tBraking was confirmed!")
    else:
        try:
            raise ConnectionError('2', 'Response was not received!')
        except Exception as e:
            print (e)
else:
    print("Sending problem")

serialHandler.sendBezierCurve(1+1j,1.56+0.44j,1.56-0.44j,1.0-1.0j,6.0,True)
serialHandler.sendPidActivation(False)