#ifndef TCP_AUTO_CALIB_H
#define TCP_AUTO_CALIB_H

#include <atomic>
#include <shared_mutex>

#include <aubo/aubo_api.h>

using namespace arcs::common_interface;

namespace arcs {
namespace aubo_sdk {

class RtdeClient;
class RpcClient;
using RtdeClientPtr = std::shared_ptr<RtdeClient>;
using RpcClientPtr = std::shared_ptr<RpcClient>;

using Vec3 = std::array<double, 3>;

class ARCS_ABI TcpAutoCalib
{
public:
    TcpAutoCalib(RpcClientPtr rpc, RtdeClientPtr rtde);

    /**
     * @brief setTcpAutoCalibParam
     * 设置TCP自动标定参数
     * @param circle_radius 圆周运动半径
     * @param second_plane_offset  第二个圆周运动相对于第一个圆周运动在当前TCP
     * Z+方向偏移的距离
     * @param move_line_speed 运动速度
     * @return
     */
    int setTcpAutoCalibParam(const double circle_radius,
                             const double second_plane_offset,
                             const double move_line_speed);

    /**
     * @brief tcpAutoCalibrate
     * 基于交叉激光传感器的TCP位置自动标定
     * @param pose 示教不同姿态的位姿 需要大于等于4组
     * @param io_index_1 DI索引
     * @param io_index_2 DI索引
     * @return 成功返回0和TCP位置（x,y,z），失败返回-1
     */
    ResultWithErrno tcpAutoCalibrate(std::vector<std::vector<double>> poses,
                                     const int io_index_1,
                                     const int io_index_2);

    /**
     * @brief tcpAutoCorrect
     * 基于交叉激光传感器的TCP自动纠偏
     * @param sensor_pose 传感器位姿
     * @param io_index_1 DI索引
     * @param io_index_2 DI索引
     * @return 成功返回0和纠偏后的TCP，失败返回-1
     */
    ResultWithErrno tcpAutoCorrect(const std::vector<double> &sensor_pose,
                                   const int io_index_1, const int io_index_2);

    /**
     * @brief calibrateSensorPoseWithTCP
     * 已知TCP标定传感器位姿
     * @param io_index_1 DI索引
     * @param io_index_2 DI索引
     * @return 成功返回0和传感器位姿，失败返回-1
     */
    ResultWithErrno calibrateSensorPoseWithTCP(const int io_index_1,
                                               const int io_index_2);

private:
    void subscribeRealtimeState();

    std::pair<std::vector<std::vector<double>>,
              std::vector<std::vector<double>>>
    runTwoCirclesCapture();

    std::vector<std::vector<double>> runOneCircleAndCaptureFlange(
        const std::vector<double> &circle_center, const bool flag = true);

    std::vector<std::vector<double>> genCirclePoints(
        const std::vector<double> &center, double radius, int npts,
        bool is_offset = false);

    void moveTcpPoseAlignZ(const Vec3 &target_z);

    std::vector<double> alignTcpZToBaseZ();

    std::vector<double> probeAlongAxisFromPointWithZOffset(
        const Vec3 &axis_origin, const Vec3 &axis_z, double start_along_z);

    int waitArrival();

    int waitDIEdgeChanged(const int timeout);

private:
    double second_plane_offset_ = 0.01;
    double circle_radius_ = 0.01;
    double move_line_acc_ = 0.8;
    double move_line_speed_ = 0.005;

    RpcClientPtr rpc_;
    RtdeClientPtr rtde_;
    RobotInterfacePtr robot_;
    int task_id_{ -1 };

    std::atomic_int io_index_1_{ 0 };
    std::atomic_int io_index_2_{ 0 };

    std::shared_mutex tool_poses_mutex_;
    std::vector<std::vector<double>> tool_poses_;
};

} // namespace aubo_sdk
} // namespace arcs

#endif // TCP_AUTO_CALIB_H
