#!/usr/bin/env python3
from ev3dev2.motor import MoveTank, OUTPUT_B, OUTPUT_C
from ev3dev2.sensor.lego import ColorSensor, TouchSensor, UltrasonicSensor
from ev3dev2.button import Button
from ev3dev2.sound import Sound


btn = Button()
tank_pair = MoveTank(OUTPUT_B, OUTPUT_C)
ts = TouchSensor()
cl = ColorSensor()
us = UltrasonicSensor()
sound = Sound()




def correct_angle(direction):
    # correct_angle reverses to then last the known black tile and then turns either left or right depending
    # the parameter direction
    tank_pair.on_for_rotations(-20, -20, 0.8)

    if direction == 1:
        tank_pair.on_for_rotations(0, 5, 0.1)
    else:
        tank_pair.on_for_rotations(5, 0, 0.1)



def search():
    # search looks 0.8 rotations ahead for the next black tile
    tank_pair.on_for_rotations(20, 20, 0.8)
    if cl.reflected_light_intensity < 17:
        sound.beep()
        return True
    else:
        return False



def find_black():
    # find black first assumes robot is on correct path so calls search method straight away
    # if this fails to find the next black tile then it begins correcting its angle by 9 degrees
    # first to the right then back to the left
    if search():
        return True
    else:
        failed_attemps = 0
        # this means it didnt find a black tile where it expected to so it needs to correct its angle
        while failed_attemps < 2:
            correct_angle(1)
            if search():
                return True
            else:
                failed_attemps += 1

        failed_attemps = 0
        while failed_attemps < 4:
            correct_angle(0)
            if search():
                return True
            else:
                failed_attemps += 1



def tower():
    tank_pair.on_for_rotations(15, 0, 1)
    us.mode = 'US-DIST-CM'
    tank_pair.on_for_rotations(20,20, 13)

    distance = us.value()/10

    while distance > 70:
        tank_pair.on_for_rotations(0,10,0.5)
        distance = us.value()/10



    while not ts.value():
        tank_pair.on_for_rotations(20,20,1)

    tank_pair.on_for_rotations(-10,-10,1)

    while cl.reflected_light_intensity > 40:
        tank_pair.on_for_rotations(100,100,1)

    """
    while not ts.value() and cl.reflected_light_intensity > 50:
        tank_pair.on_for_rotations(50,50,1)
    """








    sound.beep("Finished")



def move_forward(tiles):
    count = 0

    while count < tiles:

        # While we are over black just go forward
        while cl.reflected_light_intensity < 17:
            tank_pair.on(10, 10)

        # When we are no longer over a black tile we must search for the next one
        tank_pair.off()
        if find_black() == True:
            count += 1




def turn_right():
    tank_pair.on_for_rotations(20, 0, 1)



move_forward(1)
turn_right()
move_forward(14)
tank_pair.on_for_rotations(-15, -15, 0.4)
tower()
sound.speak("complete")