#include "McuTaskImu.h"
#include "McuJY60Drv.h"
#include "McuTaskBlueTooth.h"
#include "BeiDouHtyDrv.h"
#include "ZthyToolDataTypeSwitch.h"
#include "McuTaskFlash.h"

#include <string.h>
#include <stdio.h>

float gAx;
float gAy;
float gAz;
float gWx;
float gWy;
float gWz;
float gRoll;
float gPitch;
float gYaw;

uint32_t gAx32;
uint32_t gAy32;
uint32_t gAz32;
uint32_t gWx32;
uint32_t gWy32;
uint32_t gWz32;
uint32_t gRoll32;
uint32_t gPitch32;
uint32_t gYaw32;

/* 蓝牙发送标志位 */
static int gMcuTaskImuBlueToothSendFlag = 0;
static unsigned short gMcuTaskImuBlueToothSerial = 0;
static unsigned char gMcuTaskImuBlueToothSendBuf[37] = {0};
static unsigned char gMcuTaskImuBeiDouSerial = 0;

/* 北斗发送标志位 */
static int gMcuTaskImuBeiDouSendFlag = 0;

int McuTaskImuInit(void)
{
    int rt;
    rt = McuDrvJy60Init();
    if (rt < 0){
        /* 初始化 */
        return -1;
    }
    return 0;
}

/* IMU任务 */
int McuTaskImuTask(void)
{
    if (gMcuTaskImuBlueToothSendFlag == 1 || gMcuTaskImuBeiDouSendFlag == 1) {
        /* 获取数据 */
        McuTaskFlashWriteOpt("Imu Join\r\n", strlen("Imu Join\r\n"));
        McuDrvJy60GetData(&gAx, &gAy, &gAz, &gWx, &gWy, &gWz, &gRoll, &gPitch, &gYaw); // 可能会导致假死
        if (gMcuTaskImuBlueToothSendFlag == 1) {
            gMcuTaskImuBlueToothSendBuf[0] = 1;
            ZthyToolFloatToUint32(gAx, &gAx32);
            ZthyToolFloatToUint32(gAy, &gAy32);
            ZthyToolFloatToUint32(gAz, &gAz32);
            ZthyToolFloatToUint32(gWx, &gWx32);
            ZthyToolFloatToUint32(gWy, &gWy32);
            ZthyToolFloatToUint32(gWz, &gWz32);
            ZthyToolFloatToUint32(gRoll, &gRoll32);
            ZthyToolFloatToUint32(gPitch, &gPitch32);
            ZthyToolFloatToUint32(gYaw, &gYaw32);
            gMcuTaskImuBlueToothSendBuf[1] = (uint8_t)((gAx32 & 0xff000000) >> 24);
            gMcuTaskImuBlueToothSendBuf[2] = (uint8_t)((gAx32 & 0x00ff0000) >> 16);
            gMcuTaskImuBlueToothSendBuf[3] = (uint8_t)((gAx32 & 0x0000ff00) >> 8);
            gMcuTaskImuBlueToothSendBuf[4] = (uint8_t)(gAx32 & 0x000000ff);
            gMcuTaskImuBlueToothSendBuf[5] = (uint8_t)((gAy32 & 0xff000000) >> 24);
            gMcuTaskImuBlueToothSendBuf[6] = (uint8_t)((gAy32 & 0x00ff0000) >> 16);
            gMcuTaskImuBlueToothSendBuf[7] = (uint8_t)((gAy32 & 0x0000ff00) >> 8);
            gMcuTaskImuBlueToothSendBuf[8] = (uint8_t)(gAy32 & 0x000000ff);
            gMcuTaskImuBlueToothSendBuf[9] = (uint8_t)((gAz32 & 0xff000000) >> 24);
            gMcuTaskImuBlueToothSendBuf[10] = (uint8_t)((gAz32 & 0x00ff0000) >> 16);
            gMcuTaskImuBlueToothSendBuf[11] = (uint8_t)((gAz32 & 0x0000ff00) >> 8);
            gMcuTaskImuBlueToothSendBuf[12] = (uint8_t)(gAz32 & 0x000000ff);
            gMcuTaskImuBlueToothSendBuf[13] = (uint8_t)((gWx32 & 0xff000000) >> 24);
            gMcuTaskImuBlueToothSendBuf[14] = (uint8_t)((gWx32 & 0x00ff0000) >> 16);
            gMcuTaskImuBlueToothSendBuf[15] = (uint8_t)((gWx32 & 0x0000ff00) >> 8);
            gMcuTaskImuBlueToothSendBuf[16] = (uint8_t)(gWx32 & 0x000000ff);
            gMcuTaskImuBlueToothSendBuf[17] = (uint8_t)((gWy32 & 0xff000000) >> 24);
            gMcuTaskImuBlueToothSendBuf[18] = (uint8_t)((gWy32 & 0x00ff0000) >> 16);
            gMcuTaskImuBlueToothSendBuf[19] = (uint8_t)((gWy32 & 0x0000ff00) >> 8);
            gMcuTaskImuBlueToothSendBuf[20] = (uint8_t)(gWy32 & 0x000000ff);
            gMcuTaskImuBlueToothSendBuf[21] = (uint8_t)((gWz32 & 0xff000000) >> 24);
            gMcuTaskImuBlueToothSendBuf[22] = (uint8_t)((gWz32 & 0x00ff0000) >> 16);
            gMcuTaskImuBlueToothSendBuf[23] = (uint8_t)((gWz32 & 0x0000ff00) >> 8);
            gMcuTaskImuBlueToothSendBuf[24] = (uint8_t)(gWz32 & 0x000000ff);
            gMcuTaskImuBlueToothSendBuf[25] = (uint8_t)((gRoll32 & 0xff000000) >> 24);
            gMcuTaskImuBlueToothSendBuf[26] = (uint8_t)((gRoll32 & 0x00ff0000) >> 16);
            gMcuTaskImuBlueToothSendBuf[27] = (uint8_t)((gRoll32 & 0x0000ff00) >> 8);
            gMcuTaskImuBlueToothSendBuf[28] = (uint8_t)(gRoll32 & 0x000000ff);
            gMcuTaskImuBlueToothSendBuf[29] = (uint8_t)((gPitch32 & 0xff000000) >> 24);
            gMcuTaskImuBlueToothSendBuf[30] = (uint8_t)((gPitch32 & 0x00ff0000) >> 16);
            gMcuTaskImuBlueToothSendBuf[31] = (uint8_t)((gPitch32 & 0x0000ff00) >> 8);
            gMcuTaskImuBlueToothSendBuf[32] = (uint8_t)(gPitch32 & 0x000000ff);
            gMcuTaskImuBlueToothSendBuf[33] = (uint8_t)((gYaw32 & 0xff000000) >> 24);
            gMcuTaskImuBlueToothSendBuf[34] = (uint8_t)((gYaw32 & 0x00ff0000) >> 16);
            gMcuTaskImuBlueToothSendBuf[35] = (uint8_t)((gYaw32 & 0x0000ff00) >> 8);
            gMcuTaskImuBlueToothSendBuf[36] = (uint8_t)(gYaw32 & 0x000000ff);
            McuTaskFlashWriteOpt("Imu BlueTh Send Data\r\n", strlen("Imu BlueTh Send Data\r\n"));
            TaskBlueToothSendMessage(MCU_TASK_IMU_SEND_MESSAGE_ID, &gMcuTaskImuBlueToothSerial, gMcuTaskImuBlueToothSendBuf, sizeof(gMcuTaskImuBlueToothSendBuf));
            gMcuTaskImuBlueToothSerial++;
            gMcuTaskImuBlueToothSendFlag = 0;
        }
        if (gMcuTaskImuBeiDouSendFlag == 1) {
            unsigned char crc = 0;
            int i = 0;
            unsigned char BeiDouSend[70] = {0};
            gMcuTaskImuBeiDouSendFlag = 0;
            /* 发送加速度 */
            sprintf((char *)BeiDouSend, "Imu 01 %03d %0.2f %0.2f %0.2f ", gMcuTaskImuBeiDouSerial, gAx, gAy, gAz);
            for (i = 0; i < strlen((char *)BeiDouSend); i++) {
                crc ^= BeiDouSend[i];
            }
            sprintf((char *)&BeiDouSend[strlen((char *)BeiDouSend)], "%x", crc);
            McuDrvBeiDouHtySendPacket(BeiDouSend, strlen((char *)BeiDouSend));
            /* 发送角速度 */
            memset(BeiDouSend, 0, sizeof(BeiDouSend));
            sprintf((char *)BeiDouSend, "Imu 02 %03d %0.2f %0.2f %0.2f", gMcuTaskImuBeiDouSerial, gWx, gWy, gWz);
            for (i = 0; i < strlen((char *)BeiDouSend); i++) {
                crc ^= BeiDouSend[i];
            }
            sprintf((char *)&BeiDouSend[strlen((char *)BeiDouSend)], "%x \r\n", crc);
            McuDrvBeiDouHtySendPacket(BeiDouSend, strlen((char *)BeiDouSend));
            
            /* 发送角度 */
            memset(BeiDouSend, 0, sizeof(BeiDouSend));
            sprintf((char *)BeiDouSend, "Imu 03 %03d %0.2f %0.2f %0.2f", gMcuTaskImuBeiDouSerial, gRoll, gPitch, gYaw);
            for (i = 0; i < strlen((char *)BeiDouSend); i++) {
                crc ^= BeiDouSend[i];
            }
            sprintf((char *)&BeiDouSend[strlen((char *)BeiDouSend)], "%x \r\n", crc);
            McuDrvBeiDouHtySendPacket(BeiDouSend, strlen((char *)BeiDouSend));
            
            gMcuTaskImuBeiDouSerial++;
            McuTaskFlashWriteOpt("Imu BeiDou Send Data\r\n", strlen("Imu BeiDou Send Data\r\n"));
        }
        /* 保存数据到Flash */
        McuTaskFlashWriteImu(gAx, gAy, gAz, gWx, gWy, gWz, gRoll, gPitch, gYaw);
    }
    return 0;
}

/* 蓝牙请求数据 */
void McuTaskImuBlueToothAsk(void)
{
    gMcuTaskImuBlueToothSendFlag = 1;
}

/* 北斗请求数据 */
void McuTaskImuBeiDouAsk(void)
{
    gMcuTaskImuBeiDouSendFlag = 1;
}

/* 获取数据请求状态 */
int McuTaskImuAskStatus(void)
{
    return ((gMcuTaskImuBlueToothSendFlag * 2) | gMcuTaskImuBeiDouSendFlag);
}