#ifndef _HONEYWELL_I300_HPP_
#define _HONEYWELL_I300_HPP_

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

#define HW_I300_PREAMB  0x0E        /* frame preamble */
#define OXA1_LEN        20
#define OXA2_LEN        44
#define OXA3_LEN        32
#define OXAC_LEN        26
#define OXAD_LEN        50
#define OXAE_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  DeltaAngleX;
    double  DeltaAngleY;
    double  DeltaAngleZ;
    
    // 速度增量，单位为m/sec
    double  DeltaVelocityX;
    double  DeltaVelocityY;
    double  DeltaVelocityZ;
} raw_hw_i300_t;

/* 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 */
}

// 小端模式
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 < OXA2_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)
{
    // 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;
    unsigned short  checksum;
    raw_hw_i300_t   imuData = { 0 };
    
    if (len < OXA3_LEN || buff[0] != HW_I300_PREAMB || 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
    //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.DeltaAngleX     = val*NAVDELTAANGSCALE;
    memcpy(&val, &buff[i], 4); i+=4;
    imuData.DeltaAngleY     = val*NAVDELTAANGSCALE;
    memcpy(&val, &buff[i], 4); i+=4;
    imuData.DeltaAngleZ     = val*NAVDELTAANGSCALE;

    memcpy(&val, &buff[i], 4); i+=4;
    imuData.DeltaVelocityX  = val*NAVDELTAVELSCALE;
    memcpy(&val, &buff[i], 4); i+=4;
    imuData.DeltaVelocityY  = val*NAVDELTAVELSCALE;
    memcpy(&val, &buff[i], 4); i+=4;
    imuData.DeltaVelocityZ  = val*NAVDELTAVELSCALE;
    
    imuData.status          = getbits(buff,i*8,32); i+=4;
    
    memcpy(&checksum, &buff[30], 2);
    if (get_u16_checksum(buff, OXA3_LEN) != checksum)
        return 0;
    
    if (0)
    {
       printf(" %X %.10lf %.10lf %.10lf %.10lf %.10lf %.10lf\n", imuData.status, 
            imuData.DeltaAngleX, imuData.DeltaAngleY, imuData.DeltaAngleZ, 
            imuData.DeltaVelocityX, imuData.DeltaVelocityY, imuData.DeltaVelocityZ);     
    }
    
    if (rawImu != NULL)
        *rawImu = imuData;
    
    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;
}


#endif