import numpy as np


import numpy as np

def angle_between(pkt1, pkt2, pkt3):
    """
    Oblicza kąt między trzema punktami w stopniach z zachowaniem znaku.
    pkt2 jest wierzchołkiem kąta.

    Parameters:
        pkt1, pkt2, pkt3 : array-like (x, y) lub (x, y, z)

    Returns:
        Kąt w stopniach (ujemny lub dodatni)
    """

    pkt1 = np.array(pkt1[:2].cpu().numpy())
    pkt2 = np.array(pkt2[:2].cpu().numpy())
    pkt3 = np.array(pkt3[:2].cpu().numpy())

    # wektory względem pkt2
    a = pkt1 - pkt2
    b = pkt3 - pkt2

    # iloczyn skalarny i cosinus kąta
    dot = np.dot(a, b)
    norm = np.linalg.norm(a) * np.linalg.norm(b)
    cos_theta = dot / norm
    cos_theta = np.clip(cos_theta, -1.0, 1.0)

    # kąt bez znaku
    angle = np.degrees(np.arccos(cos_theta))

    # znak z iloczynu wektorowego (w 2D to skalar = z-component)
    cross = a[0]*b[1] - a[1]*b[0]

    if cross < 0:
        angle = -angle

    return angle

def compare_poses(f1, f2):
    # Odległość euklidesowa
    l2_dist = np.linalg.norm(f1 - f2)

    # Cosine similarity
    cos_sim = np.dot(f1, f2) / (np.linalg.norm(f1) * np.linalg.norm(f2) + 1e-6)

    return l2_dist, cos_sim

def compare_poses_boolean(f1, f2):
    l2, cos_sim = compare_poses(f1, f2)

    return l2 < 0.7 and cos_sim > 0.90

def center(keypoints):
    mid_hip = (keypoints[11] + keypoints[12]) / 2  # left_hip=11, right_hip=12
    keypoints = keypoints - mid_hip

    return keypoints

def normalize_pose(keypoints):
    """
    keypoints: np.array shape (17, 2) [x,y] dla COCO
    Zwraca wektor cech odporny na skalę i przesunięcie
    """

    # 1. translacja -> środek bioder jako początek układu
    mid_hip = (keypoints[11] + keypoints[12]) / 2  # left_hip=11, right_hip=12
    keypoints = keypoints - mid_hip

    # 2. normalizacja skali -> odległość między barkami
    shoulder_dist = np.linalg.norm(keypoints[5] - keypoints[6])  # left_shoulder=5, right_shoulder=6
    if shoulder_dist > 0:
        keypoints = keypoints / shoulder_dist

    # 3. definicja segmentów (przykład: łokieć-ramię, nadgarstek-łokieć)
    limbs = [
        (5, 7),  # ramię L
        (7, 9),  # przedramię L
        (6, 8),  # ramię P
        (8, 10),  # przedramię P
        (11, 13),  # udo L
        (13, 15),  # goleń L
        (12, 14),  # udo P
        (14, 16),  # goleń P
    ]

    # 4. oblicz kąty
    angles = []
    for (a, b), (c, d) in zip(limbs[::2], limbs[1::2]):  # np. (ramię, przedramię)
        v1 = keypoints[b] - keypoints[a]
        v2 = keypoints[d] - keypoints[c]
        cos_angle = np.dot(v1, v2) / (np.linalg.norm(v1) * np.linalg.norm(v2) + 1e-6)
        angle = np.arccos(np.clip(cos_angle, -1, 1))
        angles.append(angle)

    # 5. opcjonalnie: dodać wektory kończyn (znormalizowane)
    vectors = []
    for (a, b) in limbs:
        v = keypoints[b] - keypoints[a]
        v_norm = v / (np.linalg.norm(v) + 1e-6)
        vectors.extend(v_norm)

    # finalny wektor cech = kąty + wektory
    feature_vector = np.concatenate([angles, vectors])

    return feature_vector


def denormalize_pose(feature_vector):
    """
    feature_vector: wynik normalize_pose
    Zwraca przybliżone współrzędne keypoints (w układzie znormalizowanym)
    """
    # 1. oddziel kąty i wektory
    angles = feature_vector[:4]
    vectors_flat = feature_vector[4:]
    vectors = vectors_flat.reshape(-1, 2)

    # 2. inicjalizacja keypoints
    keypoints = np.zeros((17, 2))

    # 3. przybliżona rekonstrukcja kończyn
    limbs = [
        (5, 7), (7, 9), (6, 8), (8, 10),
        (11, 13), (13, 15), (12, 14), (14, 16)
    ]

    for (a, b), v in zip(limbs, vectors):
        keypoints[b] = keypoints[a] + v  # przybliżona rekonstrukcja

    # 4. punkt startowy (biodra) = (0,0), skalowanie w oryginale trzeba by przywrócić osobno
    return keypoints