#include <opencv.hpp>
#include <iostream>

static double degToRad(double deg) {
    return deg * M_PI / 180.0;
}
static double radToDeg(double deg) {
    return deg / M_PI * 180.0;
}
static cv::Vec3d radToDeg(cv::Vec3d rad) {
    rad[0] /= M_PI / 180.0;
    rad[1] /= M_PI / 180.0;
    rad[2] /= M_PI / 180.0;
    return rad;
}
// 计算旋转矩阵
static cv::Mat get_rotation_matrix(double alpha, double beta, double gamma) {
    // Roll (x-axis rotation)
    cv::Mat R_x = (cv::Mat_<double>(3, 3) <<
        1, 0, 0,
        0, cos(alpha), -sin(alpha),
        0, sin(alpha), cos(alpha));

    // Pitch (y-axis rotation)
    cv::Mat R_y = (cv::Mat_<double>(3, 3) <<
        cos(beta), 0, sin(beta),
        0, 1, 0,
        -sin(beta), 0, cos(beta));

    // Yaw (z-axis rotation)
    cv::Mat R_z = (cv::Mat_<double>(3, 3) <<
        cos(gamma), -sin(gamma), 0,
        sin(gamma), cos(gamma), 0,
        0, 0, 1);

    // Combined rotation matrix
    return R_z * R_y * R_x;
}

// 主函数
static cv::Mat transform_coordinates(cv::Mat coords, cv::Vec3d radInput1, cv::Vec3d radInput2) {
    // 原始旋转矩阵
    cv::Mat R_old = get_rotation_matrix(radInput1[0], radInput1[1], radInput1[2]);
    // 新的旋转矩阵
    cv::Mat R_new = get_rotation_matrix(radInput2[0], radInput2[1], radInput2[2]);
    // 计算新坐标
    cv::Mat new_coords = R_new * R_old.inv() * coords;
    return new_coords;
}

#pragma once