import numpy as np

class ArmController:
    def __init__(self, config, mirror=False):
        #self.config = config
        #self.mirror = mirror

        #self.shoulder_idx = {'left': 11, 'right': 12}
        #self.elbow_idx = {'left': 13, 'ri ght': 14}
        #self.wrist_idx = {'left': 15, 'right': 16}
        #self.hip_idx = {'left': 23, 'right': 24}

        self.r_wirst_id = 15 #16
        self.r_elbow_id = 13 #14
        self.r_shoulder_id = 11 # 12

        self.error = 23
        self.bag = 27

        self.debug = config.get('debug', False)


    def _get_angle(self, x1, y1, x2, y2):
        angle = np.arctan2(y2 - y1, x2 - x1)
        return angle



    def compute_speeds(self, landmarks):

        linear = 0.
        angular = 0.

        if (landmarks[11][3] < 0.5 or landmarks[12][3] < 0.5 or
            landmarks[15][3] < 0.5 or landmarks[16][3] < 0.5):
            if self.debug:
                print("Руки не видны")
            return 0.0, 0.0

        right_shoulder_point = landmarks[self.r_shoulder_id]
        right_shoulder_x = right_shoulder_point[0]
        right_shoulder_y = right_shoulder_point[1]

        right_elbow_point = landmarks[self.r_elbow_id] # [x, y, z, v]
        right_elbow_x = right_elbow_point[0]
        right_elbow_y = right_elbow_point[1]

        angle_shoulder_elbow = self._get_angle(right_shoulder_x, right_shoulder_y, right_elbow_x, right_elbow_y)

        right_wirst_point = landmarks[self.r_wirst_id]
        right_wirst_x = right_wirst_point[0]
        right_wirst_y = right_wirst_point[1]

        angle_elbow_wirst = self._get_angle(right_elbow_x, right_elbow_y, right_wirst_x, right_wirst_y)

        five_deg_in_rad = np.deg2rad(self.error)


        if (angle_shoulder_elbow < np.pi/2.5 + five_deg_in_rad) and (angle_shoulder_elbow > np.pi/2.5 -five_deg_in_rad):
            print(f"Angle between shoulder and elbow is {angle_shoulder_elbow} {np.rad2deg(angle_shoulder_elbow)}")
            #print(f"Angle between elbow and wirst is {angle_elbow_wirst}")
            if (angle_elbow_wirst > np.deg2rad(-90 - self.error)) and (angle_elbow_wirst < np.deg2rad(-90 + self.error)):
                linear = 1.
            if (angle_elbow_wirst > np.deg2rad(90 - self.error)) and (angle_elbow_wirst < np.deg2rad(90 + self.error)):
                linear = -1.
            if (angle_elbow_wirst > np.deg2rad(-45 - self.bag)) and (angle_elbow_wirst < np.deg2rad(-45 + self.bag)):
                angular = -1.
            if (angle_elbow_wirst > np.deg2rad(-135 - self.bag)) and (angle_elbow_wirst < np.deg2rad(-135 + self.bag)):
                angular = 1.

        return linear, angular