#include "QMI8658.h"

#include <Wire.h>
#include <math.h>

#define QMI8658_UINT_MG_DPS
//#define M_PI			(3.14159265358979323846f)
#define ONE_G			(9.807f)


static qmi8658_state g_imu;

namespace {
uint8_t qmi8658_address()
{
	return g_imu.slave ? g_imu.slave : QMI8658_ADDR;
}
}


void QMI8658::write_reg(uint8_t reg,uint8_t value)
{
  Wire.beginTransmission(qmi8658_address());
  Wire.write(reg);
  Wire.write(value);
  last_status = Wire.endTransmission();
}

uint8_t QMI8658::read_reg(uint8_t reg)
{
	uint8_t ret = 0;
	unsigned int retry = 0;

	while(retry++ < 5)
	{
	    Wire.beginTransmission(qmi8658_address());
	    Wire.write(reg);
	    last_status = Wire.endTransmission(false);
	    if (last_status != 0) {
	    	continue;
	    }
	    if (Wire.requestFrom((int)qmi8658_address(), 1) != 1) {
	    	continue;
	    }
	    if (Wire.available()) {
	    	ret = (uint8_t)Wire.read();
	    	break;
	    }
	}
	return ret;
}

uint16_t QMI8658::readWord_reg(uint8_t reg)
{
	uint8_t retH=0;
  uint8_t retL=0;

  Wire.beginTransmission(qmi8658_address());
  Wire.write(reg);
  last_status = Wire.endTransmission(false);
  if (last_status != 0) {
  	return 0;
  }
  if (Wire.requestFrom((int)qmi8658_address(), 2) != 2) {
  	return 0;
  }
  if (Wire.available() < 2) {
  	return 0;
  }
  retL = (uint8_t)Wire.read();
  retH = (uint8_t)Wire.read();

	return ((retH << 8) | retL);
}


void QMI8658::read_sensor_data(float acc[3], float gyro[3])
{
	unsigned char	buf_reg[12];
	short 			raw_acc_xyz[3];
	short 			raw_gyro_xyz[3];

	raw_acc_xyz[0] = (short)((unsigned short)( readWord_reg(Qmi8658Register_Ax_L) ));
	raw_acc_xyz[1] = (short)((unsigned short)( readWord_reg(Qmi8658Register_Ay_L) ));
	raw_acc_xyz[2] = (short)((unsigned short)( readWord_reg(Qmi8658Register_Az_L) ));

	raw_gyro_xyz[0] = (short)((unsigned short)( readWord_reg(Qmi8658Register_Gx_L) ));
	raw_gyro_xyz[1] = (short)((unsigned short)( readWord_reg(Qmi8658Register_Gy_L) ));
	raw_gyro_xyz[2] = (short)((unsigned short)( readWord_reg(Qmi8658Register_Gz_L) ));

#if defined(QMI8658_UINT_MG_DPS)
	// mg
  // Serial.println("mg");
	acc[0] = (float)(raw_acc_xyz[0]*1000.0f)/g_imu.ssvt_a - TempAcc.X_Off_Err;
	acc[1] = (float)(raw_acc_xyz[1]*1000.0f)/g_imu.ssvt_a - TempAcc.Y_Off_Err;
	acc[2] = (float)(raw_acc_xyz[2]*1000.0f)/g_imu.ssvt_a - TempAcc.Z_Off_Err;
#else
	// m/s2
  // Serial.println("m/s2");
	acc[0] = (float)(raw_acc_xyz[0]*ONE_G)/g_imu.ssvt_a;
	acc[1] = (float)(raw_acc_xyz[1]*ONE_G)/g_imu.ssvt_a;
	acc[2] = (float)(raw_acc_xyz[2]*ONE_G)/g_imu.ssvt_a;
#endif

#if defined(QMI8658_UINT_MG_DPS)
	// dps
  // Serial.println("dps");
	gyro[0] = (float)(raw_gyro_xyz[0]*1.0f)/g_imu.ssvt_g - TempGyr.X_Off_Err;
	gyro[1] = (float)(raw_gyro_xyz[1]*1.0f)/g_imu.ssvt_g - TempGyr.Y_Off_Err;
	gyro[2] = (float)(raw_gyro_xyz[2]*1.0f)/g_imu.ssvt_g - TempGyr.Z_Off_Err;
#else
	// rad/s
  // Serial.println("rad/s");
	gyro[0] = (float)(raw_gyro_xyz[0]*M_PI)/(g_imu.ssvt_g*180);		// *pi/180
	gyro[1] = (float)(raw_gyro_xyz[1]*M_PI)/(g_imu.ssvt_g*180);
	gyro[2] = (float)(raw_gyro_xyz[2]*M_PI)/(g_imu.ssvt_g*180);
#endif
}

void QMI8658::read_acc(float acc[3])
{
	unsigned char	buf_reg[12];
	short 		raw_acc_xyz[3];

	raw_acc_xyz[0] = (short)((unsigned short)( readWord_reg(Qmi8658Register_Ax_L) ));
	raw_acc_xyz[1] = (short)((unsigned short)( readWord_reg(Qmi8658Register_Ay_L) ));
	raw_acc_xyz[2] = (short)((unsigned short)( readWord_reg(Qmi8658Register_Az_L) ));

#if defined(QMI8658_UINT_MG_DPS)
	// mg
	acc[0] = (float)(raw_acc_xyz[0]*1000.0f)/g_imu.ssvt_a;
	acc[1] = (float)(raw_acc_xyz[1]*1000.0f)/g_imu.ssvt_a;
	acc[2] = (float)(raw_acc_xyz[2]*1000.0f)/g_imu.ssvt_a;
#else
	// m/s2
  // Serial.println("m/s2");
	acc[0] = (float)(raw_acc_xyz[0]*ONE_G)/g_imu.ssvt_a;
	acc[1] = (float)(raw_acc_xyz[1]*ONE_G)/g_imu.ssvt_a;
	acc[2] = (float)(raw_acc_xyz[2]*ONE_G)/g_imu.ssvt_a;
#endif
}

void QMI8658::read_gyro(float gyro[3])
{
	unsigned char	buf_reg[12];
	short 		raw_gyro_xyz[3];

	raw_gyro_xyz[0] = (short)((unsigned short)( readWord_reg(Qmi8658Register_Gx_L) ));
	raw_gyro_xyz[1] = (short)((unsigned short)( readWord_reg(Qmi8658Register_Gy_L) ));
	raw_gyro_xyz[2] = (short)((unsigned short)( readWord_reg(Qmi8658Register_Gz_L) ));

#if defined(QMI8658_UINT_MG_DPS)
	// dps
	gyro[0] = (float)(raw_gyro_xyz[0]*1.0f)/g_imu.ssvt_g;
	gyro[1] = (float)(raw_gyro_xyz[1]*1.0f)/g_imu.ssvt_g;
	gyro[2] = (float)(raw_gyro_xyz[2]*1.0f)/g_imu.ssvt_g;
#else
	// rad/s
  // Serial.println("rad/s");
	gyro[0] = (float)(raw_gyro_xyz[0]*M_PI)/(g_imu.ssvt_g*180);		// *pi/180
	gyro[1] = (float)(raw_gyro_xyz[1]*M_PI)/(g_imu.ssvt_g*180);
	gyro[2] = (float)(raw_gyro_xyz[2]*M_PI)/(g_imu.ssvt_g*180);
#endif
}

float QMI8658::read_temperature()
{
	const int16_t raw_temp = (int16_t)((unsigned short)(readWord_reg(Qmi8658Register_Tempearture_L)));
	return raw_temp / 256.0f;
}



void QMI8658::axis_convert(float data_a[3], float data_g[3], int layout)
{
	float raw[3],raw_g[3];

	raw[0] = data_a[0];
	raw[1] = data_a[1];
	//raw[2] = data[2];
	raw_g[0] = data_g[0];
	raw_g[1] = data_g[1];
	//raw_g[2] = data_g[2];

	if(layout >=4 && layout <= 7)
	{
		data_a[2] = -data_a[2];
		data_g[2] = -data_g[2];
	}

	if(layout%2)
	{
		data_a[0] = raw[1];
		data_a[1] = raw[0];

		data_g[0] = raw_g[1];
		data_g[1] = raw_g[0];
	}
	else
	{
		data_a[0] = raw[0];
		data_a[1] = raw[1];

		data_g[0] = raw_g[0];
		data_g[1] = raw_g[1];
	}

	if((layout==1)||(layout==2)||(layout==4)||(layout==7))
	{
		data_a[0] = -data_a[0];
		data_g[0] = -data_g[0];
	}
	if((layout==2)||(layout==3)||(layout==6)||(layout==7))
	{
		data_a[1] = -data_a[1];
		data_g[1] = -data_g[1];
	}
}



void QMI8658::read_xyz(float acc[3], float gyro[3])
{
	unsigned char	status;
	unsigned char data_ready = 0;

#if defined(QMI8658_SYNC_SAMPLE_MODE)
	status = read_reg(Qmi8658Register_StatusInt);
	if(status&0x01)
	{
		data_ready = 1;
		delayMicroseconds(6);
	}
#else
	status = read_reg(Qmi8658Register_Status0);
	if(status&0x03)
	{
		data_ready = 1;
	}
#endif
	if(data_ready)
	{
		read_sensor_data(acc, gyro);
		axis_convert(acc, gyro, 0);
	#if defined(QMI8658_USE_CALI)
		qmi8658_data_cali(1, acc);
		qmi8658_data_cali(2, gyro);
#endif
		g_imu.imu[0] = acc[0];
		g_imu.imu[1] = acc[1];
		g_imu.imu[2] = acc[2];
		g_imu.imu[3] = gyro[0];
		g_imu.imu[4] = gyro[1];
		g_imu.imu[5] = gyro[2];
	}
	else
	{
		acc[0] = g_imu.imu[0];
		acc[1] = g_imu.imu[1];
		acc[2] = g_imu.imu[2];
		gyro[0] = g_imu.imu[3];
		gyro[1] = g_imu.imu[4];
		gyro[2] = g_imu.imu[5];
		Serial.print("data ready fail!\n");
	}
}



void QMI8658::config_acc(enum qmi8658_AccRange range, enum qmi8658_AccOdr odr, enum qmi8658_LpfConfig lpfEnable, enum qmi8658_StConfig stEnable)
{
	unsigned char ctl_dada;

	switch(range)
	{
		case Qmi8658AccRange_2g:
			g_imu.ssvt_a = (1<<14);
			break;
		case Qmi8658AccRange_4g:
			g_imu.ssvt_a = (1<<13);
			break;
		case Qmi8658AccRange_8g:
			g_imu.ssvt_a = (1<<12);
			break;
		case Qmi8658AccRange_16g:
			g_imu.ssvt_a = (1<<11);
			break;
		default:
			range = Qmi8658AccRange_8g;
			g_imu.ssvt_a = (1<<12);
	}
	if(stEnable == Qmi8658St_Enable)
		ctl_dada = (unsigned char)range|(unsigned char)odr|0x80;
	else
		ctl_dada = (unsigned char)range|(unsigned char)odr;

	write_reg(Qmi8658Register_Ctrl2, ctl_dada);
// set LPF & HPF
	ctl_dada = read_reg(Qmi8658Register_Ctrl5);
	ctl_dada &= 0xf0;
	if(lpfEnable == Qmi8658Lpf_Enable)
	{
		ctl_dada |= A_LSP_MODE_3;
		ctl_dada |= 0x01;
	}
	else
	{
		ctl_dada &= ~0x01;
	}
	//ctl_dada = 0x00;
	write_reg(Qmi8658Register_Ctrl5,ctl_dada);
// set LPF & HPF
}

void QMI8658::config_gyro(enum qmi8658_GyrRange range, enum qmi8658_GyrOdr odr, enum qmi8658_LpfConfig lpfEnable, enum qmi8658_StConfig stEnable)
{
	// Set the CTRL3 register to configure dynamic range and ODR
	unsigned char ctl_dada;

	// Store the scale factor for use when processing raw data
	switch (range)
	{
		case Qmi8658GyrRange_16dps:
			g_imu.ssvt_g = 2048;
			break;
		case Qmi8658GyrRange_32dps:
			g_imu.ssvt_g = 1024;
			break;
		case Qmi8658GyrRange_64dps:
			g_imu.ssvt_g = 512;
			break;
		case Qmi8658GyrRange_128dps:
			g_imu.ssvt_g = 256;
			break;
		case Qmi8658GyrRange_256dps:
			g_imu.ssvt_g = 128;
			break;
		case Qmi8658GyrRange_512dps:
			g_imu.ssvt_g = 64;
			break;
		case Qmi8658GyrRange_1024dps:
			g_imu.ssvt_g = 32;
			break;
		case Qmi8658GyrRange_2048dps:
			g_imu.ssvt_g = 16;
			break;
//		case Qmi8658GyrRange_4096dps:
//			g_imu.ssvt_g = 8;
//			break;
		default:
			range = Qmi8658GyrRange_512dps;
			g_imu.ssvt_g = 64;
			break;
	}

	if(stEnable == Qmi8658St_Enable)
		ctl_dada = (unsigned char)range|(unsigned char)odr|0x80;
	else
		ctl_dada = (unsigned char)range | (unsigned char)odr;
	write_reg(Qmi8658Register_Ctrl3, ctl_dada);

// Conversion from degrees/s to rad/s if necessary
// set LPF & HPF
	ctl_dada = read_reg(Qmi8658Register_Ctrl5);
	ctl_dada &= 0x0f;
	if(lpfEnable == Qmi8658Lpf_Enable)
	{
		ctl_dada |= G_LSP_MODE_3;
		ctl_dada |= 0x10;
	}
	else
	{
		ctl_dada &= ~0x10;
	}
	//ctl_dada = 0x00;
	write_reg(Qmi8658Register_Ctrl5,ctl_dada);
// set LPF & HPF
}

void QMI8658::enableSensors(unsigned char enableFlags)
{
#if defined(QMI8658_SYNC_SAMPLE_MODE)
	write_reg(Qmi8658Register_Ctrl7, enableFlags | 0x80);
#elif defined(QMI8658_USE_FIFO)
	//qmi8658_write_reg(Qmi8658Register_Ctrl7, enableFlags|QMI8658_DRDY_DISABLE);
	write_reg(Qmi8658Register_Ctrl7, enableFlags);
#else
	write_reg(Qmi8658Register_Ctrl7, enableFlags);
#endif
	g_imu.cfg.enSensors = enableFlags&0x03;

	delay(1);
}

void QMI8658::config_reg(unsigned char low_power)
{
	enableSensors(QMI8658_DISABLE_ALL);
	if(low_power)
	{
		g_imu.cfg.enSensors = QMI8658_ACC_ENABLE;
		g_imu.cfg.accRange = Qmi8658AccRange_8g;
		g_imu.cfg.accOdr = Qmi8658AccOdr_LowPower_21Hz;
		g_imu.cfg.gyrRange = Qmi8658GyrRange_1024dps;
		g_imu.cfg.gyrOdr = Qmi8658GyrOdr_250Hz;
	}
	else
	{
		g_imu.cfg.enSensors = QMI8658_ACCGYR_ENABLE;
		g_imu.cfg.accRange = Qmi8658AccRange_16g;
		g_imu.cfg.accOdr = Qmi8658AccOdr_1000Hz;
		g_imu.cfg.gyrRange = Qmi8658GyrRange_2048dps;
		g_imu.cfg.gyrOdr = Qmi8658GyrOdr_1000Hz;
	}

	if(g_imu.cfg.enSensors & QMI8658_ACC_ENABLE)
	{
		config_acc(g_imu.cfg.accRange, g_imu.cfg.accOdr, Qmi8658Lpf_Disable, Qmi8658St_Disable);
	}
	if(g_imu.cfg.enSensors & QMI8658_GYR_ENABLE)
	{
		config_gyro(g_imu.cfg.gyrRange, g_imu.cfg.gyrOdr, Qmi8658Lpf_Disable, Qmi8658St_Disable);
	}
}

unsigned char QMI8658::get_id(void)
{
	unsigned char qmi8658_chip_id = 0x00;
	unsigned char qmi8658_revision_id = 0x00;
	unsigned char qmi8658_slave[2] = {QMI8658_SLAVE_ADDR_L, QMI8658_SLAVE_ADDR_H};
	int retry = 0;
	unsigned char iCount = 0;
	unsigned char firmware_id[3];
	unsigned char uuid[6];
	unsigned int uuid_low, uuid_high;

	while(iCount<2)
	{
		g_imu.slave = qmi8658_slave[iCount];
		retry = 0;
		while((qmi8658_chip_id != 0x05)&&(retry++ < 5))
		{
			qmi8658_chip_id = read_reg(Qmi8658Register_WhoAmI);
			Serial.printf("Qmi8658Register_WhoAmI = 0x%x\n", qmi8658_chip_id);
		}
		if(qmi8658_chip_id == 0x05)
		{
			qmi8658_on_demand_cali();

			g_imu.cfg.ctrl8_value = 0xc0;
			//QMI8658_INT1_ENABLE, QMI8658_INT2_ENABLE
			write_reg(Qmi8658Register_Ctrl1, 0x60|QMI8658_INT2_ENABLE|QMI8658_INT1_ENABLE);
			qmi8658_revision_id = read_reg(Qmi8658Register_Revision);
			// qmi8658_read_reg(Qmi8658Register_firmware_id, firmware_id, 3);
			// qmi8658_read_reg(Qmi8658Register_uuid, uuid, 6);
			write_reg(Qmi8658Register_Ctrl7, 0x00);
			write_reg(Qmi8658Register_Ctrl8, g_imu.cfg.ctrl8_value);
			// uuid_low = (unsigned int)((unsigned int)(uuid[2]<<16)|(unsigned int)(uuid[1]<<8)|(uuid[0]));
			// uuid_high = (unsigned int)((unsigned int)(uuid[5]<<16)|(unsigned int)(uuid[4]<<8)|(uuid[3]));
			// qmi8658_log("qmi8658_init slave=0x%x Revision=0x%x\n", g_imu.slave, qmi8658_revision_id);
			// qmi8658_log("Firmware ID[0x%x 0x%x 0x%x]\n", firmware_id[2], firmware_id[1],firmware_id[0]);
			// qmi8658_log("UUID[0x%x %x]\n", uuid_high ,uuid_low);
			break;
		}
		iCount++;
	}

	return qmi8658_chip_id;
}


void QMI8658::qmi8658_on_demand_cali(void)
{
	Serial.print("qmi8658_on_demand_cali start\n");
	write_reg(Qmi8658Register_Reset, 0xb0);
	delay(10);	// delay
	write_reg(Qmi8658Register_Ctrl9, (unsigned char)qmi8658_Ctrl9_Cmd_On_Demand_Cali);
	delay(2200);	// delay 2000ms above
	write_reg(Qmi8658Register_Ctrl9, (unsigned char)qmi8658_Ctrl9_Cmd_NOP);
	delay(100);	// delay
	Serial.print("qmi8658_on_demand_cali done\n");
}

unsigned char QMI8658::begin(void)
{
	if(get_id() == 0x05)
	{
#if defined(QMI8658_USE_AMD)
		qmi8658_config_amd();
#endif
#if defined(QMI8658_USE_PEDOMETER)
		qmi8658_config_pedometer(125);
		qmi8658_enable_pedometer(1);
#endif
		config_reg(0);
		enableSensors(g_imu.cfg.enSensors);
    Serial.println("Position your ICM20948 flat and don't move it - calibrating...");
    delay(1000);
    autoOffsets();
    // Serial.print();
		dump_reg();
#if defined(QMI8658_USE_CALI)
		memset(&g_cali, 0, sizeof(g_cali));
#endif
		return 1;
	}
	else
	{
		// Serial.print("qmi8658_init fail\n");
		return 0;
	}
}


void QMI8658::dump_reg(void)
{
	// unsigned char read_data[8];

	// qmi8658_read_reg(Qmi8658Register_Ctrl1, read_data, 8);
	// qmi8658_log("Ctrl1[0x%x]\nCtrl2[0x%x]\nCtrl3[0x%x]\nCtrl4[0x%x]\nCtrl5[0x%x]\nCtrl6[0x%x]\nCtrl7[0x%x]\nCtrl8[0x%x]\n",
	// 				read_data[0],read_data[1],read_data[2],read_data[3],read_data[4],read_data[5],read_data[6],read_data[7]);
}

void QMI8658::autoOffsets(void){
    float acc[3],gyro[3];

    TempAcc.X_Off_Err = 0.0f;
    TempAcc.Y_Off_Err = 0.0f;
    TempAcc.Z_Off_Err = 0.0f;
    TempGyr.X_Off_Err = 0.0f;
    TempGyr.Y_Off_Err = 0.0f;
    TempGyr.Z_Off_Err = 0.0f;

    for(int i=0; i<50; i++){
        QMI8658::read_acc(acc);
        TempAcc.X_Off_Err += acc[0];
        TempAcc.Y_Off_Err += acc[1];
        TempAcc.Z_Off_Err += acc[2];
        delay(10);
    }
    
    TempAcc.X_Off_Err /= 50;
    TempAcc.Y_Off_Err /= 50;
    TempAcc.Z_Off_Err /= 50;
    TempAcc.Z_Off_Err -= 1000.0f;
    
    for(int i=0; i<50; i++){
        QMI8658::read_gyro(gyro);
        TempGyr.X_Off_Err += gyro[0];
        TempGyr.Y_Off_Err += gyro[1];
        TempGyr.Z_Off_Err += gyro[2];
        delay(1);
    }
    
    TempGyr.X_Off_Err /= 50;
    TempGyr.Y_Off_Err /= 50;
    TempGyr.Z_Off_Err /= 50;
    
}