"""
@file : hpe_classification.py
@author: yunikim
@license: DAOOLDNS
@brief: HPE 자세 추정 알고리즘.
        작업중지동작(cross arm) 및 넘어짐 감지.
        기존의 두 선분의 교차여부, 교차점 좌표 및 해당 좌표의 각 선분 상 위치 비율을 얻는 로직을 통합함.

@section Modify History
- 2023-06-21 yunikim base

"""

import math
import numpy as np
from hpe_classification import config
import logging
import project_config


class HPETypeMask:
    NORMAL = 0x0000
    FALL_DOWN = 0x0080
    CROSS_ARM = 0x0100


class HPEClassification:
    # keypoint는 실제 사용번호와 index가 다르다.
    # index 0~16 => keypoint 1~17
    class __KPIndex:
        NOSE = 0  # 코
        RIGHT_EYE = 1  # 눈
        LEFT_EYE = 2
        RIGHT_EAR = 3  # 귀
        LEFT_EAR = 4
        RIGHT_SHOULDER = 5  # 어깨
        LEFT_SHOULDER = 6
        RIGHT_ELBOW = 7  # 팔꿈치
        LEFT_ELBOW = 8
        RIGHT_WRIST = 9  # 손목
        LEFT_WRIST = 10
        RIGHT_HIP = 11  # 엉덩이
        LEFT_HIP = 12
        RIGHT_KNEE = 13  # 무릎
        LEFT_KNEE = 14
        RIGHT_FEET = 15  # 발
        LEFT_FEET = 16
        MAX = 17

    class __HPELevel:
        NORMAL = 0
        FALLDOWN = 8
        CROSSARM = 9

    # parameter:
    #       pose_info: <dict> {
    #           'person': (cx, cy, w, h, c),
    #           'keypoints': [ (x, y, c), None, (x, y, c), ... ]    # 1번이 index 0, 2번 index 1, ... 17번 index 16
    #       }
    def __init__(self, pose_info: dict,
                 cross_ratio_threshold: float = config.CROSS_ARM_RATIO_THRESHOLD,
                 cross_angle_threshold: tuple = config.CROSS_ARM_ANGLE_THRESHOLD,
                 arm_angle_threshold: tuple = config.ARM_ANGLE_THRESHOLD, 
                 elbow_angle_threshold: tuple = config.ELBOW_ANGLE_THRESHOLD, 
                 falldown_tilt_ratio: float = config.FALLDOWN_TILT_RATIO,
                 falldown_tilt_angle: float = config.FALLDOWN_TILT_ANGLE,
                 body_tilt_ratio: float = config.BODY_TILT_RATIO,
                 yolo_version: int = config.DEFAULT_YOLO_VERSION):
        """
        :param pose_info: HPE 좌표 정보
        :param cross_ratio_threshold: 팔 교차시 최소 비율
        :param cross_angle_threshold: 팔 교차시 검출 각도 범위. (min, max)
        :param arm_angle_threshold: 팔 각도 범위. (min, max)
        :param elbow_angle_threshold: 팔꿈치 각도 범위.  (min, max)
        :param falldown_tilt_ratio: 넘어짐 감지를 위한 비율
        :param falldown_tilt_angle: 작업중 상체 구부러짐 판정각
        :param body_tilt_ratio: 작업중 상체 구부러짐 판정을 위한 비율 임계치
        :param yolo_version: yolo version
        """

        self.__raw_data = pose_info
        self.__bounding_box = pose_info['person']
        self.__keypoints = pose_info['keypoints']

        # logger 설정
        self.__logger = self.__set_logger(self.__class__.__name__)

        # yolov8 대응
        # v8 에서는 잡히지 않은 point => (0,0)
        # v7 에서는 잡히지 않은 point => None
        if yolo_version == 8:
            self.__convert_zero_point_to_none(self.__keypoints)

        # 작업중지동작 detect 관련 변수
        self.__cross_ratio_threshold = cross_ratio_threshold
        self.__cross_angle_threshold = cross_angle_threshold
        self.__cross_point = None
        self.__cross_ratio = None
        self.__cross_angle = None
        self.__line1_point1 = self.__keypoints[self.__KPIndex.RIGHT_ELBOW][0:2] if self.__keypoints[self.__KPIndex.RIGHT_ELBOW] else None
        self.__line1_point2 = self.__keypoints[self.__KPIndex.RIGHT_WRIST][0:2] if self.__keypoints[self.__KPIndex.RIGHT_WRIST] else None
        self.__line2_point1 = self.__keypoints[self.__KPIndex.LEFT_ELBOW][0:2] if self.__keypoints[self.__KPIndex.LEFT_ELBOW] else None
        self.__line2_point2 = self.__keypoints[self.__KPIndex.LEFT_WRIST][0:2] if self.__keypoints[self.__KPIndex.LEFT_WRIST] else None

        self.__detect_cross_arms()  # 팔 교차 detect
        
        # 팔각도 detect 관련 변수
        self.__cross_arm_angle_threshold = arm_angle_threshold
        self.__cross_left_arm_angle = None
        self.__cross_right_arm_angle = None
        
        self.__cross_elbow_angle_threshold = elbow_angle_threshold
        self.__cross_left_elbow_angle = None
        self.__cross_right_elbow_angle = None
        
        self.__left_wrist = self.__keypoints[self.__KPIndex.LEFT_WRIST][0:2] if self.__keypoints[self.__KPIndex.LEFT_WRIST] else None
        self.__left_elbow = self.__keypoints[self.__KPIndex.LEFT_ELBOW][0:2] if self.__keypoints[self.__KPIndex.LEFT_ELBOW] else None
        self.__left_shoulder = self.__keypoints[self.__KPIndex.LEFT_SHOULDER][0:2] if self.__keypoints[self.__KPIndex.LEFT_SHOULDER] else None
        
        self.__right_wrist = self.__keypoints[self.__KPIndex.RIGHT_WRIST][0:2] if self.__keypoints[self.__KPIndex.RIGHT_WRIST] else None
        self.__right_elbow = self.__keypoints[self.__KPIndex.RIGHT_ELBOW][0:2] if self.__keypoints[self.__KPIndex.RIGHT_ELBOW] else None 
        self.__right_shoulder = self.__keypoints[self.__KPIndex.RIGHT_SHOULDER][0:2] if self.__keypoints[self.__KPIndex.RIGHT_SHOULDER] else None
        
        self.__detect_angle_arms()  # 팔 각도 detect

        # 넘어짐 detect 관련 변수
        self.__falldown_tilt_ratio = falldown_tilt_ratio
        self.__falldown_tilt_angle = falldown_tilt_angle
        self.__body_tilt_ratio = body_tilt_ratio
        self.__body_tilt_angle = None
        self.__overturn_upper = False
        self.__overturn_lower = False
        self.__badly_tilt = False
        self.__kp_rhip = self.__keypoints[self.__KPIndex.RIGHT_HIP][0:2] if self.__keypoints[self.__KPIndex.RIGHT_HIP] else None
        self.__kp_lhip = self.__keypoints[self.__KPIndex.LEFT_HIP][0:2] if self.__keypoints[self.__KPIndex.LEFT_HIP] else None
        self.__kp_rsldr = self.__keypoints[self.__KPIndex.RIGHT_SHOULDER][0:2] if self.__keypoints[self.__KPIndex.RIGHT_SHOULDER] else None
        self.__kp_lsldr = self.__keypoints[self.__KPIndex.LEFT_SHOULDER][0:2] if self.__keypoints[self.__KPIndex.LEFT_SHOULDER] else None
        self.__kp_rft = self.__keypoints[self.__KPIndex.RIGHT_FEET][0:2] if self.__keypoints[self.__KPIndex.RIGHT_FEET] else None
        self.__kp_lft = self.__keypoints[self.__KPIndex.LEFT_FEET][0:2] if self.__keypoints[self.__KPIndex.LEFT_FEET] else None

        self.__detect_falldown()    # 넘어짐 detect

    @property
    def falldown_ratio(self):
        return self.__falldown_tilt_ratio

    @property
    def cross_threshold(self):
        return self.__cross_ratio_threshold

    @property
    def body_tilt_angle(self):
        return self.__body_tilt_angle

    def __convert_zero_point_to_none(self, points):
        """
        좌표가 (0,0)인 경우 None으로 대체
        (yolov8 대응)
        :param points: 좌표리스트
        """
        for idx, point in enumerate(points):
            if point is not None and point[0] == 0 and point[1] == 0:
                points[idx] = None

    def get_pose_info(self):
        return self.__raw_data

    def get_keypoints(self):
        return self.__keypoints

    def get_bounding_box(self):
        return self.__bounding_box

    def is_cross_arms(self):
        """
        팔이 cross 상태인지 여부 확인
        """
        if self.__cross_point is not None and self.__cross_ratio is not None and self.__cross_angle is not None:
            if self.__cross_ratio[0] > self.__cross_ratio_threshold and self.__cross_ratio[1] > self.__cross_ratio_threshold:
                if self.__cross_angle_threshold[0] < self.__cross_angle < self.__cross_angle_threshold[1]:
                    return True

        return False
    
    def is_angle_arms(self):
        """
        팔이 꺽여있는지 확인
        """
        if project_config.ADD_CROSS_ARM:
            if self.__cross_left_arm_angle is not None and self.__cross_right_arm_angle is not None:
                if self.__cross_arm_angle_threshold[0] <= self.__cross_left_arm_angle and self.__cross_arm_angle_threshold[1] >= self.__cross_left_arm_angle and \
                    self.__cross_arm_angle_threshold[0] <= self.__cross_right_arm_angle and self.__cross_arm_angle_threshold[1] >= self.__cross_right_arm_angle:
                        return True
        else:
            if self.__cross_left_arm_angle is not None and self.__cross_right_arm_angle is not None and self.__cross_left_elbow_angle is not None and self.__cross_left_elbow_angle is not None:
                if (self.__cross_arm_angle_threshold[0] <= self.__cross_left_arm_angle and self.__cross_arm_angle_threshold[1] >= self.__cross_left_arm_angle) and \
                    (self.__cross_arm_angle_threshold[0] <= self.__cross_right_arm_angle and self.__cross_arm_angle_threshold[1] >= self.__cross_right_arm_angle) and \
                    (self.__cross_elbow_angle_threshold[0] <= self.__cross_left_elbow_angle and self.__cross_elbow_angle_threshold[1] >= self.__cross_left_arm_angle) and \
                    (self.__cross_elbow_angle_threshold[0] <= self.__cross_right_elbow_angle and self.__cross_elbow_angle_threshold[1] >= self.__cross_right_arm_angle):
                        return True
        return False
        
    def get_cross_point(self):
        if self.__cross_point is None:
            self.__logger.debug("no cross info")
        return self.__cross_point

    def get_cross_ratio(self):
        if self.__cross_ratio is None:
            self.__logger.debug("no cross info")
        return self.__cross_ratio

    def get_cross_angle(self):
        if self.__cross_angle is None:
            self.__logger.debug("no cross info")
        return self.__cross_angle

    def set_cross_ratio_threshold(self, threshold):
        """
        교차여부를 결정할 때 사용하는 교차점 위치 최소 비율 설정
        :param threshold: 0.0 ~ 0.5 사이의 값
        """
        self.__cross_ratio_threshold = threshold

    def set_cross_angle_threshold(self, angle_range: tuple):
        """
        교차여부를 결정할 때 사용하는 교차각 범위 설정
        :param angle_range: 교차각 범위, ex) (15.0, 165.0)
        """
        self.__cross_angle_threshold = angle_range

    def set_falldown_tilt_ratio(self, tilt_ratio):
        self.__falldown_tilt_ratio = tilt_ratio
        self.__detect_falldown()

    def set_falldown_tilt_angle(self, tilt_angle):
        """
        :param tilt_angle: 작업중 상체 구부러짐 판정각
        """
        self.__falldown_tilt_angle = tilt_angle
        self.__detect_falldown()

    def is_falldown(self, is_working_on=True):
        """
        넘어짐 상태 여부 확인
        :param is_working_on: 현재 작업중인지 여부. 작업중인 상태에서는 상체 구부러짐 각도를 반영한다.
        """
        result = True if self.__overturn_upper or self.__overturn_lower or self.__badly_tilt else False
        tilt_angle = False
        if self.__body_tilt_angle is not None:
            if (self.__body_tilt_angle[0] < self.__falldown_tilt_angle) or (self.__body_tilt_angle[1] < self.__falldown_tilt_angle):
                tilt_angle = True

        return result or (is_working_on and tilt_angle)

    def get_hpe_type(self, query=None, is_working_on=True):
        """
        감지된 Pose의 bit masking 획득하거나 특정 pose 여부를 확인한다.
        :param query: 확인하고자 하는 Pose Mask. 생략시 Pose bit mask 리턴
        :param is_working_on: 현재 작업중인지 여부. 작업중인 상태에서는 상체 구부러짐 각도를 반영한다.
        :return: query 입력시 True/False, 미입력시 Pose bit mask
        """
        result = 0x0000

        if self.is_falldown(is_working_on):
            result |= HPETypeMask.FALL_DOWN
        if project_config.ADD_CROSS_ARM:
            if self.is_cross_arms():
                result |= HPETypeMask.CROSS_ARM
            elif self.is_angle_arms():
                result |= HPETypeMask.CROSS_ARM
        else:
            if self.is_angle_arms():
                result |= HPETypeMask.CROSS_ARM
                self.__logger.debug(msg=f"***angle arms -> left angle: {int(self.__cross_left_arm_angle)}, right angle: {int(self.__cross_right_arm_angle)}, left elbow: {int(self.__cross_left_elbow_angle)}, right elbow: {int(self.__cross_right_elbow_angle)}")

        if query is None:
            return result

        return (result & query) != 0

    # 현재 type별 level값을 return 하고 있으나,
    # 추후 특정 계산식으로 level을 계산하게 될 예정
    def get_hpe_level(self, is_working_on=True):
        """
        :param is_working_on: 현재 작업중인지 여부. 작업중인 상태에서는 상체 구부러짐 각도를 반영한다.
        """
        result = 0

        if self.is_falldown(is_working_on) and result < self.__HPELevel.FALLDOWN:
            result = self.__HPELevel.FALLDOWN

        if (self.is_cross_arms() or self.is_angle_arms()) and result < self.__HPELevel.CROSSARM:
            result = self.__HPELevel.CROSSARM

        return result

    def __detect_cross_arms(self):
        self.__cross_point = self.__cross_ratio = self.__cross_angle = None

        if not self.__line1_point1 or not self.__line1_point2 or not self.__line2_point1 or not self.__line2_point2:
            self.__logger.debug("need 4 points for detecting cross arms")
            return

        # 손목이 팔꿈치 아래에 있는 경우는 제외
        if self.__line1_point1[1] < self.__line2_point2[1] and self.__line2_point1[1] < self.__line1_point2[1]:
            self.__logger.debug("both wrist is below elbow.")
            return

        line1_ccw = self.__ccw(self.__line1_point1, self.__line1_point2, self.__line2_point1) * \
                    self.__ccw(self.__line1_point1, self.__line1_point2, self.__line2_point2)
        line2_ccw = self.__ccw(self.__line2_point1, self.__line2_point2, self.__line1_point1) * \
                    self.__ccw(self.__line2_point1, self.__line2_point2, self.__line1_point2)

        if line1_ccw < 0 and line2_ccw < 0:
            self.__cross_point = self.__get_cross_point()
            self.__cross_ratio = self.__get_cross_ratio()
            self.__cross_angle = self.__get_cross_angle()

    # 두 선분의 교차지점 좌표 획득.
    # 교차됨이 확인 된 후에 사용할 것.
    # 출력: 교차지점 좌표 tuple
    def __get_cross_point(self):
        """
        두 직선의 교차점 좌표를 구함
        :return:
        """
        x1, y1 = self.__line1_point1[0:2]
        x2, y2 = self.__line1_point2[0:2]
        x3, y3 = self.__line2_point1[0:2]
        x4, y4 = self.__line2_point2[0:2]

        cx = ((x1 * y2 - x2 * y1) * (x3 - x4) - (x1 - x2) * (x3 * y4 - x4 * y3)) / \
             ((x1 - x2) * (y3 - y4) - (y1 - y2) * (x3 - x4))
        cy = ((x1 * y2 - x2 * y1) * (y3 - y4) - (y1 - y2) * (x3 * y4 - x4 * y3)) / \
             ((x1 - x2) * (y3 - y4) - (y1 - y2) * (x3 - x4))

        return cx, cy

    def __get_cross_angle(self, p1=None, p2=None, cross_point=None):
        """
        교차하는 팔의 각도를 구함
        :param p1: p2: 구하고자 하는 vector 방향의 각 좌표
        :param cross_point: 교차점
        """
        if cross_point is None and self.__cross_point is None:
            return None

        p1 = self.__line1_point1[0:2] if p1 is None else p1   # 오른 팔꿈치
        p2 = self.__line2_point2[0:2] if p2 is None else p2   # 왼 손목
        cross_point = self.__cross_point if cross_point is None else cross_point

        return self.three_point_angle(p1, p2, cross_point)

    @staticmethod
    def three_point_angle(p1, p2, cross_point):
        """
        세 점 사이의 각도를 구함
        """
        x1, y1 = p1[0:2]
        x2, y2 = p2[0:2]
        cx, cy = cross_point[0:2]

        radian = np.arctan2((y1-cy), (x1-cx)) - np.arctan2((y2-cy), (x2-cx))
        radian = np.fabs(radian * 180 / np.pi)
        if radian > 180:
            radian = 360 - radian

        return radian
    
    def __get_cross_ratio(self):
        """
        선분 위의 점이 해당 선분의 어느정도 위치에 있는지 확인
        (끝점에서 얼마나 떨어져 있는지 선분에 대한 비율)
        :return: 작은 비율 기준으로 리턴됨. 0.0 ~ 0.5, 0.5 이면 가운데
        """
        length_line1 = math.dist(self.__line1_point1[0:2], self.__line1_point2[0:2])
        length_point1 = math.dist(self.__line1_point1[0:2], self.__cross_point)

        ratio1 = length_point1 / length_line1
        if ratio1 > 0.5:
            ratio1 = math.fabs(1.0 - ratio1)

        length_line2 = math.dist(self.__line2_point1[0:2], self.__line2_point2[0:2])
        length_point2 = math.dist(self.__line2_point1[0:2], self.__cross_point)

        ratio2 = length_point2 / length_line2
        if ratio2 > 0.5:
            ratio2 = math.fabs(1.0 - ratio2)

        return ratio1, ratio2

    # 입력: 3개의 point, 각 (x, y)
    # 출력: 0, 1, -1
    @staticmethod
    def __ccw(point1, point2, point3):
        """
        3개의 점에 대한 CCW 계산 함수
        :return: -1, 0, 1
        """
        x1, y1 = point1[0:2]
        x2, y2 = point2[0:2]
        x3, y3 = point3[0:2]
        cross_product = ((x2 - x1) * (y3 - y1)) - ((x3 - x1) * (y2 - y1))

        if cross_product > 0:
            return 1  # 반시계방향 (counter clockwise)
        elif cross_product < 0:
            return -1  # 시계방향 (clockwise)

        return 0  # 평행 (collinear)

    def __detect_angle_arms(self):
        self.__cross_left_arm_angle = self.__cross_right_arm_angle = self.__cross_left_elbow_angle = self.__cross_right_elbow_angle = None
        
        if project_config.ADD_CROSS_ARM:
            if self.__left_elbow is None or self.__left_shoulder is None or self.__right_elbow is None or self.__right_shoulder is None:
                self.__logger.debug("need 4 points for detecting angle arms")
                return
        else:
            if self.__left_elbow is None or self.__left_shoulder is None or self.__right_elbow is None or self.__right_shoulder is None or \
                self.__left_wrist is None or self.__right_wrist is None:
                    self.__logger.debug("need 6 points for detecting angle arms")
                    return
       
        self.__cross_left_arm_angle, self.__cross_right_arm_angle, self.__cross_left_elbow_angle ,self.__cross_right_elbow_angle = self.__get_angle_arms()
    
    @staticmethod
    def three_point_arm_angle(p1, p2, cross_point):
        """
        세 점 사이(팔)의 각도를 구함
        p1: 몸쪽에서 가장 먼 keypoint
        p2: 몸쪽에서 가장 가까운 keypoint
        """
        x1, y1 = p1[0:2]
        x2, y2 = p2[0:2]
        cx, cy = cross_point[0:2]

        radian = np.arctan2((y1-cy), (x1-cx)) - np.arctan2((y2-cy), (x2-cx))
        radian = np.fabs(radian * 180 / np.pi)
        if y1 > cy or y1 > y2:
            return None
        
        if radian > 180 :
            radian = 360 - radian

        return radian
    
    def __get_angle_arms(self):
        left_angle = self.three_point_arm_angle(p1=self.__left_elbow,cross_point=self.__left_shoulder,p2=self.__right_shoulder)
        right_angle = self.three_point_arm_angle(p1=self.__right_elbow,cross_point=self.__right_shoulder,p2=self.__left_shoulder)
        left_elbow = None if project_config.ADD_CROSS_ARM else self.three_point_arm_angle(p1=self.__left_wrist,cross_point=self.__left_elbow,p2=self.__left_shoulder)
        right_elbow = None if project_config.ADD_CROSS_ARM else self.three_point_arm_angle(p1=self.__right_wrist,cross_point=self.__right_elbow,p2=self.__right_shoulder)
        
        return left_angle,right_angle,left_elbow,right_elbow
    
    def __detect_falldown(self):
        """
        인스턴스 생성시 주어진 key points를 바탕으로 falldown 판단
          - shoulder 좌표가 hip 좌표 아래에 있는 경우: __overturn_upper set
          - hip 좌표가 feet 좌표 아래에 있는 경우: __overturn_lower set
          - shoulder 좌표 및 feet 좌표의 ratio를 계산하여 임계치 이하인 경우: __badly_tilt set
          - shoulder 좌표 및 hip 좌표를 통해 body tilt angle 계산 __body_tilt_angle에 저장
        """
        self.__overturn_upper = False
        self.__overturn_lower = False
        self.__badly_tilt = False
        self.__body_tilt_angle = None

        # 1. 상체 뒤집힘
        if self.__kp_lhip and self.__kp_rhip and self.__kp_rsldr and self.__kp_lsldr:
            if self.__kp_lhip[1] < self.__kp_lsldr[1] and self.__kp_rhip[1] < self.__kp_rsldr[1]:
                self.__overturn_upper = True
        else:
            self.__logger.debug("need hip and shoulder points for detecting upper body falldown")

        # 2. 하체 뒤집힘
        if self.__kp_rft and self.__kp_lft and self.__kp_lhip and self.__kp_rhip:
            if self.__kp_lhip[1] > self.__kp_lft[1] and self.__kp_rhip[1] > self.__kp_rft[1]:
                self.__overturn_lower = True
        else:
            self.__logger.debug("need feet points for detecting lower body falldown")

        # 3. 신체의 기울어짐 (좌우 어깨, 좌우 발)
        if self.__kp_lft and self.__kp_rft and self.__kp_rsldr and self.__kp_lsldr:
            xmax = np.max([self.__kp_lsldr[0], self.__kp_rsldr[0], self.__kp_lft[0], self.__kp_rft[0]])
            ymax = np.max([self.__kp_lsldr[1], self.__kp_rsldr[1], self.__kp_lft[1], self.__kp_rft[1]])
            xmin = np.min([self.__kp_lsldr[0], self.__kp_rsldr[0], self.__kp_lft[0], self.__kp_rft[0]])
            ymin = np.min([self.__kp_lsldr[1], self.__kp_rsldr[1], self.__kp_lft[1], self.__kp_rft[1]])
            body_width = math.fabs(xmax - xmin)
            body_height = math.fabs(ymax - ymin)

            if (body_width > 1e-4) and ((body_height / body_width) < self.__falldown_tilt_ratio):
                self.__badly_tilt = True
        else:
            self.__logger.debug("need feet and shoulder points for detecting badly tilt")

        # 4. 상체 기울어짐 (좌우 어깨, 좌우 엉덩이)
        if self.__kp_lhip and self.__kp_rhip and self.__kp_rsldr and self.__kp_lsldr:
            angle_left = self.three_point_angle(self.__kp_lsldr, self.__kp_rhip, self.__kp_lhip)
            angle_right = self.three_point_angle(self.__kp_rsldr, self.__kp_lhip, self.__kp_rhip)
            self.__body_tilt_angle = (angle_left, angle_right)

            xmax = np.max([self.__kp_lsldr[0], self.__kp_rsldr[0], self.__kp_lhip[0], self.__kp_rhip[0]])
            ymax = np.max([self.__kp_lsldr[1], self.__kp_rsldr[1], self.__kp_lhip[1], self.__kp_rhip[1]])
            xmin = np.min([self.__kp_lsldr[0], self.__kp_rsldr[0], self.__kp_lhip[0], self.__kp_rhip[0]])
            ymin = np.min([self.__kp_lsldr[1], self.__kp_rsldr[1], self.__kp_lhip[1], self.__kp_rhip[1]])
            body_width = math.fabs(xmax - xmin)
            body_height = math.fabs(ymax - ymin)

            if (body_height > 1e-4) and ((body_width / body_height) < self.__body_tilt_ratio):
                self.__body_tilt_angle = None
        else:
            self.__logger.debug("need feet and shoulder points for calculating body tilt angle")

    def __set_logger(self, name):
        logger = logging.getLogger(name)
        logger_handler = logging.StreamHandler()
        logger_handler.setFormatter(logging.Formatter("[%(levelname)s][%(asctime)s][%(module)s::%(funcName)s()] %(message)s"))
        logger.addHandler(logger_handler)
        logger.setLevel(config.DEFAULT_LOG_LEVEL)

        return logger

    def log(self, level, message):
        self.__logger.log(level, message)


if __name__ == "__main__":
    # pose_info: <dict> {
    #     'person': (cx, cy, w, h, c),
    #     'keypoints': [ (x, y, c), None, (x, y, c), ... ]    # 1번이 index 0, 2번 index 1, ... 17번 index 16
    # }
    pose_info1 = {
        'person': (0, 0, 0, 0, 0),
        'keypoints': [
            (0, 0, 0), (0, 0, 0), (0, 0, 0), (0, 0, 0), (0, 0, 0),
            (4.64, 3, 0), (-3.2, 2.85, 0),   # 오른쪽, 왼쪽 어깨
            (-2.83, 2.2, 0), (5.45, 3.34, 0),     # 오른쪽, 왼쪽 팔꿈치
            (6.5, 1.6, 0), (0.98, 1.77, 0),   # 오른쪽, 왼쪽 손목
            (3.4, 3.9, 0), (-8.6, 3.2, 0),   # 오른쪽, 왼쪽 엉덩이
            (0, 0, 0), (0, 0, 0),
            (0, 20, 0), (0, 20, 0)    # 오른쪽, 왼쪽 발
        ]
    }
    test1 = HPEClassification(pose_info1, cross_ratio_threshold=0.1)
    print(f'is_cross_arms({test1.cross_threshold}):', test1.is_cross_arms())
    print('angle:', test1.get_cross_angle())
    test1.set_cross_ratio_threshold(0.05)
    print(f'is_cross_arms({test1.cross_threshold}):', test1.is_cross_arms())
    print(test1.get_cross_point())
    print(test1.get_cross_ratio())
    print('angle:', test1.get_cross_angle())
    print('is_falldown:', test1.is_falldown(True))
    print(test1.get_hpe_type(HPETypeMask.CROSS_ARM))
    print(test1.get_hpe_type(HPETypeMask.FALL_DOWN))
    print(hex(test1.get_hpe_type()))
    print('hpe level:', test1.get_hpe_level())
    print('--------------------------')
    print('body tilt angle:', test1.body_tilt_angle)

    print('--------------------------')

    pose_info2 = {
        'person': (0, 0, 0, 0, 0),
        'keypoints': [
            (0, 0, 0), (0, 0, 0), (0, 0, 0), (0, 0, 0), (0, 0, 0),
            (5, 20, 0), (3, 17, 0),   # 오른쪽, 왼쪽 어깨
            (-2.83, 2.2, 0), (0.98, 1.77, 0),   # 오른쪽, 왼쪽 팔꿈치
            (6.5, 1.6, 0), (5.45, 3.34, 0),     # 오른쪽, 왼쪽 손목
            (0, 30, 0), (0, 30, 0),   # 오른쪽, 왼쪽 엉덩이
            (0, 0, 0), (0, 0, 0),
            (37, 31, 0), (40, 42, 0)    # 오른쪽, 왼쪽 발
        ]
    }
    test2 = HPEClassification(pose_info2, falldown_tilt_ratio=0.5)
    print('is_cross_arms:', test2.is_cross_arms())
    print(f'is_falldown({test2.falldown_ratio}):', test2.is_falldown())
    test2.set_falldown_tilt_ratio(0.7)
    print(f'is_falldown({test2.falldown_ratio}):', test2.is_falldown())
    print('hpe level:', test2.get_hpe_level())