import numpy as np
import json
import cv2
import threading
import time

from dataclasses import dataclass, field, asdict
from typing import List, Optional, Tuple, Dict

from pygrabber.dshow_graph import FilterGraph


class DShowMJPGCapture:
    """
    cv2.VideoCapture-like wrapper (read/release/isOpened) that selects an exact
    DirectShow media type (e.g. MJPG at a given resolution) before capturing.

    This exists because cv2.VideoCapture(..., cv2.CAP_DSHOW).set(CAP_PROP_FOURCC, ...)
    is unreliable on some webcams: OpenCV opens its own separate DirectShow filter
    graph, which renegotiates its own format independently of anything set through
    pygrabber beforehand. Doing format selection AND frame grabbing in the same
    graph (via pygrabber's sample grabber) avoids that.
    """

    def __init__(self, index: int, width: int, height: int,
                 fourcc_substr: str = "MJPG", timeout: float = 2.0):
        self.index = index
        self.width = width
        self.height = height
        self.selected_format = None
        self._opened = False
        self._latest_frame = None
        self._frame_event = threading.Event()
        self._timeout = timeout

        self._graph = FilterGraph()
        self._graph.add_video_input_device(index)

        device = self._graph.get_input_device()
        formats = device.get_formats()
        match = next(
            (f for f in formats
             if f["width"] == width and f["height"] == height
             and fourcc_substr.upper() in f["media_type_str"].upper()),
            None,
        )
        if match is None:
            raise RuntimeError(
                f"No {fourcc_substr} format at {width}x{height} available on camera {index}. "
                f"Available formats: {formats}"
            )
        device.set_format(match["index"])
        self.selected_format = match

        self._graph.add_sample_grabber(self._on_frame)
        self._graph.add_null_render()
        self._graph.prepare_preview_graph()
        self._graph.run()
        self._opened = True

    def _on_frame(self, frame):
        self._latest_frame = frame
        self._frame_event.set()

    def isOpened(self) -> bool:
        return self._opened

    def read(self):
        if not self._opened:
            return False, None
        self._frame_event.clear()
        self._graph.grab_frame()
        if not self._frame_event.wait(self._timeout):
            return False, None
        return True, self._latest_frame

    def release(self):
        if self._opened:
            self._graph.stop()
            self._graph.remove_filters()
            self._opened = False

    # No-ops for compatibility with code paths that still call .set()/.get()
    # on the capture object (real config happens via device.set_format above).
    def set(self, prop_id, value):
        return False

    def get(self, prop_id):
        return 0.0


# ----- Helper Functions -----
def compute_distance(p1, p2):
    return np.hypot(p2[0] - p1[0], p2[1] - p1[1])

# 4) compute straightness per finger
def finger_straight(i_tip, i_pip, i_mcp):
    d1 = np.hypot(*(np.array(i_tip)-np.array(i_pip)))
    d2 = np.hypot(*(np.array(i_pip)-np.array(i_mcp)))
    return float(np.clip(d1/(d2+1e-3), 0, 1))


def default_profile():
    return []

@dataclass
class CameraObject():
    capture: Optional[cv2.VideoCapture]
    index: int
    pxHeight: int = 1536
    pxWidth: int = 2048

    # Calibration
    camera_matrix: Optional[np.ndarray] = None          # 3x3
    distortion_coefficients: Optional[np.ndarray] = None       # 1x5 / 1x8
    rotation_matrix_world_to_camera: Optional[np.ndarray] = None          # 3x3 (world->cam)
    translation_vector_world_to_camera: Optional[np.ndarray] = None          # 3x1 (world->cam)
    camera_projection_matrix: Optional[np.ndarray] = None

    def load_video_capture(self):
        #self.capture = cv2.VideoCapture(self.index, cv2.CAP_MSMF)
        self.capture = cv2.VideoCapture(self.index, cv2.CAP_ANY)
        #self.capture = cv2.VideoCapture(self.index, cv2.CAP_DSHOW)
        self.capture.set(cv2.CAP_PROP_FRAME_WIDTH, 2048.0)
        self.capture.set(cv2.CAP_PROP_FRAME_HEIGHT, 1536.0)

    def open(self, width: int = 1920, height: int = 1080, max_attempts: int = 5) -> bool:
        """
        Open this camera on demand via MSMF. Only MSMF has been able to open all
        6 cameras on this rig at all (DSHOW hits a hard concurrent-instance limit
        around 2-3 devices on this hardware) - the tradeoff is MSMF cameras should
        not be kept open simultaneously in large numbers, so callers are expected
        to open() right before use and close() right after.
        """
        if self.capture is not None:
            return True

        for attempt in range(1, max_attempts + 1):
            capture = cv2.VideoCapture(self.index, cv2.CAP_MSMF)
            if capture.isOpened():
                capture.set(cv2.CAP_PROP_FRAME_WIDTH, width)
                capture.set(cv2.CAP_PROP_FRAME_HEIGHT, height)
                capture.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter_fourcc(*'MJPG'))
                self.capture = capture
                self.pxWidth = width
                self.pxHeight = height
                return True

            capture.release()
            time.sleep(0.3 * attempt)

        return False

    def close(self):
        if self.capture is not None:
            self.capture.release()
            self.capture = None


@dataclass
class Hand2D():
    camera_id: str                   # e.g. "overhead_left"
    handedness: str                  # "Left" / "Right"
    score: float                     # detection/tracking confidence
    landmarks_px: np.ndarray         # shape (21,2) in pixels


@dataclass
class CharucoDetection:
    camera_id: int
    
    # Output of Charuco detection
    charuco_corners: np.ndarray    # (N, 1, 2) float32
    charuco_ids: np.ndarray        # (N, 1) int32

    gray_frame: Optional[np.ndarray] = None


@dataclass
class CalibrationSample:
    detections: dict[int, CharucoDetection]  # camera_id → detection