import numpy as np
import math

from touying.ImageReproject_python.common_types import POS, ImgEO


# from common_types import POS, ImgEO

class DPAL:
    """数字摄影测量算法库(Digital Photogrammetry Algorithm Library)"""
    
    @staticmethod
    def convert_pos_r2_map_r(pos, north_angle, r=None):
        """将导航坐标系下的旋转矩阵转换为投影坐标系下的旋转矩阵
        
        Args:
            pos: POS对象，包含位置和姿态信息
            north_angle: 子午线收敛角(弧度)
            r: 输出旋转矩阵
            
        Returns:
            旋转矩阵
        """
        # 计算导航坐标系下的旋转矩阵
        r_pos = DPAL.computing_r_mat_pos(pos)
        
        # 子午线收敛角旋转矩阵
        mt = np.array([
            [math.cos(-north_angle), math.sin(-north_angle), 0.0],
            [-math.sin(-north_angle), math.cos(-north_angle), 0.0],
            [0.0, 0.0, 1.0]
        ])
        
        # 应用子午线收敛角旋转
        r_pos = np.matmul(r_pos, mt)
        
        # 坐标系转换矩阵
        t = np.array([
            [0.0, 1.0, 0.0],
            [1.0, 0.0, 0.0],
            [0.0, 0.0, -1.0]
        ])
        
        # 计算最终旋转矩阵
        if r is None:
            r = np.matmul(t, np.matmul(r_pos, t))
        else:
            r[:] = np.matmul(t, np.matmul(r_pos, t))
            
        return r
    
    @staticmethod
    def pos2map_eo(pos, north_angle, sys_er_r, eo, angle_type=0):
        """将POS转换为投影坐标系下的EO
        
        Args:
            pos: POS对象，包含位置和姿态信息
            north_angle: 子午线收敛角(弧度)
            sys_er_r: 系统误差旋转矩阵
            eo: 输出的EO对象
            angle_type: 角度类型，0为国际转角，1为中国转角
            
        Returns:
            None
        """
        eo.name = pos.name
        eo.Xs = pos.L
        eo.Ys = pos.B
        eo.Zs = pos.H
        
        # 计算旋转矩阵
        r = DPAL.convert_pos_r2_map_r(pos, north_angle)
        
        # 应用系统误差
        r = np.matmul(sys_er_r, r)
        
        # 根据角度类型计算外方位元素
        if angle_type == 0:
            DPAL.rotation2_angle(eo, r)
        else:
            DPAL.rotation2_angle_cn(eo, r)
    
    @staticmethod
    def rotation2_angle(eo, r):
        """从旋转矩阵分解出外方位角元素(国际转角系统)
        
        Args:
            eo: 输出的EO对象
            r: 旋转矩阵
            
        Returns:
            None
        """
        eo.omega = math.atan2(-r[2, 1], r[2, 2])
        eo.phi = math.asin(r[2, 0])
        eo.kappa = math.atan2(-r[1, 0], r[0, 0])
    
    @staticmethod
    def rotation2_angle_cn(eo_cn, r):
        """从旋转矩阵分解出外方位角元素(中国转角系统)
        
        Args:
            eo_cn: 输出的EO对象
            r: 旋转矩阵
            
        Returns:
            None
        """
        eo_cn.phi = math.atan2(-r[2, 0], r[2, 2])
        eo_cn.omega = math.asin(-r[2, 1])
        eo_cn.kappa = math.atan2(r[0, 1], r[1, 1])
    
    @staticmethod
    def eo2eo_cn(eo, eo_cn):
        """国际转角系统的EO转换为中国转角系统的EO
        
        Args:
            eo: 国际转角系统的EO对象
            eo_cn: 输出的中国转角系统的EO对象
            
        Returns:
            None
        """
        eo_cn.Xs = eo.Xs
        eo_cn.Ys = eo.Ys
        eo_cn.Zs = eo.Zs
        
        # 计算旋转矩阵
        r = DPAL.computing_r_mat(eo)
        
        # 分解为中国转角
        DPAL.rotation2_angle_cn(eo_cn, r)
    
    @staticmethod
    def eo_cn2eo(eo_cn, eo):
        """中国转角系统的EO转换为国际转角系统的EO
        
        Args:
            eo_cn: 中国转角系统的EO对象
            eo: 输出的国际转角系统的EO对象
            
        Returns:
            None
        """
        eo.Xs = eo_cn.Xs
        eo.Ys = eo_cn.Ys
        eo.Zs = eo_cn.Zs
        
        # 计算旋转矩阵
        r = DPAL.computing_r_mat_cn(eo_cn)
        
        # 分解为国际转角
        DPAL.rotation2_angle(eo, r)
    
    @staticmethod
    def eo_cn2eo_zyx(eo_cn, eo_zyx):
        """中国转角系统的EO转换为顺序为zyx转角的EO
        
        Args:
            eo_cn: 中国转角系统的EO对象
            eo_zyx: 输出的zyx转角系统的EO对象
            
        Returns:
            None
        """
        eo_zyx.Xs = eo_cn.Xs
        eo_zyx.Ys = eo_cn.Ys
        eo_zyx.Zs = eo_cn.Zs
        
        # 计算旋转矩阵
        r = DPAL.computing_r_mat_cn(eo_cn)
        rt = r.T
        
        # 分解为zyx转角
        eo_temp = ImgEO()
        DPAL.rotation2_angle(eo_temp, rt)
        eo_zyx.kappa = -eo_temp.kappa
        eo_zyx.phi = -eo_temp.phi
        eo_zyx.omega = -eo_temp.omega
    
    @staticmethod
    def eo2eo_zyx(eo, eo_zyx):
        """国际转角系统的EO转换为顺序为zyx转角的EO
        
        Args:
            eo: 国际转角系统的EO对象
            eo_zyx: 输出的zyx转角系统的EO对象
            
        Returns:
            None
        """
        eo_zyx.Xs = eo.Xs
        eo_zyx.Ys = eo.Ys
        eo_zyx.Zs = eo.Zs
        
        # 计算旋转矩阵
        r = DPAL.computing_r_mat(eo)
        rt = r.T
        
        # 分解为zyx转角
        eo_temp = ImgEO()
        DPAL.rotation2_angle(eo_temp, rt)
        eo_zyx.kappa = -eo_temp.kappa
        eo_zyx.phi = -eo_temp.phi
        eo_zyx.omega = -eo_temp.omega
    
    @staticmethod
    def computing_k_mat(io, k=None, k_type=0):
        """根据内方位元素计算内参矩阵
        
        Args:
            io: 内方位元素对象
            k: 输出内参矩阵
            k_type: 内参类型，0为左上角为原点，向右X向下Y；非0为中心为原点，向右X向下Y
            
        Returns:
            内参矩阵
        """
        if k_type != 0:
            k_mat = np.array([
                [-io.f, 0.0, 0.0],
                [0.0, -io.f, 0.0],
                [0.0, 0.0, 1.0]
            ])
        else:
            k_mat = np.array([
                [-io.f, 0.0, io.cx],
                [0.0, io.f, io.cy],
                [0.0, 0.0, 1.0]
            ])
        
        if k is None:
            return k_mat
        else:
            k[:] = k_mat
            return k
    
    @staticmethod
    def computing_r_mat_pos(pos, r_pos=None):
        """根据POS元素计算导航坐标系下的旋转矩阵
        
        Args:
            pos: POS对象
            r_pos: 输出旋转矩阵
            
        Returns:
            旋转矩阵
        """
        # 计算各轴旋转矩阵
        r_roll = np.array([
            [1.0, 0.0, 0.0],
            [0.0, math.cos(pos.roll), math.sin(pos.roll)],
            [0.0, -math.sin(pos.roll), math.cos(pos.roll)]
        ])
        
        r_pitch = np.array([
            [math.cos(pos.pitch), 0.0, -math.sin(pos.pitch)],
            [0.0, 1.0, 0.0],
            [math.sin(pos.pitch), 0.0, math.cos(pos.pitch)]
        ])
        
        r_yaw = np.array([
            [math.cos(pos.yaw), math.sin(pos.yaw), 0.0],
            [-math.sin(pos.yaw), math.cos(pos.yaw), 0.0],
            [0.0, 0.0, 1.0]
        ])
        
        # 计算最终旋转矩阵
        if r_pos is None:
            return np.matmul(r_roll, np.matmul(r_pitch, r_yaw))
        else:
            r_pos[:] = np.matmul(r_roll, np.matmul(r_pitch, r_yaw))
            return r_pos
    
    @staticmethod
    def computing_r_mat(eo, r=None):
        """根据外方位元素(国际转角)计算旋转矩阵
        
        Args:
            eo: 外方位元素对象
            r: 输出旋转矩阵
            
        Returns:
            旋转矩阵
        """
        # 计算各轴旋转矩阵
        r_omega = np.array([
            [1.0, 0.0, 0.0],
            [0.0, math.cos(eo.omega), math.sin(eo.omega)],
            [0.0, -math.sin(eo.omega), math.cos(eo.omega)]
        ])
        
        r_phi = np.array([
            [math.cos(eo.phi), 0.0, -math.sin(eo.phi)],
            [0.0, 1.0, 0.0],
            [math.sin(eo.phi), 0.0, math.cos(eo.phi)]
        ])
        
        r_kappa = np.array([
            [math.cos(eo.kappa), math.sin(eo.kappa), 0.0],
            [-math.sin(eo.kappa), math.cos(eo.kappa), 0.0],
            [0.0, 0.0, 1.0]
        ])
        
        # 计算最终旋转矩阵
        if r is None:
            return np.matmul(r_kappa, np.matmul(r_phi, r_omega))
        else:
            r[:] = np.matmul(r_kappa, np.matmul(r_phi, r_omega))
            return r
    
    @staticmethod
    def computing_r_mat_cn(eo_cn, r=None):
        """根据外方位元素(中国转角)计算旋转矩阵
        
        Args:
            eo_cn: 外方位元素对象
            r: 输出旋转矩阵
            
        Returns:
            旋转矩阵
        """
        # 计算各轴旋转矩阵
        r_phi = np.array([
            [math.cos(eo_cn.phi), 0.0, math.sin(eo_cn.phi)],
            [0.0, 1.0, 0.0],
            [-math.sin(eo_cn.phi), 0.0, math.cos(eo_cn.phi)]
        ])
        
        r_omega = np.array([
            [1.0, 0.0, 0.0],
            [0.0, math.cos(eo_cn.omega), math.sin(eo_cn.omega)],
            [0.0, -math.sin(eo_cn.omega), math.cos(eo_cn.omega)]
        ])
        
        r_kappa = np.array([
            [math.cos(eo_cn.kappa), math.sin(eo_cn.kappa), 0.0],
            [-math.sin(eo_cn.kappa), math.cos(eo_cn.kappa), 0.0],
            [0.0, 0.0, 1.0]
        ])
        
        # 计算最终旋转矩阵
        if r is None:
            return np.matmul(r_kappa, np.matmul(r_omega, r_phi))
        else:
            r[:] = np.matmul(r_kappa, np.matmul(r_omega, r_phi))
            return r