#include "McuJY60Drv.h"
#include <string.h>

#define JY60_RXBUF_LEN 60

static void McuDrvJy60Recv(unsigned char data);
/* 读取一个字符 */
static unsigned char McuDrvJy60ReadByte(void);
static void McuDrvJy60Handle(void);

static char gJy60RxBuf[JY60_RXBUF_LEN] = {0}; // 能够存放住两帧数据，主要为了容错
static unsigned char gJy60RxBufLocal = 0; //当前读取指针所在位置
static unsigned char gJy60RxBufTemp = 0; // 当前接收数据的末期所在位置
static unsigned char gJy60AngleCalibration[3] = {0xFF, 0xAA, 0x52}; // 角速度初始化
static unsigned char gJy60AccCalibration[3] = {0xFF, 0xAA, 0x67}; // 加速度初始化
//static unsigned char gJy60Sleep[3] = {0xFF, 0xAA, 0x60}; //进入睡眠模式
//static unsigned char gJy60WakeUp[1] = {0x00}; // 由于输入任何信号就会使得芯片唤醒。
static unsigned char gJy60Horizon[3] = {0xFF, 0xAA, 0x65}; //水平安装
static unsigned char gJy60Vertical[3] = {0xFF, 0xAA, 0x66}; //垂直安装

static float gAx = 0.0f;
static float gAy = 0.0f;
static float gAz = 0.0f;

static float gWx = 0.0f;
static float gWy = 0.0f;
static float gWz = 0.0f;

static float gRoll = 0.0f;
static float gPitch = 0.0f;
static float gYaw = 0.0f;

/* JY60初始化 */
int McuDrvJy60Init(void)
{
	/* 初始化串口 */
	MCU_BSP_UART_CONF UartConfig = {0};
	UartConfig.BaudRate = MCU_DRV_JY60_UART_BAUDRATE;
	UartConfig.WordLength = MCU_DRV_JY60_UART_WORDLEN;
	UartConfig.StopBits = MCU_DRV_JY60_UART_STOPBITS;
	UartConfig.Parity = MCU_DRV_JY60_UART_PARITY;
	UartConfig.Mode = MCU_DRV_JY60_UART_MODE;
	if (McuBspUartInit(MCU_DRV_JY60_UART_ID, &UartConfig, NULL, McuDrvJy60Recv) < 0) {
		return -1;
	}
    McuBspUartStartRecv(MCU_DRV_JY60_UART_ID);
    /* 电源控制 */
    McuBspGpioInit(MCU_DRV_JY60_CTRL_POWER_GPIO_PORT, MCU_DRV_JY60_CTRL_POWER_GPIO_PIN,
                   MCU_DRV_JY60_CTRL_POWER_GPIO_MODE, MCU_DRV_JY60_CTRL_POWER_GPIO_SPEED,
                   MCU_DRV_JY60_CTRL_POWER_GPIO_PULL, 0, NULL);
    McuBspGpioSetLevel(MCU_DRV_JY60_CTRL_POWER_GPIO_PORT, MCU_DRV_JY60_CTRL_POWER_GPIO_PIN,
                       MCU_BSP_GPIO_OUT_LOW);
	return 0;
}

/* Jy60校准 */
int McuDrvJy60Calibration(void)
{
	/* 发送角度初始化 */
	if (McuBspUartSend(MCU_DRV_JY60_UART_ID, gJy60AngleCalibration, sizeof(gJy60AngleCalibration)) < 0) {
		return -1;
	}
	/* 发送加速读校准 */
	if (McuBspUartSend(MCU_DRV_JY60_UART_ID, gJy60AccCalibration, sizeof(gJy60AccCalibration)) < 0) {
		return -1;
	}
	return 0;
}

/* 垂直或者水平安装 */
int McuDrvJy60InstallType(unsigned char Type)
{
	if (Type == MCU_DRV_JY60_HORIZON_TYPE) {
		if (McuBspUartSend(MCU_DRV_JY60_UART_ID, gJy60Horizon, sizeof(gJy60Horizon)) < 0) {
			return -1;
		}
	} else {
		if (McuBspUartSend(MCU_DRV_JY60_UART_ID, gJy60Vertical, sizeof(gJy60Vertical)) < 0) {
			return -1;
		}
	}
	return 0;
}

/* 获取最新一组数据 */
void McuDrvJy60GetData(float *Ax, float *Ay, float *Az,
                       float *Wx, float *Wy, float *Wz,
                       float *Roll, float *Pitch, float *Yaw)
{
	/* 处理一组数据 */
    McuBspGpioSetLevel(MCU_DRV_JY60_CTRL_POWER_GPIO_PORT, MCU_DRV_JY60_CTRL_POWER_GPIO_PIN,
                           MCU_BSP_GPIO_OUT_HIGH);
	McuDrvJy60Handle();
    McuBspGpioSetLevel(MCU_DRV_JY60_CTRL_POWER_GPIO_PORT, MCU_DRV_JY60_CTRL_POWER_GPIO_PIN,
                           MCU_BSP_GPIO_OUT_LOW);
	/* 将处理完的数据输出 */
	*Ax = gAx;
	*Ay = gAy;
	*Az = gAz;

	*Wx = gWx;
	*Wy = gWy;
	*Wz = gWz;

	*Roll = gRoll;
	*Pitch = gPitch;
	*Yaw = gYaw;
}

/* 处理最新一帧数据 */
static void McuDrvJy60Handle(void)
{
    short Ax;
    short Ay;
    short Az;
    
    short Wx;
    short Wy;
    short Wz;
    
    short Roll;
    short Pitch;
    short Yaw;
    uint32_t i = 0;
    
	unsigned char sum = 0;
	
	unsigned char AxL = 0;
	unsigned char AxH = 0;
	unsigned char AyL = 0;
	unsigned char AyH = 0;
	unsigned char AzL = 0;
	unsigned char AzH = 0;
	
	unsigned char WxL = 0;
	unsigned char WxH = 0;
	unsigned char WyL = 0;
	unsigned char WyH = 0;
	unsigned char WzL = 0;
	unsigned char WzH = 0;
	
	unsigned char RollL = 0;
	unsigned char RollH = 0;
	unsigned char PitchL = 0;
	unsigned char PitchH = 0;
	unsigned char YawL = 0;
	unsigned char YawH = 0;
	
	/* 清空整个接收空间 */
	memset(gJy60RxBuf, 0, sizeof(gJy60RxBuf));
	gJy60RxBufTemp = 0;
	gJy60RxBufLocal = 0;
	while (1) {
        i++;
        if (i > 2000) {
            break;
        }
		if (McuDrvJy60ReadByte() == 0x55) {
			if (McuDrvJy60ReadByte() == 0x51) {
				sum = 0;
				sum += 0x55;
				sum += 0x51;
				AxL = McuDrvJy60ReadByte();
				sum += AxL;
				AxH = McuDrvJy60ReadByte();
				sum += AxH;
				AyL = McuDrvJy60ReadByte();
				sum += AyL;
				AyH = McuDrvJy60ReadByte();
				sum += AyH;
				AzL = McuDrvJy60ReadByte();
				sum += AzL;
				AzH = McuDrvJy60ReadByte();
				sum += AzH;
				sum += McuDrvJy60ReadByte(); // 模块温度低不获取
				sum += McuDrvJy60ReadByte(); // 模块温度高不获取
				if (sum == McuDrvJy60ReadByte()) {
					break;
				}
			}
		}
        HAL_Delay(1);
	}
    i = 0;
	while (1) {
        i++;
        if (i > 2000) {
            break;
        }
		if (McuDrvJy60ReadByte() == 0x55) {
			if (McuDrvJy60ReadByte() == 0x52) {
				sum = 0;
				sum += 0x55;
				sum += 0x52;
				WxL = McuDrvJy60ReadByte();
				sum += WxL;
				WxH = McuDrvJy60ReadByte();
				sum += WxH;
				WyL = McuDrvJy60ReadByte();
				sum += WyL;
				WyH = McuDrvJy60ReadByte();
				sum += WyH;
				WzL = McuDrvJy60ReadByte();
				sum += WzL;
				WzH = McuDrvJy60ReadByte();
				sum += WzH;
				sum += McuDrvJy60ReadByte();
				sum += McuDrvJy60ReadByte();
				if (sum == McuDrvJy60ReadByte()) {
					break;
				}
			}
		}
        HAL_Delay(1);
	}
    i = 0;
	while (1) {
        i++;
        if (i > 2000) {
            break;
        }
		if (McuDrvJy60ReadByte() == 0x55) {
			if (McuDrvJy60ReadByte() == 0x53) {
				sum = 0;
				sum += 0x55;
				sum += 0x53;
				RollL = McuDrvJy60ReadByte();
				sum += RollL;
				RollH = McuDrvJy60ReadByte();
				sum += RollH;
				PitchL = McuDrvJy60ReadByte();
				sum += PitchL;
				PitchH = McuDrvJy60ReadByte();
				sum += PitchH;
				YawL = McuDrvJy60ReadByte();
				sum += YawL;
				YawH = McuDrvJy60ReadByte();
				sum += YawH;
				sum += McuDrvJy60ReadByte();
				sum += McuDrvJy60ReadByte();
				if (sum == McuDrvJy60ReadByte()) {
					break;
				}
			}
		}
        HAL_Delay(1);
	}
    Ax = ((((short)AxH) << 8) | AxL);
    Ay = ((((short)AyH) << 8) | AyL);
    Az = ((((short)AzH) << 8) | AzL);
   
    Wx = ((((short)WxH) << 8) | WxL);
    Wy = ((((short)WyH) << 8) | WyL);
    Wz = ((((short)WzH) << 8) | WzL);
    
    Roll = ((((short)RollH) << 8) | RollL);
    Pitch = ((((short)PitchH) << 8) | PitchL);
    Yaw = ((((short)YawH) << 8) | YawL);
    
	/* 计算 */
	gAx = Ax / 32768.0f * 16 * 9.8f;
	gAy = Ay / 32768.0f * 16 * 9.8f;
	gAz = Az / 32768.0f * 16 * 9.8f;
	
	gWx = Wx / 32768.0f * 2000.0f;
	gWy = Wy / 32768.0f * 2000.0f;
	gWz = Wz / 32768.0f * 2000.0f;
	
	gRoll = Roll / 32768.0f * 180.0f;
	gPitch = Pitch / 32768.0f * 180.0f;
	gYaw = Yaw / 32768.0f * 180.0f;
}

/* 读取一个字符 */
static unsigned char McuDrvJy60ReadByte(void)
{
	while (gJy60RxBufLocal == gJy60RxBufTemp){
	}
	gJy60RxBufLocal++;
	gJy60RxBufLocal = gJy60RxBufLocal % JY60_RXBUF_LEN;
	return gJy60RxBuf[gJy60RxBufLocal];
}

/* 接收函数 */
static void McuDrvJy60Recv(unsigned char data)
{
	gJy60RxBuf[gJy60RxBufTemp] = data;
	gJy60RxBufTemp ++;
	gJy60RxBufTemp = gJy60RxBufTemp % JY60_RXBUF_LEN;
}