#!/usr/bin/python3 -u
# Publish gpsd fixes as OwnTracks location messages over MQTT.
# Ported from gps_to_owntracks.py (wayback era): gpsdclient -> python3-gps
# (apt-only deps), paho 1.x -> 2.x callback API, hard-coded broker secrets ->
# /etc/van-gps/config.json. Exits on MQTT disconnect/DNS failure by design —
# systemd Restart=always reconnects with a fresh session.

import sys
import json
import signal
import os
import time
import socket

import gps as gpsd
import paho.mqtt.client as mqtt
from paho.mqtt.properties import Properties
from paho.mqtt.packettypes import PacketTypes

from datetime import datetime
from math import radians, sin, cos, acos

debug = False           # toggle at runtime with SIGHUP
config_path = sys.argv[1] if len(sys.argv) > 1 else "/etc/van-gps/config.json"
with open(config_path) as f:
    cfg = json.load(f)

version = cfg.get("mqtt_version", "3")          # '3' or '5'
mytransport = "tcp"                             # or 'websockets'
broker = cfg["broker"]
myport = cfg.get("port", 1883)
mq_user = cfg["username"]
mq_pw = cfg["password"]
mq_id = cfg.get("client_id", "vanq-wan")
gpsd_host = cfg.get("gpsd_host", "127.0.0.1")
idle_delay = cfg.get("idle_delay_s", 600)       # seconds between messages when not moving
keep_alive = idle_delay + 60
distance_move_trigger = cfg.get("move_trigger_m", 250)  # meters of movement that triggers a message

mytopic = cfg.get("topic", "owntracks/rv/gps")
tid = cfg.get("tid", "rv")


def handler(signum, frame):
    global debug
    debug = not debug
    print('debug is now', debug)


def great_circle(llon1, llat1, llon2, llat2):
    lon1, lat1, lon2, lat2 = map(radians, [llon1, llat1, llon2, llat2])

    a = sin(lat1) * sin(lat2) + cos(lat1) * cos(lat2) * cos(lon1 - lon2)
    if a >= 1.0:
        return 0
    try:
        res = int(6371000 * (acos(a)))
    except ValueError as e:
        print("gc error: ", e)
        print(" coords: ", llon1, llat1, llon2, llat2)
        return 0
    return res


def on_disconnect(client, userdata, flags, reason_code, properties):
    print("mqtt disconnect:", reason_code)
    os._exit(1)


def connect():
    if version == '5':
        properties = Properties(PacketTypes.CONNECT)
        properties.SessionExpiryInterval = 30 * 60  # in seconds

        client.connect(broker,
                       port=myport,
                       clean_start=mqtt.MQTT_CLEAN_START_FIRST_ONLY,
                       properties=properties,
                       keepalive=keep_alive)
    elif version == '3':
        client.connect(broker, port=myport, keepalive=keep_alive)
    print("mqtt connect")


#
# Main
#

print("van-gps-owntracks start", time.asctime(), debug, os.getpid())

signal.signal(signal.SIGHUP, handler)

if version == '5':
    client = mqtt.Client(mqtt.CallbackAPIVersion.VERSION2,
                         client_id=mq_id,
                         transport=mytransport,
                         protocol=mqtt.MQTTv5)
if version == '3':
    client = mqtt.Client(mqtt.CallbackAPIVersion.VERSION2,
                         client_id=mq_id,
                         transport=mytransport,
                         protocol=mqtt.MQTTv311,
                         clean_session=True)

client.username_pw_set(mq_user, mq_pw)
client.on_disconnect = on_disconnect

try:
    connect()
except socket.gaierror as e:
    print("could not connect mqtt, %s" % e)
    sys.exit(1)
client.loop_start()

pubproperties = Properties(PacketTypes.PUBLISH)
pubproperties.MessageExpiryInterval = keep_alive  # in seconds; MQTT5 only

session = gpsd.gps(host=gpsd_host)
session.stream(gpsd.WATCH_ENABLE)
lastmsg = {'tst': 0, 'tid': tid, 'lat': 0, 'lon': 0}
while session.read() == 0:
    result = session.data
    if not result or result.get('class') != 'TPV':
        continue
    msg = {'_type': 'location', 'tid': tid, 't': 'p'}

    if result.get('mode', 0) <= 1:
        continue
    if not ('lat' in result and 'lon' in result):
        continue
    msg['lat'] = result['lat']
    msg['lon'] = result['lon']
    if 'alt' in result:
        msg['alt'] = int(result['alt'])
    if 'speed' in result:
        msg['vel'] = int(result['speed']) * 3.6
    if 'eph' in result:
        msg['acc'] = int(result['eph'])
    if 'track' in result:
        msg['cog'] = int(result['track'])
    else:
        continue
    if 'time' not in result:
        continue
    msg['tst'] = int(datetime.fromisoformat(result['time']).timestamp())
    jmsg = json.dumps(msg)
    if lastmsg != msg:
        gc = great_circle(lastmsg['lat'], lastmsg['lon'], msg['lat'], msg['lon'])
        if debug:
            print("\rgc is ", gc, ' age is ', msg['tst'] - lastmsg['tst'], "   ", end="")
        if msg['tst'] - lastmsg['tst'] >= idle_delay or gc > distance_move_trigger:
            if debug:
                print('* ', gc, jmsg)
                print('* ', mytopic, pubproperties)
            rc = client.publish(mytopic, jmsg, qos=1, retain=True, properties=pubproperties)
            if debug:
                print("*  client.publish rc=", rc)
            lastmsg = msg

print("gpsd stream ended", file=sys.stderr)
sys.exit(1)