
"""
このモジュールはさまざまなベクトル計算を行うためのクラスを提供します。https://programming-surgeon.com/script/match-3points-mesh/

クラス:
    VectorCalc - さまざまなベクトル計算を行うためのクラス。

使用例:

vector = np.array([1, 2, 3])
length = VectorCalc.vector_length(vector)
print(length)

angle = VectorCalc.angle_between_vectors(np.array([1, 0, 0]), np.array([0, 1, 0]))
print(angle)
"""

'''
クラスにすることでコードの再利用性、拡張性、構造化が向上します。以下に、あなたのコードをクラスとして構造化した例を示します。
このクラスでは、各関数は @staticmethod デコレータを用いて静的メソッドとして定義されています。これにより、クラスのインスタンス化なしにメソッドを呼び出すことができます。
'''

__version__ = "1.0.0"


import numpy as np
from scipy.linalg import lstsq

class VectorCalc:
    """さまざまなベクトル計算を行うためのクラス。"""

    def __init__(self):
        pass

    @staticmethod
    def vector_length(vector):
        """ベクトルの長さ（ノルム）を計算する。

        Args:
            vector (numpy.ndarray): 長さを計算する対象のベクトル。

        Returns:
            float: ベクトルの長さ。
        """
        norm = np.linalg.norm(vector)
        return norm

    @staticmethod
    def dot_product(vector1, vector2):
        """2つのベクトルの内積を計算する。

        Args:
            vector1 (numpy.ndarray): 1つ目のベクトル。
            vector2 (numpy.ndarray): 2つ目のベクトル。

        Returns:
            float: ベクトルの内積。
        """
        return np.dot(vector1, vector2)

    @staticmethod
    def cross_product(vector1, vector2):
        """2つのベクトルの外積を計算する。

        Args:
            vector1 (numpy.ndarray): 1つ目のベクトル。
            vector2 (numpy.ndarray): 2つ目のベクトル。

        Returns:
            numpy.ndarray: ベクトルの外積。
        """
        return np.cross(vector1, vector2)

    @staticmethod
    def angle_between_vectors(vector1, vector2):
        """2つのベクトルの成す角度を度単位で計算する。

        Args:
            vector1 (numpy.ndarray): 1つ目のベクトル。
            vector2 (numpy.ndarray): 2つ目のベクトル。

        Returns:
            float: 2つのベクトルの成す角度（度）。
        """
        dot_product = np.dot(vector1, vector2)
        norm2 = np.linalg.norm(vector2)
        norm1 = np.linalg.norm(vector1)
        cos_theta = dot_product / (norm1 * norm2)
        theta = np.arccos(np.clip(cos_theta, -1.0, 1.0))
        return np.degrees(theta)

    @staticmethod
    def rodrigues_rotation_matrix(theta_rad, rot_axis):
        """ロドリゲスの回転公式を使用して、指定された軸と角度での回転行列を返します。

        Args:
            rot_axis (array_like): 回転軸を表すベクトル。
            theta (float): 回転角（ラジアン）。

        Returns:
            numpy.ndarray: 回転行列。
        """
        axis = np.asarray(rot_axis)
        axis = axis / np.linalg.norm(axis) # 回転ベクトルを正規化して単位ベクトルへ
        cos = np.cos(theta_rad)
        sin = np.sin(theta_rad)
        n1, n2, n3 = axis

        R = np.array([[n1**2 * (1 - cos) + cos       ,  n1 * n2 * (1 - cos) - n3 * sin,  n1 * n3 * (1 - cos) + n2 * sin],
                      [n1 * n2 * (1 - cos) + n3 * sin,  n2**2 * (1 - cos) + cos       ,  n2 * n3 * (1 - cos) - n1 * sin],
                      [n1 * n3 * (1 - cos) - n2 * sin,  n2 * n3 * (1 - cos) + n1 * sin,  n3**2 * (1 - cos) + cos        ]])
        return R

    @staticmethod
    def rotation_matrix_to_euler_xyz(R):
        """XYZオイラー角を使用して、回転行列からオイラー角を計算します。

        Args:
            R (numpy.ndarray): 3x3の回転行列。

        Returns:
            tuple: XYZオイラー角（ラジアン）。
        """
        roll = np.arctan2(R[2, 1], R[2, 2])
        pitch = -np.arcsin(R[2, 0])
        yaw = np.arctan2(R[1, 0], R[0, 0])
        return roll, pitch, yaw

    @staticmethod
    def distance_to_plane(plane_points, point):
        """点から平面までの距離と垂線が交わる点を計算する。

        Args:
            plane_points (list): 平面を定義する3つの点のリスト。
            point (numpy.ndarray): 距離を計算する対象の点。

        Returns:
            tuple: 平面までの距離と垂線が交わる点を含むタプル。
        """
        p1, p2, p3 = plane_points
        v1 = np.array(p2) - np.array(p1)
        v2 = np.array(p3) - np.array(p1)
        normal_vector = np.cross(v1, v2)
        norm_n = np.linalg.norm(normal_vector)
        distance = np.abs(np.dot(normal_vector, np.array(point) - np.array(p1))) / norm_n
        perpendicular_vector = np.array(point) - np.array(p1)
        dot_product = np.dot(normal_vector, perpendicular_vector)
        q = np.array(point) - (dot_product / norm_n**2) * normal_vector
        return distance, q

    @staticmethod
    def cross_product_vector(points):
        """与えられた3つの点から外積を計算し、法線ベクトルを得る。

        Args:
            points (list): 3つの点を含むリスト。

        Returns:
            numpy.ndarray: 外積によって得られた法線ベクトル。
        """
        vec1 = np.array(points[1]) - np.array(points[0])
        vec2 = np.array(points[2]) - np.array(points[0])
        normal_vector = np.cross(vec1, vec2)
        return normal_vector

    @staticmethod
    def fit_plane_normal_vector(points):
        """複数の点に対して最小二乗法を用いて平面をフィットし、その法線ベクトルを返す。

        Args:
            points (numpy.ndarray): 各点が[x, y, z]の配列である点の配列。

        Returns:
            numpy.ndarray: フィットした平面の法線ベクトル。
        """
        num_points = len(points)
        A = np.c_[[p[:2] for p in points], np.ones(num_points)]
        b = np.array([p[2] for p in points])
        coeffs, _, _, _ = lstsq(A, b)
        normal_vector = coeffs[:2]
        normal_vector = np.append(normal_vector, -1)
        return normal_vector

    @staticmethod
    def fit_plane(points):
        # 与えられた点の数
        num_points = len(points)
        
        # データ行列を作成する
        A = np.c_[[p[:2] for p in points], np.ones(num_points)]
        b = np.array([p[2] for p in points])
        
        # 最小二乗法で平面をフィットする
        coeffs, _, _, _ = lstsq(A, b)
        
        # 平面の法線ベクトルを計算する
        normal_vector = coeffs[:2]
        normal_vector = np.append(normal_vector, -1)
        
        # 平面上の2つの点を計算する
        centroid = np.mean(points, axis=0)[:2]
        
        # 法線ベクトルと直交するベクトルを求める
        v = np.array([1, -normal_vector[0] / normal_vector[1]])
        v /= np.linalg.norm(v)  # 長さを1に正規化する
        
        # 始点と終点を求める
        start_point = centroid - v
        end_point = centroid + v
        
        return normal_vector, start_point, end_point

    @staticmethod
    def transform_matrix_old(p1, p2, p3, q1, q2, q3):
        """
        入力パラメータ: P上の点p1, p2, p3の座標、Q上のq1, q2, q3の座標
        出力パラメータ: 回転行列 M, 移動行列t
        """

        #表面Pについてパラメータを計算
        #px, pyのベクトルを計算する
        px = p2 - p1
        py = p3 - p1
        #単位ベクトルex, ey, ezで計算する
        ex_p = px / np.linalg.norm(px)
        ez_p = np.cross(px, py) / np.linalg.norm(np.cross(px, py))
        ey_p = np.cross(ez_p, ex_p)
        #縦の単位ベクトルを横に並べたベクトルが回転行列
        Mp = np.array([ex_p, ey_p, ez_p]).T

        #同様に表面Qについても計算
        qx = q2 - q1
        qy = q3 - q1
        ex_q = qx / np.linalg.norm(qx)
        ez_q = np.cross(qx, qy) / np.linalg.norm(np.cross(qx, qy))
        ey_q = np.cross(ez_q, ex_q)
        Mq = np.array([ex_q, ey_q, ez_q]).T

        #ベクトルの式より、表面P⇒Qに移動する回転行列、移動行列を計算
        M = np.dot(Mq, Mp.T)
        t = - np.dot(np.dot(Mq, Mp.T), p1) + q1
        return M, t

    @staticmethod
    def transform_matrix(p_points, q_points):
        """
        入力パラメータ:
        p_points: P上の点p1, p2, p3の座標 (3x3のリストまたはNumPy配列)
        q_points: Q上の点q1, q2, q3の座標 (3x3のリストまたはNumPy配列)
        出力パラメータ: 回転行列 M, 移動行列t
        """
        # リストをNumPy配列に変換
        p_points = np.array(p_points)
        q_points = np.array(q_points)

        # 各点を取得
        # p1, p2, p3 = p_points
        # q1, q2, q3 = q_points
        p1 = points_before[:, 0]
        p2 = points_before[:, 1]
        p3 = points_before[:, 2]
        q1 = points_after[:, 0]
        q2 = points_after[:, 1]
        q3 = points_after[:, 2]
        
    
        # 表面Pについてパラメータを計算
        # px, pyのベクトルを計算する
        px = p2 - p1
        py = p3 - p1
        # 単位ベクトルex, ey, ezで計算する
        ex_p = px / np.linalg.norm(px)
        ez_p = np.cross(px, py) / np.linalg.norm(np.cross(px, py))
        ey_p = np.cross(ez_p, ex_p)
        # 縦の単位ベクトルを横に並べたベクトルが回転行列
        Mp = np.array([ex_p, ey_p, ez_p]).T
    
        # 同様に表面Qについても計算
        qx = q2 - q1
        qy = q3 - q1
        ex_q = qx / np.linalg.norm(qx)
        ez_q = np.cross(qx, qy) / np.linalg.norm(np.cross(qx, qy))
        ey_q = np.cross(ez_q, ex_q)
        Mq = np.array([ex_q, ey_q, ez_q]).T
    
        # ベクトルの式より、表面P⇒Qに移動する回転行列、移動行列を計算
        M = np.dot(Mq, Mp.T)
        t = - np.dot(np.dot(Mq, Mp.T), p1) + q1
    
        return M, t
    
if __name__ == "__main__":
    # デモンストレーション用のコード

    # 回転前の行列
    points_before = np.array([
        [1, 2, 3],
        [3, 4, 2],
        [4, 5, 2],
    ])
    print('回転前の行列')
    print(points_before)
    print('-------------------------------')


    # ロドリゲスの回転行列を計算
    axis = [0, 0, 1]  # 回転軸
    theta = 0.5  # 回転角
    rad = np.deg2rad(0.05)
    R = VectorCalc.rodrigues_rotation_matrix(rad, axis)
    print("ロドリゲスの回転行列:")
    print(R)
    print('-------------------------------')


    # 回転後の行列
    points_after = np.dot(R, points_before) 
    print("回転後の行列")
    print(points_after)
    print('-------------------------------')
    print('回転前後の座標系の集合から、回転行列を求める')
    print('-------------------------------')

    # 回転前の行列と回転後の行列から、回転行列を求める
    M, t = VectorCalc.transform_matrix(points_before, points_after)
    # M, t = VectorCalc.transform_matrix(points_after, points_before)
    print('回転行列')
    print(M)
    print('-------------------------------')


    # 回転行列からXYZオイラー角を計算
    roll, pitch, yaw = VectorCalc.rotation_matrix_to_euler_xyz(M)
    print("XYZオイラー角:")
    print(f"Roll: {roll} rad")
    print(f"Pitch: {pitch} rad")
    print(f"Yaw: {yaw} rad")
    print('-------------------------------')
    print(f"Roll: {round_decimal(np.rad2deg(roll), 5)} deg")
    print(f"Pitch: {round_decimal(np.rad2deg(pitch), 5)} deg")
    print(f"Yaw: {round_decimal(np.rad2deg(yaw), 5)} deg")

    p1 = points_before[:, 0]
    p2 = points_before[:, 1]
    p3 = points_before[:, 2]
    q1 = points_after[:, 0]
    q2 = points_after[:, 1]
    q3 = points_after[:, 2]