import time

from compLib.CompLibClient import CompLibClient


# def send(data, size):
#     with socket.socket(socket.AF_UNIX, socket.SOCK_STREAM) as sock:
#         sock.connect(SOCKET_PATH)
#         sock.sendall(size.to_bytes(1, byteorder='big'))
#         sock.sendall(data)
#
#         response_size_bytes = sock.recv(1)
#         response_size = int.from_bytes(response_size_bytes, byteorder="big")
#         # print(f"Response size: {response_size}")
#
#         response_bytes = sock.recv(response_size)
#         generic_response = CompLib_pb2.GenericResponse()
#
#         generic_response.ParseFromString(response_bytes)
#         # print(f"Response: {generic_response}")
#
#         # reponseBytes =


def main():
    # encoder_read_positions_request = CompLib_pb2.EncoderReadPositionsRequest()
    # # readSensorsRequest.header = CompLib_pb2.Header()
    # encoder_read_positions_request.header.message_type = encoder_read_positions_request.DESCRIPTOR.full_name
    #
    # start_time = time.time()
    # for i in range(100000):
    #     send(encoder_read_positions_request.SerializeToString(), encoder_read_positions_request.ByteSize())
    # print("--- %s seconds ---" % (time.time() - start_time))

    from compLib.IRSensor import IRSensor
    IRSensor.enable()

    startTime = time.time()
    while time.time() - startTime < 10:
        print(IRSensor.read_all())
        time.sleep(0.01)

    # from compLib.Encoder import Encoder
    # print(Encoder.read_all_positions())
    # print(Encoder.read_all_velocities())

    # from compLib.Motor import Motor

    # Motor.speed(0, -50)
    # Motor.speed(3, 50)

    # Motor.power(0, -50)
    # Motor.power(3, 50)

    # time.sleep(5)


#
# import time
# time.sleep(2)
#
# Motor.speed(0, 0)
# Motor.speed(3, -0)

# Motor.power(0, 0)
# Motor.power(3, 0)

# import math
# from compLib.Movement import Movement
# Movement.turn_degrees(90, math.pi * 2)
# Movement.turn_degrees(-90, math.pi * 2)
#
# Movement.turn_degrees(90, math.pi * 2)
# Movement.turn_degrees(90, -math.pi * 2)
#
# Movement.turn_degrees(90, math.pi * 2)
# Movement.turn_degrees(-90, -math.pi * 2)

# from compLib.Movement import Movement
# Movement.drive_distance(0.1, 0.5)
# Movement.drive_distance(-0.1, 0.5)
#
# Movement.drive_distance(0.1, 0.5)
# Movement.drive_distance(0.1, -0.5)
#
# Movement.drive_distance(0.1, 0.5)
# Movement.drive_distance(-0.1, -0.5)

# from compLib.Movement import Movement
# import math
# import time
# Movement.drive_distance(0.5, 0.5)
# time.sleep(1)
# Movement.turn_degrees(90, math.pi * 2)
# time.sleep(1)
#
# Movement.drive_distance(0.5, 0.5)
# time.sleep(1)
# Movement.turn_degrees(90, math.pi * 2)
# time.sleep(1)
#
# Movement.drive_distance(0.5, 0.5)
# time.sleep(1)
# Movement.turn_degrees(90, math.pi * 2)
# time.sleep(1)
#
# Movement.drive_distance(0.5, 0.5)
# time.sleep(1)
# Movement.turn_degrees(90, math.pi * 2)
# time.sleep(1)

# import time
#
# from compLib.IRSensor import IRSensor
# from compLib.Motor import Motor
#
# IRSensor.enable()
#
# DRIVE_SPEED = 2.0
# COLOR_BREAK = 900
# KP = 0.25
# KD = 0.0
#
#
# def drive(leftSpeed, rightSpeed):
#     Motor.speed(0, -rightSpeed)
#     Motor.power(3, leftSpeed)
#
#
# def follow(sleepTime=0.1):
#     lastError = 0
#     sensorsBlack = 0
#
#     while sensorsBlack < 3:
#         data = IRSensor.read_all()
#
#         sensorsBlack = 0
#         for i in range(len(data)):
#             if data[i] > COLOR_BREAK:
#                 sensorsBlack += 1
#
#         error = lastError
#         if data[2] > COLOR_BREAK:
#             error = 0
#         elif data[0] > COLOR_BREAK:
#             error = -1.5
#         elif data[4] > COLOR_BREAK:
#             error = 1.5
#         elif data[1] > COLOR_BREAK:
#             error = -1
#         elif 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)
#
#     drive(0, 0)
#     time.sleep(sleepTime)


if __name__ == '__main__':
    CompLibClient.use_tcp_socket("dev03.local")
    # follow()
    main()