import threading
import time
from http.server import HTTPServer, SimpleHTTPRequestHandler
from functools import partial
from pathlib import Path

import numpy as np
from reachy_mini import ReachyMiniApp
from reachy_mini.reachy_mini import ReachyMini
from scipy.spatial.transform import Rotation as R


class ExampleApp(ReachyMiniApp):
    def __init__(self):
        super().__init__()

    def run(self, reachy_mini: ReachyMini, stop_event: threading.Event):
        def start_static_server(host="127.0.0.1", port=8080, directory=None):
            directory = directory or Path(__file__).resolve().parent
            
            server = HTTPServer((host, port), partial(SimpleHTTPRequestHandler, directory=str(directory)))

            print("📄 Open: http://127.0.0.1:8080/stream.html")
            print("📄 3D Viz: http://127.0.0.1:8080/robot_viz.html")
            print()
            server.serve_forever()

        threading.Thread(
            target=start_static_server,
            kwargs={"directory": Path(__file__).parent},
            daemon=True,
        ).start()

        try:
            while not stop_event.is_set():
                now = time.time()
                swing = np.sin(2 * np.pi * 0.3 * now + np.pi)
                bob = np.sin(2 * np.pi * 0.5 * now)

                pose = np.eye(4)
                pose[:3, :3] = R.from_euler("z", 0.5 * swing, degrees=False).as_matrix()
                pose[2, 3] = 0.005 * swing + 0.01 * bob

                antennas = np.full(2, bob)
                reachy_mini.set_target(head=pose, antennas=antennas)
                time.sleep(0.02)
        except KeyboardInterrupt:
            pass


if __name__ == "__main__":
    print("🤖 Connecting to Reachy Mini...")
    try:
        reachy_mini = ReachyMini(
            localhost_only=True,
            spawn_daemon=False,
            use_sim=False,
            timeout=5.0,
            log_level="INFO",
        )
        print("✅ Connected to Reachy Mini!")
        ExampleApp().run(reachy_mini, threading.Event())
    except KeyboardInterrupt:
        print("\n🛑 Interrupted by user")
    except Exception as e:
        print(f"❌ Error connecting to Reachy Mini: {e}")
    finally:
        print("👋 Goodbye!")