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

namespace AlgoParamConverter
{

RansacPlaneSegmentationParams ToAlgoParam(const VrRansacPlaneSegmentationParam& param)
{
    RansacPlaneSegmentationParams algo;
    algo.distanceThreshold = static_cast<float>(param.distanceThreshold);
    algo.maxIterations = param.maxIterations;
    algo.minPlanePoints = param.minPlanePoints;
    algo.maxPlanes = param.maxPlanes;
    algo.growthZThreshold = static_cast<float>(param.growthZThreshold);
    algo.minPlaneRatio = static_cast<float>(param.minPlaneRatio);
    algo.maxNormalAngleDeg = static_cast<float>(param.maxNormalAngleDeg);
    return algo;
}

SHoleDetectionParams ToAlgoParam(const VrHoleDetectionParam& param)
{
    SHoleDetectionParams algo;
    algo.angleThresholdPos = static_cast<float>(param.angleThresholdPos);
    algo.angleThresholdNeg = static_cast<float>(param.angleThresholdNeg);
    algo.angleSearchDistance = static_cast<float>(param.angleSearchDistance);
    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;
    algo.jumpThresholdResidual = static_cast<float>(param.jumpThresholdResidual);
    algo.gapJumpThresholdResidual = static_cast<float>(param.gapJumpThresholdResidual);
    algo.maxGapPointsInLine = param.maxGapPointsInLine;
    algo.minFeatureSpan = static_cast<float>(param.minFeatureSpan);
    algo.residualSmoothWindow = param.residualSmoothWindow;
    algo.slopeAngleThreshold = static_cast<float>(param.slopeAngleThreshold);
    algo.edgeBoundaryFilterDist = static_cast<float>(param.edgeBoundaryFilterDist);
    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);
    algo.minHoleDepth = static_cast<float>(param.minHoleDepth);
    return algo;
}

void LogAlgoParams(const std::string& logTag,
                   const RansacPlaneSegmentationParams& ransacParams,
                   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 RansacParams: distanceThreshold=%.2f, maxIterations=%d, minPlanePoints=%d, maxPlanes=%d, growthZThreshold=%.2f, minPlaneRatio=%.2f, maxNormalAngleDeg=%.1f\n",
        logTag.c_str(),
        ransacParams.distanceThreshold,
        ransacParams.maxIterations,
        ransacParams.minPlanePoints,
        ransacParams.maxPlanes,
        ransacParams.growthZThreshold,
        ransacParams.minPlaneRatio,
        ransacParams.maxNormalAngleDeg);

    LOG_INFO("%s DetectionParams: angleThresholdPos=%.1f, angleThresholdNeg=%.1f, angleSearchDistance=%.2f, minPitDepth=%.2f\n",
        logTag.c_str(),
        detectionParams.angleThresholdPos,
        detectionParams.angleThresholdNeg,
        detectionParams.angleSearchDistance,
        detectionParams.minPitDepth);
    LOG_INFO("%s DetectionParams: minRadius=%.2f, maxRadius=%.2f, expansionSize1=%d, expansionSize2=%d\n",
        logTag.c_str(),
        detectionParams.minRadius,
        detectionParams.maxRadius,
        detectionParams.expansionSize1,
        detectionParams.expansionSize2);
    LOG_INFO("%s DetectionParams: minVTransitionPoints=%d, jumpThresholdResidual=%.2f, gapJumpThresholdResidual=%.2f, maxGapPointsInLine=%d\n",
        logTag.c_str(),
        detectionParams.minVTransitionPoints,
        detectionParams.jumpThresholdResidual,
        detectionParams.gapJumpThresholdResidual,
        detectionParams.maxGapPointsInLine);
    LOG_INFO("%s DetectionParams: minFeatureSpan=%.2f, residualSmoothWindow=%d, slopeAngleThreshold=%.2f, edgeBoundaryFilterDist=%.2f\n",
        logTag.c_str(),
        detectionParams.minFeatureSpan,
        detectionParams.residualSmoothWindow,
        detectionParams.slopeAngleThreshold,
        detectionParams.edgeBoundaryFilterDist);

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

} // namespace AlgoParamConverter