import numpy as np
import math
from models.pose_estimator.pose_estimator_model_setup import get_pose_estimation

def applyPositionTightnessError(filepath, pose_pred=None):
    if pose_pred is None and filepath != "":
        diver_box, pose_pred = get_pose_estimation(filepath)
    if pose_pred is not None:
        pose_pred = np.array(pose_pred)[0]
        vector1 = [pose_pred[7][0] - pose_pred[2][0], pose_pred[7][1] - pose_pred[2][1]]
        
        vector2 = [pose_pred[1][0] - pose_pred[2][0], pose_pred[1][1] - pose_pred[2][1]]
        unit_vector_1 = vector1 / np.linalg.norm(vector1)
        unit_vector_2 = vector2 / np.linalg.norm(vector2)
        dot_product = np.dot(unit_vector_1, unit_vector_2)
        angle = math.degrees(np.arccos(dot_product))
        return angle
    else:
        # print('pose_pred is None')
        return None