import asyncio
import random
import user.lib.controller as controller
from fischertechnik.controller.Motor import Motor
from fischertechnik.logging import log as print

speed = None
directions = None


async def backward(speed):
    global directions
    controller.RX_M1_motor.set_speed(int(speed), Motor.CW)
    controller.RX_M1_motor.start()
    controller.RX_M2_motor.set_speed(int(speed), Motor.CW)
    controller.RX_M2_motor.start()


async def forward(speed):
    global directions
    controller.RX_M1_motor.set_speed(int(speed), Motor.CCW)
    controller.RX_M1_motor.start()
    controller.RX_M2_motor.set_speed(int(speed), Motor.CCW)
    controller.RX_M2_motor.start()


async def turn_left():
    global speed, directions
    controller.RX_M1_motor.set_speed(int(250), Motor.CW)
    controller.RX_M1_motor.start()
    controller.RX_M2_motor.set_speed(int(250), Motor.CCW)
    controller.RX_M2_motor.start()


async def turn_right():
    global speed, directions
    controller.RX_M1_motor.set_speed(int(250), Motor.CCW)
    controller.RX_M1_motor.start()
    controller.RX_M2_motor.set_speed(int(250), Motor.CW)
    controller.RX_M2_motor.start()


async def stop():
    global speed, directions
    controller.RX_M1_motor.stop()
    controller.RX_M2_motor.stop()


async def follow_line():
    global speed, directions
    while ((controller.RX_I7_trail_follower.get_state() == 0) or (controller.RX_I8_trail_follower.get_state() == 0)) and (controller.RX_I2C_1_gesture_sensor.get_proximity()) < 240:
        if controller.RX_I8_trail_follower.get_state() == 1:
            await turn_left()
            await asyncio.sleep(0.05)
        if controller.RX_I7_trail_follower.get_state() == 1:
            await turn_right()
            await asyncio.sleep(0.05)
        if (controller.RX_I7_trail_follower.get_state() == 0) and (controller.RX_I8_trail_follower.get_state() == 0):
            await forward(250)
        await asyncio.sleep(0.01)
    if (controller.RX_I2C_1_gesture_sensor.get_proximity()) >= 240:
        await backward(250)
        await asyncio.sleep(1)
        if 1 == random.randint(1, 2):
            directions = 'Right'
            await turn_right()
        else:
            directions = 'Left'
            await turn_left()
        await asyncio.sleep(0.5)


async def find_line(directions):
    global speed
    while (controller.RX_I7_trail_follower.get_state() == 1) and (controller.RX_I8_trail_follower.get_state() == 1):
        if directions == 'Right':
            await turn_right()
        else:
            await turn_left()
        await asyncio.sleep(0.01)


async def run(instance):
    global speed, directions
    await controller.init(instance)
    controller.RX_I2C_1_gesture_sensor.enable_proximity()
    directions = 'Left'
    while True:
        await find_line(directions)
        await follow_line()
        await asyncio.sleep(0.01)