python3 -m venv venv
source venv/bin/activate
pip install opencv-python-headless numpy flask matplotlib scipy pillow

#!/usr/bin/env python3

import time
import threading
import math
import io
import base64
from typing import List, Tuple
import numpy as np
import cv2
from flask import Flask, Response, render_template_string, jsonify
import matplotlib
matplotlib.use('Agg')
import matplotlib.pyplot as plt
from PIL import Image
import heapq

app = Flask(__name__)

class SensorHealth:
    def __init__(self):
        t = time.time()
        self.last_imu = t
        self.last_odom = t
        self.last_cam = t
        self.last_lidar = t

    def update(self, imu=False, odom=False, cam=False, lidar=False):
        now = time.time()
        if imu: self.last_imu = now
        if odom: self.last_odom = now
        if cam: self.last_cam = now
        if lidar: self.last_lidar = now

    def ok(self, timeout=1.0):
        now = time.time()
        return {
            'imu': (now - self.last_imu) < timeout,
            'odom': (now - self.last_odom) < timeout,
            'cam': (now - self.last_cam) < timeout,
            'lidar': (now - self.last_lidar) < timeout
        }

def read_imu():
    wz = np.random.normal(0.0, 0.02)
    ax = np.random.normal(0.0, 0.1)
    return float(wz), float(ax)

def read_wheel_odometry():
    d = 0.02 + np.random.normal(0.0, 0.004)
    dtheta = np.random.normal(0.0, 0.004)
    return float(d), float(dtheta)

def read_lidar_scan(num_beams=180, max_range=5.0):
    angles = np.linspace(-np.pi/2, np.pi/2, num_beams)
    ranges = max_range * np.ones_like(angles)
    center_idx = num_beams // 2
    ranges[center_idx-3:center_idx+4] = 1.5 + np.random.normal(0.0, 0.03, 7)
    ranges += np.random.normal(0.0, 0.02, size=angles.shape)
    ranges = np.clip(ranges, 0.02, max_range)
    return ranges.astype(float), angles.astype(float)

class CameraWrapper:
    def __init__(self, source=0, width=320, height=240):
        self.cap = cv2.VideoCapture(source)
        self.cap.set(cv2.CAP_PROP_FRAME_WIDTH, width)
        self.cap.set(cv2.CAP_PROP_FRAME_HEIGHT, height)
        self.last_frame = None

    def read(self):
        ret, frame = self.cap.read()
        if not ret or frame is None:
            frame = np.zeros((240,320,3), dtype=np.uint8)
        self.last_frame = frame
        return frame

    def release(self):
        self.cap.release()

class EKF:
    def __init__(self):
        self.x = np.zeros((3,1)) # x, y, theta
        self.P = np.diag([0.1, 0.1, 0.05])  # noise covariance
        self.Q = np.diag([0.02, 0.02, 0.01])  # process noise

    def predict(self, delta_dist, delta_theta):
        x, y, th = self.x.flatten()
        th_new = th + delta_theta
        x_new = x + delta_dist * np.cos(th_new)
        y_new = y + delta_dist * np.sin(th_new)
        self.x = np.array([[x_new],[y_new],[th_new]])
        self.P += self.Q

    def update_pose(self, z, R):
        H = np.eye(3)
        y = z - H.dot(self.x)
        y[2] = (y[2] + np.pi) % (2*np.pi) - np.pi
        S = H.dot(self.P).dot(H.T) + R
        K = self.P.dot(H.T).dot(np.linalg.inv(S))
        self.x += K.dot(y)
        self.P = (np.eye(3) - K.dot(H)).dot(self.P)

    def get_state(self):
        return self.x.flatten()

class OccupancyGrid:
    def __init__(self, width_m=10.0, height_m=10.0, resolution=0.05):
        self.resolution = resolution
        self.width = int(width_m / resolution)
        self.height = int(height_m / resolution)
        self.log_odds = np.zeros((self.height, self.width), dtype=np.float32)
        self.l_free = -0.4
        self.l_occ = 0.85
        self.origin_x = -width_m / 2.0
        self.origin_y = -height_m / 2.0

    def integrate_scan(self, pose: Tuple[float,float,float], ranges: np.ndarray, angles: np.ndarray):
        # Implement logic to integrate Lidar scan into occupancy grid
        pass

class PurePursuitController:
    def __init__(self, lookahead_m=0.3, max_speed=0.2):
        self.lookahead = lookahead_m
        self.max_speed = max_speed

    def compute_control(self, pose: Tuple[float,float,float], path_world: List[Tuple[float,float]], obstacle_dist: float):
        # Implement control logic
        pass

@app.route('/')
def index():
    return render_template_string("""
    <h1>Robot Dashboard</h1>
    <p>Đang chạy robot không GPS.</p>
    """)

@app.route('/state')
def state():
    return jsonify(_state)

def run_dashboard_server():
    app.run(host='0.0.0.0', port=5000, debug=False, use_reloader=False)

def main_loop(rate_hz=10):
    dt = 1.0 / rate_hz
    ekf = EKF()
    vo = VisualOdometry()
    grid = OccupancyGrid()
    controller = PurePursuitController()
    sensor_health = SensorHealth()
    cam = CameraWrapper()

    while True:
        # Read sensors and update state
        time.sleep(dt)