#include "rtcm.h"
#define SQR(x)   ((x)*(x))

#define MU_GPS   3.9860050E14     /* gravitational constant         ref [1] */
#define MU_GAL   3.986004418E14   /* earth gravitational constant   ref [7] */
#define MU_CMP   3.986004418E14   /* earth gravitational constant   ref [9] */
#define OMGE_GAL 7.2921151467E-5  /* earth angular velocity (rad/s) ref [7] */
#define OMGE_CMP 7.292115E-5      /* earth angular velocity (rad/s) ref [9] */

#define SIN_5 -0.0871557427476582 /* sin(-5.0 deg) */
#define COS_5  0.9961946980917456 /* cos(-5.0 deg) */

#define TSTEP    60.0             /* integration step glonass ephemeris (s) */
#define RTOL_KEPLER 1E-13         /* relative tolerance for Kepler equation */

#define DEFURASSR 0.15            /* default accuracy of ssr corr (m) */
#define MAXECORSSR 10.0           /* max orbit correction of ssr (m) */
#define MAXCCORSSR (1E-6*CLIGHT)  /* max clock correction of ssr (m) */
#define MAXAGESSR 90.0            /* max age of ssr orbit and clock (s) */
#define MAXAGESSR_HRCLK 10.0      /* max age of ssr high-rate clock (s) */
#define STD_BRDCCLK 30.0          /* error of broadcast clock (m) */
#define STD_GAL_NAPA 500.0        /* error of galileo ephemeris for NAPA (m) */

#define MAX_ITER_KEPLER 30        /* max number of iteration of Kepler */
/* constants/macros ----------------------------------------------------------*/

#define SQR(x)      ((x)*(x))
#define MAX(x,y)    ((x)>=(y)?(x):(y))

#define NX          (4+5)       /* # of estimated parameters */
#define MAXITR      10          /* max number of iteration for point pos */
#define ERR_ION     5.0         /* ionospheric delay Std (m) */
#define ERR_TROP    3.0         /* tropspheric delay Std (m) */
#define ERR_SAAS    0.3         /* Saastamoinen model error Std (m) */
#define ERR_BRDCI   0.5         /* broadcast ionosphere model error factor */
#define ERR_CBIAS   0.3         /* code bias error Std (m) */
#define REL_HUMI    0.7         /* relative humidity for Saastamoinen model */
#define MIN_EL      (5.0*D2R)   /* min elevation for measurement error (rad) */
# define MAX_GDOP   30          /* max gdop for valid solution  */
#define MIN_HGT     -1000.0     /* min user height (m) */
#define VAR_NOTEC   SQR(30.0)   /* variance of no tec */
/* constants -----------------------------------------------------------------*/

#define VER_RTKLIB  "demo5"             /* library version */
#define PATCH_LEVEL "b34i"               /* patch level */
#define COPYRIGHT_RTKLIB \
            "Copyright (C) 2007-2020 T.Takasu\nAll rights reserved."

#define PI          3.1415926535897932  /* pi */
#define D2R         (PI/180.0)          /* deg to rad */
#define R2D         (180.0/PI)          /* rad to deg */
#define CLIGHT      299792458.0         /* speed of light (m/s) */
#define SC2RAD      3.1415926535898     /* semi-circle to radian (IS-GPS) */
#define AU          149597870691.0      /* 1 AU (m) */
#define AS2R        (D2R/3600.0)        /* arc sec to radian */

#define OMGE        7.2921151467E-5     /* earth angular velocity (IS-GPS) (rad/s) */

#define RE_WGS84    6378137.0           /* earth semimajor axis (WGS84) (m) */
#define FE_WGS84    (1.0/298.257223563) /* earth flattening (WGS84) */

#define HION        350000.0            /* ionosphere height (m) */

#define MAXFREQ     6                   /* max NFREQ */
#define FREQL1      1.57542E9           /* L1/E1  frequency (Hz) */
#define FREQL2      1.22760E9           /* L2     frequency (Hz) */
#define FREQE5b     1.20714E9           /* E5b    frequency (Hz) */
#define FREQL5      1.17645E9           /* L5/E5a/B2a frequency (Hz) */
#define FREQL6      1.27875E9           /* E6/L6 frequency (Hz) */
#define FREQE5ab    1.191795E9          /* E5a+b  frequency (Hz) */
#define FREQs       2.492028E9           /* S      frequency (Hz) */
#define FREQ1_CMP   1.561098E9          /* BDS B1I     frequency (Hz) */
#define FREQ2_CMP   1.20714E9           /* BDS B2I/B2b frequency (Hz) */
#define FREQ3_CMP   1.26852E9           /* BDS B3      frequency (Hz) */
#define SNR_UNIT    0.001               /* SNR unit (dBHz) */
/* select ephemeris --------------------------------------------------------*/
int eph_sel[] = { /* GPS,GLO,GAL,QZS,BDS,IRN,SBS */
    0,0,0,0,0,0,0
};
extern double eph2clk(gtime_t time, const eph_t* eph)
{
    double t, ts;
    int i;

    trace(4, "eph2clk : time=%s sat=%2d\n", time_str(time, 3), eph->sat);

    t = ts = timediff(time, eph->toc);

    for (i = 0; i < 2; i++) {
        t = ts - (eph->f0 + eph->f1 * t + eph->f2 * t * t);
    }
    trace(4, "ephclk: t=%.12f ts=%.12f dts=%.12f f0=%.12f f1=%.9f f2=%.9f\n", t, ts,
        eph->f0 + eph->f1 * t + eph->f2 * t * t, eph->f0, eph->f1, eph->f2);

    return eph->f0 + eph->f1 * t + eph->f2 * t * t;
}
/* variance by ura ephemeris -------------------------------------------------*/
static double var_uraeph(int sys, int ura)
{
    const double ura_value[] = {
        2.4,3.4,4.85,6.85,9.65,13.65,24.0,48.0,96.0,192.0,384.0,768.0,1536.0,
        3072.0,6144.0
    };
    if (sys == SYS_GAL) { /* galileo sisa (ref [7] 5.1.11) */
        if (ura <= 49) return SQR(ura * 0.01);
        if (ura <= 74) return SQR(0.5 + (ura - 50) * 0.02);
        if (ura <= 99) return SQR(1.0 + (ura - 75) * 0.04);
        if (ura <= 125) return SQR(2.0 + (ura - 100) * 0.16);
        return SQR(STD_GAL_NAPA);
    }
    else { /* gps ura (ref [1] 20.3.3.3.1.1) */
        return ura < 0 || 14 < ura ? SQR(6144.0) : SQR(ura_value[ura]);
    }
}
/* broadcast ephemeris to satellite position and clock bias --------------------
* compute satellite position and clock bias with broadcast ephemeris (gps,
* galileo, qzss)
* args   : gtime_t time     I   time (gpst)
*          eph_t *eph       I   broadcast ephemeris
*          double *rs       O   satellite position (ecef) {x,y,z} (m)
*          double *dts      O   satellite clock bias (s)
*          double *var      O   satellite position and clock variance (m^2)
* return : none
* notes  : see ref [1],[7],[8]
*          satellite clock includes relativity correction without code bias
*          (tgd or bgd)
*-----------------------------------------------------------------------------*/
extern void eph2pos(gtime_t time, const eph_t* eph, double* rs, double* dts,
    double* var)
{
    double tk, M, E, Ek, sinE, cosE, u, r, i, O, sin2u, cos2u, x, y, sinO, cosO, cosi, mu, omge;
    double xg, yg, zg, sino, coso;
    int n, sys, prn;

    trace(4, "eph2pos : time=%s sat=%2d\n", time_str(time, 3), eph->sat);

    if (eph->A <= 0.0) {
        rs[0] = rs[1] = rs[2] = *dts = *var = 0.0;
        return;
    }
    tk = timediff(time, eph->toe);

    switch ((sys = satsys(eph->sat, &prn))) {
    case SYS_GAL: mu = MU_GAL; omge = OMGE_GAL; break;
    case SYS_CMP: mu = MU_CMP; omge = OMGE_CMP; break;
    default:      mu = MU_GPS; omge = OMGE;     break;
    }
    M = eph->M0 + (sqrt(mu / (eph->A * eph->A * eph->A)) + eph->deln) * tk;

    for (n = 0, E = M, Ek = 0.0; fabs(E - Ek) > RTOL_KEPLER && n < MAX_ITER_KEPLER; n++) {
        Ek = E; E -= (E - eph->e * sin(E) - M) / (1.0 - eph->e * cos(E));
    }
    sinE = sin(E); cosE = cos(E);
    u = atan2(sqrt(1.0 - eph->e * eph->e) * sinE, cosE - eph->e) + eph->omg;
    r = eph->A * (1.0 - eph->e * cosE);
    i = eph->i0 + eph->idot * tk;
    sin2u = sin(2.0 * u); cos2u = cos(2.0 * u);
    u += eph->cus * sin2u + eph->cuc * cos2u;
    r += eph->crs * sin2u + eph->crc * cos2u;
    i += eph->cis * sin2u + eph->cic * cos2u;
    x = r * cos(u); y = r * sin(u); cosi = cos(i);

    /* beidou geo satellite */
    if (sys == SYS_CMP && (prn <= 5 || prn >= 59)) { /* ref [9] table 4-1 */
        O = eph->OMG0 + eph->OMGd * tk - omge * eph->toes;
        sinO = sin(O); cosO = cos(O);
        xg = x * cosO - y * cosi * sinO;
        yg = x * sinO + y * cosi * cosO;
        zg = y * sin(i);
        sino = sin(omge * tk); coso = cos(omge * tk);
        rs[0] = xg * coso + yg * sino * COS_5 + zg * sino * SIN_5;
        rs[1] = -xg * sino + yg * coso * COS_5 + zg * coso * SIN_5;
        rs[2] = -yg * SIN_5 + zg * COS_5;
    }
    else {
        O = eph->OMG0 + (eph->OMGd - omge) * tk - omge * eph->toes;
        sinO = sin(O); cosO = cos(O);
        rs[0] = x * cosO - y * cosi * sinO;
        rs[1] = x * sinO + y * cosi * cosO;
        rs[2] = y * sin(i);
    }
    tk = timediff(time, eph->toc);
    *dts = eph->f0 + eph->f1 * tk + eph->f2 * tk * tk;
    /* relativity correction */
    *dts -= 2.0 * sqrt(mu * eph->A) * eph->e * sinE / SQR(CLIGHT);

    /* position and clock error variance */
    *var = var_uraeph(sys, eph->sva);
    trace(4, "eph2pos: sat=%d, dts=%.10f rs=%.4f %.4f %.4f var=%.3f\n", eph->sat,
        *dts, rs[0], rs[1], rs[2], *var);
}
extern int getseleph(int sys)
{
    switch (sys) {
    case SYS_GPS: return eph_sel[0];
    case SYS_GAL: return eph_sel[2];
    case SYS_CMP: return eph_sel[4];
    }
    return 0;
}
static eph_t* seleph(gtime_t time, int sat, int iode, const nav_t* nav)
{
    double t, tmax, tmin;
    int i, j = -1, sys, sel = 0;

    trace(4, "seleph  : time=%s sat=%2d iode=%d\n", time_str(time, 3), sat, iode);

    sys = satsys(sat, NULL);
    switch (sys) {
    case SYS_GPS: tmax = MAXDTOE + 1.0; sel = eph_sel[0]; break;
    case SYS_GAL: tmax = MAXDTOE_GAL; sel = eph_sel[2]; break;
    case SYS_CMP: tmax = MAXDTOE_CMP + 1.0; sel = eph_sel[4]; break;
    default: tmax = MAXDTOE + 1.0; break;
    }
    tmin = tmax + 1.0;

    for (i = 0; i < nav->n; i++) {
        if (nav->eph[i].sat != sat) continue;
        if (iode >= 0 && nav->eph[i].iode != iode) continue;
        if (sys == SYS_GAL) {
            sel = getseleph(SYS_GAL);
            /* this code is from 2.4.3 b34 but does not seem to be fully supported,
               so for now I have dropped back to the b33 code */
               /* if (sel==0&&!(nav->eph[i].code&(1<<9))) continue; */ /* I/NAV */
               /*if (sel==1&&!(nav->eph[i].code&(1<<8))) continue; */ /* F/NAV */
            if (sel == 1 && !(nav->eph[i].code & (1 << 9))) continue; /* I/NAV */
            if (sel == 2 && !(nav->eph[i].code & (1 << 8))) continue; /* F/NAV */
            if (timediff(nav->eph[i].toe, time) >= 0.0) continue; /* AOD<=0 */
        }
        if ((t = fabs(timediff(nav->eph[i].toe, time))) > tmax) continue;
        if (iode >= 0) return nav->eph + i;
        if (t <= tmin) { j = i; tmin = t; } /* toe closest to time */
    }
    if (iode >= 0 || j < 0) {
        trace(2, "no broadcast ephemeris: %s sat=%2d iode=%3d\n", time_str(time, 0),
            sat, iode);
        return NULL;
    }
    trace(4, "seleph: sat=%d dt=%.0f\n", sat, tmin);
    return nav->eph + j;
}
static int ephclk(gtime_t time, gtime_t teph, int sat, const nav_t* nav,
    double* dts)
{
    eph_t* eph;
    geph_t* geph;
    seph_t* seph;
    int sys;
    sys = satsys(sat, NULL);
    if (sys == SYS_GPS || sys == SYS_GAL ||  sys == SYS_CMP ) {
        if (!(eph = seleph(teph, sat, -1, nav))) return 0;
        *dts = eph2clk(time, eph);
    }
    else return 0;
    return 1;
}
static int ephpos(gtime_t time, gtime_t teph, int sat, const nav_t* nav,
    int iode, double* rs, double* dts, double* var, int* svh)
{
    eph_t* eph;
    geph_t* geph;
    seph_t* seph;
    double rst[3], dtst[1], tt = 1E-3;
    int i, sys;

    trace(4, "ephpos  : time=%s sat=%2d iode=%d\n", time_str(time, 3), sat, iode);

    sys = satsys(sat, NULL);

    *svh = -1;

    if (sys == SYS_GPS || sys == SYS_GAL ||  sys == SYS_CMP) {
        if (!(eph = seleph(teph, sat, iode, nav))) return 0;
        eph2pos(time, eph, rs, dts, var);
        time = timeadd(time, tt);
        eph2pos(time, eph, rst, dtst, var);
        *svh = eph->svh;
    }
    else return 0;

    /* satellite velocity and clock drift by differential approx */
    for (i = 0; i < 3; i++) rs[i + 3] = (rst[i] - rs[i]) / tt;
    dts[1] = (dtst[0] - dts[0]) / tt;

    return 1;
}
extern int satpos(gtime_t time, gtime_t teph, int sat, int ephopt,
    const nav_t* nav, double* rs, double* dts, double* var,
    int* svh)
{
    *svh = 0;
    switch (ephopt) {
    case EPHOPT_BRDC: return ephpos(time, teph, sat, nav, -1, rs, dts, var, svh);
    }
    *svh = -1;
    return 0;
}
/* data index (i:lat,j:lon,k:hgt) --------------------------------------------*/
static int dataindex(int i, int j, int k, const int* ndata)
{
    if (i < 0 || ndata[0] <= i || j < 0 || ndata[1] <= j || k < 0 || ndata[2] <= k) return -1;
    return i + ndata[0] * (j + ndata[1] * k);
}
/* interpolate tec grid data -------------------------------------------------*/
static int interptec(const tec_t* tec, int k, const double* posp, double* value,
    double* rms)
{
    double dlat, dlon, a, b, d[4] = { 0 }, r[4] = { 0 };
    int i, j, n, index;

    trace(3, "interptec: k=%d posp=%.2f %.2f\n", k, posp[0] * R2D, posp[1] * R2D);
    *value = *rms = 0.0;

    if (tec->lats[2] == 0.0 || tec->lons[2] == 0.0) return 0;

    dlat = posp[0] * R2D - tec->lats[0];
    dlon = posp[1] * R2D - tec->lons[0];
    if (tec->lons[2] > 0.0) dlon -= floor(dlon / 360) * 360.0; /*  0<=dlon<360 */
    else                  dlon += floor(-dlon / 360) * 360.0; /* -360<dlon<=0 */

    a = dlat / tec->lats[2];
    b = dlon / tec->lons[2];
    i = (int)floor(a); a -= i;
    j = (int)floor(b); b -= j;

    /* get gridded tec data */
    for (n = 0; n < 4; n++) {
        if ((index = dataindex(i + (n % 2), j + (n < 2 ? 0 : 1), k, tec->ndata)) < 0) continue;
        d[n] = tec->data[index];
        r[n] = tec->rms[index];
    }
    if (d[0] > 0.0 && d[1] > 0.0 && d[2] > 0.0 && d[3] > 0.0) {

        /* bilinear interpolation (inside of grid) */
        *value = (1.0 - a) * (1.0 - b) * d[0] + a * (1.0 - b) * d[1] + (1.0 - a) * b * d[2] + a * b * d[3];
        *rms = (1.0 - a) * (1.0 - b) * r[0] + a * (1.0 - b) * r[1] + (1.0 - a) * b * r[2] + a * b * r[3];
    }
    /* nearest-neighbour extrapolation (outside of grid) */
    else if (a <= 0.5 && b <= 0.5 && d[0] > 0.0) { *value = d[0]; *rms = r[0]; }
    else if (a > 0.5 && b <= 0.5 && d[1] > 0.0) { *value = d[1]; *rms = r[1]; }
    else if (a <= 0.5 && b > 0.5 && d[2] > 0.0) { *value = d[2]; *rms = r[2]; }
    else if (a > 0.5 && b > 0.5 && d[3] > 0.0) { *value = d[3]; *rms = r[3]; }
    else {
        i = 0;
        for (n = 0; n < 4; n++) if (d[n] > 0.0) { i++; *value += d[n]; *rms += r[n]; }
        if (i == 0) return 0;
        *value /= i; *rms /= i;
    }
    return 1;
}
/* ionosphere delay by tec grid data -----------------------------------------*/
static int iondelay(gtime_t time, const tec_t* tec, const double* pos,
    const double* azel, int opt, double* delay, double* var)
{
    const double fact = 40.30E16 / FREQL1 / FREQL1; /* tecu->L1 iono (m) */
    double fs, posp[3] = { 0 }, vtec, rms, hion, rp;
    int i;

    trace(3, "iondelay: time=%s pos=%.1f %.1f azel=%.1f %.1f\n", time_str(time, 0),
        pos[0] * R2D, pos[1] * R2D, azel[0] * R2D, azel[1] * R2D);

    *delay = *var = 0.0;

    for (i = 0; i < tec->ndata[2]; i++) { /* for a layer */

        hion = tec->hgts[0] + tec->hgts[2] * i;

        /* ionospheric pierce point position */
        fs = ionppp(pos, azel, tec->rb, hion, posp);

        if (opt & 2) {
            /* modified single layer mapping function (M-SLM) ref [2] */
            rp = tec->rb / (tec->rb + hion) * sin(0.9782 * (PI / 2.0 - azel[1]));
            fs = 1.0 / sqrt(1.0 - rp * rp);
        }
        if (opt & 1) {
            /* earth rotation correction (sun-fixed coordinate) */
            posp[1] += 2.0 * PI * timediff(time, tec->time) / 86400.0;
        }
        /* interpolate tec grid data */
        if (!interptec(tec, i, posp, &vtec, &rms)) return 0;

        *delay += fact * fs * vtec;
        *var += fact * fact * fs * fs * rms * rms;
    }
    trace(4, "iondelay: delay=%7.2f std=%6.2f\n", *delay, sqrt(*var));

    return 1;
}
/* ionosphere model by tec grid data -------------------------------------------
* compute ionospheric delay by tec grid data
* args   : gtime_t time     I   time (gpst)
*          nav_t  *nav      I   navigation data
*          double *pos      I   receiver position {lat,lon,h} (rad,m)
*          double *azel     I   azimuth/elevation angle {az,el} (rad)
*          int    opt       I   model option
*                                bit0: 0:earth-fixed,1:sun-fixed
*                                bit1: 0:single-layer,1:modified single-layer
*          double *delay    O   ionospheric delay (L1) (m)
*          double *var      O   ionospheric dealy (L1) variance (m^2)
* return : status (1:ok,0:error)
* notes  : before calling the function, read tec grid data by calling readtec()
*          return ok with delay=0 and var=VAR_NOTEC if el<MIN_EL or h<MIN_HGT
*-----------------------------------------------------------------------------*/
extern int iontec(gtime_t time, const nav_t* nav, const double* pos,
    const double* azel, int opt, double* delay, double* var)
{
    double dels[2], vars[2], a, tt;
    int i, stat[2];

    trace(3, "iontec  : time=%s pos=%.1f %.1f azel=%.1f %.1f\n", time_str(time, 0),
        pos[0] * R2D, pos[1] * R2D, azel[0] * R2D, azel[1] * R2D);

    if (azel[1] < MIN_EL || pos[2] < MIN_HGT) {
        *delay = 0.0;
        *var = VAR_NOTEC;
        return 1;
    }
    for (i = 0; i < nav->nt; i++) {
        if (timediff(nav->tec[i].time, time) > 0.0) break;
    }
    if (i == 0 || i >= nav->nt) {
        trace(2, "%s: tec grid out of period\n", time_str(time, 0));
        return 0;
    }
    if ((tt = timediff(nav->tec[i].time, nav->tec[i - 1].time)) == 0.0) {
        trace(2, "tec grid time interval error\n");
        return 0;
    }
    /* ionospheric delay by tec grid data */
    stat[0] = iondelay(time, nav->tec + i - 1, pos, azel, opt, dels, vars);
    stat[1] = iondelay(time, nav->tec + i, pos, azel, opt, dels + 1, vars + 1);

    if (!stat[0] && !stat[1]) {
        trace(2, "%s: tec grid out of area pos=%6.2f %7.2f azel=%6.1f %5.1f\n",
            time_str(time, 0), pos[0] * R2D, pos[1] * R2D, azel[0] * R2D, azel[1] * R2D);
        return 0;
    }
    if (stat[0] && stat[1]) { /* linear interpolation by time */
        a = timediff(time, nav->tec[i - 1].time) / tt;
        *delay = dels[0] * (1.0 - a) + dels[1] * a;
        *var = vars[0] * (1.0 - a) + vars[1] * a;
    }
    else if (stat[0]) { /* nearest-neighbour extrapolation by time */
        *delay = dels[0];
        *var = vars[0];
    }
    else {
        *delay = dels[1];
        *var = vars[1];
    }
    trace(3, "iontec  : delay=%5.2f std=%5.2f\n", *delay, sqrt(*var));
    return 1;
}
/* ionospheric correction ------------------------------------------------------
* compute ionospheric correction
* args   : gtime_t time     I   time
*          nav_t  *nav      I   navigation data
*          int    sat       I   satellite number
*          double *pos      I   receiver position {lat,lon,h} (rad|m)
*          double *azel     I   azimuth/elevation angle {az,el} (rad)
*          int    ionoopt   I   ionospheric correction option (IONOOPT_???)
*          double *ion      O   ionospheric delay (L1) (m)
*          double *var      O   ionospheric delay (L1) variance (m^2)
* return : status(1:ok,0:error)
*-----------------------------------------------------------------------------*/
extern int ionocorr(gtime_t time, const nav_t* nav, int sat, const double* pos,
    const double* azel, int ionoopt, double* ion, double* var)
{
    int err = 0;
    /* IONEX TEC model */
    if (ionoopt == IONOOPT_TEC) {
        if (iontec(time, nav, pos, azel, 1, ion, var)) return 1;
        err = 1;
    }
    /* GPS broadcast ionosphere model */
    if (ionoopt == IONOOPT_BRDC || err == 1) {
        *ion = ionmodel(time, nav->ion_gps, pos, azel);
        *var = SQR(*ion * ERR_BRDCI);
        return 1;
    }
    *ion = 0.0;
    *var = ionoopt == IONOOPT_OFF ? SQR(ERR_ION) : 0.0;
    return 1;
}
/* tropospheric correction -----------------------------------------------------
* compute tropospheric correction
* args   : gtime_t time     I   time
*          nav_t  *nav      I   navigation data
*          double *pos      I   receiver position {lat,lon,h} (rad|m)
*          double *azel     I   azimuth/elevation angle {az,el} (rad)
*          int    tropopt   I   tropospheric correction option (TROPOPT_???)
*          double *trp      O   tropospheric delay (m)
*          double *var      O   tropospheric delay variance (m^2)
* return : status(1:ok,0:error)
*-----------------------------------------------------------------------------*/
extern int tropcorr(gtime_t time, const nav_t* nav, const double* pos,
    const double* azel, int tropopt, double* trp, double* var)
{
    trace(4, "tropcorr: time=%s opt=%d pos=%.3f %.3f azel=%.3f %.3f\n",
        time_str(time, 3), tropopt, pos[0] * R2D, pos[1] * R2D, azel[0] * R2D,
        azel[1] * R2D);

    /* Saastamoinen model */
    if (tropopt == TROPOPT_SAAS || tropopt == TROPOPT_EST || tropopt == TROPOPT_ESTG) {
        *trp = tropmodel(time, pos, azel, REL_HUMI);
        *var = SQR(ERR_SAAS / (sin(azel[1]) + 0.1));
        return 1;
    }
    /* no correction */
    *trp = 0.0;
    *var = tropopt == TROPOPT_OFF ? SQR(ERR_TROP) : 0.0;
    return 1;
}
/* select iono-free linear combination (L1/L2 or L1/L5) ----------------------*/
extern int seliflc(int optnf, int sys)
{
    /* use L1/L5 for Galileo if L5 is enabled */
    return((optnf == 2 || sys != SYS_GAL) ? 1 : 2);
}
/* test SNR mask -------------------------------------------------------------*/
static int snrmask(const obsd_t* obs, const double* azel, const prcopt_t* opt)
{
    int f2;

    if (testsnr(0, 0, azel[1], obs->SNR[0] * SNR_UNIT, &opt->snrmask)) {
        return 0;
    }
    return 1;
}
static char* obscodes1[] = {       /* observation code strings */

    ""  ,"1C","1P","1W","1Y", "1M","1N","1S","1L","1E", /*  0- 9 */
    "1A","1B","1X","1Z","2C", "2D","2S","2L","2X","2P", /* 10-19 */
    "2W","2Y","2M","2N","5I", "5Q","5X","7I","7Q","7X", /* 20-29 */
    "6A","6B","6C","6X","6Z", "6S","6L","8L","8Q","8X", /* 30-39 */
    "2I","2Q","6I","6Q","3I", "3Q","3X","1I","1Q","5A", /* 40-49 */
    "5B","5C","9A","9B","9C", "9X","1D","5D","5P","5Z", /* 50-59 */
    "6E","7D","7P","7Z","8D", "8P","4A","4B","4X",""    /* 60-69 */
};
extern char* code2obs1(unsigned char code)
{
    if (code <= CODE_NONE || MAXCODE < code) return "";
    return obscodes1[code];
}
/* GPS obs code to frequency -------------------------------------------------*/
static int code2freq_GPS(unsigned char code, double* freq)
{
    char* obs = code2obs1(code);

    switch (obs[0]) {
    case '1': *freq = FREQL1; return 0; /* L1 */
    case '2': *freq = FREQL2; return 1; /* L2 */
    case '5': *freq = FREQL5; return 2; /* L5 */
    }
    return -1;
}
/* Galileo obs code to frequency ---------------------------------------------*/
static int code2freq_GAL(unsigned char code, double* freq)
{
    char* obs = code2obs1(code);

    switch (obs[0]) {
    case '1': *freq = FREQL1; return 0; /* E1 */
    case '7': *freq = FREQE5b; return 1; /* E5b */
    case '5': *freq = FREQL5; return 2; /* E5a */
    case '6': *freq = FREQL6; return 3; /* E6 */
    case '8': *freq = FREQE5ab; return 4; /* E5ab */
    }
    return -1;
}
/* BDS obs code to frequency -------------------------------------------------*/
static int code2freq_BDS(unsigned char code, double* freq)
{
    char* obs = code2obs1(code);

    switch (obs[0]) {
    case '1': *freq = FREQL1;     return 0; /* B1C */
    case '2': *freq = FREQ1_CMP; return 0; /* B1I */
    case '7': *freq = FREQ2_CMP; return 1; /* B2I/B2b */
    case '6': *freq = FREQ3_CMP; return 2; /* B3 */
    case '5': *freq = FREQL5;     return 3; /* B2a */
    case '8': *freq = FREQE5ab;     return 4; /* B2ab */
    }
    return -1;
}
/* system and obs code to frequency --------------------------------------------
* convert system and obs code to carrier frequency
* args   : int    sys       I   satellite system (SYS_???)
*          uint8_t code     I   obs code (CODE_???)
*          int    fcn       I   frequency channel number for GLONASS
* return : carrier frequency (Hz) (0.0: error)
*-----------------------------------------------------------------------------*/
extern double code2freq(int sys, unsigned char code, int fcn)
{
    double freq = 0.0;

    switch (sys) {
    case SYS_GPS: (void)code2freq_GPS(code, &freq); break;
    case SYS_GAL: (void)code2freq_GAL(code, &freq); break;
    case SYS_CMP: (void)code2freq_BDS(code, &freq); break;
    }
    return freq;
}
/* satellite and obs code to frequency -----------------------------------------
* convert satellite and obs code to carrier frequency
* args   : int    sat       I   satellite number
*          uint8_t code     I   obs code (CODE_???)
*          nav_t  *nav_t    I   navigation data for GLONASS (NULL: not used)
* return : carrier frequency (Hz) (0.0: error)
*-----------------------------------------------------------------------------*/
extern double sat2freq(int sat, unsigned char code, const nav_t* nav)
{
    int i, fcn = 0, sys, prn;
    sys = satsys(sat, &prn);
    return code2freq(sys, code, fcn);
}
/* get group delay parameter (m) ---------------------------------------------*/
static double gettgd(int sat, const nav_t* nav, int type)
{
    int i, sys = satsys(sat, NULL);

        for (i = 0; i < nav->n; i++) {
            if (nav->eph[i].sat == sat) break;
        }
        return (i >= nav->n) ? 0.0 : nav->eph[i].tgd[type] * CLIGHT;
}
/* iono-free or "pseudo iono-free" pseudorange with code bias correction -----*/
static double prange(const obsd_t* obs, const nav_t* nav, double* var)
{
    double P1, P2, gamma, b1, b2;
    int sat, sys, f2, bias_ix;

    sat = obs->sat;
    sys = satsys(sat, NULL);
    P1 = obs->P[0];
    f2 = seliflc(2, satsys(obs->sat, NULL));
    P2 = obs->P[f2];
    *var = SQR(ERR_CBIAS);
    if (sys == SYS_GPS) { /* L1 */
        b1 = gettgd(sat, nav, 0); /* TGD (m) */
        return P1 - b1;
    }
    else if (sys == SYS_GAL) { /* E1 */
        if (getseleph(SYS_GAL)) b1 = gettgd(sat, nav, 0); /* BGD_E1E5a */
        else                    b1 = gettgd(sat, nav, 1); /* BGD_E1E5b */
        return P1 - b1;
    }
    else if (sys == SYS_CMP) { /* B1I/B1Cp/B1Cd */
        if (obs->code[0] == CODE_L2I) b1 = gettgd(sat, nav, 0); /* TGD_B1I */
        else if (obs->code[0] == CODE_L1P) b1 = gettgd(sat, nav, 2); /* TGD_B1Cp */
        else b1 = gettgd(sat, nav, 2) + gettgd(sat, nav, 4); /* TGD_B1Cp+ISC_B1Cd */
        return P1 - b1;
    }
    return P1;
}
/* pseudorange measurement error variance ------------------------------------*/
static double varerr(const ssat_t* ssat, const obsd_t* obs, double el, int sys)
{
    double fact = 1.0, varr, snr_rover;

    switch (sys) {
    case SYS_GPS: fact *= EFACT_GPS; break;
    case SYS_CMP: fact *= EFACT_CMP; break;
    default:      fact *= EFACT_GPS; break;
    }
    if (el < MIN_EL) el = MIN_EL;
    /* var = R^2*(a^2 + (b^2/sin(el) + c^2*(10^(0.1*(snr_max-snr_rover)))) + (d*rcv_std)^2) */
    varr = SQR(0) + SQR(0.03) / sin(el);
    varr *= SQR(300);
    if (1 == IONOOPT_IFLC) varr *= SQR(3.0); /* iono-free */
    return SQR(fact) * varr;
}
extern void matmul0(const char* tr, int n, int k, int m,
    const double* A, const double* B, double* C)
{
    int f = (tr[0] != 'N') * 2 + (tr[1] != 'N');

    switch (f) {
    case 0: /* NN */
        for (int j = 0; j < k; j++) {
            for (int i = 0; i < n; i++) {
                double d = 0.0;
                for (int x = 0; x < m; x++) d += A[i + x * n] * B[x + j * m];
                C[i + j * n] = d;
            }
        }
        break;
    case 1: /* NT */
        for (int j = 0; j < k; j++) {
            for (int i = 0; i < n; i++) {
                double d = 0.0;
                for (int x = 0; x < m; x++) d += A[i + x * n] * B[j + x * k];
                C[i + j * n] = d;
            }
        }
        break;
    case 2: /* TN */
        for (int j = 0; j < k; j++) {
            for (int i = 0; i < n; i++) {
                double d = 0.0;
                for (int x = 0; x < m; x++) d += A[x + i * m] * B[x + j * m];
                C[i + j * n] = d;
            }
        }
        break;
    case 3: /* TT */
        for (int j = 0; j < k; j++) {
            for (int i = 0; i < n; i++) {
                double d = 0.0;
                for (int x = 0; x < m; x++) d += A[x + i * m] * B[j + x * k];
                C[i + j * n] = d;
            }
        }
        break;
    }
}
extern double dot3(const double* a, const double* b)
{
    return a[0] * b[0] + a[1] * b[1] + a[2] * b[2];
}
/* range rate residuals ------------------------------------------------------*/
static int resdop(const obsd_t* obs, int n, const double* rs, const double* dts,
    const nav_t* nav, const double* rr, const double* x,
    const double* azel, const int* vsat, double err, double* v,
    double* H)
{
    double freq, rate, pos[3], E[9], a[3], e[3], vs[3], cosel, sig;
    int i, j, nv = 0;

    ecef2pos(rr, pos); xyz2enu(pos, E);

    for (i = 0; i < n && i < MAXOBS; i++) {

        freq = sat2freq(obs[i].sat, obs[i].code[0], nav);

        if (obs[i].D[0] == 0.0 || freq == 0.0 || !vsat[i] || norm(rs + 3 + i * 6, 3) <= 0.0) {
            continue;
        }
        /* LOS (line-of-sight) vector in ECEF */
        cosel = cos(azel[1 + i * 2]);
        a[0] = sin(azel[i * 2]) * cosel;
        a[1] = cos(azel[i * 2]) * cosel;
        a[2] = sin(azel[1 + i * 2]);
        matmul0("TN", 3, 1, 3, E, a, e);

        /* satellite velocity relative to receiver in ECEF */
        for (j = 0; j < 3; j++) {
            vs[j] = rs[j + 3 + i * 6] - x[j];
        }
        /* range rate with earth rotation correction */
        rate = dot3(vs, e) + OMGE / CLIGHT * (rs[4 + i * 6] * rr[0] + rs[1 + i * 6] * x[0] -
            rs[3 + i * 6] * rr[1] - rs[i * 6] * x[1]);

        /* Std of range rate error (m/s) */
        sig = (err <= 0.0) ? 1.0 : err * CLIGHT / freq;

        /* range rate residual (m/s) */
        v[nv] = (-obs[i].D[0] * CLIGHT / freq - (rate + x[3] - CLIGHT * dts[1 + i * 2])) / sig;

        /* design matrix */
        for (j = 0; j < 4; j++) {
            H[j + nv * 4] = ((j < 3) ? -e[j] : 1.0) / sig;
        }
        nv++;
    }
    return nv;
}
/* validate solution ---------------------------------------------------------*/
static int valsol(const double* azel, const int* vsat, int n,const double* v, int nv, int nx,double minazel,double* dopp)
{
    double azels[MAXOBS * 2], dop[4], vv;
    int i, ns;
    /* Chi-square validation of residuals */
    vv = dot(v, v, nv);
    /* large GDOP check */
    for (i = ns = 0; i < n; i++) {
        if (!vsat[i]) continue;
        azels[ns * 2] = azel[i * 2];
        azels[1 + ns * 2] = azel[1 + i * 2];
        ns++;
    }
    dops(ns, azels, D2R*minazel, dop);
    if (dop[0] <= 0.0 || dop[0] > MAX_GDOP) {
        return 0;
    }
    for (i = 0; i < 4; i++) {
        dopp[i] = dop[i];
    }
    return 1;
}
/* pseudorange residuals -----------------------------------------------------*/
static int rescode(int iter, const obsd_t* obs, int n, const double* rs,
    const double* dts, const double* vare, const int* svh,
    const nav_t* nav, const double* x, const ssat_t* ssat, double* v, double* H, double* var,
    double* azel, int* vsat, double* resp, int* ns, double minazel,FILE* satpos)
{
    gtime_t time;
    double r, freq, dion = 0.0, dtrp = 0.0, vmeas, vion = 0.0, vtrp = 0.0, rr[3], pos[3], dtr = x[3], e[3], P;
    int i, j, nv = 0, sat, sys, mask[NX - 3] = { 0 };

    for (i = 0; i < 3; i++) {
        rr[i] = x[i];
    }
    ecef2pos(rr, pos);
    //逐颗计算卫星信息
    for (i = *ns = 0; i < n && i < MAXOBS; i++) {
        vsat[i] = 0; azel[i * 2] = azel[1 + i * 2] = resp[i] = 0.0;
        time = obs[i].time;
        sat = obs[i].sat;
        //判断系统
        if (!(sys = satsys(sat, NULL))) {
            continue;
        }
        /* 判断重复观测值 */
        if (i < n - 1 && i < MAXOBS - 1 && sat == obs[i + 1].sat) {
            i++;
            continue;
        }
        /* 是否排除卫星 */
        //if (satexclude(sat, vare[i], svh[i], opt)) continue;

        /* 判断、计算几何距离  判断计算卫星方位角和仰角 */  //选星算法
        if ((r = geodist(rs + i * 6, rr, e)) <= 0.0 ||
            satazel(pos, e, azel + i * 2) < D2R * minazel) {
            continue;
        }
        if (iter > 0) {
            /* test SNR mask */
            //if (!snrmask(obs + i, azel + i * 2, opt)) continue;

            /* 电离层校正  1 代表开 0 关*/
            if (!ionocorr(time, nav, sat, pos, azel + i * 2, 1, &dion, &vion)) {
                continue;
            }
            if ((freq = sat2freq(sat, obs[i].code[0], nav)) == 0.0) continue;
            /* Convert from FREQL1 to freq */
            dion *= SQR(FREQL1 / freq);
            vion *= SQR(SQR(FREQL1 / freq));
            /* 对流层校正 1代表开 0 关*/
            if (!tropcorr(time, nav, pos, azel + i * 2, 1, &dtrp, &vtrp)) {
                continue;
            }
        }
        /* 校正伪距 */
        if ((P = prange(obs + i, nav, &vmeas)) == 0.0) continue;

        /* 伪距残差 */
        v[nv] = P - (r + dtr - CLIGHT * dts[i * 2] + dion + dtrp);//x[3]应改为dtr
        printf("sat=%d: v=%.3f P=%.3f r=%.3f dtr=%.6f dts=%.6f dion=%.3f dtrp=%.3f\n",sat, v[nv], P, r, dtr, dts[i * 2], dion, dtrp);
        fprintf(satpos,"sat=%d\tv=%.3f\tP=%.3f\tr=%.3f\tdtr=%.6f\tdts=%.6f\tdion=%.3f\tdtrp=%.3f\n", sat, v[nv], P, r, dtr, dts[i * 2], dion, dtrp);
        /* design matrix */
        for (j = 0; j < NX; j++) {
            H[j + nv * NX] = j < 3 ? -e[j] : (j == 3 ? 1.0 : 0.0);
        }
        /* time system offset and receiver bias correction */
        if (sys == SYS_GLO) { v[nv] -= x[4]; H[4 + nv * NX] = 1.0; mask[1] = 1; }
        else if (sys == SYS_GAL) { v[nv] -= x[5]; H[5 + nv * NX] = 1.0; mask[2] = 1; }
        else if (sys == SYS_CMP) { v[nv] -= x[6]; H[6 + nv * NX] = 1.0; mask[3] = 1; }
        vsat[i] = 1; resp[i] = v[nv]; (*ns)++;
        /* variance of pseudorange error */
        var[nv] = vare[i] + vmeas + vion + vtrp;
        if (ssat) {
            var[nv++] += varerr(&ssat[i], &obs[i], azel[1 + i * 2], sys);
        }
        else {
            var[nv++] += varerr(NULL, &obs[i], azel[1 + i * 2], sys);
            //printf("sat=%2d azel=%5.1f %4.1f res=%7.3f sig=%5.3f\n", obs[i].sat,azel[i * 2] * R2D, azel[1 + i * 2] * R2D, resp[i], sqrt(var[nv - 1]));
            fprintf(satpos,"sat=%2d azel=%5.1f %4.1f res=%7.3f sig=%5.3f\n", obs[i].sat, azel[i * 2] * R2D, azel[1 + i * 2] * R2D, resp[i], sqrt(var[nv - 1]));
        }
    }
    /* constraint to avoid rank-deficient */
    for (i = 0; i < NX - 3; i++) {
        if (mask[i]) continue;
        v[nv] = 0.0;
        for (j = 0; j < NX; j++) H[j + nv * NX] = j == i + 3 ? 1.0 : 0.0;
        var[nv++] = 0.01;
    }
    return nv;
}
/* satellite positions and clocks ----------------------------------------------
* compute satellite positions, velocities and clocks
* args   : gtime_t teph     I   time to select ephemeris (gpst)
*          obsd_t *obs      I   observation data
*          int    n         I   number of observation data
*          nav_t  *nav      I   navigation data
*          int    ephopt    I   ephemeris option (EPHOPT_???)
*          double *rs       O   satellite positions and velocities (ecef)
*          double *dts      O   satellite clocks
*          double *var      O   sat position and clock error variances (m^2)
*          int    *svh      O   sat health flag (-1:correction not available)
* return : none
* notes  : rs [(0:2)+i*6]= obs[i] sat position {x,y,z} (m)
*          rs [(3:5)+i*6]= obs[i] sat velocity {vx,vy,vz} (m/s)
*          dts[(0:1)+i*2]= obs[i] sat clock {bias,drift} (s|s/s)
*          var[i]        = obs[i] sat position and clock error variance (m^2)
*          svh[i]        = obs[i] sat health flag
*          if no navigation data, set 0 to rs[], dts[], var[] and svh[]
*          satellite position and clock are values at signal transmission time
*          satellite position is referenced to antenna phase center
*          satellite clock does not include code bias correction (tgd or bgd)
*          any pseudorange and broadcast ephemeris are always needed to get
*          signal transmission time
*-----------------------------------------------------------------------------*/
extern void satposs(gtime_t teph, const obsd_t* obs, int n, const nav_t* nav,
    int ephopt, double* rs, double* dts, double* var, int* svh, FILE* SatposFile)
{
    gtime_t time[2 * MAXOBS] = { {0} };
    double dt, pr;
    int i, j;
    if (n > 15) {
        fprintf(SatposFile, "%d\t%d\n", teph.time, n);
    }
    fprintf(SatposFile, "satposs : teph=%s\tn=%d\tephopt=%d\n", time_str(teph, 3), n, ephopt);

    for (i = 0; i < n && i < 2 * MAXOBS; i++) {
        for (j = 0; j < 6; j++) {
            rs[j + i * 6] = 0.0;
        }
        for (j = 0; j < 2; j++) {
            dts[j + i * 2] = 0.0;
        }
        var[i] = 0.0; svh[i] = 0;

        /* search any pseudorange */
        for (j = 0, pr = 0.0; j < NFREQ; j++) if ((pr = obs[i].P[j]) != 0.0) break;
        if (j >= NFREQ) {
            trace(2, "no pseudorange %s sat=%2d\n", time_str(obs[i].time, 3), obs[i].sat);
            continue;
        }
        /* transmission time by satellite clock */
        time[i] = timeadd(obs[i].time, -pr / CLIGHT);

        /* satellite clock bias by broadcast ephemeris */
        int flag = ephclk(time[i], teph, obs[i].sat, nav, &dt);
        if (!ephclk(time[i], teph, obs[i].sat, nav, &dt)) {
            continue;
        }
        time[i] = timeadd(time[i], -dt);
        /* satellite position and clock at transmission time */
        if (!satpos(time[i], teph, obs[i].sat, ephopt, nav, rs + i * 6, dts + i * 2, var + i,
            svh + i)) {
            printf("no ephemeris %s sat=%2d\n", time_str(time[i], 3), obs[i].sat);
            continue;
        }
    }
    for (i = 0; i < n && i < 2 * MAXOBS; i++) {
        fprintf(SatposFile, "%s sat=%2d\trs=%13.3f\t%13.3f\t%13.3f\tdts=%12.3f\tvar=%7.3f\tsvh=%02X\n",
            time_str(time[i], 9), obs[i].sat, rs[i * 6], rs[1 + i * 6], rs[2 + i * 6], rs[3 + i * 6],
            dts[i * 2] * 1E9, var[i], svh[i]);
        printf("%s sat=%2d rs=%13.3 %13.3f %13.3f dts=%12.3f var=%7.3f svh=%02X\n",
             time_str(time[i], 9), obs[i].sat, rs[i * 6], rs[1 + i * 6], rs[2 + i * 6],
             dts[i * 2] * 1E9, var[i], svh[i]);
    }
}
/* estimate receiver position ------------------------------------------------*/
static int estpos(const obsd_t* obs, int n, const double* rs, const double* dts,
    const double* vare, const int* svh, const nav_t* nav, const ssat_t* ssat, sol_t* sol, double* azel,
    int* vsat, double* resp, FILE* recpos, FILE* satpos)
{
    double x[NX] = { 0 }, dx[NX], Q[NX * NX], * v, * H, * var, sig;
    double blh[3] = { 0 };
    int i, j, k, info, stat, nv, ns;
    double minazel = 5;
    v = mat(n + NX - 3, 1); H = mat(NX, n + NX - 3); var = mat(n + NX - 3, 1);

    for (i = 0; i < 3; i++) {
        x[i] = sol->rr[i];
    }
    for (i = 0; i < MAXITR; i++) {
        /* pseudorange residuals (m) */
        nv = rescode(i, obs, n, rs, dts, vare, svh, nav, x, ssat, v, H, var, azel, vsat, resp,
            &ns, minazel,satpos);
        if (nv < NX) {  //lack of valid sats
            break;
        }
        /* 权重矩阵 */
        for (j = 0; j < nv; j++) {
            sig = sqrt(var[j]);
            v[j] /= sig;
            for (k = 0; k < NX; k++) {
                H[k + j * NX] /= sig;
            }
        }
        /* 最小二乘 */
        if ((info = lsq(H, v, NX, nv, dx, Q))) {
            break;
        }
        for (j = 0; j < NX; j++) {
            x[j] += dx[j];
        }
        double ds = norm(dx, NX);
        if (norm(dx, NX) < 1E-1) {
            sol->type = 0;
            sol->time = timeadd(obs[0].time, -x[3] / CLIGHT);
            sol->dtr[0] = x[3] / CLIGHT; /* receiver clock bias (s) */
            sol->dtr[2] = x[5] / CLIGHT; /* GAL-GPS time offset (s) */
            sol->dtr[3] = x[6] / CLIGHT; /* BDS-GPS time offset (s) */
            for (j = 0; j < 6; j++) sol->rr[j] = j < 3 ? x[j] : 0.0;
            for (j = 0; j < 3; j++) sol->qr[j] = (float)Q[j + j * NX];
            sol->qr[3] = (float)Q[1];    /* cov xy */
            sol->qr[4] = (float)Q[2 + NX]; /* cov yz */
            sol->qr[5] = (float)Q[2];    /* cov zx */
            sol->ns = (unsigned char)ns;
            sol->age = sol->ratio = 0.0;
            /* validate solution */
            double DOP[4];
            if ((stat = valsol(azel, vsat, n, v, nv, NX, minazel, DOP))) {
                sol->stat = 0 == EPHOPT_SBAS ? SOLQ_SBAS : SOLQ_SINGLE;
            }
            free(v); free(H); free(var);
            //printf("the ECEF of reciver is %lf %lf %lf", x[0], x[1], x[2], blh[0], blh[1], blh[2]);
            ecef2pos(x, blh);
            printf(" ECEF: %lf %lf %lf	BLH: %lf %lf %lf Dtr:%lf %lf %lf\n",
                x[0], x[1], x[2], blh[1] * R2D, blh[0] * R2D, blh[2], x[3], x[5], x[6]);
            if (x[3] != 0&&x[5] != 0&&x[6] != 0) {
                fprintf(recpos, "%d\t%lf\t%lf\t%lf\t%lf\t%lf\t%lf\t%lf\t%lf\t%lf\t%lf\t%lf\t%lf\t%lf\n",
                    sol->time.time, x[0], x[1], x[2],
                    blh[1] * R2D, blh[0] * R2D, blh[2],
                    x[3], x[5], x[6],
                    DOP[0], DOP[1], DOP[2], DOP[3]);
            }
            return stat;
        }
    }
    free(v); free(H); free(var);
    return 0;
}
/* single-point positioning ----------------------------------------------------
* compute receiver position, velocity, clock bias by single-point positioning
* with pseudorange and doppler observables
* args   : obsd_t *obs      I   observation data
*          int    n         I   number of observation data
*          nav_t  *nav      I   navigation data
*          sol_t  *sol      IO  solution
* return : status(1:ok,0:error)
*-----------------------------------------------------------------------------*/
extern int pntpos(const obsd_t* obs, int n, const nav_t* nav, sol_t* sol, double* azel, ssat_t* ssat, FILE* recpos, FILE* satpos,double* ecef) {
    double* rs, * dts, * var, * azel_, * resp;
    int i, stat = 0, vsat[MAXOBS] = { 0 }, svh[MAXOBS];
    printf("pntpos  : time of obs=%s  n=%d\n", time_str(obs[0].time, 3), n);
    sol->stat = SOLQ_NONE;
    sol->time = obs[0].time;
    rs = mat(6, n); dts = mat(2, n); var = mat(1, n); azel_ = zeros(2, n); resp = mat(1, n);
    if (n <= 0) {
        return 0;
    }
    satposs(sol->time, obs, n, nav, 0, rs, dts, var, svh, satpos);
    stat = estpos(obs, n, rs, dts, var, svh, nav, ssat, sol, azel_, vsat, resp, recpos,satpos);
    free(rs); free(dts); free(var); free(azel_); free(resp);
    return stat;
}