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, 'right': 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.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)

        five_deg_in_rad = np.deg2rad(5)

        print(f"Angle between shoulder and elbow is {angle_shoulder_elbow}")

        if (angle_shoulder_elbow < five_deg_in_rad) and (angle_shoulder_elbow > -five_deg_in_rad):
            linear = 1.

        return linear, angular