// This file is part of OpenCV project.
// It is subject to the license terms in the LICENSE file found in the top-level directory
// of this distribution and at http://opencv.org/license.html

#include "precomp.hpp"
#include "utils.hpp"

namespace cv
{

class OdometrySettings::Impl
{
public:
    Impl() {};
    virtual ~Impl() {};
    virtual void setCameraMatrix(InputArray val) = 0;
    virtual void getCameraMatrix(OutputArray val) const = 0;
    virtual void setIterCounts(InputArray val) = 0;
    virtual void getIterCounts(OutputArray val) const = 0;

    virtual void  setMinDepth(float val) = 0;
    virtual float getMinDepth() const = 0;
    virtual void  setMaxDepth(float val) = 0;
    virtual float getMaxDepth() const = 0;
    virtual void  setMaxDepthDiff(float val) = 0;
    virtual float getMaxDepthDiff() const = 0;
    virtual void  setMaxPointsPart(float val) = 0;
    virtual float getMaxPointsPart() const = 0;

    virtual void setSobelSize(int val) = 0;
    virtual int  getSobelSize() const = 0;
    virtual void   setSobelScale(double val) = 0;
    virtual double getSobelScale() const = 0;

    virtual void setNormalWinSize(int val) = 0;
    virtual int  getNormalWinSize() const = 0;
    virtual void setNormalDiffThreshold(float val) = 0;
    virtual float getNormalDiffThreshold() const = 0;
    virtual void setNormalMethod(RgbdNormals::RgbdNormalsMethod nm) = 0;
    virtual RgbdNormals::RgbdNormalsMethod getNormalMethod() const = 0;

    virtual void  setAngleThreshold(float val) = 0;
    virtual float getAngleThreshold() const = 0;
    virtual void  setMaxTranslation(float val) = 0;
    virtual float getMaxTranslation() const = 0;
    virtual void  setMaxRotation(float val) = 0;
    virtual float getMaxRotation() const = 0;

    virtual void  setMinGradientMagnitude(float val) = 0;
    virtual float getMinGradientMagnitude() const = 0;
    virtual void setMinGradientMagnitudes(InputArray val) = 0;
    virtual void getMinGradientMagnitudes(OutputArray val) const = 0;
};

class OdometrySettingsImplCommon : public OdometrySettings::Impl
{
public:
    OdometrySettingsImplCommon();
    ~OdometrySettingsImplCommon() {};
    virtual void setCameraMatrix(InputArray val) override;
    virtual void getCameraMatrix(OutputArray val) const override;
    virtual void setIterCounts(InputArray val) override;
    virtual void getIterCounts(OutputArray val) const override;

    virtual void  setMinDepth(float val) override;
    virtual float getMinDepth() const override;
    virtual void  setMaxDepth(float val) override;
    virtual float getMaxDepth() const override;
    virtual void  setMaxDepthDiff(float val) override;
    virtual float getMaxDepthDiff() const override;
    virtual void  setMaxPointsPart(float val) override;
    virtual float getMaxPointsPart() const override;

    virtual void setSobelSize(int val) override;
    virtual int  getSobelSize() const override;
    virtual void   setSobelScale(double val) override;
    virtual double getSobelScale() const override;

    virtual void setNormalWinSize(int val) override;
    virtual int  getNormalWinSize() const override;
    virtual void setNormalDiffThreshold(float val) override;
    virtual float getNormalDiffThreshold() const override;
    virtual void setNormalMethod(RgbdNormals::RgbdNormalsMethod nm) override;
    virtual RgbdNormals::RgbdNormalsMethod getNormalMethod() const override;

    virtual void  setAngleThreshold(float val) override;
    virtual float getAngleThreshold() const override;
    virtual void  setMaxTranslation(float val) override;
    virtual float getMaxTranslation() const override;
    virtual void  setMaxRotation(float val) override;
    virtual float getMaxRotation() const override;

    virtual void  setMinGradientMagnitude(float val) override;
    virtual float getMinGradientMagnitude() const override;
    virtual void setMinGradientMagnitudes(InputArray val) override;
    virtual void getMinGradientMagnitudes(OutputArray val) const override;

private:
    Matx33f cameraMatrix;
    std::vector<int> iterCounts;

    float minDepth;
    float maxDepth;
    float maxDepthDiff;
    float maxPointsPart;

    int sobelSize;
    double sobelScale;

    int normalWinSize;
    float normalDiffThreshold;
    RgbdNormals::RgbdNormalsMethod normalMethod;

    float angleThreshold;
    float maxTranslation;
    float maxRotation;

    float minGradientMagnitude;
    std::vector<float> minGradientMagnitudes;

public:
    class DefaultSets {
    public:
        static const int width  = 640;
        static const int height = 480;
        static constexpr float fx = 525.f;
        static constexpr float fy = 525.f;
        static constexpr float cx = float(width) / 2.f - 0.5f;
        static constexpr float cy = float(height) / 2.f - 0.5f;
        const cv::Matx33f defaultCameraMatrix = { fx, 0, cx, 0, fy, cy, 0, 0, 1 };
        const std::vector<int> defaultIterCounts = { 7, 7, 7, 10 };

        static constexpr float defaultMinDepth = 0.f;
        static constexpr float defaultMaxDepth = 4.f;
        static constexpr float defaultMaxDepthDiff = 0.07f;
        static constexpr float defaultMaxPointsPart = 0.07f;

        static const int defaultSobelSize = 3;
        static constexpr double defaultSobelScale = 1. / 8.;

        static const int defaultNormalWinSize = 5;
        static const RgbdNormals::RgbdNormalsMethod defaultNormalMethod = RgbdNormals::RGBD_NORMALS_METHOD_FALS;
        static constexpr float defaultNormalDiffThreshold = 50.f;

        static constexpr float defaultAngleThreshold = (float)(30. * CV_PI / 180.);
        static constexpr float defaultMaxTranslation = 0.15f;
        static constexpr float defaultMaxRotation = 15.f;

        static constexpr float defaultMinGradientMagnitude = 10.f;
        const std::vector<float> defaultMinGradientMagnitudes = std::vector<float>(defaultIterCounts.size(), 10.f /*defaultMinGradientMagnitude*/);
    };
};


OdometrySettings::OdometrySettings()
{
    this->impl = makePtr<OdometrySettingsImplCommon>();
}

OdometrySettings::OdometrySettings(const OdometrySettings& s)
{
    this->impl = makePtr<OdometrySettingsImplCommon>(*s.impl.dynamicCast<OdometrySettingsImplCommon>());
}

OdometrySettings& OdometrySettings::operator=(const OdometrySettings& s)
{
    this->impl = makePtr<OdometrySettingsImplCommon>(*s.impl.dynamicCast<OdometrySettingsImplCommon>());
    return *this;
}

void OdometrySettings::setCameraMatrix(InputArray val) { this->impl->setCameraMatrix(val); }
void OdometrySettings::getCameraMatrix(OutputArray val) const { this->impl->getCameraMatrix(val); }
void OdometrySettings::setIterCounts(InputArray val) { this->impl->setIterCounts(val); }
void OdometrySettings::getIterCounts(OutputArray val) const { this->impl->getIterCounts(val); }

void  OdometrySettings::setMinDepth(float val) { this->impl->setMinDepth(val); }
float OdometrySettings::getMinDepth() const { return this->impl->getMinDepth(); }
void  OdometrySettings::setMaxDepth(float val) { this->impl->setMaxDepth(val); }
float OdometrySettings::getMaxDepth() const { return this->impl->getMaxDepth(); }
void  OdometrySettings::setMaxDepthDiff(float val) { this->impl->setMaxDepthDiff(val); }
float OdometrySettings::getMaxDepthDiff() const { return this->impl->getMaxDepthDiff(); }
void  OdometrySettings::setMaxPointsPart(float val) { this->impl->setMaxPointsPart(val); }
float OdometrySettings::getMaxPointsPart() const { return this->impl->getMaxPointsPart(); }

void OdometrySettings::setSobelSize(int val) { this->impl->setSobelSize(val); }
int  OdometrySettings::getSobelSize() const { return this->impl->getSobelSize(); }
void   OdometrySettings::setSobelScale(double val) { this->impl->setSobelScale(val); }
double OdometrySettings::getSobelScale() const { return this->impl->getSobelScale(); }

void  OdometrySettings::setNormalWinSize(int val) { this->impl->setNormalWinSize(val); }
int   OdometrySettings::getNormalWinSize() const { return this->impl->getNormalWinSize(); }
void  OdometrySettings::setNormalDiffThreshold(float val) { this->impl->setNormalDiffThreshold(val); }
float OdometrySettings::getNormalDiffThreshold() const { return this->impl->getNormalDiffThreshold(); }
void  OdometrySettings::setNormalMethod(RgbdNormals::RgbdNormalsMethod nm) { this->impl->setNormalMethod(nm); }
RgbdNormals::RgbdNormalsMethod OdometrySettings::getNormalMethod() const { return this->impl->getNormalMethod(); }

void  OdometrySettings::setAngleThreshold(float val) { this->impl->setAngleThreshold(val); }
float OdometrySettings::getAngleThreshold() const { return this->impl->getAngleThreshold(); }
void  OdometrySettings::setMaxTranslation(float val) { this->impl->setMaxTranslation(val); }
float OdometrySettings::getMaxTranslation() const { return this->impl->getMaxTranslation(); }
void  OdometrySettings::setMaxRotation(float val) { this->impl->setMaxRotation(val); }
float OdometrySettings::getMaxRotation() const { return this->impl->getMaxRotation(); }

void  OdometrySettings::setMinGradientMagnitude(float val) { this->impl->setMinGradientMagnitude(val); }
float OdometrySettings::getMinGradientMagnitude() const { return this->impl->getMinGradientMagnitude(); }
void OdometrySettings::setMinGradientMagnitudes(InputArray val) { this->impl->setMinGradientMagnitudes(val); }
void OdometrySettings::getMinGradientMagnitudes(OutputArray val) const { this->impl->getMinGradientMagnitudes(val); }


OdometrySettingsImplCommon::OdometrySettingsImplCommon()
{
    DefaultSets ds;
    this->cameraMatrix = ds.defaultCameraMatrix;
    this->iterCounts = ds.defaultIterCounts;

    this->minDepth = ds.defaultMinDepth;
    this->maxDepth = ds.defaultMaxDepth;
    this->maxDepthDiff = ds.defaultMaxDepthDiff;
    this->maxPointsPart = ds.defaultMaxPointsPart;

    this->sobelSize = ds.defaultSobelSize;
    this->sobelScale = ds.defaultSobelScale;

    this->normalWinSize = ds.defaultNormalWinSize;
    this->normalDiffThreshold = ds.defaultNormalDiffThreshold;
    this->normalMethod = ds.defaultNormalMethod;

    this->angleThreshold = ds.defaultAngleThreshold;
    this->maxTranslation = ds.defaultMaxTranslation;
    this->maxRotation = ds.defaultMaxRotation;

    this->minGradientMagnitude = ds.defaultMinGradientMagnitude;
    this->minGradientMagnitudes = ds.defaultMinGradientMagnitudes;
}

void OdometrySettingsImplCommon::setCameraMatrix(InputArray val)
{
    if (!val.empty())
    {
        CV_Assert(val.rows() == 3 && val.cols() == 3 && val.channels() == 1);
        CV_Assert(val.type() == CV_32F);
        val.copyTo(cameraMatrix);
    }
    else
    {
        DefaultSets ds;
        this->cameraMatrix = ds.defaultCameraMatrix;
    }
}

void OdometrySettingsImplCommon::getCameraMatrix(OutputArray val) const
{
    Mat(this->cameraMatrix).copyTo(val);
}

void OdometrySettingsImplCommon::setIterCounts(InputArray val)
{
    if (!val.empty())
    {
        size_t nLevels = val.size(-1).width;
        std::vector<Mat> pyramids;
        val.getMatVector(pyramids);
        this->iterCounts.clear();
        for (size_t i = 0; i < nLevels; i++)
            this->iterCounts.push_back(pyramids[i].at<int>(0));
    }
    else
    {
        DefaultSets ds;
        this->iterCounts = ds.defaultIterCounts;
    }
}

void OdometrySettingsImplCommon::getIterCounts(OutputArray val) const
{
    Mat(this->iterCounts).copyTo(val);
}

void OdometrySettingsImplCommon::setMinDepth(float val)
{
    this->minDepth = val;
}
float OdometrySettingsImplCommon::getMinDepth() const
{
    return this->minDepth;
}
void  OdometrySettingsImplCommon::setMaxDepth(float val)
{
    this->maxDepth = val;
}
float OdometrySettingsImplCommon::getMaxDepth() const
{
    return this->maxDepth;
}
void  OdometrySettingsImplCommon::setMaxDepthDiff(float val)
{
    this->maxDepthDiff = val;
}
float OdometrySettingsImplCommon::getMaxDepthDiff() const
{
    return this->maxDepthDiff;
}
void  OdometrySettingsImplCommon::setMaxPointsPart(float val)
{
    this->maxPointsPart = val;
}
float OdometrySettingsImplCommon::getMaxPointsPart() const
{
    return this->maxPointsPart;
}

void OdometrySettingsImplCommon::setSobelSize(int val)
{
    this->sobelSize = val;
}
int  OdometrySettingsImplCommon::getSobelSize() const
{
    return this->sobelSize;
}
void   OdometrySettingsImplCommon::setSobelScale(double val)
{
    this->sobelScale = val;
}
double OdometrySettingsImplCommon::getSobelScale() const
{
    return this->sobelScale;
}
void OdometrySettingsImplCommon::setNormalWinSize(int val)
{
    this->normalWinSize = val;
}
int  OdometrySettingsImplCommon::getNormalWinSize() const
{
    return this->normalWinSize;
}
void OdometrySettingsImplCommon::setNormalDiffThreshold(float val)
{
    this->normalDiffThreshold = val;
}
float OdometrySettingsImplCommon::getNormalDiffThreshold() const
{
    return this->normalDiffThreshold;
}
void OdometrySettingsImplCommon::setNormalMethod(RgbdNormals::RgbdNormalsMethod nm)
{
    this->normalMethod = nm;
}
RgbdNormals::RgbdNormalsMethod OdometrySettingsImplCommon::getNormalMethod() const
{
    return this->normalMethod;
}
void  OdometrySettingsImplCommon::setAngleThreshold(float val)
{
    this->angleThreshold = val;
}
float OdometrySettingsImplCommon::getAngleThreshold() const
{
    return this->angleThreshold;
}
void  OdometrySettingsImplCommon::setMaxTranslation(float val)
{
    this->maxTranslation = val;
}
float OdometrySettingsImplCommon::getMaxTranslation() const
{
    return this->maxTranslation;
}
void  OdometrySettingsImplCommon::setMaxRotation(float val)
{
    this->maxRotation = val;
}
float OdometrySettingsImplCommon::getMaxRotation() const
{
    return this->maxRotation;
}

void  OdometrySettingsImplCommon::setMinGradientMagnitude(float val)
{
    this->minGradientMagnitude = val;
}
float OdometrySettingsImplCommon::getMinGradientMagnitude() const
{
    return this->minGradientMagnitude;
}
void OdometrySettingsImplCommon::setMinGradientMagnitudes(InputArray val)
{
    if (!val.empty())
    {
        size_t nLevels = val.size(-1).width;
        std::vector<Mat> pyramids;
        val.getMatVector(pyramids);
        this->minGradientMagnitudes.clear();
        for (size_t i = 0; i < nLevels; i++)
            this->minGradientMagnitudes.push_back(pyramids[i].at<float>(0));
    }
    else
    {
        DefaultSets ds;
        this->minGradientMagnitudes = ds.defaultMinGradientMagnitudes;
    }
}
void OdometrySettingsImplCommon::getMinGradientMagnitudes(OutputArray val) const
{
    Mat(this->minGradientMagnitudes).copyTo(val);
}

}
