"""BlazePose gait analysis."""
import numpy as np
import math
from config import LM, POSE_MODEL

_pose_lm = None


def _load():
    global _pose_lm
    if _pose_lm is None:
        import mediapipe as mp
        opts = mp.tasks.vision.PoseLandmarkerOptions(
            base_options=mp.tasks.BaseOptions(model_asset_path=POSE_MODEL),
            running_mode=mp.tasks.vision.RunningMode.IMAGE,
            num_poses=1,
            min_pose_detection_confidence=0.5,
        )
        _pose_lm = mp.tasks.vision.PoseLandmarker.create_from_options(opts)
    return _pose_lm


def _angle(a, b, c):
    ba = np.array(a) - np.array(b)
    bc = np.array(c) - np.array(b)
    cos = np.dot(ba, bc) / (np.linalg.norm(ba) * np.linalg.norm(bc) + 1e-8)
    return math.degrees(math.acos(np.clip(cos, -1, 1)))


def _vert(top, bot):
    return math.degrees(math.atan2(bot[0] - top[0], bot[1] - top[1]))


def _foot_tilt(heel, toe):
    dx = toe[0] - heel[0]
    dy = -(toe[1] - heel[1])
    return math.degrees(math.atan2(dy, dx))


def analyze(frame_rgb, w, h):
    """Run pose detection and extract gait metrics.

    Returns (gait_dict, landmarks, foot_y) or (None, None, None).
    """
    import mediapipe as mp
    lm_model = _load()
    mp_img = mp.Image(image_format=mp.ImageFormat.SRGB, data=frame_rgb)
    res = lm_model.detect(mp_img)

    if not res.pose_landmarks:
        return None, None, None

    lms = res.pose_landmarks[0]
    foot_y = max(lms[i].y * h for i in [27, 28, 29, 30, 31, 32])

    gait = {}
    for side, pfx in [('L', 'L_'), ('R', 'R_')]:
        p = {n: (lms[LM[f'{pfx}{n}']].x * w, lms[LM[f'{pfx}{n}']].y * h)
             for n in ['SHOULDER', 'HIP', 'KNEE', 'ANKLE', 'HEEL', 'FOOT']}
        gait[f'{side}_knee'] = round(_angle(p['HIP'], p['KNEE'], p['ANKLE']), 1)
        gait[f'{side}_ankle'] = round(_angle(p['KNEE'], p['ANKLE'], p['FOOT']), 1)
        gait[f'{side}_hip'] = round(_angle(p['SHOULDER'], p['HIP'], p['KNEE']), 1)
        gait[f'{side}_shin'] = round(_vert(p['KNEE'], p['ANKLE']), 1)
        gait[f'{side}_lean'] = round(_vert(p['SHOULDER'], p['HIP']), 1)
        gait[f'{side}_foot_tilt'] = round(_foot_tilt(p['HEEL'], p['FOOT']), 1)

    for k in ['knee', 'ankle', 'hip', 'shin', 'lean', 'foot_tilt']:
        gait[f'avg_{k}'] = round((gait[f'L_{k}'] + gait[f'R_{k}']) / 2, 1)
    gait['symmetry'] = round(abs(gait['L_knee'] - gait['R_knee']), 1)

    return gait, lms, foot_y