import math

from MNPoseDetection.consensus import PASS_TEST


def euclidean_distance(p1, p2):
    return math.sqrt((p1[0] - p2[0]) ** 2 + (p1[1] - p2[1]) ** 2)


def angle_between_points(p1, p2, p3):
    a = euclidean_distance(p2, p3)
    b = euclidean_distance(p1, p3)
    c = euclidean_distance(p1, p2)
    if a * c == 0:  # Prevent division by zero
        return 0
    return math.degrees(math.acos((b ** 2 - a ** 2 - c ** 2) / (-2 * a * c)))


def valid_skeleton_judge(data, shoulder_hip_tolerance=50, shin_thigh_tolerance=55, arm_forearm_tolerance=60):
    # 骨架检查
    # 肩膀和胯宽比较
    shoulder_width = euclidean_distance(data['left_shoulder']['key_points'], data['right_shoulder']['key_points'])
    hip_width = euclidean_distance(data['left_hip']['key_points'], data['right_hip']['key_points'])
    if not (abs(shoulder_width - hip_width) < shoulder_hip_tolerance):
        valid = False
        # Shoulder width is greater than hip width.
        data['left_shoulder'][PASS_TEST] = False
        data['right_shoulder'][PASS_TEST] = False
        data['left_hip'][PASS_TEST] = False
        data['right_hip'][PASS_TEST] = False

    # 小腿和大腿比较
    left_shin = euclidean_distance(data['left_knee']['key_points'], data['left_ankle']['key_points'])
    left_thigh = euclidean_distance(data['left_hip']['key_points'], data['left_knee']['key_points'])
    if not (abs(left_shin - left_thigh) < shin_thigh_tolerance):
        valid = False
        # Left shin is longer than left thigh.
        data['left_knee'][PASS_TEST] = False
        data['left_ankle'][PASS_TEST] = False
        data['left_hip'][PASS_TEST] = False

    right_shin = euclidean_distance(data['right_knee']['key_points'], data['right_ankle']['key_points'])
    right_thigh = euclidean_distance(data['right_hip']['key_points'], data['right_knee']['key_points'])
    if not (abs(right_shin - right_thigh) < shin_thigh_tolerance):
        valid = False
        # Right shin is longer than right thigh.
        data['right_knee'][PASS_TEST] = False
        data['right_ankle'][PASS_TEST] = False
        data['right_hip'][PASS_TEST] = False

    left_upper_arm = euclidean_distance(data['left_shoulder']['key_points'], data['left_elbow']['key_points'])
    left_forearm = euclidean_distance(data['left_elbow']['key_points'], data['left_wrist']['key_points'])
    if not (abs(left_upper_arm - left_forearm) < arm_forearm_tolerance):
        valid = False
        # Left forearm is longer than upper arm.
        data['left_shoulder'][PASS_TEST] = False
        data['left_elbow'][PASS_TEST] = False
        data['left_wrist'][PASS_TEST] = False

    right_upper_arm = euclidean_distance(data['right_shoulder']['key_points'], data['right_elbow']['key_points'])
    right_forearm = euclidean_distance(data['right_elbow']['key_points'], data['right_wrist']['key_points'])
    if not (abs(right_upper_arm - right_forearm) < arm_forearm_tolerance):
        valid = False
        # Right forearm is longer than upper arm.
        data['right_shoulder'][PASS_TEST] = False
        data['right_wrist'][PASS_TEST] = False
        data['right_elbow'][PASS_TEST] = False

    return data