#include "AlgoParamConverter.h"
#include "VrLog.h"
#include <cstring>

namespace AlgoParamConverter
{

WD_workpieceHoleParam ToAlgoParam(const VrWorkpieceHoleParam& param)
{
    WD_workpieceHoleParam algo;
    algo.workpieceType = param.workpieceType;
    algo.holeDiameter = param.holeDiameter;
    algo.holeDist_L = param.holeDist_L;
    algo.holeDist_W = param.holeDist_W;
    algo.xLen = param.xLen;
    algo.yLen = param.yLen;
    algo.H = param.H;
    return algo;
}

SSG_lineSegParam ToAlgoParam(const VrLineSegParam& param)
{
    SSG_lineSegParam algo;
    algo.distScale = param.distScale;
    algo.segGapTh_y = param.segGapTh_y;
    algo.segGapTh_z = param.segGapTh_z;
    return algo;
}

SSG_outlierFilterParam ToAlgoParam(const VrOutlierFilterParam& param)
{
    SSG_outlierFilterParam algo;
    algo.continuityTh = param.continuityTh;
    algo.outlierTh = param.outlierTh;
    return algo;
}

SSG_treeGrowParam ToAlgoParam(const VrTreeGrowParam& param)
{
    SSG_treeGrowParam algo;
    algo.yDeviation_max = param.yDeviation_max;
    algo.zDeviation_max = param.zDeviation_max;
    algo.maxLineSkipNum = param.maxLineSkipNum;
    algo.maxSkipDistance = param.maxSkipDistance;
    algo.minLTypeTreeLen = param.minLTypeTreeLen;
    algo.minVTypeTreeLen = param.minVTypeTreeLen;
    return algo;
}

SSG_planeCalibPara ToAlgoPlaneCalibParam(const VrCameraPlaneCalibParam* cameraCalibParam)
{
    SSG_planeCalibPara algo;
    if (cameraCalibParam) {
        memcpy(algo.planeCalib, cameraCalibParam->planeCalib, sizeof(double) * 9);
        memcpy(algo.invRMatrix, cameraCalibParam->invRMatrix, sizeof(double) * 9);
        algo.planeHeight = cameraCalibParam->planeHeight;
    } else {
        // 使用默认单位矩阵
        double identity[9] = {1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0};
        memcpy(algo.planeCalib, identity, sizeof(double) * 9);
        memcpy(algo.invRMatrix, identity, sizeof(double) * 9);
        algo.planeHeight = -1.0;
    }
    return algo;
}

void LogAlgoParams(const std::string& logTag,
                   const WD_workpieceHoleParam& workpiecePara,
                   const SSG_lineSegParam& lineSegPara,
                   const SSG_outlierFilterParam& filterParam,
                   const SSG_treeGrowParam& growParam,
                   const SSG_planeCalibPara& groundCalibPara,
                   const double clibMatrix[16])
{
    LOG_INFO("%s clibMatrix: \n\t[%.3f, %.3f, %.3f, %.3f] \n\t[ %.3f, %.3f, %.3f, %.3f] \n\t[ %.3f, %.3f, %.3f, %.3f] \n\t[ %.3f, %.3f, %.3f, %.3f]\n",
        logTag.c_str(),
        clibMatrix[0], clibMatrix[1], clibMatrix[2], clibMatrix[3],
        clibMatrix[4], clibMatrix[5], clibMatrix[6], clibMatrix[7],
        clibMatrix[8], clibMatrix[9], clibMatrix[10], clibMatrix[11],
        clibMatrix[12], clibMatrix[13], clibMatrix[14], clibMatrix[15]);

    LOG_INFO("%s WorkpieceHole: type=%d, holeDiameter=%.1f, holeDist_L=%.1f, holeDist_W=%.1f, xLen=%.1f, yLen=%.1f, H=%.1f\n",
        logTag.c_str(),
        workpiecePara.workpieceType, workpiecePara.holeDiameter, workpiecePara.holeDist_L, workpiecePara.holeDist_W,
        workpiecePara.xLen, workpiecePara.yLen, workpiecePara.H);

    LOG_INFO("%s LineSeg: distScale=%.1f, segGapTh_y=%.1f, segGapTh_z=%.1f\n",
        logTag.c_str(),
        lineSegPara.distScale, lineSegPara.segGapTh_y, lineSegPara.segGapTh_z);

    LOG_INFO("%s Tree Grow: yDeviation_max=%.1f, zDeviation_max=%.1f, maxLineSkipNum=%d, maxSkipDistance=%.1f, minLTypeTreeLen=%.1f, minVTypeTreeLen=%.1f\n",
        logTag.c_str(),
        growParam.yDeviation_max, growParam.zDeviation_max, growParam.maxLineSkipNum,
        growParam.maxSkipDistance, growParam.minLTypeTreeLen, growParam.minVTypeTreeLen);

    LOG_INFO("%s Filter: continuityTh=%.1f, outlierTh=%.1f\n",
        logTag.c_str(),
        filterParam.continuityTh, filterParam.outlierTh);

    LOG_INFO("%s Plane height: %.3f\n", logTag.c_str(), groundCalibPara.planeHeight);
    LOG_INFO("%s Plane calibration matrix: [%f, %f, %f; %f, %f, %f; %f, %f, %f]\n",
             logTag.c_str(),
             groundCalibPara.planeCalib[0], groundCalibPara.planeCalib[1], groundCalibPara.planeCalib[2],
             groundCalibPara.planeCalib[3], groundCalibPara.planeCalib[4], groundCalibPara.planeCalib[5],
             groundCalibPara.planeCalib[6], groundCalibPara.planeCalib[7], groundCalibPara.planeCalib[8]);

    LOG_INFO("%s Plane invRMatrix matrix: [%f, %f, %f; %f, %f, %f; %f, %f, %f]\n",
             logTag.c_str(),
             groundCalibPara.invRMatrix[0], groundCalibPara.invRMatrix[1], groundCalibPara.invRMatrix[2],
             groundCalibPara.invRMatrix[3], groundCalibPara.invRMatrix[4], groundCalibPara.invRMatrix[5],
             groundCalibPara.invRMatrix[6], groundCalibPara.invRMatrix[7], groundCalibPara.invRMatrix[8]);
}

} // namespace AlgoParamConverter