#include "rtcm.h"

/* constants -----------------------------------------------------------------*/

#define PRUNIT_GPS  299792.458  /* rtcm ver.3 unit of gps pseudorange (m) */
#define PRUNIT_GLO  599584.916  /* rtcm ver.3 unit of glonass pseudorange (m) */
#define RANGE_MS    (CLIGHT*0.001)      /* range in 1 ms */
#define P2_10       0.0009765625          /* 2^-10 */
#define P2_34       5.820766091346740E-11 /* 2^-34 */
#define P2_46       1.421085471520200E-14 /* 2^-46 */
#define P2_59       1.734723475976810E-18 /* 2^-59 */
#define P2_66       1.355252715606880E-20 /* 2^-66 */

/* type definition -----------------------------------------------------------*/

typedef struct {                    /* multi-signal-message header type */
    unsigned char iod;              /* 数据站发行版本号 */
    unsigned char time_s;           /* 累计数据传输时间 */
    unsigned char clk_str;          /* 时钟控制指示器 */
    unsigned char clk_ext;          /* 外部时钟指示器 */
    unsigned char smooth;           /* 无偏差平滑指示器 */
    unsigned char tint_s;           /* 平滑间隔 */
    unsigned char nsat, nsig;        /* 卫星数和卫星号 */
    unsigned char sats[64];         /* 卫星数组 */
    unsigned char sigs[32];         /* 信号数组 */
    unsigned char cellmask[64];     /* cell mask */
} msm_h_t;

/* msm signal id table -------------------------------------------------------*/
const char* msm_sig_gps[32] = {
    /* GPS: ref [13] table 3.5-87, ref [14][15] table 3.5-91 */
    ""  ,"1C","1P","1W","1Y","1M",""  ,"2C","2P","2W","2Y","2M", /*  1-12 */
    ""  ,""  ,"2S","2L","2X",""  ,""  ,""  ,""  ,"5I","5Q","5X", /* 13-24 */
    ""  ,""  ,""  ,""  ,""  ,"1S","1L","1X"                      /* 25-32 */
};
const char* msm_sig_gal[32] = {
    /* Galileo: ref [15] table 3.5-100 */
    ""  ,"1C","1A","1B","1X","1Z",""  ,"6C","6A","6B","6X","6Z",
    ""  ,"7I","7Q","7X",""  ,"8I","8Q","8X",""  ,"5I","5Q","5X",
    ""  ,""  ,""  ,""  ,""  ,""  ,""  ,""
};
const char* msm_sig_cmp[32] = {
    /* BeiDou: ref [15] table 3.5-106 */
    ""  ,"1I","1Q","1X",""  ,""  ,""  ,"6I","6Q","6X",""  ,""  ,
    ""  ,"7I","7Q","7X",""  ,""  ,""  ,""  ,""  ,""  ,""  ,""  ,
    ""  ,""  ,""  ,""  ,""  ,""  ,""  ,""
};
/* get sign-magnitude bits ---------------------------------------------------*/
static double getbitg(const unsigned char* buff, int pos, int len)
{
    double value = getbitu(buff, pos + 1, len - 1);
    return getbitu(buff, pos, 1) ? -value : value;
}
/* adjust weekly rollover of gps time ----------------------------------------*/
static void adjweek(rtcm_t* rtcm, double tow) {
    double tow_p;
    int week;
    /* if no time, get cpu time */
    if (rtcm->time.time == 0) {
        rtcm->time = utc2gpst(timeget());
        rtcm->time.time = rtcm->time.time - 7 * 2 * 24 * 3600;
    }
    tow_p = time2gpst(rtcm->time, &week);
    if (tow < tow_p - 302400.0) tow += 604800.0;
    else if (tow > tow_p + 302400.0) tow -= 604800.0;
    rtcm->time = gpst2time(week, tow);
}
/* adjust weekly rollover of bdt time ----------------------------------------*/
static int adjbdtweek(int week)
{
    int w;
    (void)time2bdt(gpst2bdt(utc2gpst(timeget())), &w);
    if (w < 1) w = 1; /* use 2006/1/1 if time is earlier than 2006/1/1 */
    return week + (w - week + 512) / 1024 * 1024;
}
/* adjust daily rollover of glonass time -------------------------------------*/
static void adjday_glot(rtcm_t* rtcm, double tod)
{
    gtime_t time;
    double tow, tod_p;
    int week;

    if (rtcm->time.time == 0) rtcm->time = utc2gpst(timeget());
    time = timeadd(gpst2utc(rtcm->time), 10800.0); /* glonass time */
    tow = time2gpst(time, &week);
    tod_p = fmod(tow, 86400.0); tow -= tod_p;
    if (tod < tod_p - 43200.0) tod += 86400.0;
    else if (tod > tod_p + 43200.0) tod -= 86400.0;
    time = gpst2time(week, tow + tod);
    rtcm->time = utc2gpst(timeadd(time, -10800.0));
}
/* adjust carrier-phase rollover ---------------------------------------------*/
static double adjcp(rtcm_t* rtcm, int sat, int freq, double cp)
{
    if (rtcm->cp[sat - 1][freq] == 0.0);
    else if (cp < rtcm->cp[sat - 1][freq] - 750.0) cp += 1500.0;
    else if (cp > rtcm->cp[sat - 1][freq] + 750.0) cp -= 1500.0;
    rtcm->cp[sat - 1][freq] = cp;
    return cp;
}
/* loss-of-lock indicator ----------------------------------------------------*/
static int lossoflock(rtcm_t* rtcm, int sat, int freq, int lock)
{
    int lli = (!lock && !rtcm->lock[sat - 1][freq]) || lock < rtcm->lock[sat - 1][freq];
    rtcm->lock[sat - 1][freq] = (unsigned short)lock;
    return lli;
}
/* s/n ratio -----------------------------------------------------------------*/
static unsigned char snratio(double snr)
{
    return (unsigned char)(snr <= 0.0 || 255.5 <= snr ? 0.0 : snr * 4.0 + 0.5);
}
/* get observation data index ------------------------------------------------*/
static int obsindex(obs_t* obs, gtime_t time, int sat)
{
    int i, j;

    for (i = 0; i < obs->n; i++) {
        if (obs->data[i].sat == sat) return i; /* field already exists */
    }
    if (i >= MAXOBS) return -1; /* overflow */

    /* add new field */
    obs->data[i].time = time;
    obs->data[i].sat = sat;
    for (j = 0; j < NFREQ + NEXOBS; j++) {
        obs->data[i].L[j] = obs->data[i].P[j] = 0.0;
        obs->data[i].D[j] = 0.0;
        obs->data[i].SNR[j] = obs->data[i].LLI[j] = obs->data[i].code[j] = 0;
    }
    obs->n++;
    return i;
}
/* test station id consistency -----------------------------------------------*/
static int test_staid(rtcm_t* rtcm, int staid)
{
    char* p;
    int type, id;

    /* test station id option */
    if ((p = strstr(rtcm->opt, "-STA=")) && sscanf(p, "-STA=%d", &id) == 1) {
        if (staid != id) return 0;
    }
    /* save station id */
    if (rtcm->staid == 0 || rtcm->obsflag) {
        rtcm->staid = staid;
    }
    else if (staid != rtcm->staid) {
        type = getbitu(rtcm->buff, 24, 12);
        trace(2, "rtcm3 %d staid invalid id=%d %d\n", type, staid, rtcm->staid);

        /* reset station id if station id error */
        rtcm->staid = 0;
        return 0;
    }
    return 1;
}
/* get signed 38bit field ----------------------------------------------------*/
static double getbits_38(const unsigned char* buff, int pos)
{
    return (double)getbits(buff, pos, 32) * 64.0 + getbitu(buff, pos + 32, 6);
}

/* decode type 1019: GPS ephemerides -----------------------------------------*/
static int decode_type1019(rtcm_t* rtcm)
{
    eph_t eph = { 0 };
    double toc, sqrtA, tt;
    char* msg;
    int i = 24 + 12, prn, sat, week, sys = SYS_GPS;

    if (i + 476 <= rtcm->len * 8) {
        prn = getbitu(rtcm->buff, i, 6);                i += 6;
        week = getbitu(rtcm->buff, i, 10);              i += 10;
        eph.sva = getbitu(rtcm->buff, i, 4);            i += 4;
        eph.code = getbitu(rtcm->buff, i, 2);           i += 2;
        eph.idot = getbits(rtcm->buff, i, 14) * P2_43 * SC2RAD; i += 14;
        eph.iode = getbitu(rtcm->buff, i, 8);           i += 8;
        toc = getbitu(rtcm->buff, i, 16) * 16.0;        i += 16;
        eph.f2 = getbits(rtcm->buff, i, 8) * P2_55;     i += 8;
        eph.f1 = getbits(rtcm->buff, i, 16) * P2_43;    i += 16;
        eph.f0 = getbits(rtcm->buff, i, 22) * P2_31;        i += 22;
        eph.iodc = getbitu(rtcm->buff, i, 10);              i += 10;
        eph.crs = getbits(rtcm->buff, i, 16) * P2_5;         i += 16;
        eph.deln = getbits(rtcm->buff, i, 16) * P2_43 * SC2RAD; i += 16;
        eph.M0 = getbits(rtcm->buff, i, 32) * P2_31 * SC2RAD; i += 32;
        eph.cuc = getbits(rtcm->buff, i, 16) * P2_29;        i += 16;
        eph.e = getbitu(rtcm->buff, i, 32) * P2_33;        i += 32;
        eph.cus = getbits(rtcm->buff, i, 16) * P2_29;        i += 16;
        sqrtA = getbitu(rtcm->buff, i, 32) * P2_19;        i += 32;
        eph.toes = getbitu(rtcm->buff, i, 16) * 16.0;         i += 16;
        eph.cic = getbits(rtcm->buff, i, 16) * P2_29;        i += 16;
        eph.OMG0 = getbits(rtcm->buff, i, 32) * P2_31 * SC2RAD; i += 32;
        eph.cis = getbits(rtcm->buff, i, 16) * P2_29;        i += 16;
        eph.i0 = getbits(rtcm->buff, i, 32) * P2_31 * SC2RAD; i += 32;
        eph.crc = getbits(rtcm->buff, i, 16) * P2_5;         i += 16;
        eph.omg = getbits(rtcm->buff, i, 32) * P2_31 * SC2RAD; i += 32;
        eph.OMGd = getbits(rtcm->buff, i, 24) * P2_43 * SC2RAD; i += 24;
        eph.tgd[0] = getbits(rtcm->buff, i, 8) * P2_31;        i += 8;
        eph.svh = getbitu(rtcm->buff, i, 6);              i += 6;
        eph.flag = getbitu(rtcm->buff, i, 1);              i += 1;
        eph.fit = getbitu(rtcm->buff, i, 1) ? 0.0 : 4.0; /* 0:4hr,1:>4hr */
    }
    else {
        return -1;
    }
    if (prn >= 40) {
        sys = SYS_SBS; prn += 80;
    }

    if (rtcm->outtype) {
        msg = rtcm->msgtype + strlen(rtcm->msgtype);
        sprintf(msg, " prn=%2d iode=%3d iodc=%3d week=%d toe=%6.0f toc=%6.0f svh=%02X",
            prn, eph.iode, eph.iodc, week, eph.toes, toc, eph.svh);
    }
    if (!(sat = satno(sys, prn))) {
        trace(2, "rtcm3 1019 satellite number error: prn=%d\n", prn);
        return -1;
    }
    eph.sat = sat;
    eph.week = adjgpsweek(week);
    if (rtcm->time.time == 0) rtcm->time = utc2gpst(timeget());
    tt = timediff(gpst2time(eph.week, eph.toes), rtcm->time);
    if (tt < -302400.0) eph.week++;
    else if (tt >= 302400.0) eph.week--;
    eph.toe = gpst2time(eph.week, eph.toes);
    eph.toc = gpst2time(eph.week, toc);
    eph.ttr = rtcm->time;
    eph.A = sqrtA * sqrtA;
    if (!strstr(rtcm->opt, "-EPHALL")) {
        if (eph.iode == rtcm->nav.eph[sat - 1].iode) return 0; /* unchanged */
    }
    rtcm->nav.eph[sat - 1] = eph;
    rtcm->ephsat = sat;
    //rtcm->ephset = 0;
    return 2;
}
/* decode type 1046: Galileo I/NAV satellite ephemerides ---------------------*/
static int decode_type1046(rtcm_t* rtcm)
{
    eph_t eph = { 0 };
    double toc, sqrtA, tt;
    char* msg;
    int i = 24 + 12, prn, sat, week, e5b_hs, e5b_dvs, e1_hs, e1_dvs, sys = SYS_GAL;

    if (strstr(rtcm->opt, "-GALFNAV")) return 0;

    if (i + 492 <= rtcm->len * 8) {
        prn = getbitu(rtcm->buff, i, 6);              i += 6;
        week = getbitu(rtcm->buff, i, 12);              i += 12;
        eph.iode = getbitu(rtcm->buff, i, 10);              i += 10;
        eph.sva = getbitu(rtcm->buff, i, 8);              i += 8;
        eph.idot = getbits(rtcm->buff, i, 14) * P2_43 * SC2RAD; i += 14;
        toc = getbitu(rtcm->buff, i, 14) * 60.0;         i += 14;
        eph.f2 = getbits(rtcm->buff, i, 6) * P2_59;        i += 6;
        eph.f1 = getbits(rtcm->buff, i, 21) * P2_46;        i += 21;
        eph.f0 = getbits(rtcm->buff, i, 31) * P2_34;        i += 31;
        eph.crs = getbits(rtcm->buff, i, 16) * P2_5;         i += 16;
        eph.deln = getbits(rtcm->buff, i, 16) * P2_43 * SC2RAD; i += 16;
        eph.M0 = getbits(rtcm->buff, i, 32) * P2_31 * SC2RAD; i += 32;
        eph.cuc = getbits(rtcm->buff, i, 16) * P2_29;        i += 16;
        eph.e = getbitu(rtcm->buff, i, 32) * P2_33;        i += 32;
        eph.cus = getbits(rtcm->buff, i, 16) * P2_29;        i += 16;
        sqrtA = getbitu(rtcm->buff, i, 32) * P2_19;        i += 32;
        eph.toes = getbitu(rtcm->buff, i, 14) * 60.0;         i += 14;
        eph.cic = getbits(rtcm->buff, i, 16) * P2_29;        i += 16;
        eph.OMG0 = getbits(rtcm->buff, i, 32) * P2_31 * SC2RAD; i += 32;
        eph.cis = getbits(rtcm->buff, i, 16) * P2_29;        i += 16;
        eph.i0 = getbits(rtcm->buff, i, 32) * P2_31 * SC2RAD; i += 32;
        eph.crc = getbits(rtcm->buff, i, 16) * P2_5;         i += 16;
        eph.omg = getbits(rtcm->buff, i, 32) * P2_31 * SC2RAD; i += 32;
        eph.OMGd = getbits(rtcm->buff, i, 24) * P2_43 * SC2RAD; i += 24;
        eph.tgd[0] = getbits(rtcm->buff, i, 10) * P2_32;        i += 10; /* E5a/E1 */
        eph.tgd[1] = getbits(rtcm->buff, i, 10) * P2_32;        i += 10; /* E5b/E1 */
        e5b_hs = getbitu(rtcm->buff, i, 2);              i += 2; /* E5b OSHS */
        e5b_dvs = getbitu(rtcm->buff, i, 1);              i += 1; /* E5b OSDVS */
        e1_hs = getbitu(rtcm->buff, i, 2);              i += 2; /* E1 OSHS */
        e1_dvs = getbitu(rtcm->buff, i, 1);              i += 1; /* E1 OSDVS */
    }
    else {
        trace(2, "rtcm3 1046 length error: len=%d\n", rtcm->len);
        return -1;
    }
    trace(4, "decode_type1046: prn=%d iode=%d toe=%.0f\n", prn, eph.iode, eph.toes);

    if (rtcm->outtype) {
        msg = rtcm->msgtype + strlen(rtcm->msgtype);
        sprintf(msg, " prn=%2d iode=%3d week=%d toe=%6.0f toc=%6.0f hs=%d %d dvs=%d %d",
            prn, eph.iode, week, eph.toes, toc, e5b_hs, e1_hs, e5b_dvs, e1_dvs);
    }
    if (!(sat = satno(sys, prn))) {
        trace(2, "rtcm3 1046 satellite number error: prn=%d\n", prn);
        return -1;
    }
    if (strstr(rtcm->opt, "-GALFNAV")) {
        return 0;
    }
    eph.sat = sat;
    eph.week = week + 1024; /* gal-week = gst-week + 1024 */
    if (rtcm->time.time == 0) rtcm->time = utc2gpst(timeget());
    tt = timediff(gpst2time(eph.week, eph.toes), rtcm->time);
    if (tt < -302400.0) eph.week++;
    else if (tt >= 302400.0) eph.week--;
    eph.toe = gpst2time(eph.week, eph.toes);
    eph.toc = gpst2time(eph.week, toc);
    eph.ttr = rtcm->time;
    eph.A = sqrtA * sqrtA;
    eph.svh = (e5b_hs << 7) + (e5b_dvs << 6) + (e1_hs << 1) + (e1_dvs << 0);
    eph.code = (1 << 0) + (1 << 2) + (1 << 9); /* data source = I/NAV+E1+E5b */
    eph.iodc = eph.iode;
    if (!strstr(rtcm->opt, "-EPHALL")) {
        if (eph.iode == rtcm->nav.eph[sat - 1].iode) return 0; /* unchanged */
    }
    rtcm->nav.eph[sat - 1] = eph;
    rtcm->ephsat = sat;
    //rtcm->ephset = 0; /* I/NAV */
    return 2;
}
/* decode type 1042/63: Beidou ephemerides -----------------------------------*/
static int decode_type1042(rtcm_t* rtcm)
{
    eph_t eph = { 0 };
    double toc, sqrtA, tt;
    char* msg;
    int i = 24 + 12, prn, sat, week, sys = SYS_CMP;

    if (i + 499 <= rtcm->len * 8) {
        prn = getbitu(rtcm->buff, i, 6);              i += 6;
        week = getbitu(rtcm->buff, i, 13);              i += 13;
        eph.sva = getbitu(rtcm->buff, i, 4);              i += 4;
        eph.idot = getbits(rtcm->buff, i, 14) * P2_43 * SC2RAD; i += 14;
        eph.iode = getbitu(rtcm->buff, i, 5);              i += 5; /* AODE */
        toc = getbitu(rtcm->buff, i, 17) * 8.0;          i += 17;
        eph.f2 = getbits(rtcm->buff, i, 11) * P2_66;        i += 11;
        eph.f1 = getbits(rtcm->buff, i, 22) * P2_50;        i += 22;
        eph.f0 = getbits(rtcm->buff, i, 24) * P2_33;        i += 24;
        eph.iodc = getbitu(rtcm->buff, i, 5);              i += 5; /* AODC */
        eph.crs = getbits(rtcm->buff, i, 18) * P2_6;         i += 18;
        eph.deln = getbits(rtcm->buff, i, 16) * P2_43 * SC2RAD; i += 16;
        eph.M0 = getbits(rtcm->buff, i, 32) * P2_31 * SC2RAD; i += 32;
        eph.cuc = getbits(rtcm->buff, i, 18) * P2_31;        i += 18;
        eph.e = getbitu(rtcm->buff, i, 32) * P2_33;        i += 32;
        eph.cus = getbits(rtcm->buff, i, 18) * P2_31;        i += 18;
        sqrtA = getbitu(rtcm->buff, i, 32) * P2_19;        i += 32;
        eph.toes = getbitu(rtcm->buff, i, 17) * 8.0;          i += 17;
        eph.cic = getbits(rtcm->buff, i, 18) * P2_31;        i += 18;
        eph.OMG0 = getbits(rtcm->buff, i, 32) * P2_31 * SC2RAD; i += 32;
        eph.cis = getbits(rtcm->buff, i, 18) * P2_31;        i += 18;
        eph.i0 = getbits(rtcm->buff, i, 32) * P2_31 * SC2RAD; i += 32;
        eph.crc = getbits(rtcm->buff, i, 18) * P2_6;         i += 18;
        eph.omg = getbits(rtcm->buff, i, 32) * P2_31 * SC2RAD; i += 32;
        eph.OMGd = getbits(rtcm->buff, i, 24) * P2_43 * SC2RAD; i += 24;
        eph.tgd[0] = getbits(rtcm->buff, i, 10) * 1E-10;        i += 10;
        eph.tgd[1] = getbits(rtcm->buff, i, 10) * 1E-10;        i += 10;
        eph.svh = getbitu(rtcm->buff, i, 1);              i += 1;
    }
    else {
        trace(2, "rtcm3 1042 length error: len=%d\n", rtcm->len);
        return -1;
    }
    trace(4, "decode_type1042: prn=%d iode=%d toe=%.0f\n", prn, eph.iode, eph.toes);

    if (rtcm->outtype) {
        msg = rtcm->msgtype + strlen(rtcm->msgtype);
        sprintf(msg, " prn=%2d iode=%3d iodc=%3d week=%d toe=%6.0f toc=%6.0f svh=%02X",
            prn, eph.iode, eph.iodc, week, eph.toes, toc, eph.svh);
    }
    if (!(sat = satno(sys, prn))) {
        trace(2, "rtcm3 1042 satellite number error: prn=%d\n", prn);
        return -1;
    }
    eph.sat = sat;
    eph.week = adjbdtweek(week);
    if (rtcm->time.time == 0) rtcm->time = utc2gpst(timeget());
    tt = timediff(bdt2gpst(bdt2time(eph.week, eph.toes)), rtcm->time);
    if (tt < -302400.0) eph.week++;
    else if (tt >= 302400.0) eph.week--;
    eph.toe = bdt2gpst(bdt2time(eph.week, eph.toes)); /* bdt -> gpst */
    eph.toc = bdt2gpst(bdt2time(eph.week, toc));      /* bdt -> gpst */
    eph.ttr = rtcm->time;
    eph.A = sqrtA * sqrtA;
    if (!strstr(rtcm->opt, "-EPHALL")) {
        if (timediff(eph.toe, rtcm->nav.eph[sat - 1].toe) == 0.0 &&
            eph.iode == rtcm->nav.eph[sat - 1].iode &&
            eph.iodc == rtcm->nav.eph[sat - 1].iodc) return 0; /* unchanged */
    }
    rtcm->nav.eph[sat - 1] = eph;
    //rtcm->ephset = 0;
    rtcm->ephsat = sat;
    return 2;
}
/* ssr 3 and 7 signal and tracking mode ids ----------------------------------*/
static const int codes_gps[] = {
    CODE_L1C,CODE_L1P,CODE_L1W,CODE_L1Y,CODE_L1M,CODE_L2C,CODE_L2D,CODE_L2S,
    CODE_L2L,CODE_L2X,CODE_L2P,CODE_L2W,CODE_L2Y,CODE_L2M,CODE_L5I,CODE_L5Q,
    CODE_L5X
};
static const int codes_glo[] = {
    CODE_L1C,CODE_L1P,CODE_L2C,CODE_L2P
};
static const int codes_gal[] = {
    CODE_L1A,CODE_L1B,CODE_L1C,CODE_L1X,CODE_L1Z,CODE_L5I,CODE_L5Q,CODE_L5X,
    CODE_L7I,CODE_L7Q,CODE_L7X,CODE_L8I,CODE_L8Q,CODE_L8X,CODE_L6A,CODE_L6B,
    CODE_L6C,CODE_L6X,CODE_L6Z
};
static const int codes_qzs[] = {
    CODE_L1C,CODE_L1S,CODE_L1L,CODE_L2S,CODE_L2L,CODE_L2X,CODE_L5I,CODE_L5Q,
    CODE_L5X,CODE_L6S,CODE_L6L,CODE_L6X,CODE_L1X
};
static const int codes_bds[] = {
    CODE_L1I,CODE_L1Q,CODE_L1X,CODE_L7I,CODE_L7Q,CODE_L7X,CODE_L6I,CODE_L6Q,
    CODE_L6X
};
static const int codes_sbs[] = {
    CODE_L1C,CODE_L5I,CODE_L5Q,CODE_L5X
};

/* get signal index ----------------------------------------------------------*/
static void sigindex(int sys, const unsigned char* code, const int* freq, int n,
    const char* opt, int* ind)
{
    int i, nex, pri, pri_h[8] = { 0 }, index[8] = { 0 }, ex[32] = { 0 };

    /* test code priority */
    for (i = 0; i < n; i++) {
        if (!code[i]) continue;

        if (freq[i] > NFREQ) { /* save as extended signal if freq > NFREQ */
            ex[i] = 1;
            continue;
        }
        /* code priority */
        pri = getcodepri(sys, code[i], opt);

        /* select highest priority signal */
        if (pri > pri_h[freq[i] - 1]) {
            if (index[freq[i] - 1]) ex[index[freq[i] - 1] - 1] = 1;
            pri_h[freq[i] - 1] = pri;
            index[freq[i] - 1] = i + 1;
        }
        else ex[i] = 1;
    }
    /* signal index in obs data */
    for (i = nex = 0; i < n; i++) {
        if (ex[i] == 0) ind[i] = freq[i] - 1;
        else if (nex < NEXOBS) ind[i] = NFREQ + nex++;
        else { /* no space in obs data */
            trace(2, "rtcm msm: no space in obs data sys=%d code=%d\n", sys, code[i]);
            ind[i] = -1;
        }
#if 0
        trace(2, "sig pos: sys=%d code=%d ex=%d ind=%d\n", sys, code[i], ex[i], ind[i]);
#endif
    }
}
/* save obs data in msm message ----------------------------------------------*/
static void save_msm_obs(rtcm_t* rtcm, int sys, msm_h_t* h, const double* r,
    const double* pr, const double* cp, const double* rr,
    const double* rrf, const double* cnr, const int* lock,
    const int* ex, const int* half)
{
    const char* sig[32];
    double tt, wl;
    unsigned char code[32];
    char* msm_type = "", * q = NULL;
    int i, j, k, type, prn, sat, fn, index = 0, freq[32], ind[32];

    type = getbitu(rtcm->buff, 24, 12);

    switch (sys) {
    case SYS_GPS: msm_type = q = rtcm->msmtype[0]; break;
    case SYS_GAL: msm_type = q = rtcm->msmtype[2]; break;
    case SYS_CMP: msm_type = q = rtcm->msmtype[5]; break;
    }
    /* id to signal */
    for (i = 0; i < h->nsig; i++) {
        switch (sys) {
        case SYS_GPS: sig[i] = msm_sig_gps[h->sigs[i] - 1]; break;
        case SYS_GAL: sig[i] = msm_sig_gal[h->sigs[i] - 1]; break;
        case SYS_CMP: sig[i] = msm_sig_cmp[h->sigs[i] - 1]; break;
        default: sig[i] = ""; break;
        }
        /* signal to rinex obs type */
        code[i] = obs2code(sig[i], freq + i);

        /* freqency index for beidou */
        if (sys == SYS_CMP) {
            if (freq[i] == 5) freq[i] = 2; /* B2 */
            else if (freq[i] == 4) freq[i] = 3; /* B3 */
        }
        if (code[i] != CODE_NONE) {
            if (q) q += sprintf(q, "L%s%s", sig[i], i < h->nsig - 1 ? "," : "");
        }
        else {
            if (q) q += sprintf(q, "(%d)%s", h->sigs[i], i < h->nsig - 1 ? "," : "");

            trace(2, "rtcm3 %d: unknown signal id=%2d\n", type, h->sigs[i]);
        }
    }
    //printf("rtcm3 %d: signals=%s ", type, msm_type);
    /* get signal index */
    sigindex(sys, code, freq, h->nsig, rtcm->opt, ind);

    for (i = j = 0; i < h->nsat; i++) {

        prn = h->sats[i];
        if (sys == SYS_QZS) prn += MINPRNQZS - 1;
        else if (sys == SYS_SBS) prn += MINPRNSBS - 1;

        if ((sat = satno(sys, prn))) {
            tt = timediff(rtcm->obs.data[0].time, rtcm->time);
            if (rtcm->obsflag || fabs(tt) > 1E-9) {
                rtcm->obs.n = rtcm->obsflag = 0;
            }
            index = obsindex(&rtcm->obs, rtcm->time, sat);
        }
        else {
            trace(2, "rtcm3 %d satellite error: prn=%d\n", type, prn);
        }
        for (k = 0; k < h->nsig; k++) {
            if (!h->cellmask[k + i * h->nsig]) continue;

            if (sat && index >= 0 && ind[k] >= 0) {

                /* satellite carrier wave length */
                wl = satwavelen(sat, freq[k] - 1, &rtcm->nav);

                /* glonass wave length by extended info */
                if (sys == SYS_GLO && ex && ex[i] <= 13) {
                    fn = ex[i] - 7;
                    wl = CLIGHT / ((freq[k] == 2 ? FREQ2_GLO : FREQ1_GLO) +
                        (freq[k] == 2 ? DFRQ2_GLO : DFRQ1_GLO) * fn);
                }
                /* pseudorange (m) */
                if (r[i] != 0.0 && pr[j] > -1E12) {
                    rtcm->obs.data[index].P[ind[k]] = r[i] + pr[j];
                }
                /* carrier-phase (cycle) */
                if (r[i] != 0.0 && cp[j] > -1E12 && wl > 0.0) {
                    rtcm->obs.data[index].L[ind[k]] = (r[i] + cp[j]) / wl;
                }
                /* doppler (hz) */
                if (rr && rrf && rrf[j] > -1E12 && wl > 0.0) {
                    rtcm->obs.data[index].D[ind[k]] = (float)(-(rr[i] + rrf[j]) / wl);
                }
                rtcm->obs.data[index].LLI[ind[k]] =
                    lossoflock(rtcm, sat, ind[k], lock[j]) + (half[j] ? 3 : 0);
                rtcm->obs.data[index].SNR[ind[k]] = (unsigned char)(cnr[j] * 4.0);
                rtcm->obs.data[index].code[ind[k]] = code[k];
            }
            j++;
        }
    }
}
/* decode type msm message header --------------------------------------------*/
static int decode_msm_head(rtcm_t* rtcm, int sys, int* sync, int* iod,
    msm_h_t* h, int* hsize) {
    msm_h_t h0 = { 0 };
    double tow, tod;
    char* msg;
    int i = 24, j, dow, mask, staid, type, ncell = 0;
    type = getbitu(rtcm->buff, i, 12);
    i += 12;
    *h = h0;
    if (i + 157 <= rtcm->len * 8) {
        staid = getbitu(rtcm->buff, i, 12);       i += 12;
        if (sys == SYS_CMP) {
            tow = getbitu(rtcm->buff, i, 30) * 0.001; i += 30;
            tow += 14.0; /* BDT -> GPST */
            adjweek(rtcm, tow);
        }
        else {
            tow = getbitu(rtcm->buff, i, 30) * 0.001;   i += 30;
            adjweek(rtcm, tow);
        }
        *sync = getbitu(rtcm->buff, i, 1);      i += 1;
        *iod = getbitu(rtcm->buff, i, 3);       i += 3;
        h->time_s = getbitu(rtcm->buff, i, 7);  i += 7;
        h->clk_str = getbitu(rtcm->buff, i, 2); i += 2;
        h->clk_ext = getbitu(rtcm->buff, i, 2); i += 2;
        h->smooth = getbitu(rtcm->buff, i, 1);  i += 1;
        h->tint_s = getbitu(rtcm->buff, i, 3);  i += 3;
        for (j = 1; j <= 64; j++) {
            mask = getbitu(rtcm->buff, i, 1);
            i += 1;
            if (mask) h->sats[h->nsat++] = j;
        }
        for (j = 1; j <= 32; j++) {
            mask = getbitu(rtcm->buff, i, 1);
            i += 1;
            if (mask) h->sigs[h->nsig++] = j;
        }
    }
    else {
        trace(2, "rtcm3 %d length error: len=%d\n", type, rtcm->len);
        return -1;
    }
    /* test station id */
    if (!test_staid(rtcm, staid)) return -1;

    if (h->nsat * h->nsig > 64) {
        printf("rtcm3 %d number of sats and sigs error: nsat=%d nsig=%d\n",
            type, h->nsat, h->nsig);
        return -1;
    }
    if (i + h->nsat * h->nsig > rtcm->len * 8) {
        trace(2, "rtcm3 %d length error: len=%d nsat=%d nsig=%d\n", type,
            rtcm->len, h->nsat, h->nsig);
        return -1;
    }
    for (j = 0; j < h->nsat * h->nsig; j++) {
        h->cellmask[j] = getbitu(rtcm->buff, i, 1);
        i += 1;
        if (h->cellmask[j]) ncell++;
    }
    *hsize = i;
    //printf("decode_head_msm: time=%s sys=%d staid=%d nsat=%d nsig=%d sync=%d iod=%d ncell=%d\n",
    //    time_str(rtcm->time, 2), sys, staid, h->nsat, h->nsig, *sync, *iod, ncell);
    if (rtcm->outtype) {
        msg = rtcm->msgtype + strlen(rtcm->msgtype);
        sprintf(msg, " staid=%4d %s nsat=%2d nsig=%2d iod=%2d ncell=%2d sync=%d",
            staid, time_str(rtcm->time, 2), h->nsat, h->nsig, *iod, ncell, *sync);
    }
    return ncell;
}//返回 8

/* decode msm 7: full pseudorange and phaserange plus cnr --------------------*/
static int decode_msm7(rtcm_t* rtcm, int sys)
{
    msm_h_t h = { 0 };
    double r[64], rr[64], pr[64], cp[64], rrf[64], cnr[64];
    int i, j, type, sync, iod, ncell, rng, rng_m, rate, prv, cpv, rrv, lock[64];
    int ex[64], half[64];

    type = getbitu(rtcm->buff, 24, 12);

    /* decode msm header */
    if ((ncell = decode_msm_head(rtcm, sys, &sync, &iod, &h, &i)) < 0) {
        return -1;
    }
    if (i + h.nsat * 36 + ncell * 80 > rtcm->len * 8) {
        trace(2, "rtcm3 %d length error: nsat=%d ncell=%d len=%d\n", type, h.nsat,
            ncell, rtcm->len);
        rtcm->obsflag = !sync;  /* header ok, so return sync bit */
        return sync ? 0 : 1;
    }
    for (j = 0; j < h.nsat; j++) {
        r[j] = rr[j] = 0.0; ex[j] = 15;
    }
    for (j = 0; j < ncell; j++) {
        pr[j] = cp[j] = rrf[j] = -1E16;
    }
    /* decode satellite data */
    for (j = 0; j < h.nsat; j++) { /* range */
        rng = getbitu(rtcm->buff, i, 8); i += 8;
        if (rng != 255) {
            r[j] = rng * RANGE_MS;
        }
    }
    for (j = 0; j < h.nsat; j++) { /* extended info */
        ex[j] = getbitu(rtcm->buff, i, 4); i += 4;
    }
    for (j = 0; j < h.nsat; j++) {
        rng_m = getbitu(rtcm->buff, i, 10); i += 10;
        if (r[j] != 0.0) {
            r[j] += rng_m * P2_10 * RANGE_MS;
        }
    }
    for (j = 0; j < h.nsat; j++) { /* phaserangerate */
        rate = getbits(rtcm->buff, i, 14); i += 14;
        if (rate != -8192) {
            rr[j] = rate * 1.0;
            if (strstr(rtcm->opt, "-INVPRR")) rr[j] = -rr[j];
        }
    }
    /* decode signal data */
    for (j = 0; j < ncell; j++) { /* pseudorange */
        prv = getbits(rtcm->buff, i, 20); i += 20;
        if (prv != -524288) pr[j] = prv * P2_29 * RANGE_MS;
    }
    for (j = 0; j < ncell; j++) { /* phaserange */
        cpv = getbits(rtcm->buff, i, 24); i += 24;
        if (cpv != -8388608) cp[j] = cpv * P2_31 * RANGE_MS;
    }
    for (j = 0; j < ncell; j++) { /* lock time */
        lock[j] = getbitu(rtcm->buff, i, 10); i += 10;
    }
    for (j = 0; j < ncell; j++) { /* half-cycle amiguity */
        half[j] = getbitu(rtcm->buff, i, 1); i += 1;
    }
    for (j = 0; j < ncell; j++) { /* cnr */
        cnr[j] = getbitu(rtcm->buff, i, 10) * 0.0625; i += 10;
    }
    for (j = 0; j < ncell; j++) { /* phaserangerate */
        rrv = getbits(rtcm->buff, i, 15); i += 15;
        if (rrv != -16384) {
            rrf[j] = rrv * 0.0001;
            if (strstr(rtcm->opt, "-INVPRR")) rrf[j] = -rrf[j];
        }
    }
    /* save obs data in msm message */
    save_msm_obs(rtcm, sys, &h, r, pr, cp, rr, rrf, cnr, lock, ex, half);

    rtcm->obsflag = !sync;
    return sync ? 0 : 1;
}

/* decode rtcm ver.3 message -------------------------------------------------*/
extern int decode_rtcm3(rtcm_t* rtcm,int posflag) {
    double tow;
    int ret = 0, type = getbitu(rtcm->buff, 24, 12), week;
    //if (rtcm->outtype) {
    //    sprintf(rtcm->msgtype, "RTCM %4d (number of bytes:%4d):", type, rtcm->len);
    //printf("RTCM %4d (number of bytes:%4d):\n", type, rtcm->len); //RTCM类型和一帧的长度
    //}
    /* real-time input option */
    if (strstr(rtcm->opt, "-RT_INP")) {
        tow = time2gpst(utc2gpst(timeget()), &week);
        rtcm->time = gpst2time(week, floor(tow));
    }
    switch (type) {
        char s1[32]; //定义字符数组，用来将gtime_t 时间变为 字符
        double pos[3]; //定义BLH；
    case 1019: ret = decode_type1019(rtcm);         //GPS nav
        break;
    case 1042: ret = decode_type1042(rtcm);         //BDS nav
        break;
    case 1046: ret = decode_type1046(rtcm);         //GAL nav
        break;
    case 1077: ret = decode_msm7(rtcm, SYS_GPS);
            rtcm->sys++;
            rtcm->timeflag += rtcm->time.sec;
        break;
    case 1097: ret = decode_msm7(rtcm, SYS_GAL);
            rtcm->sys++;
            rtcm->timeflag += rtcm->time.sec;
        break;
    case 1127: ret = decode_msm7(rtcm, SYS_CMP);
            rtcm->sys++;
            rtcm->timeflag += rtcm->time.sec;
        break;
    }
    if (ret >= 0) {
        type -= 1000;
        if (1 <= type && type <= 299) rtcm->nmsg3[type]++; /* 1001-1299 */
        else if (1000 <= type && type <= 1099) rtcm->nmsg3[type - 700]++; /* 2000-2099 */
        else rtcm->nmsg3[0]++;
    }
    //if ((rtcm->sys == 2) && rtcm->tflag == rtcm->time.sec * 2) {
    //    return 3;
    //}
    //&& rtcm->sys == 3 && rtcm->timeflag == rtcm->time.sec * 3
        return 3;
   // return ret;
}