#include "rtklib.h"
#include <rtthread.h>
#include <rtdevice.h>
#include <ulog.h>
#include <rtdef.h>

#define MIN_INT_RESET 30000 /* mininum interval of reset command (ms) */
#define LOG_TAG "ST_rtksvr.c"



/* 静态变量 ------------------------------------*/
#ifdef STATIC
#define SVR_STATIC
#endif

/* read strial -----------------------------------------------------------------
 * read data from strial (unblocked)
 * args   : unsinged char *buff O data buffer
 *          int    n         I  maximum data length
 *          int    index     I  (user or refer)
 * return : read data length
 * notes  : if no data, return immediately with no data
 *-----------------------------------------------------------------------------*/
//static int read_strial(uint8_t *buff, int n, int index)
//{
//    int nr = 0;
//    // TODO:  从串口读取数据
//    // index:{rov,base} 对应的串口
//    switch (index)
//    {
//    case 0:
//        break;
//    case 1:
//        break;
//    default: // 正常不会进到这里
//        break;
//    }
//    return nr;
//}

/* save output buffer --------------------------------------------------------*/
//static void saveoutbuf(rtksvr_t *svr, uint8_t *buff, int n, int index)
//{
//    rtksvrlock(svr);
//
//    n = n < svr->buffsize - svr->nsb[index] ? n : svr->buffsize - svr->nsb[index]; // 判断是否能完整写入
//    memcpy(svr->sbuf[index] + svr->nsb[index], buff, n);
//    svr->nsb[index] += n;
//
//    rtksvrunlock(svr);
//}

/* write solution to output stream -------------------------------------------*/
void writesol_mini(rtksvr_t *svr, int index)
{
    // solopt_t solopt = solopt_default;
    uint8_t buff[MAXSOLMSG + 1];
    int i=0;
    int n=0;

    tracet(4, "writesol: index=%d\n", index);

    for (i = 0; i < 2; i++)
    {

        if (svr->solopt[i].posf == SOLF_STAT)
        {

            /* output solution status */
//            rtksvrlock(svr);
//            // n = rtkoutstat(&svr->rtk, (char*)buff);
//            rtksvrunlock(svr);
        }
        else
        {
            /* output solution */
            n = outsols(buff, &svr->rtk.sol, svr->rtk.rb, svr->solopt + i);
        }
        // strwrite(svr->stream + i + 3, buff, n);
        // TODO: 这里应该改为从串口输出解算后的数据

        /* 输出NMEA GSA GSV 相关信息 */
        // n = outsolexs(buff, &svr->rtk.sol, svr->rtk.ssat, svr->solopt + i);
    }
}
#ifdef ENAGLO
/* update glonass frequency channel number in raw data struct ----------------*/
static void update_glofcn(rtksvr_t *svr)
{
    int i, j, sat, frq;

    for (i = 0; i < MAXPRNGLO; i++)
    {
        sat = satno(SYS_GLO, i + 1);

        for (j = 0, frq = -999; j < 3; j++)
        {
            if (svr->raw[j].nav.geph[i].sat != sat)
                continue;
            frq = svr->raw[j].nav.geph[i].frq;
        }
        if (frq < -7 || frq > 6)
            continue;

        for (j = 0; j < 3; j++)
        {
            if (svr->raw[j].nav.geph[i].sat == sat)
                continue;
            svr->raw[j].nav.geph[i].sat = sat;
            svr->raw[j].nav.geph[i].frq = frq;
        }
    }
}
#endif
/* update observation data ---------------------------------------------------*/
static void update_obs(rtksvr_t *svr, obs_t *obs, int index, int iobs)
{
    int i, n = 0, sat, sys;

    if (iobs < MAXOBSBUF)
    {
        for (i = 0; i < obs->n; i++)
        {
            sat = obs->data[i].sat;
            sys = satsys(sat, NULL);
            if (svr->rtk.opt.exsats[sat - 1] == 1 || !(sys & svr->rtk.opt.navsys))
            {
                continue;
            }
            svr->obs[index][iobs].data[n] = obs->data[i];
            svr->obs[index][iobs].data[n++].rcv = index + 1;
        }
        svr->obs[index][iobs].n = n;
        sortobs(&svr->obs[index][iobs]);
    }
    svr->nmsg[index][0]++;
}
static void update_eph(rtksvr_t *svr, nav_t *nav, int ephsat, int ephset,
                       int index)
{
    eph_t *eph1, *eph2;
#ifdef ENAGLO
    geph_t *geph1, *geph2;
#endif
    int prn;

    if (satsys(ephsat, &prn) != SYS_GLO)
    {
        if (!svr->navsel || svr->navsel == index + 1)
        {
            /* svr->nav.eph={current_set1,current_set2} */
            eph1 = nav->eph + ephsat - 1 + MAXSAT * ephset;     /* 接收到的 */
            eph2 = svr->nav.eph + ephsat - 1 + MAXSAT * ephset; /* 当前的 */
            if (eph2->ttr.time == 0 || (eph1->iode != eph2->iode) || (timediff(eph1->toe, eph2->toe) != 0.0) || (timediff(eph1->toc, eph2->toc) != 0.0))
            {
                *eph2 = *eph1; /* received->current */
            }
        }
        svr->nmsg[index][1]++;
    }
#ifdef ENAGLO
    else
    {
        if (!svr->navsel || svr->navsel == index + 1)
        {
            geph1 = nav->geph + prn - 1;
            geph2 = svr->nav.geph + prn - 1;
            if (geph2->tof.time == 0 ||
                (geph1->iode != geph2->iode))
            {
                *geph2 = *geph1;
                update_glofcn(svr);
            }
        }
        svr->nmsg[index][6]++;
    }
#endif
}
#ifdef ENASBS
/* update sbas message -------------------------------------------------------*/
static void update_sbs(rtksvr_t *svr, sbsmsg_t *sbsmsg, int index)
{
    int i, sbssat = svr->rtk.opt.sbassatsel;
    if (sbsmsg && (sbssat == sbsmsg->prn || sbssat == 0))
    {
        sbsmsg->rcv = index + 1;
        if (svr->nsbs < MAXSBSMSG)
        {
            svr->sbsmsg[svr->nsbs++] = *sbsmsg;
        }
        else
        {
            for (i = 0; i < MAXSBSMSG - 1; i++)
                svr->sbsmsg[i] = svr->sbsmsg[i + 1];
            svr->sbsmsg[i] = *sbsmsg;
        }
        sbsupdatecorr(sbsmsg, &svr->nav);
    }
    svr->nmsg[index][3]++;
}
/* update ssr corrections ----------------------------------------------------*/
static void update_ssr(rtksvr_t *svr, int index)
{
    int i, sys, prn, iode;

    for (i = 0; i < MAXSAT; i++)
    {
        if (!svr->rtcm[index].ssr[i].update)
            continue;

        /* check consistency between iods of orbit and clock */
        if (svr->rtcm[index].ssr[i].iod[0] != svr->rtcm[index].ssr[i].iod[1])
        {
            continue;
        }
        svr->rtcm[index].ssr[i].update = 0;

        iode = svr->rtcm[index].ssr[i].iode;
        sys = satsys(i + 1, &prn);

        /* check corresponding ephemeris exists */
        if (sys == SYS_GPS || sys == SYS_GAL || sys == SYS_QZS)
        {
            if (svr->nav.eph[i].iode != iode &&
                svr->nav.eph[i + MAXSAT].iode != iode)
            {
                continue;
            }
        }
        else if (sys == SYS_GLO)
        {
            if (svr->nav.geph[prn - 1].iode != iode &&
                svr->nav.geph[prn - 1 + MAXPRNGLO].iode != iode)
            {
                continue;
            }
        }
        svr->nav.ssr[i] = svr->rtcm[index].ssr[i];
    }
    svr->nmsg[index][7]++;
}
#endif
/* update ion/utc parameters -------------------------------------------------*/
static void update_ionutc(rtksvr_t *svr, nav_t *nav, int index)
{
    if (svr->navsel == 0 || svr->navsel == index + 1)
    {
        matcpy(svr->nav.utc_gps, nav->utc_gps, 8, 1);
        // matcpy(svr->nav.utc_glo, nav->utc_glo, 8, 1);
        // matcpy(svr->nav.utc_gal, nav->utc_gal, 8, 1);
        // matcpy(svr->nav.utc_qzs, nav->utc_qzs, 8, 1);
        // matcpy(svr->nav.utc_cmp, nav->utc_cmp, 8, 1);
        // matcpy(svr->nav.utc_irn, nav->utc_irn, 9, 1);
        // matcpy(svr->nav.utc_sbs, nav->utc_sbs, 4, 1);
        matcpy(svr->nav.ion_gps, nav->ion_gps, 8, 1);
        // matcpy(svr->nav.ion_gal, nav->ion_gal, 4, 1);
        // matcpy(svr->nav.ion_qzs, nav->ion_qzs, 8, 1);
        // matcpy(svr->nav.ion_cmp, nav->ion_cmp, 8, 1);
        // matcpy(svr->nav.ion_irn, nav->ion_irn, 8, 1);
    }
    svr->nmsg[index][2]++;
}
/* update antenna position ---------------------------------------------------*/
static void update_antpos(rtksvr_t *svr, int index)
{
    sta_t *sta;
    double pos[3], del[3] = {0}, dr[3];
    int i;

    if (svr->rtk.opt.refpos == POSOPT_RTCM && index == 1)
    {
        if (svr->format[1] == STRFMT_RTCM3)
        {
            sta = &svr->rtcm[1].sta;
            /* update base station position */
            for (i = 0; i < 3; i++)
            {
                svr->rtk.rb[i] = sta->pos[i];
            }
            /* antenna delta */
            ecef2pos(svr->rtk.rb, pos);
            if (sta->deltype)
            { /* xyz */
                del[2] = sta->hgt;
                enu2ecef(pos, del, dr);
                for (i = 0; i < 3; i++)
                {
                    svr->rtk.rb[i] += sta->del[i] + dr[i];
                }
            }
            else
            { /* enu */
                enu2ecef(pos, sta->del, dr);
                for (i = 0; i < 3; i++)
                {
                    svr->rtk.rb[i] += dr[i];
                }
            }
        }
    }
    svr->nmsg[index][4]++;
}

/* update rtk server struct --------------------------------------------------*/
// static void update_svr(rtksvr_t *svr, int ret, obs_t *obs, nav_t *nav,
//                        int ephsat, int ephset, sbsmsg_t *sbsmsg, int index,
//                        int iobs)
static void update_svr(rtksvr_t *svr, int ret, obs_t *obs, nav_t *nav,
                       int ephsat, int ephset, int index, int iobs)
{
    int nbyte = 0;
    tracet(4, "updatesvr: ret=%d ephsat=%d ephset=%d index=%d\n", ret, ephsat,
           ephset, index);

    if (ret == 1)
    { /* observation data */
        update_obs(svr, obs, index, iobs);
        if (index == 0)
        {
            nbyte = dbg_show_obs(svr->obs[0][0].data, svr->obs[0][0].n, 0, (char *)NULL);
        }
    }
    else if (ret == 2)
    { /* ephemeris */
        update_eph(svr, nav, ephsat, ephset, index);
    }
    // else if (ret==3) { /* sbas message */
    //     update_sbs(svr,sbsmsg,index);
    // }
    else if (ret == 9)
    { /* ion/utc parameters */
        update_ionutc(svr, nav, index);
    }
    else if (ret == 5)
    { /* antenna postion */
        update_antpos(svr, index);
    }
    else if (ret == 7)
    { /* dgps correction */
        svr->nmsg[index][5]++;
    }
    // else if (ret==10) { /* ssr message */
    //     update_ssr(svr,index);
    // }
    else if (ret == -1)
    { /* error */
        svr->nmsg[index][9]++;
    }
}
/* decode receiver raw/rtcm data ---------------------------------------------*/
int decoderaw(rtksvr_t *svr, int index)
{
    obs_t *obs;
    nav_t *nav;
    // sbsmsg_t *sbsmsg=NULL;
    int i, ret, ephsat, ephset, fobs = 0;
    rtksvrlock(svr);

    for (i = 0; i < svr->nb[index]; i++)
    {

        /* input rtcm/receiver raw data from stream */
        /*
        if (svr->format[index]==STRFMT_RTCM3) {
           ret=input_rtcm3(svr->rtcm+index,svr->buff[index][i]);
           obs=&svr->rtcm[index].obs;
           nav=&svr->rtcm[index].nav;
           ephsat=svr->rtcm[index].ephsat;
           ephset=svr->rtcm[index].ephset;
        }
        else {
            ret=input_raw(svr->raw+index,svr->format[index],svr->buff[index][i]);
            obs=&svr->raw[index].obs;
            nav=&svr->raw[index].nav;
            ephsat=svr->raw[index].ephsat;
            ephset=svr->raw[index].ephset;
            sbsmsg=&svr->raw[index].sbsmsg;
        }*/
        ret = input_rtcm3(svr->rtcm + index, svr->buff[index][i]);
        /* (-1: error message, 0: no message, 1: input observation data,
            2: input ephemeris, 5: input station pos/ant parameters,
            10: input ssr messages) */
        obs = &svr->rtcm[index].obs;
        nav = &svr->rtcm[index].nav;
        ephsat = svr->rtcm[index].ephsat;
        // ephset = svr->rtcm[index].ephset; /* 没有使能GAL卫星 所以这个标志位永远为0 */
        ephset = 0; /* 星历选择*/

        /* update rtk server */
        if (ret > 0)
        {
            // update_svr(svr, ret, obs, nav, ephsat, ephset, sbsmsg, index, fobs);
            update_svr(svr, ret, obs, nav, ephsat, ephset, index, fobs);
        }
        /* observation data received */
        if (ret == 1) /* 接受一次完整的观测值电文 */
        {
            if (fobs < MAXOBSBUF)
                fobs++;
            else
                svr->prcout++;
        }
    }
    svr->nb[index] = 0;

    rtksvrunlock(svr);

    return fobs;
}
/* carrier-phase bias (fcb) correction ---------------------------------------*/
// static void corr_phase_bias(obsd_t *obs, int n, const nav_t *nav)
//{
//     double freq;
//     uint8_t code;
//     int i,j;
//
//     for (i=0;i<n;i++) for (j=0;j<NFREQ;j++) {
//         code=obs[i].code[j];
//         if ((freq=sat2freq(obs[i].sat,code,nav))==0.0) continue;
//
//         /* correct phase bias (cyc) */
//         obs[i].L[j]-=nav->ssr[obs[i].sat-1].pbias[code-1]*freq/CLIGHT;
//     }
// }
/* periodic command ----------------------------------------------------------*/
static void periodic_cmd(int cycle, const char *cmd, stream_t *stream)
{
    const char *p = cmd, *q;
    char msg[1024], *r;
    int n, period;

    for (p = cmd;; p = q + 1)
    {
        for (q = p;; q++)
            if (*q == '\r' || *q == '\n' || *q == '\0')
                break;
        n = (int)(q - p);
        strncpy(msg, p, n);
        msg[n] = '\0';

        period = 0;
        if ((r = strrchr(msg, '#')))
        {
            sscanf(r, "# %d", &period);
            *r = '\0';
            while (*--r == ' ')
                *r = '\0'; /* delete tail spaces */
        }
        if (period <= 0)
            period = 1000;
        if (*msg && cycle % period == 0)
        {
            strsendcmd(stream, msg);
        }
        if (!*q)
            break;
    }
}
/* baseline length -----------------------------------------------------------*/
static double baseline_len(const rtk_t *rtk)
{
    double dr[3];
    int i;

    if (norm(rtk->sol.rr, 3) <= 0.0 || norm(rtk->rb, 3) <= 0.0)
        return 0.0;

    for (i = 0; i < 3; i++)
    {
        dr[i] = rtk->sol.rr[i] - rtk->rb[i];
    }
    return norm(dr, 3) * 0.001; /* (km) */
}
/* send nmea request to base/nrtk input stream -------------------------------*/
//todo static void send_nmea(rtksvr_t *svr, uint32_t *tickreset)
//{
//    sol_t sol_nmea = {{0}};
//    double vel, bl;
//    uint32_t tick = tickget();
//    int i;
//
//// todo   if (svr->stream[1].state != 1)
////        return;
//    sol_nmea.ns = 10; /* Some servers don't like when ns = 0 */
//
//    if (svr->nmeareq == 1)
//    { /* lat-lon-hgt mode */
//        sol_nmea.stat = SOLQ_SINGLE;
//        sol_nmea.time = utc2gpst(timeget());
//        matcpy(sol_nmea.rr, svr->nmeapos, 3, 1);
////todo        strsendnmea(svr->stream + 1, &sol_nmea);
//    }
//    else if (svr->nmeareq == 2)
//    { /* single-solution mode */
//        if (norm(svr->rtk.sol.rr, 3) <= 0.0)
//            return;
//        sol_nmea.stat = SOLQ_SINGLE;
//        sol_nmea.time = utc2gpst(timeget());
//        matcpy(sol_nmea.rr, svr->rtk.sol.rr, 3, 1);
//// todo       strsendnmea(svr->stream + 1, &sol_nmea);
//    }
//    else if (svr->nmeareq == 3)
//    { /* reset-and-single-sol mode */
//
//        /* send reset command if baseline over threshold */
//        bl = baseline_len(&svr->rtk);
//        if (bl >= svr->bl_reset && (int)(tick - *tickreset) > MIN_INT_RESET)
//        {
//            // strsendcmd(svr->stream+1,svr->cmd_reset);
//
//            tracet(2, "send reset: bl=%.3f rr=%.3f %.3f %.3f rb=%.3f %.3f %.3f\n",
//                   bl, svr->rtk.sol.rr[0], svr->rtk.sol.rr[1], svr->rtk.sol.rr[2],
//                   svr->rtk.rb[0], svr->rtk.rb[1], svr->rtk.rb[2]);
//            *tickreset = tick;
//        }
//        if (norm(svr->rtk.sol.rr, 3) <= 0.0)
//            return;
//        sol_nmea.stat = SOLQ_SINGLE;
//        sol_nmea.time = utc2gpst(timeget());
//        matcpy(sol_nmea.rr, svr->rtk.sol.rr, 3, 1);
//
//        /* set predicted position if velocity > 36km/h */
//        if ((vel = norm(svr->rtk.sol.rr + 3, 3)) > 10.0)
//        {
//            for (i = 0; i < 3; i++)
//            {
//                sol_nmea.rr[i] += svr->rtk.sol.rr[i + 3] / vel * svr->bl_reset * 0.8;
//            }
//        }
////todo        strsendnmea(svr->stream + 1, &sol_nmea);
//
//        tracet(3, "send nmea: rr=%.3f %.3f %.3f\n", sol_nmea.rr[0], sol_nmea.rr[1],
//               sol_nmea.rr[2]);
//    }
//}
/* rtk server mini thread ---------------------------------------------------------*/

//static void *rtksvrthread_mini(void *arg)
//{
//    rtksvr_t *svr = (rtksvr_t *)arg;
//    obs_t obs;               /* 28 B */
//    obsd_t data[MAXOBS * 2]; /* 7616 B */
//    sol_t sol = {{0}};       /* 199 B */
//    double tt;
//    uint32_t tick, ticknmea, tick1hz, tickreset;
//    uint8_t *p, *q;
//    int i, j, n = 0, fobs[2] = {0}, cycle, cputime;
//    uint8_t nr, nu;
//    tracet(3, "rtksvrthread:\n");
//
//    svr->state = 1;
//    obs.data = data;
//    svr->tick = tickget();
//    ticknmea = tick1hz = svr->tick - 1000;
//    tickreset = svr->tick - MIN_INT_RESET;
//
//    while(1)
//    {
//
//
//        tick = tickget();
//        for (i = 0; i < 2; i++)
//        {
//            p = svr->buff[i] + svr->nb[i];
//            q = svr->buff[i] + svr->buffsize;
//
//            /* 读取基准站/接收机发送来的消息 */
//            if ((n = read_strial(p, q - p, i)) <= 0)
//                continue;
//            svr->nb[i] += n;
//        }
//        /* 解码基准站/接收机的消息 解码后数据保存在RTCM[index] 结构中 再更新到svr->obs */
//        for (i = 0; i < 2; i++)
//        {
//            fobs[i] = decoderaw(svr, i); //传入svr->buff[i] 解算成功后会对 svr->nb[i] 清零
//        }
//        /* 将基准站和移动站的观测数据合二为一 这里重新申请了一个MAXOBS*2 的数组, 我觉得是不是有点问题? */
//        for (i = 0; i < fobs[0]; i++)
//        {
//            obs.n = 0;
//            nr = nu = 0;
//            for (j = 0; j < svr->obs[0][i].n && obs.n < MAXOBS * 2; j++)
//            {
//                obs.data[obs.n++] = svr->obs[0][i].data[j];
//                nu++;
//            }
//
//            for (j = 0; j < svr->obs[1][0].n && obs.n < MAXOBS * 2; j++)
//            {
//                obs.data[obs.n++] = svr->obs[1][0].data[j];
//                nr++;
//            }
//
//            /* rtk positioning */
//            rtksvrlock(svr);
//            rtkpos(&svr->rtk, obs.data, obs.n, &svr->nav);
//
//            rtksvrunlock(svr);
//
//            /* 如果解算结果有效, 设置解算时间点 */
//            if (svr->rtk.sol.stat != SOLQ_NONE)
//            {
//                /* adjust current time */
//                tt = (int)(tickget() - tick) / 1000.0 + DTTOL;
//                timeset(gpst2utc(timeadd(svr->rtk.sol.time, tt)));
//
//                /* write solution */
//                writesol_mini(svr, i);
//            }
//            /* 如果设定的运算时间到了发现还没有计算完, 则记录没有计算完的观测次数 */
//            if ((int)(tickget() - tick) >= svr->cycle)
//            {
//                svr->prcout += fobs[0] - i - 1;
//            }
//        }
//        /* send null solution if no solution (1hz) */
//        if (svr->rtk.sol.stat == SOLQ_NONE && (int)(tick - tick1hz) >= 1000)
//        {
//            writesol_mini(svr, 0);
//            tick1hz = tick;
//        }
//        /* send nmea request to base/nrtk input stream */
//        if (svr->nmeacycle > 0 && (int)(tick - ticknmea) >= svr->nmeacycle)
//        {
//            // TODO: 完成send_nmea 到到串口的移植
//            // send_nmea(svr, &tickreset);
//            ticknmea = tick;
//        }
//        cputime = (int)(tickget() - tick);
//        if (cputime > 0)
//            svr->cputime = cputime;
//
//        /* sleep until next cycle */
//        sleepms(svr->cycle - cputime);
//    }
//    /* 关闭已经打开的流 */
////    for (i = 0; i < MAXSTRRTK; i++)
////        // TODO: 这里应该是关闭嵌入式的数据流 例如接收机的串口 移动站的串口
////        // strclose(svr->stream + i);
////        for (i = 0; i < 2; i++)
////        {
////            svr->nb[i] = 0;
//////            svr->npb[i] = 0;
////            free(svr->buff[i]);
////            svr->buff[i] = NULL;
////            // free(svr->pbuf[i]);
////            // svr->pbuf[i] = NULL;
////            // free_raw (svr->raw +i);
////            free_rtcm(svr->rtcm + i);
////        }
//    // for (i = 0; i < 2; i++)
//    // {
//    //     svr->nsb[i] = 0;
//    //     free(svr->sbuf[i]);
//    //     svr->sbuf[i] = NULL;
//    // }
//    return 0;
//}
/* initialize rtk server -------------------------------------------------------
 * initialize rtk server
 * args   : rtksvr_t *svr    IO rtk server
 * return : status (0:error,1:ok)
 *-----------------------------------------------------------------------------*/
int rtksvrinit(rtksvr_t *svr)
{
    gtime_t time0 = {0};
    sol_t sol0 = {{0}};
    eph_t eph0 = {0, -1, -1};
    int i, j;

    tracet(3, "rtksvrinit:\n");

    rtkinit(&svr->rtk, &prcopt_default); /* 初始化了RTK需要的数组 */
    svr->state = svr->cycle = svr->nmeacycle = svr->nmeareq = 0;
    for (i = 0; i < 3; i++)
        svr->nmeapos[i] = 0.0; /* 初始化NMEA坐标 */
    svr->buffsize = 0;
    for (i = 0; i < 2; i++)
        svr->format[i] = 0; /* init 格式 */
    for (i = 0; i < 2; i++)
        svr->solopt[i] = solopt_default; /* init 输出格式 */
    svr->navsel = svr->nsol = 0;
//    svr->nsbs = 0;
    for (i = 0; i < 2; i++)
        svr->nb[i] = 0;
//    for (i = 0; i < 2; i++)
//        svr->nsb[i] = 0;
//    for (i = 0; i < 2; i++)
//        svr->npb[i] = 0;
    for (i = 0; i < 2; i++)
        svr->buff[i] = NULL;
//    for (i = 0; i < 2; i++)
//        svr->sbuf[i] = NULL;
//    for (i = 0; i < 2; i++)
//        svr->pbuf[i] = NULL;
    for (i = 0; i < MAXSOLBUF; i++)
        svr->solbuf[i] = sol0;
    for (i = 0; i < 2; i++)
        for (j = 0; j < 10; j++)
            svr->nmsg[i][j] = 0;

    svr->tick = 0;
//todo    svr->thread = 0;
    svr->cputime = svr->prcout = svr->nave = 0;
    for (i = 0; i < 3; i++)
        svr->rb_ave[i] = 0.0;

    memset(&svr->nav, 0, sizeof(nav_t));

#ifdef SVR_STATIC
    extern eph_t svr_eph[MAXSAT];
    extern obsd_t svr_obsd[2][MAXOBS];

    svr->nav.eph = svr_eph;
    for (i = 0; i < MAXSAT * 1; i++)
        svr->nav.eph[i] = eph0;
    svr->nav.n = MAXSAT * 1;
#ifdef ENAGLO
    extern geph_t svr_geph[NSATGLO];
    geph_t geph0 = {0, -1};
    svr->nav.geph = svr_geph;
    for (i = 0; i < NSATGLO * 1; i++)
        svr->nav.geph[i] = geph0;
    svr->nav.ng = NSATGLO * 1;
#endif
#ifdef ENASBS
    extern seph_t svr_seph[NSATSBS];
    seph_t seph0 = {0};
    svr->nav.seph = svr_seph;
    for (i = 0; i < NSATSBS * 1; i++)
        svr->nav.seph[i] = seph0;
    svr->nav.ns = NSATSBS * 1;
#endif
    for (i = 0; i < 2; i++)
        for (j = 0; j < MAXOBSBUF; j++)
        {
            svr->obs[i][j].data = &(svr_obsd[i][j]);
        }
#else
    if (!(svr->nav.eph = (eph_t *)malloc(sizeof(eph_t) * MAXSAT * 1)))
    {
        tracet(1, "rtksvrinit: malloc error\n");
        return 0;
    }
    for (i = 0; i < MAXSAT * 1; i++)
        svr->nav.eph[i] = eph0;
    svr->nav.n = MAXSAT * 1;
#ifdef ENAGLO
    geph_t geph0 = {0, -1};
    if (!(svr->nav.geph = (geph_t *)malloc(sizeof(geph_t) * NSATGLO * 1)))
    {
        tracet(1, "rtksvrinit: malloc error\n");
        return 0;
    }
    for (i = 0; i < NSATGLO * 1; i++)
        svr->nav.geph[i] = geph0;
    svr->nav.ng = NSATGLO * 1;
#endif
#ifdef ENASBS
    seph_t seph0 = {0};
    if (!(svr->nav.seph = (seph_t *)malloc(sizeof(seph_t) * NSATSBS * 1)))
    {
        tracet(1, "rtksvrinit: malloc error\n");
        return 0;
    }
    for (i = 0; i < NSATSBS * 1; i++)
        svr->nav.seph[i] = seph0;
    svr->nav.ns = NSATSBS * 1;
#endif

    for (i = 0; i < 2; i++)
        for (j = 0; j < MAXOBSBUF; j++)
        {
            if (!(svr->obs[i][j].data = (obsd_t *)malloc(sizeof(obsd_t) * MAXOBS)))
            {
                tracet(1, "rtksvrinit: malloc error\n");
                return 0;
            }
        }
#endif
    for (i = 0; i < 2; i++)
    {
        // memset(svr->raw +i,0,sizeof(raw_t ));
        memset(svr->rtcm + i, 0, sizeof(rtcm_t));
    }
//todo    for (i = 0; i < MAXSTRRTK; i++)
//        strinit(svr->stream + i);

    // for (i=0;i<2;i++) *svr->cmds_periodic[i]='\0';x
    //*svr->cmd_reset='\0';
    // for (i=0;i<3;i++) svr->ftime[i]=time0;
    // for (i=0;i<2;i++) svr->files[i][0]='\0';
    // svr->moni=NULL;
    svr->bl_reset = 10.0;
//todo    initlock(&svr->lock);

    return 1;
}
/* free rtk server -------------------------------------------------------------
 * free rtk server
 * args   : rtksvr_t *svr    IO rtk server
 * return : none
 *-----------------------------------------------------------------------------*/
extern void rtksvrfree(rtksvr_t *svr)
{

#ifdef SVR_STATIC
    memset(svr_obsd, 0, sizeof(obsd_t) * 2 * MAXOBSBUF); // 尚未测试 2022年6月16日

// 如果是动态申请的
#else
    int i, j;
    free(svr->nav.eph);
    svr->nav.eph = NULL;
#ifdef ENAGLO
    free(svr->nav.geph);
    svr->nav.geph = NULL;
#endif
#ifdef ENASBS
    free(svr->nav.seph);
    svr->nav.seph = NULL;
#endif
    for (i = 0; i < 3; i++)
        for (j = 0; j < MAXOBSBUF; j++)
        {
            free(svr->obs[i][j].data);
        }
#endif
    rtkfree(&svr->rtk);
}
/* lock/unlock rtk server ------------------------------------------------------
 * lock/unlock rtk server
 * args   : rtksvr_t *svr    IO rtk server
 * return : status (1:ok 0:error)
 *-----------------------------------------------------------------------------*/
/* start rtk server mini ------------------------------------------------------------
 * start rtk server thread
 * args   : rtksvr_t *svr    IO rtk server
 *          int     cycle    I  server cycle (ms)
 *          int     buffsize I  input buffer size (bytes)
 *          int     *format  I  input stream formats (STRFMT_???)
 *                              format[0]=input stream rover
 *                              format[1]=input stream base station
 *          int     nmeacycle I nmea request cycle (ms) (0:no request)
 *          int     nmeareq  I  nmea request type
 *                              (0:no,1:base pos,2:single sol,3:reset and single)
 *          double *nmeapos  I  transmitted nmea position (ecef) (m)
 *          prcopt_t *prcopt I  rtk processing options
 *          solopt_t *solopt I  solution options
 *                              solopt[0]=solution 1 options
 *                              solopt[1]=solution 2 options
 * return : status (1:ok 0:error)
 * 开始RTK服务线程
 *-----------------------------------------------------------------------------*/
extern int rtksvrstart_mini(rtksvr_t *svr, int cycle, int buffsize, int *formats,
                            int nmeacycle, int nmeareq, const double *nmeapos,
                            prcopt_t *prcopt, solopt_t *solopt)
{
    gtime_t time, time0 = {0};
    int i, j;

    /*  STEP-1 初始化并连接结构体指针和变量   */

    svr->cycle = cycle > 1 ? cycle : 1;
    svr->nmeacycle = nmeacycle > 1000 ? nmeacycle : 1000;
    svr->nmeareq = nmeareq;
    for (i = 0; i < 3; i++)
        svr->nmeapos[i] = nmeapos[i];
    svr->buffsize = buffsize > 4096 ? buffsize : 4096;
    for (i = 0; i < 2; i++)
        svr->format[i] = formats[i];
    svr->navsel = 0; /* navigation message select (0:rover,1:base,3:all) */
//    svr->nsbs = 0;   /* number of sbas message */
    svr->nsol = 0;   /* number of solution buffer */
    svr->prcout = 0; /* missing observation data count */
//    重复释放
//    rtkfree(&svr->rtk);

    rtkinit(&svr->rtk, prcopt);
#ifdef STATIC
    init_rtcm_static(svr->rtcm + 0, 0); /* 初始化移动站数组 */
    init_rtcm_static(svr->rtcm + 1, 1); /* 初始化基准站数组 */
#endif
    if (prcopt->initrst)
    { /* init averaging pos by restart */
        svr->nave = 0;
        for (i = 0; i < 3; i++)
            svr->rb_ave[i] = 0.0;
    }

    /*  STEP-2 初始化数据缓存buff   */
    for (i = 0; i < 2; i++)
    { /* input/log streams */
        svr->nb[i] = 0;
//        svr->npb[i] = 0;
        if ((svr->buff[i] = (uint8_t *)rt_malloc(buffsize))==RT_NULL)
        {
            LOG_E("rtksvrstart: buff malloc error");
            return 0;
        }
        memset(&svr->nmsg,0,sizeof(svr->nmsg));
        memset(&svr->obs,0,sizeof(svr->obs));
//        for (j = 0; j < 10; j++)
//            svr->nmsg[i][j] = 0;
//        for (j = 0; j < MAXOBSBUF; j++)
//            svr->obs[i][j].n = 0;
        init_rtcm(&svr->rtcm[i]);
    }

    /* set solution options */
    for (i = 0; i < 2; i++)
    {
        svr->solopt[i] = solopt[i];
    }
    /* set base station position */
    if (prcopt->refpos != POSOPT_SINGLE)
    {
        for (i = 0; i < 6; i++)
            svr->rtk.rb[i] = i < 3 ? prcopt->rb[i] : 0.0;
    }
    /* update navigation data */
    for (i = 0; i < MAXSAT * 1; i++)
        svr->nav.eph[i].ttr = time0;
#ifdef ENAGLO
    for (i = 0; i < NSATGLO * 1; i++)
        svr->nav.geph[i].tof = time0;
#endif
#ifdef ENASBS
    for (i = 0; i < NSATSBS * 1; i++)
        svr->nav.seph[i].tof = time0;
#endif

    /*  STEP-3 打开基准站和移动站的输入流   */
    // TODO: 嵌入式平台进行输入流的初始化 或者打开输入流开关 如串口
    for (i = 0; i < 8; i++)
    {
        /* set initial time for rtcm and raw */
        if (i < 2)
        {
            time = utc2gpst(timeget());
            svr->rtcm[i].time = time;
        }
    }
    /*  STEP-4 打开处理线程   */
    // TODO: 嵌入式系统打开对应线程, 创建对应线程
    // if (!(svr->thread = CreateThread(NULL, 0, rtksvrthread_mini, svr, 0, NULL)))
    // {
    //     for (i = 0; i < MAXSTRRTK; i++)
    //         strclose(svr->stream + i);
    //     return 0;
    // }
    return 1;
}
/* stop rtk server -------------------------------------------------------------
 * start rtk server thread
 * args   : rtksvr_t *svr    IO rtk server
 *          char    **cmds   I  input stream stop commands
 *                              cmds[0]=input stream rover (NULL: no command)
 *                              cmds[1]=input stream base  (NULL: no command)
 *                              cmds[2]=input stream ephem (NULL: no command)
 * return : none
 *-----------------------------------------------------------------------------*/
extern void rtksvrstop(rtksvr_t *svr, char **cmds)
{
    int i;

    tracet(3, "rtksvrstop:\n");

    /* write stop commands to input streams */
//todo    rtksvrlock(svr);
//    for (i = 0; i < 3; i++)
//    {
//        if (cmds[i])
//            strsendcmd(svr->stream + i, cmds[i]);
//    }
    rtksvrunlock(svr);

    /* stop rtk server */
    svr->state = 0;

    /* free rtk server thread */
    // TODO: 嵌入式系统支持 等待线程结束
    // #ifdef WIN32
    //     WaitForSingleObject(svr->thread, 10000);
    //     CloseHandle(svr->thread);
    // #else
    //     pthread_join(svr->thread, NULL);
    // #endif
}
/* open output/log stream ------------------------------------------------------
 * open output/log stream
 * args   : rtksvr_t *svr    IO rtk server
 *          int     index    I  output/log stream index
 *                              (3:solution 1,4:solution 2,5:log rover,
 *                               6:log base station,7:log correction)
 *          int     str      I  output/log stream types (STR_???)
 *          char    *path    I  output/log stream path
 *          solopt_t *solopt I  solution options
 * return : status (1:ok 0:error)
 *-----------------------------------------------------------------------------*/
extern int rtksvropenstr(rtksvr_t *svr, int index, int str, const char *path,
                         const solopt_t *solopt)
{
    // TODO: 添加SD支持
    // tracet(3, "rtksvropenstr: index=%d str=%d path=%s\n", index, str, path);

    // if (index < 3 || index > 7 || !svr->state)
    //     return 0;

    // rtksvrlock(svr);

    // if (svr->stream[index].state > 0)
    // {
    //     rtksvrunlock(svr);
    //     return 0;
    // }
    // if (!stropen(svr->stream + index, str, STR_MODE_W, path))
    // {
    //     tracet(2, "stream open error: index=%d\n", index);
    //     rtksvrunlock(svr);
    //     return 0;
    // }
    // if (index <= 4)
    // {
    //     svr->solopt[index - 3] = *solopt;

    //     /* write solution header to solution stream */
    //     writesolhead(svr->stream + index, svr->solopt + index - 3);
    // }
    // rtksvrunlock(svr);
    return 1;
}
/* close output/log stream -----------------------------------------------------
 * close output/log stream
 * args   : rtksvr_t *svr    IO rtk server
 *          int     index    I  output/log stream index
 *                              (3:solution 1,4:solution 2,5:log rover,
 *                               6:log base station,7:log correction)
 * return : none
 *-----------------------------------------------------------------------------*/
extern void rtksvrclosestr(rtksvr_t *svr, int index)
{
    // TODO: 添加SD支持
    //  tracet(3, "rtksvrclosestr: index=%d\n", index);

    // if (index < 3 || index > 7 || !svr->state)
    //     return;

    // rtksvrlock(svr);

    // strclose(svr->stream + index);

    // rtksvrunlock(svr);
}
/* get observation data status -------------------------------------------------
 * get current observation data status
 * args   : rtksvr_t *svr    I  rtk server
 *          int     rcv      I  receiver (0:rover,1:base,2:ephem)
 *          gtime_t *time    O  time of observation data
 *          int     *sat     O  satellite prn numbers
 *          double  *az      O  satellite azimuth angles (rad)
 *          double  *el      O  satellite elevation angles (rad)
 *          int     **snr    O  satellite snr for each freq (dBHz)
 *                              snr[i][j] = sat i freq j snr
 *          int     *vsat    O  valid satellite flag
 * return : number of satellites
 *-----------------------------------------------------------------------------*/
extern int rtksvrostat(rtksvr_t *svr, int rcv, gtime_t *time, int *sat,
                       double *az, double *el, int **snr, int *vsat)
{
    int i, j, ns;

    tracet(4, "rtksvrostat: rcv=%d\n", rcv);

    if (!svr->state)
        return 0;
    rtksvrlock(svr);
    ns = svr->obs[rcv][0].n;
    if (ns > 0)
    {
        *time = svr->obs[rcv][0].data[0].time;
    }
    for (i = 0; i < ns; i++)
    {
        sat[i] = svr->obs[rcv][0].data[i].sat;
        az[i] = svr->rtk.ssat[sat[i] - 1].azel[0];
        el[i] = svr->rtk.ssat[sat[i] - 1].azel[1];
        for (j = 0; j < NFREQ; j++)
        {
            snr[i][j] = (int)(svr->obs[rcv][0].data[i].SNR[j] * SNR_UNIT + 0.5);
        }
        if (svr->rtk.sol.stat == SOLQ_NONE || svr->rtk.sol.stat == SOLQ_SINGLE)
        {
            vsat[i] = svr->rtk.ssat[sat[i] - 1].vs;
        }
        else
        {
            vsat[i] = svr->rtk.ssat[sat[i] - 1].vsat[0];
        }
    }
    rtksvrunlock(svr);
    return ns;
}
/* get stream status -----------------------------------------------------------
 * get current stream status
 * args   : rtksvr_t *svr    I  rtk server
 *          int     *sstat   O  status of streams
 *          char    *msg     O  status messages
 * return : none
 *-----------------------------------------------------------------------------*/
extern void rtksvrsstat(rtksvr_t *svr, int *sstat, char *msg)
{
    int i;
    char s[MAXSTRMSG], *p = msg;

    tracet(4, "rtksvrsstat:\n");

    rtksvrlock(svr);
    for (i = 0; i < MAXSTRRTK; i++)
    {
        // TODO: 获取数据流状态 或许要添加移动站情况?
        //  sstat[i] = strstat(svr->stream + i, s);
        // strstat() 函数的返回值 return : status (-1:error,0:close,1:wait,2:connect,3:active)
        if (*s)
            p += sprintf(p, "(%d) %s ", i + 1, s);
    }
    rtksvrunlock(svr);
}
/* mark current position -------------------------------------------------------
 * open output/log stream
 * args   : rtksvr_t *svr    IO rtk server
 *          char    *name    I  marker name
 *          char    *comment I  comment string
 * return : status (1:ok 0:error)
 *-----------------------------------------------------------------------------*/
extern int rtksvrmark(rtksvr_t *svr, const char *name, const char *comment)
{
    char buff[MAXSOLMSG + 1], tstr[32], *p, *q;
    double tow, pos[3];
    int i, sum, week;

    tracet(4, "rtksvrmark:name=%s comment=%s\n", name, comment);

    if (!svr->state)
        return 0;

    rtksvrlock(svr);

    time2str(svr->rtk.sol.time, tstr, 3);
    tow = time2gpst(svr->rtk.sol.time, &week);
    ecef2pos(svr->rtk.sol.rr, pos);

    for (i = 0; i < 2; i++)
    {
        p = buff;
        if (svr->solopt[i].posf == SOLF_STAT)
        {
            p += sprintf(p, "$MARK,%d,%.3f,%d,%.4f,%.4f,%.4f,%s,%s\r\n", week, tow,
                         svr->rtk.sol.stat, svr->rtk.sol.rr[0], svr->rtk.sol.rr[1],
                         svr->rtk.sol.rr[2], name, comment);
        }
        else if (svr->solopt[i].posf == SOLF_NMEA)
        {
            p += sprintf(p, "$GPTXT,01,01,02,MARK:%s,%s,%.9f,%.9f,%.4f,%d,%s",
                         name, tstr, pos[0] * R2D, pos[1] * R2D, pos[2], svr->rtk.sol.stat,
                         comment);
            for (q = (char *)buff + 1, sum = 0; *q; q++)
                sum ^= *q; /* check-sum */
            p += sprintf(p, "*%02X\r\n", sum);
        }
        else
        {
            p += sprintf(p, "%s MARK: %s,%s,%.9f,%.9f,%.4f,%d,%s\r\n", COMMENTH,
                         name, tstr, pos[0] * R2D, pos[1] * R2D, pos[2], svr->rtk.sol.stat,
                         comment);
        }
        // TODO: 数据流支持
        // strwrite(svr->stream + i + 3, (uint8_t *)buff, (int)(p - buff));
//        saveoutbuf(svr, (uint8_t *)buff, (int)(p - buff), i);
    }
    rtksvrunlock(svr);
    return 1;
}
/* 输出由RTCM电文转换的观测值内容. ----------------------------------------------
 * 将RTCM电文转换成为obs,在串口中输出rinex格式的观测值内容
 * args   : obsd_t *obs  I   当前时刻的观测值结构体
 *          int   nobs   I   当前时刻观测值的个数
 *          char  mode   I   输出的格式(0:L1 1:L1+L2 2:L1+L5)
 * return :
 * notes  :
 *-----------------------------------------------------------------------------*/
extern int dbg_show_obs(const obsd_t *obs, int nobs, char mode, char *buffer)
{
    char str[128], id[16];
    int i, n = 0;

    time2str(obs[i].time, str, 3);
//    SEGGER_RTT_printf(0, RTT_CTRL_TEXT_BRIGHT_WHITE "%s\n", str);
    LOG_D("%s\n",str);
     n = sprintf(buffer, "%s\r\n", str);
    for (i = 0; i < nobs; i++)
    {
        satno2id(obs[i].sat, id);
        switch (mode)
        {
        case 0:
            n += sprintf(str, "%2d %3s %13.3f %13.3f %4.3f %3.3f\n", i, id, obs[i].L[0], obs[i].P[0], obs[i].D[0], obs[i].SNR[0] * SNR_UNIT);
            LOG_D("%s", str);
            // n += sprintf(buffer + n, "%3s %13.3f %13.3f %4.3f %3.3f\n", id, obs[i].L[0], obs[i].P[0], obs[i].D[0], obs[i].SNR[0] * SNR_UNIT);
            break;
        // case 1:
        //     n += sprintf(buffer + n, "%3s %13.3f %13.3f %4.3f %3.3f %13.3f %13.3f %4.3f %3.3f\n",
        //                  id, obs[i].L[0], obs[i].P[0], obs[i].D[0], obs[i].SNR[0] * SNR_UNIT,
        //                  obs[i].L[1], obs[i].P[1], obs[i].D[1], obs[i].SNR[1] * SNR_UNIT);
        //     break;
        // case 2:
        //     n += sprintf(buffer + n, "%3s %13.3f %13.3f %4.3f %3.3f %13.3f %13.3f %4.3f %3.3f\n",
        //                  id, obs[i].L[0], obs[i].P[0], obs[i].D[0], obs[i].SNR[0] * SNR_UNIT,
        //                  obs[i].L[2], obs[i].P[2], obs[i].D[2], obs[i].SNR[2] * SNR_UNIT);
        //     break;
        default:
            break;
        }
    }
    return n;
}