#!/usr/bin/env python3
"""
Debug RTCM 1005/1006 message parsing with detailed bit extraction.
"""
import base64
import socket
import time
from datetime import datetime
import math


# NTRIP configuration
CASTER_HOST = "truertk.pointonenav.com"
CASTER_PORT = 2101
MOUNTPOINT = "AUTO"
USERNAME = "9t7fwfbm57"
PASSWORD = "96m7bec9g8"

LAT = 36.1140884
LON = -97.0880663
ALT = 390.0


def build_gga(lat: float, lon: float, alt: float) -> bytes:
    """Build NMEA GGA sentence."""
    now_utc = datetime.utcnow().strftime("%H%M%S")
    lat_hemi = "N" if lat >= 0 else "S"
    lat_abs = abs(lat)
    lat_deg = int(lat_abs)
    lat_min = (lat_abs - lat_deg) * 60.0
    lat_str = f"{lat_deg:02d}{lat_min:07.4f}"

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

    fields = ["GPGGA", now_utc, lat_str, lat_hemi, lon_str, lon_hemi, "1", "12", "1.0", f"{alt:.1f}", "M", "", "M", "", ""]
    core = ",".join(fields)
    checksum = 0
    for char in core:
        checksum ^= ord(char)
    return f"${core}*{checksum:02X}\r\n".encode("ascii")


def make_ntrip_request(host: str, port: int, mount: str, user: str, password: str) -> bytes:
    """Create NTRIP v2 HTTP request."""
    auth = base64.b64encode(f"{user}:{password}".encode("utf-8")).decode("ascii")
    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: Debug-1005/1.0\r\n"
        f"Connection: close\r\n"
        f"Authorization: Basic {auth}\r\n\r\n"
    )
    return req.encode("ascii")


def parse_chunked_data(data: bytes) -> bytes:
    """Parse HTTP chunked encoding."""
    clean_data = bytearray()
    i = 0

    while i < len(data):
        line_end = data.find(b'\r\n', i)
        if line_end == -1:
            break

        chunk_size_line = data[i:line_end]
        try:
            chunk_size_str = chunk_size_line.decode('ascii', errors='ignore').split(';')[0].strip()
            chunk_size = int(chunk_size_str, 16)

            if chunk_size == 0:
                break

            chunk_data_start = line_end + 2
            chunk_data_end = chunk_data_start + chunk_size

            if chunk_data_end + 2 > len(data):
                break

            chunk_data = data[chunk_data_start:chunk_data_end]
            clean_data.extend(chunk_data)
            i = chunk_data_end + 2

        except (ValueError, UnicodeDecodeError):
            i += 1
            continue

    return bytes(clean_data)


def debug_parse_1005(payload: bytes):
    """Debug parse RTCM 1005 with detailed output."""
    print(f"\n{'=' * 80}")
    print(f"RTCM 1005 MESSAGE DEBUG")
    print(f"{'=' * 80}")
    print(f"Payload length: {len(payload)} bytes")
    print(f"Hex: {payload[:50].hex()}")
    print()

    # Show as bits
    bits_str = ''.join(f'{b:08b}' for b in payload[:25])
    print("First 200 bits:")
    for i in range(0, min(200, len(bits_str)), 50):
        print(f"  {i:3d}: {bits_str[i:i+50]}")
    print()

    # Parse fields manually
    bit_array = []
    for byte in payload:
        for i in range(7, -1, -1):
            bit_array.append((byte >> i) & 1)

    def get_bits(start, length):
        """Extract bits from array."""
        value = 0
        for i in range(length):
            value = (value << 1) | bit_array[start + i]
        return value

    def get_signed_bits(start, length):
        """Extract signed value."""
        value = get_bits(start, length)
        if value & (1 << (length - 1)):
            value -= (1 << length)
        return value

    pos = 0

    # Message Number (12 bits)
    msg_num = get_bits(pos, 12)
    print(f"Bit {pos:3d}-{pos+11:3d} (12 bits): Message Number = {msg_num}")
    pos += 12

    # Station ID (12 bits)
    station_id = get_bits(pos, 12)
    print(f"Bit {pos:3d}-{pos+11:3d} (12 bits): Station ID = {station_id}")
    pos += 12

    # ITRF Year (6 bits)
    itrf = get_bits(pos, 6)
    print(f"Bit {pos:3d}-{pos+5:3d}  (6 bits):  ITRF Year = {itrf} ({1980 + itrf if itrf else 'N/A'})")
    pos += 6

    # GPS (1), GLONASS (1), Galileo (1), Ref Station (1)
    gps = get_bits(pos, 1)
    pos += 1
    glonass = get_bits(pos, 1)
    pos += 1
    galileo = get_bits(pos, 1)
    pos += 1
    ref_station = get_bits(pos, 1)
    pos += 1
    print(f"Indicators: GPS={gps}, GLONASS={glonass}, Galileo={galileo}, RefStation={ref_station}")

    # ECEF-X (38 bits, signed, 0.0001m resolution)
    ecef_x_raw = get_signed_bits(pos, 38)
    ecef_x = ecef_x_raw * 0.0001
    print(f"Bit {pos:3d}-{pos+37:3d} (38 bits): ECEF-X = {ecef_x_raw} raw = {ecef_x:.4f} m")
    pos += 38

    # Single Receiver Oscillator (1 bit)
    oscillator = get_bits(pos, 1)
    pos += 1

    # Reserved (1 bit)
    pos += 1

    # ECEF-Y (38 bits, signed)
    ecef_y_raw = get_signed_bits(pos, 38)
    ecef_y = ecef_y_raw * 0.0001
    print(f"Bit {pos:3d}-{pos+37:3d} (38 bits): ECEF-Y = {ecef_y_raw} raw = {ecef_y:.4f} m")
    pos += 38

    # Quarter Cycle Indicator (2 bits)
    quarter = get_bits(pos, 2)
    pos += 2

    # ECEF-Z (38 bits, signed)
    ecef_z_raw = get_signed_bits(pos, 38)
    ecef_z = ecef_z_raw * 0.0001
    print(f"Bit {pos:3d}-{pos+37:3d} (38 bits): ECEF-Z = {ecef_z_raw} raw = {ecef_z:.4f} m")
    pos += 38

    print(f"\nTotal bits parsed: {pos}")
    print()

    # Convert ECEF to LLA
    print(f"ECEF Coordinates:")
    print(f"  X: {ecef_x:14.4f} m")
    print(f"  Y: {ecef_y:14.4f} m")
    print(f"  Z: {ecef_z:14.4f} m")
    print()

    # WGS84 conversion
    a = 6378137.0
    e2 = 6.69437999014e-3

    lon_rad = math.atan2(ecef_y, ecef_x)
    p = math.sqrt(ecef_x * ecef_x + ecef_y * ecef_y)
    lat_rad = math.atan2(ecef_z, p * (1 - e2))

    for _ in range(10):
        N = a / math.sqrt(1 - e2 * math.sin(lat_rad) ** 2)
        alt = p / math.cos(lat_rad) - N
        lat_new = math.atan2(ecef_z, p * (1 - e2 * N / (N + alt)))
        if abs(lat_new - lat_rad) < 1e-12:
            break
        lat_rad = lat_new

    N = a / math.sqrt(1 - e2 * math.sin(lat_rad) ** 2)
    alt = p / math.cos(lat_rad) - N if abs(math.cos(lat_rad)) > 1e-10 else ecef_z / math.sin(lat_rad) - N * (1 - e2)

    lat = math.degrees(lat_rad)
    lon = math.degrees(lon_rad)

    print(f"Geodetic Coordinates (WGS84):")
    print(f"  Latitude:  {lat:12.7f}°")
    print(f"  Longitude: {lon:12.7f}°")
    print(f"  Altitude:  {alt:12.2f} m")
    print()

    # Distance from rover
    rover_lat = 36.1140884
    rover_lon = -97.0880663

    lat1_rad = math.radians(rover_lat)
    lat2_rad = math.radians(lat)
    delta_lat = math.radians(lat - rover_lat)
    delta_lon = math.radians(lon - rover_lon)
    a_hav = math.sin(delta_lat / 2) ** 2 + math.cos(lat1_rad) * math.cos(lat2_rad) * math.sin(delta_lon / 2) ** 2
    distance = 2 * 6371008.8 * math.asin(min(1.0, math.sqrt(a_hav)))

    print(f"Distance from rover ({rover_lat:.4f}°, {rover_lon:.4f}°):")
    print(f"  {distance:.2f} m ({distance/1000:.3f} km)")


def main():
    print("RTCM 1005/1006 Debug Tool")
    print("Connecting to NTRIP caster...")

    try:
        sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        sock.settimeout(30)
        sock.connect((CASTER_HOST, CASTER_PORT))
        sock.sendall(make_ntrip_request(CASTER_HOST, CASTER_PORT, MOUNTPOINT, USERNAME, PASSWORD))

        # Read headers
        header = b""
        while b"\r\n\r\n" not in header:
            chunk = sock.recv(1)
            if not chunk:
                print("ERROR: Connection closed")
                return
            header += chunk

        if "200 OK" not in header.decode("iso-8859-1", errors="replace"):
            print("ERROR: Connection failed")
            return

        print("Connected! Searching for RTCM 1005 or 1006 message...\n")

        last_gga = 0
        found_count = 0

        while found_count < 3:
            now = time.monotonic()
            if now - last_gga >= 10:
                sock.sendall(build_gga(LAT, LON, ALT))
                last_gga = now

            raw_data = sock.recv(4096)
            if not raw_data:
                break

            # Parse chunks
            data = parse_chunked_data(raw_data)

            # Find RTCM messages
            i = 0
            while i < len(data):
                if data[i] == 0xD3 and i + 2 < len(data):
                    length = ((data[i+1] & 0x03) << 8) | data[i+2]
                    msg_total_len = 3 + length + 3

                    if i + msg_total_len <= len(data) and length >= 3:
                        payload = data[i+3:i+3+length]
                        msg_type = (payload[0] << 4) | (payload[1] >> 4)

                        if msg_type in [1005, 1006]:
                            found_count += 1
                            print(f"\n{'#' * 80}")
                            print(f"FOUND MESSAGE {msg_type} (#{found_count})")
                            print(f"{'#' * 80}")
                            debug_parse_1005(payload)

                        i += msg_total_len
                        continue
                i += 1

    except KeyboardInterrupt:
        print("\nInterrupted")
    except Exception as e:
        print(f"ERROR: {e}")
        import traceback
        traceback.print_exc()
    finally:
        if 'sock' in locals():
            sock.close()


if __name__ == "__main__":
    main()