#include "resend_protl.h"

void resend_init(resend_device_t *resend_device, int (*send)(void* data, uint16_t len, int timeout), int (*recv)(void* data, uint16_t len, int timeout), int (*get_length )(void), void (*resend_user_parse)(void *resend_device))
{
    resend_device->send = send;
    resend_device->recv = recv;
    resend_device->get_length = get_length;
    resend_device->resend_user_parse = resend_user_parse;

    resend_device->status.c_state = RESEND_IDLE;

    resend_device->tx_frame.header_0 = RESEND_HEADER_0;
    resend_device->tx_frame.header_1 = RESEND_HEADER_1;
    resend_device->tx_frame.seq = 0;
    resend_device->tx_frame.cmd = 0;
    resend_device->tx_frame.retry_cnt = 0;
    resend_device->tx_frame.len = 0;
    resend_device->tx_frame.crc = 0;
}
void resend_set(resend_device_t *resend_device)
{
    resend_device->tx_frame.seq = resend_device->rx_frame.seq;
}
int resend_encode(resend_device_t *resend_device, void *out, uint8_t cmd, void* data, uint16_t len)
{
    if(!out ) return -1;
    uint8_t* c  = (uint8_t*)out;
    // header
    c[0] = resend_device->tx_frame.header_0;
    c[1] = resend_device->tx_frame.header_1;
    // seq
    if(cmd != RESEND_CMD_DATA && cmd != RESEND_CMD_ACK && resend_device->tx_frame.retry_cnt == 0)
        resend_device->tx_frame.seq++;

    c[2] = resend_device->tx_frame.seq;
    // cmd
    c[3] = resend_device->tx_frame.cmd = cmd;
    // retry_cnt
    c[4] = resend_device->tx_frame.retry_cnt;
    // len big endian
    c[5] = (uint8_t)((len >> 8) & 0xFF);
    c[6] = (uint8_t)(len & 0xFF);
    // payload
    if (len > 0 && data) memcpy(&c[7], data, len);
    // compute crc over seq..payload (i.e. bytes p+1 .. p+5+len)
    uint16_t crc = fmav_crc_calculate(&c[2], 5 + len);
    c[7 + len] = (uint8_t)((crc >> 8) & 0xFF);
    c[7 + len + 1] = (uint8_t)(crc & 0xFF);

    return (int)(len + RESEND_MAX_PAYLOAD);
}

int resend_decode(resend_device_t *resend_device, uint8_t c)
{
    static uint16_t frame_crc = 0;
    switch(resend_device->status.c_state)
    {
        case RESEND_IDLE:
            if (c == RESEND_HEADER_0) {
                resend_device->status.c_state = RESEND_HDR;
            } 
            else{
                resend_device->status.c_state = RESEND_IDLE;
            }
            break;
        case RESEND_HDR:
            if (c == RESEND_HEADER_1) {
                resend_device->status.c_state = RESEND_SEQ;
                fmav_crc_init(&frame_crc);
            }else{
                resend_device->status.c_state = RESEND_IDLE;
            }
            break;
        case RESEND_SEQ:
            resend_device->rx_frame.seq = c;
            fmav_crc_accumulate(&frame_crc, c);
            resend_device->status.c_state = RESEND_CMD;
            break;
        case RESEND_CMD:
            resend_device->rx_frame.cmd = c;
            fmav_crc_accumulate(&frame_crc, c);
            resend_device->status.c_state = RESEND_RETRY_CNT;
            break;
        case RESEND_RETRY_CNT:
            printf("cnt %d\r\n",c);
            resend_device->rx_frame.retry_cnt = c;
            fmav_crc_accumulate(&frame_crc, c);
            resend_device->status.c_state = RESEND_LEN_1;
            break;
        case RESEND_LEN_1:
            resend_device->rx_frame.len = 0;
            resend_device->rx_frame.len = (uint16_t)(c << 8) & 0xF0;
            fmav_crc_accumulate(&frame_crc, c);
            resend_device->status.c_state = RESEND_LEN_2;
            break;
        case RESEND_LEN_2:
            resend_device->rx_frame.len |= c;
            fmav_crc_accumulate(&frame_crc, c);
            if(resend_device->rx_frame.len > 0)
            {
                resend_device->status.data_offset = 0;
                resend_device->status.c_state = RESEND_DATA;
            }
            else
                resend_device->status.c_state = RESEND_CRC_1;
            break;
        case RESEND_DATA:
            resend_device->rx_frame.payload[resend_device->status.data_offset ++] = c;
            fmav_crc_accumulate(&frame_crc, c);
            if(resend_device->rx_frame.len == resend_device->status.data_offset){
                resend_device->status.data_offset = 0;
                resend_device->status.c_state = RESEND_CRC_1;
            }
            break;
        case RESEND_CRC_1:
            if((uint8_t)(frame_crc >> 8 & 0xFF) == c){
                resend_device->status.c_state = RESEND_CRC_2;
            }else{
                resend_device->status.c_state = RESEND_IDLE;
            }
            break;
        case RESEND_CRC_2:
            if((uint8_t)(frame_crc) == c){
                resend_device->status.c_state = RESEND_IDLE;
                resend_device->rx_frame.crc = frame_crc;
                if(resend_device->rx_frame.cmd != RESEND_CMD_DATA && resend_device->rx_frame.cmd != RESEND_CMD_ACK && resend_device->rx_frame.retry_cnt == 0)
                    resend_device->tx_frame.seq++;
                return 0;
            }else{
                resend_device->status.c_state = RESEND_IDLE;
            }
            break;
        default:    
            break;
    }
    return 1;
}

int resend_recv_data(resend_device_t *resend_device, int timeout)
{
    uint8_t data[RESEND_FRAME_SIZE];
    int ret = resend_device->recv(data, RESEND_FRAME_SIZE, timeout);
    if(ret <= 0)
        return -1;
    int ret_later = ret;
    static uint8_t seq_lat = 0;
    while(ret_later --)
    {
        if(!resend_decode(resend_device, data[ret - ret_later - 1]))
        {
            printf("tx_req %d rx_req %d retry_cnt %d cmd %d\r\n", resend_device->tx_frame.seq, resend_device->rx_frame.seq, resend_device->rx_frame.retry_cnt, resend_device->rx_frame.cmd);

            if(seq_lat == resend_device->rx_frame.seq){
                resend_device->status.resend_flag = 1;
            } else {
                resend_device->status.resend_flag = 0;
            }
            
            resend_device->resend_user_parse(resend_device);
            resend_parse_data(resend_device);

            seq_lat = resend_device->rx_frame.seq;
        }
    }
    return ret;
}

int resend_send_cmd(resend_device_t *resend_device, uint8_t cmd, int timeout)
{
    uint8_t out[RESEND_MAX_PAYLOAD];
    int ret = resend_encode(resend_device, out, cmd, NULL, 0);
    if (ret <= 0) 
        return -1;
    return resend_device->send(out, ret, timeout);
}
int resend_send_data(resend_device_t *resend_device, uint8_t cmd, void* data, uint16_t len, int timeout)
{   
    uint8_t out[RESEND_MAX_PAYLOAD + len];
    memcpy(resend_device->tx_frame.payload, data, len);
    
    resend_device->tx_frame.retry_cnt = 0;

    int ret = resend_encode(resend_device, out, cmd, data, len);
    if (ret <= 0) 
        return -1;
    // 初始化ack应答标志位，需等待应答
    resend_device->status.ack_flag = 1;

    while(resend_device->status.ack_flag)
    {
        if(resend_device->tx_frame.retry_cnt)
        {
            //进行重发 ， 重新编码
            ret = resend_encode(resend_device, out, cmd, data, len);
            if (ret <= 0) 
                return -1;
            printf("data retry cnt %d\n", resend_device->tx_frame.retry_cnt);
        }
            
        ret = resend_device->send(out, ret, 0);
        if(ret < RESEND_MAX_PAYLOAD)
            return ret;
        
        // 特殊命令以及应答信号不需要等待
        if(cmd == RESEND_CMD_DATA || cmd == RESEND_CMD_ACK)
            resend_device->status.ack_flag = 0;

        resend_device->tx_frame.retry_cnt ++;

        // 接收数据
        resend_recv_data(resend_device, timeout);
        // 超过允许的等待次数
        if(resend_device->tx_frame.retry_cnt > RESEND_MAX_RETRY_CNT)
        {
            printf("timeout cnt max\n");
            resend_device->tx_frame.retry_cnt = 0;
            resend_device->status.ack_flag = 0;
            return -1;
        }
    }
    
    return ret;
    // return resend_device->send(out, resend_frame_encode(resend_device, out, data, len), timeout);
} 

int resend_parse_data(resend_device_t *resend_device)
{
    switch(resend_device->rx_frame.cmd){
        case RESEND_CMD_DATA:
            for (size_t i = 0; i < resend_device->rx_frame.len; i++)
            {
                printf("%02X ", resend_device->rx_frame.payload[i]);
            }
            printf("\r\n");
            break;
        case RESEND_CMD_ACK:
            printf("ACK\n");
            resend_device->status.ack_flag = 0;
            break;
        case RESEND_CMD_STATUS:
            resend_set(resend_device);
            resend_send_cmd(resend_device, RESEND_CMD_ACK, 0);
            break;
        case RESEND_CMD_DATA_ACK:
            for (size_t i = 0; i < resend_device->rx_frame.len; i++)
            {
                printf("%02X ", resend_device->rx_frame.payload[i]);
            }
            printf("\r\n");
            resend_send_cmd(resend_device, RESEND_CMD_ACK, 0);
            break;
        case RESEND_CMD_OTA_START:
            break;
        case RESEND_CMD_OTA_DATA:
            break;
        case RESEND_CMD_OTA_END:
            break;
        default:
            break;
    }
    return 0;
}