/**
* @file
* @brief    
* @details  
* @author   Junlong Cheng. Email: chengjunlong@whu.edu.cn
* @date     2021/10/19
* @version  1.0
* @par      Copyright(c) 2012-2020 School of Geodesy and Geomatics, University of Wuhan. All Rights Reserved.
* @par      History:
*           2021/10/19,Junlong Cheng, new \n
*/

#ifndef _HONEYWELL_I300_HPP_
#define _HONEYWELL_I300_HPP_

#include "common_utils.hpp"

#ifdef WIN_DLL
#define EXPORT __declspec(dllexport) /* for Windows DLL */
#else
#define EXPORT
#endif

#define HW_I300_PREAMB  0x0E        /* frame preamble */
#define HW_I300_A1_LEN        20
#define HW_I300_A2_LEN        44
#define HW_I300_A3_LEN        32
#define HW_I300_AC_LEN        26
#define HW_I300_AD_LEN        50
#define HW_I300_AE_LEN        38

#define P2_05           3.125000000000000E-02 /* 2^-05 */
#define P2_11           4.882812500000000E-04 /* 2^-11 */
#define P2_27           7.450580596923828E-09 /* 2^-27 */
#define P2_33           1.164153218269348E-10 /* 2^-33 */

#define NAVDELTAANGSCALE   (P2_33)        /* 2^-33 */
#define NAVDELTAVELSCALE   (0.3048*P2_27) /* 0.3048*2^-27 */

typedef struct {
    // 时间
    int     week;
    int     sow;
    int     usec;
    int     status;
    long    seq;
    
    // 角度增量，单位为radians
    double  gx;
    double  gy;
    double  gz;
    
    // 速度增量，单位为m/sec
    double  ax;
    double  ay;
    double  az;
} raw_hw_i300_t;

// 小端模式
static unsigned short  get_u16_checksum(const unsigned char *buff, int len)
{
    int     num = (len - 2)/2;
    unsigned short  checksum = 0;
    unsigned short  tempval  = 0;
    
    for(int i = 0; i < num; i++)
    {        
        memcpy(&tempval, &buff[2*i], 2);
        checksum += tempval;
    }
    
    return checksum;
}

EXPORT int decode_raw_hw_i300_OXA1(unsigned char *buff, const int len)
{
    return 0;
}

EXPORT int decode_raw_hw_i300_OXA2(unsigned char *buff, const int len)
{
    //1       IMU Address - 0x0E  1
    //2       Message ID - 0xA2   1
    //3-8     Control Data        12
    //9-10    Status              4
    //11-16   Navigation Data     24
    //17      Checksum            2
    //        Total Bytes         44
    
    if (len < HW_I300_A2_LEN || buff[0] != HW_I300_PREAMB || buff[1] != 0xA2)
        return 0;
    
    return 0;
}

EXPORT int decode_raw_hw_i300_OXA3(const unsigned char *buff, const int len, raw_hw_i300_t *rawImu, const bool bScale = false)
{
    // 1      IMU Address-0x0E    1
    // 2      Message ID-0xA3     1
    //3-8     Navigation Data     24
    //9-10    Status              4   
    // 11     Checksum            2
    //          Total Bytes       32
    int		i = 2, val = 0;
	double	scale = 1.0;
    unsigned short  checksum;
    raw_hw_i300_t   imuData = { 0 };
    
    if (len < HW_I300_A3_LEN || buff[1] != 0xA3)
        return 0;
    
    // Navigation Data
    //14  Delta Angle X       4   2^-33           radians or equivalently,radians/second/Hz
    //15  Delta Angle Y       4   2^-33           radians or equivalently,radians/second/Hz
    //16  Delta Angle Z       4   2^-33           radians or equivalently,radians/second/Hz
	memcpy(&val, &buff[i], 4); i += 4; imuData.gx = val;
	memcpy(&val, &buff[i], 4); i += 4; imuData.gy = val;
	memcpy(&val, &buff[i], 4); i += 4; imuData.gz = val;
	if (bScale)
	{
		scale = pow(2, -33);
		imuData.gx *= scale;
		imuData.gy *= scale;
		imuData.gz *= scale;
	}

	//17  Delta Velocity X    4   0.3048*2^-27    m/sec or equivalently, m/sec2/Hz
	//18  Delta Velocity Y    4   0.3048*2^-27    m/sec or equivalently, m/sec2/Hz
	//19  Delta Velocity Z    4   0.3048*2^-27    m/sec or equivalently, m/sec2/Hz
	memcpy(&val, &buff[i], 4); i += 4; imuData.ax = val;
	memcpy(&val, &buff[i], 4); i += 4; imuData.ay = val;
	memcpy(&val, &buff[i], 4); i += 4; imuData.az = val;
	if (bScale)
	{
		scale = 0.3048 * pow(2, -27);
		imuData.ax *= scale;
		imuData.ay *= scale;
		imuData.az *= scale;
	}
    
    imuData.status          = getbits(buff,i*8,32); i+=4;
    
    memcpy(&checksum, &buff[30], 2);
    if (get_u16_checksum(buff, HW_I300_A3_LEN) != checksum)
        return 0;
    
    if (0)
    {
       printf(" %X %.10lf %.10lf %.10lf %.10lf %.10lf %.10lf\n", imuData.status, 
            imuData.gx, imuData.gy, imuData.gz, 
            imuData.ax, imuData.ay, imuData.az);     
    }
    
    if (rawImu != NULL)
    {
        // 角度增量，单位为radians
        rawImu->gx = imuData.gx;
        rawImu->gy = imuData.gy;
        rawImu->gz = imuData.gz;
	     
        // 速度增量，单位为m/sec
        rawImu->ax = imuData.ax;
        rawImu->ay = imuData.ay;
        rawImu->az = imuData.az; 
    }
    
    return 1;
}

EXPORT int decode_raw_hw_i300_OXAC(unsigned char *buff, const int len)
{
    return 0;
}

EXPORT int decode_raw_hw_i300_OXAD(unsigned char *buff, const int len)
{
    return 0;
}

EXPORT int decode_raw_hw_i300_OXAE(unsigned char *buff, const int len)
{
    return 0;
}

EXPORT int decode_raw_hw_i300(unsigned char *buff, const int len)
{
    return 0;
}

EXPORT void init_hw_i300_ins_header(INS_HEAR& head, const int week, const double sow)
{
	strcpy(head.szHeader, "$IMURAW");
	head.bIsIntelOrMotorola = ' ';
	head.dVersionNumber = 0.0;
	head.bDeltaTheta = 1;
	head.bDeltaVelocity = 1;
	head.dDataRateHz = 200;
	head.dGyrosScaleFactor = pow(2, -33);
	head.dAccelScaleFactor = pow(2, -27) * 0.3048;

	head.iUtcOrGpsTime = 2;
	head.iRcvTimeOrCorrTime = 2;
	head.dTimeTagBias = 0.0;
	strcpy(head.szImuName, "HW_I300");

	double	ep[6];
	time2epoch(gpst2time(week, sow), ep);
	head.tCreate.year = (short)ep[0];
	head.tCreate.month = (short)ep[1];
	head.tCreate.day = (short)ep[2];
	head.tCreate.hour = (short)ep[3];
	head.tCreate.minute = (short)ep[4];
	head.tCreate.second = (short)ep[5];
}

bool hw_i300_to_imu(const std::string bin_file, const std::string imu_file)
{
	size_t	i = 0, nr = 0, nbyte = 0;
	unsigned char	data[1024] = { 0 };
	unsigned char	buff[1024] = { 0 };

	int			flag = 1;
	INS_HEAR	head;
	raw_hw_i300_t	rawImu;

	const int len = 70;
	const unsigned char PREAMB = 0x24;

	FILE	*fp_bin = fopen(bin_file.c_str(), "rb");
	if (fp_bin == NULL)
	{
		fprintf(stdout, " stim300_to_imu file open falied, %s", bin_file.c_str());
		return false;
	}

	FILE	*fp_imu = fopen(imu_file.c_str(), "w");
	if (fp_imu == NULL)
	{
		fprintf(stdout, " stim300_to_imu file open falied, %s\n", bin_file.c_str());
		return false;
	}

	while (!feof(fp_bin))
	{
		if ((nr = fread(data, sizeof(unsigned char), sizeof(data), fp_bin)) < 1)
			break;

		for (i = 0; i < nr; i++)
		{
			// 帧头同步
			if (nbyte == 0) {
				if (data[i] != PREAMB)
					continue;

				buff[nbyte++] = data[i];
				continue;
			}

			// 当前帧的数据
			buff[nbyte++] = data[i];
			if (nbyte < len)
				continue;

			nbyte = 0; // 当前帧的计数清空

			// 解析时间
			flag = 1;
			flag = flag & sscanf((char *)&buff[5], "%ld", &rawImu.seq);
			flag = flag & sscanf((char *)&buff[15], "%d", &rawImu.week);
			flag = flag & sscanf((char *)&buff[22], "%d", &rawImu.sow);
			flag = flag & sscanf((char *)&buff[29], "%d", &rawImu.usec);
			if (!flag)
			{
				buff[36] = 0;
				printf(" imuHandler_stim300 decode time failed, %s\n", buff);
				continue;
			}

			if (!decode_raw_hw_i300_OXA3(buff + 36, HW_I300_A3_LEN, &rawImu))
			{
				printf(" imuHandler_stim300 decode_raw_hw_i300_OXA3 failed\n");
				break;
			}

			if (fp_imu)
			{
				if (0 == ftell(fp_imu))
				{
					// IMU文件头
					init_hw_i300_ins_header(head, rawImu.week, rawImu.sow + rawImu.usec*1.0E-06);
					Write_IMU_Head(fp_imu, head);
				}

				fprintf(fp_imu, "%.6f %d %d %d %d %d %d\n", rawImu.sow + rawImu.usec*1.0E-06,
					(int)rawImu.gx, (int)rawImu.gy, (int)rawImu.gz,
					(int)rawImu.ax, (int)rawImu.ay, (int)rawImu.az);
			}
		}
	}

	if (fp_imu != NULL) { fclose(fp_imu); fp_imu = NULL; }
	if (fp_bin != NULL) { fclose(fp_bin); fp_bin = NULL; }

	return true;
}

void convert_hw_i300(const std::string log_file)
{
	if (log_file == "")
		return;

	std::string	imu_file = log_file + ".imu";
	std::string	imr_file = log_file + ".imr";
	if (log_file.length() > 4 &&
		log_file.substr(log_file.length() - 4, log_file.length()) == ".log")
	{
		imu_file = log_file.substr(0, log_file.length() - 4) + ".imu";
		imr_file = log_file.substr(0, log_file.length() - 4) + ".imr";
	}

	if (false == hw_i300_to_imu(log_file, imu_file))
		return;

	ConverteIMU2IMR(imu_file, imr_file);

	return;
}


#endif