"""
path_edges_node.py — ROS2 node for lane/path edge detection (Issue #339).

Subscribes
----------
  /camera/color/image_raw  (sensor_msgs/Image)

Publishes
---------
  /saltybot/path_edges  (saltybot_scene_msgs/PathEdges)

Parameters
----------
  roi_frac        (float,  default 0.50)  bottom fraction of image as ROI
  blur_ksize      (int,    default 5)     Gaussian blur kernel size (odd)
  canny_low       (int,    default 50)    Canny lower threshold
  canny_high      (int,    default 150)   Canny upper threshold
  hough_threshold (int,    default 30)    minimum Hough votes
  min_line_len    (int,    default 40)    minimum Hough segment length (px)
  max_line_gap    (int,    default 20)    maximum Hough gap (px)
  min_slope       (float,  default 0.3)   |slope| below this → discard
  birdseye_size   (int,    default 400)   bird-eye square image side (px)
"""

from __future__ import annotations

import cv2
import numpy as np

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image
from std_msgs.msg import Header

from saltybot_scene_msgs.msg import PathEdges

from ._path_edges import PathEdgeConfig, process_frame


class PathEdgesNode(Node):
    def __init__(self) -> None:
        super().__init__('path_edges_node')

        # Declare parameters
        self.declare_parameter('roi_frac',        0.50)
        self.declare_parameter('blur_ksize',      5)
        self.declare_parameter('canny_low',       50)
        self.declare_parameter('canny_high',      150)
        self.declare_parameter('hough_threshold', 30)
        self.declare_parameter('min_line_len',    40)
        self.declare_parameter('max_line_gap',    20)
        self.declare_parameter('min_slope',       0.3)
        self.declare_parameter('birdseye_size',   400)

        self._sub = self.create_subscription(
            Image,
            '/camera/color/image_raw',
            self._image_cb,
            10,
        )
        self._pub = self.create_publisher(PathEdges, '/saltybot/path_edges', 10)

        self.get_logger().info('PathEdgesNode ready — subscribing /camera/color/image_raw')

    # ------------------------------------------------------------------

    def _build_config(self) -> PathEdgeConfig:
        p = self.get_parameter
        return PathEdgeConfig(
            roi_frac        = p('roi_frac').value,
            blur_ksize      = p('blur_ksize').value,
            canny_low       = p('canny_low').value,
            canny_high      = p('canny_high').value,
            hough_threshold = p('hough_threshold').value,
            min_line_len    = p('min_line_len').value,
            max_line_gap    = p('max_line_gap').value,
            min_slope       = p('min_slope').value,
            birdseye_size   = p('birdseye_size').value,
        )

    def _image_cb(self, msg: Image) -> None:
        # Convert ROS Image → BGR numpy array
        bgr = self._ros_image_to_bgr(msg)
        if bgr is None:
            return

        cfg    = self._build_config()
        result = process_frame(bgr, cfg)

        out              = PathEdges()
        out.header       = msg.header
        out.line_count   = len(result.lines)
        out.roi_top      = result.roi_top

        # Flat float32 arrays: [x1,y1,x2,y2, x1,y1,x2,y2, ...]
        out.segments_px = [v for seg in result.lines for v in seg]
        out.segments_birdseye_px = [v for seg in result.birdseye_lines for v in seg]

        # Left edge
        if result.left_edge is not None:
            x1, y1, x2, y2 = result.left_edge
            out.left_x1, out.left_y1 = float(x1), float(y1)
            out.left_x2, out.left_y2 = float(x2), float(y2)
            out.left_detected = True
        else:
            out.left_detected = False

        if result.birdseye_left is not None:
            bx1, by1, bx2, by2 = result.birdseye_left
            out.left_birdseye_x1, out.left_birdseye_y1 = float(bx1), float(by1)
            out.left_birdseye_x2, out.left_birdseye_y2 = float(bx2), float(by2)

        # Right edge
        if result.right_edge is not None:
            x1, y1, x2, y2 = result.right_edge
            out.right_x1, out.right_y1 = float(x1), float(y1)
            out.right_x2, out.right_y2 = float(x2), float(y2)
            out.right_detected = True
        else:
            out.right_detected = False

        if result.birdseye_right is not None:
            bx1, by1, bx2, by2 = result.birdseye_right
            out.right_birdseye_x1, out.right_birdseye_y1 = float(bx1), float(by1)
            out.right_birdseye_x2, out.right_birdseye_y2 = float(bx2), float(by2)

        self._pub.publish(out)

    @staticmethod
    def _ros_image_to_bgr(msg: Image) -> np.ndarray | None:
        """Convert a sensor_msgs/Image to a uint8 BGR numpy array."""
        enc = msg.encoding.lower()
        data = np.frombuffer(msg.data, dtype=np.uint8)

        if enc in ('rgb8', 'bgr8', 'mono8'):
            channels = 1 if enc == 'mono8' else 3
            try:
                img = data.reshape((msg.height, msg.width, channels))
            except ValueError:
                return None
            if enc == 'rgb8':
                img = cv2.cvtColor(img, cv2.COLOR_RGB2BGR)
            elif enc == 'mono8':
                img = cv2.cvtColor(img, cv2.COLOR_GRAY2BGR)
            return img

        # Unsupported encoding
        return None


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


if __name__ == '__main__':
    main()
