import math

import cv2
import mediapipe as mp
import time
import numpy as np
ptime=0
# cap=cv2.VideoCapture('videos/2.mp4')
cap=cv2.VideoCapture(0)
mpPose=mp.solutions.pose
pose=mpPose.Pose()
mpDraw=mp.solutions.drawing_utils

#计算角度
def cal_ang(x1,y1,x2,y2,x3,y3):
    a = math.sqrt(
        (x2 - x3) * (x2 - x3) + (y2 - y3) * (y2 - y3))
    b = math.sqrt(
        (x1 - x3) * (x1 - x3) + (y1 - y3) * (y1 - y3))
    c = math.sqrt(
        (x1 - x2) * (x1 - x2) + (y1 - y2) * (y1 - y2))
    A = math.degrees(math.acos((a * a - b * b - c * c) / (-2 * b * c)))
    B = math.degrees(math.acos((b * b - a * a - c * c) / (-2 * a * c)))
    C = math.degrees(math.acos((c * c - a * a - b * b) / (-2 * a * b)))
    return B

count=0
flaglow=flaghigh=-1
while True:
    left_arm=[]
    success,img=cap.read()
    imgRGB=cv2.cvtColor(img,cv2.COLOR_BGR2RGB)
    results=pose.process(imgRGB)
    if results.pose_landmarks:
        #mpDraw.draw_landmarks(img,results.pose_landmarks,mpPose.POSE_CONNECTIONS) #mediapipe画图函数

        for id , lm in enumerate(results.pose_landmarks.landmark):
            h,w,c=img.shape
            cx,cy=int(lm.x*w),int(lm.y*h)
            # cv2.circle(img,(cx,cy),5,(255,255,0),cv2.FILLED)  描绘出所有关节点
            if(id==13 or id ==15 or id ==11):
                cv2.circle(img,(cx,cy),15,(255,255,255),2)  #胳膊三个关节点描绘
                left_arm.append(cx)
                left_arm.append(cy)
        # 三个关节点的连线
        cv2.line(img,(left_arm[0],left_arm[1]),(left_arm[2],left_arm[3]),(255,255,255),3)
        cv2.line(img,(left_arm[4], left_arm[5]), (left_arm[2], left_arm[3]), (255, 255, 255), 3)

        #三个关节点连线形成的角度,并将角度写道角旁边
        angle=cal_ang(left_arm[0],left_arm[1],left_arm[2],left_arm[3],left_arm[4],left_arm[5])
        cv2.putText(img, f'{int(angle)}', (left_arm[2]-30, left_arm[3]-30), cv2.FONT_HERSHEY_PLAIN, 3, (254, 255, 125), 5)
        bar = np.interp(angle,(50,100),(400,200))
        print(bar)
        #print(angle)

        #计数
        if(angle>=125):
            flaglow=0
        if(angle<=50):
            flaghigh=1
        if((flaghigh-flaglow)==1):
            count+=0.5
            flaglow=flaghigh=-1
        cv2.putText(img, f'{int(count)}', (180,70), cv2.FONT_HERSHEY_PLAIN, 3,
                    (254, 255, 125), 5)


        #进度条
        a = (bar-200)/(400-200)*100
        cv2.rectangle(img,(50,400),(100,200),(255,0,255),3)
        cv2.rectangle(img, (50,400), (100,600-int(bar)), (255, 0, 255), cv2.FILLED)
        cv2.putText(img, f'{int(a)} %', (50, 175), cv2.FONT_HERSHEY_PLAIN, 4,
                    (254, 255, 125), 4)

        

    #FPS
    cTime=time.time()
    fps=1/(cTime-ptime)
    ptime=cTime
    cv2.putText(img,str(int(fps)),(10,70),cv2.FONT_HERSHEY_PLAIN,3,(255,0,0),3)



    cv2.imshow('img',img)
    if cv2.waitKey(5) & 0xFF == 27:
        break
cap.release()