"""Programming O1."""

from PiBot import PiBot
import rospy

robot = PiBot()  # The only global variable that this program will use.


def noise_filter(value, value_buffer):
    av = 0
    for el in value_buffer:
        av += el / len(value_buffer)
    clean_value = av
    del value_buffer[0]
    value_buffer.append(value)
    return clean_value, value_buffer


def create_list(length: int, value=None):
    """
    Create a list for keeping track of previous IR values and fill it with placeholder values.

    If value is None; start tracking and fill the list with 'length' amount of front IR sensor values.
    Else, fill it with the value 'length' times.
    Returns a tuple in the form of: (index = 0, list)
    """
    if value is None:
        return (0, [robot.get_front_middle_ir() for _ in range(length)])
    return (0, [value for _ in range(length)])


def update_previous_values(index: int, previous_values: list):
    """Update the values of front middle ir sensor and store it."""
    previous_values[index] = robot.get_front_middle_ir()
    index = (index + 1) % len(previous_values)
    return index, previous_values


def set_speeds(left: int, right: int) -> None:
    """Set the wheels speed."""
    robot.set_left_wheel_speed(left)
    robot.set_right_wheel_speed(right)


'''
def fix_speeds(left_encoder_start: int, right_encoder_start: int) -> None:
    """Balance the speed of the wheels to be equal."""
    left_encoder = abs(robot.get_left_wheel_encoder()) - abs(left_encoder_start)
    right_encoder = abs(robot.get_right_wheel_encoder()) - abs(right_encoder_start)
    delta = left_encoder / right_encoder
    if delta == 1:  # Wheels have moved in sync.
        left_speed = default_speed
        right_speed = default_speed
    elif delta < 1:  # Right encoder has a higher value - needs to move a bit more to the right.
        left_speed = int(default_speed * delta)
        right_speed = int(default_speed * (2 - delta))
    else:  # Left encoder has a higher value - needs to move a bit more to the left.
        left_speed = int(default_speed * (2 - delta))
        right_speed = int(default_speed * delta)
    set_speeds(left_speed, right_speed)
'''


def find_object(previous_values: list, deviation: float) -> None:
    """Find the object by spinning around until a sharp jump in the front IR distance is noticed when compared to the average of the last values."""
    while True:
        clean_value, previous_values = noise_filter(robot.get_front_middle_ir(), previous_values)
        result = abs(clean_value - previous_values[0])
        if result > deviation:
            print("Object found.")
            break
        #index, previous_values = update_previous_values(index, previous_values)


def refind_object(left_speed: int, right_speed: int, average: int, attempts: int, deviation: int) -> bool:
    """
    Find the object, judged by a sharp jump in the front IR distance.

    Each "attempt" occurs in 0.1 second intervals.
    """
    set_speeds(left_speed, right_speed)
    for attempt in range(attempts):
        rospy.sleep(0.1)
        value = robot.get_front_middle_ir()
        if abs(value - average) < deviation:
            print("Object FOUND.")
            return True
    print("Object NOT FOUND.")
    return False


def drive_towards_object(left_speed: int, right_speed: int, index: int, previous_values: list, deviation: int) -> None:
    """
    Drive towards the object.

    In case of driving forward unevenly (too much leaning to either side - front IR sensor loses object), refind the object.
    """
    set_speeds(left_speed, right_speed)
    while True:
        value = robot.get_front_middle_ir()
        if value < 0.16:  # Break condition - robot has reached the object.
            print("Object reached.")
            break
        average = sum(previous_values) / len(previous_values)
        if abs(value - average) > deviation:  # Steered away from the object?
            print("Object lost.")
            #  See if the object is to the right.
            print("Seeing if the object is to the right...")
            if not refind_object(11, -11, average, 5, deviation):
                #  See if the object is to the left.
                print("Seeing if the object is to the left...")
                if not refind_object(-11, 11, average, 10, deviation):
                    set_speeds(0, 0)
                    print("Couldn't refind the object. Exiting prematurely.")
                    exit(1)
            set_speeds(left_speed, right_speed)
        index, previous_values = update_previous_values(index, previous_values)


def reverse() -> None:
    """
    Reverse until the robot is close enough to the object to consider the objective completed.

    This function will also refind the object automatically, in case it is lost.
    """
    while robot.get_rear_left_straight_ir() > 0.05:  # While the robot is not close enough...
        while robot.get_rear_left_straight_ir() == 0.16:  # If the object has been lost...
            set_speeds(11, -11)  # Steer until the object is found again.
        set_speeds(-11, -11)


def main():
    """Main function of the program."""
    default_speed = 20
    deviation = 0.2
    set_speeds(11, -11)
    index, previous_values = create_list(5)

    #  1. Seek a "jump" in the front IR distance (object found?).
    print("Attempting to find object...")
    find_object(previous_values, deviation)
    #  Overwrite the list with a shorter one.
    index, previous_values = create_list(5, robot.get_front_middle_ir())
    #  2. Drive towards the object.
    print("Driving towards the object...")
    drive_towards_object(default_speed, default_speed, index, previous_values, deviation)
    #  3. Move even closer using more precise back sensor.
    print("Reversing...")
    reverse()

    print("Done")
    set_speeds(0, 0)


if __name__ == "__main__":
    main()