Wayfinder/robot_gui.py

import time
import sys
import os 
from PyQt6.QtWidgets import (
    QApplication, QWidget, QVBoxLayout, QHBoxLayout, QPushButton, QLabel, 
    QFrame, QGridLayout, QSpacerItem, QSizePolicy
)
from PyQt6.QtGui import QPixmap, QImage, QFont
from PyQt6.QtCore import Qt, QTimer
import numpy as np
import cv2
from lidar_processor import LidarProcessor  # Import LIDAR processing

def hex_to_bgr(hex_color):
    """Convert HEX to OpenCV BGR"""
    hex_color = hex_color.lstrip("#")
    r, g, b = tuple(int(hex_color[i:i+2], 16) for i in (0, 2, 4))
    return (b, g, r)

class RobotGUI(QWidget):
    def __init__(self, lidar_processor):
        super().__init__()
        self.lidar_processor = lidar_processor
        self.robot_path = []

        self.setWindowTitle("Robot Control GUI")
        self.setGeometry(100, 100, 800, 480)
        self.showFullScreen()

        # Colors
        self.bg_color = hex_to_bgr("#2E3440")
        self.wall_color = hex_to_bgr("#FFD700")

        # Layout
        layout = QVBoxLayout()
        self.setLayout(layout)

        # Top Bar
        status_bar = QHBoxLayout()
        self.icon_label = QLabel("🤖")
        self.icon_label.setFont(QFont("Noto Color Emoji", 20))
        self.battery_label = QLabel("Battery: 100%")
        self.speed_label = QLabel("Speed: 0 m/s")
        self.power_label = QLabel("Power: 0W")
        for widget in [self.icon_label, self.battery_label, self.speed_label, self.power_label]:
            status_bar.addWidget(widget)
        layout.addLayout(status_bar)

        # Main Content
        main_content = QHBoxLayout()
        layout.addLayout(main_content)

        # Buttons
        button_container = QVBoxLayout()
        button_grid = QGridLayout()
        self.start_btn = QPushButton("Start")
        self.stop_btn = QPushButton("Stop")
        self.feature_btn = QPushButton("Feature")
        self.drift_btn = QPushButton("Drift")
        button_size = 150
        for btn in [self.start_btn, self.stop_btn, self.feature_btn, self.drift_btn]:
            btn.setObjectName("controlButton")
            btn.setMinimumSize(button_size, button_size)

        button_grid.addWidget(self.start_btn, 0, 0)
        button_grid.addWidget(self.stop_btn, 0, 1)
        button_grid.addWidget(self.feature_btn, 1, 0)
        button_grid.addWidget(self.drift_btn, 1, 1)
        button_container.addSpacerItem(QSpacerItem(20, 40, QSizePolicy.Policy.Minimum, QSizePolicy.Policy.Expanding))
        button_container.addLayout(button_grid)
        main_content.addLayout(button_container)

        # Map Display
        self.map_label = QLabel()
        self.map_label.setObjectName("mapLabel")
        self.map_label.setFrameStyle(QFrame.Shape.Box)
        self.map_label.setAlignment(Qt.AlignmentFlag.AlignCenter)
        self.map_label.setMinimumSize(350, 350)
        main_content.addWidget(self.map_label)

        # Timer
        self.timer = QTimer(self)
        self.timer.timeout.connect(self.update_map)
        self.timer.start(100)

        self.show()

        # Load stylesheet
        self.load_stylesheet()

    def load_stylesheet(self):
        try:
            qss_path = os.path.join(os.path.dirname(__file__), "styles.qss")
            with open(qss_path, "r") as f:
                self.setStyleSheet(f.read())
        except FileNotFoundError:
            print("Warning: styles.qss not found. Running without stylesheet.")

    def update_map(self):
        map_img, pose = self.lidar_processor.get_data()
        height, width = map_img.shape

        # --- Colors ---
        nord_bgr = (64, 52, 46)         # Nord Background (#2E3440)
        yellow_bgr = (0, 215, 255)      # Cyberpunk Yellow (#FFD700)
        white_bgr = (255, 255, 255)
        green_bgr = (0, 255, 128)       # Cyberpunk Green (custom pick)

        # --- Start with yellow background ---
        color_map = np.full((height, width, 3), yellow_bgr, dtype=np.uint8)

        # --- Threshold walls from map ---
        _, wall_mask = cv2.threshold(map_img, 100, 255, cv2.THRESH_BINARY)

        # --- Apply Nord Background where wall mask is present
        color_map[wall_mask == 255] = nord_bgr

        # --- Convert robot pose to pixel coordinates ---
        MM_TO_PIXELS = 100
        CENTER = height // 2
        x_pix = int(pose[0] * MM_TO_PIXELS + CENTER)
        y_pix = int(pose[1] * MM_TO_PIXELS + CENTER)

        # Determine movement speed (basic)
        if len(self.robot_path) > 1:
            dx = self.robot_path[-1][0] - self.robot_path[-2][0]
            dy = self.robot_path[-1][1] - self.robot_path[-2][1]
            movement = np.hypot(dx, dy)
        else:
            movement = 0

        # Breathing pulse (only if moving)
        base_radius = 6
        pulse_amplitude = 2  # how big the pulse gets
        pulse_speed = 2.0    # Hz

        if movement > 1:
            # Animate based on time
            t = time.time()
            pulse = int(pulse_amplitude * (0.5 + 0.5 * np.sin(2 * np.pi * pulse_speed * t)))
            dot_radius = base_radius + pulse
        else:
            dot_radius = base_radius

        # --- Draw robot as white dot ---
        cv2.circle(color_map, (x_pix, y_pix), dot_radius, white_bgr, -1)

        # --- Trail logic ---
        self.robot_path.append((x_pix, y_pix))
        if len(self.robot_path) > 50:
            self.robot_path.pop(0)

        # --- Draw fading green trail ---
        for i in range(1, len(self.robot_path)):
            alpha = int(255 * (i / len(self.robot_path)))  # Fade effect
            trail_color = (
                int(green_bgr[0] * (alpha / 255)),
                int(green_bgr[1] * (alpha / 255)),
                int(green_bgr[2] * (alpha / 255))
            )
            cv2.line(color_map, self.robot_path[i - 1], self.robot_path[i], trail_color, 1)

        # --- Convert to RGB for Qt display ---
        rgb_image = cv2.cvtColor(color_map, cv2.COLOR_BGR2RGB)
        qimg = QImage(rgb_image.data, width, height, width * 3, QImage.Format.Format_RGB888)
        self.map_label.setPixmap(QPixmap.fromImage(qimg))

    def close_application(self):
        print("🔌 Stopping LIDAR and closing GUI...")
        self.timer.stop()
        self.lidar_processor.stop()
        self.close()
        sys.exit()

    def closeEvent(self, event):
        self.close_application()


if __name__ == "__main__":
    app = QApplication(sys.argv)
    lidar_processor = LidarProcessor()
    window = RobotGUI(lidar_processor)
    sys.exit(app.exec())