import argparse
import struct
import threading

import serial


SYNC = b"\xA5\x5A"
PACKET_FORMAT = "<2sBBBI10hB"
PACKET_SIZE = struct.calcsize(PACKET_FORMAT)


def decode_packet(packet):
    fields = struct.unpack(PACKET_FORMAT, packet)
    _, version, din_mask, dout_mask, time_ms, *rest = fields
    raw = rest[:-1]

    acc_scale = 16.0 / 32768.0
    gyro_scale = 2000.0 / 32768.0
    angle_scale = 180.0 / 32768.0

    return {
        "version": version,
        "t_ms": time_ms,
        "din": [(din_mask >> bit) & 1 for bit in range(4)],
        "dout": [(dout_mask >> bit) & 1 for bit in range(2)],
        "acc_g": [round(value * acc_scale, 5) for value in raw[0:3]],
        "gyro_dps": [round(value * gyro_scale, 4) for value in raw[3:6]],
        "angle_deg": [round(value * angle_scale, 4) for value in raw[6:9]],
        "temp_c": round(raw[9] / 100.0, 2),
    }


def send_gripper_command(ser, mode):
    if mode == 1:
        ser.write(b"o")
    elif mode == 2:
        ser.write(b"c")
    else:
        ser.write(b"s")


def keyboard_control(ser):
    print("Commands: o=OPEN, c=CLOSE, s=STOP")
    while True:
        text = input().strip().lower()
        if text == "o":
            send_gripper_command(ser, 1)
        elif text == "c":
            send_gripper_command(ser, 2)
        elif text == "s":
            send_gripper_command(ser, 0)
        else:
            print("Unknown command. Use o, c, or s.")


def main():
    parser = argparse.ArgumentParser()
    parser.add_argument("port", help="ESP32 serial port, for example COM5")
    parser.add_argument("--baud", type=int, default=230400)
    args = parser.parse_args()

    buffer = bytearray()
    packet_count = 0
    last_time_ms = None

    with serial.Serial(args.port, args.baud, timeout=1) as ser:
        print("Reading {} at {} baud".format(args.port, args.baud))
        threading.Thread(
            target=keyboard_control,
            args=(ser,),
            daemon=True,
        ).start()

        while True:
            chunk = ser.read(ser.in_waiting or 1)
            if chunk:
                buffer.extend(chunk)

            while len(buffer) >= PACKET_SIZE:
                sync_index = buffer.find(SYNC)
                if sync_index < 0:
                    del buffer[:-1]
                    break
                if sync_index:
                    del buffer[:sync_index]
                if len(buffer) < PACKET_SIZE:
                    break

                packet = bytes(buffer[:PACKET_SIZE])
                if (sum(packet[:-1]) & 0xFF) != packet[-1]:
                    del buffer[0]
                    continue

                del buffer[:PACKET_SIZE]
                data = decode_packet(packet)
                packet_count += 1

                if last_time_ms is None:
                    frequency = 0.0
                else:
                    delta = (data["t_ms"] - last_time_ms) & 0xFFFFFFFF
                    frequency = 1000.0 / delta if delta else 0.0
                last_time_ms = data["t_ms"]

                print(
                    "#{:8d} {:6.1f}Hz DIN={} DOUT={} "
                    "ACC={} GYRO={} ANGLE={}".format(
                        packet_count,
                        frequency,
                        data["din"],
                        data["dout"],
                        data["acc_g"],
                        data["gyro_dps"],
                        data["angle_deg"],
                    )
                )


if __name__ == "__main__":
    main()