import statistics
import time

import erpc

from servo_service import client


PORT = "COM8"
BAUD = 115200
N = 180
JOINT_COUNT = 48


def main():
    xport = erpc.transport.SerialTransport(PORT, BAUD)
    client_mgr = erpc.client.ClientManager(xport, erpc.basic_codec.BasicCodec)
    api = client.servo_serviceClient(client_mgr)

    angles = [0.0] * JOINT_COUNT

    while True:
        costs_ms = []

        for _ in range(N):
            t0 = time.perf_counter()
            api.moveJ(angles)
            t1 = time.perf_counter()
            costs_ms.append((t1 - t0) * 1000.0)

        print(f"N={N}")
        print(f"avg = {statistics.mean(costs_ms):.3f} ms")
        print(f"min = {min(costs_ms):.3f} ms")
        print(f"max = {max(costs_ms):.3f} ms")
        if N >= 2:
            print(f"p50 = {statistics.median(costs_ms):.3f} ms")
            print(f"p95 ~= {sorted(costs_ms)[int(0.95 * (N - 1))]:.3f} ms")


if __name__ == "__main__":
    main()