import numpy as np
import cv2 as cv
import math
import sys
from demo_func import run_openpose_for_image_front, run_openpose_for_image_side


def angle(v1, v2):
    dx1 = v1[2] - v1[0]
    dy1 = v1[3] - v1[1]
    dx2 = v2[2] - v2[0]
    dy2 = v2[3] - v2[1]
    angle1 = math.atan2(dy1, dx1)
    angle1 = round(angle1 * 180.0 / math.pi, 2)
    # print(angle1)
    angle2 = math.atan2(dy2, dx2)
    angle2 = round(angle2 * 180.0 / math.pi, 2)
    # print(angle2)
    if angle1 * angle2 >= 0:
        included_angle = abs(angle1 - angle2)
    else:
        included_angle = abs(angle1) + abs(angle2)
        # if included_angle > 180:
        #    included_angle = 360 - included_angle
    return included_angle


def angle1(v1, v2):
    dx1 = v1[2] - v1[0]
    dy1 = v1[3] - v1[1]
    angle1 = (float)(math.atan2(dy1, dx1))
    angle2 = abs(round(angle1 * 180.0 / math.pi, 2))
    return angle2


def analyse_npy_side_juanfu(name, num, npy_side):
    result = {}
    data = np.load(npy_side)
    j = 0

    if (len(data) != 18):
        return False
    p = "./cap_file/" + name

    file = open("./cap_file/" + name + "/file2.txt", "a+")
    for i in range(0, len(data)):
        file.write(str(data[i]) + "\n")
    file.close()
    nose_x = data[0][0]
    nose_y = data[0][1]
    shoulder_r_x = data[2][0]
    shoulder_r_y = data[2][1]
    elbow_r_x = data[3][0]
    elbow_r_y = data[3][1]
    wrist_r_x = data[4][0]
    wrist_r_y = data[4][1]

    hip_r_x = data[8][0]
    hip_r_y = data[8][1]
    knee_r_x = data[9][0]
    knee_r_y = data[9][1]
    neck_x = data[1][0]
    neck_y = data[1][1]
    ankle_r_x = data[10][0]
    ankle_r_y = data[10][1]

    leg_heigh = data[10][1]
    neck_hip_angle = round(angle1([neck_x, neck_y, hip_r_x, hip_r_y], [0, 0, 0, -1]), 2)
    nose_neck_angle = round(angle1([nose_x, nose_y, neck_x, neck_y], [0, 0, 0, -1]), 2)
    hip_knee_angle = round(angle1([hip_r_x, hip_r_y, knee_r_x, knee_r_y], [0, 0, 0, -1]), 2)
    knee_ankle_angle = round(angle1([knee_r_x, knee_r_y, ankle_r_x, ankle_r_y], [0, 0, 0, -1]), 2)  # 膝盖到脚

    result["neck_hip_angle"] = neck_hip_angle
    result["nose_neck_angle"] = nose_neck_angle
    result["hip_knee_angle"] = hip_knee_angle
    result["knee_ankle_angle"] = knee_ankle_angle

    print(hip_r_x)
    print(hip_r_y)
    print(knee_r_x)
    print(knee_r_y)

    print(hip_knee_angle)
    print(p)
    # print(npy_side)
    return result


def analyse_npy_side_pingban(name, num, npy_side):
    result = {}
    data = np.load(npy_side)
    j = 0

    if (len(data) < 15):
        return False
    p = "./cap_file/" + name

    file = open("./cap_file/" + name + "/file2.txt", "a+")
    for i in range(0, len(data)):
        file.write(str(data[i]) + "\n")
    file.close()
    shoulder_r_x = data[2][0]
    shoulder_r_y = data[2][1]
    elbow_r_x = data[3][0]
    elbow_r_y = data[3][1]
    wrist_r_x = data[4][0]
    wrist_r_y = data[4][1]

    hip_r_x = data[8][0]
    hip_r_y = data[8][1]
    knee_r_x = data[9][0]
    knee_r_y = data[9][1]
    neck_x = data[1][0]
    neck_y = data[1][1]
    ankle_r_x = data[10][0]
    ankle_r_y = data[10][1]
    knee_ankle_angle = round(angle1([knee_r_x, knee_r_y, ankle_r_x, ankle_r_y], [0, 0, 0, -1]), 2)  # 膝盖到脚
    leg_heigh = data[10][1]
    neck_hip_angle = round(angle1([neck_x, neck_y, hip_r_x, hip_r_y], [0, 0, 0, -1]), 2)
    hip_knee_angle = round(angle1([hip_r_x, hip_r_y, knee_r_x, knee_r_y], [0, 0, 0, -1]), 2)
    s_e_angle = round(angle1([shoulder_r_x, shoulder_r_y, elbow_r_x, elbow_r_y], []), 2)
    e_w_angle = round(angle1([elbow_r_x, elbow_r_y, wrist_r_x, wrist_r_y], []), 2)
    hip_heigh = hip_r_y

    result["knee_ankle_angle"] = knee_ankle_angle
    result["leg_heigh"] = leg_heigh
    result["neck_hip_angle"] = neck_hip_angle
    result["hip_knee_angle"] = hip_knee_angle
    result["hip_heigh"] = hip_heigh
    result["s_e_angle"] = s_e_angle
    result["e_w_angle"] = e_w_angle

    # print(npy_side)
    return result


def analyse_npy_side_jump(name, num, npy_side):
    result = {}
    data = np.load(npy_side)
    j = 0
    file = open("./cap_file/" + name + "/file2.txt", "a+")
    file.write(str(num) + "\n")
    for i in range(0, len(data)):
        file.write(str(data[i]) + "\n")
    file.close()

    if (len(data) != 18):
        return False
    p = "./cap_file/" + name

    # file=open("./cap_file/"+name+"/file2.txt","a+")
    # file.write(str(num)+"\n")
    # for i in range(0,len(data)):
    #    file.write(str(data[i])+"\n")
    # file.close()
    # print(data)
    hip_r_x = data[8][0]
    hip_r_y = data[8][1]
    knee_r_x = data[9][0]
    knee_r_y = data[9][1]
    # print(head_forward_level)
    # upper part of body
    up_risk_level = ""
    up_state = ""
    neck_x = data[1][0]
    neck_y = data[1][1]
    ankle_r_x = data[10][0]
    ankle_r_y = data[10][1]
    knee_ankle_angle = round(angle1([knee_r_x, knee_r_y, ankle_r_x, ankle_r_y], [0, 0, 0, -1]), 2)  # 膝盖到脚
    leg_heigh = data[10][1]
    neck_hip_angle = round(angle1([neck_x, neck_y, hip_r_x, hip_r_y], [0, 0, 0, -1]), 2)
    hip_knee_angle = round(angle1([hip_r_x, hip_r_y, knee_r_x, knee_r_y], [0, 0, 0, -1]), 2)
    hip_heigh = hip_r_y

    result["knee_ankle_angle"] = knee_ankle_angle
    result["leg_heigh"] = leg_heigh
    result["neck_hip_angle"] = neck_hip_angle
    result["hip_knee_angle"] = hip_knee_angle
    result["hip_heigh"] = hip_heigh

    print(npy_side)
    return result


def analyse_npy_side_gaotaitui(name, num, npy_side):
    result = {}
    data = np.load(npy_side)
    j = 0

    file = open("./cap_file/" + name + "/file2.txt", "a+")
    file.write(str(num) + "\n")
    for i in range(0, len(data)):
        file.write(str(data[i]) + "\n")
    file.close()
    if (len(data) != 18):
        return False
    p = "./cap_file/" + name
    # print(data)
    hip_r_x = data[8][0]
    hip_r_y = data[8][1]
    knee_r_x = data[9][0]
    knee_r_y = data[9][1]

    hip_l_x = data[11][0]
    hip_l_y = data[11][1]
    knee_l_x = data[12][0]
    knee_l_y = data[12][1]
    # print(head_forward_level)
    # upper part of body
    up_risk_level = ""
    up_state = ""
    neck_x = data[1][0]
    neck_y = data[1][1]
    ankle_r_x = data[10][0]
    ankle_r_y = data[10][1]
    ankle_l_x = data[13][0]
    ankle_l_y = data[13][1]
    knee_ankle_r_angle = round(angle1([knee_r_x, knee_r_y, ankle_r_x, ankle_r_y], [0, 0, 0, -1]), 2)  # 膝盖到脚
    knee_ankle_l_angle = round(angle1([knee_l_x, knee_l_y, ankle_l_x, ankle_l_y], [0, 0, 0, -1]), 2)  # 膝盖到脚

    hip_knee_r_angle = round(angle1([hip_r_x, hip_r_y, knee_r_x, knee_r_y], [0, 0, 0, -1]), 2)
    hip_knee_l_angle = round(angle1([hip_l_x, hip_l_y, knee_l_x, knee_l_y], [0, 0, 0, -1]), 2)
    neck_hip_angle = round(angle1([neck_x, neck_y, hip_r_x, hip_r_y], [0, 0, 0, -1]), 2)
    # hip_knee_angle=round(angle1([hip_r_x, hip_r_y,knee_r_x, knee_r_y], [0, 0, 0, -1]), 2)
    hip_heigh = hip_r_y

    result["knee_ankle_r_angle"] = knee_ankle_r_angle
    result["knee_ankle_l_angle"] = knee_ankle_l_angle
    result["hip_knee_r_angle"] = hip_knee_r_angle
    result["hip_knee_l_angle"] = hip_knee_l_angle
    result["r_d"] = abs(0 - float(hip_knee_r_angle) + float(knee_ankle_r_angle))
    result["l_d"] = abs(0 + float(knee_ankle_l_angle) - float(hip_knee_l_angle))

    return result


def analyse_npy_side_shendun(name, num, npy_side):
    result = {}
    data = np.load(npy_side)
    j = 0

    file = open("./cap_file/" + name + "/file2.txt", "a+")
    file.write(str(num) + "\n")

    if (len(data) != 18):
        return False
    for i in range(0, len(data)):
        file.write(str(data[i]) + "\n")
    file.close()
    p = "./cap_file/" + name
    # print(data)
    hip_r_x = data[8][0]
    hip_r_y = data[8][1]
    knee_r_x = data[9][0]
    knee_r_y = data[9][1]

    hip_l_x = data[11][0]
    hip_l_y = data[11][1]
    knee_l_x = data[12][0]
    knee_l_y = data[12][1]
    # print(head_forward_level)
    # upper part of body
    up_risk_level = ""
    up_state = ""
    neck_x = data[1][0]
    neck_y = data[1][1]
    ankle_r_x = data[10][0]
    ankle_r_y = data[10][1]
    ankle_l_x = data[13][0]
    ankle_l_y = data[13][1]
    knee_ankle_r_angle = round(angle1([knee_r_x, knee_r_y, ankle_r_x, ankle_r_y], [0, 0, 0, -1]), 2)  # 膝盖到脚
    knee_ankle_l_angle = round(angle1([knee_l_x, knee_l_y, ankle_l_x, ankle_l_y], [0, 0, 0, -1]), 2)  # 膝盖到脚

    hip_knee_r_angle = round(angle1([hip_r_x, hip_r_y, knee_r_x, knee_r_y], [0, 0, 0, -1]), 2)
    hip_knee_l_angle = round(angle1([hip_l_x, hip_l_y, knee_l_x, knee_l_y], [0, 0, 0, -1]), 2)
    neck_hip_angle = round(angle1([neck_x, neck_y, hip_r_x, hip_r_y], [0, 0, 0, -1]), 2)
    # hip_knee_angle=round(angle1([hip_r_x, hip_r_y,knee_r_x, knee_r_y], [0, 0, 0, -1]), 2)
    hip_heigh = hip_r_y

    result["knee_ankle_r_angle"] = knee_ankle_r_angle
    result["knee_ankle_l_angle"] = knee_ankle_l_angle
    result["hip_knee_r_angle"] = hip_knee_r_angle
    result["hip_knee_l_angle"] = hip_knee_l_angle
    result["r_d"] = abs(float(result["hip_knee_r_angle"]) - float(result["knee_ankle_r_angle"]))

    return result