import time

from compLib.CompLibClient import CompLibClient
from compLib.IRSensor import IRSensor
from compLib.Motor import Motor

DRIVE_SPEED = 5.0
COLOR_BREAK = 1500.0
KP = 2.0
KD = 0.0


def drive(left_speed, right_speed):
    print(left_speed, right_speed)

    right_speed *= -1.0

    Motor.speed(0, right_speed)
    Motor.speed(3, left_speed)


def follow(sleep_time=0.1):
    last_error = 0
    sensors_black = 0
    while sensors_black <= 3:
        sensor_values = IRSensor.read_all()

        sensors_black = 0
        for sensor in sensor_values:
            if sensor > COLOR_BREAK:
                sensors_black += 1

        error = last_error
        if sensor_values[2] > COLOR_BREAK:
            error = 0
        elif sensor_values[0] > COLOR_BREAK:
            error = -1.5
        elif sensor_values[4] > COLOR_BREAK:
            error = 1.5
        elif sensor_values[1] > COLOR_BREAK:
            error = -1
        elif sensor_values[3] > COLOR_BREAK:
            error = 1
        elif error == 1.5:
            error = 3.5
        elif error == -1.5:
            error = -3.5

        last_error = error

        adjustment = KP * error + KD * (error - last_error)
        left_speed = DRIVE_SPEED + adjustment
        right_speed = DRIVE_SPEED - adjustment

        print(sensor_values)
        print(f"{left_speed} {right_speed} {adjustment} {error}")
        drive(left_speed, right_speed)

    drive(0, 0)
    time.sleep(sleep_time)


def follow_simple():
    left_speed = DRIVE_SPEED
    right_speed = DRIVE_SPEED
    sensor_values = IRSensor.read_all()
    while True:
        sensor_values = IRSensor.read_all()
        # for i in range(len(sensor_values)):
        #     sensor_values[i] = (sensor_values[i] + new_sensor_values[i]) / 2.0

        print(sensor_values)

        if sensor_values[0] > COLOR_BREAK and sensor_values[4] > COLOR_BREAK:
            break

        if sensor_values[0] > COLOR_BREAK:
            left_speed = -DRIVE_SPEED / 2
            right_speed = DRIVE_SPEED
        elif sensor_values[4] > COLOR_BREAK:
            left_speed = DRIVE_SPEED
            right_speed = -DRIVE_SPEED / 2
        elif sensor_values[2] > COLOR_BREAK:
            left_speed = DRIVE_SPEED
            right_speed = DRIVE_SPEED

        drive(left_speed, right_speed)


def main():
    CompLibClient.use_unix_socket()

    IRSensor.enable()
    time.sleep(0.1)

    # while True:
    #     print(IRSensor.read_all())

    # follow_simple()
    # drive(5, 5)
    # time.sleep(5)
    # follow()
    # follow()
    # follow()
    # follow()
    # follow(0.2)


main()