"""FastMCP server for LC29H GNSS modules.

Exposes GNSS functionality as MCP tools for LLM integration.

Usage:
    lc29h-mcp                          # Start MCP server
    claude mcp add lc29h -- uv run lc29h-mcp  # Add to Claude Code

Environment variables:
    LC29H_PORT  Serial port path (default: /dev/ttyS0)
    LC29H_BAUD  Serial baud rate (default: 115200)
"""

import asyncio
import logging
import os
import sys
from collections.abc import AsyncIterator
from contextlib import asynccontextmanager

from fastmcp import Context, FastMCP

from . import __version__
from .commands import (
    pair_cold_start,
    pair_get_easy_status,
    pair_get_periodic_mode,
    pair_get_version,
    pair_hot_start,
    pair_save_config,
    pair_set_constellation,
    pair_set_easy,
    pair_set_fix_rate,
    pair_set_periodic_mode,
    pair_warm_start,
    pqtm_cfg_clamping,
    pqtm_cfg_dr_mode,
    pqtm_cfg_elevation_mask,
    pqtm_cfg_odometer,
    pqtm_cfg_survey_in,
    pqtm_get_dr_mode,
    pqtm_get_elevation_mask,
    pqtm_get_survey_in,
    pqtm_reset_odometer,
    pqtm_resume,
    pqtm_save_params,
    pqtm_suspend,
    pqtm_version,
)
from .models import NtripConfig
from .ntrip import NtripClient
from .parser import NMEAStream, parse_pqtm_cfgsvin
from .protocol import ACK_SUCCESS, parse_ack, parse_response
from .transport import SerialTransport

logger = logging.getLogger(__name__)


class GNSSState:
    """Shared state for the MCP server lifespan."""

    def __init__(self):
        self.transport: SerialTransport | None = None
        self.stream: NMEAStream = NMEAStream()
        self.ntrip_client: NtripClient | None = None
        self._monitor_task: asyncio.Task | None = None
        self._forward_task: asyncio.Task | None = None

    async def start_monitoring(self):
        """Background task: sole reader of the serial stream.

        Reads every NMEA line and either dispatches it to a waiting
        command query (via response watchers) or feeds it to the
        parser for position/satellite updates.
        """
        if not self.transport:
            return
        self.transport._monitor_active = True
        while True:
            try:
                line = await self.transport.read_line(timeout=2.0)
                # Check if a command query is waiting for this line
                if not self.transport.dispatch_line(line):
                    # Nobody claimed it — feed to parser
                    self.stream.parse_line(line)
            except TimeoutError:
                continue
            except Exception as e:
                logger.error("Monitor error: %s", e)
                await asyncio.sleep(1)


@asynccontextmanager
async def lifespan(server: FastMCP) -> AsyncIterator[GNSSState]:
    """Manage serial connection lifecycle."""
    state = GNSSState()
    port = os.environ.get("LC29H_PORT", "/dev/ttyS0")
    baud = int(os.environ.get("LC29H_BAUD", "115200"))

    state.transport = SerialTransport(port, baud)
    try:
        await state.transport.connect()
        # Start background NMEA monitoring
        state._monitor_task = asyncio.create_task(state.start_monitoring())
        yield state
    finally:
        if state._forward_task:
            state._forward_task.cancel()
        if state._monitor_task:
            state._monitor_task.cancel()
        if state.ntrip_client and state.ntrip_client.connected:
            await state.ntrip_client.disconnect()
        if state.transport:
            await state.transport.disconnect()


mcp = FastMCP(
    "lc29h",
    lifespan=lifespan,
)


# ---------------------------------------------------------------------------
# Position & Status Tools
# ---------------------------------------------------------------------------


@mcp.tool()
async def get_position(ctx: Context) -> dict:
    """Get the current GNSS position fix.

    Returns latitude, longitude, altitude, fix type, number of satellites,
    HDOP, speed, and course. Updates continuously from the module's
    NMEA output stream.
    """
    state: GNSSState = ctx.request_context.lifespan_context
    pos = state.stream.position
    return {
        "latitude": pos.latitude,
        "longitude": pos.longitude,
        "altitude": pos.altitude,
        "fix_type": pos.fix_type.name,
        "satellites": pos.satellites,
        "hdop": pos.hdop,
        "speed_knots": pos.speed_knots,
        "course": pos.course,
        "timestamp": pos.timestamp.isoformat() if pos.timestamp else None,
    }


@mcp.tool()
async def get_satellites(ctx: Context) -> list[dict]:
    """Get information about all tracked satellites.

    Returns PRN, elevation, azimuth, signal-to-noise ratio,
    and constellation for each satellite in view.
    """
    state: GNSSState = ctx.request_context.lifespan_context
    return [
        {
            "prn": sat.prn,
            "constellation": sat.constellation.name,
            "elevation": sat.elevation,
            "azimuth": sat.azimuth,
            "snr": sat.snr,
            "used_in_fix": sat.used_in_fix,
        }
        for sat in state.stream.satellites
    ]


@mcp.tool()
async def get_module_info(ctx: Context) -> dict:
    """Query module firmware version and model information.

    Sends both PAIR021 and PQTMVERNO commands to get complete
    hardware and software identification.
    """
    state: GNSSState = ctx.request_context.lifespan_context
    transport = state.transport
    if not transport:
        return {"error": "Not connected"}

    line = await transport.query(pair_get_version(), "$PAIR021")
    if line:
        state.stream.parse_line(line)

    line = await transport.query(pqtm_version(), "$PQTMVERNO")
    if line:
        state.stream.parse_line(line)

    mi = state.stream.module_info
    return {
        "model": mi.model,
        "firmware_version": mi.firmware_version,
        "build_date": mi.build_date,
        "build_time": mi.build_time,
        "chip_model": mi.chip_model,
        "chip_version": mi.chip_version,
    }


# ---------------------------------------------------------------------------
# Configuration Tools
# ---------------------------------------------------------------------------


@mcp.tool()
async def configure_constellations(
    ctx: Context,
    gps: bool = True,
    glonass: bool = True,
    galileo: bool = True,
    beidou: bool = True,
    qzss: bool = True,
) -> dict:
    """Configure which GNSS constellations the module uses.

    Each constellation can be individually enabled or disabled.
    At least one must remain enabled. Changes take effect immediately
    but must be saved with save_config to persist across reboots.
    """
    state: GNSSState = ctx.request_context.lifespan_context
    transport = state.transport
    if not transport:
        return {"error": "Not connected"}

    cmd = pair_set_constellation(gps, glonass, galileo, beidou, qzss)
    ack = await transport.send_command(cmd)
    if ack:
        _, result = parse_ack(ack)
        enabled = [
            n
            for n, v in [
                ("GPS", gps),
                ("GLONASS", glonass),
                ("Galileo", galileo),
                ("BeiDou", beidou),
                ("QZSS", qzss),
            ]
            if v
        ]
        return {"success": result == ACK_SUCCESS, "constellations": enabled}
    return {"success": False, "error": "No ACK received"}


@mcp.tool()
async def set_fix_rate(ctx: Context, ms: int = 1000) -> dict:
    """Set the position fix rate in milliseconds.

    Controls how often the module computes a position solution.
    Range: 100ms (10Hz) to 10000ms (0.1Hz). Default is 1000ms (1Hz).
    Higher rates use more power. Save with save_config to persist.
    """
    state: GNSSState = ctx.request_context.lifespan_context
    transport = state.transport
    if not transport:
        return {"error": "Not connected"}

    ack = await transport.send_command(pair_set_fix_rate(ms))
    if ack:
        _, result = parse_ack(ack)
        return {"success": result == ACK_SUCCESS, "fix_rate_ms": ms, "hz": 1000 / ms}
    return {"success": False, "error": "No ACK received"}


@mcp.tool()
async def save_config(ctx: Context) -> dict:
    """Save current module configuration to flash memory.

    Persists all settings (constellation, fix rate, baud rate, etc.)
    across power cycles. Without saving, changes revert on reboot.
    """
    state: GNSSState = ctx.request_context.lifespan_context
    transport = state.transport
    if not transport:
        return {"error": "Not connected"}

    await transport.write(pair_save_config())
    await transport.write(pqtm_save_params())
    return {"success": True, "message": "Configuration saved to flash"}


@mcp.tool()
async def restart_module(ctx: Context, mode: str = "hot") -> dict:
    """Restart the GNSS engine.

    Modes:
    - hot: Keep all data, fastest restart (~1s TTFF)
    - warm: Clear ephemeris, keep almanac (~30s TTFF)
    - cold: Clear all data, full satellite search (~60s TTFF)
    """
    state: GNSSState = ctx.request_context.lifespan_context
    transport = state.transport
    if not transport:
        return {"error": "Not connected"}

    cmd_map = {"hot": pair_hot_start(), "warm": pair_warm_start(), "cold": pair_cold_start()}
    if mode not in cmd_map:
        return {"error": f"Invalid mode: {mode}. Use hot, warm, or cold."}

    await transport.write(cmd_map[mode])
    return {"success": True, "mode": mode}


# ---------------------------------------------------------------------------
# NTRIP Tools
# ---------------------------------------------------------------------------


@mcp.tool()
async def ntrip_connect(
    ctx: Context,
    host: str,
    port: int = 2101,
    mountpoint: str = "",
    username: str = "",
    password: str = "",
) -> dict:
    """Connect to an NTRIP caster for RTK corrections.

    Establishes a connection to an NTRIP caster and begins streaming
    RTCM3 correction data to the GNSS module. This enables centimeter-level
    RTK positioning (requires DA or BA variant).

    If mountpoint is empty, returns the source table (list of available
    mount points) instead of connecting.
    """
    state: GNSSState = ctx.request_context.lifespan_context
    config = NtripConfig(
        host=host,
        port=port,
        mountpoint=mountpoint,
        username=username,
        password=password,
    )
    client = NtripClient(config)

    if not mountpoint:
        mounts = await client.get_source_table()
        return {
            "mount_points": [
                {"name": mp.name, "format": mp.format, "nav_system": mp.nav_system} for mp in mounts
            ]
        }

    state.ntrip_client = client
    await client.connect()

    # Start RTCM forwarding in background
    async def _forward():
        transport = state.transport
        if not transport:
            return
        async for data in client.stream_rtcm():
            await transport.write(data)

    state._forward_task = asyncio.create_task(_forward())
    return {"success": True, "host": host, "mountpoint": mountpoint}


@mcp.tool()
async def ntrip_status(ctx: Context) -> dict:
    """Check NTRIP connection status."""
    state: GNSSState = ctx.request_context.lifespan_context
    if state.ntrip_client:
        return {"connected": state.ntrip_client.connected}
    return {"connected": False}


@mcp.tool()
async def ntrip_disconnect(ctx: Context) -> dict:
    """Disconnect from the NTRIP caster."""
    state: GNSSState = ctx.request_context.lifespan_context
    if state._forward_task:
        state._forward_task.cancel()
        state._forward_task = None
    if state.ntrip_client:
        await state.ntrip_client.disconnect()
        state.ntrip_client = None
        return {"success": True}
    return {"success": False, "error": "Not connected"}


# ---------------------------------------------------------------------------
# Survey-in Tools (BS variant)
# ---------------------------------------------------------------------------


@mcp.tool()
async def survey_in_start(ctx: Context, duration_s: int = 3600, accuracy_m: float = 1.0) -> dict:
    """Start base station survey-in.

    The module averages its position over the specified duration to
    determine a precise fixed location. Only available on BS variant.

    Once complete, the module can output RTCM3 corrections for nearby
    RTK rovers.
    """
    state: GNSSState = ctx.request_context.lifespan_context
    transport = state.transport
    if not transport:
        return {"error": "Not connected"}

    cmd = pqtm_cfg_survey_in(mode=1, duration_s=duration_s, accuracy_m=accuracy_m)
    await transport.write(cmd)
    return {"success": True, "duration_s": duration_s, "accuracy_m": accuracy_m}


@mcp.tool()
async def survey_in_status(ctx: Context) -> dict:
    """Check survey-in progress.

    Returns whether survey-in is active, valid, elapsed duration,
    current accuracy estimate, and WGS84 lat/lon/alt converted from
    the mean ECEF position.
    """
    state: GNSSState = ctx.request_context.lifespan_context
    transport = state.transport
    if not transport:
        return {"error": "Not connected"}

    line = await transport.query(pqtm_get_survey_in(), "$PQTMCFGSVIN")
    if not line:
        return {"error": "No response"}

    try:
        _, fields = parse_response(line)
        status = parse_pqtm_cfgsvin(fields)
    except ValueError:
        return {"response": line}

    result = {
        "active": status.active,
        "valid": status.valid,
        "duration_s": status.duration,
        "mean_accuracy_m": status.mean_accuracy,
        "ecef_x": status.mean_x,
        "ecef_y": status.mean_y,
        "ecef_z": status.mean_z,
    }

    if status.latitude is not None:
        result["latitude"] = round(status.latitude, 8)
        result["longitude"] = round(status.longitude, 8)
        result["altitude_m"] = round(status.altitude_m, 3)

    return result


# ---------------------------------------------------------------------------
# Dead Reckoning Tools (BA/CA variants)
# ---------------------------------------------------------------------------


@mcp.tool()
async def dead_reckoning_config(
    ctx: Context,
    mode: int = -1,
) -> dict:
    """Get or set the dead reckoning (DR) mode for BA/CA module variants.

    Dead reckoning fuses GNSS with inertial sensors (accelerometer/gyroscope)
    to maintain positioning through tunnels, urban canyons, and signal gaps.

    To read the current mode, call with no arguments (mode=-1).
    To set a mode, pass one of:
      0 = GNSS only (DR disabled)
      1 = DR only (no GNSS, inertial navigation only)
      2 = GNSS+DR auto (module decides when to use DR)

    Only available on LC29H(BA) and LC29H(CA) variants. Returns an error
    response on AA/DA/BS modules.
    """
    state: GNSSState = ctx.request_context.lifespan_context
    transport = state.transport
    if not transport:
        return {"error": "Not connected"}

    if mode == -1:
        line = await transport.query(pqtm_get_dr_mode(), "$PQTMCFGDRMODE")
        if line:
            return {"response": line}
        return {"error": "No response — module may not support DR"}

    if mode not in (0, 1, 2):
        return {"error": f"Invalid mode: {mode}. Use 0 (GNSS), 1 (DR), or 2 (auto)."}

    ack = await transport.send_command(pqtm_cfg_dr_mode(mode))
    if ack:
        _, result = parse_ack(ack)
        mode_names = {0: "GNSS only", 1: "DR only", 2: "GNSS+DR auto"}
        return {"success": result == ACK_SUCCESS, "mode": mode, "description": mode_names[mode]}
    return {"success": False, "error": "No ACK received"}


# ---------------------------------------------------------------------------
# EASY / AGNSS Tools (AA variant)
# ---------------------------------------------------------------------------


@mcp.tool()
async def easy_status(ctx: Context) -> dict:
    """Query EASY (Extended Prediction Orbit) status on the AA variant.

    EASY is Quectel's self-generated ephemeris prediction system. It caches
    satellite orbit data and predicts orbits for up to 3 days, enabling faster
    time-to-first-fix (TTFF) when the module temporarily loses signal.

    Returns whether EASY is enabled and the number of extension days available
    (0-3 days of predicted orbits stored). Only works on LC29H(AA).
    """
    state: GNSSState = ctx.request_context.lifespan_context
    transport = state.transport
    if not transport:
        return {"error": "Not connected"}

    line = await transport.query(pair_get_easy_status(), "$PAIR491")
    if line:
        return {"response": line}
    return {"error": "No response — module may not support EASY (AA variant only)"}


@mcp.tool()
async def easy_enable(ctx: Context, enabled: bool = True) -> dict:
    """Enable or disable EASY ephemeris prediction on the AA variant.

    When enabled, the module caches satellite data and predicts orbits for
    up to 3 days, allowing faster TTFF after signal loss. Save with
    save_config to persist across reboots.
    """
    state: GNSSState = ctx.request_context.lifespan_context
    transport = state.transport
    if not transport:
        return {"error": "Not connected"}

    ack = await transport.send_command(pair_set_easy(enabled))
    if ack:
        _, result = parse_ack(ack)
        return {"success": result == ACK_SUCCESS, "enabled": enabled}
    return {"success": False, "error": "No ACK received"}


# ---------------------------------------------------------------------------
# Odometer Tools
# ---------------------------------------------------------------------------


@mcp.tool()
async def odometer(ctx: Context, action: str = "enable") -> dict:
    """Control the GNSS odometer.

    Actions:
      enable  — Enable distance accumulation
      disable — Disable distance accumulation
      reset   — Reset accumulated distance to zero

    The odometer tracks total distance traveled based on GNSS position fixes.
    Save with save_config to persist the enable/disable state.
    """
    state: GNSSState = ctx.request_context.lifespan_context
    transport = state.transport
    if not transport:
        return {"error": "Not connected"}

    if action == "reset":
        ack = await transport.send_command(pqtm_reset_odometer())
        if ack:
            _, result = parse_ack(ack)
            return {"success": result == ACK_SUCCESS, "action": "reset"}
        return {"success": False, "error": "No ACK received"}

    if action in ("enable", "disable"):
        enabled = action == "enable"
        ack = await transport.send_command(pqtm_cfg_odometer(enabled))
        if ack:
            _, result = parse_ack(ack)
            return {"success": result == ACK_SUCCESS, "action": action, "enabled": enabled}
        return {"success": False, "error": "No ACK received"}

    return {"error": f"Invalid action: {action}. Use enable, disable, or reset."}


# ---------------------------------------------------------------------------
# Elevation Mask Tools
# ---------------------------------------------------------------------------


@mcp.tool()
async def elevation_mask(ctx: Context, degrees: int = -1) -> dict:
    """Get or set the satellite elevation mask angle.

    Satellites below this elevation (in degrees above the horizon) are
    excluded from position computation. Higher values reduce multipath
    interference from low-elevation signals but decrease the number of
    usable satellites.

    To read the current mask, call with no arguments (degrees=-1).
    To set a mask, pass degrees (0-90, typical range 5-15).
    """
    state: GNSSState = ctx.request_context.lifespan_context
    transport = state.transport
    if not transport:
        return {"error": "Not connected"}

    if degrees == -1:
        line = await transport.query(pqtm_get_elevation_mask(), "$PQTMCFGEAMASK")
        if line:
            return {"response": line}
        return {"error": "No response"}

    if not 0 <= degrees <= 90:
        return {"error": f"Invalid elevation mask: {degrees}. Must be 0-90 degrees."}

    ack = await transport.send_command(pqtm_cfg_elevation_mask(degrees))
    if ack:
        _, result = parse_ack(ack)
        return {"success": result == ACK_SUCCESS, "elevation_mask_degrees": degrees}
    return {"success": False, "error": "No ACK received"}


# ---------------------------------------------------------------------------
# Speed Clamping Tools
# ---------------------------------------------------------------------------


@mcp.tool()
async def speed_clamping(ctx: Context, enabled: bool = True) -> dict:
    """Enable or disable static speed clamping.

    When enabled, the module reports zero speed when it detects the receiver
    is stationary. This prevents position drift from being misinterpreted as
    movement. Useful for stationary applications or when very slow speeds
    should be reported as zero.

    Save with save_config to persist across reboots.
    """
    state: GNSSState = ctx.request_context.lifespan_context
    transport = state.transport
    if not transport:
        return {"error": "Not connected"}

    ack = await transport.send_command(pqtm_cfg_clamping(enabled))
    if ack:
        _, result = parse_ack(ack)
        return {"success": result == ACK_SUCCESS, "enabled": enabled}
    return {"success": False, "error": "No ACK received"}


# ---------------------------------------------------------------------------
# GNSS Suspend/Resume Tools
# ---------------------------------------------------------------------------


@mcp.tool()
async def gnss_suspend_resume(ctx: Context, action: str = "suspend") -> dict:
    """Suspend or resume the GNSS engine.

    Actions:
      suspend — Put the GNSS engine into idle mode. The module stops
                computing fixes and tracking satellites but remains
                responsive to serial commands. Useful for saving power
                while keeping the serial link alive.
      resume  — Wake the GNSS engine from idle and resume normal operation.
                Satellite reacquisition begins immediately (similar to hot start).
    """
    state: GNSSState = ctx.request_context.lifespan_context
    transport = state.transport
    if not transport:
        return {"error": "Not connected"}

    if action == "suspend":
        await transport.write(pqtm_suspend())
        return {"success": True, "action": "suspend"}
    elif action == "resume":
        await transport.write(pqtm_resume())
        return {"success": True, "action": "resume"}
    else:
        return {"error": f"Invalid action: {action}. Use suspend or resume."}


# ---------------------------------------------------------------------------
# Periodic Power Saving Mode
# ---------------------------------------------------------------------------


@mcp.tool()
async def periodic_mode(
    ctx: Context,
    mode: int = -1,
    run_ms: int = 1000,
    sleep_ms: int = 1000,
) -> dict:
    """Get or set periodic power saving mode.

    Periodic mode alternates between GNSS tracking and sleep to reduce
    average power consumption. The module computes fixes during the run
    window, then enters low-power sleep.

    To read the current setting, call with no arguments (mode=-1).
    To configure, pass:
      mode=0 — Disabled (continuous tracking)
      mode=1 — Smart (position-aware sleep; stays awake if position is uncertain)
      mode=2 — Strict (fixed timing; always sleeps after run_ms regardless)
      run_ms  — Duration to track satellites (milliseconds)
      sleep_ms — Duration to sleep between tracking windows (milliseconds)

    Save with save_config to persist across reboots.
    """
    state: GNSSState = ctx.request_context.lifespan_context
    transport = state.transport
    if not transport:
        return {"error": "Not connected"}

    if mode == -1:
        line = await transport.query(pair_get_periodic_mode(), "$PAIR691")
        if line:
            return {"response": line}
        return {"error": "No response"}

    if mode not in (0, 1, 2):
        return {"error": f"Invalid mode: {mode}. Use 0 (disabled), 1 (smart), or 2 (strict)."}

    ack = await transport.send_command(pair_set_periodic_mode(mode, run_ms, sleep_ms))
    if ack:
        _, result = parse_ack(ack)
        mode_names = {0: "disabled", 1: "smart", 2: "strict"}
        return {
            "success": result == ACK_SUCCESS,
            "mode": mode,
            "description": mode_names[mode],
            "run_ms": run_ms,
            "sleep_ms": sleep_ms,
        }
    return {"success": False, "error": "No ACK received"}


# ---------------------------------------------------------------------------
# Resources
# ---------------------------------------------------------------------------


@mcp.resource("gnss://position")
async def position_resource(ctx: Context) -> str:
    """Live GNSS position — updated each fix cycle."""
    state: GNSSState = ctx.request_context.lifespan_context
    pos = state.stream.position
    return (
        f"Lat: {pos.latitude:.7f}, Lon: {pos.longitude:.7f}, Alt: {pos.altitude:.1f}m\n"
        f"Fix: {pos.fix_type.name}, Sats: {pos.satellites}, HDOP: {pos.hdop}\n"
        f"Speed: {pos.speed_knots:.1f} kn, Course: {pos.course:.1f} deg"
    )


@mcp.resource("gnss://satellites")
async def satellites_resource(ctx: Context) -> str:
    """Current satellite constellation view."""
    state: GNSSState = ctx.request_context.lifespan_context
    sats = state.stream.satellites
    lines = [f"Satellites in view: {len(sats)}"]
    for sat in sorted(sats, key=lambda s: (s.constellation, s.prn)):
        snr = f"{sat.snr}dB" if sat.snr else "---"
        lines.append(
            f"  {sat.constellation.name} PRN{sat.prn:3d}"
            f"  el={sat.elevation:2d}  az={sat.azimuth:3d}  {snr}"
        )
    return "\n".join(lines)


@mcp.resource("gnss://status")
async def status_resource(ctx: Context) -> str:
    """Module status including NTRIP state."""
    state: GNSSState = ctx.request_context.lifespan_context
    pos = state.stream.position
    is_connected = state.ntrip_client and state.ntrip_client.connected
    ntrip_status = "connected" if is_connected else "disconnected"
    return f"Fix: {pos.fix_type.name}\nSatellites: {pos.satellites}\nNTRIP: {ntrip_status}"


def main():
    print(f"lc29h-mcp v{__version__}", file=sys.stderr)
    mcp.run()
