#include "stmisp.h"
#include <termios.h>
#include <sys/time.h>
extern int uart_fd;
// Utility: XOR checksum of bytes
uint8_t xor_checksum(const uint8_t *buf, size_t len) {
    uint8_t cs = 0;
    for (size_t i = 0; i < len; ++i) cs ^= buf[i];
    return cs;
}

int stmisp_init(stmisp_device_t *stmisp_device, int (*send)(void* data, uint16_t len, int timeout), int (*recv)(void* data, uint16_t len, int timeout), int (*get_length )(void))
{
    stmisp_device->send = send;
    stmisp_device->recv = recv;
    stmisp_device->get_length = get_length;
    return 0;
}

int wait_ack(stmisp_device_t *stmisp_device, uint32_t timeout)
{
    uint8_t ack = 0;
    uint8_t size =stmisp_device->recv(&ack, 1, timeout);
    
    if(size > 0)
    {
        switch (ack)
        {
        case ACK:
            return 1;
        case NACK:
            return 2;
        
        default:
            break;
        }
    }
    return 0;
}

// int wait_ack(stmisp_device_t *stmisp_device, uint32_t timeout) {
//     double elapsed = 0.0;
//     const double step = 0.02; // poll step
//     struct timeval t0;
//     gettimeofday(&t0, NULL);
//     while (elapsed < timeout) {
//         uint8_t b;
//         int r = stmisp_device->recv(&b, 1, 0);
//         if (r < 0) return -1;
//         if (r == 0) {
//             // nothing
//             // printf("stmisp: No ACK (timeout)\n");
//         } else {
//             if (b == ACK) return 1;
//             if (b == NACK) return 2;
//             // else ignore and continue
//         }
//         struct timeval t1;
//         gettimeofday(&t1, NULL);
//         elapsed = (t1.tv_sec - t0.tv_sec) + (t1.tv_usec - t0.tv_usec) / 1e6;
//     }
//     return 0; // timeout
// }

int send_cmd_wait(stmisp_device_t *stmisp_device, uint8_t cmd, uint32_t timeout)
{
    uint8_t pkt[2];
    pkt[0] = cmd; pkt[1] = cmd ^ 0xFF;
    tcflush(uart_fd, TCIOFLUSH);
    stmisp_device->send(pkt, 2, 1000);
    uint8_t ack = wait_ack(stmisp_device, timeout);
    
    switch(ack)
    {
        case 1:
            printf("stmisp: ACK to cmd 0x%02x\n", cmd);
            return 1;
        case 2:
            printf("stmisp: NACK to cmd 0x%02x\n", cmd);
            return 2;
        default:
            break;
    }
    printf("stmisp: No ACK to cmd 0x%02x (timeout)\n", cmd);
    return 0;
}

int send_sync(stmisp_device_t *stmisp_device, uint32_t retries)
{
    for(int i = 0; i < retries; i++)
    {
        uint8_t sync = SYNC_BYTE;
        stmisp_device->send(&sync, 1, 1000);
        if(wait_ack(stmisp_device, 1000) == 1)
        {
            return 1;
        }
    }
    return 0;
}

int cmd_get(stmisp_device_t *stmisp_device)
{
    uint8_t ack = send_cmd_wait(stmisp_device, GET_CMD, 2000);
    if(ack == 1)
    {
        stmisp_device->cmd_count = 0;
        
        uint8_t size = stmisp_device->recv(&stmisp_device->cmd_count, 1, 1000);
        size = stmisp_device->recv(&stmisp_device->version, 1, 1000);
        printf("stmisp: version: %x cmd_count %d\n", stmisp_device->version, stmisp_device->cmd_count);
        if(stmisp_device->cmd_count > 0 && size > 0)
        {
            size = stmisp_device->recv(stmisp_device->cmd_data, stmisp_device->cmd_count, 1000);
            
            for(int i = 0; i < stmisp_device->cmd_count; i++)
            {
                printf(" %0x", stmisp_device->cmd_data[i]);
            }
            printf("\n");
            if(wait_ack(stmisp_device, 1000) == 1){
                return 1;
            } else {
                printf("stmisp: No ACK to cmd 0x%02x (timeout)\n", GET_CMD);
                return 0;
            }
        }
    }
    
    return 0;
}

int cmd_getid(stmisp_device_t *stmisp_device)
{
    uint8_t ack = send_cmd_wait(stmisp_device, GETID_CMD, 1000);

    if(ack == 1)
    {
        uint8_t size = stmisp_device->recv(&stmisp_device->num, 1, 1000);
        size = stmisp_device->recv(&stmisp_device->pid, 2, 1000);
        if(size == 2)
        {
            if(wait_ack(stmisp_device, 1000) == 1)
            {
                return 1;
            }
        }
    }
    return 0;
}

int cmd_extended_erase_mass(stmisp_device_t *stmisp_device)
{
    uint8_t ack = send_cmd_wait(stmisp_device, ERASE_CMD, 1000);
    if(ack == 1)
    {
        uint8_t payload[3];
        payload[0] = 0xFF; payload[1] = 0xFF; payload[2] = xor_checksum(payload, 2);
        stmisp_device->send(&payload, 3, 1000);
        if(wait_ack(stmisp_device, 60000) == 1)
        {
            return 1;
        }
    }
    return 0;
}

int cmd_write_unprotect(stmisp_device_t *stmisp_device)
{
    uint8_t ack = send_cmd_wait(stmisp_device, WRITE_UN_CMD, 1000);
    if(ack == 1)
    {
      if(wait_ack(stmisp_device, 1000) == 1)  
      {
        return 1;
      }
    }
    return 0;
}

int cmd_read_protect(stmisp_device_t *stmisp_device)
{
    uint8_t ack = send_cmd_wait(stmisp_device, READ_UN_CMD, 1000);
    if(ack == 1)
    {
      if(wait_ack(stmisp_device, 1000) == 1)  
      {
        return 1;
      }
    }
    return 0;
}

int cmd_write_memory(stmisp_device_t *stmisp_device, uint32_t addr, const uint8_t *data, size_t len, bool verify)
{
    size_t idx = 0;
    while(idx < len)
    {
        size_t chunk = (len - idx) > 256 ? 256 : (len - idx);

        uint8_t ack = send_cmd_wait(stmisp_device, WRITE_CMD, 1000);
        if(ack == 1)
        {
            uint32_t a = addr + (uint32_t)idx;
            uint8_t addrb[4] = { (uint8_t)(a >> 24), (uint8_t)(a >> 16), (uint8_t)(a >> 8), (uint8_t)(a >> 0) };
            uint8_t cs_addr = xor_checksum(addrb, 4);
            uint8_t pkt[5]; memcpy(pkt, addrb, 4); pkt[4] = cs_addr;

            stmisp_device->send(pkt, 5, 1000);

            if(wait_ack(stmisp_device, 1000) != 1) return 0;

            uint8_t nfield = (uint8_t)(chunk - 1);

            uint8_t cs = nfield;
            for (size_t i = 0; i < chunk; ++i) cs ^= data[idx + i];

            stmisp_device->send(&nfield, 1, 1000);
            stmisp_device->send((void *)&data[idx], chunk, 1000);
            stmisp_device->send(&cs, 1, 1000);

            if(wait_ack(stmisp_device, 5000) != 1) return 0;
            //是否需要校验写入的数据块
            if (verify)
            {

            }
            idx += chunk;
        }
    }
    return 1;
}