import cv2
from biohead.algo import *
import numpy as np


def calc_feature(color, landmarks, ray_origins, ray_directions):
    h, w, _ = color.shape
    face = landmarks.multi_face_landmarks[0].landmark
    idxs = {"left":234, "right":454, "top":10, "bottom":152, "front":1}
    kp = {k: np.array([face[v].x*w, face[v].y*h, face[v].z*w]) for k, v in idxs.items()}
    left, right, top, bottom, front = kp.values()

    right_axis = right - left
    right_axis /= np.linalg.norm(right_axis)
    up_axis = top - bottom
    up_axis /= np.linalg.norm(up_axis)
    forward = np.cross(right_axis, up_axis)
    forward /= np.linalg.norm(forward)
    forward = -forward

    center = (left + right + top + bottom + front) / 5
    ray_origins.append(center)
    ray_directions.append(forward)
    origin = np.mean(ray_origins, axis=0)
    forward = np.mean(ray_directions, axis=0)
    forward /= np.linalg.norm(forward)

    uv = {}
    for j in FaceMap().to_dict().values():
        p = np.array([face[j].x*w, face[j].y*h, face[j].z*w])
        vec = p - origin
        u = np.dot(vec, right_axis)
        v = np.dot(vec, up_axis)
        z = np.dot(vec, forward)
        uv[j] = (u, v, z)
        x2d, y2d = int(p[0]), int(p[1])
        cv2.circle(color, (x2d, y2d), 2, (0, 255, 255), -1)

    left_mouth_corner = np.array([face[FaceMap.left_mouth_corner].x * w, face[FaceMap.left_mouth_corner].y * h, face[FaceMap.left_mouth_corner].z * w])
    right_mouth_corner = np.array([face[FaceMap.right_mouth_corner].x * w, face[FaceMap.right_mouth_corner].y * h, face[FaceMap.right_mouth_corner].z * w])

    # 计算唇角的z值：可以取左右唇角的z值的平均值
    lip_corners_z = (left_mouth_corner[2] + right_mouth_corner[2]) / 2  # 计算左右唇角的z值的平均值

    print(f"唇角的z值为: {lip_corners_z}")

    return uv


def update_text(color, result: HeadJoints):
    y0 = 30
    dy = 30
    font = cv2.FONT_HERSHEY_SIMPLEX
    font_scale = 0.7
    color_text = (0, 255, 255)

    # 眉毛
    r_brow_text = f"R_eyebrow out:{result.right_eyebrow_outside_y:.2f} in:{result.right_eyebrow_inside_y:.2f}"
    l_brow_text = f"L_eyebrow out:{result.left_eyebrow_outside_y:.2f} in:{result.left_eyebrow_inside_y:.2f}"
    cv2.putText(color, r_brow_text, (30, y0), font, font_scale, color_text, 2)
    cv2.putText(color, l_brow_text, (30, y0 + dy), font, font_scale, color_text, 2)
    y0 += 2*dy

    # 眼睛
    r_eye_text = f"R_eyelid U:{result.right_eye_upper_lid_y:.2f} L:{result.right_eye_lower_lid_y:.2f}"
    l_eye_text = f"L_eyelid U:{result.left_eye_upper_lid_y:.2f} L:{result.left_eye_lower_lid_y:.2f}"
    cv2.putText(color, r_eye_text, (30, y0), font, font_scale, color_text, 2)
    cv2.putText(color, l_eye_text, (30, y0 + dy), font, font_scale, color_text, 2)
    y0 += 2 * dy

    # ---- 眼球位置 ----
    l_eyeball_text = f"L_eyeBall X:{result.left_eye_ball_x:.2f} Y:{result.left_eye_ball_y:.2f}"
    r_eyeball_text = f"R_eyeBall X:{result.right_eye_ball_x:.2f} Y:{result.right_eye_ball_y:.2f}"
    cv2.putText(color, r_eyeball_text, (30, y0), font, font_scale, color_text, 2)
    cv2.putText(color, l_eyeball_text, (30, y0 + dy), font, font_scale, color_text, 2)
    y0 += 2*dy

    # ---- 嘴部中央上下 ----
    upper_text = f"UpperLip Y:{result.upper_lip_y:.2f} Z:{result.upper_lip_z:.2f}"
    lower_text = f"LowerLip Y:{result.lower_lip_y:.2f} Z:{result.lower_lip_z:.2f}"
    cv2.putText(color, upper_text, (30, y0), font, font_scale, color_text, 2)
    y0 += dy
    cv2.putText(color, lower_text, (30, y0), font, font_scale, color_text, 2)
    y0 += dy

    # ---- 嘴部左右详细6点 ----
    mouth_points = [
        ("upper_left_lip", result.upper_left_lip_x, result.upper_left_lip_y),
        ("upper_right_lip", result.upper_right_lip_x, result.upper_right_lip_y),
        ("left_corner_lip", result.left_corner_lip_x, result.left_corner_lip_y,result.left_corner_lip_z),
        ("right_corner_lip", result.right_corner_lip_x, result.right_corner_lip_y,result.right_corner_lip_z),
        ("lower_left_lip", result.lower_left_lip_x, result.lower_left_lip_y),
        ("lower_right_lip", result.lower_right_lip_x, result.lower_right_lip_y),
    ]

    print(f"Left corner lip Z: {result.left_corner_lip_z}")
    print(f"Right corner lip Z: {result.right_corner_lip_z}")
    for name, x_val, y_val,z_val in mouth_points:
        text = f"{name}: X:{x_val:.2f} Y:{y_val:.2f} Z:{z_val:.2f}"
        cv2.putText(color, text, (30, y0), font, font_scale, color_text, 2)
        y0 += dy



    # Jaw
    jaw_text = f"Jaw X:{result.jaw_x:.2f} Y:{result.jaw_y:.2f}"
    cv2.putText(color, jaw_text, (30, y0), font, font_scale, color_text, 2)
    y0 += dy


def norm(result: HeadJoints, interval, clamp=True):
    """
    Normalize result fields to 0-1 range based on interval spec.

    Args:
        result: HeadJoints instance
        interval: dict with key -> [min, max]
        clamp: if True, limit outputs to [0,1]

    Returns:
        dict of normalized values
    """
    result_dict = result.to_dict()
    normalized = HeadJoints()
    for key, value in result_dict.items():
        if key in interval:
            min_val, max_val = interval[key]
            denom = max_val - min_val + 1e-6
            norm_val = (value - min_val) / denom
            if clamp:
                norm_val = max(0.0, min(1.0, norm_val))
            setattr(normalized, key, norm_val)
        else:
            setattr(normalized, key, value)

    return normalized