#pragma once
#include "dt.h"
#include <iostream>
#include "CMath.h"
#include "JAxis.h"
#include "CForceControl.h"
#include "CMachineCalibration.h"

/*
 * 垂直测量类，用于进行垂直方向的校准测量操作。
 * 通过在不同角度下测量高度，计算出垂直轴相对于 X 轴和 Y 轴的倾斜角度。
 */
class __declspec(dllexport) CVerticalMeasure
{
public:
    /**
     * @brief 构造函数，初始化垂直测量对象。
     *
     * @param headId 头部的唯一标识符，用于区分不同的测量单元。
     * @param forceController 指向力控制对象的指针，用于控制和执行力相关操作。
     * @param rotationAxis 指向旋转轴对象的指针，用于控制旋转轴的运动。
     */
    CVerticalMeasure(int headId, CForceControl* forceController, CAxis* rotationAxis);

    /**
     * @brief 析构函数，释放垂直测量对象占用的资源。
     */
    ~CVerticalMeasure();

    /**
     * @brief 开始垂直测量校准操作。
     *
     * 该函数会控制旋转轴旋转到指定角度，移动到相应的测试位置进行高度测量，
     * 根据测量得到的三个点计算垂直轴相对于 X 轴和 Y 轴的倾斜角度。
     *
     * @return LONG 校准操作的结果，成功返回 OK，失败返回相应的错误码。
     */
    LONG StartCalib();

    /**
     * @brief 从机器校准数据库中获取垂直测量所需的参数。
     */
    void GetParam();

    /**
     * @brief 将垂直测量得到的参数保存到机器校准数据库中。
     *
     * 目前该函数为空，可根据需求进行实现。
     */
    void SaveParam();

    void Stop() { m_bStop = true; }

    void GetAngleResult(double& ZAxisAngleX, double& ZAxisAngleY)
    {
        ZAxisAngleX = m_calibrationParams.dZAxisAngleX;
        ZAxisAngleY = m_calibrationParams.dZAxisAngleY;
    }

private:
    /**
     * @brief 辅助函数，移动到测试位置并进行高度测量。
     *
     * @param testPosition 测试位置的二维坐标。
     * @param searchHeight 搜索高度，用于高度测试的参考值。
     * @param measuredHeight 输出的实际测量高度值。
     * @return LONG 操作结果，成功返回 OK，失败返回相应的错误码。
     */
    LONG MoveToTestPositionAndMeasureHeight(const XY_DOUBLE_STRUCT& testPosition, double searchHeight, double& measuredHeight);

    /**
     * @brief 辅助函数，将旋转轴旋转到指定角度。
     *
     * @param targetAngle 要旋转到的目标角度。
     * @return LONG 操作结果，成功返回 OK，失败返回相应的错误码。
     */
    LONG RotateAxisToAngle(double targetAngle);

    int m_headId;  // 头部的唯一标识符
    CForceControl* m_forceController = nullptr;  // 力控制对象指针
    CAxis* m_rotationAxis = nullptr;  // 旋转轴对象指针
    CMachineCalibration* m_machineCalibrationDB = nullptr;  // 机器校准数据库对象指针
    VERTICAL_MEASURE m_calibrationParams;  // 垂直测量的参数结构体
    bool m_bStop = false;
};