silver_pose/v1/evidence.py

"""Pose quality and geometric evidence calculations without alarm decisions."""

from dataclasses import dataclass
from math import acos, degrees, hypot
from typing import Optional

from v1.pose import PersonPose


_TORSO_JOINTS = (5, 6, 11, 12)          # shoulders and hips (used by the geometry)
_LOWER_JOINTS = (13, 14, 15, 16)        # knees and ankles (often occluded in a fall)
_REQUIRED_JOINTS = _TORSO_JOINTS + _LOWER_JOINTS


@dataclass(frozen=True)
class PoseQuality:
    accepted: bool
    reason: str
    visible_joint_count: int


@dataclass(frozen=True)
class PoseEvidence:
    accepted: bool
    horizontal_pose: bool
    rapid_vertical_change: bool
    horizontal_angle_degrees: Optional[float]
    hip_center_y: Optional[float]
    torso_length: Optional[float]
    reason: str


def assess_pose_quality(
    pose: Optional[PersonPose], threshold: float, require_lower_body: bool = True
) -> PoseQuality:
    if not 0.0 <= threshold <= 1.0:
        raise ValueError("threshold must be between 0 and 1")
    if pose is None:
        return PoseQuality(False, "missing_pose", 0)
    if len(pose.keypoints) != 17:
        return PoseQuality(False, "invalid_keypoint_count", 0)
    visible_count = sum(
        point.confidence >= threshold for point in pose.keypoints
    )
    required = _REQUIRED_JOINTS if require_lower_body else _TORSO_JOINTS
    if any(pose.keypoints[index].confidence < threshold for index in required):
        return PoseQuality(False, "required_joint_low_confidence", visible_count)
    return PoseQuality(True, "accepted", visible_count)


def extract_evidence(
    pose: Optional[PersonPose],
    quality: PoseQuality,
    previous: Optional[PoseEvidence],
    rapid_drop_torso_ratio: float = 0.5,
    horizontal_angle_threshold_degrees: float = 35.0,
) -> PoseEvidence:
    """Return geometric facts; a later state machine decides whether to alarm."""

    if not quality.accepted or pose is None:
        return PoseEvidence(False, False, False, None, None, None, quality.reason)
    if rapid_drop_torso_ratio < 0:
        raise ValueError("rapid_drop_torso_ratio must be non-negative")
    shoulder_x, shoulder_y = _midpoint(pose, 5, 6)
    hip_x, hip_y = _midpoint(pose, 11, 12)
    vector_x = hip_x - shoulder_x
    vector_y = hip_y - shoulder_y
    torso_length = hypot(vector_x, vector_y)
    if torso_length == 0:
        return PoseEvidence(False, False, False, None, hip_y, 0.0, "degenerate_torso")
    cosine_to_horizontal = max(-1.0, min(1.0, abs(vector_x) / torso_length))
    horizontal_angle = degrees(acos(cosine_to_horizontal))
    horizontal_pose = horizontal_angle <= horizontal_angle_threshold_degrees
    rapid_vertical_change = False
    if previous is not None and previous.accepted and previous.hip_center_y is not None:
        rapid_vertical_change = (
            hip_y - previous.hip_center_y >= rapid_drop_torso_ratio * torso_length
        )
    return PoseEvidence(
        accepted=True,
        horizontal_pose=horizontal_pose,
        rapid_vertical_change=rapid_vertical_change,
        horizontal_angle_degrees=horizontal_angle,
        hip_center_y=hip_y,
        torso_length=torso_length,
        reason="accepted",
    )


def _midpoint(pose: PersonPose, first_index: int, second_index: int):
    first = pose.keypoints[first_index]
    second = pose.keypoints[second_index]
    return ((first.x + second.x) / 2.0, (first.y + second.y) / 2.0)