#ifndef HANDEYECALIB_H
#define HANDEYECALIB_H

#include "IHandEyeCalib.h"

/**
 * @brief 手眼标定实现类
 * 使用Eigen库进行SVD分解计算旋转平移矩阵
 */
class HandEyeCalib : public IHandEyeCalib
{
public:
    HandEyeCalib();
    ~HandEyeCalib() override;

    int CalculateRT(
        const std::vector<HECPoint3D>& eyePoints,
        const std::vector<HECPoint3D>& robotPoints,
        HECCalibResult& result) override;

    int CalculateRT(
        const std::vector<HECCalibPointPair>& pointPairs,
        HECCalibResult& result) override;

    void TransformPoint(
        const HECRotationMatrix& R,
        const HECTranslationVector& T,
        const HECPoint3D& srcPoint,
        HECPoint3D& dstPoint) override;

    void TransformPointWithCenter(
        const HECRotationMatrix& R,
        const HECTranslationVector& T,
        const HECPoint3D& srcCenter,
        const HECPoint3D& dstCenter,
        const HECPoint3D& srcPoint,
        HECPoint3D& dstPoint) override;

    void RotatePoint(
        const HECRotationMatrix& R,
        const HECPoint3D& srcPoint,
        HECPoint3D& dstPoint) override;

    void RotationMatrixToEulerZYX(
        const HECRotationMatrix& R,
        HECEulerAngles& angles) override;

    void RotationMatrixToEuler(
        const HECRotationMatrix& R,
        HECEulerOrder order,
        HECEulerAngles& angles) override;

    void EulerZYXToRotationMatrix(
        const HECEulerAngles& angles,
        HECRotationMatrix& R) override;

    void EulerToRotationMatrix(
        const HECEulerAngles& angles,
        HECEulerOrder order,
        HECRotationMatrix& R) override;

    void TransformPose(
        const HECCalibResult& calibResult,
        const HECPoint3D& eyePoint,
        const std::vector<HECPoint3D>& eyeDirVectors,
        bool invertYZ,
        HECPoseResult& poseResult) override;

    double CalculateError(
        const std::vector<HECPoint3D>& eyePoints,
        const std::vector<HECPoint3D>& robotPoints,
        const HECCalibResult& calibResult) override;

    int CalculateEyeInHand(
        const std::vector<HECEyeInHandData>& calibData,
        HECCalibResult& result) override;

    int CalculateEyeInHandWithTarget(
        const std::vector<HECEyeInHandData>& calibData,
        const HECPoint3D& targetInBase,
        HECCalibResult& result) override;
};

#endif // HANDEYECALIB_H