/*
 * MCU version probe via ttyS7 - try multiple commands
 */

#define _POSIX_C_SOURCE 200809L

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <fcntl.h>
#include <termios.h>
#include <sys/select.h>
#include <errno.h>

#define PORT "/dev/ttyS7"
#define BAUD 460800

static int serial_open(const char *port, int baud)
{
    int fd = open(port, O_RDWR | O_NOCTTY | O_NONBLOCK);
    if (fd < 0) { perror("open"); return -1; }
    struct termios tty;
    if (tcgetattr(fd, &tty) != 0) { perror("tcgetattr"); close(fd); return -1; }
    speed_t speed = B0;
    switch (baud) {
        case 115200: speed = B115200; break;
        case 460800: speed = B460800; break;
        default: fprintf(stderr, "unsupported baud: %d\n", baud); close(fd); return -1;
    }
    cfsetispeed(&tty, speed);
    cfsetospeed(&tty, speed);
    tty.c_cflag &= ~(PARENB | CSTOPB | CSIZE | CRTSCTS);
    tty.c_cflag |= CS8 | CREAD | CLOCAL;
    tty.c_iflag &= ~(IXON | IXOFF | IXANY | IGNBRK | BRKINT | PARMRK | ISTRIP
                    | INLCR | IGNCR | ICRNL);
    tty.c_oflag &= ~(OPOST | ONLCR);
    tty.c_lflag &= ~(ICANON | ECHO | ECHOE | ECHONL | ISIG);
    tty.c_cc[VMIN]  = 0;
    tty.c_cc[VTIME] = 0;
    if (tcsetattr(fd, TCSANOW, &tty) != 0) { perror("tcsetattr"); close(fd); return -1; }
    int flags = fcntl(fd, F_GETFL);
    if (flags >= 0) fcntl(fd, F_SETFL, flags & ~O_NONBLOCK);
    tcflush(fd, TCIOFLUSH);
    return fd;
}

static int serial_read(int fd, uint8_t *buf, size_t cap, int timeout_ms)
{
    fd_set rfds;
    struct timeval tv;
    FD_ZERO(&rfds);
    FD_SET(fd, &rfds);
    tv.tv_sec  = timeout_ms / 1000;
    tv.tv_usec = (timeout_ms % 1000) * 1000;
    int ret = select(fd + 1, &rfds, NULL, NULL, &tv);
    if (ret <= 0) return ret;
    return (int)read(fd, buf, cap);
}

static int serial_write(int fd, const uint8_t *data, size_t len)
{
    size_t written = 0;
    while (written < len) {
        ssize_t ret = write(fd, data + written, len - written);
        if (ret < 0) {
            if (errno == EINTR || errno == EAGAIN) continue;
            return -1;
        }
        if (ret == 0) break;
        written += (size_t)ret;
    }
    tcdrain(fd);
    return (int)written;
}

static uint8_t calc_checksum(const uint8_t *data, size_t len)
{
    uint16_t sum = 0;
    for (size_t i = 0; i < len; ++i) sum += data[i];
    return (uint8_t)(~(sum & 0xFF));
}

static size_t build_frame(uint8_t *out, uint8_t seq, uint8_t ft,
                          const uint8_t *payload, size_t payload_len)
{
    size_t total = payload_len + 7;
    out[0] = 0xFF; out[1] = 0xAA;
    out[2] = (uint8_t)((total >> 8) & 0xFF);
    out[3] = (uint8_t)(total & 0xFF);
    out[4] = seq; out[5] = ft;
    if (payload_len > 0) memcpy(out + 6, payload, payload_len);
    out[6 + payload_len] = calc_checksum(out + 2, payload_len + 4);
    return total;
}

static void print_hex(const char *label, const uint8_t *data, size_t len)
{
    printf("%s [len=%zu] ", label, len);
    for (size_t i = 0; i < len; ++i) printf("%02X ", data[i]);
    printf("\n");
}

static int parse_frame(const uint8_t *raw, size_t raw_len,
                       uint8_t *seq, uint8_t *ft,
                       uint8_t *payload, size_t *payload_len)
{
    if (raw_len < 7 || raw[0] != 0xFF || raw[1] != 0xAA) return -1;
    uint16_t decl = ((uint16_t)raw[2] << 8) | raw[3];
    if (decl != raw_len) return -1;
    uint8_t cs = calc_checksum(raw + 2, raw_len - 3);
    if (cs != raw[raw_len - 1]) return -1;
    *seq = raw[4]; *ft = raw[5];
    *payload_len = raw_len - 7;
    if (*payload_len > 0) memcpy(payload, raw + 6, *payload_len);
    return 0;
}

static int read_frame(int fd, uint8_t *frame_out, size_t *frame_len, int timeout_ms)
{
    uint8_t buf[256];
    uint8_t accum[4096];
    size_t accum_len = 0;
    struct timespec ts;
    clock_gettime(CLOCK_MONOTONIC, &ts);
    uint64_t start = (uint64_t)ts.tv_sec * 1000 + ts.tv_nsec / 1000000;

    while (1) {
        int n = serial_read(fd, buf, sizeof(buf), 50);
        if (n > 0) {
            if (accum_len + (size_t)n > sizeof(accum)) {
                size_t keep = accum_len > 200 ? 200 : accum_len;
                memmove(accum, accum + accum_len - keep, keep);
                accum_len = keep;
            }
            memcpy(accum + accum_len, buf, n);
            accum_len += (size_t)n;
            for (size_t i = 0; i + 7 <= accum_len; ) {
                if (accum[i] != 0xFF || accum[i+1] != 0xAA) { ++i; continue; }
                uint16_t flen = ((uint16_t)accum[i+2] << 8) | accum[i+3];
                if (flen < 7 || flen > 4096) { ++i; continue; }
                if (i + flen > accum_len) break;
                uint8_t cs = calc_checksum(accum + i + 2, flen - 3);
                if (cs == accum[i + flen - 1]) {
                    memcpy(frame_out, accum + i, flen);
                    *frame_len = flen;
                    size_t consumed = i + flen;
                    if (consumed < accum_len) {
                        memmove(accum, accum + consumed, accum_len - consumed);
                        accum_len -= consumed;
                    } else accum_len = 0;
                    return 0;
                }
                ++i;
            }
        }
        clock_gettime(CLOCK_MONOTONIC, &ts);
        uint64_t now = (uint64_t)ts.tv_sec * 1000 + ts.tv_nsec / 1000000;
        if ((int)(now - start) >= timeout_ms) break;
    }
    return -1;
}

static void send_and_wait(int fd, const uint8_t *payload, size_t plen,
                          const char *name, uint8_t seq)
{
    uint8_t tx[64];
    size_t txlen = build_frame(tx, seq, 0x06, payload, plen);
    printf("\n--- %s ---\n", name);
    print_hex("TX", tx, txlen);
    serial_write(fd, tx, txlen);

    uint8_t rx[4096];
    size_t rxlen;
    uint8_t rseq, rft, rpld[4096];
    size_t rpllen;

    /* Wait for ACK */
    if (read_frame(fd, rx, &rxlen, 2500) == 0) {
        parse_frame(rx, rxlen, &rseq, &rft, rpld, &rpllen);
        print_hex("RX ACK", rx, rxlen);
    }

    /* Wait for response frames for 3s */
    int got_resp = 0;
    for (int i = 0; i < 10 && !got_resp; ++i) {
        if (read_frame(fd, rx, &rxlen, 3000) != 0) {
            printf("[TIMEOUT] no more frames\n");
            break;
        }
        if (parse_frame(rx, rxlen, &rseq, &rft, rpld, &rpllen) != 0) {
            print_hex("RX (bad)", rx, rxlen);
            continue;
        }
        print_hex("RX", rx, rxlen);
        printf("  -> ft=0x%02X seq=0x%02X payload_len=%zu\n", rft, rseq, rpllen);
        if (rpllen > 0) {
            printf("  -> payload: ");
            for (size_t j = 0; j < rpllen; ++j) printf("%02X ", rpld[j]);
            printf("\n  -> ascii:   ");
            for (size_t j = 0; j < rpllen; ++j) {
                printf("%c", rpld[j] >= 0x20 && rpld[j] < 0x7F ? rpld[j] : '.');
            }
            printf("\n");
        }
        /* Auto-ACK */
        if (rft == 0x06 || rft == 0x08 || rft == 0x05) {
            uint8_t ack[16]; size_t alen = build_frame(ack, rseq, 0x02, NULL, 0);
            print_hex("TX ACK", ack, alen);
            serial_write(fd, ack, alen);
        }
        if (rft == 0x06 && rpllen >= 2) {
            printf("  -> [CMD] mt=0x%02X cmd=0x%02X\n", rpld[0], rpld[1]);
            got_resp = 1;
        }
    }
}

int main(void)
{
    int fd = serial_open(PORT, BAUD);
    if (fd < 0) return 1;
    printf("Opened %s @ %d\n\n", PORT, BAUD);

    uint8_t drain[256];
    while (serial_read(fd, drain, sizeof(drain), 100) > 0) {}

    /* SETUP */
    uint8_t setup[16];
    size_t setup_len = build_frame(setup, 1, 0x01, NULL, 0);
    print_hex("TX SETUP", setup, setup_len);
    serial_write(fd, setup, setup_len);

    uint8_t rx[4096]; size_t rxlen;
    uint8_t seq, ft, payload[4096]; size_t plen;
    if (read_frame(fd, rx, &rxlen, 3000) != 0) {
        printf("[TIMEOUT] No SETUPACK\n");
        close(fd); return 1;
    }
    parse_frame(rx, rxlen, &seq, &ft, payload, &plen);
    print_hex("RX SETUPACK", rx, rxlen);
    if (ft != 0x03) { printf("[ERROR] Expected SETUPACK\n"); close(fd); return 1; }
    printf("[PASS] Link established\n");

    /* ACK for SETUPACK */
    uint8_t ack[16]; size_t acklen = build_frame(ack, seq, 0x02, NULL, 0);
    print_hex("TX ACK", ack, acklen);
    serial_write(fd, ack, acklen);

    /* Try 0x81 REQ_INIT */
    uint8_t req_init[] = {0x01, 0x81};
    send_and_wait(fd, req_init, sizeof(req_init), "REQ_INIT (0x81)", 2);

    /* Try 0x86 REQ_TYPE */
    uint8_t req_type[] = {0x01, 0x86, 0x02};
    send_and_wait(fd, req_type, sizeof(req_type), "REQ_TYPE (0x86)", 3);

    /* Try 0x9A ARM_HW_VERSION */
    uint8_t req_hw[] = {0x01, 0x9A};
    send_and_wait(fd, req_hw, sizeof(req_hw), "ARM_HW_VERSION (0x9A)", 4);

    /* Try 0x99 ARM_SW_VERSION */
    uint8_t req_sw[] = {0x01, 0x99};
    send_and_wait(fd, req_sw, sizeof(req_sw), "ARM_SW_VERSION (0x99)", 5);

    /* CLOSE */
    uint8_t closef[16]; size_t clen = build_frame(closef, 6, 0x04, NULL, 0);
    print_hex("\nTX CLOSE", closef, clen);
    serial_write(fd, closef, clen);

    close(fd);
    return 0;
}