"""Reachy Mini Hand Tracker App

This app makes Reachy Mini follow your hand.

It uses the robot camera to detect the hand position and adjusts the head pose accordingly.
It uses mediapipe to track the hand and OpenCV for image processing.
"""

import threading

import cv2
import numpy as np
from reachy_mini import ReachyMini, ReachyMiniApp
from reachy_mini.io.cam_utils import find_camera
from scipy.spatial.transform import Rotation as R

from reachy_mini_hand_tracker_app.hand_tracker import HandTracker


class HandTrackerApp(ReachyMiniApp):
    # Proportional gain for the controller
    # Reduce/Increase to make the head movement smoother or more responsive)
    kp = 0.2

    # Maximum delta for the head position adjustments
    # This limits how much the head can move in one iteration to prevent abrupt movements
    max_delta = 0.3

    # Proportional gains for the head height adjustment
    # Adjust this value to control how much the head moves up/down based on vertical error
    kz = 0.04

    def run(self, reachy_mini: ReachyMini, stop_event: threading.Event):
        cap = find_camera()
        if cap is None:
            raise RuntimeError("No camera found. Please connect a camera.")

        hand_tracker = HandTracker()

        head_pose = np.eye(4)
        euler_rot = np.array([0.0, 0.0, 0.0])

        while not stop_event.is_set():
            success, img = cap.read()

            if not success:
                print("Failed to capture image from camera.")
                continue

            hands = hand_tracker.get_hands_positions(img)
            if hands:
                hand = hands[0]  # Assuming we only track the first detected hand
                draw_hand(img, hand)

                error = np.array([0, 0]) - hand
                error = np.clip(
                    error, -self.max_delta, self.max_delta
                )  # Limit error to avoid extreme movements
                euler_rot += np.array(
                    [0.0, -self.kp * 0.1 * error[1], self.kp * error[0]]
                )

                head_pose[:3, :3] = R.from_euler(
                    "xyz", euler_rot, degrees=False
                ).as_matrix()
                head_pose[:3, 3][2] = (
                    error[1] * self.kz
                )  # Adjust height based on vertical error

                reachy_mini.set_target(head=head_pose)

            cv2.imshow("Reachy Mini Hand Tracker App", img)
            if cv2.waitKey(1) & 0xFF == ord("q"):
                break


def draw_hand(img, hand):
    """Draw debug information on the image."""
    h, w, _ = img.shape
    draw_palm = [(-hand[0] + 1) / 2, (hand[1] + 1) / 2]  # [0, 1]
    cv2.circle(
        img,
        (int(w - draw_palm[0] * w), int(draw_palm[1] * h)),
        radius=5,
        color=(0, 0, 255),
        thickness=-1,
    )

    _target = [0.5, 0.5]
    cv2.circle(
        img,
        (int(_target[0] * w), int(_target[1] * h)),
        radius=5,
        color=(255, 0, 0),
        thickness=-1,
    )

    cv2.line(
        img,
        (int(draw_palm[0] * w), int(draw_palm[1] * h)),
        (int(_target[0] * w), int(_target[1] * h)),
        color=(0, 255, 0),
        thickness=2,
    )


if __name__ == "__main__":
    # You can run the app directly from this script
    with ReachyMini() as mini:
        app = HandTrackerApp()

        stop = threading.Event()

        try:
            print("Running '{{ app_name }}' a ReachyMiniApp...")
            print("Press Ctrl+C to stop the app.")
            app.run(mini, stop)
            print("App has stopped.")

        except KeyboardInterrupt:
            print("Stopping the app...")
            stop.set()