#include "AlgoParamConverter.h"
#include "VrLog.h"

namespace AlgoParamConverter
{

SHoleDetectionParams ToAlgoParam(const VrHoleDetectionParam& param)
{
    SHoleDetectionParams algo;
    algo.angleThresholdPos   = static_cast<float>(param.angleThresholdPos);
    algo.angleThresholdNeg   = static_cast<float>(param.angleThresholdNeg);
    algo.minPitDepth         = static_cast<float>(param.minPitDepth);
    algo.minRadius           = static_cast<float>(param.minRadius);
    algo.maxRadius           = static_cast<float>(param.maxRadius);
    algo.expansionSize1      = param.expansionSize1;
    algo.expansionSize2      = param.expansionSize2;
    algo.minVTransitionPoints = param.minVTransitionPoints;
    return algo;
}

SHoleFilterParams ToAlgoParam(const VrHoleFilterParam& param)
{
    SHoleFilterParams algo;
    algo.maxEccentricity    = static_cast<float>(param.maxEccentricity);
    algo.minAngularCoverage = static_cast<float>(param.minAngularCoverage);
    algo.maxRadiusFitRatio  = static_cast<float>(param.maxRadiusFitRatio);
    algo.minQualityScore    = static_cast<float>(param.minQualityScore);
    algo.maxPlaneResidual   = static_cast<float>(param.maxPlaneResidual);
    algo.maxAngularGap      = static_cast<float>(param.maxAngularGap);
    algo.minInlierRatio     = static_cast<float>(param.minInlierRatio);
    return algo;
}

void LogAlgoParams(const std::string& logTag,
                   const SHoleDetectionParams& detectionParams,
                   const SHoleFilterParams& filterParams,
                   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 DetectionParams: angleThresholdPos=%.1f, angleThresholdNeg=%.1f, minPitDepth=%.1f\n",
        logTag.c_str(), detectionParams.angleThresholdPos,
        detectionParams.angleThresholdNeg, detectionParams.minPitDepth);
    LOG_INFO("%s DetectionParams: minRadius=%.1f, maxRadius=%.1f, expansionSize1=%d, expansionSize2=%d, minVTransitionPoints=%d\n",
        logTag.c_str(), detectionParams.minRadius, detectionParams.maxRadius,
        detectionParams.expansionSize1, detectionParams.expansionSize2, detectionParams.minVTransitionPoints);

    LOG_INFO("%s FilterParams: maxEccentricity=%.5f, minAngularCoverage=%.1f, maxRadiusFitRatio=%.2f\n",
        logTag.c_str(), filterParams.maxEccentricity, filterParams.minAngularCoverage, filterParams.maxRadiusFitRatio);
    LOG_INFO("%s FilterParams: minQualityScore=%.2f, maxPlaneResidual=%.1f, maxAngularGap=%.1f, minInlierRatio=%.2f\n",
        logTag.c_str(), filterParams.minQualityScore, filterParams.maxPlaneResidual,
        filterParams.maxAngularGap, filterParams.minInlierRatio);
}

} // namespace AlgoParamConverter