/*!
 * \file rtklib_preceph.cc
 * \brief precise ephemeris and clock functions
 * \authors <ul>
 *          <li> 2007-2013, T. Takasu
 *          <li> 2017, Javier Arribas
 *          <li> 2017, Carles Fernandez
 *          </ul>
 *
 * This is a derived work from RTKLIB http://www.rtklib.com/
 * The original source code at https://github.com/tomojitakasu/RTKLIB is
 * released under the BSD 2-clause license with an additional exclusive clause
 * that does not apply here. This additional clause is reproduced below:
 *
 * " The software package includes some companion executive binaries or shared
 * libraries necessary to execute APs on Windows. These licenses succeed to the
 * original ones of these software. "
 *
 * Neither the executive binaries nor the shared libraries are required by, used
 * or included in GNSS-SDR.
 *
 * -----------------------------------------------------------------------------
 * Copyright (C) 2007-2013, T. Takasu
 * Copyright (C) 2017, Javier Arribas
 * Copyright (C) 2017, Carles Fernandez
 * All rights reserved.
 *
 * SPDX-License-Identifier: BSD-2-Clause
 *
 *
 * References :
 *    [1] S.Hilla, The Extended Standard Product 3 Orbit Format (SP3-c),
 *        12 February, 2007
 *    [2] J.Ray, W.Gurtner, RINEX Extensions to Handle Clock Information,
 *        27 August, 1998
 *    [3] D.D.McCarthy, IERS Technical Note 21, IERS Conventions 1996, July 1996
 *    [4] D.A.Vallado, Fundamentals of Astrodynamics and Applications 2nd ed,
 *        Space Technology Library, 2004
 *
 * -----------------------------------------------------------------------------
 */

#include "rtklib_preceph.h"
#include "rtklib_rtkcmn.h"
#include <cstring>

/* satellite code to satellite system ----------------------------------------*/
int code2sys(char code)
{
    if (code == 'G' || code == ' ')
        {
            return SYS_GPS;
        }
    if (code == 'R')
        {
            return SYS_GLO;
        }
    if (code == 'E')
        {
            return SYS_GAL; /* extension to sp3-c */
        }
    if (code == 'J')
        {
            return SYS_QZS; /* extension to sp3-c */
        }
    if (code == 'C')
        {
            return SYS_BDS; /* extension to sp3-c */
        }
    if (code == 'L')
        {
            return SYS_LEO; /* extension to sp3-c */
        }
    return SYS_NONE;
}


/* read sp3 header -----------------------------------------------------------*/
int readsp3h(FILE *fp, gtime_t *time, char *type, int *sats,
    double *bfact, char *tsys)
{
    int i;
    int j;
    int k = 0;
    int ns = 0;
    int sys;
    int prn;
    char buff[1024];

    trace(3, "readsp3h:\n");

    for (i = 0; i < 22; i++)
        {
            if (!fgets(buff, sizeof(buff), fp))
                {
                    break;
                }

            if (i == 0)
                {
                    *type = buff[2];
                    if (str2time(buff, 3, 28, time))
                        {
                            return 0;
                        }
                }
            else if (2 <= i && i <= 6)
                {
                    if (i == 2)
                        {
                            ns = static_cast<int>(str2num(buff, 4, 2));
                        }
                    for (j = 0; j < 17 && k < ns; j++)
                        {
                            sys = code2sys(buff[9 + 3 * j]);
                            prn = static_cast<int>(str2num(buff, 10 + 3 * j, 2));
                            if (k < MAXSAT)
                                {
                                    sats[k++] = satno(sys, prn);
                                }
                        }
                }
            else if (i == 12)
                {
                    strncpy(tsys, buff + 9, 3);
                    tsys[3] = '\0';
                }
            else if (i == 14)
                {
                    bfact[0] = str2num(buff, 3, 10);
                    bfact[1] = str2num(buff, 14, 12);
                }
        }
    return ns;
}


/* add precise ephemeris -----------------------------------------------------*/
int addpeph(nav_t *nav, peph_t *peph)
{
    peph_t *nav_peph;

    if (nav->ne >= nav->nemax)
        {
            nav->nemax += 256;
            if (!(nav_peph = static_cast<peph_t *>(realloc(nav->peph, sizeof(peph_t) * nav->nemax))))
                {
                    trace(1, "readsp3b malloc error n=%d\n", nav->nemax);
                    free(nav->peph);
                    nav->peph = nullptr;
                    nav->ne = nav->nemax = 0;
                    return 0;
                }
            nav->peph = nav_peph;
        }
    nav->peph[nav->ne++] = *peph;
    return 1;
}


/* read sp3 body -------------------------------------------------------------*/
void readsp3b(FILE *fp, char type, int *sats __attribute__((unused)), int ns, const double *bfact,
    char *tsys, int index, int opt, nav_t *nav)
{
    peph_t peph;
    gtime_t time;
    double val;
    double std;
    double base;
    int i;
    int j;
    int sat;
    int sys;
    int prn;
    int n = ns * (type == 'P' ? 1 : 2);
    int pred_o;
    int pred_c;
    int v;
    char buff[1024];

    trace(3, "readsp3b: type=%c ns=%d index=%d opt=%d\n", type, ns, index, opt);

    while (fgets(buff, sizeof(buff), fp))
        {
            if (!strncmp(buff, "EOF", 3))
                {
                    break;
                }

            if (buff[0] != '*' || str2time(buff, 3, 28, &time))
                {
                    trace(2, "sp3 invalid epoch %31.31s\n", buff);
                    continue;
                }
            if (!strcmp(tsys, "UTC"))
                {
                    time = utc2gpst(time); /* utc->gpst */
                }
            peph.time = time;
            peph.index = index;

            for (i = 0; i < MAXSAT; i++)
                {
                    for (j = 0; j < 4; j++)
                        {
                            peph.pos[i][j] = 0.0;
                            peph.std[i][j] = 0.0F;
                            peph.vel[i][j] = 0.0;
                            peph.vst[i][j] = 0.0F;
                        }
                    for (j = 0; j < 3; j++)
                        {
                            peph.cov[i][j] = 0.0F;
                            peph.vco[i][j] = 0.0F;
                        }
                }
            for (i = pred_o = pred_c = v = 0; i < n && fgets(buff, sizeof(buff), fp); i++)
                {
                    if (strlen(buff) < 4 || (buff[0] != 'P' && buff[0] != 'V'))
                        {
                            continue;
                        }

                    sys = buff[1] == ' ' ? SYS_GPS : code2sys(buff[1]);
                    prn = static_cast<int>(str2num(buff, 2, 2));
                    if (sys == SYS_SBS)
                        {
                            prn += 100;
                        }
                    else if (sys == SYS_QZS)
                        {
                            prn += 192; /* extension to sp3-c */
                        }

                    if (!(sat = satno(sys, prn)))
                        {
                            continue;
                        }

                    if (buff[0] == 'P')
                        {
                            pred_c = strlen(buff) >= 76 && buff[75] == 'P';
                            pred_o = strlen(buff) >= 80 && buff[79] == 'P';
                        }
                    for (j = 0; j < 4; j++)
                        {
                            /* read option for predicted value */
                            if (j < 3 && (opt & 1) && pred_o)
                                {
                                    continue;
                                }
                            if (j < 3 && (opt & 2) && !pred_o)
                                {
                                    continue;
                                }
                            if (j == 3 && (opt & 1) && pred_c)
                                {
                                    continue;
                                }
                            if (j == 3 && (opt & 2) && !pred_c)
                                {
                                    continue;
                                }

                            val = str2num(buff, 4 + j * 14, 14);
                            std = str2num(buff, 61 + j * 3, j < 3 ? 2 : 3);

                            if (buff[0] == 'P')
                                { /* position */
                                    if (val != 0.0 && fabs(val - 999999.999999) >= 1e-6)
                                        {
                                            peph.pos[sat - 1][j] = val * (j < 3 ? 1000.0 : 1e-6);
                                            v = 1; /* valid epoch */
                                        }
                                    if ((base = bfact[j < 3 ? 0 : 1]) > 0.0 && std > 0.0)
                                        {
                                            peph.std[sat - 1][j] = static_cast<float>(std::pow(base, std) * (j < 3 ? 1e-3 : 1e-12));
                                        }
                                }
                            else if (v)
                                { /* velocity */
                                    if (val != 0.0 && fabs(val - 999999.999999) >= 1e-6)
                                        {
                                            peph.vel[sat - 1][j] = val * (j < 3 ? 0.1 : 1e-10);
                                        }
                                    if ((base = bfact[j < 3 ? 0 : 1]) > 0.0 && std > 0.0)
                                        {
                                            peph.vst[sat - 1][j] = static_cast<float>(std::pow(base, std) * (j < 3 ? 1e-7 : 1e-16));
                                        }
                                }
                        }
                }
            if (v)
                {
                    if (!addpeph(nav, &peph))
                        {
                            return;
                        }
                }
        }
}


/* compare precise ephemeris -------------------------------------------------*/
int cmppeph(const void *p1, const void *p2)
{
    const auto *q1 = static_cast<const peph_t *>(p1);
    const auto *q2 = static_cast<const peph_t *>(p2);
    double tt = timediff(q1->time, q2->time);
    return tt < -1e-9 ? -1 : (tt > 1e-9 ? 1 : q1->index - q2->index);
}


/* combine precise ephemeris -------------------------------------------------*/
void combpeph(nav_t *nav, int opt)
{
    int i;
    int j;
    int k;
    int m;

    trace(3, "combpeph: ne=%d\n", nav->ne);

    qsort(nav->peph, nav->ne, sizeof(peph_t), cmppeph);

    if (opt & 4)
        {
            return;
        }

    for (i = 0, j = 1; j < nav->ne; j++)
        {
            if (fabs(timediff(nav->peph[i].time, nav->peph[j].time)) < 1e-9)
                {
                    for (k = 0; k < MAXSAT; k++)
                        {
                            if (norm_rtk(nav->peph[j].pos[k], 4) <= 0.0)
                                {
                                    continue;
                                }
                            for (m = 0; m < 4; m++)
                                {
                                    nav->peph[i].pos[k][m] = nav->peph[j].pos[k][m];
                                }
                            for (m = 0; m < 4; m++)
                                {
                                    nav->peph[i].std[k][m] = nav->peph[j].std[k][m];
                                }
                            for (m = 0; m < 4; m++)
                                {
                                    nav->peph[i].vel[k][m] = nav->peph[j].vel[k][m];
                                }
                            for (m = 0; m < 4; m++)
                                {
                                    nav->peph[i].vst[k][m] = nav->peph[j].vst[k][m];
                                }
                        }
                }
            else if (++i < j)
                {
                    nav->peph[i] = nav->peph[j];
                }
        }
    nav->ne = i + 1;

    trace(4, "combpeph: ne=%d\n", nav->ne);
}


/* read sp3 precise ephemeris file ---------------------------------------------
 * read sp3 precise ephemeris/clock files and set them to navigation data
 * args   : char   *file       I   sp3-c precise ephemeris file
 *                                 (wind-card * is expanded)
 *          nav_t  *nav        IO  navigation data
 *          int    opt         I   options (1: only observed + 2: only predicted +
 *                                 4: not combined)
 * return : none
 * notes  : see ref [1]
 *          precise ephemeris is appended and combined
 *          nav->peph and nav->ne must by properly initialized before calling the
 *          function
 *          only files with extensions of .sp3, .SP3, .eph* and .EPH* are read
 *-----------------------------------------------------------------------------*/
void readsp3(const char *file, nav_t *nav, int opt)
{
    FILE *fp;
    gtime_t time = {0, 0};
    double bfact[2] = {};
    int i;
    int j;
    int n;
    int ns;
    int sats[MAXSAT] = {};
    char *efiles[MAXEXFILE];
    char *ext;
    char type = ' ';
    char tsys[4] = "";

    trace(3, "readpephs: file=%s\n", file);

    for (i = 0; i < MAXEXFILE; i++)
        {
            if (!(efiles[i] = static_cast<char *>(malloc(MAXSTRPATH + 255))))
                {
                    for (i--; i >= 0; i--)
                        {
                            free(efiles[i]);
                        }
                    return;
                }
        }
    /* expand wild card in file path */
    n = expath(file, efiles, MAXEXFILE);

    for (i = j = 0; i < n; i++)
        {
            if (!(ext = strrchr(efiles[i], '.')))
                {
                    continue;
                }

            if (!strstr(ext + 1, "sp3") && !strstr(ext + 1, ".SP3") &&
                !strstr(ext + 1, "eph") && !strstr(ext + 1, ".EPH"))
                {
                    continue;
                }

            if (!(fp = fopen(efiles[i], "re")))
                {
                    trace(2, "sp3 file open error %s\n", efiles[i]);
                    continue;
                }
            /* read sp3 header */
            ns = readsp3h(fp, &time, &type, sats, bfact, tsys);

            /* read sp3 body */
            readsp3b(fp, type, sats, ns, bfact, tsys, j++, opt, nav);

            fclose(fp);
        }
    for (i = 0; i < MAXEXFILE; i++)
        {
            free(efiles[i]);
        }

    /* combine precise ephemeris */
    if (nav->ne > 0)
        {
            combpeph(nav, opt);
        }
}


/* read satellite antenna parameters -------------------------------------------
 * read satellite antenna parameters
 * args   : char   *file       I   antenna parameter file
 *          gtime_t time       I   time
 *          nav_t  *nav        IO  navigation data
 * return : status (1:ok,0:error)
 * notes  : only support antex format for the antenna parameter file
 *-----------------------------------------------------------------------------*/
int readsap(const char *file, gtime_t time, nav_t *nav)
{
    pcvs_t pcvs{};
    pcv_t pcv0{};
    pcv_t *pcv;
    int i;

    trace(3, "readsap : file=%s time=%s\n", file, time_str(time, 0));

    if (!readpcv(file, &pcvs))
        {
            return 0;
        }

    for (i = 0; i < MAXSAT; i++)
        {
            pcv = searchpcv(i + 1, "", time, &pcvs);
            nav->pcvs[i] = pcv ? *pcv : pcv0;
        }
    free(pcv);
    return 1;
}


/* read dcb parameters file --------------------------------------------------*/
int readdcbf(const char *file, nav_t *nav, const sta_t *sta)
{
    FILE *fp;
    double cbias;
    char buff[256];
    char str1[32];
    char str2[32] = "";
    int i;
    int j;
    int sat;
    int type = 0;

    trace(3, "readdcbf: file=%s\n", file);

    if (!(fp = fopen(file, "re")))
        {
            trace(2, "dcb parameters file open error: %s\n", file);
            return 0;
        }
    while (fgets(buff, sizeof(buff), fp))
        {
            if (strstr(buff, "DIFFERENTIAL (P1-P2) CODE BIASES"))
                {
                    type = 1;
                }
            else if (strstr(buff, "DIFFERENTIAL (P1-C1) CODE BIASES"))
                {
                    type = 2;
                }
            else if (strstr(buff, "DIFFERENTIAL (P2-C2) CODE BIASES"))
                {
                    type = 3;
                }

            if (!type || sscanf(buff, "%s %s", str1, str2) < 1)
                {
                    continue;
                }

            if ((cbias = str2num(buff, 26, 9)) == 0.0)
                {
                    continue;
                }

            if (sta && (!strcmp(str1, "G") || !strcmp(str1, "R")))
                { /* receiver dcb */
                    for (i = 0; i < MAXRCV; i++)
                        {
                            if (!strcmp(sta[i].name, str2))
                                {
                                    break;
                                }
                        }
                    if (i < MAXRCV)
                        {
                            j = !strcmp(str1, "G") ? 0 : 1;
                            nav->rbias[i][j][type - 1] = cbias * 1e-9 * SPEED_OF_LIGHT_M_S; /* ns -> m */
                        }
                }
            else if ((sat = satid2no(str1)))
                {                                                                      /* satellite dcb */
                    nav->cbias[sat - 1][type - 1] = cbias * 1e-9 * SPEED_OF_LIGHT_M_S; /* ns -> m */
                }
        }
    fclose(fp);

    return 1;
}


/* read dcb parameters ---------------------------------------------------------
 * read differential code bias (dcb) parameters
 * args   : char   *file       I   dcb parameters file (wild-card * expanded)
 *          nav_t  *nav        IO  navigation data
 *          sta_t  *sta        I   station info data to import receiver dcb
 *                                 (NULL: no use)
 * return : status (1:ok,0:error)
 * notes  : currently only p1-c1 bias of code *.dcb file
 *-----------------------------------------------------------------------------*/
int readdcb(const char *file, nav_t *nav, const sta_t *sta)
{
    int i;
    int j;
    int n;
    char *efiles[MAXEXFILE] = {};

    trace(3, "readdcb : file=%s\n", file);

    for (i = 0; i < MAXSAT; i++)
        {
            for (j = 0; j < 3; j++)
                {
                    nav->cbias[i][j] = 0.0;
                }
        }
    for (i = 0; i < MAXEXFILE; i++)
        {
            if (!(efiles[i] = static_cast<char *>(malloc(MAXSTRPATH + 255))))
                {
                    for (i--; i >= 0; i--)
                        {
                            free(efiles[i]);
                        }
                    return 0;
                }
        }
    n = expath(file, efiles, MAXEXFILE);

    for (i = 0; i < n; i++)
        {
            readdcbf(efiles[i], nav, sta);
        }
    for (i = 0; i < MAXEXFILE; i++)
        {
            free(efiles[i]);
        }

    return 1;
}


/* polynomial interpolation by Neville's algorithm ---------------------------*/
double interppol(const double *x, double *y, int n)
{
    int i;
    int j;

    for (j = 1; j < n; j++)
        {
            for (i = 0; i < n - j; i++)
                {
                    y[i] = (x[i + j] * y[i] - x[i] * y[i + 1]) / (x[i + j] - x[i]);
                }
        }
    return y[0];
}


/* satellite position by precise ephemeris -----------------------------------*/
int pephpos(gtime_t time, int sat, const nav_t *nav, double *rs,
    double *dts, double *vare, double *varc)
{
    double t[NMAX + 1];
    double p[3][NMAX + 1];
    double c[2];
    double *pos;
    double std = 0.0;
    double s[3];
    double sinl;
    double cosl;
    int i;
    int j;
    int k;
    int index;

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

    rs[0] = rs[1] = rs[2] = dts[0] = 0.0;

    if (nav->ne < NMAX + 1 ||
        timediff(time, nav->peph[0].time) < -MAXDTE ||
        timediff(time, nav->peph[nav->ne - 1].time) > MAXDTE)
        {
            trace(3, "no prec ephem %s sat=%2d\n", time_str(time, 0), sat);
            return 0;
        }
    /* binary search */
    for (i = 0, j = nav->ne - 1; i < j;)
        {
            k = (i + j) / 2;
            if (timediff(nav->peph[k].time, time) < 0.0)
                {
                    i = k + 1;
                }
            else
                {
                    j = k;
                }
        }
    index = i <= 0 ? 0 : i - 1;

    /* polynomial interpolation for orbit */
    i = index - (NMAX + 1) / 2;
    if (i < 0)
        {
            i = 0;
        }
    else if (i + NMAX >= nav->ne)
        {
            i = nav->ne - NMAX - 1;
        }

    for (j = 0; j <= NMAX; j++)
        {
            t[j] = timediff(nav->peph[i + j].time, time);
            if (norm_rtk(nav->peph[i + j].pos[sat - 1], 3) <= 0.0)
                {
                    trace(3, "prec ephem outage %s sat=%2d\n", time_str(time, 0), sat);
                    return 0;
                }
        }
    for (j = 0; j <= NMAX; j++)
        {
            pos = nav->peph[i + j].pos[sat - 1];
#if 0
            p[0][j] = pos[0];
            p[1][j] = pos[1];
#else
            /* correciton for earh rotation ver.2.4.0 */
            sinl = sin(GNSS_OMEGA_EARTH_DOT * t[j]);
            cosl = cos(GNSS_OMEGA_EARTH_DOT * t[j]);
            p[0][j] = cosl * pos[0] - sinl * pos[1];
            p[1][j] = sinl * pos[0] + cosl * pos[1];
#endif
            p[2][j] = pos[2];
        }
    for (i = 0; i < 3; i++)
        {
            rs[i] = interppol(t, p[i], NMAX + 1);
        }
    if (vare)
        {
            for (i = 0; i < 3; i++)
                {
                    s[i] = nav->peph[index].std[sat - 1][i];
                }
            std = norm_rtk(s, 3);

            /* extrapolation error for orbit */
            if (t[0] > 0.0)
                {
                    std += EXTERR_EPH * std::pow(t[0], 2.0) / 2.0;
                }
            else if (t[NMAX] < 0.0)
                {
                    std += EXTERR_EPH * std::pow(t[NMAX], 2.0) / 2.0;
                }
            *vare = std::pow(std, 2.0);
        }
    /* linear interpolation for clock */
    t[0] = timediff(time, nav->peph[index].time);
    t[1] = timediff(time, nav->peph[index + 1].time);
    c[0] = nav->peph[index].pos[sat - 1][3];
    c[1] = nav->peph[index + 1].pos[sat - 1][3];

    if (t[0] <= 0.0)
        {
            if ((dts[0] = c[0]) != 0.0)
                {
                    std = nav->peph[index].std[sat - 1][3] * SPEED_OF_LIGHT_M_S - EXTERR_CLK * t[0];
                }
        }
    else if (t[1] >= 0.0)
        {
            if ((dts[0] = c[1]) != 0.0)
                {
                    std = nav->peph[index + 1].std[sat - 1][3] * SPEED_OF_LIGHT_M_S + EXTERR_CLK * t[1];
                }
        }
    else if (c[0] != 0.0 && c[1] != 0.0)
        {
            dts[0] = (c[1] * t[0] - c[0] * t[1]) / (t[0] - t[1]);
            i = t[0] < -t[1] ? 0 : 1;
            std = nav->peph[index + i].std[sat - 1][3] + EXTERR_CLK * fabs(t[i]);
        }
    else
        {
            dts[0] = 0.0;
        }
    if (varc)
        {
            *varc = std::pow(std, 2.0);
        }
    return 1;
}


/* satellite clock by precise clock ------------------------------------------*/
int pephclk(gtime_t time, int sat, const nav_t *nav, double *dts,
    double *varc)
{
    double t[2];
    double c[2];
    double std;
    int i;
    int j;
    int k;
    int index;

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

    if (nav->nc < 2 ||
        timediff(time, nav->pclk[0].time) < -MAXDTE ||
        timediff(time, nav->pclk[nav->nc - 1].time) > MAXDTE)
        {
            trace(3, "no prec clock %s sat=%2d\n", time_str(time, 0), sat);
            return 1;
        }
    /* binary search */
    for (i = 0, j = nav->nc - 1; i < j;)
        {
            k = (i + j) / 2;
            if (timediff(nav->pclk[k].time, time) < 0.0)
                {
                    i = k + 1;
                }
            else
                {
                    j = k;
                }
        }
    index = i <= 0 ? 0 : i - 1;

    /* linear interpolation for clock */
    t[0] = timediff(time, nav->pclk[index].time);
    t[1] = timediff(time, nav->pclk[index + 1].time);
    c[0] = nav->pclk[index].clk[sat - 1][0];
    c[1] = nav->pclk[index + 1].clk[sat - 1][0];

    if (t[0] <= 0.0)
        {
            if ((dts[0] = c[0]) == 0.0)
                {
                    return 0;
                }
            std = nav->pclk[index].std[sat - 1][0] * SPEED_OF_LIGHT_M_S - EXTERR_CLK * t[0];
        }
    else if (t[1] >= 0.0)
        {
            if ((dts[0] = c[1]) == 0.0)
                {
                    return 0;
                }
            std = nav->pclk[index + 1].std[sat - 1][0] * SPEED_OF_LIGHT_M_S + EXTERR_CLK * t[1];
        }
    else if (c[0] != 0.0 && c[1] != 0.0)
        {
            dts[0] = (c[1] * t[0] - c[0] * t[1]) / (t[0] - t[1]);
            i = t[0] < -t[1] ? 0 : 1;
            std = nav->pclk[index + i].std[sat - 1][0] * SPEED_OF_LIGHT_M_S + EXTERR_CLK * fabs(t[i]);
        }
    else
        {
            trace(3, "prec clock outage %s sat=%2d\n", time_str(time, 0), sat);
            return 0;
        }
    if (varc)
        {
            *varc = std::pow(std, 2.0);
        }
    return 1;
}


/* satellite antenna phase center offset ---------------------------------------
 * compute satellite antenna phase center offset in ecef
 * args   : gtime_t time       I   time (gpst)
 *          double *rs         I   satellite position and velocity (ecef)
 *                                 {x,y,z,vx,vy,vz} (m|m/s)
 *          int    sat         I   satellite number
 *          nav_t  *nav        I   navigation data
 *          double *dant       I   satellite antenna phase center offset (ecef)
 *                                 {dx,dy,dz} (m) (iono-free LC value)
 * return : none
 *-----------------------------------------------------------------------------*/
void satantoff(gtime_t time, const double *rs, int sat, const nav_t *nav,
    double *dant)
{
    const double *lam = nav->lam[sat - 1];
    const pcv_t *pcv = nav->pcvs + sat - 1;
    double ex[3];
    double ey[3];
    double ez[3];
    double es[3];
    double r[3];
    double rsun[3];
    double gmst;
    double erpv[5] = {};
    double gamma;
    double C1;
    double C2;
    double dant1;
    double dant2;
    int i;
    int j = 0;
    int k = 1;

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

    /* sun position in ecef */
    sunmoonpos(gpst2utc(time), erpv, rsun, nullptr, &gmst);

    /* unit vectors of satellite fixed coordinates */
    for (i = 0; i < 3; i++)
        {
            r[i] = -rs[i];
        }
    if (!normv3(r, ez))
        {
            return;
        }
    for (i = 0; i < 3; i++)
        {
            r[i] = rsun[i] - rs[i];
        }
    if (!normv3(r, es))
        {
            return;
        }
    cross3(ez, es, r);
    if (!normv3(r, ey))
        {
            return;
        }
    cross3(ey, ez, ex);

    if (NFREQ >= 3 && (satsys(sat, nullptr) & (SYS_GAL | SYS_SBS)))
        {
            k = 2;
        }

    if (NFREQ < 2 || lam[j] == 0.0 || lam[k] == 0.0)
        {
            return;
        }

    gamma = std::pow(lam[k], 2.0) / std::pow(lam[j], 2.0);
    C1 = gamma / (gamma - 1.0);
    C2 = -1.0 / (gamma - 1.0);

    /* iono-free LC */
    for (i = 0; i < 3; i++)
        {
            dant1 = pcv->off[j][0] * ex[i] + pcv->off[j][1] * ey[i] + pcv->off[j][2] * ez[i];
            dant2 = pcv->off[k][0] * ex[i] + pcv->off[k][1] * ey[i] + pcv->off[k][2] * ez[i];
            dant[i] = C1 * dant1 + C2 * dant2;
        }
}


/* satellite position/clock by precise ephemeris/clock -------------------------
 * compute satellite position/clock with precise ephemeris/clock
 * args   : gtime_t time       I   time (gpst)
 *          int    sat         I   satellite number
 *          nav_t  *nav        I   navigation data
 *          int    opt         I   sat position option
 *                                 (0: center of mass, 1: antenna phase center)
 *          double *rs         O   sat position and velocity (ecef)
 *                                 {x,y,z,vx,vy,vz} (m|m/s)
 *          double *dts        O   sat clock {bias,drift} (s|s/s)
 *          double *var        IO  sat position and clock error variance (m)
 *                                 (NULL: no output)
 * return : status (1:ok,0:error or data outage)
 * notes  : clock includes relativistic correction but does not contain code bias
 *          before calling the function, nav->peph, nav->ne, nav->pclk and
 *          nav->nc must be set by calling readsp3(), readrnx() or readrnxt()
 *          if precise clocks are not set, clocks in sp3 are used instead
 *-----------------------------------------------------------------------------*/
int peph2pos(gtime_t time, int sat, const nav_t *nav, int opt,
    double *rs, double *dts, double *var)
{
    double rss[3];
    double rst[3];
    double dtss[1];
    double dtst[1];
    double dant[3] = {};
    double vare = 0.0;
    double varc = 0.0;
    double tt = 1e-3;
    int i;

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

    if (sat <= 0 || MAXSAT < sat)
        {
            return 0;
        }

    /* satellite position and clock bias */
    if (!pephpos(time, sat, nav, rss, dtss, &vare, &varc) ||
        !pephclk(time, sat, nav, dtss, &varc))
        {
            return 0;
        }

    time = timeadd(time, tt);
    if (!pephpos(time, sat, nav, rst, dtst, nullptr, nullptr) ||
        !pephclk(time, sat, nav, dtst, nullptr))
        {
            return 0;
        }

    /* satellite antenna offset correction */
    if (opt)
        {
            satantoff(time, rss, sat, nav, dant);
        }
    for (i = 0; i < 3; i++)
        {
            rs[i] = rss[i] + dant[i];
            rs[i + 3] = (rst[i] - rss[i]) / tt;
        }
    /* relativistic effect correction */
    if (dtss[0] != 0.0)
        {
            dts[0] = dtss[0] - 2.0 * dot(rs, rs + 3, 3) / SPEED_OF_LIGHT_M_S / SPEED_OF_LIGHT_M_S;
            dts[1] = (dtst[0] - dtss[0]) / tt;
        }
    else
        { /* no precise clock */
            dts[0] = dts[1] = 0.0;
        }
    if (var)
        {
            *var = vare + varc;
        }

    return 1;
}