/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * @file testExpression.cpp
 * @date September 18, 2014
 * @author Frank Dellaert
 * @author Paul Furgale
 * @brief unit tests for Block Automatic Differentiation
 */

#include <gtsam/3rdparty/ceres/example.h>
#include <gtsam/nonlinear/AdaptAutoDiff.h>
#include <gtsam/nonlinear/Expression.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/Cal3Bundler.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/Testable.h>

#include <CppUnitLite/TestHarness.h>

#include <boost/assign/list_of.hpp>
using boost::assign::list_of;
using boost::assign::map_list_of;

namespace gtsam {

// Special version of Cal3Bundler so that default constructor = 0,0,0
struct Cal3Bundler0 : public Cal3Bundler {
  Cal3Bundler0(double f = 0, double k1 = 0, double k2 = 0, double u0 = 0,
               double v0 = 0)
      : Cal3Bundler(f, k1, k2, u0, v0) {}
  Cal3Bundler0 retract(const Vector& d) const {
    return Cal3Bundler0(fx() + d(0), k1() + d(1), k2() + d(2), px(), py());
  }
  Vector3 localCoordinates(const Cal3Bundler0& T2) const {
    return T2.vector() - vector();
  }
};

template <>
struct traits<Cal3Bundler0> : public internal::Manifold<Cal3Bundler0> {};

// With that, camera below behaves like Snavely's 9-dim vector
typedef PinholeCamera<Cal3Bundler0> Camera;
}

using namespace std;
using namespace gtsam;

/* ************************************************************************* */
// Check that ceres rotation convention is the same
TEST(AdaptAutoDiff, Rotation) {
  Vector3 axisAngle(0.1, 0.2, 0.3);
  Matrix3 expected = Rot3::Rodrigues(axisAngle).matrix();
  Matrix3 actual;
  ceres::AngleAxisToRotationMatrix(axisAngle.data(), actual.data());
  EXPECT(assert_equal(expected, actual));
}

/* ************************************************************************* */
// Some Ceres Snippets copied for testing
// Copyright 2010, 2011, 2012 Google Inc. All rights reserved.
template <typename T>
inline T& RowMajorAccess(T* base, int rows, int cols, int i, int j) {
  return base[cols * i + j];
}

inline double RandDouble() {
  double r = static_cast<double>(rand());
  return r / RAND_MAX;
}

// A structure for projecting a 3x4 camera matrix and a
// homogeneous 3D point, to a 2D inhomogeneous point.
struct Projective {
  // Function that takes P and X as separate vectors:
  //   P, X -> x
  template <typename A>
  bool operator()(A const P[12], A const X[4], A x[2]) const {
    A PX[3];
    for (int i = 0; i < 3; ++i) {
      PX[i] = RowMajorAccess(P, 3, 4, i, 0) * X[0] +
              RowMajorAccess(P, 3, 4, i, 1) * X[1] +
              RowMajorAccess(P, 3, 4, i, 2) * X[2] +
              RowMajorAccess(P, 3, 4, i, 3) * X[3];
    }
    if (PX[2] != 0.0) {
      x[0] = PX[0] / PX[2];
      x[1] = PX[1] / PX[2];
      return true;
    }
    return false;
  }

  // Adapt to eigen types
  Vector2 operator()(const MatrixRowMajor& P, const Vector4& X) const {
    Vector2 x;
    if (operator()(P.data(), X.data(), x.data()))
      return x;
    else
      throw std::runtime_error("Projective fail");
  }
};

/* ************************************************************************* */
// Test Ceres AutoDiff
TEST(AdaptAutoDiff, AutoDiff) {
  using ceres::internal::AutoDiff;

  // Instantiate function
  Projective projective;

  // Make arguments
  typedef Eigen::Matrix<double, 3, 4, Eigen::RowMajor> RowMajorMatrix34;
  RowMajorMatrix34 P;
  P << 1, 0, 0, 0, 0, 1, 0, 5, 0, 0, 1, 0;
  Vector4 X(10, 0, 5, 1);

  // Apply the mapping, to get image point b_x.
  Vector expected = Vector2(2, 1);
  Vector2 actual = projective(P, X);
  EXPECT(assert_equal(expected, actual, 1e-9));

  // Get expected derivatives
  Matrix E1 = numericalDerivative21<Vector2, RowMajorMatrix34, Vector4>(
      Projective(), P, X);
  Matrix E2 = numericalDerivative22<Vector2, RowMajorMatrix34, Vector4>(
      Projective(), P, X);

  // Get derivatives with AutoDiff
  Vector2 actual2;
  MatrixRowMajor H1(2, 12), H2(2, 4);
  double* parameters[] = {P.data(), X.data()};
  double* jacobians[] = {H1.data(), H2.data()};
  CHECK((AutoDiff<Projective, double, 12, 4>::Differentiate(
      projective, parameters, 2, actual2.data(), jacobians)));
  EXPECT(assert_equal(E1, H1, 1e-8));
  EXPECT(assert_equal(E2, H2, 1e-8));
}

/* ************************************************************************* */
// Test Ceres AutoDiff on Snavely, defined in ceres_example.h
// Adapt to GTSAM types
Vector2 adapted(const Vector9& P, const Vector3& X) {
  SnavelyProjection snavely;
  Vector2 x;
  if (snavely(P.data(), X.data(), x.data()))
    return x;
  else
    throw std::runtime_error("Snavely fail");
}

/* ************************************************************************* */
namespace example {
Camera camera(Pose3(Rot3().retract(Vector3(0.1, 0.2, 0.3)), Point3(0, 5, 0)),
              Cal3Bundler0(1, 0, 0));
Point3 point(10, 0, -5);  // negative Z-axis convention of Snavely!
Vector9 P = Camera().localCoordinates(camera);
Vector3 X = point;
#ifdef GTSAM_POSE3_EXPMAP
Vector2 expectedMeasurement(1.3124675, 1.2057287);
#else
Vector2 expectedMeasurement(1.2431567, 1.2525694);
#endif
Matrix E1 = numericalDerivative21<Vector2, Vector9, Vector3>(adapted, P, X);
Matrix E2 = numericalDerivative22<Vector2, Vector9, Vector3>(adapted, P, X);
}

/* ************************************************************************* */
// Check that Local worked as expected
TEST(AdaptAutoDiff, Local) {
  using namespace example;
#ifdef GTSAM_POSE3_EXPMAP
  Vector9 expectedP = (Vector9() << 0.1, 0.2, 0.3, 0.7583528428, 4.9582357859, -0.224941471539, 1, 0, 0).finished();
#else
  Vector9 expectedP = (Vector9() << 0.1, 0.2, 0.3, 0, 5, 0, 1, 0, 0).finished();
#endif
  EXPECT(equal_with_abs_tol(expectedP, P));
  Vector3 expectedX(10, 0, -5);  // negative Z-axis convention of Snavely!
  EXPECT(equal_with_abs_tol(expectedX, X));
}

/* ************************************************************************* */
// Test Ceres AutoDiff
TEST(AdaptAutoDiff, AutoDiff2) {
  using namespace example;
  using ceres::internal::AutoDiff;

  // Apply the mapping, to get image point b_x.
  Vector2 actual = adapted(P, X);
  EXPECT(assert_equal(expectedMeasurement, actual, 1e-6));

  // Instantiate function
  SnavelyProjection snavely;

  // Get derivatives with AutoDiff
  Vector2 actual2;
  MatrixRowMajor H1(2, 9), H2(2, 3);
  double* parameters[] = {P.data(), X.data()};
  double* jacobians[] = {H1.data(), H2.data()};
  CHECK((AutoDiff<SnavelyProjection, double, 9, 3>::Differentiate(
      snavely, parameters, 2, actual2.data(), jacobians)));
  EXPECT(assert_equal(E1, H1, 1e-8));
  EXPECT(assert_equal(E2, H2, 1e-8));
}

/* ************************************************************************* */
// Test AutoDiff wrapper Snavely
TEST(AdaptAutoDiff, AdaptAutoDiff) {
  using namespace example;

  typedef AdaptAutoDiff<SnavelyProjection, 2, 9, 3> Adaptor;
  Adaptor snavely;

  // Apply the mapping, to get image point b_x.
  Vector2 actual = snavely(P, X);
  EXPECT(assert_equal(expectedMeasurement, actual, 1e-6));

  // Get derivatives with AutoDiff, not gives RowMajor results!
  Matrix29 H1;
  Matrix23 H2;
  Vector2 actual2 = snavely(P, X, H1, H2);
  EXPECT(assert_equal(expectedMeasurement, actual2, 1e-6));
  EXPECT(assert_equal(E1, H1, 1e-8));
  EXPECT(assert_equal(E2, H2, 1e-8));
}

/* ************************************************************************* */
// Test AutoDiff wrapper in an expression
TEST(AdaptAutoDiff, SnavelyExpression) {
  typedef AdaptAutoDiff<SnavelyProjection, 2, 9, 3> Adaptor;

  Expression<Vector9> P(1);
  Expression<Vector3> X(2);

  Expression<Vector2> expression(Adaptor(), P, X);

  std::size_t RecordSize =
    sizeof(internal::BinaryExpression<Vector2, Vector9, Vector3>::Record);

  EXPECT_LONGS_EQUAL(
    internal::upAligned(RecordSize) + P.traceSize() + X.traceSize(),
    expression.traceSize());

  set<Key> expected = list_of(1)(2);

  EXPECT(expected == expression.keys());
}

/* ************************************************************************* */
int main() {
  TestResult tr;
  return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */


// std includes
#include <iostream> // cout, endl
#include <memory> // shared_ptr
#include <vector>
#include <tuple>
// thirdparties includes
#include <Eigen/Dense>
// lib includes
// // s0s
#include "s0s/runge_kutta_fehlberg.h"
// // sl0
#include "sl0/object.h"
#include "sl0/point.h"
// // sa0
#include "sa0/active.h"
#include "sa0/actuator/point.h"
#include "sa0/agent.h"
#include "sa0/behaviour.h"
// simple includes
#include "flow.h"

using TypeScalar = double;
template<typename ...Args>
using TypeContainer = std::vector<Args...>;
// Space
constexpr unsigned int DIM = 3;
using TypeVector = Eigen::Matrix<TypeScalar, DIM, 1>;
// Ref and View
template<int StateSize>
using TypeState = Eigen::Matrix<TypeScalar, StateSize, 1>;
template<typename ...Args>
using TypeRef = Eigen::Ref<Args...>;
template<typename ...Args>
using TypeView = Eigen::Map<Args...>;
// Point
using TypeStepPoint = sl0::StepPoint<TypeState, DIM, TypeRef, TypeView, Flow>;
// Choose passive
using TypeStepPassive = TypeStepPoint;
// Active
using TypeStepActuator = sl0::sa0::StepActuator<TypeStepPassive::TypeStateStatic, TypeRef, TypeStepPassive>;
using TypeStepPointSwim = sl0::sa0::StepPointSwim<TypeStepPassive::TypeStateStatic, TypeRef, TypeStepPassive, TypeVector>;
using TypeStepActive = sl0::sa0::StepActive<TypeState, TypeRef, TypeView, TypeStepPassive, TypeStepActuator>;
// Agent
class BehaviourCustom : public sl0::sa0::Behaviour<TypeStepActive::TypeStateStatic, TypeRef, TypeStepActive> {
    public:
        std::shared_ptr<Flow> sFlow;
    public:
        BehaviourCustom(const std::shared_ptr<Flow> p_sFlow) : sFlow(p_sFlow) {
        }
        void operator()(const TypeRef<const TypeStepActive::TypeStateStatic>& state, const double& t, const TypeStepActive&  stepActive) const override {
            dynamic_cast<TypeStepPointSwim&>(*stepActive.sStepActuators[0]).velocity = sFlow->getVelocity(stepActive.cX(state), t);
        }
};
using TypeBehaviour = BehaviourCustom;
using TypeStep = sl0::sa0::StepAgent<TypeState, TypeRef, TypeView, TypeStepActive, TypeBehaviour>;
// Solver
using TypeSolver = s0s::SolverRungeKuttaFehlberg;

int main () {
    TypeVector us = TypeVector::Constant(0.0);
    TypeVector x0 = TypeVector::Constant(1.0);
    double t0 = 0.0;
    double dt = 1e-3;
    double tEnd = 0.5;
    unsigned int nt = std::round((tEnd - t0) / dt);
    // Create flow
    std::shared_ptr<Flow> sFlow = std::make_shared<Flow>();
    // Create agent
    sl0::sa0::Agent<TypeState, TypeRef, TypeView, TypeStepActive, TypeBehaviour, TypeSolver> agent(TypeStepActive(TypeStepPassive(sFlow)), std::make_shared<TypeBehaviour>(sFlow));
    std::shared_ptr<TypeStepPointSwim> sStepPointSwim = std::make_shared<TypeStepPointSwim>(us);
    agent.sStep->register_actuator(sStepPointSwim);
    // Set initial state
    agent.sStep->x(agent.state) = x0;
    agent.t = t0;
    // Computation
    for(std::size_t i = 0; i < nt; i++) {
        agent.update(dt);
    }
    // out
    std::cout << "\n";
    std::cout << "agent advected and swimming in an exponential flow, t = " << agent.t << ", x = " << "\n";
    std::cout << "\n";
    std::cout << "agent position : " << "\n" << agent.sStep->x(agent.state) << "\n";
    std::cout << std::endl;
}


#ifdef _MSC_VER
#include <boost/math/tr1.hpp>
using namespace boost::math::tr1;
#else
#include <math.h>
#endif

#include "Hmm.hh"

StateDuration::StateDuration()
  : a(0), b(0), a0(0), mode(0)
{
}

void StateDuration::set_parameters(float a, float b)
{
  float temp;

  this->a = a;
  this->b = b;
  mode = 0;
  if (a > 0)
  {
    const_term = -a*logf(b)-lgammaf(a);
    temp = b*(a-1); // Mode of the gamma distribution
    mode = (int)floor(temp);
    if (get_log_prob(mode) < get_log_prob(mode+1))
      mode++;
  }
}

float StateDuration::get_log_prob(int duration) const
{
  if (a > 0)
    return (a-1)*logf(duration)-duration/b+const_term;
  return 0; // No duration penalty
}

void StateDuration::set_sr_parameters(float a0, float a1, float b0, float b1)
{
  this->a0 = a0;
  this->a1 = a1;
  this->b0 = b0;
  this->b1 = b1;
}

float StateDuration::get_sr_comp_log_prob(int duration, float sr) const
{
  if (a0 > 0)
  {
    float ia = a0 + sr*a1;
    float ib = b0 + sr*b1;
    return (ia-1)*logf((float)duration)-(float)duration/ib-ia*logf(ib)-
      lgammaf(ia);
  }
  else
  {
    return get_log_prob(duration);
  }
}


#include <boost/test/unit_test.hpp>
#include "camera.h"

BOOST_AUTO_TEST_SUITE(camera)

BOOST_AUTO_TEST_CASE(translation_rotation_1)
{
	graphic::Camera c;
	c.set_translation_rotation(math::vec<3>(0, 0, 0), math::vec<3>(0, 0, 0));

	BOOST_REQUIRE (((math::vec<3>(0, 0, 0) * c.get_view_matrix()) - math::vec<3>(0, 0, 0)).length_sq() < math::EPSILON);
	BOOST_REQUIRE ((math::vec<3>(0, 0, 0) - c.get_eye_point()).length_sq() < math::EPSILON);
	BOOST_REQUIRE ((math::vec<3>(0, 0, 1) - c.get_view_dir()).length_sq() < math::EPSILON);
	BOOST_REQUIRE ((math::vec<3>(0, 1, 0) - c.get_up_dir()).length_sq() < math::EPSILON);
	BOOST_REQUIRE ((math::vec<3>(1, 0, 0) - c.get_cross_dir()).length_sq() < math::EPSILON);
}

BOOST_AUTO_TEST_CASE(translation_rotation_2)
{
	graphic::Camera c;
	c.set_translation_rotation(math::vec<3>(-123, -456, -789), math::vec<3>(0, 0, 0));

	BOOST_REQUIRE (((math::vec<3>(0, 0, 0) * c.get_view_matrix()) + math::vec<3>(123, 456, 789)).length_sq() < math::EPSILON);
	BOOST_REQUIRE ((math::vec<3>(123, 456, 789) - c.get_eye_point()).length_sq() < math::EPSILON);
	BOOST_REQUIRE ((math::vec<3>(0, 0, 1) - c.get_view_dir()).length_sq() < math::EPSILON);
	BOOST_REQUIRE ((math::vec<3>(0, 1, 0) - c.get_up_dir()).length_sq() < math::EPSILON);
	BOOST_REQUIRE ((math::vec<3>(1, 0, 0) - c.get_cross_dir()).length_sq() < math::EPSILON);
}

BOOST_AUTO_TEST_CASE(translation_rotation_3)
{
	graphic::Camera c;
	c.set_translation_rotation(math::vec<3>(0, 0, 0), math::vec<3>(-math::PI/4, 0, 0));

	BOOST_REQUIRE (((math::vec<3>(0, 0, 0) * c.get_view_matrix()) + math::vec<3>(0, 0, 0)).length_sq() < math::EPSILON);
	BOOST_REQUIRE ((math::vec<3>(0, 0, 0) - c.get_eye_point()).length_sq() < math::EPSILON);
	BOOST_REQUIRE ((math::vec<3>(0, -0.707106f, 0.707106f) - c.get_view_dir()).length_sq() < math::EPSILON);
	BOOST_REQUIRE ((math::vec<3>(0, 0.707106f, 0.707106f) - c.get_up_dir()).length_sq() < math::EPSILON);
	BOOST_REQUIRE ((math::vec<3>(1, 0, 0) - c.get_cross_dir()).length_sq() < math::EPSILON);
}

BOOST_AUTO_TEST_CASE(translation_rotation_4)
{
	graphic::Camera c;
	c.set_translation_rotation(math::vec<3>(0, 0, 0), math::vec<3>(math::PI/2, 0, 0));

	BOOST_REQUIRE (((math::vec<3>(0, 0, 0) * c.get_view_matrix()) + math::vec<3>(0, 0, 0)).length_sq() < math::EPSILON);
	BOOST_REQUIRE ((math::vec<3>(0, 0, 0) - c.get_eye_point()).length_sq() < math::EPSILON);
	BOOST_REQUIRE ((math::vec<3>(0, 1, 0) - c.get_view_dir()).length_sq() < math::EPSILON);
	BOOST_REQUIRE ((math::vec<3>(0, 0, -1) - c.get_up_dir()).length_sq() < math::EPSILON);
	BOOST_REQUIRE ((math::vec<3>(1, 0, 0) - c.get_cross_dir()).length_sq() < math::EPSILON);
}

BOOST_AUTO_TEST_CASE(eye_lookat_up_1)
{
	graphic::Camera c;
	c.set_eye_lookat_up(math::vec<3>(0, 0, 0), math::vec<3>(0, 0, 1), math::vec<3>(0, 1, 0));

	BOOST_REQUIRE (((math::vec<3>(0, 0, 0) * c.get_view_matrix()) - math::vec<3>(0, 0, 0)).length_sq() < math::EPSILON);
	BOOST_REQUIRE ((math::vec<3>(0, 0, 0) - c.get_eye_point()).length_sq() < math::EPSILON);
	BOOST_REQUIRE ((math::vec<3>(0, 0, 1) - c.get_view_dir()).length_sq() < math::EPSILON);
	BOOST_REQUIRE ((math::vec<3>(0, 1, 0) - c.get_up_dir()).length_sq() < math::EPSILON);
	BOOST_REQUIRE ((math::vec<3>(1, 0, 0) - c.get_cross_dir()).length_sq() < math::EPSILON);
}

BOOST_AUTO_TEST_CASE(eye_lookat_up_2)
{
	graphic::Camera c;
	c.set_eye_lookat_up(math::vec<3>(123, 456, 789), math::vec<3>(123, 456, 790), math::vec<3>(0, 1, 0));

	BOOST_REQUIRE (((math::vec<3>(0, 0, 0) * c.get_view_matrix()) + math::vec<3>(123, 456, 789)).length_sq() < math::EPSILON);
	BOOST_REQUIRE ((math::vec<3>(123, 456, 789) - c.get_eye_point()).length_sq() < math::EPSILON);
	BOOST_REQUIRE ((math::vec<3>(0, 0, 1) - c.get_view_dir()).length_sq() < math::EPSILON);
	BOOST_REQUIRE ((math::vec<3>(0, 1, 0) - c.get_up_dir()).length_sq() < math::EPSILON);
	BOOST_REQUIRE ((math::vec<3>(1, 0, 0) - c.get_cross_dir()).length_sq() < math::EPSILON);
}

BOOST_AUTO_TEST_SUITE_END()


#include "TextureAnalysis.hpp"

#include <math.h>

#include <Eigen/Eigenvalues>
#include <algorithm>
#include <ctime>
#include <filesystem>
#include <fstream>
#include <iomanip>

const int white_color = 255;
const int black_color = 0;

using namespace glcm;

namespace fs = std::filesystem;

TextureAnalysis::TextureAnalysis(int Ng) : _Ng(Ng) {
    if (Ng > 0) {
        // initialize probability matrices
        _P_H.resize(_Ng, std::vector<int>(_Ng));
        _P_V.resize(_Ng, std::vector<int>(_Ng));
        _P_LD.resize(_Ng, std::vector<int>(_Ng));
        _P_RD.resize(_Ng, std::vector<int>(_Ng));

        _p_H.resize(_Ng, std::vector<double>(_Ng));
        _p_V.resize(_Ng, std::vector<double>(_Ng));
        _p_LD.resize(_Ng, std::vector<double>(_Ng));
        _p_RD.resize(_Ng, std::vector<double>(_Ng));

        // initialize probability vectors
        _px_H.resize(_Ng);
        _px_V.resize(_Ng);
        _px_LD.resize(_Ng);
        _px_RD.resize(_Ng);

        _py_H.resize(_Ng);
        _py_V.resize(_Ng);
        _py_LD.resize(_Ng);
        _py_RD.resize(_Ng);

        _p_xpy_H.resize(2 * _Ng - 1);
        _p_xpy_V.resize(2 * _Ng - 1);
        _p_xpy_LD.resize(2 * _Ng - 1);
        _p_xpy_RD.resize(2 * _Ng - 1);

        _p_xny_H.resize(_Ng);
        _p_xny_V.resize(_Ng);
        _p_xny_LD.resize(_Ng);
        _p_xny_RD.resize(_Ng);

        // reset factors as zeros
        ResetFactors();
    } else {
        std::cerr << "Invalid Ng assignment (Ng < 0)!\n";
    }
}

void TextureAnalysis::ProcessRectImage(const cv::Mat &image, int distance) {
    // Clear the cache
    ResetCache();

    // Calculate matrices elements: central pixel coord (m ,n), where "m" is the row index, and "n" is the column index
    for (int m = 0; m < image.rows; ++m) {
        for (int n = 0; n < image.cols; ++n) {
            // Nearest neighborhood pixel coord (k ,l), where "k" is the row index, and "l" is the column index
            for (int k = m - distance; k <= m + distance; ++k) {
                for (int l = n - distance; l <= n + distance; ++l) {
                    if ((k >= 0) && (l >= 0) && (k < image.rows) && (l < image.cols)) {
                        //// ToDo: need to check are pixel values in coordinates (m,n) and (k,l) nan or masked!!
                        int j = (int) (image.at<uchar>(m, n)); // I(m,n)
                        int i = (int) (image.at<uchar>(k, l)); // I(k,l)
                        if (((k - m) == 0) && (abs(l - n) == distance)) {
                            CountElemH(i, j);
                        } else if ((((k - m) == distance) && ((l - n) == -distance)) ||
                                   (((k - m) == -distance) && ((l - n) == distance))) {
                            CountElemRD(i, j);
                        } else if ((abs(k - m) == distance) && (l - n == 0)) {
                            CountElemV(i, j);
                        } else if ((((k - m) == distance) && ((l - n) == distance)) ||
                                   (((k - m) == -distance) && ((l - n) == -distance))) {
                            CountElemLD(i, j);
                        } else if ((m == k) && (n == l)) {
                            PushPixelValue(i);
                        } else {
                            // if ((m != k) || (n != l)) {
                            //    cerr << "unknown element:" << endl;
                            //    cerr << "central element: (m, n) = (" << m << "," << n << ")" << endl;
                            //    cerr << "neighborhood element: (k, l) = (" << k << "," << l << ")" << endl;
                            //}
                        }
                    }
                }
            }
        }
    }

    // Normalize the matrices
    Normalization();
}

void TextureAnalysis::ProcessPolygonImage(const cv::Mat &original_image, const cv::Mat &mask_image, int distance) {
    // Clear the cache
    ResetCache();

    // Calculate matrices elements: central pixel coord (m ,n), where "m" is the row index, and "n" is the column index
    int masked = 0;
    int non_masked = 0;
    for (int m = 0; m < original_image.rows; ++m) {
        for (int n = 0; n < original_image.cols; ++n) {
            // Nearest neighborhood pixel coord (k ,l), where "k" is the row index, and "l" is the column index
            for (int k = m - distance; k <= m + distance; ++k) {
                for (int l = n - distance; l <= n + distance; ++l) {
                    if ((k >= 0) && (l >= 0) && (k < original_image.rows) && (l < original_image.cols)) {
                        // Check is the nearest neighborhood pixel coord (k ,l) masked
                        int mask_pixel_value = (int) (mask_image.at<uchar>(k, l));
                        if (mask_pixel_value ==
                            white_color) {             // if nearest neighborhood pixel coord (k ,l) is not masked
                            int j = (int) (original_image.at<uchar>(m, n)); // I(m,n)
                            int i = (int) (original_image.at<uchar>(k, l)); // I(k,l)
                            if (((k - m) == 0) && (abs(l - n) == distance)) {
                                CountElemH(i, j);
                            } else if ((((k - m) == distance) && ((l - n) == -distance)) ||
                                       (((k - m) == -distance) && ((l - n) == distance))) {
                                CountElemRD(i, j);
                            } else if ((abs(k - m) == distance) && (l - n == 0)) {
                                CountElemV(i, j);
                            } else if ((((k - m) == distance) && ((l - n) == distance)) ||
                                       (((k - m) == -distance) && ((l - n) == -distance))) {
                                CountElemLD(i, j);
                            } else if ((m == k) && (n == l)) {
                                PushPixelValue(i);
                            } else {
                                // if ((m != k) || (n != l)) {
                                //    cerr << "unknown element:" << endl;
                                //    cerr << "central element: (m, n) = (" << m << "," << n << ")" << endl;
                                //    cerr << "neighborhood element: (k, l) = (" << k << "," << l << ")" << endl;
                                //}
                            }
                            ++non_masked;
                        } else {
                            // cerr << "masked coord: (k, l) = (" << k << "," << l << "), pixel value = " << mask_pixel_value << endl;
                            ++masked;
                        }
                    }
                }
            }
        }
    }

    // Normalize the matrices
    Normalization();
}

void TextureAnalysis::ResetCache() {
    // reset probability matrices as zeros
    for (int i = 0; i < _Ng; ++i) {
        std::fill(_P_H[i].begin(), _P_H[i].end(), 0);
        std::fill(_P_V[i].begin(), _P_V[i].end(), 0);
        std::fill(_P_LD[i].begin(), _P_LD[i].end(), 0);
        std::fill(_P_RD[i].begin(), _P_RD[i].end(), 0);

        std::fill(_p_H[i].begin(), _p_H[i].end(), 0);
        std::fill(_p_V[i].begin(), _p_V[i].end(), 0);
        std::fill(_p_LD[i].begin(), _p_LD[i].end(), 0);
        std::fill(_p_RD[i].begin(), _p_RD[i].end(), 0);
    }

    _pixel_values.clear();
    _pixel_values_mean = std::numeric_limits<double>::quiet_NaN();
    _pixel_values_STD = std::numeric_limits<double>::quiet_NaN();

    // reset probability vectors as zeros
    std::fill(_px_H.begin(), _px_H.end(), 0);
    std::fill(_px_V.begin(), _px_V.end(), 0);
    std::fill(_px_LD.begin(), _px_LD.end(), 0);
    std::fill(_px_RD.begin(), _px_RD.end(), 0);

    std::fill(_py_H.begin(), _py_H.end(), 0);
    std::fill(_py_V.begin(), _py_V.end(), 0);
    std::fill(_py_LD.begin(), _py_LD.end(), 0);
    std::fill(_py_RD.begin(), _py_RD.end(), 0);

    std::fill(_p_xpy_H.begin(), _p_xpy_H.end(), 0);
    std::fill(_p_xpy_V.begin(), _p_xpy_V.end(), 0);
    std::fill(_p_xpy_LD.begin(), _p_xpy_LD.end(), 0);
    std::fill(_p_xpy_RD.begin(), _p_xpy_RD.end(), 0);

    std::fill(_p_xny_H.begin(), _p_xny_H.end(), 0);
    std::fill(_p_xny_V.begin(), _p_xny_V.end(), 0);
    std::fill(_p_xny_LD.begin(), _p_xny_LD.end(), 0);
    std::fill(_p_xny_RD.begin(), _p_xny_RD.end(), 0);

    // reset factors as zeros
    ResetFactors();
}

void TextureAnalysis::ResetFactors() {
    // reset normalization factors as zeros
    _R_H = 0;
    _R_V = 0;
    _R_LD = 0;
    _R_RD = 0;

    // initialize entropy factors
    _HX_H = 0;
    _HX_V = 0;
    _HX_LD = 0;
    _HX_RD = 0;

    _HY_H = 0;
    _HY_V = 0;
    _HY_LD = 0;
    _HY_RD = 0;

    _HXY_H = 0;
    _HXY_V = 0;
    _HXY_LD = 0;
    _HXY_RD = 0;

    _HXY1_H = 0;
    _HXY1_V = 0;
    _HXY1_LD = 0;
    _HXY1_RD = 0;

    _HXY2_H = 0;
    _HXY2_V = 0;
    _HXY2_LD = 0;
    _HXY2_RD = 0;
}

void TextureAnalysis::CountElemH(int i, int j) {
    ++_P_H[i][j];
    ++_R_H;
}

void TextureAnalysis::CountElemV(int i, int j) {
    ++_P_V[i][j];
    ++_R_V;
}

void TextureAnalysis::CountElemLD(int i, int j) {
    ++_P_LD[i][j];
    ++_R_LD;
}

void TextureAnalysis::CountElemRD(int i, int j) {
    ++_P_RD[i][j];
    ++_R_RD;
}

void TextureAnalysis::PushPixelValue(int pixel_value) {
    _pixel_values.push_back((double) pixel_value);
}

void TextureAnalysis::Normalization() {
    for (int i = 0; i < _Ng; ++i) {
        for (int j = 0; j < _Ng; ++j) {
            _p_H[i][j] = (double) _P_H[i][j] / (double) _R_H;
            _p_V[i][j] = (double) _P_V[i][j] / (double) _R_V;
            _p_LD[i][j] = (double) _P_LD[i][j] / (double) _R_LD;
            _p_RD[i][j] = (double) _P_RD[i][j] / (double) _R_RD;
        }
    }

    // calculate probability vectors
    Calculate_px();
    Calculate_py();
    Calculate_p_xpy();
    Calculate_p_xny();

    // calculate pixels mean and STD in the region
    CalculatePixelSTD(_pixel_values);
}

void TextureAnalysis::Calculate_px() {
    for (int i = 0; i < _Ng; ++i) {
        for (int j = 0; j < _Ng; ++j) {
            _px_H[i] += _p_H[i][j];
            _px_V[i] += _p_V[i][j];
            _px_LD[i] += _p_LD[i][j];
            _px_RD[i] += _p_RD[i][j];
        }
    }
}

void TextureAnalysis::Calculate_py() {
    for (int j = 0; j < _Ng; ++j) {
        for (int i = 0; i < _Ng; ++i) {
            _py_H[j] += _p_H[i][j];
            _py_V[j] += _p_V[i][j];
            _py_LD[j] += _p_LD[i][j];
            _py_RD[j] += _p_RD[i][j];
        }
    }
}

void TextureAnalysis::Calculate_p_xpy() {
    for (int i = 0; i < _Ng; ++i) {
        for (int j = 0; j < _Ng; ++j) {
            int k = i + j;
            _p_xpy_H[k] += _p_H[i][j];
            _p_xpy_V[k] += _p_V[i][j];
            _p_xpy_LD[k] += _p_LD[i][j];
            _p_xpy_RD[k] += _p_RD[i][j];
        }
    }
}

void TextureAnalysis::Calculate_p_xny() {
    for (int i = 0; i < _Ng; ++i) {
        for (int j = 0; j < _Ng; ++j) {
            int k = abs(i - j);
            _p_xny_H[k] += _p_H[i][j];
            _p_xny_V[k] += _p_V[i][j];
            _p_xny_LD[k] += _p_LD[i][j];
            _p_xny_RD[k] += _p_RD[i][j];
        }
    }
}

double TextureAnalysis::CalculateMean(const std::vector<double> &vec) {
    double sum = 0.0;
    for (int i = 0; i < vec.size(); ++i) {
        sum += i * vec[i];
    }
    return sum;
}

double TextureAnalysis::CalculateSTD(const std::vector<double> &vec) {
    double mean = CalculateMean(vec);
    double sum = 0.0;
    for (int i = 0; i < vec.size(); ++i) {
        sum += (i - mean) * (i - mean) * vec[i];
    }
    return sqrt(sum);
}

double TextureAnalysis::CalculateGLCMMean_i(const std::vector<std::vector<double>> &mat) {
    double mean = 0.0;
    for (int i = 0; i < _Ng; ++i) {
        for (int j = 0; j < _Ng; ++j) {
            mean += i * mat[i][j];
        }
    }
    return mean;
}

double TextureAnalysis::CalculateGLCMMean_j(const std::vector<std::vector<double>> &mat) {
    double mean = 0.0;
    for (int i = 0; i < _Ng; ++i) {
        for (int j = 0; j < _Ng; ++j) {
            mean += j * mat[i][j];
        }
    }
    return mean;
}

double TextureAnalysis::CalculateGLCMSTD_i(const std::vector<std::vector<double>> &mat) {
    double mu_x = CalculateGLCMMean_i(mat);
    double sigma_x = 0.0;

    for (int i = 0; i < _Ng; ++i) {
        for (int j = 0; j < _Ng; ++j) {
            sigma_x += (i - mu_x) * (i - mu_x) * mat[i][j];
        }
    }

    return sqrt(sigma_x);
}

double TextureAnalysis::CalculateGLCMSTD_j(const std::vector<std::vector<double>> &mat) {
    double mu_y = CalculateGLCMMean_j(mat);
    double sigma_y = 0.0;

    for (int i = 0; i < _Ng; ++i) {
        for (int j = 0; j < _Ng; ++j) {
            sigma_y += (j - mu_y) * (j - mu_y) * mat[i][j];
        }
    }

    return sqrt(sigma_y);
}

void TextureAnalysis::CalculateHX() {
    for (int i = 0; i < _Ng; ++i) {
        if (_px_H[i] > 0) {
            _HX_H -= _px_H[i] * log(_px_H[i]);
        }
        if (_px_V[i] > 0) {
            _HX_V -= _px_V[i] * log(_px_V[i]);
        }
        if (_px_LD[i] > 0) {
            _HX_LD -= _px_LD[i] * log(_px_LD[i]);
        }
        if (_px_RD[i] > 0) {
            _HX_RD -= _px_RD[i] * log(_px_RD[i]);
        }
    }
}

void TextureAnalysis::CalculateHY() {
    for (int i = 0; i < _Ng; ++i) {
        if (_py_H[i] > 0) {
            _HY_H -= _py_H[i] * log(_py_H[i]);
        }
        if (_py_V[i] > 0) {
            _HY_V -= _py_V[i] * log(_py_V[i]);
        }
        if (_py_LD[i] > 0) {
            _HY_LD -= _py_LD[i] * log(_py_LD[i]);
        }
        if (_py_RD[i] > 0) {
            _HY_RD -= _py_RD[i] * log(_py_RD[i]);
        }
    }
}

void TextureAnalysis::CalculateHXY() {
    for (int i = 0; i < _Ng; ++i) {
        for (int j = 0; j < _Ng; ++j) {
            if (_p_H[i][j] > 0) {
                _HXY_H -= _p_H[i][j] * log(_p_H[i][j]);
            }
            if (_p_V[i][j] > 0) {
                _HXY_V -= _p_V[i][j] * log(_p_V[i][j]);
            }
            if (_p_LD[i][j] > 0) {
                _HXY_LD -= _p_LD[i][j] * log(_p_LD[i][j]);
            }
            if (_p_RD[i][j] > 0) {
                _HXY_RD -= _p_RD[i][j] * log(_p_RD[i][j]);
            }
        }
    }
}

void TextureAnalysis::CalculateHXY1() {
    for (int i = 0; i < _Ng; ++i) {
        for (int j = 0; j < _Ng; ++j) {
            if (_px_H[i] * _py_H[j] > 0) {
                _HXY1_H -= _p_H[i][j] * log(_px_H[i] * _py_H[j]);
            }
            if (_px_V[i] * _py_V[j] > 0) {
                _HXY1_V -= _p_V[i][j] * log(_px_V[i] * _py_V[j]);
            }
            if (_px_LD[i] * _py_LD[j] > 0) {
                _HXY1_LD -= _p_LD[i][j] * log(_px_LD[i] * _py_LD[j]);
            }
            if (_px_RD[i] * _py_RD[j] > 0) {
                _HXY1_RD -= _p_RD[i][j] * log(_px_RD[i] * _py_RD[j]);
            }
        }
    }
}

void TextureAnalysis::CalculateHXY2() {
    for (int i = 0; i < _Ng; ++i) {
        for (int j = 0; j < _Ng; ++j) {
            if (_px_H[i] * _py_H[j] > 0) {
                _HXY2_H -= _px_H[i] * _py_H[j] * log(_px_H[i] * _py_H[j]);
            }
            if (_px_V[i] * _py_V[j] > 0) {
                _HXY2_V -= _px_V[i] * _py_V[j] * log(_px_V[i] * _py_V[j]);
            }
            if (_px_LD[i] * _py_LD[j] > 0) {
                _HXY2_LD -= _px_LD[i] * _py_LD[j] * log(_px_LD[i] * _py_LD[j]);
            }
            if (_px_RD[i] * _py_RD[j] > 0) {
                _HXY2_RD -= _px_RD[i] * _py_RD[j] * log(_px_RD[i] * _py_RD[j]);
            }
        }
    }
}

Features TextureAnalysis::CalculateQ(int i, int j) {
    double Q_H = 0.0;
    double Q_V = 0.0;
    double Q_LD = 0.0;
    double Q_RD = 0.0;

    for (int k = 0; k < _Ng; ++k) {
        if ((_px_H[i] * _py_H[k]) != 0) {
            Q_H += (_p_H[i][k] * _p_H[j][k]) / (_px_H[i] * _py_H[k]);
        }
        if ((_px_V[i] * _py_V[k]) != 0) {
            Q_V += (_p_V[i][k] * _p_V[j][k]) / (_px_V[i] * _py_V[k]);
        }
        if ((_px_LD[i] * _py_LD[k]) != 0) {
            Q_LD += (_p_LD[i][k] * _p_LD[j][k]) / (_px_LD[i] * _py_LD[k]);
        }
        if ((_px_RD[i] * _py_RD[k]) != 0) {
            Q_RD += (_p_RD[i][k] * _p_RD[j][k]) / (_px_RD[i] * _py_RD[k]);
        }
    }

    return {Q_H, Q_V, Q_LD, Q_RD};
}

//===============================================================================================================
// Calculate texture feature coefficients
//===============================================================================================================

void TextureAnalysis::GetEnergy(Features &f) {
    double f_H = 0.0;
    double f_V = 0.0;
    double f_LD = 0.0;
    double f_RD = 0.0;

    for (int i = 0; i < _Ng; ++i) {
        for (int j = 0; j < _Ng; ++j) {
            f_H += _p_H[i][j] * _p_H[i][j];
            f_V += _p_V[i][j] * _p_V[i][j];
            f_LD += _p_LD[i][j] * _p_LD[i][j];
            f_RD += _p_RD[i][j] * _p_RD[i][j];
        }
    }

    f(f_H, f_V, f_LD, f_RD);
}

void TextureAnalysis::GetContrast(Features &f) {
    std::vector<double> sub_H(_Ng);
    std::vector<double> sub_V(_Ng);
    std::vector<double> sub_LD(_Ng);
    std::vector<double> sub_RD(_Ng);

    for (int i = 0; i < _Ng; ++i) {
        for (int j = 0; j < _Ng; ++j) {
            int n = abs(i - j);
            sub_H[n] += _p_H[i][j];
            sub_V[n] += _p_V[i][j];
            sub_LD[n] += _p_LD[i][j];
            sub_RD[n] += _p_RD[i][j];
        }
    }

    double f_H = 0.0;
    double f_V = 0.0;
    double f_LD = 0.0;
    double f_RD = 0.0;

    for (int n = 0; n < _Ng; ++n) {
        f_H += (n * n) * sub_H[n];
        f_V += (n * n) * sub_V[n];
        f_LD += (n * n) * sub_LD[n];
        f_RD += (n * n) * sub_RD[n];
    }

    f(f_H, f_V, f_LD, f_RD);
}

void TextureAnalysis::GetContrastAnotherWay(Features &f) {
    double f_H = 0.0;
    double f_V = 0.0;
    double f_LD = 0.0;
    double f_RD = 0.0;

    for (int i = 0; i < _Ng; ++i) {
        for (int j = 0; j < _Ng; ++j) {
            f_H += (i - j) * (i - j) * _p_H[i][j];
            f_V += (i - j) * (i - j) * _p_V[i][j];
            f_LD += (i - j) * (i - j) * _p_LD[i][j];
            f_RD += (i - j) * (i - j) * _p_RD[i][j];
        }
    }

    f(f_H, f_V, f_LD, f_RD);
}

void TextureAnalysis::GetCorrelationI(Features &f) {
    // Calculate means
    double mu_x_H = CalculateMean(_px_H);
    double mu_x_V = CalculateMean(_px_V);
    double mu_x_LD = CalculateMean(_px_LD);
    double mu_x_RD = CalculateMean(_px_RD);

    double mu_y_H = CalculateMean(_py_H);
    double mu_y_V = CalculateMean(_py_V);
    double mu_y_LD = CalculateMean(_py_LD);
    double mu_y_RD = CalculateMean(_py_RD);

    // Calculate STDs
    double sigma_x_H = CalculateSTD(_px_H);
    double sigma_x_V = CalculateSTD(_px_V);
    double sigma_x_LD = CalculateSTD(_px_LD);
    double sigma_x_RD = CalculateSTD(_px_RD);

    double sigma_y_H = CalculateSTD(_py_H);
    double sigma_y_V = CalculateSTD(_py_V);
    double sigma_y_LD = CalculateSTD(_py_LD);
    double sigma_y_RD = CalculateSTD(_py_RD);

    double f_H = 0.0;
    double f_V = 0.0;
    double f_LD = 0.0;
    double f_RD = 0.0;

    for (int i = 0; i < _Ng; ++i) {
        for (int j = 0; j < _Ng; ++j) {
            f_H += (i - mu_x_H) * (j - mu_y_H) * _p_H[i][j] / (sigma_x_H * sigma_y_H);
            f_V += (i - mu_x_V) * (j - mu_y_V) * _p_V[i][j] / (sigma_x_V * sigma_y_V);
            f_LD += (i - mu_x_LD) * (j - mu_y_LD) * _p_LD[i][j] / (sigma_x_LD * sigma_y_LD);
            f_RD += (i - mu_x_RD) * (j - mu_y_RD) * _p_RD[i][j] / (sigma_x_RD * sigma_y_RD);
        }
    }

    f(f_H, f_V, f_LD, f_RD);
}

void TextureAnalysis::GetCorrelationIAnotherWay(Features &f) {
    // Calculate means
    double mu_x_H = CalculateGLCMMean_i(_p_H);
    double mu_x_V = CalculateGLCMMean_i(_p_V);
    double mu_x_LD = CalculateGLCMMean_i(_p_LD);
    double mu_x_RD = CalculateGLCMMean_i(_p_RD);

    double mu_y_H = CalculateGLCMMean_j(_p_H);
    double mu_y_V = CalculateGLCMMean_j(_p_V);
    double mu_y_LD = CalculateGLCMMean_j(_p_LD);
    double mu_y_RD = CalculateGLCMMean_j(_p_RD);

    // Calculate STDs
    double sigma_x_H = CalculateGLCMSTD_i(_p_H);
    double sigma_x_V = CalculateGLCMSTD_i(_p_V);
    double sigma_x_LD = CalculateGLCMSTD_i(_p_LD);
    double sigma_x_RD = CalculateGLCMSTD_i(_p_RD);

    double sigma_y_H = CalculateGLCMSTD_j(_p_H);
    double sigma_y_V = CalculateGLCMSTD_j(_p_V);
    double sigma_y_LD = CalculateGLCMSTD_j(_p_LD);
    double sigma_y_RD = CalculateGLCMSTD_j(_p_RD);

    double f_H = 0.0;
    double f_V = 0.0;
    double f_LD = 0.0;
    double f_RD = 0.0;

    for (int i = 0; i < _Ng; ++i) {
        for (int j = 0; j < _Ng; ++j) {
            f_H += (i - mu_x_H) * (j - mu_y_H) * _p_H[i][j] / (sigma_x_H * sigma_y_H);
            f_V += (i - mu_x_V) * (j - mu_y_V) * _p_V[i][j] / (sigma_x_V * sigma_y_V);
            f_LD += (i - mu_x_LD) * (j - mu_y_LD) * _p_LD[i][j] / (sigma_x_LD * sigma_y_LD);
            f_RD += (i - mu_x_RD) * (j - mu_y_RD) * _p_RD[i][j] / (sigma_x_RD * sigma_y_RD);
        }
    }

    f(f_H, f_V, f_LD, f_RD);
}

void TextureAnalysis::GetCorrelationII(Features &f) {
    // Calculate means
    double mu_x_H = CalculateMean(_px_H);
    double mu_x_V = CalculateMean(_px_V);
    double mu_x_LD = CalculateMean(_px_LD);
    double mu_x_RD = CalculateMean(_px_RD);

    double mu_y_H = CalculateMean(_py_H);
    double mu_y_V = CalculateMean(_py_V);
    double mu_y_LD = CalculateMean(_py_LD);
    double mu_y_RD = CalculateMean(_py_RD);

    // Calculate STDs
    double sigma_x_H = CalculateSTD(_px_H);
    double sigma_x_V = CalculateSTD(_px_V);
    double sigma_x_LD = CalculateSTD(_px_LD);
    double sigma_x_RD = CalculateSTD(_px_RD);

    double sigma_y_H = CalculateSTD(_py_H);
    double sigma_y_V = CalculateSTD(_py_V);
    double sigma_y_LD = CalculateSTD(_py_LD);
    double sigma_y_RD = CalculateSTD(_py_RD);

    double f_H = 0.0;
    double f_V = 0.0;
    double f_LD = 0.0;
    double f_RD = 0.0;

    for (int i = 0; i < _Ng; ++i) {
        for (int j = 0; j < _Ng; ++j) {
            f_H += (i * j) * _p_H[i][j];
            f_V += (i * j) * _p_V[i][j];
            f_LD += (i * j) * _p_LD[i][j];
            f_RD += (i * j) * _p_RD[i][j];
        }
    }

    f_H = (f_H - (mu_x_H * mu_y_H)) / (sigma_x_H * sigma_y_H);
    f_V = (f_V - (mu_x_V * mu_y_V)) / (sigma_x_V * sigma_y_V);
    f_LD = (f_LD - (mu_x_LD * mu_y_LD)) / (sigma_x_LD * sigma_y_LD);
    f_RD = (f_RD - (mu_x_RD * mu_y_RD)) / (sigma_x_RD * sigma_y_RD);

    f(f_H, f_V, f_LD, f_RD);
}

void TextureAnalysis::GetCorrelationIIAnotherWay(Features &f) {
    // Calculate means
    double mu_x_H = CalculateGLCMMean_i(_p_H);
    double mu_x_V = CalculateGLCMMean_i(_p_V);
    double mu_x_LD = CalculateGLCMMean_i(_p_LD);
    double mu_x_RD = CalculateGLCMMean_i(_p_RD);

    double mu_y_H = CalculateGLCMMean_j(_p_H);
    double mu_y_V = CalculateGLCMMean_j(_p_V);
    double mu_y_LD = CalculateGLCMMean_j(_p_LD);
    double mu_y_RD = CalculateGLCMMean_j(_p_RD);

    // Calculate STDs
    double sigma_x_H = CalculateGLCMSTD_i(_p_H);
    double sigma_x_V = CalculateGLCMSTD_i(_p_V);
    double sigma_x_LD = CalculateGLCMSTD_i(_p_LD);
    double sigma_x_RD = CalculateGLCMSTD_i(_p_RD);

    double sigma_y_H = CalculateGLCMSTD_j(_p_H);
    double sigma_y_V = CalculateGLCMSTD_j(_p_V);
    double sigma_y_LD = CalculateGLCMSTD_j(_p_LD);
    double sigma_y_RD = CalculateGLCMSTD_j(_p_RD);

    double f_H = 0.0;
    double f_V = 0.0;
    double f_LD = 0.0;
    double f_RD = 0.0;

    for (int i = 0; i < _Ng; ++i) {
        for (int j = 0; j < _Ng; ++j) {
            f_H += (i * j) * _p_H[i][j];
            f_V += (i * j) * _p_V[i][j];
            f_LD += (i * j) * _p_LD[i][j];
            f_RD += (i * j) * _p_RD[i][j];
        }
    }

    f_H = (f_H - (mu_x_H * mu_y_H)) / (sigma_x_H * sigma_y_H);
    f_V = (f_V - (mu_x_V * mu_y_V)) / (sigma_x_V * sigma_y_V);
    f_LD = (f_LD - (mu_x_LD * mu_y_LD)) / (sigma_x_LD * sigma_y_LD);
    f_RD = (f_RD - (mu_x_RD * mu_y_RD)) / (sigma_x_RD * sigma_y_RD);

    f(f_H, f_V, f_LD, f_RD);
}

void TextureAnalysis::GetCorrelationIII(Features &f) {
    // Calculate means
    double mu_x_H = CalculateMean(_px_H);
    double mu_x_V = CalculateMean(_px_V);
    double mu_x_LD = CalculateMean(_px_LD);
    double mu_x_RD = CalculateMean(_px_RD);

    double mu_y_H = CalculateMean(_py_H);
    double mu_y_V = CalculateMean(_py_V);
    double mu_y_LD = CalculateMean(_py_LD);
    double mu_y_RD = CalculateMean(_py_RD);

    // Calculate STDs
    double sigma_x_H = CalculateSTD(_px_H);
    double sigma_x_V = CalculateSTD(_px_V);
    double sigma_x_LD = CalculateSTD(_px_LD);
    double sigma_x_RD = CalculateSTD(_px_RD);

    double sigma_y_H = CalculateSTD(_py_H);
    double sigma_y_V = CalculateSTD(_py_V);
    double sigma_y_LD = CalculateSTD(_py_LD);
    double sigma_y_RD = CalculateSTD(_py_RD);

    double f_H = 0.0;
    double f_V = 0.0;
    double f_LD = 0.0;
    double f_RD = 0.0;

    for (int i = 0; i < _Ng; ++i) {
        for (int j = 0; j < _Ng; ++j) {
            f_H += (i * j) * _p_H[i][j];
            f_V += (i * j) * _p_V[i][j];
            f_LD += (i * j) * _p_LD[i][j];
            f_RD += (i * j) * _p_RD[i][j];
        }
    }

    f_H = (f_H - (mu_x_H * mu_y_H)) / (sigma_x_H * sigma_y_H * sigma_x_H * sigma_y_H);
    f_V = (f_V - (mu_x_V * mu_y_V)) / (sigma_x_V * sigma_y_V * sigma_x_V * sigma_y_V);
    f_LD = (f_LD - (mu_x_LD * mu_y_LD)) / (sigma_x_LD * sigma_y_LD * sigma_x_LD * sigma_y_LD);
    f_RD = (f_RD - (mu_x_RD * mu_y_RD)) / (sigma_x_RD * sigma_y_RD * sigma_x_RD * sigma_y_RD);

    f(f_H, f_V, f_LD, f_RD);
}

void TextureAnalysis::GetSumOfSquares(Features &f) {
    double mean_H_x = CalculateGLCMMean_i(_p_H);
    double mean_V_x = CalculateGLCMMean_i(_p_V);
    double mean_LD_x = CalculateGLCMMean_i(_p_LD);
    double mean_RD_x = CalculateGLCMMean_i(_p_RD);

    double mean_H_y = CalculateGLCMMean_j(_p_H);
    double mean_V_y = CalculateGLCMMean_j(_p_V);
    double mean_LD_y = CalculateGLCMMean_j(_p_LD);
    double mean_RD_y = CalculateGLCMMean_j(_p_RD);

    double f_H = 0.0;
    double f_V = 0.0;
    double f_LD = 0.0;
    double f_RD = 0.0;

    for (int i = 0; i < _Ng; ++i) {
        for (int j = 0; j < _Ng; ++j) {
            f_H += (i - mean_H_x) * (i - mean_H_x) * _p_H[i][j] + (j - mean_H_y) * (j - mean_H_y) * _p_H[i][j];
            f_V += (i - mean_V_x) * (i - mean_V_x) * _p_V[i][j] + (j - mean_V_y) * (j - mean_V_y) * _p_V[i][j];
            f_LD += (i - mean_LD_x) * (i - mean_LD_x) * _p_LD[i][j] + (j - mean_LD_y) * (j - mean_LD_y) * _p_LD[i][j];
            f_RD += (i - mean_RD_x) * (i - mean_RD_x) * _p_RD[i][j] + (j - mean_RD_y) * (j - mean_RD_y) * _p_RD[i][j];
        }
    }

    f(f_H, f_V, f_LD, f_RD);
}

void TextureAnalysis::GetSumOfSquares_i(Features &f) {
    double mean_H = CalculateGLCMMean_i(_p_H);
    double mean_V = CalculateGLCMMean_i(_p_V);
    double mean_LD = CalculateGLCMMean_i(_p_LD);
    double mean_RD = CalculateGLCMMean_i(_p_RD);

    double f_H = 0.0;
    double f_V = 0.0;
    double f_LD = 0.0;
    double f_RD = 0.0;

    for (int i = 0; i < _Ng; ++i) {
        for (int j = 0; j < _Ng; ++j) {
            f_H += (i - mean_H) * (i - mean_H) * _p_H[i][j];
            f_V += (i - mean_V) * (i - mean_V) * _p_V[i][j];
            f_LD += (i - mean_LD) * (i - mean_LD) * _p_LD[i][j];
            f_RD += (i - mean_RD) * (i - mean_RD) * _p_RD[i][j];
        }
    }

    f(f_H, f_V, f_LD, f_RD);
}

void TextureAnalysis::GetSumOfSquares_j(Features &f) {
    double mean_H = CalculateGLCMMean_j(_p_H);
    double mean_V = CalculateGLCMMean_j(_p_V);
    double mean_LD = CalculateGLCMMean_j(_p_LD);
    double mean_RD = CalculateGLCMMean_j(_p_RD);

    double f_H = 0.0;
    double f_V = 0.0;
    double f_LD = 0.0;
    double f_RD = 0.0;

    for (int i = 0; i < _Ng; ++i) {
        for (int j = 0; j < _Ng; ++j) {
            f_H += (j - mean_H) * (j - mean_H) * _p_H[i][j];
            f_V += (j - mean_V) * (j - mean_V) * _p_V[i][j];
            f_LD += (j - mean_LD) * (j - mean_LD) * _p_LD[i][j];
            f_RD += (j - mean_RD) * (j - mean_RD) * _p_RD[i][j];
        }
    }

    f(f_H, f_V, f_LD, f_RD);
}

void TextureAnalysis::GetHomogeneityII(Features &f) {
    double f_H = 0.0;
    double f_V = 0.0;
    double f_LD = 0.0;
    double f_RD = 0.0;

    for (int i = 0; i < _Ng; ++i) {
        for (int j = 0; j < _Ng; ++j) {
            f_H += _p_H[i][j] / (1 + (i - j) * (i - j));
            f_V += _p_V[i][j] / (1 + (i - j) * (i - j));
            f_LD += _p_LD[i][j] / (1 + (i - j) * (i - j));
            f_RD += _p_RD[i][j] / (1 + (i - j) * (i - j));
        }
    }

    f(f_H, f_V, f_LD, f_RD);
}

void TextureAnalysis::GetSumAverage(Features &f) {
    double f_H = 0.0;
    double f_V = 0.0;
    double f_LD = 0.0;
    double f_RD = 0.0;

    for (int i = 0; i < (2 * _Ng - 1); ++i) {
        f_H += i * _p_xpy_H[i];
        f_V += i * _p_xpy_V[i];
        f_LD += i * _p_xpy_LD[i];
        f_RD += i * _p_xpy_RD[i];
    }

    f(f_H, f_V, f_LD, f_RD);
}

void TextureAnalysis::GetSumVariance(Features &f) {
    Features f8;
    GetSumEntropy(f8);

    double f_H = 0.0;
    double f_V = 0.0;
    double f_LD = 0.0;
    double f_RD = 0.0;

    for (int i = 0; i < (2 * _Ng - 1); ++i) {
        f_H += (i - f8.H) * (i - f8.H) * _p_xpy_H[i];
        f_V += (i - f8.V) * (i - f8.V) * _p_xpy_V[i];
        f_LD += (i - f8.LD) * (i - f8.LD) * _p_xpy_LD[i];
        f_RD += (i - f8.RD) * (i - f8.RD) * _p_xpy_RD[i];
    }

    f(f_H, f_V, f_LD, f_RD);
}

void TextureAnalysis::GetSumEntropy(Features &f) {
    double f_H = 0.0;
    double f_V = 0.0;
    double f_LD = 0.0;
    double f_RD = 0.0;

    for (int i = 0; i < (2 * _Ng - 1); ++i) {
        if (_p_xpy_H[i] > 0) {
            f_H -= _p_xpy_H[i] * log(_p_xpy_H[i]);
        }
        if (_p_xpy_V[i] > 0) {
            f_V -= _p_xpy_V[i] * log(_p_xpy_V[i]);
        }
        if (_p_xpy_LD[i] > 0) {
            f_LD -= _p_xpy_LD[i] * log(_p_xpy_LD[i]);
        }
        if (_p_xpy_RD[i] > 0) {
            f_RD -= _p_xpy_RD[i] * log(_p_xpy_RD[i]);
        }
    }

    f(f_H, f_V, f_LD, f_RD);
}

void TextureAnalysis::GetEntropy(Features &f) {
    double f_H = 0.0;
    double f_V = 0.0;
    double f_LD = 0.0;
    double f_RD = 0.0;

    for (int i = 0; i < _Ng; ++i) {
        for (int j = 0; j < _Ng; ++j) {
            if (_p_H[i][j] > 0) {
                f_H -= _p_H[i][j] * log(_p_H[i][j]);
            }
            if (_p_V[i][j] > 0) {
                f_V -= _p_V[i][j] * log(_p_V[i][j]);
            }
            if (_p_LD[i][j] > 0) {
                f_LD -= _p_LD[i][j] * log(_p_LD[i][j]);
            }
            if (_p_RD[i][j] > 0) {
                f_RD -= _p_RD[i][j] * log(_p_RD[i][j]);
            }
        }
    }

    f(f_H, f_V, f_LD, f_RD);
}

void TextureAnalysis::GetDifferenceVariance(Features &f) {
    double f_H = 0.0;
    double f_V = 0.0;
    double f_LD = 0.0;
    double f_RD = 0.0;

    for (int i = 0; i < _Ng; ++i) {
        f_H += i * i * _p_xny_H[i];
        f_V += i * i * _p_xny_V[i];
        f_LD += i * i * _p_xny_LD[i];
        f_RD += i * i * _p_xny_RD[i];
    }

    f(f_H, f_V, f_LD, f_RD);
}

void TextureAnalysis::GetDifferenceEntropy(Features &f) {
    double f_H = 0.0;
    double f_V = 0.0;
    double f_LD = 0.0;
    double f_RD = 0.0;

    for (int i = 0; i < _Ng; ++i) {
        if (_p_xny_H[i] > 0) {
            f_H -= _p_xny_H[i] * log(_p_xny_H[i]);
        }
        if (_p_xny_V[i] > 0) {
            f_V -= _p_xny_V[i] * log(_p_xny_V[i]);
        }
        if (_p_xny_LD[i] > 0) {
            f_LD -= _p_xny_LD[i] * log(_p_xny_LD[i]);
        }
        if (_p_xny_RD[i] > 0) {
            f_RD -= _p_xny_RD[i] * log(_p_xny_RD[i]);
        }
    }

    f(f_H, f_V, f_LD, f_RD);
}

void TextureAnalysis::GetInformationMeasuresOfCorrelation(Features &f1, Features &f2) {
    // calculate entropy factors
    CalculateHX();
    CalculateHY();
    CalculateHXY();
    CalculateHXY1();
    CalculateHXY2();

    // calculate the first Information Measures of Correlation
    double f1_H;
    double f1_V;
    double f1_LD;
    double f1_RD;

    f1_H = (_HXY_H - _HXY1_H) / std::max(_HX_H, _HY_H);
    f1_V = (_HXY_V - _HXY1_V) / std::max(_HX_V, _HY_V);
    f1_LD = (_HXY_LD - _HXY1_LD) / std::max(_HX_LD, _HY_LD);
    f1_RD = (_HXY_RD - _HXY1_RD) / std::max(_HX_RD, _HY_RD);

    f1(f1_H, f1_V, f1_LD, f1_RD);

    // calculate the second Information Measures of Correlation
    double f2_H = sqrt(1.0 - exp(-2.0 * (_HXY2_H - _HXY_H)));
    double f2_V = sqrt(1.0 - exp(-2.0 * (_HXY2_V - _HXY_V)));
    double f2_LD = sqrt(1.0 - exp(-2.0 * (_HXY2_LD - _HXY_LD)));
    double f2_RD = sqrt(1.0 - exp(-2.0 * (_HXY2_RD - _HXY_RD)));

    f2(f2_H, f2_V, f2_LD, f2_RD);
}

void TextureAnalysis::GetMaximalCorrelationCoefficient(Features &f) {
    // fill in Q matrices
    Eigen::MatrixXd Q_H(_Ng, _Ng);
    Eigen::MatrixXd Q_V(_Ng, _Ng);
    Eigen::MatrixXd Q_LD(_Ng, _Ng);
    Eigen::MatrixXd Q_RD(_Ng, _Ng);

    for (int i = 0; i < _Ng; ++i) {
        for (int j = 0; j < _Ng; ++j) {
            Features q = CalculateQ(i, j);
            // std::cout << "q_H = " << q.H << ", q_V = " << q.V << ", q_LD = " << q.LD << ", q_RD = " << q.RD << std::endl;
            Q_H(i, j) = q.H;
            Q_V(i, j) = q.V;
            Q_LD(i, j) = q.LD;
            Q_RD(i, j) = q.RD;
        }
    }

    Eigen::EigenSolver<Eigen::MatrixXd> eigen_solver_Q_H(Q_H);
    Eigen::EigenSolver<Eigen::MatrixXd> eigen_solver_Q_V(Q_V);
    Eigen::EigenSolver<Eigen::MatrixXd> eigen_solver_Q_LD(Q_LD);
    Eigen::EigenSolver<Eigen::MatrixXd> eigen_solver_Q_RD(Q_RD);

    // get eigenvalues
    std::vector<double> eigens_H;
    std::vector<double> eigens_V;
    std::vector<double> eigens_LD;
    std::vector<double> eigens_RD;

    for (int i = 0; i < _Ng; ++i) {
        std::complex<double> E_H = eigen_solver_Q_H.eigenvalues().col(0)[i];
        std::complex<double> E_V = eigen_solver_Q_V.eigenvalues().col(0)[i];
        std::complex<double> E_LD = eigen_solver_Q_LD.eigenvalues().col(0)[i];
        std::complex<double> E_RD = eigen_solver_Q_RD.eigenvalues().col(0)[i];

        eigens_H.push_back(E_H.real());
        eigens_V.push_back(E_V.real());
        eigens_LD.push_back(E_LD.real());
        eigens_RD.push_back(E_RD.real());
    }

    // get second largest eigenvalues
    std::nth_element(eigens_H.begin(), eigens_H.begin() + 1, eigens_H.end(), std::greater<double>());
    std::nth_element(eigens_V.begin(), eigens_V.begin() + 1, eigens_V.end(), std::greater<double>());
    std::nth_element(eigens_LD.begin(), eigens_LD.begin() + 1, eigens_LD.end(), std::greater<double>());
    std::nth_element(eigens_RD.begin(), eigens_RD.begin() + 1, eigens_RD.end(), std::greater<double>());

    f(eigens_H[1], eigens_V[1], eigens_LD[1], eigens_RD[1]);
}

void TextureAnalysis::GetMean(Features &f) {
    double f_H = _pixel_values_mean;
    double f_V = _pixel_values_mean;
    double f_LD = _pixel_values_mean;
    double f_RD = _pixel_values_mean;
    f(f_H, f_V, f_LD, f_RD);
}

void TextureAnalysis::GetStd(Features &f) {
    double f_H = _pixel_values_STD;
    double f_V = _pixel_values_STD;
    double f_LD = _pixel_values_STD;
    double f_RD = _pixel_values_STD;
    f(f_H, f_V, f_LD, f_RD);
}

void TextureAnalysis::GetAutoCorrelation(Features &f) {
    double f_H = 0.0;
    double f_V = 0.0;
    double f_LD = 0.0;
    double f_RD = 0.0;

    for (int i = 0; i < _Ng; ++i) {
        for (int j = 0; j < _Ng; ++j) {
            f_H += i * j * _p_H[i][j];
            f_V += i * j * _p_V[i][j];
            f_LD += i * j * _p_LD[i][j];
            f_RD += i * j * _p_RD[i][j];
        }
    }

    f(f_H, f_V, f_LD, f_RD);
}

void TextureAnalysis::GetClusterProminence(Features &f) {
    // Calculate means
    double mu_x_H = CalculateMean(_px_H);
    double mu_x_V = CalculateMean(_px_V);
    double mu_x_LD = CalculateMean(_px_LD);
    double mu_x_RD = CalculateMean(_px_RD);

    double mu_y_H = CalculateMean(_py_H);
    double mu_y_V = CalculateMean(_py_V);
    double mu_y_LD = CalculateMean(_py_LD);
    double mu_y_RD = CalculateMean(_py_RD);

    double f_H = 0.0;
    double f_V = 0.0;
    double f_LD = 0.0;
    double f_RD = 0.0;

    for (int i = 0; i < _Ng; ++i) {
        for (int j = 0; j < _Ng; ++j) {
            f_H += pow((i + j - mu_x_H - mu_y_H), 4) * _p_H[i][j];
            f_V += pow((i + j - mu_x_V - mu_y_V), 4) * _p_V[i][j];
            f_LD += pow((i + j - mu_x_LD - mu_y_LD), 4) * _p_LD[i][j];
            f_RD += pow((i + j - mu_x_RD - mu_y_RD), 4) * _p_RD[i][j];
        }
    }

    f(f_H, f_V, f_LD, f_RD);
}

void TextureAnalysis::GetClusterShade(Features &f) {
    // Calculate means
    double mu_x_H = CalculateMean(_px_H);
    double mu_x_V = CalculateMean(_px_V);
    double mu_x_LD = CalculateMean(_px_LD);
    double mu_x_RD = CalculateMean(_px_RD);

    double mu_y_H = CalculateMean(_py_H);
    double mu_y_V = CalculateMean(_py_V);
    double mu_y_LD = CalculateMean(_py_LD);
    double mu_y_RD = CalculateMean(_py_RD);

    double f_H = 0.0;
    double f_V = 0.0;
    double f_LD = 0.0;
    double f_RD = 0.0;

    for (int i = 0; i < _Ng; ++i) {
        for (int j = 0; j < _Ng; ++j) {
            f_H += pow((i + j - mu_x_H - mu_y_H), 3) * _p_H[i][j];
            f_V += pow((i + j - mu_x_V - mu_y_V), 3) * _p_V[i][j];
            f_LD += pow((i + j - mu_x_LD - mu_y_LD), 3) * _p_LD[i][j];
            f_RD += pow((i + j - mu_x_RD - mu_y_RD), 3) * _p_RD[i][j];
        }
    }

    f(f_H, f_V, f_LD, f_RD);
}

void TextureAnalysis::GetDissimilarity(Features &f) {
    double f_H = 0.0;
    double f_V = 0.0;
    double f_LD = 0.0;
    double f_RD = 0.0;

    for (int i = 0; i < _Ng; ++i) {
        for (int j = 0; j < _Ng; ++j) {
            f_H += fabs(i - j) * _p_H[i][j];
            f_V += fabs(i - j) * _p_V[i][j];
            f_LD += fabs(i - j) * _p_LD[i][j];
            f_RD += fabs(i - j) * _p_RD[i][j];
        }
    }

    f(f_H, f_V, f_LD, f_RD);
}

void TextureAnalysis::GetHomogeneityI(Features &f) {
    double f_H = 0.0;
    double f_V = 0.0;
    double f_LD = 0.0;
    double f_RD = 0.0;

    for (int i = 0; i < _Ng; ++i) {
        for (int j = 0; j < _Ng; ++j) {
            f_H += _p_H[i][j] / (1 + fabs(i - j));
            f_V += _p_V[i][j] / (1 + fabs(i - j));
            f_LD += _p_LD[i][j] / (1 + fabs(i - j));
            f_RD += _p_RD[i][j] / (1 + fabs(i - j));
        }
    }

    f(f_H, f_V, f_LD, f_RD);
}

void TextureAnalysis::GetMaximumProbability(Features &f) {
    double f_H = 0.0;
    double f_V = 0.0;
    double f_LD = 0.0;
    double f_RD = 0.0;

    for (int i = 0; i < _Ng; ++i) {
        for (int j = 0; j < _Ng; ++j) {
            if (_p_H[i][j] > f_H) {
                f_H = _p_H[i][j];
            }
            if (_p_V[i][j] > f_V) {
                f_V = _p_V[i][j];
            }
            if (_p_LD[i][j] > f_LD) {
                f_LD = _p_LD[i][j];
            }
            if (_p_RD[i][j] > f_RD) {
                f_RD = _p_RD[i][j];
            }
        }
    }

    f(f_H, f_V, f_LD, f_RD);
}

void TextureAnalysis::GetInverseDifferenceNormalized(Features &f) {
    double f_H = 0.0;
    double f_V = 0.0;
    double f_LD = 0.0;
    double f_RD = 0.0;

    for (int i = 0; i < _Ng; ++i) {
        for (int j = 0; j < _Ng; ++j) {
            f_H += _p_H[i][j] / (1 + (abs(i - j) * abs(i - j) / _Ng));
            f_V += _p_V[i][j] / (1 + (abs(i - j) * abs(i - j) / _Ng));
            f_LD += _p_LD[i][j] / (1 + (abs(i - j) * abs(i - j) / _Ng));
            f_RD += _p_RD[i][j] / (1 + (abs(i - j) * abs(i - j) / _Ng));
        }
    }

    f(f_H, f_V, f_LD, f_RD);
}

void TextureAnalysis::GetInverseDifferenceMomentNormalized(Features &f) {
    double f_H = 0.0;
    double f_V = 0.0;
    double f_LD = 0.0;
    double f_RD = 0.0;

    for (int i = 0; i < _Ng; ++i) {
        for (int j = 0; j < _Ng; ++j) {
            f_H += _p_H[i][j] / (1 + ((i - j) * (i - j) / _Ng));
            f_V += _p_V[i][j] / (1 + ((i - j) * (i - j) / _Ng));
            f_LD += _p_LD[i][j] / (1 + ((i - j) * (i - j) / _Ng));
            f_RD += _p_RD[i][j] / (1 + ((i - j) * (i - j) / _Ng));
        }
    }

    f(f_H, f_V, f_LD, f_RD);
}

std::map<Type, Features> TextureAnalysis::Calculate(const std::set<Type> &types) {
    std::map<Type, Features> results;
    bool information_measures_of_correlation_done = false;
    for (auto type : types) {
        switch (type) {
            case Type::Mean:
                GetMean(results[Type::Mean]);
                break;
            case Type::Std:
                GetStd(results[Type::Std]);
                break;
            case Type::AutoCorrelation:
                GetAutoCorrelation(results[Type::AutoCorrelation]);
                break;
            case Type::Contrast:
                GetContrast(results[Type::Contrast]);
                break;
            case Type::ContrastAnotherWay:
                GetContrastAnotherWay(results[Type::ContrastAnotherWay]);
                break;
            case Type::CorrelationI:
                GetCorrelationI(results[Type::CorrelationI]);
                break;
            case Type::CorrelationIAnotherWay:
                GetCorrelationIAnotherWay(results[Type::CorrelationIAnotherWay]);
                break;
            case Type::CorrelationII:
                GetCorrelationII(results[Type::CorrelationII]);
                break;
            case Type::CorrelationIIAnotherWay:
                GetCorrelationIIAnotherWay(results[Type::CorrelationIIAnotherWay]);
                break;
            case Type::CorrelationIII:
                GetCorrelationIII(results[Type::CorrelationIII]);
                break;
            case Type::ClusterProminence:
                GetClusterProminence(results[Type::ClusterProminence]);
                break;
            case Type::ClusterShade:
                GetClusterShade(results[Type::ClusterShade]);
                break;
            case Type::Dissimilarity:
                GetDissimilarity(results[Type::Dissimilarity]);
                break;
            case Type::Energy:
                GetEnergy(results[Type::Energy]);
                break;
            case Type::Entropy:
                GetEntropy(results[Type::Entropy]);
                break;
            case Type::HomogeneityI:
                GetHomogeneityI(results[Type::HomogeneityI]);
                break;
            case Type::HomogeneityII:
                GetHomogeneityII(results[Type::HomogeneityII]);
                break;
            case Type::MaximumProbability:
                GetMaximumProbability(results[Type::MaximumProbability]);
                break;
            case Type::SumOfSquares:
                GetSumOfSquares(results[Type::SumOfSquares]);
                break;
            case Type::SumOfSquaresI:
                GetSumOfSquares_i(results[Type::SumOfSquaresI]);
                break;
            case Type::SumOfSquaresJ:
                GetSumOfSquares_j(results[Type::SumOfSquaresJ]);
                break;
            case Type::SumAverage:
                GetSumAverage(results[Type::SumAverage]);
                break;
            case Type::SumEntropy:
                GetSumEntropy(results[Type::SumEntropy]);
                break;
            case Type::SumVariance:
                GetSumVariance(results[Type::SumVariance]);
                break;
            case Type::DifferenceVariance:
                GetDifferenceVariance(results[Type::DifferenceVariance]);
                break;
            case Type::DifferenceEntropy:
                GetDifferenceEntropy(results[Type::DifferenceEntropy]);
                break;
            case Type::InformationMeasuresOfCorrelationI:
                if (!information_measures_of_correlation_done) {
                    GetInformationMeasuresOfCorrelation(
                            results[Type::InformationMeasuresOfCorrelationI],
                            results[Type::InformationMeasuresOfCorrelationII]);
                    information_measures_of_correlation_done = true;
                }
                break;
            case Type::InformationMeasuresOfCorrelationII:
                if (!information_measures_of_correlation_done) {
                    GetInformationMeasuresOfCorrelation(
                            results[Type::InformationMeasuresOfCorrelationI],
                            results[Type::InformationMeasuresOfCorrelationII]);
                    information_measures_of_correlation_done = true;
                }
                break;
            case Type::InverseDifferenceNormalized:
                GetInverseDifferenceNormalized(results[Type::InverseDifferenceNormalized]);
                break;
            case Type::InverseDifferenceMomentNormalized:
                GetInverseDifferenceMomentNormalized(results[Type::InverseDifferenceMomentNormalized]);
                break;
            default:
                std::cerr << "Unknown feature type!\n";
                break;
        }
    }

    return results;
}

void TextureAnalysis::CalculateScore(double age, std::map<Type, Features> &features_map) {
    if ((age > 0) && features_map.count(Type::Mean) && features_map.count(Type::Entropy) &&
        features_map.count(Type::Contrast)) {
        std::vector<double> params = {1.138, -1.814, 1.416, 1.714};

        if (age < 60) {
            age = 0;
        } else {
            age = 1;
        }

        double intensity_H = features_map.at(Type::Mean).H;
        double intensity_V = features_map.at(Type::Mean).V;
        double intensity_LD = features_map.at(Type::Mean).LD;
        double intensity_RD = features_map.at(Type::Mean).RD;
        double intensity = (intensity_H + intensity_V + intensity_LD + intensity_RD) / 4.0;

        if (intensity < 51.39) {
            intensity = 0;
        } else {
            intensity = 1;
        }

        double entropy_H = features_map.at(Type::Entropy).H;
        double entropy_V = features_map.at(Type::Entropy).V;
        double entropy_LD = features_map.at(Type::Entropy).LD;
        double entropy_RD = features_map.at(Type::Entropy).RD;
        double entropy = (entropy_H + entropy_V + entropy_LD + entropy_RD) / 4.0;

        if (entropy < 7.119) {
            entropy = 0;
        } else {
            entropy = 1;
        }

        double contrast_H = features_map.at(Type::Contrast).H;
        double contrast_V = features_map.at(Type::Contrast).V;
        double contrast_LD = features_map.at(Type::Contrast).LD;
        double contrast_RD = features_map.at(Type::Contrast).RD;
        double contrast = (contrast_H + contrast_V + contrast_LD + contrast_RD) / 4.0 / 10.0;

        if (contrast < 7.1491) {
            contrast = 0;
        } else {
            contrast = 1;
        }

        double f_H = params[0] * age + params[1] * intensity + params[2] * entropy +
                     params[3] * contrast;
        double f_V = params[0] * age + params[1] * intensity + params[2] * entropy +
                     params[3] * contrast;
        double f_LD = params[0] * age + params[1] * intensity + params[2] * entropy +
                      params[3] * contrast;
        double f_RD = params[0] * age + params[1] * intensity + params[2] * entropy +
                      params[3] * contrast;

        features_map[Type::Score](f_H, f_V, f_LD, f_RD);
        features_map[Type::Age](age, age, age, age);
    } else {
        std::cerr << "Can not calculate the Score!\n";
    }
}

std::string TextureAnalysis::TypeToString(const Type &type) {
    std::string result;
    switch (type) {
        case Type::Mean:
            result = "Mean";
            break;
        case Type::Std:
            result = "STD";
            break;
        case Type::AutoCorrelation:
            result = "Auto Correlation";
            break;
        case Type::Contrast:
            result = "Contrast";
            break;
        case Type::ContrastAnotherWay:
            result = "Contrast (Check)";
            break;
        case Type::CorrelationI:
            result = "Correlation I";
            break;
        case Type::CorrelationIAnotherWay:
            result = "Correlation I (Check)";
            break;
        case Type::CorrelationII:
            result = "Correlation II";
            break;
        case Type::CorrelationIIAnotherWay:
            result = "Correlation II (Check)";
            break;
        case Type::CorrelationIII:
            result = "Correlation III";
            break;
        case Type::ClusterProminence:
            result = "Cluster Prominence";
            break;
        case Type::ClusterShade:
            result = "Cluster Shade";
            break;
        case Type::Dissimilarity:
            result = "Dissimilarity";
            break;
        case Type::Energy:
            result = "Energy";
            break;
        case Type::Entropy:
            result = "Entropy";
            break;
        case Type::HomogeneityI:
            result = "Homogeneity I";
            break;
        case Type::HomogeneityII:
            result = "Homogeneity II (Inverse Difference Moment)";
            break;
        case Type::MaximumProbability:
            result = "Maximum Probability";
            break;
        case Type::SumOfSquares:
            result = "Sum of Squares (in x and y)";
            break;
        case Type::SumOfSquaresI:
            result = "Sum of Squares (in x)";
            break;
        case Type::SumOfSquaresJ:
            result = "Sum of Squares (in y)";
            break;
        case Type::SumAverage:
            result = "Sum Average";
            break;
        case Type::SumEntropy:
            result = "Sum Entropy";
            break;
        case Type::SumVariance:
            result = "Sum Variance";
            break;
        case Type::DifferenceVariance:
            result = "Difference Variance";
            break;
        case Type::DifferenceEntropy:
            result = "Difference Entropy";
            break;
        case Type::InformationMeasuresOfCorrelationI:
            result = "Information Measures of Correlation I";
            break;
        case Type::InformationMeasuresOfCorrelationII:
            result = "Information Measures of Correlation II";
            break;
        case Type::InverseDifferenceNormalized:
            result = "Inverse Difference Normalized";
            break;
        case Type::InverseDifferenceMomentNormalized:
            result = "Inverse Difference Moment Normalized";
            break;
        case Type::Score:
            result = "Score";
            break;
        case Type::Age:
            result = "Age";
            break;
        default:
            std::cerr << "Unknown feature type!\n";
            break;
    }
    return result;
}

std::string TextureAnalysis::DirectionToString(const Direction &direction) {
    std::string result;
    switch (direction) {
        case Direction::H:
            result = "H (0 deg)";
            break;
        case Direction::V:
            result = "V (90 deg)";
            break;
        case Direction::LD:
            result = "LD (135 deg)";
            break;
        case Direction::RD:
            result = "RD (45 deg)";
            break;
        case Direction::Avg:
            result = "Average";
            break;
        default:
            std::cerr << "Unknown feature type!\n";
            break;
    }

    return result;
}

void TextureAnalysis::Print(const std::map<Type, Features> &features) {
    for (auto feature : features) {
        std::cout << TypeToString(feature.first) << std::endl;
        std::cout << std::setw(30) << DirectionToString(Direction::H) << " = " << feature.second.H << std::endl;
        std::cout << std::setw(30) << DirectionToString(Direction::V) << " = " << feature.second.V << std::endl;
        std::cout << std::setw(30) << DirectionToString(Direction::LD) << " = " << feature.second.LD << std::endl;
        std::cout << std::setw(30) << DirectionToString(Direction::RD) << " = " << feature.second.RD << std::endl;
        std::cout << std::setw(30) << DirectionToString(Direction::Avg) << " = " << feature.second.Avg() << std::endl;
    }
}

void TextureAnalysis::SaveAsCSV(const std::string &image_name, std::map<Type, Features> features,
                                const std::string &csv_name) {
    // check whether the csv file exists or not
    bool csv_file_exists = fs::exists(csv_name);

    // get image file base name
    std::string image_base_name = fs::path(image_name).filename().string();

    // get current time
    std::string current_time = GetCurrentTime();

    // open the csv file
    std::ofstream csv_file;
    csv_file.open(csv_name,
                  std::ios::out | std::ios::app); // open as the writing mode and append the csv file at the end

    if (!csv_file) {
        std::cerr << "Can't the open file!" << std::endl;
        return;
    }

    if (!csv_file_exists) {
        // write a row of titles
        csv_file << "Date,";
        csv_file << "Image,";
        csv_file << "Direction,";
        for (std::map<Type, Features>::iterator it = features.begin(); it != features.end(); ++it) {
            csv_file << TypeToString(it->first) << ",";
        }
        csv_file << "\n";
    }

    // write a row of H values
    csv_file << current_time << ",";
    csv_file << image_base_name << ",";
    csv_file << DirectionToString(Direction::H) << ",";
    for (std::map<Type, Features>::iterator it = features.begin(); it != features.end(); ++it) {
        csv_file << it->second.H << ",";
    }
    csv_file << "\n";

    // write a row of V values
    csv_file << current_time << ",";
    csv_file << image_base_name << ",";
    csv_file << DirectionToString(Direction::V) << ",";
    for (std::map<Type, Features>::iterator it = features.begin(); it != features.end(); ++it) {
        csv_file << it->second.V << ",";
    }
    csv_file << "\n";

    // write a row of LD values
    csv_file << current_time << ",";
    csv_file << image_base_name << ",";
    csv_file << DirectionToString(Direction::LD) << ",";
    for (std::map<Type, Features>::iterator it = features.begin(); it != features.end(); ++it) {
        csv_file << it->second.LD << ",";
    }
    csv_file << "\n";

    // write a row of RD values
    csv_file << current_time << ",";
    csv_file << image_base_name << ",";
    csv_file << DirectionToString(Direction::RD) << ",";
    for (std::map<Type, Features>::iterator it = features.begin(); it != features.end(); ++it) {
        csv_file << it->second.RD << ",";
    }
    csv_file << "\n";

    // write a row of Avg values
    csv_file << current_time << ",";
    csv_file << image_base_name << ",";
    csv_file << DirectionToString(Direction::Avg) << ",";
    for (std::map<Type, Features>::iterator it = features.begin(); it != features.end(); ++it) {
        csv_file << it->second.Avg() << ",";
    }
    csv_file << "\n";

    // close the csv file
    csv_file.close();
}

std::string TextureAnalysis::GetCurrentTime() {
    time_t rawtime;
    struct tm *timeinfo;
    char buffer[80];

    time(&rawtime);
    timeinfo = localtime(&rawtime);

    strftime(buffer, sizeof(buffer), "%Y-%m-%d %H:%M:%S", timeinfo);
    std::string str(buffer);

    return str;
}

void TextureAnalysis::CalculatePixelMean(const std::vector<double> &vec) {
    _pixel_values_mean = 0.0;
    for (int i = 0; i < vec.size(); ++i) {
        _pixel_values_mean += vec[i];
    }
    _pixel_values_mean /= vec.size();
}

void TextureAnalysis::CalculatePixelSTD(const std::vector<double> &vec) {
    CalculatePixelMean(vec);
    _pixel_values_STD = 0.0;
    for (int i = 0; i < vec.size(); ++i) {
        _pixel_values_STD += (vec[i] - _pixel_values_mean) * (vec[i] - _pixel_values_mean);
    }
    _pixel_values_STD = _pixel_values_STD / (vec.size() - 1.0);
    _pixel_values_STD = sqrt(_pixel_values_STD);
}


#include <boost/math/special_functions/bessel.hpp>


#include <iostream>
#include <Eigen/Dense>
#include "DepthCamera.h"
#include <ctime>
#include "FilterPipe.h"
#include "PointCloud.h"
#include <cstdlib>
#include "PointCloudInterface.h"
#include "RadiusOutlierFilter.h"
#include "PassThroughFilter.h"
#include "PointCloudGenerator.h"
using namespace std;

int main()
{
	PointCloudRecorder pcr("out.txt");

	// camera1
	DepthCamera cam1("camera1.txt");

	FilterPipe *fp1 = new FilterPipe;
	fp1->addFilter(new RadiusOutlierFilter(25));
	fp1->addFilter(new PassThroughFilter(400, 0, 400, 0,  45, -45));
	cam1.setFilterPipe(fp1);

	Eigen::Vector3d ang1;
	Eigen::Vector3d tr1;
	ang1(0) = 0, ang1(1) = 0, ang1(2) = -90;
	tr1(0) = 100, tr1(1) = 500, tr1(2) = 50;
	Transform t1;
	t1.setRotation(ang1);
	t1.setTranslation(tr1);

	cam1.setTransformRotation(ang1);
	cam1.setTransformTranslation(tr1);

	//camera2
	DepthCamera cam2("camera2.txt");

	FilterPipe *fp2 = new FilterPipe;
	fp2->addFilter(new RadiusOutlierFilter(25));
	fp2->addFilter(new PassThroughFilter(500, 0, 500, 0, 45, -45));
	cam2.setFilterPipe(fp2);

	Eigen::Vector3d ang2;
	Eigen::Vector3d tr2;
	ang2(0) = 0, ang2(1) = 0, ang2(2) = 90;
	tr2(0) = 550, tr2(1) = 50, tr2(2) = 50;
	Transform t2;
	t2.setRotation(ang2);
	t2.setTranslation(tr2);

	cam2.setTransformRotation(ang2);
	cam2.setTransformTranslation(tr2);

	PointCloudInterface pci;
	pci.addGenerator(&cam2);
	pci.addGenerator(&cam1);
	pci.setRecorder(&pcr);
	pci.generate();
	pci.record();

	system("pause");
}


// Distributed under the MIT License.
// See LICENSE.txt for details.

#include "tests/Unit/TestingFramework.hpp"

#include <array>
#include <boost/optional.hpp>
#include <cstddef>
#include <string>
#include <unordered_map>

#include "ControlSystem/FunctionOfTime.hpp"
#include "ControlSystem/PiecewisePolynomial.hpp"
#include "DataStructures/DataVector.hpp"
#include "DataStructures/Tensor/Tensor.hpp"
#include "Domain/CoordinateMaps/Translation.hpp"
#include "Utilities/ConstantExpressions.hpp"
#include "Utilities/StdArrayHelpers.hpp"
#include "Utilities/TypeTraits.hpp"
#include "tests/Unit/Domain/CoordinateMaps/TestMapHelpers.hpp"
#include "tests/Unit/TestHelpers.hpp"

SPECTRE_TEST_CASE("Unit.Domain.CoordMapsTimeDependent.Translation",
                  "[Domain][Unit]") {
  // define vars for FunctionOfTime::PiecewisePolynomial f(t) = t**2.
  double t = -1.0;
  const double dt = 0.6;
  const double final_time = 4.0;
  constexpr size_t deriv_order = 3;

  const std::array<DataVector, deriv_order + 1> init_func{
      {{1.0}, {-2.0}, {2.0}, {0.0}}};
  FunctionsOfTime::PiecewisePolynomial<deriv_order> f_of_t_derived(t,
                                                                   init_func);
  FunctionOfTime& f_of_t = f_of_t_derived;

  const std::unordered_map<std::string, FunctionOfTime&> f_of_t_list = {
      {"trans", f_of_t}};
  const CoordMapsTimeDependent::Translation trans_map{};
  // test serialized/deserialized map
  const auto trans_map_deserialized = serialize_and_deserialize(trans_map);

  const std::array<double, 1> point_xi{{3.2}};

  while (t < final_time) {
    const std::array<double, 1> trans_x{{square(t)}};
    const std::array<double, 1> frame_vel{{f_of_t.func_and_deriv(t)[1][0]}};

    CHECK_ITERABLE_APPROX(trans_map(point_xi, t, f_of_t_list),
                          point_xi + trans_x);
    CHECK_ITERABLE_APPROX(
        trans_map.inverse(point_xi + trans_x, t, f_of_t_list).get(), point_xi);
    CHECK_ITERABLE_APPROX(trans_map.frame_velocity(point_xi, t, f_of_t_list),
                          frame_vel);

    CHECK_ITERABLE_APPROX(trans_map_deserialized(point_xi, t, f_of_t_list),
                          point_xi + trans_x);
    CHECK_ITERABLE_APPROX(
        trans_map_deserialized.inverse(point_xi + trans_x, t, f_of_t_list)
            .get(),
        point_xi);
    CHECK_ITERABLE_APPROX(trans_map_deserialized.frame_velocity(
                              point_xi + trans_x, t, f_of_t_list),
                          frame_vel);

    t += dt;
  }

  // time-independent checks
  CHECK(trans_map.inv_jacobian(point_xi).get(0, 0) == 1.0);
  CHECK(trans_map_deserialized.inv_jacobian(point_xi).get(0, 0) == 1.0);
  CHECK(trans_map.jacobian(point_xi).get(0, 0) == 1.0);
  CHECK(trans_map_deserialized.jacobian(point_xi).get(0, 0) == 1.0);

  // Check inequivalence operator
  CHECK_FALSE(trans_map != trans_map);
  CHECK_FALSE(trans_map_deserialized != trans_map_deserialized);

  // Check serialization
  CHECK(trans_map == trans_map_deserialized);
  CHECK_FALSE(trans_map != trans_map_deserialized);

  test_coordinate_map_argument_types(trans_map, point_xi, t, f_of_t_list);
}


#pragma once

#include <boost/mpl/identity.hpp>
#include <boost/multiprecision/mpfr.hpp>
#include <type_traits>

#include <cmath>

// own includes ---------------------------------------------------------
#include "traits/type_traits.hpp"

namespace math {

// ----------------------------------------------------------------------
// SQRT
template <typename T>
inline constexpr typename std::enable_if<std::is_floating_point<T>::value, T>::type
sqrt(const T& t)
{
  return std::sqrt(t);
}

template <typename T>
inline constexpr typename std::enable_if<!std::is_floating_point<T>::value, T>::type
sqrt(const T& t)
{
  return boost::multiprecision::sqrt(t);
}

// ----------------------------------------------------------------------
// EXP
template <typename T>
inline constexpr typename std::enable_if<std::is_floating_point<T>::value, T>::type
exp(const T& t)
{
  return std::exp(t);
}

template <typename T>
inline constexpr typename std::enable_if<!std::is_floating_point<T>::value, T>::type
exp(const T& t)
{
  return boost::multiprecision::exp(t);
}

// ----------------------------------------------------------------------
// LOG
template <typename T>
inline constexpr typename std::enable_if<std::is_floating_point<T>::value, T>::type
log(const T& t)
{
  return std::log(t);
}

template <typename T>
inline constexpr typename std::enable_if<!std::is_floating_point<T>::value, T>::type
log(const T& t)
{
  return boost::multiprecision::log(t);
}

// ----------------------------------------------------------------------
// POW
template <typename T>
inline constexpr typename std::enable_if<std::is_floating_point<T>::value, T>::type
pow(const T& base, const typename boost::mpl::identity<T>::type& exp)
{
  return std::pow(base, exp);
}

template <typename T>
inline constexpr typename std::enable_if<!std::is_floating_point<T>::value, T>::type
pow(const T& base, const typename boost::mpl::identity<T>::type& exp)
{
  return boost::multiprecision::pow(base, exp);
}

// ----------------------------------------------------------------------
// ABS
template <typename T>
inline constexpr typename std::enable_if<std::is_floating_point<T>::value, T>::type
abs(const T& t)
{
  return std::abs(t);
}

template <typename T>
inline constexpr typename std::enable_if<!std::is_floating_point<T>::value, T>::type
abs(const T& t)
{
  return boost::multiprecision::abs(t);
}

}  // end namespace math


// $Id$
//
// Copyright (C)  2004-2006 Rational Discovery LLC
//
//   @@ All Rights Reserved @@
//  This file is part of the RDKit.
//  The contents are covered by the terms of the BSD license
//  which is included in the file license.txt, found at the root
//  of the RDKit source tree.
//

#include "BoundsMatrix.h"
#include "TriangleSmooth.h"
#include <iostream>
#include <boost/smart_ptr.hpp>
#include <math.h>
#include <Numerics/SymmMatrix.h>
#include "DistGeomUtils.h"
#include <RDGeneral/utils.h>

using namespace DistGeom;
using namespace RDNumeric;

void test1() {
  // test triangle smoothing
  unsigned int npt = 5;
  double x = sqrt(3.0);
  BoundsMatrix *mmat = new BoundsMatrix(npt);

  mmat->setUpperBound(0, 1, 1.0);
  mmat->setLowerBound(0, 1, 1.0);
  mmat->setUpperBound(0, 2, x);
  mmat->setLowerBound(0, 2, x);
  mmat->setUpperBound(0, 3, 10.0);
  mmat->setLowerBound(0, 3, 0.0);
  mmat->setUpperBound(0, 4, 10.0);
  mmat->setLowerBound(0, 4, 0.0);
  mmat->setUpperBound(1, 2, 1.0);
  mmat->setLowerBound(1, 2, 1.0);
  mmat->setUpperBound(1, 3, x);
  mmat->setLowerBound(1, 3, x);
  mmat->setUpperBound(1, 4, 10.0);
  mmat->setLowerBound(1, 4, 0.0);
  mmat->setUpperBound(2, 3, 1.0);
  mmat->setLowerBound(2, 3, 1.0);
  mmat->setUpperBound(2, 4, x);
  mmat->setLowerBound(2, 4, x);
  mmat->setUpperBound(3, 4, 1.0);
  mmat->setLowerBound(3, 4, 1.0);

  BoundsMatPtr mptr(mmat);

  triangleSmoothBounds(mptr);
  CHECK_INVARIANT(RDKit::feq(mmat->getUpperBound(0, 1), 1.0, 0.001), "");
  CHECK_INVARIANT(RDKit::feq(mmat->getLowerBound(0, 1), 1.0, 0.001), "");
  CHECK_INVARIANT(RDKit::feq(mmat->getUpperBound(0, 2), 1.732, 0.001), "");
  CHECK_INVARIANT(RDKit::feq(mmat->getLowerBound(0, 2), 1.732, 0.001), "");
  CHECK_INVARIANT(RDKit::feq(mmat->getUpperBound(0, 3), 2.732, 0.001), "");
  CHECK_INVARIANT(RDKit::feq(mmat->getLowerBound(0, 3), 0.732, 0.001), "");
  CHECK_INVARIANT(RDKit::feq(mmat->getUpperBound(0, 4), 3.464, 0.001), "");
  CHECK_INVARIANT(RDKit::feq(mmat->getLowerBound(0, 4), 0.0, 0.001), "");
  CHECK_INVARIANT(RDKit::feq(mmat->getUpperBound(1, 2), 1.0, 0.001), "");
  CHECK_INVARIANT(RDKit::feq(mmat->getLowerBound(1, 2), 1.0, 0.001), "");
  CHECK_INVARIANT(RDKit::feq(mmat->getUpperBound(1, 3), 1.732, 0.001), "");
  CHECK_INVARIANT(RDKit::feq(mmat->getLowerBound(1, 3), 1.732, 0.001), "");
  CHECK_INVARIANT(RDKit::feq(mmat->getUpperBound(1, 4), 2.732, 0.001), "");
  CHECK_INVARIANT(RDKit::feq(mmat->getLowerBound(1, 4), 0.732, 0.001), "");
  CHECK_INVARIANT(RDKit::feq(mmat->getUpperBound(2, 3), 1.0, 0.001), "");
  CHECK_INVARIANT(RDKit::feq(mmat->getLowerBound(2, 3), 1.0, 0.001), "");
  CHECK_INVARIANT(RDKit::feq(mmat->getUpperBound(2, 4), 1.732, 0.001), "");
  CHECK_INVARIANT(RDKit::feq(mmat->getLowerBound(2, 4), 1.732, 0.001), "");
  CHECK_INVARIANT(RDKit::feq(mmat->getUpperBound(3, 4), 1.0, 0.001), "");
  CHECK_INVARIANT(RDKit::feq(mmat->getLowerBound(3, 4), 1.0, 0.001), "");

  DoubleSymmMatrix dmat(npt, 0.0);
  RDKit::rng_type generator(42u);
  generator.seed(100);
  RDKit::uniform_double distrib(0, 1.0);
  RDKit::double_source_type rng(generator, distrib);
  pickRandomDistMat(*mmat, dmat, rng);

  double sumElem = 0.0;
  for (unsigned int i = 0; i < dmat.getDataSize(); i++) {
    sumElem += dmat.getData()[i];
  }
  CHECK_INVARIANT(RDKit::feq(sumElem, 14.3079, 0.001), "");
}

void testIssue216() {
  RDNumeric::DoubleSymmMatrix dmat(4);
  dmat.setVal(0, 0, 0.0);
  dmat.setVal(0, 1, 1.0);
  dmat.setVal(0, 2, 1.0);
  dmat.setVal(0, 3, 1.0);
  dmat.setVal(1, 1, 0.0);
  dmat.setVal(1, 2, 1.0);
  dmat.setVal(1, 3, 1.0);
  dmat.setVal(2, 2, 0.0);
  dmat.setVal(2, 3, 1.0);
  dmat.setVal(3, 3, 0.0);

  std::cout << dmat;
  RDGeom::PointPtrVect pos;
  for (int i = 0; i < 4; i++) {
    RDGeom::Point3D *pt = new RDGeom::Point3D();
    pos.push_back(pt);
  }

  bool gotCoords = DistGeom::computeInitialCoords(dmat, pos);
  CHECK_INVARIANT(gotCoords, "");

  for (int i = 1; i < 4; i++) {
    RDGeom::Point3D pti = *(RDGeom::Point3D *)pos[i];
    for (int j = 0; j < i; j++) {
      RDGeom::Point3D ptj = *(RDGeom::Point3D *)pos[j];
      ptj -= pti;
      CHECK_INVARIANT(RDKit::feq(ptj.length(), 1.0, 0.02), "");
    }
  }
}
int main() {
  std::cout << "***********************************************************\n";
  std::cout << "   test1 \n";
  test1();

  std::cout << "***********************************************************\n";
  std::cout << "   testIssue216 \n";
  testIssue216();
  std::cout
      << "***********************************************************\n\n";
  return 0;
}


/*-----------------------------------------------------------------------------+
Interval Container Library
Author: Joachim Faulhaber
Copyright (c) 2007-2009: Joachim Faulhaber
Copyright (c) 1999-2006: Cortex Software GmbH, Kantstrasse 57, Berlin
+------------------------------------------------------------------------------+
   Distributed under the Boost Software License, Version 1.0.
      (See accompanying file LICENCE.txt or copy at
           http://www.boost.org/LICENSE_1_0.txt)
+-----------------------------------------------------------------------------*/

/** Example partys_tallest_guests.cpp \file partys_tallest_guests.cpp
    \brief Using <i>aggregate on overlap</i> the heights of the party's tallest
           guests are computed.

    In partys_tallest_guests.cpp we use a different instantiation of
    interval map templates to compute maxima of guest heights.

    Instead of aggregating groups of people attending the party in time
    we aggregate the maximum of guest height for the time intervals.

    Using a joining interval_map results in a smaller map: All interval
    value pairs are joined if the maximum does not change in time. Using
    a split_interval_map results in a larger map: All splits of intervals
    that occur due to entering and leaving of guests are preserved in
    the split_interval_map.

    \include partys_tallest_guests_/partys_tallest_guests.cpp
*/
//[example_partys_tallest_guests
// The next line includes <boost/date_time/posix_time/posix_time.hpp>
// and a few lines of adapter code.
#include <boost/icl/ptime.hpp>
#include <iostream>
#include <boost/icl/interval_map.hpp>
#include <boost/icl/split_interval_map.hpp>

using namespace std;
using namespace boost::posix_time;
using namespace boost::icl;

// A party's height shall be defined as the maximum height of all guests ;-)
// The last parameter 'inplace_max' is a functor template that calls a max
// aggregation on overlap.
typedef interval_map<ptime, int, partial_absorber, less, inplace_max>
    PartyHeightHistoryT;

// Using a split_interval_map we preserve interval splittings that occurred via insertion.
typedef split_interval_map<ptime, int, partial_absorber, less, inplace_max>
    PartyHeightSplitHistoryT;

void partys_height()
{
    PartyHeightHistoryT tallest_guest;

    tallest_guest +=
      make_pair(
        discrete_interval<ptime>::right_open(
          time_from_string("2008-05-20 19:30"),
          time_from_string("2008-05-20 23:00")),
        180); // Mary & Harry: Harry is 1,80 m tall.

    tallest_guest +=
      make_pair(
        discrete_interval<ptime>::right_open(
          time_from_string("2008-05-20 20:10"),
          time_from_string("2008-05-21 00:00")),
        170); // Diana & Susan: Diana is 1,70 m tall.

    tallest_guest +=
      make_pair(
        discrete_interval<ptime>::right_open(
          time_from_string("2008-05-20 22:15"),
          time_from_string("2008-05-21 00:30")),
        200); // Peters height is 2,00 m

    PartyHeightHistoryT::iterator height_ = tallest_guest.begin();
    cout << "-------------- History of maximum guest height -------------------\n";
    while(height_ != tallest_guest.end())
    {
        discrete_interval<ptime> when = height_->first;
        // Of what height are the tallest guests within the time interval 'when' ?
        int height = (*height_++).second;
        cout << "[" << first(when) << " - " << upper(when) << ")"
             << ": " << height <<" cm = " << height/30.48 << " ft" << endl;
    }

}

// Next we are using a split_interval_map instead of a joining interval_map
void partys_split_height()
{
    PartyHeightSplitHistoryT tallest_guest;

    // adding an element can be done wrt. simple aggregate functions
    // like e.g. min, max etc. in their 'inplace' or op= incarnation
    tallest_guest +=
      make_pair(
        discrete_interval<ptime>::right_open(
          time_from_string("2008-05-20 19:30"),
          time_from_string("2008-05-20 23:00")),
        180); // Mary & Harry: Harry is 1,80 m tall.

    tallest_guest +=
      make_pair(
        discrete_interval<ptime>::right_open(
          time_from_string("2008-05-20 20:10"),
          time_from_string("2008-05-21 00:00")),
        170); // Diana & Susan: Diana is 1,70 m tall.

    tallest_guest +=
      make_pair(
        discrete_interval<ptime>::right_open(
          time_from_string("2008-05-20 22:15"),
          time_from_string("2008-05-21 00:30")),
        200); // Peters height is 2,00 m

    PartyHeightSplitHistoryT::iterator height_ = tallest_guest.begin();
    cout << "\n";
    cout << "-------- Split History of maximum guest height -------------------\n";
    cout << "--- Same map as above but split for every interval insertion.  ---\n";
    while(height_ != tallest_guest.end())
    {
        discrete_interval<ptime> when = height_->first;
        // Of what height are the tallest guests within the time interval 'when' ?
        int height = (*height_++).second;
        cout << "[" << first(when) << " - " << upper(when) << ")"
             << ": " << height <<" cm = " << height/30.48 << " ft" << endl;
    }

}

int main()
{
    cout << ">>Interval Container Library: Sample partys_tallest_guests.cpp  <<\n";
    cout << "------------------------------------------------------------------\n";
    partys_height();
    partys_split_height();
    return 0;
}

// Program output:
/*-----------------------------------------------------------------------------
>>Interval Container Library: Sample partys_tallest_guests.cpp  <<
------------------------------------------------------------------
-------------- History of maximum guest height -------------------
[2008-May-20 19:30:00 - 2008-May-20 22:15:00): 180 cm = 5.90551 ft
[2008-May-20 22:15:00 - 2008-May-21 00:30:00): 200 cm = 6.56168 ft

-------- Split History of maximum guest height -------------------
--- Same map as above but split for every interval insertion.  ---
[2008-May-20 19:30:00 - 2008-May-20 20:10:00): 180 cm = 5.90551 ft
[2008-May-20 20:10:00 - 2008-May-20 22:15:00): 180 cm = 5.90551 ft
[2008-May-20 22:15:00 - 2008-May-20 23:00:00): 200 cm = 6.56168 ft
[2008-May-20 23:00:00 - 2008-May-21 00:00:00): 200 cm = 6.56168 ft
[2008-May-21 00:00:00 - 2008-May-21 00:30:00): 200 cm = 6.56168 ft
-----------------------------------------------------------------------------*/
//]


#ifndef MATHEVAL_IMPLEMENTATION
#error "Do not include ast.hpp directly!"
#endif

#pragma once

#include <boost/spirit/home/x3.hpp>
#include <boost/spirit/home/x3/support/ast/variant.hpp>

#include <list>
#include <string>

namespace matheval {

namespace x3 = boost::spirit::x3;

namespace ast {

struct nil {};
struct unary_op;
struct binary_op;
struct ternary_op;
struct expression;

// clang-format off
struct operand : x3::variant<
                 nil
                 , double
                 , std::string
                 , x3::forward_ast<unary_op>
                 , x3::forward_ast<binary_op>
                 , x3::forward_ast<ternary_op>
                 , x3::forward_ast<expression>
                 > {
    using base_type::base_type;
    using base_type::operator=;
};
// clang-format on

struct unary_op {
    double (*op)(double);
    operand rhs;
};

struct binary_op {
    double (*op)(double, double);
    operand lhs;
    operand rhs;
};

struct ternary_op {
    double (*op)(double, double, double);
    operand p1;
    operand p2;
    operand p3;
};

struct operation {
    double (*op)(double, double);
    operand rhs;
};

struct expression {
    operand lhs;
    std::list<operation> rhs;
};

} // namespace ast

} // namespace matheval


#include <opencv2/highgui/highgui.hpp>
#include "opencv2/imgproc/imgproc.hpp"
#include <Eigen/Dense>
#include <Eigen/Core>     // to use Eigen::Map
#include "contourDetection.h"

using namespace cv;
using namespace std;

Mat imageSegmentation(const cv::Mat& color_image, SegmentationType type, ImageROI roi, int theshold) {
	Rect2i ROI{roi.x_min, roi.y_min, roi.x_max - roi.x_min, roi.y_max - roi.y_min};            // create a rectangle for selecting ROI (x_min, y_min, x_max-x_min, y_max-y_min)
	Mat imageROI = color_image(ROI);
	Mat binary_image;                            // Binary image
	switch(type) {
	case SegmentationType::Black:
	{
		Mat imageGray;                              // Gray scale image
		cv::cvtColor(imageROI, imageGray, CV_BGR2GRAY);
		const int maxValue = 255;                         // Maximum value
		cv::threshold(imageGray, binary_image, theshold, maxValue, THRESH_BINARY);      // Binary image
	}
	break;
	case SegmentationType::Red:
	{
		cv::Mat hsv_img;
		cv::cvtColor(imageROI, hsv_img, cv::COLOR_BGR2HSV);

		// Threshold the HSV image, keep only the red pixels
		cv::Mat mask1;
		cv::Mat mask2;
		cv::inRange(hsv_img, cv::Scalar(0, 100, 100), cv::Scalar(theshold, 255, 255), mask1);
		cv::inRange(hsv_img, cv::Scalar(179-theshold, 100, 100), cv::Scalar(179, 255, 255), mask2);
		cv::Mat detectRed = mask1 | mask2;        // With this red will be white and rest will be black
		// Reverse black and white to fit with the active contour alg.
		cv::bitwise_not(detectRed, binary_image);
	}
	break;
	}

	Mat filtered_image;
	cv::erode(binary_image, filtered_image, Mat());
	cv::dilate(binary_image, filtered_image, Mat());

	return filtered_image;
}

Mat ContourToYXdata(const Mat &contours, int rowMax)   // function to transfer the contour data into XY coordinate data for optimization purposes
{
  // contours: Contour data
  // rowMax: number of row of the image
	// See matlab function imgdata2plotdata
  Mat XYdataReturn;
  contours.col(1) = rowMax - contours.col(1) - 1;
	// contours.col(1) = contours.col(1) + 1;  // For obtain MATLAB liked data
  return contours;
}

Mat ContourDetect(Mat &image, SegmentationType type, ImageROI roi, int canny_thresh)
{
	Mat segmentedImage = imageSegmentation(image, type, roi);
  int segmentedImageRow = segmentedImage.rows;
	vector<Vec4i> hierarchy;
	Mat canny_output;
	vector<vector<Point> > contours;
  Canny( segmentedImage, canny_output, canny_thresh, canny_thresh*2, 3 );
  findContours( canny_output, contours, hierarchy, CV_RETR_TREE, CV_CHAIN_APPROX_SIMPLE, Point(0, 0) );
  // waitKey(0);
  Mat contourData(contours[0][0]);  // create a mat to store the contour
  int index = 0;
  std::cout << "contour size " << contours.size() << '\n';
  // Find the contour with most points(it must be the right contour)
  for(int i= 0; i < contours.size(); i++)
  {
    if (contours[index].size() < contours[i].size())
    {
      index = i;
    }
  }
  // Save all the contour data
  for(int j= 0; j < contours[index].size();j++) // run until j < contours[i].size();
  {
    Mat temp(contours[index][j]);
    hconcat(contourData, temp, contourData); //do whatever
  }
  Mat contourDataT;
  transpose(contourData, contourDataT);
  contourDataT(Range(1, contourDataT.rows), Range(0,contourDataT.cols)).copyTo(contourDataT);
  Mat XYdata = ContourToYXdata(contourDataT, segmentedImageRow);
	return XYdata;
}


// Boost.Geometry Index
//
// Quickbook Examples
//
// Copyright (c) 2011-2013 Adam Wulkiewicz, Lodz, Poland.
//
// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

//[rtree_quickstart

//[rtree_quickstart_include

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point.hpp>
#include <boost/geometry/geometries/box.hpp>

#include <boost/geometry/index/rtree.hpp>

// to store queries results
#include <vector>

// just for output
#include <iostream>
#include <boost/foreach.hpp>

namespace bg = boost::geometry;
namespace bgi = boost::geometry::index;
//]

int main(void)
{
    //[rtree_quickstart_valuetype
    typedef bg::model::point<float, 2, bg::cs::cartesian> point;
    typedef bg::model::box<point> box;
    typedef std::pair<box, unsigned> value;
    //]

    //[rtree_quickstart_create
    // create the rtree using default constructor
    bgi::rtree< value, bgi::quadratic<16> > rtree;
    //]

    //[rtree_quickstart_insert
    // create some values
    for ( unsigned i = 0 ; i < 10 ; ++i )
    {
        // create a box
        box b(point(i, i), point(i + 0.5f, i + 0.5f));
        // insert new value
        rtree.insert(std::make_pair(b, i));
    }
    //]

    //[rtree_quickstart_spatial_query
    // find values intersecting some area defined by a box
    box query_box(point(0, 0), point(5, 5));
    std::vector<value> result_s;
    rtree.query(bgi::intersects(query_box), std::back_inserter(result_s));
    //]

    //[rtree_quickstart_nearest_query
    // find 5 nearest values to a point
    std::vector<value> result_n;
    rtree.query(bgi::nearest(point(0, 0), 5), std::back_inserter(result_n));
    //]

    //[rtree_quickstart_output
    // display results
    std::cout << "spatial query box:" << std::endl;
    std::cout << bg::wkt<box>(query_box) << std::endl;
    std::cout << "spatial query result:" << std::endl;
    BOOST_FOREACH(value const& v, result_s)
        std::cout << bg::wkt<box>(v.first) << " - " << v.second << std::endl;

    std::cout << "knn query point:" << std::endl;
    std::cout << bg::wkt<point>(point(0, 0)) << std::endl;
    std::cout << "knn query result:" << std::endl;
    BOOST_FOREACH(value const& v, result_n)
        std::cout << bg::wkt<box>(v.first) << " - " << v.second << std::endl;
    //]

    return 0;
}

//]