#include "drv_IAP.h"
#include "usbd_cdc_if.h"
//#include "CRC.h"
#include "CRC16.h"
#include "usb_device.h"
#include "usbd_cdc.h"

static void short_to_buf(unsigned char *buf, short *tshort){
    int i;
    unsigned char *p1 = (unsigned char *)tshort;
    unsigned char *p2 = buf;

    for(i = 0;i < 2;i++){
        *p2 = *p1;
        p1++;
        p2++;
    }
}

static void buf_to_short(short *tshort, unsigned char *buf){
    int i;
    unsigned char *p1 = (unsigned char *)tshort;
    unsigned char *p2 = buf;

    for(i = 0;i < 2;i++){
        *p1 = *p2;
        p1++;
        p2++;
    }
}

static void buf_to_int(int *tint, unsigned char *buf){
    int i;
    unsigned char *p1 = (unsigned char *)tint;
    unsigned char *p2 = buf;

    for(i = 0;i < 4;i++){
        *p1 = *p2;
        p1++;
        p2++;
    }
}

unsigned char send_what = 0;
uint8_t ack_id = 0;
short id_cont = 0;
bool IAP_STATUS_FLAG = true;

uint32_t usb_rx_index = 0;
__ALIGN_BEGIN uint8_t usb_rx_buf[IAP_PACK_SIZE + 2] __ALIGN_END= {0};
uint8_t usb_tx_buf[IAP_PACK_SIZE] = {0};
uint32_t usb_tx_index = 0;


bool recv_aid_flag = false,
    recv_init_flag = false,
    recv_erasure_flag = false,
    recv_finish_flag = false,
    recv_onepack_flag = false;

IAP_STATUS recv_iap_flag = DEFAULT;

int iap_bin_Size = 0;
short iap_bin_num = 0, pre_iap_bin_num = 0;
unsigned char one_pack_size = 0;

bool pack_send_flag = true;

mavlink_system_t mavlink_system = {
    .stx = 254,
    .headbytes = 6,
    .checkbytes = 2,
    .sysid = 0,
    .compid = 0,
    .tx_seq = 0,
   .rx_seq = 0
};

mavlink_msg_t mavlink_msg = {
    .stx = 254,
    .sysid = 0,
    .compid = 5
};

bool receive_mavlink_msg = false;

//除最后一包，前面固件包固定大小 0xF0   240字节
const uint8_t pack_size = 0xF0;

extern uint32_t rec_err , crc_err;

//发送MAVLINK_ACK
void send_mavlink_ack(void){
    int index = 0;
    uint16_t checksum = 0;
    usb_tx_buf[index++] = mavlink_system.stx;
    usb_tx_buf[index++] = mavlink_system.stx;
    usb_tx_buf[index++] = mavlink_system.tx_seq;
    usb_tx_buf[index++] = mavlink_system.sysid;
    usb_tx_buf[index++] = mavlink_system.compid;
    usb_tx_buf[index++] = MAVLINK_ACK_ID;
    usb_tx_buf[index++] = ack_id;

    short_to_buf(&usb_tx_buf[index], &id_cont);
    index += 2;

    // 计算有效载荷长度
    usb_tx_buf[1] = index - 6;
    
    checksum = Get_Crc16(usb_tx_buf,(uint16_t) index);
    usb_tx_buf[index++] = checksum & 0xff;
    usb_tx_buf[index++] = (checksum >> 8) & 0xff;

    CDC_Transmit_FS(usb_tx_buf, index);
}

uint16_t offset;
//上报MAVLINK_BOOTLOADER_STATUS
void send_mavlink_bootloader_status(uint8_t bootloader_status){
    uint16_t index = 0;
	offset = 0;
    uint16_t checksum = 0;
	  memset(usb_tx_buf,0,sizeof(usb_tx_buf));
    usb_tx_buf[offset++] = mavlink_system.stx;
	
    usb_tx_buf[offset++] = mavlink_system.stx;
    usb_tx_buf[offset++] = mavlink_system.tx_seq;
    usb_tx_buf[offset++] = mavlink_system.sysid;
    usb_tx_buf[offset++] = mavlink_system.compid;
    usb_tx_buf[offset++] = MAVLINK_BOOTLOADER_STATUS;
    
    usb_tx_buf[offset++] = bootloader_status;

    usb_tx_buf[1] = offset - 6;
    checksum = Get_Crc16(usb_tx_buf,(uint16_t) offset);

    usb_tx_buf[offset++] = checksum & 0xff;
    usb_tx_buf[offset++] = (checksum >> 8) & 0xff;
		
	  
    CDC_Transmit_FS(usb_tx_buf, offset);
}


uint8_t first_rxdata = 0, second_rxdata = 0, third_rxdata = 0, fourth_rxdata = 0, fifth_rxdata = 0, sixth_rxdata = 0;
void recv_iap_station_data_hookfunction(unsigned char rec_data){
    sixth_rxdata = fifth_rxdata;
    fifth_rxdata = fourth_rxdata;
    fourth_rxdata = third_rxdata;
    third_rxdata = second_rxdata;
    second_rxdata = first_rxdata;
    first_rxdata = rec_data;

    //如果接收到协议数据包
    if (receive_mavlink_msg == true){
        usb_rx_buf[usb_rx_index++] = rec_data;
    //如果数据包超出协议规定的最大长度
        if(mavlink_msg.payload > IAP_PACK_SIZE - mavlink_system.headbytes
                - mavlink_system.checkbytes){
            receive_mavlink_msg = false;
            usb_rx_index = 0;
        }
        //如果到达数据包尾
        if(usb_rx_index >= mavlink_msg.payload + mavlink_system.headbytes +
                mavlink_system.checkbytes){
            receive_mavlink_msg = false;
            usb_rx_index = 0;
            
            if(Get_Crc16(usb_rx_buf, mavlink_msg.payload + mavlink_system.headbytes
                                + mavlink_system.checkbytes) == 0x0000){
                switch(mavlink_msg.msgid){
                    case 200:  // 固件信息初始化包
                        buf_to_int(&iap_bin_Size, &usb_rx_buf[6]);
                        recv_iap_flag = INIT;  // 进入初始化状态
                        recv_init_flag = true;
                        break;
                    case 201:  // 固件数据传输包
                        if(recv_iap_flag == TRANSMIT){  //如果是传输状态
                            if(recv_onepack_flag != true){    
                                //USBD_Stop(&hUsbDeviceFS); // 停止USB接收
                                //USBD_CDC_Stop(&hUsbDeviceFS); // 停止USB接收
                                pack_send_flag = false;    // 停止发送
                                recv_onepack_flag = true;    // 设置包接收标志
                                buf_to_short(&iap_bin_num, &usb_rx_buf[6]);  
                                one_pack_size = mavlink_msg.payload - 2;
                                //保护usb_rx_buf数组不被更新
                            }
                        }
                        else if(recv_iap_flag == INIT){  //如果是初始化状态
                            buf_to_short(&iap_bin_num, &usb_rx_buf[6]);

                            if(iap_bin_num == 1){
                                recv_erasure_flag = true; //进行擦除
                            }
                        }
                        break;
                    case 202:
						//考虑到固件包大小刚好能被250字节整除和不被乘除的情况
                        if(pre_iap_bin_num >= iap_bin_Size / pack_size){
                            if(recv_iap_flag == TRANSMIT){
                                recv_iap_flag = FINISH;
                            }

                            recv_finish_flag = true;
                        }
                        break;
                    default:
                        break;
                }
            }
            else{
                crc_err++;
            }
        }
    //如果接收到协议头    
    }else if(sixth_rxdata == mavlink_msg.stx && 
            third_rxdata == mavlink_msg.sysid &&
            second_rxdata == mavlink_msg.compid){
        receive_mavlink_msg = true;
        usb_rx_index = 0;
        
        mavlink_msg.payload = fifth_rxdata;
        mavlink_msg.seq = fourth_rxdata;
        mavlink_msg.msgid = first_rxdata;

        //填充rx_buf头
        usb_rx_buf[0] = sixth_rxdata;
        usb_rx_buf[1] = fifth_rxdata;
        usb_rx_buf[2] = fourth_rxdata;
        usb_rx_buf[3] = third_rxdata;
        usb_rx_buf[4] = second_rxdata;
        usb_rx_buf[5] = first_rxdata;
        usb_rx_index = 6;
    }
    //如果接收到急救指令
    else if((fourth_rxdata == '$') && (third_rxdata == 'A') && (second_rxdata == 'I') 
                                && (first_rxdata == 'D')){
            recv_aid_flag = true;                        
    }
}