//
// Created by wakashaw on 22-6-19.
//
#include "rtklib.h"
static imud_t imud={0};

extern int decode_typeIMU(rtcm_t *rtcm){
    double tow;
    int i=52,sensor_type,data_type,week;
    double time;
    if(i+80<=rtcm->len*8){
        sensor_type     = getbitu(rtcm->buff,i,8);      i+=8;
        data_type       = getbits(rtcm->buff,48,4);
        /* convert right-forward-up to forward-right-down  0-1-2 to 1-0-2*/
        /* acc */
        if(sensor_type==30){
            if(data_type==3){
                imud.time= getbitu(rtcm->buff,i,32);                    i+=32;
                imud.accl[1]= getbits(rtcm->buff,i,16)*0.000061 * 9.8;  i+=16;
                imud.accl[0]= getbits(rtcm->buff,i,16)*0.000061 * 9.8;  i+=16;
                imud.accl[2]= getbits(rtcm->buff,i,16)*0.000061 * 9.8;  i+=56;
                /* up to down */
                imud.accl[2]*=-1.0;

                imud.gyro[1] = getbits(rtcm->buff,i,16)*0.00437 * D2R;  i+=16;
                imud.gyro[0]=  getbits(rtcm->buff,i,16)*0.00437 * D2R;  i+=16;
                imud.gyro[2]=  getbits(rtcm->buff,i,16)*0.00437 * D2R;
                /* up to down */
                imud.gyro[2]*=-1.0;
            }
            else{
                time= getbitu(rtcm->buff,i,32);                         i+=32;
                imud.accl[1]= getbits(rtcm->buff,i,16)*0.000061 * 9.8;  i+=16;
                imud.accl[0]= getbits(rtcm->buff,i,16)*0.000061 * 9.8;  i+=16;
                imud.accl[2]= getbits(rtcm->buff,i,16)*0.000061 * 9.8;
                /* up to down */
                imud.accl[2]*=-1.0;
                if(time != imud.time){
                    imud.time = time;
                    for(int j=0;j<3;j++) imud.gyro[j]=0;
                }
            }
        }
        /* gyro */
        else if(sensor_type==31){
            time = getbitu(rtcm->buff,i,32);                           i+=32;
            imud.gyro[1] = getbits(rtcm->buff,i,16)*0.00437 * D2R;     i+=16;
            imud.gyro[0]= getbits(rtcm->buff,i,16)*0.00437 * D2R;      i+=16;
            imud.gyro[2]= getbits(rtcm->buff,i,16)*0.00437 * D2R;
            /* up to down */
            imud.gyro[2]*=-1.0;

            if(time != imud.time){
                imud.time = time;
                for(int j=0;j<3;j++) imud.accl[j]=0;
            }
        }
        else{
//            trace(2,"Not IMU data\n");
            return -1;
        }
    }
    for(int j=0;j<3;j++){
        if(imud.accl[j]==0||imud.gyro[j]==0){
//            trace(2,"lack of acc/gyro data\n");
            return -1;
        }
    }
    rtcm->imud=imud;
    tow=time2gpst(utc2gpst(timeget()),&week);
    rtcm->imud.syn_time=gpst2time(week,tow);
    return 11;
}