#include "REG.h"
#include "app.h"
#include "cmsis_os2.h"
#include "soft_i2c.h"
#include "i2c_bus.h"
#include "wit_c_sdk.h"
#include <stdint.h>

#define USE_SOFT_IIC 1
#define USE_IIC 1
#if USE_IIC
#if USE_SOFT_IIC
static soft_i2c_t si2c;
#else
static i2c_t i2c;
#endif
#endif

#define ACC_UPDATE		0x01
#define GYRO_UPDATE		0x02
#define ANGLE_UPDATE	0x04
#define MAG_UPDATE		0x08
#define READ_UPDATE		0x80
static volatile char s_cDataUpdate = 0, s_cCmd = 0xff;
static bool g_sensor_enabled = true;   /* 由 callback_task 通过 UART 命令控制 */
static void CopeSensorData(uint32_t uiReg, uint32_t uiRegNum);
static void Delayms(uint16_t ucMs);

#if USE_IIC
static int32_t IICreadBytes(uint8_t ucAddr, uint8_t ucReg, uint8_t *p_ucVal, uint32_t uiLen) {
#if USE_SOFT_IIC
	soft_i2c_start_frame(&si2c);
	soft_i2c_send_frame(&si2c, ucAddr);
	if(soft_i2c_wait_ack_frame(&si2c) != 0)return 0;
	soft_i2c_send_frame(&si2c, ucReg);
	if(soft_i2c_wait_ack_frame(&si2c) != 0)return 0;
	soft_i2c_start_frame(&si2c);
	soft_i2c_send_frame(&si2c, ucAddr+1);
	if(soft_i2c_wait_ack_frame(&si2c) != 0)return 0;
	for(uint32_t i = 0; i < uiLen; i++)
	{
		if(i+1 == uiLen)*p_ucVal++ = soft_i2c_read_frame(&si2c, 0);  //last byte no ask
		else *p_ucVal++ = soft_i2c_read_frame(&si2c, 1);  //  ask
	}
	soft_i2c_stop_frame(&si2c);
	return 1;
#else
	return i2c_mem_read(&i2c, (uint16_t)ucAddr, ucReg, 1, p_ucVal, uiLen, 100) == 0 ? 1 : 0;
#endif
}
#endif

#if USE_IIC
static int32_t IICwriteBytes (uint8_t ucAddr, uint8_t ucReg, uint8_t *p_ucVal, uint32_t uiLen) {
#if USE_SOFT_IIC
	soft_i2c_start_frame(&si2c);
	soft_i2c_send_frame(&si2c, ucAddr);
	if(soft_i2c_wait_ack_frame(&si2c) != 0)return 0;
	soft_i2c_send_frame(&si2c, ucReg);
	if(soft_i2c_wait_ack_frame(&si2c) != 0)return 0;
	for(uint32_t i = 0; i < uiLen; i++)
	{
		soft_i2c_send_frame(&si2c, *p_ucVal++);
		if(soft_i2c_wait_ack_frame(&si2c) != 0)return 0;
	}
	soft_i2c_stop_frame(&si2c);
	return 1;
#else
	return i2c_mem_write(&i2c, (uint16_t)ucAddr, ucReg, 1, p_ucVal, uiLen, 100) == 0 ? 1 : 0;
#endif
}
#endif

static void print_float(const char* str, float v1, float v2, float v3) {
	char v1_sign = v1 > 0 ? '+' : '-';
	char v2_sign = v2 > 0 ? '+' : '-';
	char v3_sign = v3 > 0 ? '+' : '-';
	v1 = v1 > 0 ? v1 : -v1;
	v2 = v2 > 0 ? v2 : -v2;
	v3 = v3 > 0 ? v3 : -v3;
	int i1_x = (int)(v1 * 1000) / 1000;
	int i1_y = (int)(v1 * 1000) % 1000;
	int i2_x = (int)(v2 * 1000) / 1000;
	int i2_y = (int)(v2 * 1000) % 1000;
	int i3_x = (int)(v3 * 1000) / 1000;
	int i3_y = (int)(v3 * 1000) % 1000;
	log_printf("[sensor_task] %s: %c%03d.%03d, %c%03d.%03d, %c%03d.%03d\r\n", str, v1_sign, i1_x, i1_y, v2_sign, i2_x, i2_y, v3_sign, i3_x, i3_y);
}
void sensor_task(void)
{
	int i;
	int ret;
	imu_data_t imu;
#if USE_IIC
#if USE_SOFT_IIC
	ret = soft_i2c_init(&si2c, 0);
#else
	ret = i2c_init(&i2c, 1);
#endif
#endif
	log_printf("[sensor_task] i2c init = %d\r\n", ret);
#if USE_IIC
	WitInit(WIT_PROTOCOL_I2C, 0x50);
	WitI2cFuncRegister(IICwriteBytes, IICreadBytes);
#else
	WitInit(WIT_PROTOCOL_NORMAL, 0x50);
	WitSerialWriteRegister();
#endif
	WitRegisterCallBack(CopeSensorData);
	WitDelayMsRegister(Delayms);

	uint32_t last_tick = osKernelGetTickCount();

	while (1)
	{
		if(g_sensor_enabled) {
		    WitReadReg(AX, 13);
		}
		last_tick += pdMS_TO_TICKS(5);  /* 500Hz 轮询 */
		if(g_sensor_enabled && s_cDataUpdate)
		{
			for(i = 0; i < 3; i++)
			{
				imu.acc[i] = sReg[AX+i] * 0.0054931640625f;
				imu.gyro[i] = sReg[GX+i] * 0.0054931640625f;
				imu.angle[i] = sReg[Roll+i] * 0.0054931640625f;
			}
			int16_t temp = sReg[TEMP];
			// if(s_cDataUpdate & ACC_UPDATE)
			// {
			// 	print_float("acc", imu.acc[0], imu.acc[1], imu.acc[2]);
			// 	s_cDataUpdate &= ~ACC_UPDATE;
			// }
			// if(s_cDataUpdate & GYRO_UPDATE)
			// {
			// 	print_float("gyro", imu.gyro[0], imu.gyro[1], imu.gyro[2]);
			// 	s_cDataUpdate &= ~GYRO_UPDATE;
			// }
			if(s_cDataUpdate & ANGLE_UPDATE)
			{
				print_float("angle", imu.angle[0], imu.angle[1], imu.angle[2]);
				s_cDataUpdate &= ~ANGLE_UPDATE;
			}
			if(s_cDataUpdate & READ_UPDATE)
			{
				char sign = temp > 0 ? '+' : '-';
				if (temp < 0) {
					temp = -temp;
				}
				log_printf("[sensor_task] temp data: %c%3d.%02d\n", sign, temp/100, temp%100);
				s_cDataUpdate &= ~READ_UPDATE;
			}
			/* 将 IMU 数据推送到 control_task */
			if (g_imu_queue != NULL) {
				osMessageQueuePut(g_imu_queue, &imu, 0, 5);
			}
		}
		if (osDelayUntil(last_tick) != osOK) {
		    last_tick = osKernelGetTickCount();
		}
	}
}

/* ============================================================================
 * Sensor task API — 由 callback_task 通过 UART 命令调用
 * ============================================================================ */

void sensor_enable(void)
{
    g_sensor_enabled = true;
    log_printf("[sensor_task] enabled\r\n");
}

void sensor_disable(void)
{
    g_sensor_enabled = false;
    log_printf("[sensor_task] disabled\r\n");
}

void sensor_acc_cali_enable(void)
{
    WitStartAccCali();
}

void sensor_acc_cali_disable(void)
{
    WitStopAccCali();
}

void sensor_mag_cali_enable(void)
{
    WitStartMagCali();
}

void sensor_mag_cali_disable(void)
{
    WitStopMagCali();
}

static void CopeSensorData(uint32_t uiReg, uint32_t uiRegNum)
{
	int i;
    for(i = 0; i < uiRegNum; i++)
    {
        switch(uiReg)
        {
//            case AX:
//            case AY:
            // case AZ:
			// 	s_cDataUpdate |= ACC_UPDATE;
            // break;
//            case GX:
//            case GY:
            // case GZ:
			// 	s_cDataUpdate |= GYRO_UPDATE;
            // break;
//            case HX:
//            case HY:
            // case HZ:
			// 	s_cDataUpdate |= MAG_UPDATE;
            // break;
//            case Roll:
//            case Pitch:
            case Yaw:
				s_cDataUpdate |= ANGLE_UPDATE;
            break;
            default:
				s_cDataUpdate |= READ_UPDATE;
			break;
        }
		uiReg++;
    }
}

static void Delayms(uint16_t ucMs)
{
	osDelay(pdMS_TO_TICKS(ucMs));
}