import numpy as np
from scipy.spatial.transform import Rotation as R
import time

class ControlUtil:
    
    curr_rotation = 0
    
    @staticmethod
    def check_limit(new_cam_to_world):
        if new_cam_to_world[0,3] < 0 or new_cam_to_world[1,3] > 0:
        # if new_cam_to_world[0,3] > 0:
            return False
        x = abs(new_cam_to_world[0,3])
        y = abs(new_cam_to_world[1,3])
        tan_y_x = y/x
        min_angle = 0 / 180 * np.pi
        max_angle = 90 / 180 * np.pi
        if tan_y_x < np.tan(min_angle) or tan_y_x > np.tan(max_angle):
            return False
         
        return True
    
    @staticmethod
    def solve_display_table_rot_and_cam_to_world(cam_to_world: np.ndarray) -> tuple:   
        if ControlUtil.check_limit(cam_to_world):
            return 0, cam_to_world
        else:
            min_display_table_rot = 180
            min_new_cam_to_world = None
            for display_table_rot in np.linspace(0.1,360, 1800):
                new_world_to_world = ControlUtil.get_z_axis_rot_mat(display_table_rot)
                new_cam_to_new_world = cam_to_world
                new_cam_to_world = new_world_to_world @ new_cam_to_new_world
                
                if ControlUtil.check_limit(new_cam_to_world):
                    if display_table_rot < min_display_table_rot:
                        min_display_table_rot, min_new_cam_to_world = display_table_rot, new_cam_to_world
                    if abs(display_table_rot - 360) < min_display_table_rot:
                        min_display_table_rot, min_new_cam_to_world = display_table_rot - 360, new_cam_to_world
        
        if min_new_cam_to_world is None:
            raise ValueError("No valid display table rotation found")
        
        delta_degree = min_display_table_rot - ControlUtil.curr_rotation
        ControlUtil.curr_rotation = min_display_table_rot
        return delta_degree, min_new_cam_to_world
                
    @staticmethod
    def get_z_axis_rot_mat(degree):
        radian = np.radians(degree)
        return np.array([
            [np.cos(radian), -np.sin(radian), 0, 0],
            [np.sin(radian), np.cos(radian), 0, 0],
            [0, 0, 1, 0],
            [0, 0, 0, 1]
            ])