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

namespace AlgoParamConverter
{

SSX_rodParam ToAlgoParam(const VrRodParam& param)
{
    SSX_rodParam algo;
    algo.diameter = param.rodDiameter;
    algo.len = param.rodLen;
    return algo;
}

SSG_cornerParam ToAlgoParam(const VrCornerParam& param)
{
    SSG_cornerParam algo;
    algo.minEndingGap = param.minEndingGap;
    algo.minEndingGap_z = param.minEndingGap_z;
    algo.scale = param.scale;
    algo.cornerTh = param.cornerTh;
    algo.jumpCornerTh_1 = param.jumpCornerTh_1;
    algo.jumpCornerTh_2 = param.jumpCornerTh_2;
    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 && cameraCalibParam->isCalibrated) {
        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 SSX_rodParam& rodParam,
                   const SSG_cornerParam& cornerParam,
                   const SSG_outlierFilterParam& filterParam,
                   const SSG_treeGrowParam& growParam,
                   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 Rod: diameter=%.1f, len=%.1f\n",
        logTag.c_str(), rodParam.diameter, rodParam.len);

    LOG_INFO("%s Corner: cornerTh=%.1f, scale=%.1f, minEndingGap=%.1f, minEndingGap_z=%.1f, jumpCornerTh_1=%.1f, jumpCornerTh_2=%.1f\n",
        logTag.c_str(), cornerParam.cornerTh, cornerParam.scale, cornerParam.minEndingGap,
        cornerParam.minEndingGap_z, cornerParam.jumpCornerTh_1, cornerParam.jumpCornerTh_2);

    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);
}

} // namespace AlgoParamConverter