import tkinter as tk
from tkinter import ttk, scrolledtext, messagebox
import requests
import json
import time
import threading
import csv
import os
import math
from datetime import datetime

# ========= CONFIG HTTP ==========
BASE_URL = "https://192.168.110.30:8081"
ROBOT_APP_URL = "https://192.168.110.30:8081"
MOVE_ENDPOINT = "/api/RobotManager/MoveToNode"
ACTION_ENDPOINT = "/api/RobotManager/InstantActions"
STATE_ENDPOINT = "/api/RobotManager/State"
MARKER_DETECTION_POSE_ENDPOINT = "/api/marker-detection/pose/current"
MARKER_DETECTION_ENABLE_ENDPOINT = "/api/marker-detection/detection/enable"
VERIFY_SSL = False  # self-signed cert nội bộ

if not VERIFY_SSL:
    import urllib3
    urllib3.disable_warnings(urllib3.exceptions.InsecureRequestWarning)


class HikQrReader:
    """
    Class đọc dữ liệu QR từ Robot App (device hik-qr-001) qua marker-detection API.
    Giữ tên class cũ để không ảnh hưởng luồng UI hiện tại.
    """
    def __init__(self, callback=None):
        self.current_pose = None
        self.current_error = 0.0
        self.is_enabled = False
        self.callback = callback
        self.position_data = []  # Lưu các đo lường
        self.node_data = {}  # {node_name: [(pose, error), ...]}
        self.reading_thread = None
        self.stop_reading = False
        
        # Bắt đầu luồng để định kỳ lấy dữ liệu từ HTTP
        self.start_http_polling()
    
    def start_http_polling(self):
        """Bắt đầu polling dữ liệu từ HTTP server trên robot"""
        self.reading_thread = threading.Thread(target=self._http_polling_loop, daemon=True)
        self.reading_thread.start()
    
    def _http_polling_loop(self):
        """Luồng định kỳ lấy dữ liệu marker pose từ Robot App"""
        last_pose = None
        error_count = 0
        while not self.stop_reading:
            try:
                response = requests.get(
                    f"{ROBOT_APP_URL}{MARKER_DETECTION_POSE_ENDPOINT}",
                    timeout=2,
                    verify=VERIFY_SSL
                )
                if response.status_code == 200:
                    data = response.json()
                    pose = data.get('pose') or {}
                    position = pose.get('position') or {}
                    orientation = pose.get('orientation') or {}

                    if position and orientation:
                        x = float(position.get('x', 0.0))
                        y = float(position.get('y', 0.0))
                        # Chuyển quaternion -> yaw (theta) theo chuẩn ROS
                        qx = float(orientation.get('x', 0.0))
                        qy = float(orientation.get('y', 0.0))
                        qz = float(orientation.get('z', 0.0))
                        qw = float(orientation.get('w', 1.0))
                        siny_cosp = 2.0 * (qw * qz + qx * qy)
                        cosy_cosp = 1.0 - 2.0 * (qy * qy + qz * qz)
                        theta = math.atan2(siny_cosp, cosy_cosp)

                        pose_tuple = (x, y, theta)
                        if pose_tuple != last_pose:
                            self.current_pose = pose_tuple
                            self.current_error = (x**2 + y**2) ** 0.5
                            self.is_enabled = True
                        
                            self.position_data.append({
                                'timestamp': datetime.now(),
                                'pose': self.current_pose,
                                'error': self.current_error
                            })
                        
                            if self.callback:
                                try:
                                    self.callback(
                                        f"HIK-QR-001: X={x:.4f}, Y={y:.4f}, θ={theta:.4f}, Error={self.current_error:.4f}m"
                                    )
                                except:
                                    pass  # Ignore callback errors if UI not ready
                        
                            last_pose = pose_tuple
                    error_count = 0  # Reset error count on success
                elif response.status_code == 404:
                    # Marker detection chưa có pose hợp lệ
                    self.current_pose = None
                else:
                    error_count += 1
            except requests.exceptions.RequestException as e:
                error_count += 1
                # Only log error once every 10 attempts to avoid spam
                if error_count == 1 and self.callback:
                    try:
                        self.callback("⚠ Không đọc được marker pose từ Robot App (hik-qr-001).")
                    except:
                        pass
            except Exception as e:
                if self.callback:
                    try:
                        self.callback(f"❌ Lỗi lấy QR data từ hik-qr-001: {e}")
                    except:
                        pass
            
            time.sleep(0.1)  # Poll mỗi 100ms
    
    def enable_hik_qr(self, enabled=True):
        """Bật/tắt marker detection trong Robot App (nguồn hik-qr-001)."""
        self.is_enabled = enabled
        try:
            endpoint = f"{ROBOT_APP_URL}{MARKER_DETECTION_ENABLE_ENDPOINT}"
            response = requests.post(endpoint, json={'enable': enabled}, timeout=2, verify=VERIFY_SSL)
            status = "enabled" if enabled else "disabled"
            if response.status_code == 200:
                if self.callback:
                    self.callback(f"✅ Marker detection ({'hik-qr-001'}) {status}")
            else:
                if self.callback:
                    self.callback(f"⚠ Marker detection {status} (HTTP {response.status_code})")
        except requests.exceptions.ConnectionError:
            if self.callback:
                self.callback("⚠ Không thể kết nối Robot App để bật/tắt marker detection.")
        except Exception as e:
            if self.callback:
                self.callback(f"⚠ Lỗi khi bật/tắt marker detection: {str(e)[:100]}")
    
    def record_node_data(self, node_name, trial_number):
        """Ghi lại dữ liệu cho node này vào dict - CHỈ 1 SNAPSHOT DUY NHẤT"""
        if self.current_pose and self.current_error is not None:
            if node_name not in self.node_data:
                self.node_data[node_name] = []
            
            # Kiểm tra xem trial này đã ghi chưa - tránh trùng lặp
            existing = [d for d in self.node_data[node_name] if d['trial'] == trial_number]
            if existing:
                if self.callback:
                    self.callback(f"⚠ Node {node_name} trial {trial_number} đã được ghi, bỏ qua\n")
                return False
            
            # Snapshot giá trị hiện tại (chỉ lấy 1 lần)
            snapshot_pose = self.current_pose
            snapshot_error = self.current_error
            
            self.node_data[node_name].append({
                'trial': trial_number,
                'pose': snapshot_pose,
                'error': snapshot_error,
                'timestamp': datetime.now()
            })
            
            if self.callback:
                self.callback(f"📍 Ghi dữ liệu node {node_name} (trial {trial_number}): Error={snapshot_error:.4f}m\n")
            
            return True
        else:
            if self.callback:
                self.callback(f"⚠ Chưa nhận dữ liệu từ HIK-QR-001 cho node {node_name}\n")
            return False
    
    def get_node_average_error(self, node_name):
        """Tính sai số trung bình cho node"""
        if node_name not in self.node_data or not self.node_data[node_name]:
            return 0.0
        
        errors = [d['error'] for d in self.node_data[node_name]]
        return sum(errors) / len(errors)
    
    def save_to_csv(self, filename=None):
        """Lưu dữ liệu vào file CSV"""
        if filename is None:
            timestamp = datetime.now().strftime("%Y%m%d_%H%M%S")
            filename = f"hik_qr_positioning_data_{timestamp}.csv"
        
        try:
            with open(filename, 'w', newline='', encoding='utf-8') as csvfile:
                fieldnames = ['Node', 'Trial', 'X(m)', 'Y(m)', 'Theta(rad)', 'Error(m)', 'Avg_Error(m)', 'Timestamp']
                writer = csv.DictWriter(csvfile, fieldnames=fieldnames)
                
                writer.writeheader()
                
                for node_name in sorted(self.node_data.keys()):
                    avg_error = self.get_node_average_error(node_name)
                    for data in self.node_data[node_name]:
                        x, y, theta = data['pose']
                        writer.writerow({
                            'Node': node_name,
                            'Trial': data['trial'],
                            'X(m)': f"{x:.6f}",
                            'Y(m)': f"{y:.6f}",
                            'Theta(rad)': f"{theta:.6f}",
                            'Error(m)': f"{data['error']:.6f}",
                            'Avg_Error(m)': f"{avg_error:.6f}",
                            'Timestamp': data['timestamp'].strftime("%Y-%m-%d %H:%M:%S.%f")[:-3]
                        })
            
            if self.callback:
                self.callback(f"✅ Dữ liệu đã lưu vào: {filename}\n")
            
            return filename
        except Exception as e:
            if self.callback:
                self.callback(f"❌ Lỗi khi lưu CSV: {e}\n")
            return None
    
    def clear_data(self):
        """Xóa dữ liệu đã ghi"""
        self.node_data = {}
        self.position_data = []


class RobotApiClient:
    """Class API client để điều khiển robot qua HTTP"""
    def __init__(self, base_url=BASE_URL, verify_ssl=VERIFY_SSL):
        self.base_url = base_url
        self.verify_ssl = verify_ssl

    # -------- MoveToNode ----------
    def move_to_node(self, robot_id, node_name, final_action=None):
        if final_action is None:
            final_action = []

        url = self.base_url + MOVE_ENDPOINT
        body = {
            "robotId": robot_id,
            "nodeName": node_name,
            "finalAction": final_action
        }

        resp = requests.post(url, json=body, verify=self.verify_ssl)
        return resp, body

    def wait_for_order_finished(self, robot_id, timeout=60, poll_interval=0.2, callback=None):
        """
        Poll State API cho đến khi nhận được "[order] Finished Order" của order MỚI.
        Chiến lược: Lưu order_id hiện tại, chờ order_id mới + Finished Order.
        Nếu order_id không đổi sau 1.5s → robot đã ở node đích, coi như hoàn thành.
        Adaptive polling: Poll nhanh lúc đầu (0.1s), sau đó chậm dần (0.3s).
        Returns: True nếu thành công, False nếu timeout.
        """
        start_time = time.time()
        initial_order_id = None
        initial_has_finished = False
        min_elapsed_for_empty_path = 0.7  # Tránh trả sớm khi server vừa cập nhật trạng thái
        
        # === Bước 1: Lấy order_id hiện tại (nếu có) ===
        try:
            resp, _ = self.get_state(robot_id)
            if resp.status_code == 200:
                state_data = resp.json()
                info_list = state_data.get("data", {}).get("information", [])
                for info_item in info_list:
                    if info_item.get("infoType") == "order":
                        refs = info_item.get("infoReferences", [])
                        for ref in refs:
                            if ref.get("referenceKey") == "order_id":
                                initial_order_id = ref.get("referenceValue")
                                break
                        desc = info_item.get("infoDescription", "")
                        initial_has_finished = "[order] Finished Order" in desc
                        break
        except Exception as e:
            if callback:
                callback(f"⚠ Lỗi khi lấy initial order_id: {e}\n")
        
        if callback:
            callback(f"📌 Initial order_id: {initial_order_id} (finished: {initial_has_finished})\n")
        
        # === Bước 2: Chờ order_id MỚI + Finished Order ===
        no_change_timeout = 1.5  # Nếu order_id không đổi sau 1.5s → robot đã ở đích
        poll_count = 0
        order_changed = False  # Flag để track xem order_id đã thay đổi chưa
        
        while True:
            elapsed = time.time() - start_time
            if elapsed > timeout:
                if callback:
                    callback(f"⚠ Timeout {timeout}s khi chờ Finished Order\n")
                return False

            try:
                resp, _ = self.get_state(robot_id)
                if resp.status_code == 200:
                    state_data = resp.json()
                    data = state_data.get("data", {})

                    # --- CASE Empty node/edge state: coi như đã đến đích ---
                    # Một số response không cung cấp "[order] Finished Order", nhưng khi
                    # nodeStates/edgeStates rỗng và robot đứng yên thì cho phép gửi order tiếp.
                    node_states = data.get("nodeStates", data.get("node_states"))
                    edge_states = data.get("edgeStates", data.get("edge_states"))

                    safety = data.get("safetyState", data.get("safety_state", {})) or {}
                    e_stop = safety.get("eStop", safety.get("e_stop"))

                    vel = data.get("velocity", {}) or {}
                    vx = float(vel.get("vx", 0) or 0)
                    vy = float(vel.get("vy", 0) or 0)
                    omega = float(vel.get("omega", 0) or 0)

                    is_empty_path = (
                        isinstance(node_states, list) and isinstance(edge_states, list) and
                        len(node_states) == 0 and len(edge_states) == 0
                    )
                    is_safety_ok = (e_stop is None or e_stop == "NONE")
                    is_velocity_stop = (abs(vx) <= 1e-6 and abs(vy) <= 1e-6 and abs(omega) <= 1e-6)

                    if (
                        elapsed >= min_elapsed_for_empty_path and
                        is_empty_path and
                        is_safety_ok and
                        is_velocity_stop
                    ):
                        if callback:
                            callback(
                                f"✅ nodeStates/edgeStates rỗng => coi như hoàn thành (sau {elapsed:.1f}s)\n"
                            )
                        return True

                    info_list = data.get("information", [])
                    
                    for info_item in info_list:
                        if info_item.get("infoType") == "order":
                            # Lấy order_id
                            current_order_id = None
                            refs = info_item.get("infoReferences", [])
                            for ref in refs:
                                if ref.get("referenceKey") == "order_id":
                                    current_order_id = ref.get("referenceValue")
                                    break
                            
                            # Kiểm tra Finished Order
                            desc = info_item.get("infoDescription", "")
                            has_finished = "[order] Finished Order" in desc
                            
                            # Track nếu order_id đã thay đổi
                            if current_order_id and current_order_id != initial_order_id:
                                order_changed = True
                            
                            # CASE 1: order_id MỚI và đã Finished → Done!
                            if current_order_id and current_order_id != initial_order_id and has_finished:
                                if callback:
                                    callback(f"✅ Order mới hoàn thành sau {elapsed:.1f}s (order_id: {current_order_id})\n")
                                return True
                            
                            # CASE 2: order_id KHÔNG ĐỔI sau 1.5s + đã finished → Robot đã ở đích!
                            # CHỈ áp dụng nếu order CHƯA BAO GIỜ đổi (tránh trường hợp order đổi rồi đổi lại)
                            if (elapsed > no_change_timeout and 
                                not order_changed and
                                current_order_id == initial_order_id and 
                                has_finished):
                                if callback:
                                    callback(f"✅ Robot đã ở node đích, không cần di chuyển (sau {elapsed:.1f}s)\n")
                                return True
                            
                            break
            except Exception as e:
                if callback:
                    callback(f"⚠ Lỗi khi poll State: {e}\n")

            # Adaptive polling: nhanh lúc đầu, chậm sau
            poll_count += 1
            if poll_count < 10:
                time.sleep(0.1)  # 10 lần đầu: poll mỗi 0.1s (nhanh!)
            else:
                time.sleep(0.3)  # Sau đó: poll mỗi 0.3s

    def move_multiple_nodes(
        self,
        robot_id,
        nodes,
        delay=1.0,
        callback=None,
        node_actions=None,
        use_polling=True,
        poll_timeout=60,
    ):
        """
        Gửi lần lượt nhiều node.
        node_actions: dict[nodeName] = [ { "type": ..., "height": ... }, ... ]
        use_polling: True = chờ "Finished Order", False = dùng delay cố định
        poll_timeout: timeout khi chờ Finished Order (giây)
        """
        if node_actions is None:
            node_actions = {}

        for i, node in enumerate(nodes, start=1):
            if callback:
                callback(f"===== [{i}/{len(nodes)}] MoveToNode: {node} =====\n")

            # Gửi MoveToNode
            resp, body = self.move_to_node(robot_id, node)
            if callback:
                callback(self._format_response("POST", MOVE_ENDPOINT, resp, body))

            # === Chờ Finished Order (nếu bật polling) ===
            if use_polling:
                if callback:
                    callback(f"⏳ Đang chờ Finished Order cho node '{node}'...\n")
                # Delay nhỏ để đảm bảo server đã xử lý MoveToNode request
                time.sleep(0.2)
                success = self.wait_for_order_finished(
                    robot_id,
                    timeout=poll_timeout,
                    poll_interval=0.5,
                    callback=callback
                )
                if not success:
                    if callback:
                        callback(f"⚠ Không nhận được Finished Order, tiếp tục node tiếp theo\n")
            else:
                # Dùng delay cố định (chế độ cũ)
                if delay > 0 and i < len(nodes):
                    if callback:
                        callback(f"--- Wait {delay} seconds ---\n")
                    time.sleep(delay)

            # === Gửi action cho node này (nếu có) ===
            actions_for_node = node_actions.get(node, [])
            for act in actions_for_node:
                act_type = act.get("type")
                if not act_type:
                    continue

                if callback:
                    callback(f"--> Node '{node}' có action: {act_type}\n")

                if act_type == "liftCameraByHeight":
                    height = act.get("height")
                    if height is None:
                        if callback:
                            callback("⚠ Thiếu height cho liftCameraByHeight, bỏ qua.\n")
                        continue
                    a_resp, a_body = self.call_action_lift_camera_by_height(
                        robot_id, height
                    )
                    if callback:
                        callback(self._format_response("POST", ACTION_ENDPOINT, a_resp, a_body))
                else:
                    if callback:
                        callback(f"⚠ Action '{act_type}' chưa implement trong client.\n")

    # -------- InstantActions ----------
    def call_action_lift_camera_by_height(self, robot_id, height):
        url = self.base_url + ACTION_ENDPOINT

        action_body = {
            "robotId": robot_id,
            "Action": {
                "actionType": "liftCameraByHeight",
                "actionId": "liftCameraByHeight",
                "actionDescription": "liftCamera height (unit: m)",
                "blockingType": "NONE",
                "actionParameters": [
                    {"key": "ROBOT_ID", "value": robot_id},
                    {"key": "HEIGHT", "value": str(height)},
                ]
            }
        }

        resp = requests.post(url, json=action_body, verify=self.verify_ssl)
        return resp, action_body

    # -------- Cancel Move ----------
    def cancel_move(self, robot_id):
        url = f"{self.base_url}{MOVE_ENDPOINT}/{robot_id}"
        resp = requests.delete(url, verify=self.verify_ssl)
        return resp, None

    # -------- Get State ----------
    def get_state(self, robot_id):
        url = f"{self.base_url}{STATE_ENDPOINT}/{robot_id}"
        resp = requests.get(url, verify=self.verify_ssl)
        return resp, None

    # -------- Helper format ----------
    @staticmethod
    def _format_response(method, endpoint, response, body=None):
        parts = [f"{method} {endpoint} -> Status: {response.status_code}\n"]
        if body is not None:
            try:
                parts.append("Request body:\n")
                parts.append(json.dumps(body, indent=2, ensure_ascii=False))
                parts.append("\n")
            except Exception:
                pass
        try:
            js = response.json()
            parts.append("Response JSON:\n")
            parts.append(json.dumps(js, indent=2, ensure_ascii=False))
            parts.append("\n")
        except Exception:
            parts.append("Response text:\n")
            parts.append(response.text)
            parts.append("\n")
        parts.append("-" * 50 + "\n")
        return "".join(parts)


class RobotApp(tk.Tk):
    def __init__(self):
        super().__init__()
        self.title("🤖 Robot Manager Control Panel")
        self.geometry("1450x900")
        self.minsize(1300, 800)

        self.client = RobotApiClient()
        self.gls621 = HikQrReader(callback=self.append_log)  # Khởi tạo reader cho hik-qr-001

        # Premium color scheme - Dark theme with vibrant accents
        self.colors = {
            'bg': '#0a0e1a',           # Very dark blue
            'bg_light': '#131827',     # Lighter dark
            'surface': '#1a1f35',      # Card background
            'surface_hover': '#252b45', # Hover state
            'primary': '#6366f1',      # Indigo
            'primary_light': '#818cf8', # Light indigo
            'success': '#22c55e',      # Vibrant green
            'success_dark': '#16a34a', # Dark green
            'warning': '#f59e0b',      # Orange
            'warning_dark': '#d97706', # Dark orange
            'danger': '#ef4444',       # Red
            'danger_dark': '#dc2626',  # Dark red
            'text': '#f8fafc',         # Almost white
            'text_dim': '#94a3b8',     # Gray
            'text_darker': '#64748b',  # Darker gray
            'border': '#334155',       # Border color
            'accent': '#8b5cf6',       # Purple accent
        }
        
        self.configure(bg=self.colors['bg'])
        
        # Configure modern style
        style = ttk.Style(self)
        style.theme_use("clam")
        
        # Custom styles
        self._configure_styles(style)
        
        self.loop_running = False
        self.current_loop_trial = 0  # Đếm số lần loop
        self._create_widgets()
        
        # Bắt đầu update HIK-QR display
        self.after(100, self.update_gls621_display)
    
    def _configure_styles(self, style):
        """Configure premium custom styles with modern effects"""
        # Frame styles
        style.configure('Card.TFrame', 
                       background=self.colors['surface'], 
                       relief='raised',
                       borderwidth=1)
        style.configure('TFrame', background=self.colors['bg'])
        
        # Label styles
        style.configure('TLabel', 
                       background=self.colors['bg'], 
                       foreground=self.colors['text'],
                       font=('Segoe UI', 10))
        style.configure('Title.TLabel',
                       background=self.colors['surface'],
                       foreground=self.colors['text'],
                       font=('Segoe UI', 11, 'bold'))
        style.configure('Header.TLabel',
                       background=self.colors['bg'],
                       foreground=self.colors['text'],
                       font=('Segoe UI', 14, 'bold'))
        
        # Button styles with gradient effects
        style.configure('Primary.TButton',
                       background=self.colors['primary'],
                       foreground='white',
                       borderwidth=0,
                       focuscolor='none',
                       font=('Segoe UI', 10, 'bold'),
                       padding=(16, 10))
        style.map('Primary.TButton',
                 background=[('active', self.colors['primary_light']), 
                           ('pressed', '#4f46e5')],
                 foreground=[('active', 'white')])
        
        style.configure('Success.TButton',
                       background=self.colors['success'],
                       foreground='white',
                       borderwidth=0,
                       font=('Segoe UI', 10, 'bold'),
                       padding=(16, 10))
        style.map('Success.TButton',
                 background=[('active', '#4ade80'), ('pressed', self.colors['success_dark'])],
                 foreground=[('active', 'white')])
        
        style.configure('Danger.TButton',
                       background=self.colors['danger'],
                       foreground='white',
                       borderwidth=0,
                       font=('Segoe UI', 10, 'bold'),
                       padding=(16, 10))
        style.map('Danger.TButton',
                 background=[('active', '#f87171'), ('pressed', self.colors['danger_dark'])],
                 foreground=[('active', 'white')])
        
        style.configure('Warning.TButton',
                       background=self.colors['warning'],
                       foreground='white',
                       borderwidth=0,
                       font=('Segoe UI', 10, 'bold'),
                       padding=(16, 10))
        style.map('Warning.TButton',
                 background=[('active', '#fbbf24'), ('pressed', self.colors['warning_dark'])],
                 foreground=[('active', 'white')])
        
        # Entry styles with border
        style.configure('TEntry',
                       fieldbackground='#1e293b',
                       foreground=self.colors['text'],
                       borderwidth=1,
                       bordercolor=self.colors['border'],
                       lightcolor=self.colors['border'],
                       darkcolor=self.colors['border'],
                       font=('Segoe UI', 10),
                       padding=8)
        
        # LabelFrame styles with glow effect
        style.configure('Card.TLabelframe',
                       background=self.colors['surface'],
                       foreground=self.colors['text'],
                       borderwidth=1,
                       relief='solid',
                       bordercolor=self.colors['border'],
                       font=('Segoe UI', 11, 'bold'))
        style.configure('Card.TLabelframe.Label',
                       background=self.colors['surface'],
                       foreground=self.colors['primary_light'],
                       font=('Segoe UI', 12, 'bold'))
        
        # Checkbutton styles
        style.configure('TCheckbutton',
                       background=self.colors['surface'],
                       foreground=self.colors['text'],
                       font=('Segoe UI', 10))

    def _create_widgets(self):
        # Premium header with gradient
        header_frame = tk.Frame(self, bg=self.colors['surface'], height=100)
        header_frame.pack(side=tk.TOP, fill=tk.X, padx=0, pady=0)
        header_frame.pack_propagate(False)
        
        # Add subtle border at bottom
        border = tk.Frame(header_frame, bg=self.colors['primary'], height=2)
        border.pack(side=tk.BOTTOM, fill=tk.X)
        
        # Title with icon
        title_label = tk.Label(
            header_frame,
            text="🤖 ROBOT CONTROL PANEL",
            bg=self.colors['surface'],
            fg=self.colors['text'],
            font=('Segoe UI', 22, 'bold')
        )
        title_label.pack(side=tk.TOP, pady=(20, 5))
        
        # Subtitle with better styling
        subtitle = tk.Label(
            header_frame,
            text="#Ngocquynobel",
            bg=self.colors['surface'],
            fg=self.colors['text_dim'],
            font=('Segoe UI', 11)
        )
        subtitle.pack(side=tk.TOP, pady=(0, 10))
        
        # Control bar with shadow effect
        control_frame = tk.Frame(self, bg=self.colors['surface'], highlightthickness=1, 
                                highlightbackground=self.colors['border'])
        control_frame.pack(side=tk.TOP, fill=tk.X, padx=20, pady=(20, 15))
        
        control_inner = tk.Frame(control_frame, bg=self.colors['surface'])
        control_inner.pack(fill=tk.X, padx=20, pady=15)
        
        tk.Label(
            control_inner,
            text="🆔 Robot ID:",
            bg=self.colors['surface'],
            fg=self.colors['text'],
            font=('Segoe UI', 10, 'bold')
        ).grid(row=0, column=0, sticky="w", padx=(0, 10))
        
        self.robot_id_var = tk.StringVar(value="I150")
        robot_entry = tk.Entry(
            control_inner,
            textvariable=self.robot_id_var,
            width=18,
            bg='#1e293b',
            fg=self.colors['text'],
            font=('Consolas', 11, 'bold'),
            relief='solid',
            borderwidth=1,
            highlightthickness=1,
            highlightcolor=self.colors['primary'],
            highlightbackground=self.colors['border'],
            insertbackground=self.colors['primary']
        )
        robot_entry.grid(row=0, column=1, padx=(0, 30), ipady=8)
        
        self.btn_get_state = ttk.Button(
            control_inner,
            text="📊 Get State",
            command=self.on_get_state,
            style='Warning.TButton'
        )
        self.btn_get_state.grid(row=0, column=2, padx=8)
        
        self.btn_cancel_move = ttk.Button(
            control_inner,
            text="🛑 Cancel Move",
            command=self.on_cancel_move,
            style='Danger.TButton'
        )
        self.btn_cancel_move.grid(row=0, column=3, padx=8)

        # Frame giữa với spacing tốt hơn
        mid_frame = ttk.Frame(self)
        mid_frame.pack(side=tk.TOP, fill=tk.BOTH, expand=True, padx=20, pady=(0, 20))

        # --- Cột trái: Move + Action (với scrollbar) ---
        left_outer = ttk.Frame(mid_frame)
        left_outer.pack(side=tk.LEFT, fill=tk.BOTH, expand=False, padx=(0, 15))
        
        # Canvas + Scrollbar cho cột trái
        left_canvas = tk.Canvas(left_outer, bg=self.colors['bg'], highlightthickness=0, width=520)
        left_scrollbar = ttk.Scrollbar(left_outer, orient="vertical", command=left_canvas.yview)
        left_scrollbar.pack(side=tk.RIGHT, fill=tk.Y)
        left_canvas.pack(side=tk.LEFT, fill=tk.BOTH, expand=True)
        left_canvas.configure(yscrollcommand=left_scrollbar.set)
        
        # Frame bên trong canvas
        left_frame = ttk.Frame(left_canvas)
        left_canvas_window = left_canvas.create_window((0, 0), window=left_frame, anchor="nw")
        
        # Update scroll region khi resize
        def on_left_frame_configure(event):
            left_canvas.configure(scrollregion=left_canvas.bbox("all"))
        left_frame.bind("<Configure>", on_left_frame_configure)
        
        # Mouse wheel scroll
        def on_mousewheel(event):
            left_canvas.yview_scroll(int(-1*(event.delta/120)), "units")
        left_canvas.bind_all("<MouseWheel>", on_mousewheel)

        # MoveToNode Card with glow
        move_card = tk.Frame(left_frame, bg=self.colors['surface'],
                            highlightthickness=1, highlightbackground=self.colors['border'])
        move_card.pack(fill=tk.X, pady=(0, 15))
        
        move_labelframe = ttk.LabelFrame(move_card, text="  📍 NAVIGATION  ", style='Card.TLabelframe')
        move_labelframe.pack(fill=tk.X, padx=2, pady=2)
        
        move_inner = tk.Frame(move_labelframe, bg=self.colors['surface'])
        move_inner.pack(fill=tk.BOTH, padx=18, pady=15)

        # Single node
        tk.Label(
            move_inner,
            text="🎯 Single Node:",
            bg=self.colors['surface'],
            fg=self.colors['text'],
            font=('Segoe UI', 10, 'bold')
        ).grid(row=0, column=0, sticky="w", pady=(0, 10))
        
        self.node_var = tk.StringVar(value="Node5")
        node_entry = tk.Entry(
            move_inner,
            textvariable=self.node_var,
            width=15,
            bg='#1e293b',
            fg=self.colors['text'],
            font=('Consolas', 10, 'bold'),
            relief='solid',
            borderwidth=1,
            highlightthickness=1,
            highlightcolor=self.colors['primary'],
            highlightbackground=self.colors['border'],
            insertbackground=self.colors['primary']
        )
        node_entry.grid(row=0, column=1, padx=10, pady=(0, 10), ipady=6)
        
        ttk.Button(
            move_inner,
            text="▶ Move",
            command=self.on_move_single,
            style='Primary.TButton'
        ).grid(row=0, column=2, padx=5, pady=(0, 5))

        # Multi node section
        tk.Label(
            move_inner,
            text="🔗 Multiple Nodes (comma separated):",
            bg=self.colors['surface'],
            fg=self.colors['text'],
            font=('Segoe UI', 10, 'bold')
        ).grid(row=1, column=0, columnspan=3, sticky="w", pady=(15, 8))
        
        self.nodes_multi_var = tk.StringVar(value="Node1, Node2, Node3, Node4, Node5, Node6, Node7, Node8, Node9, Node10, Node11, Node12, Node13, Node14, Node15, Node16, Node15, Node14, Node13 ,Node12, Node11, Node10, Node9, Node8, Node7, Node6, Node5, Node4, Node3, Node2")
        nodes_entry = tk.Entry(
            move_inner,
            textvariable=self.nodes_multi_var,
            width=55,
            bg='#1e293b',
            fg=self.colors['text'],
            font=('Consolas', 10),
            relief='solid',
            borderwidth=1,
            highlightthickness=1,
            highlightcolor=self.colors['primary'],
            highlightbackground=self.colors['border'],
            insertbackground=self.colors['primary']
        )
        nodes_entry.grid(row=2, column=0, columnspan=3, pady=(0, 12), sticky="ew", ipady=6)

        # Polling options với border
        polling_frame = tk.Frame(move_inner, bg=self.colors['bg_light'],
                                highlightthickness=1, highlightbackground=self.colors['border'])
        polling_frame.grid(row=3, column=0, columnspan=3, sticky="ew", pady=(0, 12))
        
        polling_inner = tk.Frame(polling_frame, bg=self.colors['bg_light'])
        polling_inner.pack(fill=tk.X, padx=12, pady=10)
        
        self.use_polling_var = tk.BooleanVar(value=True)
        polling_check = ttk.Checkbutton(
            polling_inner,
            text="⏱ Wait for 'Finished Order' (Smart Polling)",
            variable=self.use_polling_var
        )
        polling_check.configure(style='TCheckbutton')
        polling_check.pack(anchor="w", pady=(0, 8))

        # Timeout và delay
        options_frame = tk.Frame(polling_inner, bg=self.colors['bg_light'])
        options_frame.pack(fill=tk.X)
        
        tk.Label(
            options_frame,
            text="⏰ Timeout:",
            bg=self.colors['bg_light'],
            fg=self.colors['text_dim'],
            font=('Segoe UI', 9)
        ).pack(side=tk.LEFT, padx=(0, 8))
        
        self.poll_timeout_var = tk.StringVar(value="60")
        timeout_entry = tk.Entry(
            options_frame,
            textvariable=self.poll_timeout_var,
            width=8,
            bg='#1e293b',
            fg=self.colors['text'],
            font=('Consolas', 10),
            relief='solid',
            borderwidth=1,
            highlightthickness=0,
            insertbackground=self.colors['primary']
        )
        timeout_entry.pack(side=tk.LEFT, padx=(0, 5), ipady=4)
        
        tk.Label(
            options_frame,
            text="s",
            bg=self.colors['bg_light'],
            fg=self.colors['text_darker'],
            font=('Segoe UI', 9)
        ).pack(side=tk.LEFT, padx=(0, 25))
        
        tk.Label(
            options_frame,
            text="⏲ Delay:",
            bg=self.colors['bg_light'],
            fg=self.colors['text_dim'],
            font=('Segoe UI', 9)
        ).pack(side=tk.LEFT, padx=(0, 8))
        
        self.delay_var = tk.StringVar(value="1.0")
        delay_entry = tk.Entry(
            options_frame,
            textvariable=self.delay_var,
            width=8,
            bg='#1e293b',
            fg=self.colors['text'],
            font=('Consolas', 10),
            relief='solid',
            borderwidth=1,
            highlightthickness=0,
            insertbackground=self.colors['primary']
        )
        delay_entry.pack(side=tk.LEFT, padx=(0, 5), ipady=4)
        
        tk.Label(
            options_frame,
            text="s",
            bg=self.colors['bg_light'],
            fg=self.colors['text_darker'],
            font=('Segoe UI', 9)
        ).pack(side=tk.LEFT)

        # === Node–Action mapping Card ===
        action_card = tk.Frame(left_frame, bg=self.colors['surface'],
                              highlightthickness=1, highlightbackground=self.colors['border'])
        action_card.pack(fill=tk.BOTH, expand=False, pady=(0, 15))
        
        map_labelframe = ttk.LabelFrame(action_card, text="  ⚙️ NODE ACTIONS  ", style='Card.TLabelframe')
        map_labelframe.pack(fill=tk.BOTH, expand=True, padx=2, pady=2)
        
        map_inner = tk.Frame(map_labelframe, bg=self.colors['surface'])
        map_inner.pack(fill=tk.BOTH, expand=True, padx=18, pady=15)

        self.node_action_text = scrolledtext.ScrolledText(
            map_inner,
            width=55,
            height=8,
            font=("Cascadia Code", 9),
            bg='#0a0e1a',
            fg='#e2e8f0',
            insertbackground='#6366f1',
            relief='solid',
            borderwidth=1,
            highlightthickness=0,
            padx=10,
            pady=8
        )
        self.node_action_text.pack(fill=tk.BOTH, expand=True)
        # ví dụ mặc định
        example = (
            # "Node1,liftCameraByHeight,0.45\n"
            # "Node14,liftCameraByHeight,0.75\n"
            # "Node19,liftCameraByHeight,0.55\n"
            # "Node15,liftCameraByHeight,0.35\n"
            # "Node2,liftCameraByHeight,0.2\n"
            # "Node4,liftCameraByHeight,0.55\n"
            # "Node12,liftCameraByHeight,0.45\n"
            # "Node20,liftCameraByHeight,0.35\n"
            # "Node11,liftCameraByHeight,0.65\n"
            # "Node3,liftCameraByHeight,0.35\n"
        )
        self.node_action_text.insert(tk.END, example)

        tk.Label(
            map_inner,
            text="📝 Format: NodeName,actionType,param... (e.g., Node3,liftCameraByHeight,0.52)",
            bg=self.colors['surface'],
            fg=self.colors['text_dim'],
            font=('Segoe UI', 9),
            wraplength=450,
            justify='left'
        ).pack(anchor="w", pady=(8, 0))

        # Buttons: chạy 1 lần & loop với spacing đẹp
        btn_frame = tk.Frame(left_frame, bg=self.colors['bg'])
        btn_frame.pack(fill=tk.X, pady=(8, 15))

        ttk.Button(
            btn_frame,
            text="▶ Run Sequence",
            command=self.on_move_multi,
            style='Success.TButton'
        ).pack(side=tk.LEFT, padx=(0, 10), fill=tk.X, expand=True)
        
        ttk.Button(
            btn_frame,
            text="🔁 Loop Mode",
            command=self.on_loop_multi,
            style='Primary.TButton'
        ).pack(side=tk.LEFT, padx=(0, 10), fill=tk.X, expand=True)
        
        ttk.Button(
            btn_frame,
            text="⏹ Stop",
            command=self.on_stop_loop,
            style='Danger.TButton'
        ).pack(side=tk.LEFT, fill=tk.X, expand=True)

        # InstantActions Card với border
        instant_card = tk.Frame(left_frame, bg=self.colors['surface'],
                               highlightthickness=1, highlightbackground=self.colors['border'])
        instant_card.pack(fill=tk.X)
        
        action_labelframe = ttk.LabelFrame(instant_card, text="  ⚡ INSTANT ACTIONS  ", style='Card.TLabelframe')
        action_labelframe.pack(fill=tk.X, padx=2, pady=2)
        
        action_inner = tk.Frame(action_labelframe, bg=self.colors['surface'])
        action_inner.pack(fill=tk.BOTH, padx=18, pady=15)

        tk.Label(
            action_inner,
            text="Action Type:",
            bg=self.colors['surface'],
            fg=self.colors['text_dim'],
            font=('Segoe UI', 9)
        ).grid(row=0, column=0, sticky="w", pady=(0, 8))
        
        self.inst_action_type_var = tk.StringVar()
        self.inst_action_type_combo = ttk.Combobox(
            action_inner,
            textvariable=self.inst_action_type_var,
            state="readonly",
            width=25,
            values=[
                "liftCameraByHeight",
            ],
            font=('Segoe UI', 10)
        )
        self.inst_action_type_combo.grid(row=0, column=1, columnspan=2, padx=5, pady=(0, 8), sticky="ew")
        self.inst_action_type_combo.current(0)

        tk.Label(
            action_inner,
            text="Height (m):",
            bg=self.colors['surface'],
            fg=self.colors['text_dim'],
            font=('Segoe UI', 9)
        ).grid(row=1, column=0, sticky="w")
        
        self.height_var = tk.StringVar(value="0.52")
        height_entry = tk.Entry(
            action_inner,
            textvariable=self.height_var,
            width=12,
            bg='#1e293b',
            fg=self.colors['text'],
            font=('Consolas', 10, 'bold'),
            relief='solid',
            borderwidth=1,
            highlightthickness=1,
            highlightcolor=self.colors['primary'],
            highlightbackground=self.colors['border'],
            insertbackground=self.colors['primary']
        )
        height_entry.grid(row=1, column=1, padx=10, ipady=6)
        
        ttk.Button(
            action_inner,
            text="🚀 Execute",
            command=self.on_instant_action,
            style='Warning.TButton'
        ).grid(row=1, column=2, padx=5)

        # HIK-QR Detector Card
        gls621_card = tk.Frame(left_frame, bg=self.colors['surface'],
                              highlightthickness=1, highlightbackground=self.colors['border'])
        gls621_card.pack(fill=tk.X, pady=(15, 0))
        
        gls621_labelframe = ttk.LabelFrame(gls621_card, text="  🔍 HIK-QR-001 DETECTOR  ", style='Card.TLabelframe')
        gls621_labelframe.pack(fill=tk.X, padx=2, pady=2)
        
        gls621_inner = tk.Frame(gls621_labelframe, bg=self.colors['surface'])
        gls621_inner.pack(fill=tk.BOTH, padx=18, pady=15)
        
        # Buttons to enable/disable HIK-QR
        gls621_btn_frame = tk.Frame(gls621_inner, bg=self.colors['surface'])
        gls621_btn_frame.pack(fill=tk.X, pady=(0, 12))
        
        ttk.Button(
            gls621_btn_frame,
            text="✅ Enable HIK-QR",
            command=lambda: self.gls621.enable_hik_qr(True),
            style='Success.TButton'
        ).pack(side=tk.LEFT, padx=(0, 8), fill=tk.X, expand=True)
        
        ttk.Button(
            gls621_btn_frame,
            text="❌ Disable HIK-QR",
            command=lambda: self.gls621.enable_hik_qr(False),
            style='Danger.TButton'
        ).pack(side=tk.LEFT, fill=tk.X, expand=True)
        
        # Display current position and error
        tk.Label(
            gls621_inner,
            text="Current Position & Error:",
            bg=self.colors['surface'],
            fg=self.colors['text'],
            font=('Segoe UI', 9, 'bold')
        ).pack(anchor="w", pady=(8, 4))
        
        self.gls621_display_text = tk.Label(
            gls621_inner,
            text="X: --- Y: --- θ: --- Error: ---",
            bg=self.colors['bg_light'],
            fg=self.colors['text'],
            font=('Consolas', 9),
            relief='solid',
            borderwidth=1,
            padx=10,
            pady=8,
            wraplength=450,
            justify='left'
        )
        self.gls621_display_text.pack(fill=tk.X, pady=(0, 12))
        
        # CSV export buttons
        csv_btn_frame = tk.Frame(gls621_inner, bg=self.colors['surface'])
        csv_btn_frame.pack(fill=tk.X, pady=(0, 8))
        
        ttk.Button(
            csv_btn_frame,
            text="💾 Save to CSV",
            command=self.on_save_gls621_csv,
            style='Primary.TButton'
        ).pack(side=tk.LEFT, padx=(0, 8), fill=tk.X, expand=True)
        
        ttk.Button(
            csv_btn_frame,
            text="🗑 Clear Data",
            command=self.on_clear_gls621_data,
            style='Warning.TButton'
        ).pack(side=tk.LEFT, fill=tk.X, expand=True)

        # --- Cột phải: Log Terminal với glow ---
        right_frame = ttk.Frame(mid_frame)
        right_frame.pack(side=tk.RIGHT, fill=tk.BOTH, expand=True)

        log_card = tk.Frame(right_frame, bg=self.colors['surface'],
                           highlightthickness=1, highlightbackground=self.colors['border'])
        log_card.pack(fill=tk.BOTH, expand=True)
        
        log_labelframe = ttk.LabelFrame(log_card, text="  📝 CONSOLE LOG  ", style='Card.TLabelframe')
        log_labelframe.pack(fill=tk.BOTH, expand=True, padx=2, pady=2)
        
        log_inner = tk.Frame(log_labelframe, bg=self.colors['surface'])
        log_inner.pack(fill=tk.BOTH, expand=True, padx=18, pady=15)
        
        self.log_text = scrolledtext.ScrolledText(
            log_inner,
            wrap=tk.WORD,
            height=35,
            font=("Cascadia Code", 9),
            bg='#0a0e1a',
            fg='#e2e8f0',
            insertbackground='#6366f1',
            relief='solid',
            borderwidth=1,
            highlightthickness=0,
            padx=12,
            pady=12
        )
        self.log_text.pack(fill=tk.BOTH, expand=True)

    # ------ Helpers GUI ------
    def append_log(self, text: str):
        self.log_text.insert(tk.END, text)
        self.log_text.see(tk.END)

    def get_robot_id(self) -> str:
        rid = self.robot_id_var.get().strip()
        return rid or "I150"

    def _parse_nodes_and_delay(self):
        nodes_line = self.nodes_multi_var.get().strip()
        if not nodes_line:
            messagebox.showwarning("Thiếu thông tin", "Vui lòng nhập danh sách node.")
            return None, None

        nodes = [n.strip() for n in nodes_line.split(",") if n.strip()]
        if not nodes:
            messagebox.showwarning("Thiếu thông tin", "Danh sách node không hợp lệ.")
            return None, None

        try:
            delay = float(self.delay_var.get().strip())
        except ValueError:
            delay = 1.0

        return nodes, delay

    def _parse_node_actions(self):
        """
        Đọc multi-line Node–Action mapping.
        Trả về dict[nodeName] = [ { "type": ..., "height": ... }, ... ]
        Ví dụ dòng:
          Node3,liftCameraByHeight,0.3
        """
        text = self.node_action_text.get("1.0", tk.END).strip()
        if not text:
            return {}

        node_actions = {}
        lines = text.splitlines()
        for idx, line in enumerate(lines, start=1):
            line = line.strip()
            if not line:
                continue
            parts = [p.strip() for p in line.split(",") if p.strip()]
            if len(parts) < 2:
                self.append_log(f"⚠ Dòng {idx} trong Node–Action mapping không hợp lệ: '{line}'\n")
                continue

            node_name = parts[0]
            action_type = parts[1]

            action_cfg = {"type": action_type}

            # xử lý param tuỳ loại action
            if action_type == "liftCameraByHeight":
                if len(parts) < 3:
                    self.append_log(
                        f"⚠ Dòng {idx} thiếu HEIGHT cho liftCameraByHeight, bỏ qua.\n"
                    )
                    continue
                height_str = parts[2]
                try:
                    float(height_str)
                except ValueError:
                    self.append_log(
                        f"⚠ Dòng {idx}: HEIGHT không hợp lệ '{height_str}', bỏ qua.\n"
                    )
                    continue
                action_cfg["height"] = height_str
            else:
                # các actionType khác, sau này có thể parse thêm param
                self.append_log(f"⚠ Dòng {idx}: action '{action_type}' chưa implement, vẫn lưu type.\n")

            node_actions.setdefault(node_name, []).append(action_cfg)

        return node_actions

    # ------ Button handlers ------
    def on_move_single(self):
        robot_id = self.get_robot_id()
        node = self.node_var.get().strip()
        if not node:
            messagebox.showwarning("Thiếu thông tin", "Vui lòng nhập tên node.")
            return

        def task():
            try:
                self.append_log(f"\n[MoveToNode] robotId={robot_id}, node={node}\n")
                resp, body = self.client.move_to_node(robot_id, node)
                txt = self.client._format_response("POST", MOVE_ENDPOINT, resp, body)
                self.append_log(txt)
            except Exception as e:
                self.append_log(f"Error: {e}\n")

        threading.Thread(target=task, daemon=True).start()

    def on_move_multi(self):
        robot_id = self.get_robot_id()
        nodes, delay = self._parse_nodes_and_delay()
        if nodes is None:
            return

        node_actions = self._parse_node_actions()
        use_polling = self.use_polling_var.get()
        
        try:
            poll_timeout = float(self.poll_timeout_var.get().strip())
        except ValueError:
            poll_timeout = 60.0

        def task():
            mode_str = "Polling mode" if use_polling else f"Delay mode ({delay}s)"
            self.append_log(
                f"\n[MoveMultiNode] robotId={robot_id}, "
                f"nodes={nodes}, {mode_str}\n"
            )
            if use_polling:
                self.append_log(f"Timeout polling: {poll_timeout}s\n")
            self.append_log(f"Node–Action mapping: {node_actions}\n")
            try:
                self.client.move_multiple_nodes(
                    robot_id,
                    nodes,
                    delay=delay,
                    callback=self.append_log,
                    node_actions=node_actions,
                    use_polling=use_polling,
                    poll_timeout=poll_timeout,
                )
            except Exception as e:
                self.append_log(f"Error: {e}\n")

        threading.Thread(target=task, daemon=True).start()

    def on_loop_multi(self):
        """
        Loop qua list node, mỗi vòng:
          - Gọi move_multiple_nodes với node_actions.
          - Sử dụng polling hoặc delay tuỳ setting.
          - GHI DỮ LIỆU HIK-QR-001 tại mỗi node.
        """
        if self.loop_running:
            messagebox.showinfo("Loop", "Loop đã chạy rồi.")
            return

        robot_id = self.get_robot_id()
        nodes, delay = self._parse_nodes_and_delay()
        if nodes is None:
            return

        node_actions = self._parse_node_actions()
        use_polling = self.use_polling_var.get()
        
        try:
            poll_timeout = float(self.poll_timeout_var.get().strip())
        except ValueError:
            poll_timeout = 60.0
        
        self.loop_running = True
        self.current_loop_trial = 0

        def task():
            loop_count = 0
            mode_str = "Polling mode" if use_polling else f"Delay mode ({delay}s)"
            self.append_log(f"\n[LoopMultiNode + HIK-QR-001] Bắt đầu loop... ({mode_str})\n")
            
            # Bật HIK-QR từ đầu
            self.gls621.enable_hik_qr(True)
            self.append_log("🔍 HIK-QR-001 đã bật cho loop\n")
            
            while self.loop_running:
                loop_count += 1
                self.current_loop_trial = loop_count
                self.append_log(
                    f"\n🔁 Loop vòng {loop_count}\n"
                    f"nodes={nodes}, node_actions={node_actions}\n"
                )
                try:
                    for i, node in enumerate(nodes, start=1):
                        if not self.loop_running:  # Check stop
                            break
                        
                        self.append_log(f"===== [{i}/{len(nodes)}] MoveToNode: {node} =====\n")

                        # Gửi MoveToNode
                        resp, body = self.client.move_to_node(robot_id, node)
                        self.append_log(self.client._format_response("POST", MOVE_ENDPOINT, resp, body))

                        # Chờ Finished Order
                        if use_polling:
                            self.append_log(f"⏳ Đang chờ Finished Order cho node '{node}'...\n")
                            time.sleep(0.2)
                            success = self.client.wait_for_order_finished(
                                robot_id,
                                timeout=poll_timeout,
                                poll_interval=0.5,
                                callback=self.append_log
                            )
                            if not success:
                                self.append_log(f"⚠ Không nhận được Finished Order, tiếp tục node tiếp theo\n")
                        else:
                            if delay > 0 and i < len(nodes):
                                self.append_log(f"--- Wait {delay} seconds ---\n")
                                time.sleep(delay)
                        
                        # Chờ HIK-QR ổn định
                        self.append_log("⏳ Chờ HIK-QR-001 ổn định...\n")
                        time.sleep(1.5)
                        
                        # GHI DỮ LIỆU HIK-QR TẠI NODE - CHỜ HOÀN THÀNH MỚI TIẾP TỤC
                        self.append_log(f"💾 Đang ghi dữ liệu HIK-QR-001 cho node {node}...\n")
                        record_success = self.gls621.record_node_data(node, loop_count)
                        
                        # Chờ thêm để đảm bảo dữ liệu được ghi hoàn toàn
                        if record_success:
                            self.append_log(f"✅ Dữ liệu đã được ghi, tiếp tục node tiếp theo\n")
                            time.sleep(0.5)
                        else:
                            self.append_log(f"⚠ Không ghi được dữ liệu, vẫn tiếp tục\n")
                        
                        self.update_gls621_display()

                        # Gửi action cho node
                        actions_for_node = node_actions.get(node, [])
                        for act in actions_for_node:
                            act_type = act.get("type")
                            if not act_type:
                                continue

                            self.append_log(f"--> Node '{node}' có action: {act_type}\n")

                            if act_type == "liftCameraByHeight":
                                height = act.get("height")
                                if height is None:
                                    self.append_log("⚠ Thiếu height cho liftCameraByHeight, bỏ qua.\n")
                                    continue
                                a_resp, a_body = self.client.call_action_lift_camera_by_height(
                                    robot_id, height
                                )
                                self.append_log(self.client._format_response("POST", ACTION_ENDPOINT, a_resp, a_body))
                
                except Exception as e:
                    self.append_log(f"Error trong loop: {e}\n")
                    break

            # Tắt HIK-QR
            self.gls621.enable_hik_qr(False)
            self.append_log("\n🔍 HIK-QR-001 đã tắt\n")
            self.append_log(f"\n[LoopMultiNode] Hoàn thành {loop_count} vòng.\n")

        threading.Thread(target=task, daemon=True).start()

    def on_stop_loop(self):
        """Dừng loop, cancel order, tự động lưu CSV"""
        self.loop_running = False
        robot_id = self.get_robot_id()
        
        # Cancel order đang thực hiện
        self.append_log("\n🛑 Đang dừng loop và cancel order...\n")
        try:
            resp, _ = self.client.cancel_move(robot_id)
            if resp.status_code == 200:
                self.append_log("✅ Order đã được cancel\n")
            else:
                self.append_log(f"⚠ Cancel order HTTP {resp.status_code}\n")
        except Exception as e:
            self.append_log(f"⚠ Lỗi cancel order: {e}\n")
        
        # Tự động lưu CSV nếu có dữ liệu HIK-QR
        if self.gls621.node_data:
            self.append_log("\n💾 Đang tự động lưu dữ liệu HIK-QR-001...\n")
            filename = self.gls621.save_to_csv()
            if filename:
                self.append_log(f"✅ Đã lưu: {filename}\n")
        else:
            self.append_log("⚠ Không có dữ liệu HIK-QR-001 để lưu\n")

    def on_instant_action(self):
        """
        Gửi action độc lập (theo combo Instant Actions).
        Hiện tại chỉ implement liftCameraByHeight.
        """
        robot_id = self.get_robot_id()
        action_type = self.inst_action_type_var.get()

        if not action_type:
            messagebox.showwarning("Thiếu thông tin", "Vui lòng chọn loại Action.")
            return

        if action_type == "liftCameraByHeight":
            height_str = self.height_var.get().strip()
            if not height_str:
                messagebox.showwarning("Thiếu thông tin", "Vui lòng nhập Height.")
                return
            try:
                float(height_str)
            except ValueError:
                messagebox.showwarning("Sai định dạng", "Height phải là số (vd: 0.52).")
                return

            def task():
                try:
                    self.append_log(f"\n[InstantAction] {action_type} height={height_str}\n")
                    resp, body = self.client.call_action_lift_camera_by_height(robot_id, height_str)
                    txt = self.client._format_response("POST", ACTION_ENDPOINT, resp, body)
                    self.append_log(txt)
                except Exception as e:
                    self.append_log(f"Error: {e}\n")

            threading.Thread(target=task, daemon=True).start()

        else:
            messagebox.showinfo(
                "Chưa hỗ trợ",
                f"Action '{action_type}' chưa được implement ở client. Gửi mình JSON mẫu, mình thêm vào cho."
            )

    def on_cancel_move(self):
        robot_id = self.get_robot_id()

        def task():
            try:
                self.append_log(f"\n[CancelMove] robotId={robot_id}\n")
                resp, _ = self.client.cancel_move(robot_id)
                txt = self.client._format_response("DELETE", f"{MOVE_ENDPOINT}/{robot_id}", resp)
                self.append_log(txt)
            except Exception as e:
                self.append_log(f"Error: {e}\n")

        threading.Thread(target=task, daemon=True).start()

    def on_get_state(self):
        robot_id = self.get_robot_id()

        def task():
            try:
                self.append_log(f"\n[GetState] robotId={robot_id}\n")
                resp, _ = self.client.get_state(robot_id)
                txt = self.client._format_response("GET", f"{STATE_ENDPOINT}/{robot_id}", resp)
                self.append_log(txt)
            except Exception as e:
                self.append_log(f"Error: {e}\n")

        threading.Thread(target=task, daemon=True).start()

    def on_save_gls621_csv(self):
        """Lưu dữ liệu HIK-QR-001 vào CSV"""
        if not self.gls621.node_data:
            messagebox.showwarning("Không có dữ liệu", "Chưa có dữ liệu HIK-QR-001 để lưu.")
            return
        
        filename = self.gls621.save_to_csv()
        if filename:
            messagebox.showinfo("Thành công", f"Dữ liệu đã lưu vào:\n{filename}")
            self.append_log(f"✅ CSV file: {filename}\n")

    def on_clear_gls621_data(self):
        """Xóa dữ liệu HIK-QR-001"""
        self.gls621.clear_data()
        self.gls621_display_text.config(text="X: --- Y: --- θ: --- Error: ---")
        self.append_log("🗑 Dữ liệu HIK-QR-001 đã xóa\n")

    def update_gls621_display(self):
        """Cập nhật display vị trí và sai số từ HIK-QR-001"""
        if self.gls621.current_pose:
            x, y, theta = self.gls621.current_pose
            error = self.gls621.current_error
            text = f"X: {x:.4f}m  |  Y: {y:.4f}m  |  θ: {theta:.4f}rad  |  Error: {error:.4f}m"
            self.gls621_display_text.config(text=text)
        else:
            self.gls621_display_text.config(text="X: --- Y: --- θ: --- Error: ---")
        
        # Schedule next update
        self.after(100, self.update_gls621_display)
    
    def on_move_multi_with_gls621(self):
        """Chạy move_multi nhưng lưu dữ liệu HIK-QR-001 tại mỗi node"""
        robot_id = self.get_robot_id()
        nodes, delay = self._parse_nodes_and_delay()
        if nodes is None:
            return
        node_actions = self._parse_node_actions()
        use_polling = self.use_polling_var.get()
        
        try:
            poll_timeout = float(self.poll_timeout_var.get().strip())
        except ValueError:
            poll_timeout = 60.0

        def task():
            mode_str = "Polling mode" if use_polling else f"Delay mode ({delay}s)"
            self.append_log(
                f"\n[MoveMultiNode + HIK-QR-001] robotId={robot_id}, "
                f"nodes={nodes}, {mode_str}\n"
            )
            if use_polling:
                self.append_log(f"Timeout polling: {poll_timeout}s\n")
            
            # Bật HIK-QR trước khi bắt đầu
            self.gls621.enable_hik_qr(True)
            self.append_log("🔍 HIK-QR-001 đã bật\n")
            
            try:
                for i, node in enumerate(nodes, start=1):
                    if not self.loop_running:  # Kiểm tra nếu dừng
                        break
                    
                    self.append_log(f"\n===== [{i}/{len(nodes)}] MoveToNode: {node} =====\n")

                    # Gửi MoveToNode
                    resp, body = self.client.move_to_node(robot_id, node)
                    self.append_log(self.client._format_response("POST", MOVE_ENDPOINT, resp, body))

                    # Chờ Finished Order
                    if use_polling:
                        self.append_log(f"⏳ Đang chờ Finished Order cho node '{node}'...\n")
                        time.sleep(0.2)
                        success = self.client.wait_for_order_finished(
                            robot_id,
                            timeout=poll_timeout,
                            poll_interval=0.5,
                            callback=self.append_log
                        )
                        if not success:
                            self.append_log(f"⚠ Không nhận được Finished Order, tiếp tục node tiếp theo\n")
                    else:
                        if delay > 0 and i < len(nodes):
                            self.append_log(f"--- Wait {delay} seconds ---\n")
                            time.sleep(delay)
                    
                    # Chờ thêm 1s để cảm biến HIK-QR ổn định
                    time.sleep(1.0)
                    
                    # GHI DỮ LIỆU HIK-QR
                    trial = self.current_loop_trial if self.loop_running else 1
                    self.gls621.record_node_data(node, trial)
                    self.update_gls621_display()

                    # Gửi action cho node
                    actions_for_node = node_actions.get(node, [])
                    for act in actions_for_node:
                        act_type = act.get("type")
                        if not act_type:
                            continue

                        self.append_log(f"--> Node '{node}' có action: {act_type}\n")

                        if act_type == "liftCameraByHeight":
                            height = act.get("height")
                            if height is None:
                                self.append_log("⚠ Thiếu height cho liftCameraByHeight, bỏ qua.\n")
                                continue
                            a_resp, a_body = self.client.call_action_lift_camera_by_height(
                                robot_id, height
                            )
                            self.append_log(self.client._format_response("POST", ACTION_ENDPOINT, a_resp, a_body))
                
                # Tắt HIK-QR sau khi hoàn thành
                self.gls621.enable_hik_qr(False)
                self.append_log("\n🔍 HIK-QR-001 đã tắt\n")
                
            except Exception as e:
                self.append_log(f"Error trong move multi: {e}\n")

        threading.Thread(target=task, daemon=True).start()


if __name__ == "__main__":
    app = RobotApp()
    app.mainloop()