import cv2
import numpy as np

import util.const as const


class CalcStethoscopePosition:
    def __init__(self):
        self.target_points = np.array(
            [
                [const.LEFTSHOLDER_X, const.LEFTSHOLDER_Y],
                [const.RIGHTSHOLDER_X, const.RIGHTSHOLDER_Y],
                [const.LEFTHIP_X, const.LEFTHIP_Y],
                [const.RIGHTHIP_X, const.RIGHTHIP_Y],
            ],
            dtype=np.float32,
        )

    def calc_affine(self, source_points, stethoscope_x, stethoscope_y):
        mat = cv2.getPerspectiveTransform(source_points, self.target_points)
        x_0 = mat[0][0] * stethoscope_x + mat[0][1] * stethoscope_y + mat[0][2]
        y_0 = mat[1][0] * stethoscope_x + mat[1][1] * stethoscope_y + mat[1][2]
        x_1_y_1 = mat[2][0] * stethoscope_x + mat[2][1] * stethoscope_y + mat[2][2]
        stethoscope_calc = list((int(x_0 / x_1_y_1), int(y_0 / x_1_y_1)))

        if (
            stethoscope_calc[0] > const.MAXIMAIUM_SIZE
            or stethoscope_calc[1] > const.MAXIMAIUM_SIZE
            or stethoscope_calc[0] < const.MINIMUM_SIZE
            or stethoscope_calc[1] < const.MINIMUM_SIZE
        ):
            stethoscope_calc = list((0, 0))

        return stethoscope_calc