#!/usr/bin/env python
# -*- coding: utf-8 -*-
# This code is generated by scons.  Do not hand-hack it!
# This file is Copyright 2020 by the GPSD project
# SPDX-License-Identifier: BSD-2-clause

# This code runs compatibly under Python 2 and 3.x for x >= 2.
# Preserve this property!
#
"""gpssubframe -- read JSON subframe messages from gpsd and decode them."""

from __future__ import print_function

import argparse
import json           # for json.dumps()
import math
import socket
import sys
import time            # for time.time()

# pylint wants local modules last
try:
    import gps
except ImportError as e:
    sys.stderr.write(
        "%s: can't load Python gps libraries -- check PYTHONPATH.\n" %
        (sys.argv[0]))
    sys.stderr.write("%s\n" % e)
    sys.exit(1)

gps_version = '3.22'
if gps.__version__ != gps_version:
    sys.stderr.write("%s: ERROR: need gps module version %s, got %s\n" %
                     (sys.argv[0], gps_version, gps.__version__))
    sys.exit(1)


ura2ure = {0: "0.00 to 2.40",
           1: "2.40 to 3.40",
           2: "3.40 to 4.85",
           3: "4.85 to 6.85",
           4: "6.85 to 9.65",
           5: "9.65 to 13.65",
           6: "13.65 to 24.00",
           7: "24.00 to 48.00",
           8: "48.00 to 96.00",
           9: "96.00 to 192.00",
           10: "192.00 to 384.00",
           11: "384.00 to 768.00",
           12: "768.00 to 1536.00",
           13: "1536.00 to 3072.00",
           14: "3072.00 to 6144.00",
           15: "6144.00 or worse",
           }

health_str = {0: "All Signals OK",
              1: "All Signals Weak",
              2: "All Signals Dead",
              3: "All Signals Have No Data Modulation",
              4: "L1 P Signal Weak",
              5: "L1 P Signal Dead",
              6: "L1 P Signal Has No Data Modulation",
              7: "L2 P Signal Weak",
              8: "L2 P Signal Dead",
              9: "L2 P Signal Has No Data Modulation",
              10: "L1C Signal Weak",
              11: "L1C Signal Dead",
              12: "L1C Signal Has No Data Modulation",
              13: "L2C Signal Weak",
              14: "L2C Signal Dead",
              15: "L2C Signal Has No Data Modulation",
              16: "L1 & L2 P Signal Weak",
              18: "L1 & L2 P Signal Dead",
              19: "L1 & L2 P Signal Has No Data Modulation",
              20: "L1 & L2C Signal Weak",
              21: "L1 & L2C Signal Dead",
              22: "L1 & L2C Signal Has No Data Modulation",
              23: "L1 Signal Weak",
              24: "L1 Signal Dead",
              25: "L1 Signal Has No Data Modulation",
              26: "L2 Signal Weak",
              27: "L2 Signal Dead",
              28: "L2 Signal Has No Data Modulation",
              29: "SV Is Temporarily Out",
              30: "SV Will Be Temporarily Out",
              31: "Multiple Anomalies",
              }

# subframe 4 page 25 sv config
sv_conf = {
    0: "Reserved",
    1: "Block II/Block IIA/IIR SV",
    2: "M-code, L2C, Block IIR-M SV",
    3: "M-code, L2C, L5, Block IIF SV",
    4: "M-code, L2C, L5, No SA,  Block III SV",
    5: "Reserved",
    6: "Reserved",
    7: "Reserved",
    }


def kepler(e, M):
    """Keplers iteration to solve his equation."""

    # https://en.wikipedia.org/wiki/Kepler%27s_equation
    E = M             # initial guess
    for i in range(1, 20):
        temp = E
        E = M + e * math.sin(E)
        # not sure how accurate we need to be
        # but we want 18 digits of lat/lon
        if 1e-18 > math.fabs(E - temp):
            break
    return E

# another kepler algorithm here:
# http://www.alpheratz.net/dynamics/twobody/KeplerIterations_summary.pdf


# Online data check:
# real time sat positions: https://in-the-sky.org/satmap_worldmap.php
#
# See also:
# RTKLIB src/ephemeris.c
# https://www.telesens.co/2017/07/17/calculating-position-from-raw-gps-data/#1b_Code_for_Calculating_Satellite_Position
# gnss-sdr: src/core/system_parameters/gps_ephemeris.cc
def SatPos(ephm, t):
    """Calculate GPS Satellite Position from Ephemeris/Almanac.

Calculations from IS-GPS-200 table 20-IV

return (x, y, z, deltatsv) of satellite at gps_tow t
"""

    # Semi-major axis
    A = ephm['sqrtA'] ** 2      # meters

    # Computed mean motion
    n0 = math.sqrt(gps.WGS84GM / (A ** 3))           # rad/sec

    # t: GPS system time of week at time of transmission
    # tk: time from ephemeris reference epoch
    tk = t - ephm['toe']                 # seconds
    # handle week rollover
    if 302400 < tk:
        tk -= 604800
    elif -302400 > tk:
        tk += 604800

    # corrected mean motion
    n = n0 + ephm['deltan']   # rad/sec

    # mean anomaly, rads
    Mk = ephm['M0'] + (n * tk)

    # solve for eccentric anomaly
    Ek = kepler(ephm['e'], Mk)   # radians
    cosEk = math.cos(Ek)
    sinEk = math.sin(Ek)

    # true anomaly
    nuk = math.atan2(
       math.sqrt(1 - (ephm['e'] ** 2)) * sinEk /
       (1 - ephm['e'] * math.cos(Ek)),
       (cosEk - ephm['e']) / (1 - ephm['e'] * cosEk))

    # alternate true anomaly
    # close enough?
    # nuk = math.atan2(math.sqrt(1 - (ephm['e'] ** 2)) * sinEk,
    #                  (cosEk - ephm['e']))

    # Argument of Latitude
    Phik = nuk + ephm['omega']

    if 'Cus' in ephm:
        # 2nd harmonic corrections
        # Argument of Latitude Correction
        sin2Phik = math.sin(2 * Phik)
        cos2Phik = math.cos(2 * Phik)
        deltauk = (ephm['Cus'] * sin2Phik + ephm['Cuc'] * cos2Phik)
        # Radius Correction
        deltark = (ephm['Crs'] * sin2Phik + ephm['Crc'] * cos2Phik)
        # Inclination Correction
        deltaik = (ephm['Cis'] * sin2Phik + ephm['Cic'] * cos2Phik)
    else:
        # almanac does not have these corrections
        deltauk = 0
        deltark = 0
        deltaik = 0

    # Corrected Argument of Latitude
    uk = Phik + deltauk

    # Corrected Radius
    rk = A * (1 - ephm['e'] * cosEk) + deltark

    # Corrected Inclination Angle
    ik = ephm['i0'] + ephm['IDOT'] * tk + deltaik

    # Positions in orbital plane.
    xkprime = rk * math.cos(uk)
    ykprime = rk * math.sin(uk)

    # diff of sat and earth Omega dot
    delta_Omegad = ephm['Omegad'] - gps.WGS84AV
    # Corrected longitude of ascending node.
    Omegak = (ephm['Omega0'] + delta_Omegad * tk - gps.WGS84AV * ephm['toe'])

    # Earth-fixed coordinates.
    cosik = math.cos(ik)
    sinik = math.sin(ik)
    ykprimecosik = ykprime * cosik
    sinOmegak = math.sin(Omegak)
    cosOmegak = math.cos(Omegak)
    x = xkprime * cosOmegak - ykprimecosik * sinOmegak
    y = xkprime * sinOmegak + ykprimecosik * cosOmegak
    z = ykprime * sinik

    # sat velocity in ECEF coordinates, meters per second
    # FIXME: broken calculation
    # vx = (-delta_Omegad * (xkprime + ykprime * cosik) +
    #       x * cosOmegak -
    #       y * cosik * sinOmegak)
    # vy = (delta_Omegad * (xkprime * cosOmegak - ykprime * cosik * sinOmegak)
    #       + x * sinOmegak + y * cosik * cosOmegak)
    # vz = y * sinik

    # Almanac does not have af2, toc, or ure
    # but it does have af0 and af1, so compute what we can

    # relativistic correction term, seconds
    deltatr = gps.GPS_F * ephm['e'] * ephm['sqrtA'] * sinEk
    # toff == SV PRN code phase offset, seconds
    # we want tgps, but our t (tsv) is close enough.
    toff = t - ephm['toc']
    # handle week rollover
    if 302400 < toff:
        toff -= 604800
    elif -302400 > toff:
        toff += 604800
    # SV Clock Correction
    deltatsv = (ephm['af0'] + ephm['af1'] * toff +
                ephm['af2'] * toff ** 2 + deltatr)
    print("deltatr %.9e deltatsv %.9e Tgd %.9e toff %.3f" %
          (deltatr, deltatsv, ephm['Tgd'], toff))
    if 'ura' in ephm:
        # ephemeris, not almanac
        print("URE %s Health %s" %
              (ura2ure[ephm['ura']], health_str[ephm['hlth'] & 0x1f]))

    if 1 < options.debug:
        if 2 < options.debug:
            print(ephm)
        print("t %.3f tk %.3f" % (t, tk))
        print("A %s n0 %s n %s\nMk %s Ek %s" % (A, n0, n, Mk, Ek))
        print("nuk %.10g Phik %.10g" % (nuk, Phik))
        print("deltauk %.10g deltark %.10g deltaik %.10g" %
              (deltauk, deltark, deltaik))
        print("uk %.10g rk %.10g ik %.10g" % (uk, rk, ik))
        print("xkprime %.10g ykprime %.10g Omegak %.10g" %
              (xkprime, ykprime, Omegak))
        # compute distance from earth's center, and orbital speed, as
        # simple cross-check
        # GPS orbit radius about 26,600 km
        dist = math.sqrt((x * x) + (y * y) + (z * z))
        print("dist %13.3f" % dist)

    # FIXME: broken
    # if 1 < options.debug:
        # GPS orbit speed about 3.9 km/sec
        # speed = math.sqrt((vx * vx) + (vy * vy) + (vz * vz))
        # print("speed %13.3f" % speed)
    # print("vx %13.3f yv %13.3f zv %13.3f" % (vx, vy, vz))

    return (x, y, z, deltatsv)


def SatPosSum(eph, gps_tow):
    """Call SatPos() many times to compute and display more data.

Compute Velocities.  Use deltatsv to correct pseudorange.
"""

    (x, y, z, deltatsv) = SatPos(eph, gps_tow)

    # lat/lon/alt
    (lat, lon, altHAE) = gps.ecef2lla(x, y, z)
    print(" x %13.3f  y %13.3f  z %13.3f deltatsv %g\n"
          "lat %.6f lon %.6f altHAE %.3f" %
          (x, y, z, deltatsv, lat, lon, altHAE))

    # compute sat velocity by subtracting two postions.
    # because I can't get the direct method to work...
    delta_t = 0.1
    (x2, y2, z2, deltatsv2) = SatPos(eph, gps_tow + delta_t)
    vx = (x2 - x) / delta_t
    vy = (y2 - y) / delta_t
    vz = (z2 - z) / delta_t
    # GPS orbit speed about 3.9 km/sec
    speed = math.sqrt((vx * vx) + (vy * vy) + (vz * vz))
    print("vx %13.3f  yv %13.3f  zv %13.3f speed %13.3f" %
          (vx, vy, vz, speed))

    # Try ecef first, no trig.  This agrees to the meter level
    # with the next range calculation that uses lat/Log/AltHAE.
    # This one likely better.
    if (('ecefx' in all_data['tpv'] and
         'ecefy' in all_data['tpv'] and
         'ecefz' in all_data['tpv'])):
        dx = x - all_data['tpv']['ecefx']
        dy = y - all_data['tpv']['ecefy']
        dz = z - all_data['tpv']['ecefz']
        rng = math.sqrt(dx ** 2 + dy ** 2 + dz ** 2)
        print("sta x %.3f y %.3f z %.3f\n"
              "rng %.3f" %
              (all_data['tpv']['ecefx'], all_data['tpv']['ecefy'],
               all_data['tpv']['ecefz'], rng))

    # ax and el to sat
    if (('lat' in all_data['tpv'] and
         'lon' in all_data['tpv'] and
         'altHAE' in all_data['tpv'])):
        (E, N, U) = gps.ecef2enu(
            x, y, z, all_data['tpv']['lat'], all_data['tpv']['lon'],
            all_data['tpv']['altHAE'])
        (az, el, rng) = gps.enu2aer(E, N, U)
        print("E %.3f N %.3f U %.3f\n"
              "az %.3f el %.3f rng %.3f" %
              (E, N, U, az, el, rng))

        (E2, N2, U2) = gps.ecef2enu(
            x2, y2, z2, all_data['tpv']['lat'], all_data['tpv']['lon'],
            all_data['tpv']['altHAE'])
        (az2, el2, rng2) = gps.enu2aer(E2, N2, U2)
        range_speed = (rng2 - rng) / delta_t
        # doppler is postive when sat egetting nearer
        dopplerl1 = -2 * range_speed / gps.GPS_L1_WL
        print("range speed %.3f L1 doppler %.3f" %
              (range_speed, dopplerl1))

    # pseudorange is range + C * clock_errors + C * path_delays


# current GPS Ephemeris here, url split: https://cddis.nasa.gov/
#   Data_and_Derived_Products/GNSS/broadcast_ephemeris_data.html
# all the data is one big JSON for easy save/load
all_data = {
    # current almanac here: https://www.navcen.uscg.gov/?pageName=gpsAlmanacs
    'almanac': {},
    'ephemeris1': {},
    'ephemeris2': {},
    'ephemeris3': {},
    # subframe 4 page 25
    'health': {},
    # subframe 5 page 25
    'health2': {},
    # ionosphere, subframe 4, page 18, data id 56
    'ionosphere': {},
    # for current time and position
    'tpv': None
    }


ephem1_fields = {
    1: ('ura', 'URA Index'),
    2: ('WN', 'Data Sequence Propagation Week Number'),
    3: ('L2P', 'L2 P data flag'),
    4: ('hlth', 'SV health'),
    5: ('Tgd', '(s) Group Delay Differential'),
    6: ('IODC', 'Issue of Data, Clock'),
    7: ('toc', '(s) Time of Clock'),
    8: ('L2', 'Code on L2'),
    9: ('af0', '(sc) SV Clock Bias Correction Coefficient'),
    10: ('af1', '(s) SV Clock Drift Correction Coefficient'),
    11: ('af2', '(s/s) Drift Rate Correction Coefficient'),
    }

ephem2_fields = {
    1: ('IODE', 'Issue of Data, Ephemeris'),
    2: ('M0', '(sc) Mean Anomaly at Reference Time'),
    3: ('deltan', '(sc/s) Mean Motion Difference from Computed Value'),
    4: ('e', 'Eccentricity '),
    5: ('sqrtA', '(sqrt(m))Square Root of the Semi-Major Axis'),
    6: ('FIT', 'Fit Interval Flag'),
    7: ('AODO', '(s) Age of Data Offset'),
    8: ('Crs', '(m) Sine Correction Amplitude Term to Orbit Radius'),
    9: ('Cus', '(rad) Cosine Correction Amplitude Term to Orbit Radius'),
    10: ('Cuc', '(rad) Sine Harmonic Correction Term to Arg of Lat'),
    11: ('toe', '(s) Reference Time Ephemeris')
    }

ephem3_fields = {
    1: ('IODE', 'Issue of Data, Ephemeris'),
    2: ('Crc', '(m) Cosine Harmonic Correction Amplitude Orbit Radius'),
    3: ('Cic',
        '(rad) Cosine Harmonic Correction Amplitude Angle of Inclination'),
    4: ('Cis',
        '(rad) Sine Harmonic Correction Amplitude Angle of Inclination'),
    5: ('Omega0',
        '(sc) Longitude of Ascending Node of Orbit Plane Week beginning'),
    6: ('i0', '(sc) Inclination Angle at Reference Time'),
    7: ('omega', '(sc) Argument of Perigee'),
    8: ('Omegad', '(sc/s) Rate of Right Ascension'),
    9: ('IDOT', '(sc/s) Rate of Inclination Angle'),
    }

almanac_fields = {
    # 1: ('ID' same as SV
    1: ('Health', 'SV health'),
    2: ('e', 'Eccentricity '),
    3: ('toa', '(s) Almanac Rference Time'),
    4: ('deltai', '(sc) Inclination offset from 0.3 semicircles (= 54 deg)'),
    5: ('Omegad', '(sc/s) Rate of Right Ascension'),
    6: ('sqrtA', '(m-2)Square Root of the Semi-Major Axis'),
    7: ('Omega0',
        '(sc) Longitude of Ascending Node of Orbit Plane Week beginning'),
    8: ('omega', '(sc) Argument of Perigee'),
    9: ('M0', '(sc) Mean Anomaly at Reference Time'),
    10: ('af0', '(s) SV Clock Bias Correction Coefficient'),
    11: ('af1', '(s/s) SV Clock Drift Correction Coefficient'),
    }

# see IS-GPS-200 Section 20.3.3.5.2.4 Coordinated Universal Time (UTC).
ionosphere_fields = {
    1: ('A0', '(s) constant term of polynomial'),
    2: ('A1', '(s) first order term of polynomial'),
    3: ('ls', '(s) delta time due to leap seconds'),
    4: ('tot', '(s) reference time for UTC data'),
    5: ('WNt', '(wk) UTC reference week number'),
    6: ('WNlsf', '(wk) Week Number of future Leap Second'),
    7: ('DN', '(dy) Day Number of future Leap Second'),
    8: ('lsf', '(s) future Leap Second'),
    9: ('a0', '(s) constant term amplitude of vertical delay'),
    10: ('a1', '(s/sc) first order term amplitude of vertical delay'),
    11: ('a2', '(s/sc^2) second order term amplitude of vertical delay'),
    12: ('a3', '(s/sc^3) third order term amplitude of vertical delay'),
    13: ('b0', '(s) constant term period of the model'),
    14: ('b1', '(s/sc) first order term period of the model'),
    15: ('b2', '(s/sc^2) second order term period of the model'),
    16: ('b3', '(s/sc^3) third order term period of the model'),
    }


def _print_msg(ephem, fields):
    """Print Subframe Data."""
    for index in sorted(fields.keys()):
        fld = fields[index]
        print("%10s %s" % (fld[0], ephem[fld[0]]))
        if options.desc:
            print("           %-48s " % (fld[1]))


def test():
    # FIXME: Broken
    # SatPos() tests
    # lat 0.000000 lon 0.000000 altHAE 20175272.000
    ephm = {
        'af0': 0,
        'af1': 0,
        'af2': 0,
        'AODO': 0,
        'Cic': 0,
        'Cis': 0,
        'Crc': 0,
        'Crs': 0,
        'Cuc': 0,
        'Cus': 0,
        'deltan': 0,
        'e': 0,         # nominal 0
        'FIT': 0,
        'hlth': 0,
        'i0': 0,        # nominal 55.0 degrees +/- 3
        'IDOT': 0,
        'IODC': 53,
        'IODE': 53,
        'L2': 1,
        'L2P': 0,
        'M0': 0,
        'omega': 0,     # nominal 0, +/1 180 degrees
        'Omega0': 0,
        'Omegad': 0,
        'sqrtA': 5153,  # nominal 5153.542538875565
        'Tgd': 0,
        'toc': 0,
        'toe': 0,
        'ura': 0,
        'WN': 75}

    options.debug = 2
    print("\nTest Ephemeris:")
    SatPos(ephm, 0)

    # move 90 degrees
    print("\nTest omega 90:")
    ephm['omega'] = gps.GPS_PI / 2

    SatPos(ephm, 0)

    # move 180 degrees
    print("\nTest omega 180:")
    ephm['omega'] = gps.GPS_PI

    SatPos(ephm, 0)

    # move 270 degrees
    print("\nTest omega 270:")
    ephm['omega'] = gps.GPS_PI * 1.5

    SatPos(ephm, 0)

    # move omega 0, Omega0 90
    print("\nTest Omega 90:")
    ephm['omega'] = 0
    ephm['Omega0'] = gps.GPS_PI / 2

    SatPos(ephm, 0)

    # move omega 0, Omega0 180
    print("\nTest Omega 180:")
    ephm['omega'] = 0
    ephm['Omega0'] = gps.GPS_PI

    SatPos(ephm, 0)

    # move omega 90, Omega0 90
    print("\nTest omega 90 Omega0 90:")
    ephm['omega'] = gps.GPS_PI / 2
    ephm['Omega0'] = gps.GPS_PI / 2

    SatPos(ephm, 0)

    # move omega 0, Omega0 0, i0 90
    print("\nTest omega 0 Omega0 0 i0 90:")
    ephm['omega'] = 0
    ephm['Omega0'] = 0
    ephm['i0'] = gps.GPS_PI / 2

    SatPos(ephm, 0)

    # move omega 45, Omega0 0, i0 90
    print("\nTest omega 45 Omega0 0 i0 90:")
    ephm['omega'] = gps.GPS_PI / 4
    ephm['Omega0'] = 0
    ephm['i0'] = gps.GPS_PI / 2

    SatPos(ephm, 0)

    # move omega 45, Omega0 45, i0 90
    print("\nTest omega 45 Omega0 45 i0 90:")
    ephm['omega'] = gps.GPS_PI / 4
    ephm['Omega0'] = gps.GPS_PI / 4
    ephm['i0'] = gps.GPS_PI / 2

    SatPos(ephm, 0)

    # move omega 45, Omega0 0, i0 45
    print("\nTest omega 45 Omega0 0 i0 45:")
    ephm['omega'] = gps.GPS_PI / 4
    ephm['Omega0'] = 0
    ephm['i0'] = gps.GPS_PI / 4

    SatPos(ephm, 0)

    # move omega 90, Omega0 0, i0 90
    # altitude blows up.
    print("\nTest omega 0 Omega0 0 i0 0:")
    ephm['omega'] = gps.GPS_PI / 2
    ephm['Omega0'] = 0
    ephm['i0'] = gps.GPS_PI / 2

    SatPos(ephm, 0)

    # omega Omega0, i0 seem OK.

    # move omega 0, Omega0 0, i0 0
    # t = one quarter sideral day
    siderial_day = 86164.0905
    print("\nTest omega 0 Omega0 0 i0 0:")
    ephm['omega'] = 0
    ephm['Omega0'] = 0
    ephm['i0'] = 0

    SatPos(ephm, siderial_day / 4)

    # move omega 0, Omega0 0, i0 0
    # t = one half sideral day
    # sat has done one orbit, earth 1/2 orbit
    print("\nTest omega 0 Omega0 0 i0 0:")
    ephm['omega'] = 0
    ephm['Omega0'] = 0
    ephm['i0'] = 0

    SatPos(ephm, siderial_day / 2)
    return 0


description = 'Convert one gpsd JSON message class to csv format.'
usage = '%(prog)s [OPTIONS] [host[:port[:device]]]'
epilog = ('BSD terms apply: see the file COPYING in the distribution root'
          ' for details.')

parser = argparse.ArgumentParser(
    description=description,
    epilog=epilog,
    formatter_class=argparse.RawDescriptionHelpFormatter,
    usage=usage)
parser.add_argument(
    '-?',
    action="help",
    help='show this help message and exit'
)
# WIP
parser.add_argument(
    '-c',
    '--continuous',
    dest='continuous',
    default=False,
    action="store_true",
    help=('INOP: Continuously calculate a SatPos() after -x SECONDS '
           '[Default %(default)s]')
)
parser.add_argument(
    '-D',
    '--debug',
    dest='debug',
    default=0,
    type=int,
    help='Set level of debug. Must be integer. [Default %(default)s]'
)
parser.add_argument(
    '--desc',
    dest='desc',
    default=False,
    action="store_true",
    help='Print long descriptions [Default %(default)s]'
)
parser.add_argument(
    '--device',
    dest='device',
    default='',
    help='The device to connect. [Default %(default)s]'
)
parser.add_argument(
    '-f', '--file',
    dest='input_file_name',
    default=None,
    metavar='FILE',
    help='Read gpsd JSON from FILE instead of a gpsd instance.',
)
parser.add_argument(
    '--host',
    dest='host',
    default='localhost',
    help='The host to connect. [Default %(default)s]'
)
parser.add_argument(
    '-n',
    '--count',
    dest='count',
    default=0,
    type=int,
    help='Count of messages to parse. 0 to disable. [Default %(default)s]'
)
parser.add_argument(
    '--load',
    dest='loadfile',
    default=None,
    type=str,
    help=('Load saved JSON Subframe data trom loadfile. '
          ' [Default %(default)s]')
)
parser.add_argument(
    '--port',
    dest='port',
    default=gps.GPSD_PORT,
    help='The port to connect. [Default %(default)s]'
)
parser.add_argument(
    '--progress',
    dest='progress',
    default=False,
    action="store_true",
    help='Print progress reports [Default %(default)s]'
)
parser.add_argument(
    '--satpos',
    dest='satpos',
    default=False,
    action="store_true",
    help='Compute Satellite Positions [Default %(default)s]'
)
parser.add_argument(
    '--save',
    dest='savefile',
    default=None,
    type=str,
    help=('Save decoded Subframe data in savefile as JSON. '
          ' [Default %(default)s]')
)
parser.add_argument(
    '--test',
    action="store_true",
    dest='test',
    default=False,
    help='Run tests of the algorithms [Default %(default)s]'
)
parser.add_argument(
    '--time',
    dest='timeat',
    default=False,
    metavar='TIME',
    type=float,
    help='Compute sat positions at TIME seconds,  [Default: current time ]'
)
parser.add_argument(
    '-V', '--version',
    action='version',
    version="%(prog)s: Version " + gps_version + "\n",
    help='Output version to stderr, then exit'
)
parser.add_argument(
    '-x',
    '--seconds',
    dest='seconds',
    default=16,
    type=int,
    help='Seconds of messages to parse. 0 to disable. [Default %(default)s]'
)
parser.add_argument(
    'target',
    nargs='?',
    help='[host[:port[:device]]]'
)
options = parser.parse_args()

if options.loadfile:
    with open(options.loadfile, 'r') as load_file:
        # the JSON format will change, do not depend on it.
        all_data = json.load(load_file)

# the options host, port, device are set by the defaults
if options.target:
    # override host, port and device with target
    arg = options.target.split(':')
    len_arg = len(arg)
    if len_arg == 1:
        (options.host,) = arg
    elif len_arg == 2:
        (options.host, options.port) = arg
    elif len_arg == 3:
        (options.host, options.port, options.device) = arg
    else:
        parser.print_help()
        sys.exit(0)

if options.test:
    ret = test()
    sys.exit(ret)

if not options.port:
    options.port = 2947

try:
    session = gps.gps(device=options.device,
                      input_file_name=options.input_file_name,
                      host=options.host,
                      port=options.port,
                      verbose=options.debug)
except socket.error:
    sys.stderr.write("gpscsv: Could not connect to gpsd daemon\n")
    sys.exit(1)

session.stream(gps.WATCH_ENABLE | gps.WATCH_SCALED, devpath=options.device)

# save subframe 1 WN
subframe1_WN = -1

count = 0
if 0 < options.seconds:
    end_seconds = time.time() + options.seconds
else:
    end_seconds = 0

if options.continuous:
    # force --satpos
    options.satpos = True

try:
    while True:
        try:
            msg = session.next()
        except StopIteration:
            # most likely end of file, maybe lost connection to gpsd
            break

        if 'SUBFRAME' == msg['class']:
            # use %02d so it sorts nicely.
            if 'dataid' in msg:
                # data id in subframes 4 and 5
                if 0 == msg['dataid']:
                    # dummy SV
                    continue
                idx = "GP%02d" % msg['dataid']
            elif 'tSV' in msg:
                # tSV in all subframes
                idx = "GP%02d" % msg['tSV']
            else:
                # huh?
                idx = "X"

            if 1 < options.debug:
                print("Subframe %d idx %s TOW17 %s WN %d" %
                      (msg["frame"], idx, msg['TOW17'], subframe1_WN))

            if 1 == msg['frame']:
                if 'WN' in msg['EPHEM1']:
                    # save WN for later use
                    subframe1_WN = msg['EPHEM1']['WN']

                if ((idx not in all_data['ephemeris1'] or
                     msg['EPHEM1']['IODC'] !=
                     all_data['ephemeris1'][idx]['IODC'])):

                    if options.progress:
                        print("Received Ephemeris1 %5s, IODC %3d" %
                              (idx, msg['EPHEM1']['IODC']))
                    if 1 < math.fabs(msg['EPHEM1']['af0']):
                        print("ERROR: af0 %s out of range.  Unscaled data?" %
                              msg['EPHEM1']['af0'])
                        sys.exit(1)
                    all_data['ephemeris1'][idx] = msg['EPHEM1']
            elif 2 == msg['frame']:
                if ((idx not in all_data['ephemeris2'] or
                     msg['EPHEM2']['IODE'] !=
                     all_data['ephemeris2'][idx]['IODE'])):
                    if options.progress:
                        print("Received Ephemeris2 %5s, IODE %3d" %
                              (idx, msg['EPHEM2']['IODE']))
                    all_data['ephemeris2'][idx] = msg['EPHEM2']
            elif 3 == msg['frame']:
                if ((idx not in all_data['ephemeris3'] or
                     msg['EPHEM3']['IODE'] !=
                     all_data['ephemeris3'][idx]['IODE'])):
                    if options.progress:
                        print("Received Ephemeris3 %5s, IODE %3d" %
                              (idx, msg['EPHEM3']['IODE']))
                    all_data['ephemeris3'][idx] = msg['EPHEM3']
            elif 4 <= msg['frame'] <= 5:
                if 'ALMANAC' in msg:
                    # save it if:
                    #  not dummy (data id != 0)
                    #  don't already have it
                    #  newer than what we have
                    if ((idx not in all_data['almanac'] or
                         all_data['almanac'][idx]['toa'] <
                         msg['ALMANAC']['toa'])):
                        # FIXME: handle week rollover
                        if options.progress:
                            print("Received Almanac dataid %2d  toa %d" %
                                  (msg['dataid'], msg['ALMANAC']['toa']))
                        all_data['almanac'][idx] = msg['ALMANAC']

                elif 'IONO' in msg:
                    if ((not all_data['ionosphere'] or
                         msg['IONO']['tot'] < all_data['ionosphere']['tot'])):
                        # FIXME handle WNt rollover
                        # new data set
                        if options.progress:
                            print("Received Subframe 4 dataid 56 "
                                  "(Ionosphere and UTC) tot %d WNt %d" %
                                  (msg['IONO']['tot'], msg['IONO']['WNt']))
                        all_data['ionosphere'] = msg['IONO']
                elif 'HEALTH' in msg:
                    if options.progress:
                        print("Received Subframe 4 dataid 63 "
                              "(A-S, Configurations)")
                    all_data['health'] = msg['HEALTH']
                elif 'HEALTH2' in msg:
                    if ((not all_data['health2'] or
                         msg['HEALTH2']['toa'] < all_data['health2']['toa'])):
                        # FIXME handle WNa rollover
                        # new data set
                        if options.progress:
                            print("Received Subframe 5 dataid 51 "
                                  "(SV Health) toa %s WNa %s" %
                                  (msg['HEALTH2']['toa'],
                                   msg['HEALTH2']['WNa']))
                        all_data['health2'] = msg['HEALTH2']
                # else nothing to do
            # else, bad packet...
        elif 'TPV' == msg['class']:
            # message is a "dictwrapper" not a dictionary
            # Python does not document what a "dictwarapper" is.
            all_data['tpv'] = dict(msg)

        if 0 < options.count:
            count += 1
            if count >= options.count:
                break

        if 0 < options.seconds:
            if time.time() > end_seconds:
                break

except KeyboardInterrupt:
    # caught control-C
    print()
    sys.exit(1)

all_sv = (list(all_data['ephemeris1'].keys()) +
          list(all_data['ephemeris2'].keys()) +
          list(all_data['ephemeris3'].keys()) +
          list(all_data['almanac'].keys()))

all_sv = sorted(set(all_sv))

for sv in all_sv:
    print("\n%s" % sv)
    print("  Subframe 1, Clock Data")
    if sv in all_data['ephemeris1']:
        _print_msg(all_data['ephemeris1'][sv], ephem1_fields)
    else:
        print("    Missing")
    print("  Subframe 2, Orbit Data")
    if sv in all_data['ephemeris2']:
        _print_msg(all_data['ephemeris2'][sv], ephem2_fields)
    else:
        print("    Missing")
    print("  Subframe 3, Orbit Data")
    if sv in all_data['ephemeris3']:
        _print_msg(all_data['ephemeris3'][sv], ephem3_fields)
    else:
        print("    Missing")
    print("  Subframe 4 page 25 %s" % sv)
    svid = "SV%d" % int(sv[2:])
    if all_data['health'] and svid in all_data['health']:
        if 0x3f == all_data['health'][svid]:
            print("       Flags  %2d (n/a)" % all_data['health'][svid])
        else:
            print("       Flags  %2d (A/S %3s Config %s)" %
                  (all_data['health'][svid],
                   'Yes' if (all_data['health'][svid] & 8) else 'No',
                   sv_conf[all_data['health'][svid] & 0x7]))
    else:
        print("    Missing")
    print("  Almanac")
    if sv in all_data['almanac']:
        _print_msg(all_data['almanac'][sv], almanac_fields)
    else:
        print("    Missing")

gps_time = None
gps_week = None
gps_tow = None

if 'time' in all_data['tpv']:
    if 'leapseconds' in all_data['tpv']:
        leapseconds = all_data['tpv']['leapseconds']
        print("\nCurrent leapseconds: %s" % all_data['tpv']['leapseconds'])
    else:
        leapseconds = 18
        print("\nCurrent leapseconds: Unknown, assuming 18")
    # convert to UNIX time
    unix_time = gps.isotime(all_data['tpv']['time'])
    # GPS Epoch starts: Jan 1980 00:00:00 UTC, Unix time: 315964800
    gps_time = unix_time - 315964800
    gps_time += leapseconds
    # 604,800 in a GPS week
    (gps_week, gps_tow) = divmod(gps_time, 604800)

    print("Current time: %s, gps time: %s, week: %s tow: %s" %
          (unix_time, gps_time, int(gps_week), gps_tow))
    if options.timeat:
        gps_time = options.timeat - 315964800
        gps_time += leapseconds
        # 604,800 in a GPS week
        (gps_week, gps_tow) = divmod(gps_time, 604800)
        print("Use time: %s, gps time: %s, week: %s tow: %s" %
              (options.timeat, gps_time, int(gps_week), gps_tow))

    if (('lat' in all_data['tpv'] and
         'lon' in all_data['tpv'] and
         'altHAE' in all_data['tpv'])):
        print("Current position: lat %.8f lon %.8f altHAE %.3f" %
              (all_data['tpv']['lat'], all_data['tpv']['lon'],
               all_data['tpv']['altHAE']))
elif 0 < options.debug:
    print("Warning: No TPV")

if gps_week and all_data['ionosphere']:
    print("\nSubframe 4, page 18")
    # IS-GPS-200, Section 20.3.3.5.2.5, says if we use
    # this data correctly L1 single frequency error gets cut in half.
    _print_msg(all_data['ionosphere'], ionosphere_fields)
    if gps_tow:
        # tUTC = tLS + A0 + A1 (tE - tot + 604800 (WN - WNt))
        deltatutc = (all_data['ionosphere']['ls'] +
                     all_data['ionosphere']['A0'] +
                     all_data['ionosphere']['A1'] *
                     (gps_tow - all_data['ionosphere']['tot'] +
                      604800 * (gps_week - all_data['ionosphere']['WNt']))
                     )
        print("  deltatutc %.9f" % deltatutc)

if all_data['health']:
    print("\nSubframe 4, page 25")
    for sv in range(1, 33):
        svid = "SV%d" % sv
        if svid in all_data['health']:
            print("  %s %2d (A/S %3s Config %s)" %
                  (svid, all_data['health'][svid],
                   'Yes' if (all_data['health'][svid] & 8) else 'No',
                   sv_conf[all_data['health'][svid] & 0x7]))
    for sv in range(25, 33):
        svid = "SVH%d" % sv
        if svid in all_data['health']:
            if 0x3f == all_data['health'][svid]:
                print("  %s %2d (n/a)" % (svid, all_data['health'][svid]))
            else:
                print("  %s %2d (Data %s, %s)" %
                      (svid, all_data['health'][svid],
                       'Bad' if (all_data['health'][svid] & 0x20) else 'OK',
                       health_str[all_data['health'][svid] & 0x1f]))

if all_data['health2']:
    print("\nSubframe 5, page 25\n  WNa %d toa %d" %
          (all_data['health2']['WNa'], all_data['health2']['toa']))
    for sv in range(1, 25):
        id = "SVH%d" % sv
        if id in all_data['health2']:
            if 0x3f == all_data['health2'][id]:
                print("  %s %2d (n/a)" % (id, all_data['health2'][id]))
            else:
                print("  %s %2d (Data %s, %s)" %
                      (id, all_data['health2'][id],
                       'Bad' if (all_data['health2'][id] & 0x20) else 'OK',
                       health_str[all_data['health2'][id] & 0x1f]))

if gps_week and options.satpos:
    for sv in all_sv:
        # Ensure we have all 3 parts, and their IODx matches.
        if ((sv in all_data['ephemeris1'] and
             sv in all_data['ephemeris2'] and
             sv in all_data['ephemeris3'] and
             all_data['ephemeris1'][sv]['IODC'] ==
             all_data['ephemeris2'][sv]['IODE'] and
             all_data['ephemeris2'][sv]['IODE'] ==
             all_data['ephemeris3'][sv]['IODE'])):

            print("\n%5s Ephemeris position:" % sv)

            # mash into one dict
            # careful, = makes a link, not a copy, on dictionaries
            # careful, update makes a link, not a copy, on dictionaries
            eph = all_data['ephemeris1'][sv].copy()
            eph.update(all_data['ephemeris2'][sv])
            eph.update(all_data['ephemeris3'][sv])

            # convert semi-circles to radians
            # use military PI, not real pi.
            cvt = ('deltan', 'i0', 'IDOT', 'M0', 'omega', 'Omega0',
                   'Omegad')
            for i in cvt:
                eph[i] *= gps.GPS_PI

            SatPosSum(eph, gps_tow)

        if sv in all_data['almanac']:
            print("\n%s Almanac position:" % sv)

            # careful, = makes a link, not a copy, on dictionaries
            ephm = all_data['almanac'][sv].copy()
            ephm.update({'af2': 0,
                         'deltan': 0,
                         'i0': ephm['deltai'] + 0.30,
                         'IDOT': 0,
                         'Tgd': 0,
                         'toc': ephm['toa'],
                         'toe': ephm['toa']})

            # convert semi-circles to radians
            # use military PI, not real pi.
            cvt = ('deltan', 'i0', 'IDOT', 'M0', 'omega', 'Omega0',
                   'Omegad')
            for i in cvt:
                ephm[i] *= gps.GPS_PI

            SatPosSum(ephm, gps_tow)

if options.savefile:
    with open(options.savefile, 'w') as save_file:
        # the JSON format will change, do not depend on it.
        # JSON turns all the int keys into strings!
        json.dump(all_data, save_file, indent=4, sort_keys=True)
