#!/usr/bin/env python3
"""
Remote monitoring dashboard for SaltyBot — web UI on Orin port 8080.

Displays:
- Battery %, current state
- Sensor health (audio, vision, lidar)
- Map view (occupancy grid)
- Event log (last 20 from event logger #473)
- Motor speeds
- WiFi signal strength

WebSocket updates push data in real-time to connected clients.
"""

import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy
from std_msgs.msg import String, Float32
from sensor_msgs.msg import Image, BatteryState
from nav_msgs.msg import OccupancyGrid
import json
import time
from collections import deque
from threading import Thread
from flask import Flask, render_template, jsonify
from flask_socketio import SocketIO, emit, disconnect

_SENSOR_QOS = QoSProfile(
    reliability=ReliabilityPolicy.BEST_EFFORT,
    history=HistoryPolicy.KEEP_LAST,
    depth=5,
)


class DashboardNode(Node):
    def __init__(self):
        super().__init__('dashboard_node')

        # State
        self.battery_percent = 100.0
        self.battery_voltage = 0.0
        self.robot_state = "IDLE"
        self.motor_speeds = {"left": 0.0, "right": 0.0}
        self.wifi_signal = -50
        self.sensor_health = {
            "camera": True,
            "lidar": True,
            "audio": True,
            "imu": True,
        }
        self.event_log = deque(maxlen=20)
        self.map_data = None
        self.last_update = time.time()

        # Global Flask app reference for socketio
        global socketio_instance
        socketio_instance = None

        # Subscribers
        self.create_subscription(Float32, '/saltybot/battery_percent', self.battery_callback, 10)
        self.create_subscription(BatteryState, '/saltybot/battery_state', self.battery_state_callback, 10)
        self.create_subscription(String, '/saltybot/telemetry/motor_rpm', self.motor_callback, 10)
        self.create_subscription(String, '/saltybot/state', self.state_callback, 10)
        self.create_subscription(String, '/saltybot/events', self.event_callback, 10)
        self.create_subscription(OccupancyGrid, '/map', self.map_callback, _SENSOR_QOS)
        self.create_subscription(String, '/saltybot/sensor_health', self.sensor_health_callback, 10)

        # Update timer for periodic broadcasts
        self.timer = self.create_timer(0.5, self.broadcast_update)

        self.get_logger().info("Dashboard node initialized on port 8080")

    def battery_callback(self, msg):
        self.battery_percent = msg.data

    def battery_state_callback(self, msg):
        if hasattr(msg, 'voltage'):
            self.battery_voltage = msg.voltage

    def motor_callback(self, msg):
        try:
            data = json.loads(msg.data)
            if 'left' in data and 'right' in data:
                self.motor_speeds = {"left": float(data['left']), "right": float(data['right'])}
        except (json.JSONDecodeError, ValueError, KeyError):
            pass

    def state_callback(self, msg):
        self.robot_state = msg.data

    def event_callback(self, msg):
        try:
            event_data = json.loads(msg.data)
            event_data['timestamp'] = time.time()
            self.event_log.append(event_data)
        except json.JSONDecodeError:
            self.event_log.append({'message': msg.data, 'timestamp': time.time()})

    def sensor_health_callback(self, msg):
        try:
            health = json.loads(msg.data)
            self.sensor_health.update(health)
        except json.JSONDecodeError:
            pass

    def map_callback(self, msg):
        self.map_data = {
            'width': msg.info.width,
            'height': msg.info.height,
            'resolution': msg.info.resolution,
            'origin_x': msg.info.origin.position.x,
            'origin_y': msg.info.origin.position.y,
        }

    def broadcast_update(self):
        """Broadcast dashboard state to all connected clients."""
        if socketio_instance is None:
            return

        data = {
            'battery_percent': float(self.battery_percent),
            'battery_voltage': float(self.battery_voltage),
            'robot_state': self.robot_state,
            'motor_speeds': self.motor_speeds,
            'wifi_signal': self.wifi_signal,
            'sensor_health': self.sensor_health,
            'event_log': list(self.event_log),
            'map_data': self.map_data,
            'timestamp': time.time(),
        }

        try:
            socketio_instance.emit('dashboard_update', data, broadcast=True)
        except Exception as e:
            self.get_logger().warn(f"Failed to broadcast: {e}")


def create_app(dashboard_node):
    """Create Flask app with dashboard routes."""
    app = Flask(__name__, template_folder='templates', static_folder='static')
    app.config['SECRET_KEY'] = 'saltybot-dashboard'

    socketio = SocketIO(app, cors_allowed_origins="*")
    global socketio_instance
    socketio_instance = socketio

    @app.route('/')
    def index():
        return render_template('dashboard.html')

    @app.route('/api/status')
    def api_status():
        return jsonify({
            'battery_percent': dashboard_node.battery_percent,
            'battery_voltage': dashboard_node.battery_voltage,
            'robot_state': dashboard_node.robot_state,
            'motor_speeds': dashboard_node.motor_speeds,
            'wifi_signal': dashboard_node.wifi_signal,
            'sensor_health': dashboard_node.sensor_health,
            'event_log': list(dashboard_node.event_log),
            'map_data': dashboard_node.map_data,
        })

    @socketio.on('connect')
    def handle_connect():
        emit('response', {'data': 'Connected to SaltyBot dashboard'})

    @socketio.on('disconnect')
    def handle_disconnect():
        pass

    return app, socketio


def main(args=None):
    rclpy.init(args=args)
    dashboard_node = DashboardNode()

    app, socketio = create_app(dashboard_node)

    # Run Flask in a separate thread
    def run_flask():
        socketio.run(app, host='0.0.0.0', port=8080, debug=False, allow_unsafe_werkzeug=True)

    flask_thread = Thread(target=run_flask, daemon=True)
    flask_thread.start()

    # Run ROS2 spin
    try:
        rclpy.spin(dashboard_node)
    except KeyboardInterrupt:
        pass
    finally:
        dashboard_node.destroy_node()
        rclpy.shutdown()


if __name__ == '__main__':
    main()
