#include "CoordinateTransform.h"
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <cmath>

namespace CCoordinateTransform
{

// ==================== 内部辅助函数 ====================

// 将自定义 CTRotationMatrix 转为 Eigen Matrix3d
static Eigen::Matrix3d toEigenMatrix(const CTRotationMatrix& R)
{
    Eigen::Matrix3d mat;
    for (int i = 0; i < 3; ++i) {
        for (int j = 0; j < 3; ++j) {
            mat(i, j) = R.at(i, j);
        }
    }
    return mat;
}

// 将 Eigen Matrix3d 转为自定义 CTRotationMatrix
static CTRotationMatrix fromEigenMatrix(const Eigen::Matrix3d& mat)
{
    CTRotationMatrix R;
    for (int i = 0; i < 3; ++i) {
        for (int j = 0; j < 3; ++j) {
            R.at(i, j) = mat(i, j);
        }
    }
    return R;
}

// 将自定义 CTQuaternionD 转为 Eigen Quaterniond
static Eigen::Quaterniond toEigenQuaternion(const CTQuaternionD& q)
{
    return Eigen::Quaterniond(q.w, q.x, q.y, q.z);
}

// 将 Eigen Quaterniond 转为自定义 CTQuaternionD
static CTQuaternionD fromEigenQuaternion(const Eigen::Quaterniond& eq)
{
    return CTQuaternionD(eq.w(), eq.x(), eq.y(), eq.z());
}

// 将自定义 CTVec3D 转为 Eigen Vector3d
static Eigen::Vector3d toEigenVector(const CTVec3D& v)
{
    return Eigen::Vector3d(v.x, v.y, v.z);
}

// 将 Eigen Vector3d 转为自定义 CTVec3D
static CTVec3D fromEigenVector(const Eigen::Vector3d& ev)
{
    return CTVec3D(ev.x(), ev.y(), ev.z());
}

// ==================== 欧拉角与四元数互转 ====================

CTQuaternionD eulerToQuaternion(const CTEulerAngles& euler)
{
    // 使用 Eigen 的 AngleAxis 构建旋转
    Eigen::AngleAxisd rollAngle(euler.roll, Eigen::Vector3d::UnitX());
    Eigen::AngleAxisd pitchAngle(euler.pitch, Eigen::Vector3d::UnitY());
    Eigen::AngleAxisd yawAngle(euler.yaw, Eigen::Vector3d::UnitZ());

    Eigen::Quaterniond q;
    switch (euler.order)
    {
    // ==================== 内旋顺序 ====================
    case CTEulerOrder::XYZ:
        q = rollAngle * pitchAngle * yawAngle;
        break;
    case CTEulerOrder::ZYX:
        q = yawAngle * pitchAngle * rollAngle;
        break;
    case CTEulerOrder::ZXY:
        q = yawAngle * rollAngle * pitchAngle;
        break;
    case CTEulerOrder::YXZ:
        q = pitchAngle * rollAngle * yawAngle;
        break;
    case CTEulerOrder::YZX:
        q = pitchAngle * yawAngle * rollAngle;
        break;
    case CTEulerOrder::XZY:
        q = rollAngle * yawAngle * pitchAngle;
        break;

    // ==================== 外旋顺序 ====================
    case CTEulerOrder::sZYX:
        q = rollAngle * pitchAngle * yawAngle;
        break;
    case CTEulerOrder::sXYZ:
        q = yawAngle * pitchAngle * rollAngle;
        break;
    case CTEulerOrder::sZXY:
        q = pitchAngle * rollAngle * yawAngle;
        break;
    case CTEulerOrder::sYXZ:
        q = yawAngle * rollAngle * pitchAngle;
        break;
    case CTEulerOrder::sYZX:
        q = rollAngle * yawAngle * pitchAngle;
        break;
    case CTEulerOrder::sXZY:
        q = pitchAngle * yawAngle * rollAngle;
        break;

    default:
        q = yawAngle * pitchAngle * rollAngle;
        break;
    }

    return fromEigenQuaternion(q.normalized());
}

CTEulerAngles quaternionToEuler(const CTQuaternionD& q, CTEulerOrder order)
{
    Eigen::Quaterniond eq = toEigenQuaternion(q).normalized();
    Eigen::Matrix3d R = eq.toRotationMatrix();

    CTEulerAngles euler;
    euler.order = order;

    Eigen::Vector3d angles;

    switch (order)
    {
    // ==================== 内旋顺序 ====================
    case CTEulerOrder::ZYX:
        angles = R.eulerAngles(2, 1, 0);
        euler.yaw = angles(0);
        euler.pitch = angles(1);
        euler.roll = angles(2);
        break;

    case CTEulerOrder::XYZ:
        angles = R.eulerAngles(0, 1, 2);
        euler.roll = angles(0);
        euler.pitch = angles(1);
        euler.yaw = angles(2);
        break;

    case CTEulerOrder::ZXY:
        angles = R.eulerAngles(2, 0, 1);
        euler.yaw = angles(0);
        euler.roll = angles(1);
        euler.pitch = angles(2);
        break;

    case CTEulerOrder::YXZ:
        angles = R.eulerAngles(1, 0, 2);
        euler.pitch = angles(0);
        euler.roll = angles(1);
        euler.yaw = angles(2);
        break;

    case CTEulerOrder::YZX:
        angles = R.eulerAngles(1, 2, 0);
        euler.pitch = angles(0);
        euler.yaw = angles(1);
        euler.roll = angles(2);
        break;

    case CTEulerOrder::XZY:
        angles = R.eulerAngles(0, 2, 1);
        euler.roll = angles(0);
        euler.yaw = angles(1);
        euler.pitch = angles(2);
        break;

    // ==================== 外旋顺序 ====================
    case CTEulerOrder::sZYX:
        angles = R.eulerAngles(0, 1, 2);
        euler.roll = angles(0);
        euler.pitch = angles(1);
        euler.yaw = angles(2);
        break;

    case CTEulerOrder::sXYZ:
        angles = R.eulerAngles(2, 1, 0);
        euler.yaw = angles(0);
        euler.pitch = angles(1);
        euler.roll = angles(2);
        break;

    case CTEulerOrder::sZXY:
        angles = R.eulerAngles(1, 0, 2);
        euler.pitch = angles(0);
        euler.roll = angles(1);
        euler.yaw = angles(2);
        break;

    case CTEulerOrder::sYXZ:
        angles = R.eulerAngles(2, 0, 1);
        euler.yaw = angles(0);
        euler.roll = angles(1);
        euler.pitch = angles(2);
        break;

    case CTEulerOrder::sYZX:
        angles = R.eulerAngles(0, 2, 1);
        euler.roll = angles(0);
        euler.yaw = angles(1);
        euler.pitch = angles(2);
        break;

    case CTEulerOrder::sXZY:
        angles = R.eulerAngles(1, 2, 0);
        euler.pitch = angles(0);
        euler.yaw = angles(1);
        euler.roll = angles(2);
        break;

    default:
        angles = R.eulerAngles(2, 1, 0);
        euler.yaw = angles(0);
        euler.pitch = angles(1);
        euler.roll = angles(2);
        break;
    }

    return euler;
}

// ==================== 旋转矩阵与欧拉角互转 ====================

CTRotationMatrix eulerToRotationMatrix(const CTEulerAngles& euler)
{
    CTQuaternionD q = eulerToQuaternion(euler);
    return quaternionToRotationMatrix(q);
}

CTEulerAngles rotationMatrixToEuler(const CTRotationMatrix& R, CTEulerOrder order)
{
    CTQuaternionD q = rotationMatrixToQuaternion(R);
    return quaternionToEuler(q, order);
}

// ==================== 旋转矩阵与四元数互转 ====================

CTRotationMatrix quaternionToRotationMatrix(const CTQuaternionD& q)
{
    Eigen::Quaterniond eq = toEigenQuaternion(q).normalized();
    return fromEigenMatrix(eq.toRotationMatrix());
}

CTQuaternionD rotationMatrixToQuaternion(const CTRotationMatrix& R)
{
    Eigen::Matrix3d mat = toEigenMatrix(R);
    Eigen::Quaterniond eq(mat);
    return fromEigenQuaternion(eq.normalized());
}

} // namespace CCoordinateTransform

// ==================== 结构体成员函数实现 ====================

CTHomogeneousMatrix CTThreeAxisCalibResult::toHomogeneousMatrix() const
{
    CTEulerAngles euler(0.0, 0.0, rz, CTEulerOrder::ZYX);
    CTRotationMatrix R = CCoordinateTransform::eulerToRotationMatrix(euler);
    CTVec3D t(dx, dy, dz);
    return CTHomogeneousMatrix(R, t);
}

CTHomogeneousMatrix CTSixAxisCalibResult::toHomogeneousMatrix() const
{
    CTEulerAngles euler(rx, ry, rz, CTEulerOrder::ZYX);
    CTRotationMatrix R = CCoordinateTransform::eulerToRotationMatrix(euler);
    CTVec3D t(dx, dy, dz);
    return CTHomogeneousMatrix(R, t);
}

CTHomogeneousMatrix CTRobotPose::toHomogeneousMatrix() const
{
    CTEulerAngles euler(rx, ry, rz, CTEulerOrder::ZYX);
    CTRotationMatrix R = CCoordinateTransform::eulerToRotationMatrix(euler);
    CTVec3D t(x, y, z);
    return CTHomogeneousMatrix(R, t);
}

CTHomogeneousMatrix CTCameraPose::toHomogeneousMatrix() const
{
    CTEulerAngles euler(rx, ry, rz, CTEulerOrder::ZYX);
    CTRotationMatrix R = CCoordinateTransform::eulerToRotationMatrix(euler);
    CTVec3D t(x, y, z);
    return CTHomogeneousMatrix(R, t);
}

CTHomogeneousMatrix CTCameraPose::toHomogeneousMatrix(CTEulerOrder order) const
{
    CTEulerAngles euler(rx, ry, rz, order);
    CTRotationMatrix R = CCoordinateTransform::eulerToRotationMatrix(euler);
    CTVec3D t(x, y, z);
    return CTHomogeneousMatrix(R, t);
}

// ==================== 继续命名空间内的函数 ====================

namespace CCoordinateTransform
{

// ==================== 三轴手眼转换 ====================

CTVec3D threeAxisCameraToRobot(const CTVec3D& cameraPt,
                              const CTRobotPose& robotPose,
                              const CTThreeAxisCalibResult& handEye)
{
    CTHomogeneousMatrix T_hand_eye = handEye.toHomogeneousMatrix();
    CTRobotPose poseRz(robotPose.x, robotPose.y, robotPose.z, 0.0, 0.0, robotPose.rz);
    CTHomogeneousMatrix T_robot = poseRz.toHomogeneousMatrix();
    CTHomogeneousMatrix T_total = T_robot * T_hand_eye;
    return T_total.transformPoint(cameraPt);
}

CTVec3D threeAxisRobotToCamera(const CTVec3D& robotPt,
                              const CTRobotPose& robotPose,
                              const CTThreeAxisCalibResult& handEye)
{
    CTHomogeneousMatrix T_hand_eye = handEye.toHomogeneousMatrix();
    CTRobotPose poseRz(robotPose.x, robotPose.y, robotPose.z, 0.0, 0.0, robotPose.rz);
    CTHomogeneousMatrix T_robot = poseRz.toHomogeneousMatrix();
    CTHomogeneousMatrix T_total = T_robot * T_hand_eye;
    CTHomogeneousMatrix T_inv = T_total.inverse();
    return T_inv.transformPoint(robotPt);
}

void threeAxisCalcGraspPose(const CTVec3D& cameraPt,
                             double cameraAngle,
                             const CTRobotPose& robotPose,
                             const CTThreeAxisCalibResult& handEye,
                             CTRobotPose& targetPose)
{
    CTVec3D robotPt = threeAxisCameraToRobot(cameraPt, robotPose, handEye);
    double targetAngle = normalizeAngle(robotPose.rz + handEye.rz + cameraAngle);

    targetPose.x = robotPt.x;
    targetPose.y = robotPt.y;
    targetPose.z = robotPt.z;
    targetPose.rx = 0.0;
    targetPose.ry = 0.0;
    targetPose.rz = targetAngle;
}

// ==================== 六轴手眼转换 ====================

CTVec3D sixAxisEyeInHandCameraToRobot(const CTVec3D& cameraPt,
                                     const CTRobotPose& robotPose,
                                     const CTSixAxisCalibResult& handEye)
{
    CTHomogeneousMatrix T_hand_eye = handEye.toHomogeneousMatrix();
    CTHomogeneousMatrix T_robot = robotPose.toHomogeneousMatrix();
    CTHomogeneousMatrix T_total = T_robot * T_hand_eye;
    return T_total.transformPoint(cameraPt);
}

CTVec3D sixAxisEyeInHandRobotToCamera(const CTVec3D& robotPt,
                                     const CTRobotPose& robotPose,
                                     const CTSixAxisCalibResult& handEye)
{
    CTHomogeneousMatrix T_hand_eye = handEye.toHomogeneousMatrix();
    CTHomogeneousMatrix T_robot = robotPose.toHomogeneousMatrix();
    CTHomogeneousMatrix T_total = T_robot * T_hand_eye;
    CTHomogeneousMatrix T_inv = T_total.inverse();
    return T_inv.transformPoint(robotPt);
}

CTVec3D sixAxisEyeToHandCameraToRobot(const CTVec3D& cameraPt,
                                     const CTSixAxisCalibResult& cameraToBase)
{
    CTHomogeneousMatrix T_cam_to_base = cameraToBase.toHomogeneousMatrix();
    return T_cam_to_base.transformPoint(cameraPt);
}

CTVec3D sixAxisEyeToHandCameraToRobot(const CTVec3D& cameraPt,
                                     const CTHomogeneousMatrix& cameraToBaseMatrix)
{
    return cameraToBaseMatrix.transformPoint(cameraPt);
}

CTVec3D sixAxisEyeToHandRobotToCamera(const CTVec3D& robotPt,
                                     const CTSixAxisCalibResult& cameraToBase)
{
    CTHomogeneousMatrix T_cam_to_base = cameraToBase.toHomogeneousMatrix();
    CTHomogeneousMatrix T_inv = T_cam_to_base.inverse();
    return T_inv.transformPoint(robotPt);
}

void sixAxisEyeInHandCalcGraspPose(const CTCameraPose& cameraPose,
                                    const CTRobotPose& robotPose,
                                    const CTSixAxisCalibResult& handEye,
                                    CTRobotPose& targetPose)
{
    CTHomogeneousMatrix T_hand_eye = handEye.toHomogeneousMatrix();
    CTHomogeneousMatrix T_robot = robotPose.toHomogeneousMatrix();
    CTHomogeneousMatrix T_total = T_robot * T_hand_eye;

    CTVec3D cameraPt(cameraPose.x, cameraPose.y, cameraPose.z);
    CTVec3D robotPt = T_total.transformPoint(cameraPt);
    targetPose.x = robotPt.x;
    targetPose.y = robotPt.y;
    targetPose.z = robotPt.z;

    CTEulerAngles camEuler(cameraPose.rx, cameraPose.ry, cameraPose.rz, CTEulerOrder::ZYX);
    CTRotationMatrix R_cam = eulerToRotationMatrix(camEuler);
    CTRotationMatrix R_total = T_total.getRotation();
    CTRotationMatrix R_robot = R_total * R_cam;

    CTEulerAngles robotEuler = rotationMatrixToEuler(R_robot, CTEulerOrder::ZYX);
    targetPose.rx = robotEuler.roll;
    targetPose.ry = robotEuler.pitch;
    targetPose.rz = robotEuler.yaw;
}

void sixAxisEyeToHandCalcGraspPose(const CTCameraPose& cameraPose,
                                    const CTSixAxisCalibResult& cameraToBase,
                                    CTRobotPose& targetPose)
{
    CTHomogeneousMatrix T_cam_to_base = cameraToBase.toHomogeneousMatrix();

    CTVec3D cameraPt(cameraPose.x, cameraPose.y, cameraPose.z);
    CTVec3D robotPt = T_cam_to_base.transformPoint(cameraPt);
    targetPose.x = robotPt.x;
    targetPose.y = robotPt.y;
    targetPose.z = robotPt.z;

    CTEulerAngles camEuler(cameraPose.rx, cameraPose.ry, cameraPose.rz, CTEulerOrder::ZYX);
    CTRotationMatrix R_cam = eulerToRotationMatrix(camEuler);
    CTRotationMatrix R_cam_to_base = T_cam_to_base.getRotation();
    CTRotationMatrix R_robot = R_cam_to_base * R_cam;

    CTEulerAngles robotEuler = rotationMatrixToEuler(R_robot, CTEulerOrder::ZYX);
    targetPose.rx = robotEuler.roll;
    targetPose.ry = robotEuler.pitch;
    targetPose.rz = robotEuler.yaw;
}

void sixAxisEyeToHandCalcGraspPose(const CTCameraPose& cameraPose,
                                    const CTHomogeneousMatrix& cameraToBaseMatrix,
                                    CTEulerOrder eulerOrder,
                                    CTRobotPose& targetPose)
{
    CTVec3D cameraPt(cameraPose.x, cameraPose.y, cameraPose.z);
    CTVec3D robotPt = cameraToBaseMatrix.transformPoint(cameraPt);
    targetPose.x = robotPt.x;
    targetPose.y = robotPt.y;
    targetPose.z = robotPt.z;

    CTEulerAngles camEuler(cameraPose.rx, cameraPose.ry, cameraPose.rz, eulerOrder);
    CTRotationMatrix R_cam = eulerToRotationMatrix(camEuler);
    CTRotationMatrix R_cam_to_base = cameraToBaseMatrix.getRotation();
    CTRotationMatrix R_robot = R_cam_to_base * R_cam;

    CTEulerAngles robotEuler = rotationMatrixToEuler(R_robot, eulerOrder);
    targetPose.rx = robotEuler.roll;
    targetPose.ry = robotEuler.pitch;
    targetPose.rz = robotEuler.yaw;
}

// ==================== 辅助函数 ====================

double normalizeAngle(double angle)
{
    while (angle > M_PI)
        angle -= 2.0 * M_PI;
    while (angle < -M_PI)
        angle += 2.0 * M_PI;
    return angle;
}

double quaternionAngleDiff(const CTQuaternionD& q1, const CTQuaternionD& q2)
{
    Eigen::Quaterniond eq1 = toEigenQuaternion(q1).normalized();
    Eigen::Quaterniond eq2 = toEigenQuaternion(q2).normalized();

    double dot = eq1.dot(eq2);
    if (dot < 0.0) {
        dot = -dot;
    }
    if (dot > 1.0) dot = 1.0;

    return 2.0 * std::acos(dot);
}

CTQuaternionD quaternionSlerp(const CTQuaternionD& q1, const CTQuaternionD& q2, double t)
{
    Eigen::Quaterniond eq1 = toEigenQuaternion(q1).normalized();
    Eigen::Quaterniond eq2 = toEigenQuaternion(q2).normalized();

    Eigen::Quaterniond result = eq1.slerp(t, eq2);
    return fromEigenQuaternion(result);
}

} // namespace CCoordinateTransform