#!/usr/bin/env python3
"""
Simple NTRIP client to pull RTCM from a CORS/RTK caster and inject to a GNSS receiver.

- Connects to caster with HTTP Basic Auth (NTRIP v2 headers).
- Sends NMEA GGA immediately and every GGA_INTERVAL seconds.
- Writes RTCM bytes to a serial port (or optional TCP out).
"""

import base64
import os
import socket
import sys
import time
import threading
from datetime import datetime, timezone
from typing import Optional

try:
    import serial  # pip install pyserial
except ImportError:
    serial = None


# ========= USER SETTINGS =========
# --- Caster / NTRIP source ---
CASTER_HOST = "truertk.pointonenav.com"   # e.g. "12.34.56.78"
CASTER_PORT = 2101                      # e.g. 2101
MOUNTPOINT  = "AUTO"              # e.g. "RTCM3"
USERNAME    = "9t7fwfbm57"
PASSWORD    = "96m7bec9g8"

# --- Output to GNSS receiver ---
USE_SERIAL_OUT = False
SERIAL_PORT    = "/dev/tty.ML-NA001-250079"         # Windows: "COM5"
SERIAL_BAUD    = 115200

# Optional: forward RTCM to TCP instead of serial (set USE_SERIAL_OUT=False)
USE_TCP_OUT    = False
TCP_OUT_HOST   = "127.0.0.1"
TCP_OUT_PORT   = 2102

# --- GGA configuration ---
SEND_GGA          = True
GGA_INTERVAL_SEC  = 10         # caster-friendly: 5–15 seconds typical
# If you have a rough position, put it here (WGS84):
GGA_LAT_DEG       = 36.1140884    # positive N, negative S
GGA_LON_DEG       = -97.0880663   # positive E, negative W
GGA_ALT_M         = 390.0      # orthometric (approx OK)

# --- Misc/retry ---
RECV_BUF          = 4096
RECONNECT_DELAY_S = 5
SOCK_TIMEOUT_S    = 30
USER_AGENT        = "NTRIP pyclient/1.0"

# --- Debug ---
DEBUG_RTCM        = True   # Show RTCM message stats
PARSE_NMEA        = True   # Parse NMEA from receiver (read from serial)
USE_RECEIVER_POS  = True   # Use receiver's actual position for GGA to caster
DEBUG_ACCURACY    = True   # Show receiver accuracy info
# =================================


def nmea_checksum(sentence_no_dollar: str) -> str:
    csum = 0
    for ch in sentence_no_dollar:
        csum ^= ord(ch)
    return f"{csum:02X}"


def format_lat_lon(lat_deg: float, lon_deg: float):
    """
    Convert signed decimal degrees to NMEA ddmm.mmmm, dddmm.mmmm and hemispheres.
    """
    # Latitude
    lat_hemi = "N" if lat_deg >= 0 else "S"
    lat_abs = abs(lat_deg)
    lat_deg_i = int(lat_abs)
    lat_min = (lat_abs - lat_deg_i) * 60.0
    lat_str = f"{lat_deg_i:02d}{lat_min:07.4f}"

    # Longitude
    lon_hemi = "E" if lon_deg >= 0 else "W"
    lon_abs = abs(lon_deg)
    lon_deg_i = int(lon_abs)
    lon_min = (lon_abs - lon_deg_i) * 60.0
    lon_str = f"{lon_deg_i:03d}{lon_min:07.4f}"

    return lat_str, lat_hemi, lon_str, lon_hemi


def parse_nmea_position(lat_nmea: str, lat_dir: str, lon_nmea: str, lon_dir: str):
    """Convert NMEA ddmm.mmmm format to decimal degrees."""
    try:
        # Latitude: ddmm.mmmm
        lat_deg = int(float(lat_nmea) / 100)
        lat_min = float(lat_nmea) - (lat_deg * 100)
        lat_decimal = lat_deg + (lat_min / 60.0)
        if lat_dir == 'S':
            lat_decimal = -lat_decimal

        # Longitude: dddmm.mmmm
        lon_deg = int(float(lon_nmea) / 100)
        lon_min = float(lon_nmea) - (lon_deg * 100)
        lon_decimal = lon_deg + (lon_min / 60.0)
        if lon_dir == 'W':
            lon_decimal = -lon_decimal

        return lat_decimal, lon_decimal
    except (ValueError, ZeroDivisionError):
        return None, None


def parse_gga(sentence: str):
    """
    Parse NMEA GGA sentence.
    Returns dict with position, quality, sats, hdop, altitude.
    """
    parts = sentence.split(',')
    if len(parts) < 15:
        return None

    try:
        return {
            'time': parts[1],
            'lat': parts[2],
            'lat_dir': parts[3],
            'lon': parts[4],
            'lon_dir': parts[5],
            'quality': int(parts[6]) if parts[6] else 0,
            'num_sats': int(parts[7]) if parts[7] else 0,
            'hdop': float(parts[8]) if parts[8] else 0.0,
            'altitude': float(parts[9]) if parts[9] else 0.0,
        }
    except (ValueError, IndexError):
        return None


def build_gga(lat_deg: float, lon_deg: float, alt_m: float, fix_quality=1, sats=12, hdop=1.0) -> bytes:
    """
    Build a minimal NMEA GGA sentence (UTC time now, fix provided). Returns CRLF-terminated bytes.
    """
    now = datetime.now(timezone.utc).strftime("%H%M%S")
    lat_str, lat_hemi, lon_str, lon_hemi = format_lat_lon(lat_deg, lon_deg)

    # GGA fields:
    # $GPGGA,time,lat,NS,lon,EW,quality,numSV,HDOP,alt,M,sep,M,diffAge,diffStation
    fields = [
        "GPGGA",
        now,
        lat_str, lat_hemi,
        lon_str, lon_hemi,
        str(fix_quality),         # 1 = GPS fix, 4/5 = RTK; for caster seeding 1 is fine
        f"{sats:02d}",
        f"{hdop:.1f}",
        f"{alt_m:.1f}", "M",      # altitude + units
        "", "M",                   # geoid separation unknown
        "", "",                    # DGPS age/station
    ]
    core = ",".join(fields)
    csum = nmea_checksum(core)
    sentence = f"${core}*{csum}\r\n"
    return sentence.encode("ascii")


def make_ntrip_request(host: str, port: int, mount: str, user: str, password: str) -> bytes:
    auth = base64.b64encode(f"{user}:{password}".encode("utf-8")).decode("ascii")
    # NTRIP v2-style request. Mountpoint must be URL-encoded if it contains special chars; most are simple.
    req = (
        f"GET /{mount} HTTP/1.1\r\n"
        f"Host: {host}:{port}\r\n"
        f"Ntrip-Version: Ntrip/2.0\r\n"
        f"User-Agent: {USER_AGENT}\r\n"
        f"Connection: close\r\n"
        f"Authorization: Basic {auth}\r\n\r\n"
    )
    return req.encode("ascii")


class RTCMForwarder:
    def __init__(self):
        self.ser = None
        self.tcp_out_sock = None
        self.total_bytes = 0
        self.msg_count = 0
        self.start_time = None

        # For tracking receiver position/status
        self.latest_gga = None
        self.nmea_buffer = ""

    def open(self):
        self.start_time = time.monotonic()
        if USE_SERIAL_OUT:
            if serial is None:
                raise RuntimeError("pyserial is not installed. Install with: pip install pyserial")
            self.ser = serial.Serial(SERIAL_PORT, SERIAL_BAUD, timeout=0)
            print(f"[OUT] Serial open {SERIAL_PORT} @ {SERIAL_BAUD}")
        elif USE_TCP_OUT:
            self.tcp_out_sock = socket.create_connection((TCP_OUT_HOST, TCP_OUT_PORT), timeout=5)
            print(f"[OUT] TCP forward connected {TCP_OUT_HOST}:{TCP_OUT_PORT}")
        else:
            print("[OUT] No output configured; data will be discarded.")

    def read_nmea(self):
        """Read and parse NMEA data from the receiver (non-blocking)."""
        if not self.ser or not PARSE_NMEA:
            return

        try:
            # Read available data (non-blocking because timeout=0)
            if self.ser.in_waiting > 0:
                data = self.ser.read(self.ser.in_waiting)
                self.nmea_buffer += data.decode('ascii', errors='ignore')

                # Process complete NMEA sentences
                while '\n' in self.nmea_buffer:
                    line, self.nmea_buffer = self.nmea_buffer.split('\n', 1)
                    line = line.strip()

                    if line.startswith('$') and 'GGA' in line:
                        gga = parse_gga(line)
                        if gga and gga['quality'] > 0:
                            self.latest_gga = gga

                            if DEBUG_ACCURACY:
                                self._display_accuracy(gga)
        except Exception:
            # Don't crash on NMEA parse errors
            pass

    def _display_accuracy(self, gga):
        """Display receiver accuracy information."""
        quality_map = {
            0: "Invalid",
            1: "GPS (SPS)",
            2: "DGPS",
            3: "PPS",
            4: "RTK Fixed",
            5: "RTK Float",
            6: "Estimated",
        }

        quality_str = quality_map.get(gga['quality'], f"Unknown({gga['quality']})")

        # Calculate estimated accuracy
        uere_map = {
            4: 0.01,   # RTK Fixed: 1cm
            5: 0.3,    # RTK Float: 30cm
            2: 1.5,    # DGPS: 1.5m
            1: 5.0,    # GPS: 5m
            0: 999.0
        }
        uere = uere_map.get(gga['quality'], 10.0)
        accuracy = gga['hdop'] * uere

        # Convert position to decimal degrees
        lat_dd, lon_dd = parse_nmea_position(gga['lat'], gga['lat_dir'],
                                              gga['lon'], gga['lon_dir'])

        indicator = ""
        if gga['quality'] == 4:
            indicator = " ← RTK FIXED ✓"
        elif gga['quality'] == 5:
            indicator = " ← RTK FLOAT"

        print(f"[RX] {quality_str:12s} | Sats: {gga['num_sats']:2d} | HDOP: {gga['hdop']:4.1f} | "
              f"Acc: {accuracy:6.3f}m ({accuracy*100:5.1f}cm){indicator}")

        if lat_dd and lon_dd:
            print(f"     Position: {lat_dd:11.7f}°, {lon_dd:11.7f}° | Alt: {gga['altitude']:6.1f}m")

    def get_position(self):
        """Get the latest position from the receiver, or fallback to configured position."""
        if USE_RECEIVER_POS and self.latest_gga:
            lat_dd, lon_dd = parse_nmea_position(
                self.latest_gga['lat'], self.latest_gga['lat_dir'],
                self.latest_gga['lon'], self.latest_gga['lon_dir']
            )
            if lat_dd and lon_dd:
                return lat_dd, lon_dd, self.latest_gga['altitude']

        # Fallback to configured position
        return GGA_LAT_DEG, GGA_LON_DEG, GGA_ALT_M

    def write(self, data: bytes):
        if not data:
            return

        # Track statistics
        self.total_bytes += len(data)

        # Debug: parse and display RTCM messages
        if DEBUG_RTCM:
            self._debug_rtcm(data)

        if self.ser:
            self.ser.write(data)
        elif self.tcp_out_sock:
            try:
                self.tcp_out_sock.sendall(data)
            except Exception:
                # attempt to reconnect once
                try:
                    self.tcp_out_sock.close()
                except Exception:
                    pass
                self.tcp_out_sock = socket.create_connection((TCP_OUT_HOST, TCP_OUT_PORT), timeout=5)
                self.tcp_out_sock.sendall(data)
        # else: discard

    def _debug_rtcm(self, data: bytes):
        """Parse and display RTCM3 message info"""
        i = 0
        while i < len(data):
            # RTCM3 messages start with 0xD3
            if data[i] == 0xD3 and i + 2 < len(data):
                # Parse header: 0xD3 + 2 bytes (6 bits reserved + 10 bits length)
                length = ((data[i+1] & 0x03) << 8) | data[i+2]
                msg_total_len = 3 + length + 3  # header + payload + CRC

                if i + msg_total_len <= len(data) and length >= 3:
                    # Extract message type (first 12 bits of payload)
                    msg_type = (data[i+3] << 4) | (data[i+4] >> 4)
                    self.msg_count += 1

                    # Calculate bytes per hour
                    elapsed = time.monotonic() - self.start_time
                    if elapsed > 0:
                        bytes_per_hour = int(self.total_bytes / elapsed * 3600)
                        print(f"[RTCM] Msg #{self.msg_count}: Type {msg_type:4d}, {length:4d} bytes payload, {self.total_bytes:8d} total bytes ({bytes_per_hour:,} bytes/hour)")
                    else:
                        print(f"[RTCM] Msg #{self.msg_count}: Type {msg_type:4d}, {length:4d} bytes payload, {self.total_bytes:8d} total bytes")

                    i += msg_total_len
                    continue
            i += 1

    def close(self):
        try:
            if self.ser:
                self.ser.close()
            if self.tcp_out_sock:
                self.tcp_out_sock.close()
        except Exception:
            pass


def ntrip_loop():
    out = RTCMForwarder()
    out.open()

    while True:
        try:
            print(f"[NTRIP] Connecting to {CASTER_HOST}:{CASTER_PORT} /{MOUNTPOINT}")
            s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
            s.settimeout(SOCK_TIMEOUT_S)
            s.connect((CASTER_HOST, CASTER_PORT))

            # Send request
            s.sendall(make_ntrip_request(CASTER_HOST, CASTER_PORT, MOUNTPOINT, USERNAME, PASSWORD))

            # Read HTTP response headers
            header = b""
            while b"\r\n\r\n" not in header:
                chunk = s.recv(1)
                if not chunk:
                    raise ConnectionError("Caster closed before headers were received.")
                header += chunk

            header_text = header.decode("iso-8859-1", errors="replace")
            if "200 OK" not in header_text:
                s.close()
                raise ConnectionError(f"NTRIP error or mount not found:\n{header_text}")

            print("[NTRIP] 200 OK – streaming RTCM")

            # Thread to send periodic GGA
            stop_gga = threading.Event()

            def gga_sender():
                if not SEND_GGA:
                    return
                # Many casters accept GGA after the HTTP header via the same socket
                # (Write NMEA sentences to the socket; they are ignored by HTTP and consumed by caster)
                next_send = 0
                while not stop_gga.is_set():
                    now = time.monotonic()
                    if now >= next_send:
                        # Get position from receiver or use fallback
                        lat, lon, alt = out.get_position()
                        gga = build_gga(lat, lon, alt)
                        try:
                            s.sendall(gga)
                            if USE_RECEIVER_POS and out.latest_gga:
                                print(f"[GGA→] Sent receiver position to caster")
                            else:
                                print(f"[GGA→] Sent fallback position to caster")
                        except Exception:
                            break
                        next_send = now + GGA_INTERVAL_SEC
                    time.sleep(0.5)

            gga_thread = threading.Thread(target=gga_sender, daemon=True)
            gga_thread.start()

            # Main receive loop
            last_data_time = time.monotonic()
            while True:
                # Read NMEA from receiver (non-blocking)
                out.read_nmea()

                data = s.recv(RECV_BUF)
                if not data:
                    raise ConnectionError("Caster closed the connection.")
                last_data_time = time.monotonic()
                out.write(data)

                # Simple idle watchdog
                if time.monotonic() - last_data_time > SOCK_TIMEOUT_S:
                    raise TimeoutError("No data from caster.")

        except KeyboardInterrupt:
            print("\n[EXIT] Interrupted by user.")
            out.close()
            try:
                s.close()
            except Exception:
                pass
            sys.exit(0)
        except Exception as e:
            print(f"[WARN] {e}")
            try:
                s.close()
            except Exception:
                pass
            print(f"[NTRIP] Reconnecting in {RECONNECT_DELAY_S}s…")
            time.sleep(RECONNECT_DELAY_S)
            continue


if __name__ == "__main__":
    # Basic sanity checks
    if not CASTER_HOST or not MOUNTPOINT or not USERNAME:
        print("Please fill in CASTER_HOST, MOUNTPOINT, USERNAME, PASSWORD at the top of this script.")
        sys.exit(1)

    if USE_SERIAL_OUT is False and USE_TCP_OUT is False:
        print("No output path enabled. Set USE_SERIAL_OUT=True or USE_TCP_OUT=True.")
        # continue anyway (discards data)

    ntrip_loop()