#include "main.h"
#include "mmc5983.h"
#include "UartDebug.h"
#include "i2c.h"
#include "Algorithm.h"

#define	MAG_AVG_CNT					2		//每2个样本平均输出1次,输出速率=20Hz/2=10Hz
#define	MAG_POLL_MS					20		//测量完成标志轮询周期,与20Hz测量周期一致
#define	MAG_POLL_FAIL_MAX			5		//连续5次(100ms)轮询无数据则重写配置自恢复

static bool InitFlag;
static bool MMC5983IntFlag;
static MMC5983Var_t MMC5983Var;
static int32_t mag_array[XYZ][MAG_AVG_CNT];
static uint8_t magcnt;
static uint8_t PollFailCnt;

extern void MmcDataProcessCallBack(float *Mag);

/*****************************************************************************************
* 函数名称: MMC5983_Write_Reg
* 功能描述: MMC5983写寄存器
* 参    数: reg 寄存器地址
						val 寄存器数据
* 返 回 值: 成功返回true，失败返回false
*****************************************************************************************/
static bool MMC5983_Write_Reg(uint8_t reg, uint8_t val)
{
	return I2C_MasterWriteData(M0P_I2C0, MMC5983_ADDRESS, reg, &val, 1);
}

/*****************************************************************************************
* 函数名称: MMC5983_Read_Reg
* 功能描述: MMC5983读寄存器
* 参    数: reg 寄存器地址
* 返 回 值: 返回寄存器值
*****************************************************************************************/
static uint8_t MMC5983_Read_Reg(uint8_t reg)
{
	uint8_t res;
	if(I2C_MasterReadData(M0P_I2C0, MMC5983_ADDRESS, reg, &res, 1) == false)
		res = 0;
	return res;
}

/*****************************************************************************************
* 函数名称: MMC5983_Read_Buffer
* 功能描述: MMC5983读数据
* 参    数: reg 寄存器地址
						buffer 读取数据输出缓存入口
						len 读取长度
* 返 回 值: 成功返回true，失败返回false
*****************************************************************************************/
static bool MMC5983_Read_Buffer(uint8_t reg, void *buffer, uint8_t len)
{
	return I2C_MasterReadData(M0P_I2C0, MMC5983_ADDRESS, reg, buffer, len);
}

/*****************************************************************************************
* 函数名称: MMC5983StartContinuous
* 功能描述: MMC5983开启20Hz连续转换(常开,不进入低功耗)
* 参    数: 无
* 返 回 值: 无
*****************************************************************************************/
static void MMC5983StartContinuous(void)
{
	MMC5983RegCtrl0 Ctrl0Reg;
	MMC5983RegCtrl2 Ctrl2Reg;		//开启/关闭连续转换
	Ctrl0Reg.Byte = 0x00;
	Ctrl0Reg.Bit.Auto_SR_en = 1;			//自动SET/RESET,消除温漂零场漂移
	Ctrl0Reg.Bit.NT_meas_done_en = 1;		//测量完成中断使能(Stop会清CTRL0,启动时必须重写)
	MMC5983_Write_Reg(MMC5983_CTRL_0, Ctrl0Reg.Byte);
	Ctrl2Reg.Byte = 0x00;
	Ctrl2Reg.Bit.Cm_freq = 0x03;	//011=20Hz连续测量
	Ctrl2Reg.Bit.Cmm_en = 1;		//连续设1，单次设0
	Ctrl2Reg.Bit.Prd_set = 3;		//每100次测量自动SET一次,消除温漂
	Ctrl2Reg.Bit.En_prd_set = 1;
	MMC5983_Write_Reg(MMC5983_CTRL_2, Ctrl2Reg.Byte);
}

/*****************************************************************************************
* 函数名称: MMC5983_Init
* 功能描述: MMC5983初始化
* 参    数: 无
* 返 回 值: 无
*****************************************************************************************/
static void MMC5983_Init(void)
{
	uint8_t id;

	id	= MMC5983_Read_Reg(MMC5983_ID);
	//DBG_LOG("MMC5983 id=0x%x\r\n",id);
	MMC5983Var.MMC5983Delay1mSCnt = 100;
	while(id != 0x30){
		if(MMC5983Var.MMC5983Delay1mSCnt <= 0) {
			DBG_LOG("MMC5983 INIT Err...\r\n");
			InitFlag = false;
			return;
		}
	}

	MMC5983RegCtrl1 Ctrl1Reg;
	Ctrl1Reg.Byte = 0x00;
	Ctrl1Reg.Bit.SW_RST = 1;
	MMC5983_Write_Reg(MMC5983_CTRL_1, Ctrl1Reg.Byte);
	MMC5983Delay(15);

	MMC5983RegCtrl2 Ctrl2Reg;		//连续模式在StartContinuous中开启
	Ctrl2Reg.Byte = 0x00;
	MMC5983_Write_Reg(MMC5983_CTRL_2, Ctrl2Reg.Byte);

	MMC5983RegCtrl3 Ctrl3Reg;
	Ctrl3Reg.Byte = 0x00;
	Ctrl3Reg.Bit.St_enm = 1;
	Ctrl3Reg.Bit.St_enp = 1;
	MMC5983_Write_Reg(MMC5983_CTRL_3, Ctrl3Reg.Byte);

	MMC5983RegCtrl0 Ctrl0Reg;
	Ctrl0Reg.Byte = 0x00;
	Ctrl0Reg.Bit.Auto_SR_en = 1;			//自动SET/RESET,消除温漂零场漂移
	Ctrl0Reg.Bit.NT_meas_done_en = 1;		//测量完成中断使能
	MMC5983_Write_Reg(MMC5983_CTRL_0, Ctrl0Reg.Byte);

	if(InitFlag == false){
		DBG_LOG("MMC5983 INIT OK...\r\n");
		InitFlag = true;
	}
}

/*****************************************************************************************
* 函数名称: MMC5983_Read
* 功能描述: MMC5983读取原始数据(18bit)
* 参    数: MagAdcX X轴数据
				MagAdcY Y轴数据
				MagAdcZ Z轴数据
* 返 回 值: 成功返回true，失败返回false
*****************************************************************************************/
static bool MMC5983_Read(int32_t *MagAdcX, int32_t *MagAdcY, int32_t *MagAdcZ)
{
	static uint8_t buff[7];

	 if (MMC5983_Read_Buffer(MMC5983_X_OUT_H, buff, 7) == false)
        return false;

    *MagAdcX = (buff[0] << 10) | (buff[1] << 2) | ((buff[6] & 0xC0) >> 6);
    *MagAdcY = (buff[2] << 10) | (buff[3] << 2) | ((buff[6] & 0x30) >> 4);
    *MagAdcZ = (buff[4] << 10) | (buff[5] << 2) | ((buff[6] & 0x0C) >> 2);// Turn the 18 bits into unsigned 32-bit value
    return true;
}

/*****************************************************************************************
* 函数名称: MMC5983_Get_Mag
* 功能描述: 外部获取MMC5983磁场强度数据
* 参    数: mag 读取数据输出缓存入口
* 返 回 值: 无
*****************************************************************************************/
void MMC5983_Get_Mag(float *mag)
{
	if(mag == NULL)
		return;

	float ADVal[XYZ];

	ADVal[X] = MMC5983Var.Mag_Original[X] - 131072.0f;
	ADVal[Y] = MMC5983Var.Mag_Original[Y] - 131072.0f;
	ADVal[Z] = MMC5983Var.Mag_Original[Z] - 131072.0f;

	mag[0] = (float) (ADVal[X] * MMC5983_MAG_SCALE_18BIT);
   mag[1] = (float) (ADVal[Y] * MMC5983_MAG_SCALE_18BIT);
	mag[2] = (float) (ADVal[Z] * MMC5983_MAG_SCALE_18BIT);
}

/*****************************************************************************************
* 函数名称: MMC5983MeasDoneProcess
* 功能描述: 单次测量完成处理:读取样本、清除中断锁存,凑够MAG_AVG_CNT个样本后平均输出
* 参    数: 无
* 返 回 值: 无
*****************************************************************************************/
static void MMC5983MeasDoneProcess(void)
{
	MMC5983Var.MMC5983Delay1mSCnt = MAG_POLL_MS;
	if(MMC5983_Read(&mag_array[X][magcnt], &mag_array[Y][magcnt], &mag_array[Z][magcnt])){
		magcnt++;
	}
	MMC5983_Write_Reg(MMC5983_STATUS, 0x01);	//写1清Meas_M_Done,释放INT锁存,下次测量产生新上升沿
	if(magcnt >= MAG_AVG_CNT){
		MMC5983Var.Mag_Original[X] = AverageFilter_32t(mag_array[X], magcnt);
		MMC5983Var.Mag_Original[Y] = AverageFilter_32t(mag_array[Y], magcnt);
		MMC5983Var.Mag_Original[Z] = AverageFilter_32t(mag_array[Z], magcnt);
		magcnt = 0;
		MMC5983_Get_Mag(MMC5983Var.Mag);
		MmcDataProcessCallBack(MMC5983Var.Mag);
	}
}

/*****************************************************************************************
* 函数名称: MMC5983LoopHandler
* 功能描述: MMC5983循环处理函数,20Hz连续采样,2样本平均后10Hz输出
* 参    数: 无
* 返 回 值: 无
*****************************************************************************************/

void MMC5983LoopHandler(void)
{
	switch(MMC5983Var.Status) {
		case MMC5983_STATUS_IDLE:{
		break;}

		case MMC5983_STATUS_START:{
			if(!InitFlag){
				MMC5983_Init();
				if(!InitFlag){
					MMC5983Var.Status = MMC5983_STATUS_IDLE;
					break;
				}
			}
			magcnt = 0;
			PollFailCnt = 0;
			MMC5983StartContinuous();					//开启20Hz连续测量
			MMC5983Var.MMC5983Delay1mSCnt = MAG_POLL_MS;
			MMC5983Var.Status = MMC5983_STATUS_WAIT_READ;
		break;}

		case MMC5983_STATUS_WAIT_READ:{
			if(MMC5983IntFlag == true){
				MMC5983IntFlag = false;
				PollFailCnt = 0;
				MMC5983MeasDoneProcess();				//INT触发,立即读数
			}
			else if(MMC5983Var.MMC5983Delay1mSCnt <= 0){
				uint8_t StatusReg;
				MMC5983Var.MMC5983Delay1mSCnt = MAG_POLL_MS;	//20ms轮询一次测量完成标志
				StatusReg = MMC5983_Read_Reg(MMC5983_STATUS);
				if((StatusReg & 0x01) != 0){			//Meas_M_Done=1,测量完成
					PollFailCnt = 0;
					MMC5983MeasDoneProcess();
				}
				else{
					PollFailCnt++;
					if(PollFailCnt >= MAG_POLL_FAIL_MAX){
						PollFailCnt = 0;
						MMC5983StartContinuous();		//连续无数据,重写配置自恢复
					}
				}
			}
		break;}

		case MMC5983_STATUS_STOP:{
			MMC5983Stop();
			MMC5983Var.Status = MMC5983_STATUS_IDLE;
		break;}
	}
}

/*****************************************************************************************
* 函数名称: MMC59831mSRoutine
* 功能描述: MMC59831ms时基循环处理函数
* 参    数: 无
* 返 回 值: 无
*****************************************************************************************/
void MMC59831mSRoutine(void)
{
	if(MMC5983Var.MMC5983Delay1mSCnt > 0)
		MMC5983Var.MMC5983Delay1mSCnt--;
}

/*****************************************************************************************
* 函数名称: MMC5983Init
* 功能描述: MMC5983初始化
* 参    数: 无
* 返 回 值: 无
*****************************************************************************************/
void MMC5983Init(void)
{
	if(!InitFlag){
		MMC5983_Init();
		return;
	}

	MMC5983Var.Status = MMC5983_STATUS_START;
}

/*****************************************************************************************
* 函数名称: MMC5983Start
* 功能描述: MMC5983启动(幂等,已运行则直接返回)
* 参    数: 无
* 返 回 值: 无
*****************************************************************************************/
void MMC5983Start(void)
{
	if(MMC5983Var.Status == MMC5983_STATUS_WAIT_READ){
		return;									//已在连续采样,不重复配置
	}
	if(!InitFlag){
		MMC5983_Init();
		if(!InitFlag){
			return;
		}
	}

	MMC5983Var.Status = MMC5983_STATUS_START;
}

/*****************************************************************************************
* 函数名称: MMC5983Stop
* 功能描述: MMC5983停止(关闭连续转换)
* 参    数: 无
* 返 回 值: 无
*****************************************************************************************/
void MMC5983Stop(void)
{
	MMC5983RegCtrl2 Ctrl2Reg;
	Ctrl2Reg.Byte = 0x00;
	MMC5983_Write_Reg(MMC5983_CTRL_2, Ctrl2Reg.Byte);		//Cmm_en=0,关闭连续转换

	MMC5983RegCtrl0 Ctrl0Reg;
	Ctrl0Reg.Byte = 0x00;
	MMC5983_Write_Reg(MMC5983_CTRL_0, Ctrl0Reg.Byte);

	MMC5983Var.Status = MMC5983_STATUS_IDLE;
}

/*****************************************************************************************
* 函数名称: MMC5983IntCallBack
* 功能描述: MMC5983中断回调
* 参    数: 无
* 返 回 值: 无
*****************************************************************************************/
void MMC5983IntCallBack(void)
{
	MMC5983IntFlag = true;
}