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

import time
import math

DRIVE_SPEED = 20.0
COLOR_BREAK = 850
KP = 7.5
KD = 0.0

SAMPLE_TIME_S = 0.001
CUTOFF_FREQ_HZ = 50.0

RC = 1.0 / (2.0 * math.pi * CUTOFF_FREQ_HZ)

FIRST_COEFF = SAMPLE_TIME_S / (SAMPLE_TIME_S + RC)
SECOND_COEFF = RC / (SAMPLE_TIME_S + RC)

out_old = 0.0
start_time = time.time()


def drive(leftSpeed, rightSpeed):
    rightSpeed *= -0.906

    Motor.multiple_power((0, min(max(-100, rightSpeed), 100)), (3, min(max(-100, leftSpeed), 100)))

def follow(sleepTime = 0.3):
    global out_old
    lastError = 0
    sensorsBlack = 0
    while sensorsBlack < 3:
        sensor_data = IRSensor.read_all()

        sensorsBlack = 0
        for i in range(0, 5):
            if sensor_data[i] > COLOR_BREAK:
                sensorsBlack += 1


        middle_sensor = sensor_data[2]
        filtered_sensor = FIRST_COEFF * middle_sensor + SECOND_COEFF * out_old
        out_old = filtered_sensor

        sample_time = str(time.time() - start_time).replace(".", ",")
        print(f"{sample_time} {middle_sensor} {int(filtered_sensor)}")

        error = lastError
        if sensor_data[2] > COLOR_BREAK:
            error = 0
        elif sensor_data[0] > COLOR_BREAK:
            error = -1.5
        elif sensor_data[4] > COLOR_BREAK:
            error = 1.5
        elif sensor_data[1] > COLOR_BREAK:
            error = -1
        elif sensor_data[3] > COLOR_BREAK:
            error = 1
        elif error == 1.5:
            error = 3
        elif error == -1.5:
            error = -3

        lastError = error

        adjustment = KP * error + KD * (error - lastError)
        leftSpeed = DRIVE_SPEED + adjustment
        rightSpeed = DRIVE_SPEED - adjustment

        # print(f"{leftSpeed} {rightSpeed} {adjustment} {error}")
        drive(leftSpeed, rightSpeed)

        time.sleep(SAMPLE_TIME_S)

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

def main():
    CompLibClient.use_unix_socket()

    IRSensor.enable()
    time.sleep(0.5)

    # while True:
    #     print(IRSensor.read_all())
    #     time.sleep(0.01)

    follow()
    follow()
    follow()
    follow()
    follow(0.2)

main()