#ifndef HANDEYECALIB_H
#define HANDEYECALIB_H

#include <Eigen/Dense>

#include "IHandEyeCalib.h"

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 RotatePoint(const HECRotationMatrix& R,
                     const HECPoint3D& srcPoint,
                     HECPoint3D& dstPoint) override;

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

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

    bool TransformPose(const HECCalibResult& calibResult,
                       const HECPoint3D& eyeCenter,
                       const HECPoint3D& longAxisDir,
                       const HECPoint3D& normalDir,
                       int dirVectorInvert,
                       HECEulerOrder eulerOrder,
                       HECLongAxisDir longAxisMapping,
                       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;

    HECTCPCalibResult CalculateTCP(const HECTCPCalibData& data) override;

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

private:
    Eigen::Matrix3d eulerToRotationMatrix(double rx,
                                          double ry,
                                          double rz,
                                          HECEulerOrder order);
};

#endif // HANDEYECALIB_H