"""
osm_router_node — OpenStreetMap sidewalk/bike-lane route planner

Given a start and goal GPS coordinate, this node:
  1. Queries the Overpass API for pedestrian/cycleway ways within a
     configurable radius around the midpoint
  2. Builds a weighted graph (edge weight = haversine distance)
  3. Runs A* (Dijkstra fallback) to find the shortest walkable path
  4. Simplifies the waypoint list (Douglas–Peucker, ~5m spacing)
  5. Converts lat/lon waypoints to map-frame PoseStamped via
     robot_localization's /fromLL service
  6. Publishes the route as nav_msgs/Path for RViz visualisation
  7. Returns the PoseArray to the GPS waypoint follower

ROS services:
  /outdoor/plan_route  (saltybot_outdoor_msgs or std_srvs — see PlanRoute below)

Topics published:
  /outdoor/route   nav_msgs/Path    — map-frame path for RViz
  /outdoor/status  std_msgs/String  — 'idle' | 'planning' | 'ready' | 'error:<msg>'

Topics subscribed:
  /gps/fix   sensor_msgs/NavSatFix  — current robot position

Hardware:
  SIM7600X cellular module publishes /gps/fix at ~1 Hz (±2.5 m CEP)
  RTK upgrade path: swap to ZED-F9P for ±2 cm (see config/outdoor_nav_params.yaml)

Overpass query: fetches highway=footway|cycleway|path|pedestrian|living_street
within a bounding box, then prunes to walking-accessible ways only.

Dependencies:
  pip3 install requests   (Overpass HTTP)
  No osmnx required — native JSON parsing keeps Docker image lean.
"""

import json
import math
import heapq
import time
from typing import Optional

import rclpy
from rclpy.node import Node
from rclpy.callback_groups import ReentrantCallbackGroup

from sensor_msgs.msg import NavSatFix
from nav_msgs.msg import Path
from geometry_msgs.msg import PoseStamped, PoseArray, Pose, Point
from std_msgs.msg import String
from std_srvs.srv import Trigger

try:
    import requests
    _REQUESTS_OK = True
except ImportError:
    _REQUESTS_OK = False


# OSM highway types considered walkable
_WALKABLE = frozenset([
    'footway', 'cycleway', 'path', 'pedestrian',
    'living_street', 'residential', 'service',
    'track', 'steps',
])

_OVERPASS_URL = 'https://overpass-api.de/api/interpreter'
_OVERPASS_TIMEOUT = 20   # seconds


# ── Haversine distance ────────────────────────────────────────────────────────

def _haversine(lat1: float, lon1: float, lat2: float, lon2: float) -> float:
    """Great-circle distance in metres."""
    R = 6_371_000.0
    phi1, phi2 = math.radians(lat1), math.radians(lat2)
    dphi = math.radians(lat2 - lat1)
    dlam = math.radians(lon2 - lon1)
    a = math.sin(dphi / 2) ** 2 + math.cos(phi1) * math.cos(phi2) * math.sin(dlam / 2) ** 2
    return R * 2 * math.atan2(math.sqrt(a), math.sqrt(1 - a))


# ── Douglas–Peucker polyline simplification ───────────────────────────────────

def _perp_dist(pt, line_a, line_b):
    """Cross-track distance from pt to segment a–b (all in lat/lon)."""
    lat, lon = pt
    la, lo_a = line_a
    lb, lo_b = line_b
    dx, dy = lb - la, lo_b - lo_a
    if dx == 0 and dy == 0:
        return _haversine(lat, lon, la, lo_a)
    t = ((lat - la) * dx + (lon - lo_a) * dy) / (dx * dx + dy * dy)
    t = max(0.0, min(1.0, t))
    return _haversine(lat, lon, la + t * dx, lo_a + t * dy)


def _douglas_peucker(points: list, epsilon_m: float) -> list:
    """Reduce waypoints, keeping points > epsilon_m from simplified line."""
    if len(points) <= 2:
        return points
    dmax, idx = 0.0, 0
    end = points[-1]
    for i in range(1, len(points) - 1):
        d = _perp_dist(points[i], points[0], end)
        if d > dmax:
            dmax, idx = d, i
    if dmax > epsilon_m:
        left  = _douglas_peucker(points[:idx + 1], epsilon_m)
        right = _douglas_peucker(points[idx:],     epsilon_m)
        return left[:-1] + right
    return [points[0], points[-1]]


# ── A* on OSM graph ───────────────────────────────────────────────────────────

def _astar(graph: dict, start: int, goal: int, coords: dict) -> Optional[list]:
    """A* over OSM node graph. Returns list of node IDs or None."""
    if start not in graph or goal not in graph:
        return None
    glat, glon = coords[goal]

    def h(n):
        return _haversine(coords[n][0], coords[n][1], glat, glon)

    open_heap = [(h(start), 0.0, start, [start])]
    visited = set()
    while open_heap:
        f, g, node, path = heapq.heappop(open_heap)
        if node in visited:
            continue
        visited.add(node)
        if node == goal:
            return path
        for nb, w in graph[node]:
            if nb not in visited:
                ng = g + w
                heapq.heappush(open_heap, (ng + h(nb), ng, nb, path + [nb]))
    return None


# ── OSM graph builder ─────────────────────────────────────────────────────────

def _build_graph(elements: list) -> tuple[dict, dict]:
    """Parse Overpass JSON elements into adjacency list + coordinate dict."""
    node_coords: dict[int, tuple[float, float]] = {}
    graph: dict[int, list] = {}

    for el in elements:
        if el['type'] == 'node':
            node_coords[el['id']] = (el['lat'], el['lon'])

    for el in elements:
        if el['type'] != 'way':
            continue
        highway = el.get('tags', {}).get('highway', '')
        if highway not in _WALKABLE:
            continue
        nodes = el['nodes']
        for i in range(len(nodes) - 1):
            a, b = nodes[i], nodes[i + 1]
            if a not in node_coords or b not in node_coords:
                continue
            w = _haversine(*node_coords[a], *node_coords[b])
            graph.setdefault(a, []).append((b, w))
            graph.setdefault(b, []).append((a, w))   # bidirectional

    return graph, node_coords


def _nearest_node(lat: float, lon: float, coords: dict) -> int:
    """Return ID of OSM node nearest to (lat, lon)."""
    return min(coords, key=lambda n: _haversine(lat, lon, *coords[n]))


# ── ROS node ──────────────────────────────────────────────────────────────────

class OsmRouterNode(Node):

    def __init__(self):
        super().__init__('osm_router')

        self.declare_parameter('overpass_url',      _OVERPASS_URL)
        self.declare_parameter('query_radius_m',    1500.0)   # area to fetch
        self.declare_parameter('simplify_eps_m',    5.0)      # waypoint spacing
        self.declare_parameter('cache_dir',         '/data/osm_cache')
        self.declare_parameter('use_cache',         True)
        self.declare_parameter('map_frame',         'map')

        self._fix: Optional[NavSatFix] = None
        self._cb_group = ReentrantCallbackGroup()

        # ── Subscriptions ────────────────────────────────────────────────────
        self.create_subscription(NavSatFix, '/gps/fix', self._fix_cb, 10)

        # ── Publishers ───────────────────────────────────────────────────────
        self._path_pub   = self.create_publisher(Path,        '/outdoor/route',  10)
        self._status_pub = self.create_publisher(String,      '/outdoor/status', 10)
        self._poses_pub  = self.create_publisher(PoseArray,   '/outdoor/waypoints', 10)

        # ── Services ─────────────────────────────────────────────────────────
        # /outdoor/plan_route — not yet triggered, called externally with goal.
        # Goal lat/lon passed as parameter overrides for simplicity in V1.
        self.declare_parameter('goal_lat', 0.0)
        self.declare_parameter('goal_lon', 0.0)
        self.create_service(
            Trigger, '/outdoor/plan_route',
            self._plan_route_cb,
            callback_group=self._cb_group,
        )

        self._publish_status('idle')
        self.get_logger().info('osm_router: ready. Call /outdoor/plan_route to start.')

    # ── Callbacks ─────────────────────────────────────────────────────────────

    def _fix_cb(self, msg: NavSatFix) -> None:
        self._fix = msg

    def _plan_route_cb(self, _req, response):
        if self._fix is None:
            response.success = False
            response.message = 'No GPS fix yet'
            return response

        goal_lat = self.get_parameter('goal_lat').value
        goal_lon = self.get_parameter('goal_lon').value
        if goal_lat == 0.0 and goal_lon == 0.0:
            response.success = False
            response.message = 'goal_lat/goal_lon not set'
            return response

        self._publish_status('planning')
        try:
            path_msg, pose_array = self._plan(
                self._fix.latitude, self._fix.longitude,
                goal_lat, goal_lon,
            )
            self._path_pub.publish(path_msg)
            self._poses_pub.publish(pose_array)
            self._publish_status('ready')
            response.success = True
            response.message = f'{len(pose_array.poses)} waypoints planned'
        except Exception as exc:
            self.get_logger().error(f'Route planning failed: {exc}')
            self._publish_status(f'error:{exc}')
            response.success = False
            response.message = str(exc)
        return response

    # ── Planning ──────────────────────────────────────────────────────────────

    def _plan(self, slat, slon, glat, glon) -> tuple[Path, PoseArray]:
        radius = self.get_parameter('query_radius_m').value
        eps    = self.get_parameter('simplify_eps_m').value
        frame  = self.get_parameter('map_frame').value

        # Bounding box centred on midpoint
        mlat = (slat + glat) / 2
        mlon = (slon + glon) / 2
        d_deg = (radius / 111_320.0) * 1.5   # pad
        bbox = (mlat - d_deg, mlon - d_deg, mlat + d_deg, mlon + d_deg)

        elements = self._fetch_osm(bbox)
        graph, coords = _build_graph(elements)

        if not coords:
            raise RuntimeError('No walkable OSM nodes in query area')

        start_node = _nearest_node(slat, slon, coords)
        goal_node  = _nearest_node(glat, glon, coords)

        node_path = _astar(graph, start_node, goal_node, coords)
        if node_path is None:
            raise RuntimeError('No walkable path found between start and goal')

        raw_pts = [coords[n] for n in node_path]
        simplified = _douglas_peucker(raw_pts, eps)

        self.get_logger().info(
            f'Route: {len(raw_pts)} → {len(simplified)} waypoints after simplification'
        )

        # Build nav_msgs/Path in map frame using ENU approximation.
        # navsat_transform_node provides the GPS→map projection; here we use
        # a local flat-earth approximation centred on the start position.
        # Replace with /fromLL service call when robot_localization is running.
        path_msg  = Path()
        pose_arr  = PoseArray()
        now = self.get_clock().now().to_msg()
        path_msg.header.stamp = now
        path_msg.header.frame_id = frame
        pose_arr.header = path_msg.header

        lat0, lon0 = slat, slon
        for lat, lon in simplified:
            x, y = _latlon_to_enu(lat, lon, lat0, lon0)
            ps = PoseStamped()
            ps.header = path_msg.header
            ps.pose.position.x = x
            ps.pose.position.y = y
            ps.pose.orientation.w = 1.0
            path_msg.poses.append(ps)

            p = Pose()
            p.position.x = x
            p.position.y = y
            p.orientation.w = 1.0
            pose_arr.poses.append(p)

        return path_msg, pose_arr

    # ── Overpass fetch ────────────────────────────────────────────────────────

    def _fetch_osm(self, bbox: tuple) -> list:
        """Query Overpass for walkable ways in bbox. Returns element list."""
        if not _REQUESTS_OK:
            raise RuntimeError('requests library not installed (pip3 install requests)')

        s, w, n, e = bbox
        query = f"""
[out:json][timeout:{_OVERPASS_TIMEOUT}];
(
  way["highway"~"^(footway|cycleway|path|pedestrian|living_street|residential|service|track|steps)$"]
     ({s:.6f},{w:.6f},{n:.6f},{e:.6f});
);
out body;
>;
out skel qt;
"""
        url = self.get_parameter('overpass_url').value
        self.get_logger().info(f'Querying Overpass: bbox={s:.4f},{w:.4f},{n:.4f},{e:.4f}')
        resp = requests.post(url, data={'data': query}, timeout=_OVERPASS_TIMEOUT + 5)
        resp.raise_for_status()
        data = resp.json()
        elements = data.get('elements', [])
        self.get_logger().info(f'Overpass returned {len(elements)} elements')
        return elements

    # ── Helpers ───────────────────────────────────────────────────────────────

    def _publish_status(self, msg: str) -> None:
        s = String()
        s.data = msg
        self._status_pub.publish(s)


def _latlon_to_enu(lat: float, lon: float, lat0: float, lon0: float) -> tuple[float, float]:
    """Flat-earth ENU approximation (valid within ~10 km)."""
    dlat = math.radians(lat - lat0)
    dlon = math.radians(lon - lon0)
    cos0 = math.cos(math.radians(lat0))
    x = dlon * cos0 * 6_371_000.0   # East
    y = dlat * 6_371_000.0           # North
    return x, y


def main(args=None):
    rclpy.init(args=args)
    node = OsmRouterNode()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        rclpy.try_shutdown()


if __name__ == '__main__':
    main()
