#ifndef _TIME_UTILS_HPP_
#define _TIME_UTILS_HPP_

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <time.h>
#include <sstream>
#include <math.h>

#define PI				(atan(1) * 4)
#ifdef _WINDOWS
#include <windows.h>
#endif

#ifndef _WINDOWS
#include <unistd.h>
#include <sys/time.h>
#include <sys/stat.h>
#include <sys/types.h>
#endif // !_WINDOWS

#include "X_Define_Internal.h"

using namespace std;

#ifdef __cplusplus
extern "C" {
#endif

typedef struct {        /* time struct */
    time_t time;        /* time (s) expressed by standard time_t */
    double sec;         /* fraction of second under 1 s */
} gtime_t;

const static double gpst0[]={1980,1, 6,0,0,0}; /* gps time reference */
const static double gst0 []={1999,8,22,0,0,0}; /* galileo system time reference */
const static double bdt0 []={2006,1, 1,0,0,0}; /* beidou time reference */

static const unsigned short tbl_CRC16[]={
    0x0000,0x1021,0x2042,0x3063,0x4084,0x50A5,0x60C6,0x70E7,
    0x8108,0x9129,0xA14A,0xB16B,0xC18C,0xD1AD,0xE1CE,0xF1EF,
    0x1231,0x0210,0x3273,0x2252,0x52B5,0x4294,0x72F7,0x62D6,
    0x9339,0x8318,0xB37B,0xA35A,0xD3BD,0xC39C,0xF3FF,0xE3DE,
    0x2462,0x3443,0x0420,0x1401,0x64E6,0x74C7,0x44A4,0x5485,
    0xA56A,0xB54B,0x8528,0x9509,0xE5EE,0xF5CF,0xC5AC,0xD58D,
    0x3653,0x2672,0x1611,0x0630,0x76D7,0x66F6,0x5695,0x46B4,
    0xB75B,0xA77A,0x9719,0x8738,0xF7DF,0xE7FE,0xD79D,0xC7BC,
    0x48C4,0x58E5,0x6886,0x78A7,0x0840,0x1861,0x2802,0x3823,
    0xC9CC,0xD9ED,0xE98E,0xF9AF,0x8948,0x9969,0xA90A,0xB92B,
    0x5AF5,0x4AD4,0x7AB7,0x6A96,0x1A71,0x0A50,0x3A33,0x2A12,
    0xDBFD,0xCBDC,0xFBBF,0xEB9E,0x9B79,0x8B58,0xBB3B,0xAB1A,
    0x6CA6,0x7C87,0x4CE4,0x5CC5,0x2C22,0x3C03,0x0C60,0x1C41,
    0xEDAE,0xFD8F,0xCDEC,0xDDCD,0xAD2A,0xBD0B,0x8D68,0x9D49,
    0x7E97,0x6EB6,0x5ED5,0x4EF4,0x3E13,0x2E32,0x1E51,0x0E70,
    0xFF9F,0xEFBE,0xDFDD,0xCFFC,0xBF1B,0xAF3A,0x9F59,0x8F78,
    0x9188,0x81A9,0xB1CA,0xA1EB,0xD10C,0xC12D,0xF14E,0xE16F,
    0x1080,0x00A1,0x30C2,0x20E3,0x5004,0x4025,0x7046,0x6067,
    0x83B9,0x9398,0xA3FB,0xB3DA,0xC33D,0xD31C,0xE37F,0xF35E,
    0x02B1,0x1290,0x22F3,0x32D2,0x4235,0x5214,0x6277,0x7256,
    0xB5EA,0xA5CB,0x95A8,0x8589,0xF56E,0xE54F,0xD52C,0xC50D,
    0x34E2,0x24C3,0x14A0,0x0481,0x7466,0x6447,0x5424,0x4405,
    0xA7DB,0xB7FA,0x8799,0x97B8,0xE75F,0xF77E,0xC71D,0xD73C,
    0x26D3,0x36F2,0x0691,0x16B0,0x6657,0x7676,0x4615,0x5634,
    0xD94C,0xC96D,0xF90E,0xE92F,0x99C8,0x89E9,0xB98A,0xA9AB,
    0x5844,0x4865,0x7806,0x6827,0x18C0,0x08E1,0x3882,0x28A3,
    0xCB7D,0xDB5C,0xEB3F,0xFB1E,0x8BF9,0x9BD8,0xABBB,0xBB9A,
    0x4A75,0x5A54,0x6A37,0x7A16,0x0AF1,0x1AD0,0x2AB3,0x3A92,
    0xFD2E,0xED0F,0xDD6C,0xCD4D,0xBDAA,0xAD8B,0x9DE8,0x8DC9,
    0x7C26,0x6C07,0x5C64,0x4C45,0x3CA2,0x2C83,0x1CE0,0x0CC1,
    0xEF1F,0xFF3E,0xCF5D,0xDF7C,0xAF9B,0xBFBA,0x8FD9,0x9FF8,
    0x6E17,0x7E36,0x4E55,0x5E74,0x2E93,0x3EB2,0x0ED1,0x1EF0
};

double rad2deg(double rad) {
	return rad * 180 / PI;
}

/* crc-16 parity ---------------------------------------------------------------
* compute crc-16 parity for binex, nvs
* args   : unsigned char *buff I data
*          int    len    I      data length (bytes)
* return : crc-16 parity
* notes  : see reference [10] A.3.
*-----------------------------------------------------------------------------*/
extern unsigned short CRC16FromStream(const unsigned char *buff, size_t len)
{
	size_t	i = 0;
    unsigned short crc=0;
    
    for (i=0;i<len;i++) {
        crc=(crc<<8)^tbl_CRC16[((crc>>8)^buff[i])&0xFF];
    }
    return crc;
}

extern unsigned short CRC16FromString(const char *buff)
{
    return CRC16FromStream((const unsigned char*)buff, strlen(buff));
}

/* extract unsigned bits ------------------------------------------------
* extract unsigned bits from byte data
* args   : unsigned char *buff I byte data
*          int    pos    I      bit position from start of data (bits)
*          int    len    I      bit length (bits) (len<=32)
* return : extracted unsigned bits
*-----------------------------------------------------------------------------*/
static unsigned int getbitu(const unsigned char *buff, int pos, int len)
{
    unsigned int bits=0;
    int i;
    for (i=pos;i<pos+len;i++) bits=(bits<<1)+((buff[i/8]>>(7-i%8))&1u);
    return bits;
}

/* extract signed bits ------------------------------------------------
* extract signed bits from byte data
* args   : unsigned char *buff I byte data
*          int    pos    I      bit position from start of data (bits)
*          int    len    I      bit length (bits) (len<=32)
* return : extracted signed bits
*-----------------------------------------------------------------------------*/
static int getbits(const unsigned char *buff, int pos, int len)
{
    unsigned int bits=getbitu(buff,pos,len);
    if (len<=0||32<=len||!(bits&(1u<<(len-1)))) return (int)bits;
    return (int)(bits|(~0u<<len)); /* extend sign */
}

/* convert calendar day/time to time -------------------------------------------
* convert calendar day/time to gtime_t struct
* args   : double *ep       I   day/time {year,month,day,hour,min,sec}
* return : gtime_t struct
* notes  : proper in 1970-2037 or 1970-2099 (64bit time_t)
*-----------------------------------------------------------------------------*/
extern gtime_t epoch2time(const double *ep)
{
    const int doy[]={1,32,60,91,121,152,182,213,244,274,305,335};
    gtime_t time={0};
    int days,sec,year=(int)ep[0],mon=(int)ep[1],day=(int)ep[2];
    
    if (year<1970||2099<year||mon<1||12<mon) return time;
    
    /* leap year if year%4==0 in 1901-2099 */
    days=(year-1970)*365+(year-1969)/4+doy[mon-1]+day-2+(year%4==0&&mon>=3?1:0);
    sec=(int)floor(ep[5]);
    time.time=(time_t)days*86400+(int)ep[3]*3600+(int)ep[4]*60+sec;
    time.sec=ep[5]-sec;
    return time;
}

/* time to calendar day/time ---------------------------------------------------
* convert gtime_t struct to calendar day/time
* args   : gtime_t t        I   gtime_t struct
*          double *ep       O   day/time {year,month,day,hour,min,sec}
* return : none
* notes  : proper in 1970-2037 or 1970-2099 (64bit time_t)
*-----------------------------------------------------------------------------*/
extern void time2epoch(gtime_t t, double *ep)
{
	const int mday[] = { /* # of days in a month */
		31,28,31,30,31,30,31,31,30,31,30,31,31,28,31,30,31,30,31,31,30,31,30,31,
		31,29,31,30,31,30,31,31,30,31,30,31,31,28,31,30,31,30,31,31,30,31,30,31
	};
	int days, sec, mon, day;

	/* leap year if year%4==0 in 1901-2099 */
	days = (int)(t.time / 86400);
	sec = (int)(t.time - (time_t)days * 86400);
	for (day = days % 1461, mon = 0; mon < 48; mon++) {
		if (day >= mday[mon]) day -= mday[mon]; else break;
	}
	ep[0] = 1970 + days / 1461 * 4 + mon / 12; ep[1] = mon % 12 + 1; ep[2] = day + 1;
	ep[3] = sec / 3600; ep[4] = sec % 3600 / 60; ep[5] = sec % 60 + t.sec;
}


/* gps time to time ------------------------------------------------------------
* convert week and tow in gps time to gtime_t struct
* args   : int    week      I   week number in gps time
*          double sec       I   time of week in gps time (s)
* return : gtime_t struct
*-----------------------------------------------------------------------------*/
extern gtime_t gpst2time(int week, double sec)
{
    gtime_t t=epoch2time(gpst0);
    
    if (sec<-1E9||1E9<sec) sec=0.0;
    t.time+=86400*7*week+(int)sec;
    t.sec=sec-(int)sec;
    
    return t;
}

/* time to gps time ------------------------------------------------------------
* convert gtime_t struct to week and tow in gps time
* args   : gtime_t t        I   gtime_t struct
*          int    *week     IO  week number in gps time (NULL: no output)
* return : time of week in gps time (s)
*-----------------------------------------------------------------------------*/
extern double time2gpst(gtime_t t, int *week)
{
    gtime_t t0=epoch2time(gpst0);
    time_t sec=t.time-t0.time;
    int w=(int)(sec/(86400*7));
    
    if (week) *week=w;
    return (double)(sec-w*86400*7)+t.sec;
}

/* convert calendar day/time to gps time --------------------------------------
* convert calendar day/time to gps time
* args   : double *ep       I   day/time {year,month,day,hour,min,sec}
*          int    *week     IO  week number in gps time (NULL: no output)
*          int    *sec      IO  second in a week (NULL: no output)
* return : gtime_t struct
* notes  : proper in 1970-2037 or 1970-2099 (64bit time_t)
*-----------------------------------------------------------------------------*/
extern int epoch2gpst(const double *ep, int *week, double *sec)
{
    if (ep == NULL || week == NULL || sec == NULL)
        return 0;
    
    gtime_t time = epoch2time(ep);
    *sec = time2gpst(time, week);
    return 1;
}

extern void timeget(double ep[6])
{
    if (ep == NULL)
        return;
    
#ifdef _WINDOWS
    SYSTEMTIME ts;
    
    GetSystemTime(&ts); /* utc */
    ep[0]=ts.wYear; ep[1]=ts.wMonth;  ep[2]=ts.wDay;
    ep[3]=ts.wHour; ep[4]=ts.wMinute; ep[5]=ts.wSecond+ts.wMilliseconds*1E-3;
#else
    struct timeval tv;
    struct tm *tt;
    
    if (!gettimeofday(&tv,NULL)&&(tt=gmtime(&tv.tv_sec))) {
        ep[0]=tt->tm_year+1900; ep[1]=tt->tm_mon+1; ep[2]=tt->tm_mday;
        ep[3]=tt->tm_hour; ep[4]=tt->tm_min; ep[5]=tt->tm_sec+tv.tv_usec*1E-6;
    }
#endif
    
    return;
}

extern double timeget_syssec()
{
    double  ep[6] = {0.0}, sec = 0.0;;
    gtime_t t;
    
    timeget(ep);
    t = epoch2time(ep);
    sec = t.time + t.sec;
    
    if (0)
    {
        fprintf(stdout, " timeget_syssec : %04.0lf %02.0lf %02.0lf %02.0lf %02.0lf %09.6lf %.6lf\n", 
            ep[0], ep[1], ep[2], ep[3], ep[4], ep[5], sec);
    }
    
    return sec;
}

extern double timeget_gpssec()
{
    double  ep[6] = {0.0}, sec = 0.0;;
    gtime_t t;
    
    timeget(ep);
    t = epoch2time(ep);
  
    t.time -= 315964800;
    sec = t.time + t.sec;
    
    if (0)
    {
        fprintf(stdout, " timeget_gpssec : %04.0lf %02.0lf %02.0lf %02.0lf %02.0lf %09.6lf %.6lf\n", 
            ep[0], ep[1], ep[2], ep[3], ep[4], ep[5], sec);
    }
    
    return sec;
}

extern void timeget_gpstime(int *week, double *sow, const int leap_sec = 18)
{
    double  ep[6] = {0.0};
    gtime_t t;
    
    time_t sec = time(NULL);  // UTC秒数
    
    
    
    //timeget(ep);
    //t = epoch2time(ep);
    t.time = sec + leap_sec;
    //t.time += leap_sec;
    //sec = t.time + t.sec;
    *sow = time2gpst(t, week);
    
    return;
}

extern double epoch2utcsec(const double *ep)
{
    double  sec = 0.0;
    gtime_t t;
    
    t = epoch2time(ep);
    sec = t.time + t.sec;
    
    return sec;
}

extern double epoch2gpssec(const double *ep)
{
    double  sec = 0.0;
    gtime_t t;
    
    t = epoch2time(ep);
    t.time -= 315964800;
    sec = t.time + t.sec;
    
    return sec;
}

/**
* @brief       读取复制字符串
* @param[in]   src          char     源字符串,如果末位有'\n',自动除去
* @param[in]   nPos         int      读取的初始位置(从0开始)
* @param[in]   nCount       int      读取的字符串数
* @param[out]  dst          char     目标字符串, 末尾已用\0填充
* @return      void
* @note
* @par History:
*              2018/01/17,Feng Zhu, new \n
* @internals
*/
void xstrmid(const char *src, const int nPos, const int nCount, char *dst)
{
	int		i;
	const char	*str;
	char	c;

	str = src + nPos;

	for (i = 0; i < nCount; i++)
	{
		c = *(str + i);
		if (c)
		{
			*(dst + i) = c;
		}
		else
		{
			// 除去末尾的'\n'
			if (dst[i - 1] == '\n') dst[i - 1] = '\0';
			*(dst + i) = '\0';
			break;
		}
	}

	*(dst + nCount) = '\0';
}

extern void Write_IMU_Head(FILE* fp, INS_HEAR& head)
{
	if (fp == NULL)
		return;

	fprintf(fp, "         szHeader[8]     %s\n", head.szHeader);
	fprintf(fp, "  bIsIntelOrMotorola     %c\n", head.bIsIntelOrMotorola);
	fprintf(fp, "      dVersionNumber     %f\n", head.dVersionNumber);
	fprintf(fp, "         bDeltaTheta     %d\n", head.bDeltaTheta);
	fprintf(fp, "      bDeltaVelocity     %d\n", head.bDeltaVelocity);
	fprintf(fp, "         dDataRateHz     %f\n", head.dDataRateHz);
	fprintf(fp, "   dGyrosScaleFactor     %.20e\n", head.dGyrosScaleFactor);
	fprintf(fp, "   dAccelScaleFactor     %.20e\n", head.dAccelScaleFactor);
	fprintf(fp, "       iUtcOrGpsTime     %d\n", head.iUtcOrGpsTime);
	fprintf(fp, "  iRcvTimeOrCorrTime     %d\n", head.iRcvTimeOrCorrTime);
	fprintf(fp, "        dTimeTagBias     %f\n", head.dTimeTagBias);
	fprintf(fp, "           szImuName     %s\n", head.szImuName);
	fprintf(fp, "           bDirValid     %d\n", head.bDirValid);
	fprintf(fp, "                 ucX     %d\n", head.ucX);
	fprintf(fp, "                 ucY     %d\n", head.ucY);
	fprintf(fp, "                 ucZ     %d\n", head.ucZ);
	fprintf(fp, "       szProgramName     %s\n", head.szProgramName);
	fprintf(fp, "             tCreate     %04d %02d %02d %02d %02d %02d\n", head.tCreate.year, head.tCreate.month, head.tCreate.day,
		head.tCreate.hour, head.tCreate.minute, head.tCreate.second);
	fprintf(fp, "      bLeverArmValid     %d\n", head.bLeverArmValid);
	fprintf(fp, "            lXoffset     %ld\n", head.lXoffset);
	fprintf(fp, "            lYoffset     %ld\n", head.lYoffset);
	fprintf(fp, "            lZoffset     %ld\n", head.lZoffset);
	fprintf(fp, "            Reserved     %s\n", head.Reserved);

	fprintf(fp, "            END_HEAD\n");
	fprintf(fp, "----------------------------------------------------------\n");
	fprintf(fp, "# GTime(sec of week) dGyros(X Y Z)(deg) dAccel(X Y Z)(m/s)\n");
	fprintf(fp, "----------------------------------------------------------\n");

	fflush(fp);
	return;
}

bool ConverteIMU2IMR(const std::string imuFile, const std::string imrFile)
{
	FILE	*m_ifp = NULL, *m_ofp = NULL;
	IE84_MIr_header_type m_IMUHead;
	IE84_INS_type m_IMUData;

	memset(&m_IMUHead, 0, sizeof(IE84_MIr_header_type));
	memset(&m_IMUData, 0, sizeof(IE84_INS_type));

	m_ifp = fopen(imuFile.c_str(), "r");
	m_ofp = fopen(imrFile.c_str(), "wb");
	if (m_ifp == NULL || m_ofp == NULL)
	{
		if (m_ifp != NULL) { fclose(m_ifp); m_ifp = NULL; }
		if (m_ofp != NULL) { fclose(m_ofp); m_ofp = NULL; }
		return false;
	}

	// Read IMR File Head Part
	while (!feof(m_ifp))
	{
		char oneline[64] = { '\0' };
		fgets(oneline, 64, m_ifp);
		std::istringstream is(oneline);
		std::string mark;
		is >> mark;

		if (mark == string("szHeader[8]"))
		{
			is >> m_IMUHead.szHeader;
		}
		else if (mark == string("bIsIntelOrMotorola"))
		{
			is >> m_IMUHead.bIsIntelOrMotorola;
		}
		else if (mark == string("dVersionNumber"))
		{
			is >> m_IMUHead.dVersionNumber;
		}
		else if (mark == string("bDeltaTheta"))
		{
			is >> m_IMUHead.bDeltaTheta;
		}
		else if (mark == string("bDeltaVelocity"))
		{
			is >> m_IMUHead.bDeltaVelocity;
		}
		else if (mark == string("dDataRateHz"))
		{
			is >> m_IMUHead.dDataRateHz;
		}
		else if (mark == string("dGyrosScaleFactor"))
		{
			is >> m_IMUHead.dGyrosScaleFactor;
		}
		else if (mark == string("dAccelScaleFactor"))
		{
			is >> m_IMUHead.dAccelScaleFactor;
		}
		else if (mark == string("iUtcOrGpsTime"))
		{
			is >> m_IMUHead.iUtcOrGpsTime;
		}
		else if (mark == string("iRcvTimeOrCorrTime"))
		{
			is >> m_IMUHead.iRcvTimeOrCorrTime;
		}
		else if (mark == string("dTimeTagBias"))
		{
			is >> m_IMUHead.dTimeTagBias;
		}
		else if (mark == string("szImuName"))
		{
			//is >> ls_FFSHead.szImuName;
			xstrmid(oneline, 25, 100, m_IMUHead.szImuName);
		}
		else if (mark == string("bDirValid"))
		{
			is >> m_IMUHead.bDirValid;
		}
		else if (mark == string("ucX"))
		{
			is >> m_IMUHead.ucX;
		}
		else if (mark == string("ucY"))
		{
			is >> m_IMUHead.ucY;
		}
		else if (mark == string("ucZ"))
		{
			is >> m_IMUHead.ucZ;
		}
		else if (mark == string("szProgramName"))
		{
			// is >> ls_FFSHead.szProgramName;
			xstrmid(oneline, 25, 100, m_IMUHead.szProgramName);
		}
		else if (mark == string("tCreate"))
		{
			is >> m_IMUHead.tCreate.year >> m_IMUHead.tCreate.month >> m_IMUHead.tCreate.day
				>> m_IMUHead.tCreate.hour >> m_IMUHead.tCreate.minute >> m_IMUHead.tCreate.second;
		}
		else if (mark == string("bLeverArmValid"))
		{
			is >> m_IMUHead.bLeverArmValid; //
		}
		else if (mark == string("lXoffset"))
		{
			is >> m_IMUHead.lXoffset; //
		}
		else if (mark == string("lYoffset"))
		{
			is >> m_IMUHead.lYoffset; //
		}
		else if (mark == string("lZoffset"))
		{
			is >> m_IMUHead.lZoffset; //
		}
		else if (mark == string("Reserved"))
		{
			is >> m_IMUHead.Reserved;
		}
		else if (mark == string("END_HEAD"))
		{
			fgets(oneline, 64, m_ifp);
			fgets(oneline, 64, m_ifp);
			fgets(oneline, 64, m_ifp);
			break;
		}
	}

	// Write IMU File Head Part
	m_IMUHead.bIsIntelOrMotorola = 0;

	double dGs = m_IMUHead.dGyrosScaleFactor;
	double dAs = m_IMUHead.dAccelScaleFactor;
	//		double dGs = 1e-8;
	//		double dAs = 1e-8;

			// Write IMR File Head Part
	fwrite(&m_IMUHead.szHeader, sizeof(char), 8, m_ofp);
	fwrite(&m_IMUHead.bIsIntelOrMotorola, sizeof(char), 1, m_ofp);
	fwrite(&m_IMUHead.dVersionNumber, sizeof(double), 1, m_ofp);
	fwrite(&m_IMUHead.bDeltaTheta, sizeof(int), 1, m_ofp);
	fwrite(&m_IMUHead.bDeltaVelocity, sizeof(int), 1, m_ofp);
	fwrite(&m_IMUHead.dDataRateHz, sizeof(double), 1, m_ofp);
	//		fwrite(&dGs                         ,sizeof(double), 1,m_ofp);
	//		fwrite(&dAs                         ,sizeof(double), 1,m_ofp);
	fwrite(&m_IMUHead.dGyrosScaleFactor, sizeof(double), 1, m_ofp);
	fwrite(&m_IMUHead.dAccelScaleFactor, sizeof(double), 1, m_ofp);
	fwrite(&m_IMUHead.iUtcOrGpsTime, sizeof(int), 1, m_ofp);
	fwrite(&m_IMUHead.iRcvTimeOrCorrTime, sizeof(int), 1, m_ofp);
	fwrite(&m_IMUHead.dTimeTagBias, sizeof(double), 1, m_ofp);
	fwrite(&m_IMUHead.szImuName, sizeof(char), 32, m_ofp);
	fwrite(&m_IMUHead.bDirValid, sizeof(bool), 1, m_ofp);
	fwrite(&m_IMUHead.ucX, sizeof(unsigned char), 1, m_ofp);
	fwrite(&m_IMUHead.ucY, sizeof(unsigned char), 1, m_ofp);
	fwrite(&m_IMUHead.ucZ, sizeof(unsigned char), 1, m_ofp);
	fwrite(&m_IMUHead.szProgramName, sizeof(char), 32, m_ofp);
	fwrite(&m_IMUHead.tCreate, sizeof(time_type), 1, m_ofp);
	m_IMUHead.bLeverArmValid = false;
	fwrite(&m_IMUHead.bLeverArmValid, sizeof(bool), 1, m_ofp);
	// fwrite(&m_IMUHead.lXoffset, sizeof(long), 1, m_ofp);
	// fwrite(&m_IMUHead.lXoffset, sizeof(long), 1, m_ofp);
	// fwrite(&m_IMUHead.lXoffset, sizeof(long), 1, m_ofp);
	m_IMUHead.lXoffset = 0;
	m_IMUHead.lYoffset = 0;
	m_IMUHead.lZoffset = 0;
	fwrite(&m_IMUHead.lXoffset, sizeof(long), 1, m_ofp);
	fwrite(&m_IMUHead.lYoffset, sizeof(long), 1, m_ofp);
	fwrite(&m_IMUHead.lZoffset, sizeof(long), 1, m_ofp);

	fwrite(&m_IMUHead.Reserved, sizeof(bool), 354, m_ofp);

	double TimePre = -1;
	double val[7] = { 0.0 };
	// Write IMR File Dates Part
	while (!feof(m_ifp))
	{
		if (fscanf(m_ifp, "%lf %lf %lf %lf %lf %lf %lf", val, val + 1, val + 2, val + 3, val + 4, val + 5, val + 6) < 7) continue;

		m_IMUData.Time = val[0];
		m_IMUData.gx = int(rad2deg(val[1]));
		m_IMUData.gy = int(rad2deg(val[2]));
		m_IMUData.gz = int(rad2deg(val[3]));
		m_IMUData.ax = int(val[4]);
		m_IMUData.ay = int(val[5]);
		m_IMUData.az = int(val[6]);

		if (m_IMUData.gx == 0.0 && m_IMUData.gy == 0.0 && m_IMUData.gz == 0.0) continue;
		if (m_IMUData.ax == 0.0 && m_IMUData.ay == 0.0 && m_IMUData.az == 0.0) continue;

		if (TimePre == m_IMUData.Time) continue;
		TimePre = m_IMUData.Time;

		fwrite(&m_IMUData.Time, sizeof(double), 1, m_ofp);
		fwrite(&m_IMUData.gx, sizeof(long), 1, m_ofp);
		fwrite(&m_IMUData.gy, sizeof(long), 1, m_ofp);
		fwrite(&m_IMUData.gz, sizeof(long), 1, m_ofp);
		fwrite(&m_IMUData.ax, sizeof(long), 1, m_ofp);
		fwrite(&m_IMUData.ay, sizeof(long), 1, m_ofp);
		fwrite(&m_IMUData.az, sizeof(long), 1, m_ofp);

	}

	if (m_ifp != NULL) { fclose(m_ifp); m_ifp = NULL; }
	if (m_ofp != NULL) { fclose(m_ofp); m_ofp = NULL; }

	return true;
}


#ifdef __cplusplus
}
#endif

#endif // _TIME_UTILS_HPP_