import sys
import time
import random
import math
import io
import numpy as np
import matplotlib.pyplot as plt
from PyQt5.QtWidgets import (QApplication, QMainWindow, QVBoxLayout, QHBoxLayout, 
                             QWidget, QPushButton, QComboBox, QLabel, QTextEdit, 
                             QTabWidget, QGroupBox, QMessageBox)
from PyQt5.QtCore import Qt, QTimer
from matplotlib.backends.backend_qt5agg import FigureCanvasQTAgg as FigureCanvas
from matplotlib.figure import Figure

# Try to import QtWebEngine, but provide fallback
try:
    from PyQt5.QtWebEngineWidgets import QWebEngineView
    HAS_WEBENGINE = True
except ImportError:
    HAS_WEBENGINE = False
    print("QtWebEngine not available. Map display will be disabled.")
    print("To enable maps, install: pip install PyQtWebEngine")

# Try to import folium, but provide fallback
try:
    import folium
    HAS_FOLIUM = True
except ImportError:
    HAS_FOLIUM = False
    print("Folium not available. Install with: pip install folium")

# Define the beach area in South Miami (approximate coordinates)
SOUTH_MIAMI_BEACH = (25.7617, -80.1918)
BEACH_AREA_BOUNDS = {
    'min_lat': 25.755,
    'max_lat': 25.777,
    'min_lon': -80.198,
    'max_lon': -80.180
}

class Node:
    def __init__(self, position, parent=None):
        self.position = position
        self.parent = parent
        self.g = 0
        self.h = 0
        self.f = 0
        
    def __eq__(self, other):
        return self.position == other.position
    
    def __hash__(self):
        return hash(self.position)

class AStarPathPlanner:
    def __init__(self, grid_size=50):
        self.grid_size = grid_size
        self.obstacles = self.generate_obstacles()
        
    def generate_obstacles(self):
        """Generate obstacles with guaranteed free paths"""
        obstacles = []
        num_obstacles = random.randint(8, 12)
        
        for _ in range(num_obstacles):
            cluster_center_lat = random.uniform(BEACH_AREA_BOUNDS['min_lat'] + 0.002, 
                                              BEACH_AREA_BOUNDS['max_lat'] - 0.002)
            cluster_center_lon = random.uniform(BEACH_AREA_BOUNDS['min_lon'] + 0.002,
                                              BEACH_AREA_BOUNDS['max_lon'] - 0.002)
            
            for _ in range(random.randint(2, 3)):
                lat = cluster_center_lat + random.uniform(-0.001, 0.001)
                lon = cluster_center_lon + random.uniform(-0.001, 0.001)
                
                lat = max(BEACH_AREA_BOUNDS['min_lat'], min(BEACH_AREA_BOUNDS['max_lat'], lat))
                lon = max(BEACH_AREA_BOUNDS['min_lon'], min(BEACH_AREA_BOUNDS['max_lon'], lon))
                
                obstacles.append((lat, lon))
            
        return obstacles
    
    def is_on_land(self, position):
        lat, lon = position
        return (BEACH_AREA_BOUNDS['min_lat'] <= lat <= BEACH_AREA_BOUNDS['max_lat'] and
                BEACH_AREA_BOUNDS['min_lon'] <= lon <= BEACH_AREA_BOUNDS['max_lon'])
    
    def heuristic(self, a, b, heuristic_type='manhattan'):
        lat1, lon1 = a
        lat2, lon2 = b
        
        lat_dist = abs(lat1 - lat2) * 111000
        lon_dist = abs(lon1 - lon2) * 111000 * math.cos(math.radians((lat1 + lat2) / 2))
        
        if heuristic_type == 'manhattan':
            return lat_dist + lon_dist
        elif heuristic_type == 'euclidean':
            return math.sqrt(lat_dist**2 + lon_dist**2)
        elif heuristic_type == 'chebyshev':
            return max(lat_dist, lon_dist)
        else:
            return 0
    
    def is_valid_position(self, position):
        lat, lon = position
        
        if not self.is_on_land(position):
            return False
            
        for obstacle in self.obstacles:
            obstacle_dist = self.heuristic(position, obstacle, 'euclidean')
            if obstacle_dist < 50:
                return False
                
        return True
    
    def get_neighbors(self, position):
        lat, lon = position
        neighbors = []
        
        base_step = 0.0003
        directions = [
            (-base_step, 0), (base_step, 0), (0, -base_step), (0, base_step),
            (-base_step, -base_step), (-base_step, base_step), 
            (base_step, -base_step), (base_step, base_step)
        ]
        
        for dlat, dlon in directions:
            new_lat = lat + dlat
            new_lon = lon + dlon
            new_position = (new_lat, new_lon)
            
            if self.is_valid_position(new_position):
                neighbors.append(new_position)
                    
        return neighbors
    
    def a_star(self, start, goal, heuristic_type='manhattan'):
        start_time = time.time()
        
        start_node = Node(start)
        goal_node = Node(goal)
        
        open_set = set([start_node])
        open_list = [start_node]
        closed_set = set()
        
        nodes_expanded = 0
        max_nodes = 2000
        
        while open_list and nodes_expanded < max_nodes:
            current_node = min(open_list, key=lambda x: x.f)
            open_list.remove(current_node)
            open_set.remove(current_node)
            closed_set.add(current_node)
            
            nodes_expanded += 1
            
            current_dist = self.heuristic(current_node.position, goal, 'euclidean')
            if current_dist < 100:
                path = []
                current = current_node
                while current is not None:
                    path.append(current.position)
                    current = current.parent
                
                computation_time = time.time() - start_time
                return path[::-1], nodes_expanded, computation_time, len(closed_set)
            
            for neighbor_pos in self.get_neighbors(current_node.position):
                neighbor_node = Node(neighbor_pos, current_node)
                
                if neighbor_node in closed_set:
                    continue
                
                move_cost = self.heuristic(current_node.position, neighbor_pos, heuristic_type)
                neighbor_node.g = current_node.g + move_cost
                neighbor_node.h = self.heuristic(neighbor_pos, goal, heuristic_type)
                neighbor_node.f = neighbor_node.g + neighbor_node.h
                
                if neighbor_node in open_set:
                    existing_node = next((n for n in open_list if n == neighbor_node), None)
                    if existing_node and neighbor_node.g < existing_node.g:
                        existing_node.g = neighbor_node.g
                        existing_node.f = neighbor_node.f
                        existing_node.parent = current_node
                else:
                    open_set.add(neighbor_node)
                    open_list.append(neighbor_node)
        
        computation_time = time.time() - start_time
        print(f"A* failed: {nodes_expanded} nodes expanded, no path found")
        return None, nodes_expanded, computation_time, len(closed_set)

class DeliveryBot:
    def __init__(self):
        self.orders = []
        self.path_planner = AStarPathPlanner()
        self.current_path = []
        self.current_path_index = 0
        self.current_position = SOUTH_MIAMI_BEACH
        self.is_moving = False
        self.current_delivery_order = None
        self.generate_initial_orders()
        
    def generate_initial_orders(self):
        num_orders = random.randint(5, 8)
        
        for i in range(num_orders):
            attempts = 0
            while attempts < 50:
                lat = random.uniform(BEACH_AREA_BOUNDS['min_lat'] + 0.001, 
                                   BEACH_AREA_BOUNDS['max_lat'] - 0.001)
                lon = random.uniform(BEACH_AREA_BOUNDS['min_lon'] + 0.001,
                                   BEACH_AREA_BOUNDS['max_lon'] - 0.001)
                
                if self.path_planner.is_valid_position((lat, lon)):
                    self.orders.append({
                        'id': i,
                        'position': (lat, lon),
                        'status': 'waiting',
                        'order_time': time.time() - random.uniform(0, 3600)
                    })
                    break
                attempts += 1
            
            if attempts >= 50:
                fallback_pos = (
                    random.uniform(BEACH_AREA_BOUNDS['min_lat'] + 0.005, 
                                 BEACH_AREA_BOUNDS['max_lat'] - 0.005),
                    random.uniform(BEACH_AREA_BOUNDS['min_lon'] + 0.005,
                                 BEACH_AREA_BOUNDS['max_lon'] - 0.005)
                )
                self.orders.append({
                    'id': i,
                    'position': fallback_pos,
                    'status': 'waiting',
                    'order_time': time.time() - random.uniform(0, 3600)
                })
        
        if len(self.orders) > 0:
            self.orders[0]['status'] = 'delivered'
        if len(self.orders) > 1:
            self.orders[1]['status'] = 'delivered'
    
    def update_order_status(self, order_id, status):
        for order in self.orders:
            if order['id'] == order_id:
                order['status'] = status
                break
    
    def get_orders_by_status(self, status):
        return [order for order in self.orders if order['status'] == status]
    
    def get_next_waiting_order(self):
        waiting_orders = self.get_orders_by_status('waiting')
        if waiting_orders:
            return waiting_orders[0]
        return None
    
    def start_delivery(self, order, path):
        if not path or len(path) < 2:
            return False
            
        self.current_delivery_order = order
        self.current_path = path
        self.current_path_index = 0
        self.is_moving = True
        self.current_position = path[0]
        self.update_order_status(order['id'], 'in_progress')
        return True
    
    def move_to_next_position(self):
        """Move to next position in path, return True if still moving, False if complete"""
        if not self.is_moving or self.current_path_index >= len(self.current_path):
            # Delivery complete
            if self.current_delivery_order:
                self.update_order_status(self.current_delivery_order['id'], 'delivered')
                completed_order = self.current_delivery_order
                self.current_delivery_order = None
                self.is_moving = False
                return False, completed_order
            return False, None
        
        self.current_position = self.current_path[self.current_path_index]
        self.current_path_index += 1
        
        # Check if this is the final position
        if self.current_path_index >= len(self.current_path):
            # Final position reached
            if self.current_delivery_order:
                self.update_order_status(self.current_delivery_order['id'], 'delivered')
                completed_order = self.current_delivery_order
                self.current_delivery_order = None
                self.is_moving = False
                return False, completed_order
        
        return True, None
    
    def get_bot_position(self):
        return self.current_position

class MplCanvas(FigureCanvas):
    def __init__(self, parent=None, width=5, height=4, dpi=100):
        fig = Figure(figsize=(width, height), dpi=dpi)
        self.axes = fig.add_subplot(111)
        super().__init__(fig)

class MapWidget(QWidget):
    def __init__(self, delivery_bot, main_window):
        super().__init__()
        self.delivery_bot = delivery_bot
        self.main_window = main_window
        self.layout = QVBoxLayout()
        self.setLayout(self.layout)
        
        if HAS_WEBENGINE and HAS_FOLIUM:
            self.map_view = QWebEngineView()
            self.layout.addWidget(self.map_view)
        else:
            self.fallback_label = QLabel("Map display not available.\n\n"
                                       "To enable maps, install:\n"
                                       "pip install PyQtWebEngine folium")
            self.fallback_label.setAlignment(Qt.AlignCenter)
            self.fallback_label.setStyleSheet("font-size: 14px; color: gray;")
            self.layout.addWidget(self.fallback_label)
    
    def update_map(self, show_traversal=False):
        if not HAS_WEBENGINE or not HAS_FOLIUM:
            return
            
        m = folium.Map(location=SOUTH_MIAMI_BEACH, zoom_start=15)
        
        folium.Rectangle(
            bounds=[(BEACH_AREA_BOUNDS['min_lat'], BEACH_AREA_BOUNDS['min_lon']),
                   (BEACH_AREA_BOUNDS['max_lat'], BEACH_AREA_BOUNDS['max_lon'])],
            color='green',
            fill=True,
            fill_color='lightgreen',
            fill_opacity=0.2,
            popup="Delivery Area"
        ).add_to(m)
        
        for obstacle in self.delivery_bot.path_planner.obstacles:
            folium.CircleMarker(
                location=obstacle,
                radius=8,
                color='black',
                fill=True,
                fill_color='black',
                fill_opacity=0.7,
                popup="Obstacle"
            ).add_to(m)
        
        bot_position = self.delivery_bot.get_bot_position()
        folium.Marker(
            bot_position,
            popup=f"Delivery Bot - {self.delivery_bot.current_delivery_order['id'] if self.delivery_bot.current_delivery_order else 'IDLE'}",
            icon=folium.Icon(color='purple', icon='truck', prefix='fa')
        ).add_to(m)
        
        for order in self.delivery_bot.orders:
            color = 'blue'
            icon = 'cutlery'
            if order['status'] == 'in_progress':
                color = 'orange'
                icon = 'hourglass-half'
            elif order['status'] == 'delivered':
                color = 'red'
                icon = 'check'
            
            folium.Marker(
                location=order['position'],
                popup=f"Order #{order['id']} - {order['status'].replace('_', ' ').title()}",
                icon=folium.Icon(color=color, icon=icon, prefix='fa')
            ).add_to(m)
        
        if self.delivery_bot.current_path and len(self.delivery_bot.current_path) > 1:
            if self.delivery_bot.current_path_index > 0:
                completed_path = self.delivery_bot.current_path[:self.delivery_bot.current_path_index]
                if len(completed_path) > 1:
                    folium.PolyLine(
                        completed_path,
                        color='green',
                        weight=4,
                        opacity=0.8,
                        popup="Completed Path"
                    ).add_to(m)
            
            remaining_path = self.delivery_bot.current_path[self.delivery_bot.current_path_index:] 
            if len(remaining_path) > 1:
                folium.PolyLine(
                    remaining_path,
                    color='purple',
                    weight=4,
                    opacity=0.6,
                    popup="Remaining Path"
                ).add_to(m)
            
            folium.Marker(
                self.delivery_bot.current_path[0],
                popup="Start Position",
                icon=folium.Icon(color='green', icon='play', prefix='fa')
            ).add_to(m)
            
            folium.Marker(
                self.delivery_bot.current_path[-1],
                popup="Delivery Destination",
                icon=folium.Icon(color='darkred', icon='flag-checkered', prefix='fa')
            ).add_to(m)
        
        data = io.BytesIO()
        m.save(data, close_file=False)
        self.map_view.setHtml(data.getvalue().decode())

class MainWindow(QMainWindow):
    def __init__(self):
        super().__init__()
        self.delivery_bot = DeliveryBot()
        self.current_heuristic = 'euclidean'
        self.analysis_data = {
            'manhattan': {'nodes_expanded': [], 'computation_time': [], 'path_length': []},
            'euclidean': {'nodes_expanded': [], 'computation_time': [], 'path_length': []},
            'chebyshev': {'nodes_expanded': [], 'computation_time': [], 'path_length': []}
        }
        
        self.animation_timer = QTimer()
        self.animation_timer.timeout.connect(self.animate_traversal)
        self.animation_speed = 500
        
        self.init_ui()
        self.update_map()
        self.update_analysis()
        
    def init_ui(self):
        self.setWindowTitle("Autonomous Hot Dog Delivery Bot - South Miami Beach")
        self.setGeometry(100, 100, 1400, 900)
        
        central_widget = QWidget()
        self.setCentralWidget(central_widget)
        main_layout = QHBoxLayout(central_widget)
        
        left_panel = QVBoxLayout()
        left_panel.setContentsMargins(10, 10, 10, 10)
        
        controls_group = QGroupBox("Path Planning Controls")
        controls_layout = QVBoxLayout()
        
        heuristic_layout = QHBoxLayout()
        heuristic_layout.addWidget(QLabel("Heuristic:"))
        self.heuristic_combo = QComboBox()
        self.heuristic_combo.addItems(["Manhattan", "Euclidean", "Chebyshev"])
        self.heuristic_combo.setCurrentText("Euclidean")
        self.heuristic_combo.currentTextChanged.connect(self.on_heuristic_changed)
        heuristic_layout.addWidget(self.heuristic_combo)
        controls_layout.addLayout(heuristic_layout)
        
        speed_layout = QHBoxLayout()
        speed_layout.addWidget(QLabel("Animation Speed:"))
        self.speed_combo = QComboBox()
        self.speed_combo.addItems(["Slow", "Medium", "Fast"])
        self.speed_combo.setCurrentText("Medium")
        self.speed_combo.currentTextChanged.connect(self.on_speed_changed)
        speed_layout.addWidget(self.speed_combo)
        controls_layout.addLayout(speed_layout)
        
        self.plan_path_btn = QPushButton("Plan New Delivery Path")
        self.plan_path_btn.clicked.connect(self.plan_path)
        controls_layout.addWidget(self.plan_path_btn)
        
        self.start_traversal_btn = QPushButton("Start Traversal")
        self.start_traversal_btn.clicked.connect(self.start_traversal)
        self.start_traversal_btn.setEnabled(False)
        controls_layout.addWidget(self.start_traversal_btn)
        
        self.simulate_delivery_btn = QPushButton("Complete Delivery")
        self.simulate_delivery_btn.clicked.connect(self.complete_delivery)
        controls_layout.addWidget(self.simulate_delivery_btn)
        
        self.run_comparison_btn = QPushButton("Run Heuristic Comparison")
        self.run_comparison_btn.clicked.connect(self.run_heuristic_comparison)
        controls_layout.addWidget(self.run_comparison_btn)
        
        controls_group.setLayout(controls_layout)
        left_panel.addWidget(controls_group)
        
        status_group = QGroupBox("Bot Status")
        status_layout = QVBoxLayout()
        self.bot_status_label = QLabel("Status: Ready for delivery")
        self.bot_status_label.setStyleSheet("font-weight: bold; color: blue;")
        status_layout.addWidget(self.bot_status_label)
        status_group.setLayout(status_layout)
        left_panel.addWidget(status_group)
        
        status_group = QGroupBox("Delivery Status")
        status_layout = QVBoxLayout()
        
        self.status_text = QTextEdit()
        self.status_text.setReadOnly(True)
        status_layout.addWidget(self.status_text)
        
        status_group.setLayout(status_layout)
        left_panel.addWidget(status_group)
        
        analysis_tabs = QTabWidget()
        
        efficiency_tab = QWidget()
        efficiency_layout = QVBoxLayout()
        self.efficiency_canvas = MplCanvas(self, width=5, height=4, dpi=100)
        efficiency_layout.addWidget(self.efficiency_canvas)
        efficiency_tab.setLayout(efficiency_layout)
        analysis_tabs.addTab(efficiency_tab, "Search Efficiency")
        
        performance_tab = QWidget()
        performance_layout = QVBoxLayout()
        self.performance_canvas = MplCanvas(self, width=5, height=4, dpi=100)
        performance_layout.addWidget(self.performance_canvas)
        performance_tab.setLayout(performance_layout)
        analysis_tabs.addTab(performance_tab, "Computation Performance")
        
        optimality_tab = QWidget()
        optimality_layout = QVBoxLayout()
        self.optimality_canvas = MplCanvas(self, width=5, height=4, dpi=100)
        optimality_layout.addWidget(self.optimality_canvas)
        optimality_tab.setLayout(optimality_layout)
        analysis_tabs.addTab(optimality_tab, "Path Optimality")
        
        comparison_tab = QWidget()
        comparison_layout = QVBoxLayout()
        self.comparison_text = QTextEdit()
        self.comparison_text.setReadOnly(True)
        comparison_layout.addWidget(self.comparison_text)
        comparison_tab.setLayout(comparison_layout)
        analysis_tabs.addTab(comparison_tab, "Comparison Results")
        
        left_panel.addWidget(analysis_tabs)
        
        right_panel = QVBoxLayout()
        
        map_group = QGroupBox("South Miami Beach Delivery Map - Real-time Traversal")
        map_layout = QVBoxLayout()
        
        self.map_widget = MapWidget(self.delivery_bot, self)
        map_layout.addWidget(self.map_widget)
        
        legend_layout = QHBoxLayout()
        legend_layout.addWidget(QLabel("Legend:"))
        
        purple_label = QLabel("● Purple: Delivery Bot")
        purple_label.setStyleSheet("color: purple; font-weight: bold;")
        legend_layout.addWidget(purple_label)
        
        red_label = QLabel("● Red: Delivered")
        red_label.setStyleSheet("color: red; font-weight: bold;")
        legend_layout.addWidget(red_label)
        
        green_label = QLabel("● Green: Start/Completed Path")
        green_label.setStyleSheet("color: green; font-weight: bold;")
        legend_layout.addWidget(green_label)
        
        blue_label = QLabel("● Blue: Waiting for Delivery")
        blue_label.setStyleSheet("color: blue; font-weight: bold;")
        legend_layout.addWidget(blue_label)
        
        orange_label = QLabel("● Orange: In Progress")
        orange_label.setStyleSheet("color: orange; font-weight: bold;")
        legend_layout.addWidget(orange_label)
        
        legend_layout.addStretch()
        map_layout.addLayout(legend_layout)
        
        map_group.setLayout(map_layout)
        right_panel.addWidget(map_group)
        
        main_layout.addLayout(left_panel, 1)
        main_layout.addLayout(right_panel, 2)
        
        self.update_status()
    
    def on_heuristic_changed(self, text):
        self.current_heuristic = text.lower()
    
    def on_speed_changed(self, text):
        speeds = {"Slow": 1000, "Medium": 500, "Fast": 200}
        self.animation_speed = speeds.get(text, 500)
        if self.animation_timer.isActive():
            self.animation_timer.stop()
            self.animation_timer.start(self.animation_speed)
    
    def update_status(self):
        status_text = "=== DELIVERY ORDERS ===\n\n"
        
        waiting_orders = self.delivery_bot.get_orders_by_status('waiting')
        in_progress_orders = self.delivery_bot.get_orders_by_status('in_progress')
        delivered_orders = self.delivery_bot.get_orders_by_status('delivered')
        
        status_text += f"WAITING FOR DELIVERY ({len(waiting_orders)}):\n"
        for order in waiting_orders:
            status_text += f"  Order #{order['id']}: Lat {order['position'][0]:.4f}, Lon {order['position'][1]:.4f}\n"
        
        status_text += f"\nIN PROGRESS ({len(in_progress_orders)}):\n"
        for order in in_progress_orders:
            status_text += f"  Order #{order['id']}: Lat {order['position'][0]:.4f}, Lon {order['position'][1]:.4f}\n"
        
        status_text += f"\nDELIVERED ({len(delivered_orders)}):\n"
        for order in delivered_orders:
            status_text += f"  Order #{order['id']}: Lat {order['position'][0]:.4f}, Lon {order['position'][1]:.4f}\n"
        
        self.status_text.setText(status_text)
    
    def update_map(self):
        self.map_widget.update_map()
    
    def plan_path(self):
        if self.animation_timer.isActive():
            self.animation_timer.stop()
            self.delivery_bot.is_moving = False
        
        target_order = self.delivery_bot.get_next_waiting_order()
        if not target_order:
            QMessageBox.information(self, "No Orders", "No waiting orders to deliver!")
            return
            
        start_pos = SOUTH_MIAMI_BEACH
        
        print(f"Planning path from {start_pos} to order #{target_order['id']} at {target_order['position']}")
        print(f"Using {self.current_heuristic} heuristic")
        
        path, nodes_expanded, computation_time, closed_count = self.delivery_bot.path_planner.a_star(
            start_pos, target_order['position'], self.current_heuristic
        )
        
        if path:
            print(f"✓ Path found! Length: {len(path)} segments, Nodes expanded: {nodes_expanded}")
            
            self.analysis_data[self.current_heuristic]['nodes_expanded'].append(nodes_expanded)
            self.analysis_data[self.current_heuristic]['computation_time'].append(computation_time)
            self.analysis_data[self.current_heuristic]['path_length'].append(len(path))
            
            self.delivery_bot.current_path = path
            self.delivery_bot.current_delivery_order = target_order
            self.delivery_bot.current_path_index = 0
            self.delivery_bot.is_moving = False
            
            self.start_traversal_btn.setEnabled(True)
            
            self.update_map()
            self.update_status()
            self.update_analysis()
            
            QMessageBox.information(self, "Path Found", 
                                  f"✓ Path planned using {self.current_heuristic} heuristic!\n"
                                  f"• Path length: {len(path)} segments\n"
                                  f"• Nodes expanded: {nodes_expanded}\n"
                                  f"• Computation time: {computation_time:.3f}s\n\n"
                                  f"Click 'Start Traversal' to begin delivery to Order #{target_order['id']}.")
        else:
            print(f"✗ No path found to order #{target_order['id']}")
            QMessageBox.warning(self, "Path Planning Failed", 
                              f"Could not find a valid path to Order #{target_order['id']}.\n"
                              f"Try a different order or heuristic.\n"
                              f"Nodes expanded: {nodes_expanded}")
    
    def start_traversal(self):
        if not self.delivery_bot.current_path or not self.delivery_bot.current_delivery_order:
            QMessageBox.warning(self, "No Path", "No path planned! Plan a path first.")
            return
            
        print(f"Starting traversal for order #{self.delivery_bot.current_delivery_order['id']}")
        
        success = self.delivery_bot.start_delivery(
            self.delivery_bot.current_delivery_order, 
            self.delivery_bot.current_path
        )
        
        if success:
            self.bot_status_label.setText("Status: Starting delivery...")
            self.bot_status_label.setStyleSheet("font-weight: bold; color: orange;")
            
            self.plan_path_btn.setEnabled(False)
            self.start_traversal_btn.setEnabled(False)
            self.simulate_delivery_btn.setEnabled(False)
            self.run_comparison_btn.setEnabled(False)
            
            self.animation_timer.start(self.animation_speed)
            
            self.update_status()
            self.update_map()
            
            print("✓ Traversal started successfully")
        else:
            QMessageBox.warning(self, "Error", "Failed to start delivery - invalid path!")
    
    def animate_traversal(self):
        if not self.delivery_bot.is_moving:
            self.animation_timer.stop()
            return
        
        still_moving, completed_order = self.delivery_bot.move_to_next_position()
        
        if still_moving:
            # Still moving - update progress
            progress = (self.delivery_bot.current_path_index / len(self.delivery_bot.current_path)) * 100
            self.bot_status_label.setText(f"Status: Delivering order #{self.delivery_bot.current_delivery_order['id']}... {progress:.1f}%")
            
            self.update_map()
            
            if self.delivery_bot.current_path_index % 5 == 0:
                print(f"Bot position: {self.delivery_bot.current_path_index}/{len(self.delivery_bot.current_path)}")
        else:
            # Delivery complete
            self.animation_timer.stop()
            self.bot_status_label.setText("Status: Delivery Complete!")
            self.bot_status_label.setStyleSheet("font-weight: bold; color: green;")
            
            # Re-enable buttons
            self.plan_path_btn.setEnabled(True)
            self.start_traversal_btn.setEnabled(False)
            self.simulate_delivery_btn.setEnabled(True)
            self.run_comparison_btn.setEnabled(True)
            
            # Update final status
            self.update_status()
            self.update_map()
            
            print("✓ Delivery completed successfully")
            
            if completed_order:
                QMessageBox.information(self, "Delivery Complete", 
                                      f"✓ Order #{completed_order['id']} delivered successfully!")
    
    def complete_delivery(self):
        in_progress_orders = self.delivery_bot.get_orders_by_status('in_progress')
        if not in_progress_orders:
            QMessageBox.information(self, "No Deliveries", "No orders in progress!")
            return
            
        target_order = in_progress_orders[0]
        self.delivery_bot.update_order_status(target_order['id'], 'delivered')
        
        if self.animation_timer.isActive():
            self.animation_timer.stop()
            self.delivery_bot.is_moving = False
        
        self.bot_status_label.setText("Status: Ready for delivery")
        self.bot_status_label.setStyleSheet("font-weight: bold; color: blue;")
        
        self.update_status()
        self.update_map()
        
        QMessageBox.information(self, "Delivery Complete", f"Order #{target_order['id']} delivered!")
    
    def run_heuristic_comparison(self):
        target_order = self.delivery_bot.get_next_waiting_order()
        if not target_order:
            QMessageBox.information(self, "No Orders", "No waiting orders to compare!")
            return
            
        start_pos = SOUTH_MIAMI_BEACH
        
        comparison_results = {}
        
        for heuristic in ['manhattan', 'euclidean', 'chebyshev']:
            print(f"Running comparison with {heuristic} heuristic")
            path, nodes_expanded, computation_time, closed_count = self.delivery_bot.path_planner.a_star(
                start_pos, target_order['position'], heuristic
            )
            
            if path:
                comparison_results[heuristic] = {
                    'nodes_expanded': nodes_expanded,
                    'computation_time': computation_time,
                    'path_length': len(path),
                    'efficiency': len(path) / nodes_expanded if nodes_expanded > 0 else 0
                }
                
                self.analysis_data[heuristic]['nodes_expanded'].append(nodes_expanded)
                self.analysis_data[heuristic]['computation_time'].append(computation_time)
                self.analysis_data[heuristic]['path_length'].append(len(path))
        
        comparison_text = "=== HEURISTIC COMPARISON RESULTS ===\n\n"
        
        for heuristic, results in comparison_results.items():
            comparison_text += f"{heuristic.upper()} HEURISTIC:\n"
            comparison_text += f"  Nodes Expanded: {results['nodes_expanded']}\n"
            comparison_text += f"  Computation Time: {results['computation_time']:.4f} seconds\n"
            comparison_text += f"  Path Length: {results['path_length']} segments\n"
            comparison_text += f"  Exploration Efficiency: {results['efficiency']:.4f}\n\n"
        
        if len(comparison_results) == 3:
            best_nodes = min(comparison_results.items(), key=lambda x: x[1]['nodes_expanded'])
            best_time = min(comparison_results.items(), key=lambda x: x[1]['computation_time'])
            best_efficiency = max(comparison_results.items(), key=lambda x: x[1]['efficiency'])
            
            comparison_text += "=== PERFORMANCE ANALYSIS ===\n\n"
            comparison_text += f"Best for Nodes Expanded: {best_nodes[0].title()} ({best_nodes[1]['nodes_expanded']} nodes)\n"
            comparison_text += f"Best for Computation Time: {best_time[0].title()} ({best_time[1]['computation_time']:.4f}s)\n"
            comparison_text += f"Best for Exploration Efficiency: {best_efficiency[0].title()} ({best_efficiency[1]['efficiency']:.4f})\n\n"
            
            euclidean_result = comparison_results.get('euclidean', {})
            if euclidean_result:
                comparison_text += "=== EUCLIDEAN HEURISTIC FOR REAL-WORLD APPLICATIONS ===\n\n"
                comparison_text += "✅ Use Euclidean When:\n"
                comparison_text += "• Working in continuous spaces\n"
                comparison_text += "• Any-angle movement is allowed\n"
                comparison_text += "• Accuracy is more important than speed\n"
                comparison_text += "• Real-world applications\n"
                comparison_text += "Examples: Robotics, GPS navigation, drone pathfinding\n\n"
        
        self.comparison_text.setText(comparison_text)
        self.update_analysis()
        
        QMessageBox.information(self, "Comparison Complete", "Heuristic comparison completed!")
    
    def update_analysis(self):
        heuristics = ['manhattan', 'euclidean', 'chebyshev']
        colors = ['blue', 'green', 'red']
        
        self.efficiency_canvas.axes.clear()
        lines = []
        
        for i, heuristic in enumerate(heuristics):
            if self.analysis_data[heuristic]['nodes_expanded']:
                line, = self.efficiency_canvas.axes.plot(
                    range(len(self.analysis_data[heuristic]['nodes_expanded'])),
                    self.analysis_data[heuristic]['nodes_expanded'],
                    label=heuristic.title(),
                    color=colors[i],
                    marker='o'
                )
                lines.append(line)
        
        self.efficiency_canvas.axes.set_title('Search Efficiency (Nodes Expanded - Lower is Better)')
        self.efficiency_canvas.axes.set_xlabel('Path Planning Attempt')
        self.efficiency_canvas.axes.set_ylabel('Nodes Expanded')
        if lines:
            self.efficiency_canvas.axes.legend()
        self.efficiency_canvas.axes.grid(True, alpha=0.3)
        self.efficiency_canvas.draw()
        
        self.performance_canvas.axes.clear()
        lines = []
        
        for i, heuristic in enumerate(heuristics):
            if self.analysis_data[heuristic]['computation_time']:
                line, = self.performance_canvas.axes.plot(
                    range(len(self.analysis_data[heuristic]['computation_time'])),
                    self.analysis_data[heuristic]['computation_time'],
                    label=heuristic.title(),
                    color=colors[i],
                    marker='o'
                )
                lines.append(line)
        
        self.performance_canvas.axes.set_title('Computation Performance (Time - Lower is Better)')
        self.performance_canvas.axes.set_xlabel('Path Planning Attempt')
        self.performance_canvas.axes.set_ylabel('Computation Time (seconds)')
        if lines:
            self.performance_canvas.axes.legend()
        self.performance_canvas.axes.grid(True, alpha=0.3)
        self.performance_canvas.draw()
        
        self.optimality_canvas.axes.clear()
        lines = []
        
        for i, heuristic in enumerate(heuristics):
            if self.analysis_data[heuristic]['path_length']:
                line, = self.optimality_canvas.axes.plot(
                    range(len(self.analysis_data[heuristic]['path_length'])),
                    self.analysis_data[heuristic]['path_length'],
                    label=heuristic.title(),
                    color=colors[i],
                    marker='o'
                )
                lines.append(line)
        
        self.optimality_canvas.axes.set_title('Path Optimality (All Should Be Equal)')
        self.optimality_canvas.axes.set_xlabel('Path Planning Attempt')
        self.optimality_canvas.axes.set_ylabel('Path Length (segments)')
        if lines:
            self.optimality_canvas.axes.legend()
        self.optimality_canvas.axes.grid(True, alpha=0.3)
        self.optimality_canvas.draw()

def main():
    app = QApplication(sys.argv)
    
    if not HAS_WEBENGINE:
        msg = QMessageBox()
        msg.setIcon(QMessageBox.Warning)
        msg.setText("PyQtWebEngine not available")
        msg.setInformativeText("Map display will be disabled. To enable maps, install:\n\npip install PyQtWebEngine")
        msg.setWindowTitle("Module Missing")
        msg.exec_()
    
    if not HAS_FOLIUM:
        msg = QMessageBox()
        msg.setIcon(QMessageBox.Warning)
        msg.setText("Folium not available")
        msg.setInformativeText("Map display will be disabled. To enable maps, install:\n\npip install folium")
        msg.setWindowTitle("Module Missing")
        msg.exec_()
    
    window = MainWindow()
    window.show()
    sys.exit(app.exec_())

if __name__ == "__main__":
    main()