/*
 * MCU version probe via ttyS7
 * Protocol: FF AA len_hi len_lo seq ft payload... checksum
 */

#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;
}

/* Simple stream parser: scan for FF AA, try decode */
static int read_frame(int fd, uint8_t *frame_out, size_t *frame_len, int timeout_ms)
{
    uint8_t buf[256];
    uint8_t accum[2048];
    size_t  accum_len = 0;
    uint64_t start = 0;
    struct timespec ts;
    clock_gettime(CLOCK_MONOTONIC, &ts);
    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 > 100 ? 100 : accum_len;
                memmove(accum, accum + accum_len - keep, keep);
                accum_len = keep;
            }
            memcpy(accum + accum_len, buf, n);
            accum_len += (size_t)n;

            /* scan for frames */
            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 > 2048) { ++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;
                    /* shift remaining */
                    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;
}

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

    /* Drain pending bytes */
    uint8_t drain[256];
    while (serial_read(fd, drain, sizeof(drain), 100) > 0) {}

    /* Send SETUP frame */
    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);

    /* Wait for SETUPACK */
    uint8_t rx[2048];
    size_t rx_len = 0;
    uint8_t seq, ft;
    uint8_t payload[2048];
    size_t payload_len = 0;

    if (read_frame(fd, rx, &rx_len, 3000) != 0) {
        printf("[TIMEOUT] No SETUPACK received within 3s\n");
        printf("[HINT] MCU may be offline, wrong baud, or ttyS7 not connected to MCU\n");
        close(fd);
        return 1;
    }
    if (parse_frame(rx, rx_len, &seq, &ft, payload, &payload_len) != 0) {
        printf("[ERROR] Failed to parse SETUPACK\n");
        close(fd);
        return 1;
    }
    print_hex("RX", rx, rx_len);
    printf("  -> ft=0x%02X (%s) seq=0x%02X\n", ft,
           ft == 0x03 ? "SETUPACK" : "???", seq);

    if (ft != 0x03) {
        printf("[ERROR] Expected SETUPACK(0x03), got 0x%02X\n", ft);
        close(fd);
        return 1;
    }
    printf("[PASS] Link established (SETUPACK seq=0x%02X)\n\n", seq);

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

    /* Send REQ_TYPE (0x86) to query MCU version */
    uint8_t req_body[] = {0x01, 0x86, 0x02};
    uint8_t req[32];
    size_t req_len = build_frame(req, 2, 0x06, req_body, sizeof(req_body));
    print_hex("TX REQ_TYPE(0x86)", req, req_len);
    serial_write(fd, req, req_len);

    /* Wait for ACK */
    if (read_frame(fd, rx, &rx_len, 2500) == 0) {
        parse_frame(rx, rx_len, &seq, &ft, payload, &payload_len);
        print_hex("RX", rx, rx_len);
        printf("  -> ft=0x%02X seq=0x%02X\n", ft, seq);
    }

    /* Wait for MCU version response (M2A_MCU_VERSION = 0x02) */
    printf("\nWaiting for MCU version response...\n");
    int got_version = 0;
    for (int attempt = 0; attempt < 5 && !got_version; ++attempt) {
        if (read_frame(fd, rx, &rx_len, 3000) != 0) {
            printf("[TIMEOUT] attempt %d/5\n", attempt + 1);
            continue;
        }
        if (parse_frame(rx, rx_len, &seq, &ft, payload, &payload_len) != 0) {
            print_hex("RX (parse fail)", rx, rx_len);
            continue;
        }
        print_hex("RX", rx, rx_len);
        printf("  -> ft=0x%02X seq=0x%02X payload_len=%zu\n", ft, seq, payload_len);
        if (payload_len > 0) {
            printf("  -> payload: ");
            for (size_t i = 0; i < payload_len; ++i) printf("%02X ", payload[i]);
            printf("\n");
        }

        /* Auto-ACK for DATA/BOOT/HB frames */
        if (ft == 0x06 || ft == 0x08 || ft == 0x05) {
            uint8_t auto_ack[16];
            size_t auto_ack_len = build_frame(auto_ack, seq, 0x02, NULL, 0);
            print_hex("TX AUTO-ACK", auto_ack, auto_ack_len);
            serial_write(fd, auto_ack, auto_ack_len);
        }

        /* Check if this is MCU version response */
        if (ft == 0x06 && payload_len >= 2 && payload[0] == 0x01 && payload[1] == 0x02) {
            printf("\n[FOUND] MCU Version Response!\n");
            if (payload_len > 2) {
                printf("  Version string: ");
                for (size_t i = 2; i < payload_len; ++i) {
                    printf("%c", payload[i] >= 0x20 && payload[i] < 0x7F ? payload[i] : '?');
                }
                printf("\n  Version hex: ");
                for (size_t i = 2; i < payload_len; ++i) printf("%02X ", payload[i]);
                printf("\n");
            }
            got_version = 1;
        }
    }

    if (!got_version) {
        printf("\n[FAIL] Did not receive MCU version response\n");
    }

    /* Send CLOSE */
    uint8_t close_frame[16];
    size_t close_len = build_frame(close_frame, 3, 0x04, NULL, 0);
    print_hex("TX CLOSE", close_frame, close_len);
    serial_write(fd, close_frame, close_len);

    close(fd);
    return got_version ? 0 : 1;
}