import argparse
import statistics
import time

import erpc

from servo_service import client


DEFAULT_PORT = "COM8"
DEFAULT_BAUD = 115200
DEFAULT_ITERATIONS = 100
DEFAULT_JOINT_COUNT = 26
DEFAULT_WARMUP = 5


def positive_int(value):
    parsed = int(value)
    if parsed <= 0:
        raise argparse.ArgumentTypeError("value must be > 0")
    return parsed


def non_negative_float(value):
    parsed = float(value)
    if parsed < 0:
        raise argparse.ArgumentTypeError("value must be >= 0")
    return parsed


def percentile(values, ratio):
    if not values:
        raise ValueError("values must not be empty")
    ordered = sorted(values)
    index = int(ratio * (len(ordered) - 1))
    return ordered[index]


def measure_movej_latency(api, angles, iterations, warmup):
    for index in range(warmup):
        result = api.moveJ(angles)
        # if result != 0:
        #     raise RuntimeError(f"Warmup moveJ failed at iteration {index}: {result}")

    costs_ms = []
    for index in range(iterations):
        t0 = time.perf_counter()
        result = api.moveJ(angles)
        elapsed_ms = (time.perf_counter() - t0) * 1000.0
        # if result != 0:
        #     raise RuntimeError(f"moveJ failed at iteration {index}: {result}")
        costs_ms.append(elapsed_ms)
    return costs_ms


def print_summary(costs_ms):
    print(f"count = {len(costs_ms)}")
    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")
    print(f"p50   = {statistics.median(costs_ms):.3f} ms")
    print(f"p95   = {percentile(costs_ms, 0.95):.3f} ms")


def build_parser():
    parser = argparse.ArgumentParser(description="Measure moveJ RPC latency.")
    parser.add_argument("--port", default=DEFAULT_PORT, help=f"Serial port, default: {DEFAULT_PORT}")
    parser.add_argument("--baud", type=positive_int, default=DEFAULT_BAUD, help=f"Serial baud, default: {DEFAULT_BAUD}")
    parser.add_argument(
        "--iterations",
        type=positive_int,
        default=DEFAULT_ITERATIONS,
        help=f"Measured moveJ calls, default: {DEFAULT_ITERATIONS}",
    )
    parser.add_argument(
        "--warmup",
        type=positive_int,
        default=DEFAULT_WARMUP,
        help=f"Warmup moveJ calls before measuring, default: {DEFAULT_WARMUP}",
    )
    parser.add_argument(
        "--joint-count",
        type=positive_int,
        default=DEFAULT_JOINT_COUNT,
        help=f"Length of angles_rad list, default: {DEFAULT_JOINT_COUNT}",
    )
    parser.add_argument(
        "--angle-rad",
        type=non_negative_float,
        default=0.0,
        help="Angle value written into every joint slot, default: 0.0",
    )
    return parser


def main():
    args = build_parser().parse_args()
    angles = [args.angle_rad] * args.joint_count

    transport = erpc.transport.SerialTransport(args.port, args.baud)
    try:
        client_mgr = erpc.client.ClientManager(transport, erpc.basic_codec.BasicCodec)
        api = client.servo_serviceClient(client_mgr)
        print(f"port        = {args.port}")
        print(f"baud        = {args.baud}")
        print(f"iterations  = {args.iterations}")
        print(f"warmup      = {args.warmup}")
        print(f"joint_count = {args.joint_count}")
        print(f"angle_rad   = {args.angle_rad}")
        costs_ms = measure_movej_latency(api, angles, args.iterations, args.warmup)
        print_summary(costs_ms)
    finally:
        if hasattr(transport, "close"):
            transport.close()


if __name__ == "__main__":
    main()