// http://www.boost.org/doc/libs/1_55_0/doc/html/boost_random/reference.html#boost_random.reference.generators
// http://www.johndcook.com/blog/cpp_TR1_random/#gamma
#ifndef qlex_boost_rng_binding_h
#define qlex_boost_rng_binding_h

#include <ql/types.hpp>
#include <ql/math/randomnumbers/seedgenerator.hpp>
#include <ql/math/randomnumbers/rngtraits.hpp>
#include <ql/math/distributions/normaldistribution.hpp>
#include <ql/methods/montecarlo/sample.hpp>
#include <boost/static_assert.hpp>
#include <boost/random.hpp>

#include <math/randomnumbers/counter_based_engine.hpp>
#include <math/randomnumbers/threefry.hpp>
#include <math/randomnumbers/philox.hpp>
#include <math/distributions/gammadistribution.hpp>
#include <settings.hpp>

using namespace QuantLib;

namespace QLExtension {

	template<class RNG>
	class GenericBoostIntRNG
	{
	private:
		mutable RNG generator;
		static Real ceil_value;
	public:
		typedef Sample<Real> sample_type;
		typedef typename RNG::result_type result_type;

		BOOST_STATIC_ASSERT(std::numeric_limits<result_type>::is_integer);

		GenericBoostIntRNG(result_type seed = 0);

		//! return a random number in the (0.0, 1.0)-interval
		//! For random number generators with a precision larger than
		Real nextReal() const
		{
			return (Real(generator()) + 0.5) / ceil_value;
		}

		void discard(result_type n)
		{
			generator.discard(n);
		}

		// restart/reset base counter
		// seed is fixed to a name/curve.
		// Use this to restart inside the name. it is intended for use in inner loops.
		void restart(result_type n)
		{
			generator.restart({ n, 0 });
		}

		/*! returns a sample with weight 1.0 containing a random number
		in the (0.0, 1.0) interval  */
		sample_type next() const
		{
			return sample_type(nextReal(), 1.0);
		}

	private:
		uint64_t getSeed(uint64_t seed);
	};		// GenericBoostIntRNG

	// counter-bits + non-counter-bits (base counter) = domain_type
	// Attempting to set a key (either with the constructor or with seed(...) that has non-zero bits in the highest log_2(DomainBits) of its key
	//			will throw an invalid_argument exception.

	// Boost threefry 2x64 round 32
	// uint64 --> 64bit base counter
	//  threefry<N, U>::key_type    = boost::array<U, N>
	//					domain_type = boost::array<U, N>
	//					range_type = boost::array<U, N>
	typedef GenericBoostIntRNG< boost::random::counter_based_engine<uint64_t,
		boost::random::threefry<2, uint64_t>, 32> > BoostThreefryUniformRng;
	typedef GenericPseudoRandom<
		BoostThreefryUniformRng,
		InverseCumulativeNormal> PseudoRandomNormalBoostThreefry;

	// Boost Philox 2x64 round 10
	// uint64 --> 64bit base counter
	// philox<N, U>::key_type    = boost::array<U, N/2>
	//				domain_type = boost::array<U, N>
	//				range_type = boost::array<U, N>
	typedef GenericBoostIntRNG<boost::random::counter_based_engine<uint64_t,
		boost::random::philox<2, uint64_t>> > BoostPhiloxUniformRng;
	typedef GenericPseudoRandom<
		BoostPhiloxUniformRng,
		InverseCumulativeNormal> PseudoRandomNormalBoostPhilox;

	template<class RNG>
	Real GenericBoostIntRNG<RNG>::ceil_value = Real(RNG::max()) + 1.0;

	template<class RNG>
	GenericBoostIntRNG<RNG>::GenericBoostIntRNG(result_type seed) :
		generator(GenericBoostIntRNG<RNG>::getSeed(seed))
	{
	}

	template<class RNG>
	uint64_t GenericBoostIntRNG<RNG>::getSeed(
		uint64_t seed)
	{
		//unsigned long s = (seed != 0 ? seed : SeedGenerator::instance().get());
		//return (result_type)(s % RNG::max());
		return seed;
	}

	void originalthreefry(uint64_t seed, uint64_t skip, uint64_t buffer[], uint64_t length);
	void uniformthreefry(uint64_t seed, uint64_t skip, double buffer[], uint64_t length);
	void normalthreefry(uint64_t seed, uint64_t skip, double buffer[], uint64_t length);
	//QLEX_API void normalthreefry2(uint64_t seed, uint64_t skip, double buffer[], uint64_t length);
	void gammathreefry(uint64_t seed, uint64_t skip, double buffer[], uint64_t length, Real shape, Real scale);
}
#endif /* qlex_boost_rng_binding_h */


#include <foundation/types.h>
#include <geometry/camera.h>
#include <gmock/gmock.h>
#include <gtest/gtest.h>

#include <Eigen/Dense>
#include <random>

class CameraFixture : public ::testing::Test {
 public:
  CameraFixture() {
    pixels.resize(pixels_count, 2);

    std::mt19937 gen(42);
    std::uniform_real_distribution<> rand(-0.5, 0.5);
    for (int i = 0; i < pixels_count; ++i) {
      const auto x = rand(gen) * max_width;
      const auto y = rand(gen) * max_height;
      pixels.row(i) << x, y;
    }

    distortion.resize(2);
    distortion << -0.1, 0.03;
    distortion_brown.resize(5);
    distortion_brown << -0.1, 0.03, 0.001, 0.001, 0.002;
    distortion_fisheye.resize(4);
    distortion_fisheye << -0.1, 0.03, 0.001, 0.005;
    distortion_fisheye62.resize(8);
    distortion_fisheye62 << -0.1, 0.03, 0.001, 0.005, 0.02, 0.001, 0.0007,
        -0.01;
    distortion_fisheye624.resize(12);
    distortion_fisheye624 << -0.1, 0.03, 0.001, 0.005, 0.02, 0.001, 0.0007,
        -0.01, 0.01, -0.007, -0.03, 0.0053;
    distortion_radial << 0.1, 0.03;
    principal_point << 0.1, -0.05;
  }

  double ComputeError(const MatX2d& other) {
    double error = 0;
    for (int i = 0; i < pixels_count; ++i) {
      error += (other.row(i) - pixels.row(i)).norm();
    }
    return error / pixels_count;
  }

  const int pixels_count{10000};
  const int pixel_width{4000};
  const int pixel_height{3000};

  const double max_width{0.90};
  const double max_height{0.75};
  const double focal{0.4};
  const double new_focal{0.8};
  const double new_ar{0.9};

  MatX2d pixels;
  VecXd distortion;
  VecXd distortion_brown;
  VecXd distortion_fisheye;
  VecXd distortion_fisheye62;
  VecXd distortion_fisheye624;
  Vec2d distortion_radial;
  Vec2d principal_point;

  double point[3];
  double projected[2];
};

TEST_F(CameraFixture, PerspectiveIsConsistent) {
  geometry::Camera camera = geometry::Camera::CreatePerspectiveCamera(
      focal, distortion[0], distortion[1]);
  const auto projected = camera.ProjectMany(camera.BearingsMany(pixels));
  ASSERT_LT(ComputeError(projected), 2e-7);
}

TEST_F(CameraFixture, BrownIsConsistent) {
  geometry::Camera camera = geometry::Camera::CreateBrownCamera(
      focal, 1.0, principal_point, distortion_brown);
  const auto projected = camera.ProjectMany(camera.BearingsMany(pixels));
  ASSERT_LT(ComputeError(projected), 2e-7);
}

TEST_F(CameraFixture, FisheyeIsConsistent) {
  geometry::Camera camera = geometry::Camera::CreateFisheyeCamera(
      focal, distortion[0], distortion[1]);
  const auto projected = camera.ProjectMany(camera.BearingsMany(pixels));
  ASSERT_LT(ComputeError(projected), 2e-7);
}

TEST_F(CameraFixture, FisheyeIsConsistentLargeFov) {
  double short_focal = 0.3;
  geometry::Camera camera =
      geometry::Camera::CreateFisheyeCamera(short_focal, 0.001, 0.001);
  const auto projected = camera.ProjectMany(camera.BearingsMany(pixels));
  ASSERT_LT(ComputeError(projected), 2e-7);
}

TEST_F(CameraFixture, FisheyeOpencvIsConsistent) {
  geometry::Camera camera = geometry::Camera::CreateFisheyeOpencvCamera(
      focal, 1.0, principal_point, distortion_fisheye);
  const auto projected = camera.ProjectMany(camera.BearingsMany(pixels));
  ASSERT_LT(ComputeError(projected), 2e-7);
}

TEST_F(CameraFixture, Fisheye62IsConsistent) {
  geometry::Camera camera = geometry::Camera::CreateFisheye62Camera(
      focal, 1.0, principal_point, distortion_fisheye62);
  const auto projected = camera.ProjectMany(camera.BearingsMany(pixels));
  ASSERT_LT(ComputeError(projected), 2e-7);
}

TEST_F(CameraFixture, Fisheye624IsConsistent) {
  geometry::Camera camera = geometry::Camera::CreateFisheye624Camera(
      focal, 1.0, principal_point, distortion_fisheye624);
  const auto projected = camera.ProjectMany(camera.BearingsMany(pixels));
  ASSERT_LT(ComputeError(projected), 2e-7);
}

TEST_F(CameraFixture, Fisheye624MatchesReference) {
  Eigen::Matrix<double, 3, 3> points;
  points <<
    10.0, 0.0, 0.0,
    0.0, 10.0, 0.0,
    1e-6, 1e-6, 10.0;

  // This test case is constructed to result in round numbers apart from the
  // thin prism distortions.
  Eigen::Matrix<double, 3, 2> reference_pixels;
  reference_pixels <<
    149.42887062, 48.67088846,
    99.4288738, 98.67088528,
    100.0, 50.0;

  constexpr int width = 200, height = 100;
  constexpr double f = width / (2 * M_PI);
  Eigen::Matrix<double, 12, 1> distortion_thin_prism;
  distortion_thin_prism << 0, 0, 0, 0, 0, 0, 0, 0, 0.01, -0.007, -0.03, 0.0053;
  const geometry::Camera camera = geometry::Camera::CreateFisheye624Camera(
      f, 1.0, {width / 2, height / 2}, distortion_thin_prism);
  const MatX2d reference_projected = camera.ProjectMany(points);

  ASSERT_TRUE(reference_pixels.isApprox(reference_projected, 1e-5))
    << " where reference_pixels = \n" << reference_pixels << '\n'
    << " and reference_projected = \n" << reference_projected << '\n';
}

TEST_F(CameraFixture, Fisheye624WithTanMatchesReference) {
  Eigen::Matrix<double, 3, 3> points;
  points <<
    10.0, 0.0, 0.0,
    0.0, 10.0, 0.0,
    1e-6, 1e-6, 10.0;

  // This test case is constructed to result in round numbers apart from the
  // tangential and thin prism distortions.
  Eigen::Matrix<double, 3, 2> reference_pixels;
  reference_pixels <<
    151.7850648, 51.02708265,
    100.2142719, 105.7394678,
    100.0, 50.0;

  constexpr int width = 200, height = 100;
  constexpr double f = width / (2 * M_PI);
  Eigen::Matrix<double, 12, 1> distortion_tan_and_thin_prism;
  distortion_tan_and_thin_prism << 0, 0, 0, 0, 0, 0, 0.03, 0.01, 0.01, -0.007, -0.03, 0.0053;
  const geometry::Camera camera = geometry::Camera::CreateFisheye624Camera(
      f, 1.0, {width / 2, height / 2}, distortion_tan_and_thin_prism);
  const MatX2d reference_projected = camera.ProjectMany(points);

  ASSERT_TRUE(reference_pixels.isApprox(reference_projected, 1e-5))
    << " where reference_pixels = \n" << reference_pixels << '\n'
    << " and reference_projected = \n" << reference_projected << '\n';
}

TEST_F(CameraFixture, DualIsConsistent) {
  geometry::Camera camera = geometry::Camera::CreateDualCamera(
      0.5, focal, distortion[0], distortion[1]);
  const auto projected = camera.ProjectMany(camera.BearingsMany(pixels));
  ASSERT_LT(ComputeError(projected), 2e-7);
}

TEST_F(CameraFixture, SphericalIsConsistent) {
  geometry::Camera camera = geometry::Camera::CreateSphericalCamera();
  const auto projected = camera.Project(camera.Bearing(Vec2d(0.2, 0.2)));
  ASSERT_EQ(projected(0), 0.2);
  ASSERT_EQ(projected(1), 0.2);
}

TEST_F(CameraFixture, RadialIsConsistent) {
  geometry::Camera camera = geometry::Camera::CreateRadialCamera(
      focal, 1.0, principal_point, distortion_radial);
  const auto projected = camera.ProjectMany(camera.BearingsMany(pixels));
  ASSERT_LT(ComputeError(projected), 2e-7);
}

TEST_F(CameraFixture, SimpleRadialIsConsistent) {
  geometry::Camera camera = geometry::Camera::CreateSimpleRadialCamera(
      focal, 1.0, principal_point, distortion_radial[0]);
  const auto projected = camera.ProjectMany(camera.BearingsMany(pixels));
  ASSERT_LT(ComputeError(projected), 2e-7);
}

TEST_F(CameraFixture, SphericalReturnCorrectTypes) {
  geometry::Camera camera = geometry::Camera::CreateSphericalCamera();
  const auto types = camera.GetParametersTypes();
  ASSERT_EQ(1, types.size());
  ASSERT_EQ(geometry::Camera::Parameters::None, types[0]);
}

TEST_F(CameraFixture, PerspectiveReturnCorrectTypes) {
  geometry::Camera camera = geometry::Camera::CreatePerspectiveCamera(
      focal, distortion[0], distortion[1]);
  const auto types = camera.GetParametersTypes();
  const auto expected = std::vector<geometry::Camera::Parameters>(
      {geometry::Camera::Parameters::K1, geometry::Camera::Parameters::K2,
       geometry::Camera::Parameters::Focal});
  ASSERT_THAT(expected, ::testing::ContainerEq(types));
}

TEST_F(CameraFixture, FisheyeReturnCorrectTypes) {
  geometry::Camera camera = geometry::Camera::CreateFisheyeCamera(
      focal, distortion[0], distortion[1]);
  const auto types = camera.GetParametersTypes();
  const auto expected = std::vector<geometry::Camera::Parameters>(
      {geometry::Camera::Parameters::K1, geometry::Camera::Parameters::K2,
       geometry::Camera::Parameters::Focal});
  ASSERT_THAT(expected, ::testing::ContainerEq(types));
}

TEST_F(CameraFixture, FisheyeOpencvReturnCorrectTypes) {
  geometry::Camera camera = geometry::Camera::CreateFisheyeOpencvCamera(
      focal, 1.0, principal_point, distortion_fisheye);
  const auto types = camera.GetParametersTypes();
  const auto expected = std::vector<geometry::Camera::Parameters>(
      {geometry::Camera::Parameters::K1, geometry::Camera::Parameters::K2,
       geometry::Camera::Parameters::K3, geometry::Camera::Parameters::K4,
       geometry::Camera::Parameters::Focal,
       geometry::Camera::Parameters::AspectRatio,
       geometry::Camera::Parameters::Cx, geometry::Camera::Parameters::Cy});
  ASSERT_THAT(expected, ::testing::ContainerEq(types));
}

TEST_F(CameraFixture, Fisheye62ReturnCorrectTypes) {
  geometry::Camera camera = geometry::Camera::CreateFisheye62Camera(
      focal, 1.0, principal_point, distortion_fisheye62);
  const auto types = camera.GetParametersTypes();
  const auto expected = std::vector<geometry::Camera::Parameters>(
      {geometry::Camera::Parameters::K1, geometry::Camera::Parameters::K2,
       geometry::Camera::Parameters::K3, geometry::Camera::Parameters::K4,
       geometry::Camera::Parameters::K5, geometry::Camera::Parameters::K6,
       geometry::Camera::Parameters::P1, geometry::Camera::Parameters::P2,
       geometry::Camera::Parameters::Focal,
       geometry::Camera::Parameters::AspectRatio,
       geometry::Camera::Parameters::Cx, geometry::Camera::Parameters::Cy});
  ASSERT_THAT(expected, ::testing::ContainerEq(types));
}

TEST_F(CameraFixture, Fisheye624ReturnCorrectTypes) {
  geometry::Camera camera = geometry::Camera::CreateFisheye624Camera(
      focal, 1.0, principal_point, distortion_fisheye624);
  const auto types = camera.GetParametersTypes();
  const auto expected = std::vector<geometry::Camera::Parameters>(
      {geometry::Camera::Parameters::K1, geometry::Camera::Parameters::K2,
       geometry::Camera::Parameters::K3, geometry::Camera::Parameters::K4,
       geometry::Camera::Parameters::K5, geometry::Camera::Parameters::K6,
       geometry::Camera::Parameters::P1, geometry::Camera::Parameters::P2,
       geometry::Camera::Parameters::S0, geometry::Camera::Parameters::S1,
       geometry::Camera::Parameters::S2, geometry::Camera::Parameters::S3,
       geometry::Camera::Parameters::Focal,
       geometry::Camera::Parameters::AspectRatio,
       geometry::Camera::Parameters::Cx, geometry::Camera::Parameters::Cy});
  ASSERT_THAT(expected, ::testing::ContainerEq(types));
}

TEST_F(CameraFixture, RadialReturnCorrectTypes) {
  geometry::Camera camera = geometry::Camera::CreateRadialCamera(
      focal, 1.0, principal_point, distortion_radial);
  const auto types = camera.GetParametersTypes();
  const auto expected = std::vector<geometry::Camera::Parameters>(
      {geometry::Camera::Parameters::K1, geometry::Camera::Parameters::K2,
       geometry::Camera::Parameters::Focal,
       geometry::Camera::Parameters::AspectRatio,
       geometry::Camera::Parameters::Cx, geometry::Camera::Parameters::Cy});
  ASSERT_THAT(expected, ::testing::ContainerEq(types));
}

TEST_F(CameraFixture, SimpleRadialReturnCorrectTypes) {
  geometry::Camera camera = geometry::Camera::CreateSimpleRadialCamera(
      focal, 1.0, principal_point, distortion_radial[0]);
  const auto types = camera.GetParametersTypes();
  const auto expected = std::vector<geometry::Camera::Parameters>(
      {geometry::Camera::Parameters::K1, geometry::Camera::Parameters::Focal,
       geometry::Camera::Parameters::AspectRatio,
       geometry::Camera::Parameters::Cx, geometry::Camera::Parameters::Cy});
  ASSERT_THAT(expected, ::testing::ContainerEq(types));
}

TEST_F(CameraFixture, BrownReturnCorrectTypes) {
  geometry::Camera camera = geometry::Camera::CreateBrownCamera(
      focal, 1.0, principal_point, distortion_brown);
  const auto types = camera.GetParametersTypes();

  const auto expected = std::vector<geometry::Camera::Parameters>(
      {geometry::Camera::Parameters::K1, geometry::Camera::Parameters::K2,
       geometry::Camera::Parameters::K3, geometry::Camera::Parameters::P1,
       geometry::Camera::Parameters::P2, geometry::Camera::Parameters::Focal,
       geometry::Camera::Parameters::AspectRatio,
       geometry::Camera::Parameters::Cx, geometry::Camera::Parameters::Cy});
  ASSERT_THAT(expected, ::testing::ContainerEq(types));
}

TEST_F(CameraFixture, DualReturnCorrectTypes) {
  geometry::Camera camera = geometry::Camera::CreateDualCamera(
      0.5, focal, distortion[0], distortion[1]);
  const auto types = camera.GetParametersTypes();
  const auto expected = std::vector<geometry::Camera::Parameters>(
      {geometry::Camera::Parameters::Transition,
       geometry::Camera::Parameters::K1, geometry::Camera::Parameters::K2,
       geometry::Camera::Parameters::Focal});
  ASSERT_THAT(expected, ::testing::ContainerEq(types));
}

TEST_F(CameraFixture, PerspectiveReturnCorrectValues) {
  geometry::Camera camera = geometry::Camera::CreatePerspectiveCamera(
      focal, distortion[0], distortion[1]);
  const auto values = camera.GetParametersValues();

  VecXd expected(3);
  expected << distortion[0], distortion[1], focal;
  ASSERT_EQ(expected, values);
}

TEST_F(CameraFixture, FisheyeReturnCorrectValues) {
  geometry::Camera camera = geometry::Camera::CreateFisheyeCamera(
      focal, distortion[0], distortion[1]);
  const auto values = camera.GetParametersValues();

  VecXd expected(3);
  expected << distortion[0], distortion[1], focal;
  ASSERT_EQ(expected, values);
}

TEST_F(CameraFixture, FisheyeOpencvReturnCorrectValues) {
  geometry::Camera camera = geometry::Camera::CreateFisheyeOpencvCamera(
      focal, 1.0, principal_point, distortion_fisheye);
  const auto values = camera.GetParametersValues();

  VecXd expected(8);
  expected << distortion_fisheye, focal, 1.0, principal_point;
  ASSERT_EQ(expected, values);
}

TEST_F(CameraFixture, Fisheye62ReturnCorrectValues) {
  geometry::Camera camera = geometry::Camera::CreateFisheye62Camera(
      focal, 1.0, principal_point, distortion_fisheye62);
  const auto values = camera.GetParametersValues();

  VecXd expected(12);
  expected << distortion_fisheye62, focal, 1.0, principal_point;
  ASSERT_EQ(expected, values);
}

TEST_F(CameraFixture, Fisheye624ReturnCorrectValues) {
  geometry::Camera camera = geometry::Camera::CreateFisheye624Camera(
      focal, 1.0, principal_point, distortion_fisheye624);
  const auto values = camera.GetParametersValues();

  VecXd expected(16);
  expected << distortion_fisheye624, focal, 1.0, principal_point;
  ASSERT_EQ(expected, values);
}

TEST_F(CameraFixture, RadialReturnCorrectValues) {
  geometry::Camera camera = geometry::Camera::CreateRadialCamera(
      focal, 1.0, principal_point, distortion_radial);
  const auto values = camera.GetParametersValues();

  VecXd expected(6);
  expected << distortion_radial, focal, 1.0, principal_point;
  ASSERT_EQ(expected, values);
}

TEST_F(CameraFixture, SimpleRadialReturnCorrectValues) {
  geometry::Camera camera = geometry::Camera::CreateSimpleRadialCamera(
      focal, 1.0, principal_point, distortion_radial[0]);
  const auto values = camera.GetParametersValues();

  VecXd expected(5);
  expected << distortion_radial[0], focal, 1.0, principal_point;
  ASSERT_EQ(expected, values);
}

TEST_F(CameraFixture, BrownReturnCorrectValues) {
  geometry::Camera camera = geometry::Camera::CreateBrownCamera(
      focal, 1.0, principal_point, distortion_brown);
  const auto values = camera.GetParametersValues();

  VecXd expected(9);
  expected << distortion_brown, focal, 1.0, principal_point;
  ASSERT_EQ(expected, values);
}

TEST_F(CameraFixture, DualReturnCorrectValues) {
  geometry::Camera camera = geometry::Camera::Camera::CreateDualCamera(
      0.5, focal, distortion[0], distortion[1]);
  const auto values = camera.GetParametersValues();

  VecXd expected(4);
  expected << 0.5, distortion[0], distortion[1], focal;
  ASSERT_EQ(expected, values);
}

TEST_F(CameraFixture, PerspectiveReturnCorrectK) {
  geometry::Camera camera = geometry::Camera::CreatePerspectiveCamera(
      focal, distortion[0], distortion[1]);

  Mat3d expected = Mat3d::Identity();
  expected(0, 0) = expected(1, 1) = focal;

  ASSERT_TRUE(expected.isApprox(camera.GetProjectionMatrix()));
}

TEST_F(CameraFixture, CanSetParameter) {
  geometry::Camera camera = geometry::Camera::CreatePerspectiveCamera(
      focal, distortion[0], distortion[1]);

  const double new_focal = 0.1;
  camera.SetParameterValue(geometry::Camera::Parameters::Focal, new_focal);
  const auto values = camera.GetParametersValues();

  VecXd expected(3);
  expected << distortion[0], distortion[1], new_focal;
  ASSERT_EQ(expected, values);
}

TEST_F(CameraFixture, DoSetParameter) {
  geometry::Camera camera = geometry::Camera::CreatePerspectiveCamera(
      focal, distortion[0], distortion[1]);
  const auto values = camera.GetParametersValues();
  EXPECT_NO_THROW(
      camera.SetParameterValue(geometry::Camera::Parameters::Focal, 0.1));
}

TEST_F(CameraFixture, DoesntSetParameter) {
  geometry::Camera camera = geometry::Camera::CreatePerspectiveCamera(
      focal, distortion[0], distortion[1]);
  const auto values = camera.GetParametersValues();
  EXPECT_ANY_THROW(
      camera.SetParameterValue(geometry::Camera::Parameters::AspectRatio, 0.0));
}

TEST_F(CameraFixture, PerspectiveReturnCorrectKScaled) {
  geometry::Camera camera = geometry::Camera::CreatePerspectiveCamera(
      focal, distortion[0], distortion[1]);

  Mat3d expected = Mat3d::Identity();
  expected(0, 0) = expected(1, 1) = focal * std::max(pixel_width, pixel_height);
  expected(0, 2) = pixel_width * 0.5;
  expected(1, 2) = pixel_height * 0.5;

  ASSERT_TRUE(expected.isApprox(
      camera.GetProjectionMatrixScaled(pixel_width, pixel_height)));
}

TEST_F(CameraFixture, BrownReturnCorrectK) {
  geometry::Camera camera = geometry::Camera::CreateBrownCamera(
      focal, new_ar, principal_point, distortion_brown);

  Mat3d expected = Mat3d::Identity();
  expected(0, 0) = focal;
  expected(1, 1) = new_ar * focal;
  expected(0, 2) = principal_point(0);
  expected(1, 2) = principal_point(1);

  ASSERT_TRUE(expected.isApprox(camera.GetProjectionMatrix()));
}

TEST_F(CameraFixture, BrownReturnCorrectKScaled) {
  geometry::Camera camera = geometry::Camera::CreateBrownCamera(
      focal, new_ar, principal_point, distortion_brown);

  Mat3d expected = Mat3d::Identity();
  const auto normalizer = std::max(pixel_width, pixel_height);
  expected(0, 0) = focal * normalizer;
  expected(1, 1) = (focal * new_ar) * normalizer;
  expected(0, 2) = principal_point(0) * normalizer + pixel_width * 0.5;
  expected(1, 2) = principal_point(1) * normalizer + pixel_height * 0.5;

  ASSERT_TRUE(expected.isApprox(
      camera.GetProjectionMatrixScaled(pixel_width, pixel_height)));
}

TEST_F(CameraFixture, FisheyeOpencvAsFisheye62) {
  Eigen::Matrix<double, 8, 1> dist_62;
  dist_62 << distortion_fisheye[0], distortion_fisheye[1],
      distortion_fisheye[2], distortion_fisheye[3], 0, 0, 0, 0;
  geometry::Camera cam62 = geometry::Camera::CreateFisheye62Camera(
      focal, 1.0, principal_point, dist_62);
  geometry::Camera camcv = geometry::Camera::CreateFisheyeOpencvCamera(
      focal, 1.0, principal_point, distortion_fisheye);
  const MatX3d bear1 = cam62.BearingsMany(pixels);
  const MatX3d bear2 = camcv.BearingsMany(pixels);
  ASSERT_TRUE(bear1.isApprox(bear2, 1e-6));
  const auto proj1 = cam62.ProjectMany(bear1);
  const auto proj2 = camcv.ProjectMany(bear2);
  ASSERT_TRUE(proj1.isApprox(proj2, 1e-6));
}

TEST_F(CameraFixture, FisheyeOpencvAsFisheye624) {
  Eigen::Matrix<double, 12, 1> dist_624;
  dist_624 << distortion_fisheye[0], distortion_fisheye[1],
      distortion_fisheye[2], distortion_fisheye[3], 0, 0, 0, 0, 0, 0, 0, 0;
  geometry::Camera cam624 = geometry::Camera::CreateFisheye624Camera(
      focal, 1.0, principal_point, dist_624);
  geometry::Camera camcv = geometry::Camera::CreateFisheyeOpencvCamera(
      focal, 1.0, principal_point, distortion_fisheye);
  const MatX3d bear1 = cam624.BearingsMany(pixels);
  const MatX3d bear2 = camcv.BearingsMany(pixels);
  ASSERT_TRUE(bear1.isApprox(bear2, 1e-6));
  const auto proj1 = cam624.ProjectMany(bear1);
  const auto proj2 = camcv.ProjectMany(bear2);
  ASSERT_TRUE(proj1.isApprox(proj2, 1e-6));
}

TEST_F(CameraFixture, SimpleRadialAsRadial) {
  const double k1 = distortion_radial[0];
  geometry::Camera cam_simple = geometry::Camera::CreateSimpleRadialCamera(
      focal, 1.0, principal_point, k1);
  geometry::Camera cam_radial = geometry::Camera::CreateRadialCamera(
      focal, 1.0, principal_point, Vec2d(k1, 0));
  const MatX3d bear1 = cam_simple.BearingsMany(pixels);
  const MatX3d bear2 = cam_radial.BearingsMany(pixels);
  ASSERT_TRUE(bear1.isApprox(bear2, 1e-6));
  const auto proj1 = cam_simple.ProjectMany(bear1);
  const auto proj2 = cam_radial.ProjectMany(bear2);
  ASSERT_TRUE(proj1.isApprox(proj2, 1e-6));
}

TEST(Camera, TestPixelNormalizedCoordinatesConversion) {
  constexpr auto width{640}, height{480};
  auto cam = geometry::Camera::CreatePerspectiveCamera(1.0, 0.0, 0.0);
  cam.width = width;
  cam.height = height;
  const auto inv_normalizer = 1.0 / std::max(width, height);
  const Vec2d px_coord_def(200, 100);
  const Vec2d norm_coord_comp = cam.PixelToNormalizedCoordinates(px_coord_def);
  ASSERT_EQ(norm_coord_comp[0],
            (px_coord_def[0] - (width - 1) / 2.0) * inv_normalizer);
  ASSERT_EQ(norm_coord_comp[1],
            (px_coord_def[1] - (height - 1) / 2.0) * inv_normalizer);
  const Vec2d px_coord_comp = cam.NormalizedToPixelCoordinates(norm_coord_comp);
  ASSERT_EQ(px_coord_comp, px_coord_def);

  const Vec2d norm_coord_static =
      geometry::Camera::PixelToNormalizedCoordinates(px_coord_def, width,
                                                     height);
  ASSERT_EQ(norm_coord_comp[0], norm_coord_static[0]);
  ASSERT_EQ(norm_coord_comp[1], norm_coord_static[1]);
  const Vec2d px_coord_static = geometry::Camera::NormalizedToPixelCoordinates(
      norm_coord_static, width, height);
  ASSERT_EQ(px_coord_comp[0], px_coord_static[0]);
  ASSERT_EQ(px_coord_comp[1], px_coord_static[1]);
}

TEST(Camera, TestCameraProjectionTypes) {
  ASSERT_EQ(
      geometry::Camera::GetProjectionString(geometry::ProjectionType::BROWN),
      "brown");
  ASSERT_EQ(geometry::Camera::GetProjectionString(
                geometry::ProjectionType::PERSPECTIVE),
            "perspective");
  ASSERT_EQ(
      geometry::Camera::GetProjectionString(geometry::ProjectionType::DUAL),
      "dual");
  ASSERT_EQ(
      geometry::Camera::GetProjectionString(geometry::ProjectionType::FISHEYE),
      "fisheye");
  ASSERT_EQ(geometry::Camera::GetProjectionString(
                geometry::ProjectionType::FISHEYE_OPENCV),
            "fisheye_opencv");
  ASSERT_EQ(geometry::Camera::GetProjectionString(
                geometry::ProjectionType::FISHEYE62),
            "fisheye62");
  ASSERT_EQ(geometry::Camera::GetProjectionString(
                geometry::ProjectionType::FISHEYE624),
            "fisheye624");
  ASSERT_EQ(geometry::Camera::GetProjectionString(
                geometry::ProjectionType::SPHERICAL),
            "spherical");
  ASSERT_EQ(
      geometry::Camera::GetProjectionString(geometry::ProjectionType::RADIAL),
      "radial");
  ASSERT_EQ(geometry::Camera::GetProjectionString(
                geometry::ProjectionType::SIMPLE_RADIAL),
            "simple_radial");

  // One test to ensure that it is passed correctly from the cam to the static
  // method
  ASSERT_EQ(
      geometry::Camera::CreatePerspectiveCamera(0, 0, 0).GetProjectionString(),
      "perspective");
}


#define BOOST_TEST_MODULE "test_repulsive_gocontact_potential"

#ifdef BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#else
#include <boost/test/included/unit_test.hpp>
#endif

#include <mjolnir/forcefield/local/GoContactRepulsivePotential.hpp>
#include <iomanip>

BOOST_AUTO_TEST_CASE(GoContactRepulsive_double)
{
    using real_type = double;
    constexpr std::size_t N = 1000;
    constexpr real_type   h = 1e-6;

    const real_type e  = 1.0;
    const real_type r0 = 5.0;

    mjolnir::GoContactRepulsivePotential<real_type> pot(e, r0);

    {
        const real_type x_min = 0.8 * r0;
        const real_type x_max = pot.cutoff();
        const real_type dx = (x_max - x_min) / N;
        for(std::size_t i=0; i<N; ++i)
        {
            const real_type x    = x_min + i * dx;
            const real_type pot1 = pot.potential(x + h);
            const real_type pot2 = pot.potential(x - h);
            const real_type dpot = (pot1 - pot2) / (2 * h);
            const real_type deri = pot.derivative(x);

            BOOST_TEST(dpot == deri, boost::test_tools::tolerance(h));
        }
    }
    {
        const real_type x_min = pot.cutoff() + 1.01;
        const real_type x_max = 2 * pot.cutoff();
        const real_type dx = (x_max - x_min) / N;
        for(std::size_t i=0; i<N; ++i)
        {
            const real_type x = x_min + i * dx;
            BOOST_TEST(pot.potential(x)  == 0.0, boost::test_tools::tolerance(h));
            BOOST_TEST(pot.derivative(x) == 0.0, boost::test_tools::tolerance(h));
        }
    }
}

BOOST_AUTO_TEST_CASE(GoContactRepulsive_float)
{
    using real_type = float;
    constexpr std::size_t N = 100;
    constexpr real_type   h = 1e-3;

    const real_type e  = 1.0;
    const real_type r0 = 5.0;

    mjolnir::GoContactRepulsivePotential<real_type> pot(e, r0);
    {
        const real_type x_min = 0.8f * r0;
        const real_type x_max = pot.cutoff();
        const real_type dx = (x_max - x_min) / N;
        for(std::size_t i=0; i<N; ++i)
        {
            const real_type x    = x_min + i * dx;
            const real_type pot1 = pot.potential(x + h);
            const real_type pot2 = pot.potential(x - h);
            const real_type dpot = (pot1 - pot2) / (2 * h);
            const real_type deri = pot.derivative(x);

            BOOST_TEST_MESSAGE("i = " << i << ", x = " << x);
            BOOST_TEST(dpot == deri, boost::test_tools::tolerance(1e-2f));
        }
    }
    {
        const real_type x_min = pot.cutoff() + 1.01;
        const real_type x_max = 2 * pot.cutoff();
        const real_type dx = (x_max - x_min) / N;
        for(std::size_t i=0; i<N; ++i)
        {
            const real_type x = x_min + i * dx;
            BOOST_TEST(pot.potential(x)  == 0.0, boost::test_tools::tolerance(h));
            BOOST_TEST(pot.derivative(x) == 0.0, boost::test_tools::tolerance(h));
        }
    }
}


/*
 * helpers.hpp
 *
 *  Created on: Apr 22, 2021
 *      Author: jelavice
 */

#pragma once

#include <ros/ros.h>
#include <Eigen/Dense>
#include "icp_localization/common/typedefs.hpp"
#include <geometry_msgs/Pose.h>
#include "pointmatcher_ros/transform.h"
#include <memory>

namespace icp_loco {

Eigen::Vector3d getPositionFromParameterServer(const ros::NodeHandle &nh, const std::string &prefix);
Eigen::Quaterniond getOrientationFromParameterServer(const ros::NodeHandle &nh, const std::string &prefix, bool isRPYInDegrees=false);

template<typename Scalar>
Eigen::Matrix<Scalar,-1,-1> getTransformationMatrix(const Eigen::Matrix<Scalar,3,1> &position,
                                                         const Eigen::Quaternion<Scalar> &orientation);

template<typename Scalar>
void getPositionAndOrientation(const Eigen::Matrix<Scalar,-1,-1> &T, Eigen::Matrix<Scalar,3,1> *position,
                                                Eigen::Quaternion<Scalar> *orientation);

Eigen::Quaternionf toFloat(const Eigen::Quaterniond &q);
Eigen::Quaterniond toDouble(const Eigen::Quaternionf &q);

Eigen::MatrixXf toFloat(const Eigen::MatrixXd &m);
Eigen::MatrixXd toDouble(const Eigen::MatrixXf &m);

} // namespace icp_loco

template<typename Scalar>
Eigen::Matrix<Scalar,-1,-1> icp_loco::getTransformationMatrix(const Eigen::Matrix<Scalar,3,1> &position,
                                                         const Eigen::Quaternion<Scalar> &orientation)
{
  Eigen::Matrix<Scalar,-1,-1> T(4,4);
  T.setIdentity();
  T.block(0, 3, 3, 1) = position;
  T.block(0, 0, 3, 3) = orientation.toRotationMatrix();
  return T;
}

template<typename Scalar>
void icp_loco::getPositionAndOrientation(const Eigen::Matrix<Scalar,-1,-1> &T, Eigen::Matrix<Scalar,3,1> *position,
                                                Eigen::Quaternion<Scalar> *orientation)
{
  geometry_msgs::Pose pose = pointmatcher_ros::eigenMatrixToPoseMsg<Scalar>(T);
  *position = Eigen::Matrix<Scalar,3,1>(pose.position.x, pose.position.y, pose.position.z);
  const auto &q = pose.orientation;
  *orientation = Eigen::Quaternion<Scalar>(q.w, q.x, q.y, q.z);
  orientation->normalize();
}


/*=========================================================================
 *
 *  Copyright David Doria 2012 daviddoria@gmail.com
 *
 *  Licensed under the Apache License, Version 2.0 (the "License");
 *  you may not use this file except in compliance with the License.
 *  You may obtain a copy of the License at
 *
 *         http://www.apache.org/licenses/LICENSE-2.0.txt
 *
 *  Unless required by applicable law or agreed to in writing, software
 *  distributed under the License is distributed on an "AS IS" BASIS,
 *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 *  See the License for the specific language governing permissions and
 *  limitations under the License.
 *
 *=========================================================================*/

#ifndef CorrelationAcceptanceVisitor_HPP
#define CorrelationAcceptanceVisitor_HPP

#include <boost/graph/graph_traits.hpp>

// Parent class
#include "Visitors/AcceptanceVisitors/AcceptanceVisitorParent.h"
#include "Visitors/AcceptanceVisitors/VarianceFunctor.hpp"
#include "Visitors/AcceptanceVisitors/AverageFunctor.hpp"

// Custom
#include "ITKHelpers/ITKHelpers.h"

// ITK
#include "itkAddImageFilter.h"
#include "itkMultiplyImageFilter.h"
#include "itkImageRegion.h"

/**

 */
template <typename TGraph, typename TImage>
struct CorrelationAcceptanceVisitor : public AcceptanceVisitorParent<TGraph>
{
  TImage* Image;
  Mask* MaskImage;

  const unsigned int HalfWidth;

  float Threshold;

  typedef typename boost::graph_traits<TGraph>::vertex_descriptor VertexDescriptorType;

  CorrelationAcceptanceVisitor(TImage* const image, Mask* const mask,
                               const unsigned int halfWidth, const float threshold, const std::string& visitorName = "CorrelationAcceptanceVisitor") :
  AcceptanceVisitorParent<TGraph>(visitorName), Image(image), MaskImage(mask),
    HalfWidth(halfWidth), Threshold(threshold)
  {

  }

  bool AcceptMatch(VertexDescriptorType target, VertexDescriptorType source, float& computedEnergy) const override
  {
    itk::Index<2> targetPixel = ITKHelpers::CreateIndex(target);
    itk::ImageRegion<2> targetRegion = ITKHelpers::GetRegionInRadiusAroundPixel(targetPixel, HalfWidth);

    itk::Index<2> sourcePixel = ITKHelpers::CreateIndex(source);
    itk::ImageRegion<2> sourceRegion = ITKHelpers::GetRegionInRadiusAroundPixel(sourcePixel, HalfWidth);

    typedef itk::Image<float, 2> FloatImageType;

    typedef itk::VectorMagnitudeImageFilter<TImage, FloatImageType> VectorMagnitudeFilterType;
    typename VectorMagnitudeFilterType::Pointer magnitudeFilter = VectorMagnitudeFilterType::New();
    magnitudeFilter->SetInput(Image);
    magnitudeFilter->Update();

    std::vector<itk::Offset<2> > validOffsets = MaskImage->GetValidOffsetsInRegion(targetRegion);

    FloatImageType::Pointer sourceImage = FloatImageType::New();
//     sourceImage->SetRegions(ITKHelpers::CornerRegion(sourceRegion.GetSize()));
//     sourceImage->Allocate();
    ITKHelpers::ExtractRegion(magnitudeFilter->GetOutput(), sourceRegion, sourceImage.GetPointer());

    FloatImageType::Pointer targetImage = FloatImageType::New();
//     sourceImage->SetRegions(ITKHelpers::CornerRegion(targetRegion.GetSize()));
//     sourceImage->Allocate();
    ITKHelpers::ExtractRegion(magnitudeFilter->GetOutput(), targetRegion, targetImage.GetPointer());

    std::vector<itk::Index<2> > validIndices = ITKHelpers::OffsetsToIndices(validOffsets);

    VarianceFunctor varianceFunctor;
    AverageFunctor averageFunctor;
    /////////// Target region //////////
    std::vector<FloatImageType::PixelType> validPixelsTargetRegion = ITKHelpers::GetPixelValues(targetImage.GetPointer(), validIndices);
    typename TypeTraits<FloatImageType::PixelType>::LargerType targetMean = averageFunctor(validPixelsTargetRegion);
    typename TypeTraits<FloatImageType::PixelType>::LargerType targetStandardDeviation = sqrt(varianceFunctor(validPixelsTargetRegion));

    typedef itk::AddImageFilter <FloatImageType, FloatImageType, FloatImageType> AddImageFilterType;
    AddImageFilterType::Pointer targetAddImageFilter = AddImageFilterType::New();
    targetAddImageFilter->SetInput(targetImage);
    targetAddImageFilter->SetConstant2(-1.0f * targetMean);
    targetAddImageFilter->Update();

    typedef itk::MultiplyImageFilter<FloatImageType, FloatImageType, FloatImageType> MultiplyImageFilterType;
    MultiplyImageFilterType::Pointer targetMultiplyImageFilter = MultiplyImageFilterType::New();
    targetMultiplyImageFilter->SetInput(targetImage);
    targetMultiplyImageFilter->SetConstant(1.0f/targetStandardDeviation);
    targetMultiplyImageFilter->Update();

    /////////// Source region //////////
    std::vector<FloatImageType::PixelType> validPixelsSourceRegion = ITKHelpers::GetPixelValues(sourceImage.GetPointer(), validIndices);
    typename TypeTraits<FloatImageType::PixelType>::LargerType sourceMean = averageFunctor(validPixelsSourceRegion);
    typename TypeTraits<FloatImageType::PixelType>::LargerType sourceStandardDeviation = sqrt(varianceFunctor(validPixelsSourceRegion));

    AddImageFilterType::Pointer sourceAddImageFilter = AddImageFilterType::New();
    sourceAddImageFilter->SetInput(sourceImage);
    sourceAddImageFilter->SetConstant2(-1.0f * sourceMean);
    sourceAddImageFilter->Update();

    MultiplyImageFilterType::Pointer sourceMultiplyImageFilter = MultiplyImageFilterType::New();
    sourceMultiplyImageFilter->SetInput(sourceImage);
    sourceMultiplyImageFilter->SetConstant(1.0f/sourceStandardDeviation);
    sourceMultiplyImageFilter->Update();

    // Initialize
    computedEnergy = 0.0f;

    for(std::vector<itk::Index<2> >::const_iterator iter = validIndices.begin(); iter != validIndices.end(); ++iter)
    {
      computedEnergy += (sourceMultiplyImageFilter->GetOutput()->GetPixel(*iter) * targetMultiplyImageFilter->GetOutput()->GetPixel(*iter));
    }

    computedEnergy /= static_cast<float>(validIndices.size());

    if(computedEnergy < Threshold)
      {
      std::cout << this->VisitorName << ": Match accepted (" << computedEnergy << " is less than " << Threshold << ")" << std::endl << std::endl;
      return true;
      }
    else
      {
      std::cout << this->VisitorName << ": Match rejected (" << computedEnergy << " is greater than " << Threshold << ")" << std::endl << std::endl;
      return false;
      }
  };

};

#endif


/*
 * Copyright (C) 2012-2016 Open Source Robotics Foundation
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
*/
/* Desc: Random number generator
 * Author: Nate Koenig
 * Date: 27 May 2009
 */

#ifndef _RAND_HH_
#define _RAND_HH_

#include <boost/random.hpp>
#include "gazebo/util/system.hh"

namespace gazebo
{
  namespace math
  {
    /// \def GeneratorType
    /// \brief boost::mt19937
    typedef boost::mt19937 GeneratorType;
    /// \def UniformRealDist
    /// \brief boost::uniform_real<double>
    typedef boost::uniform_real<double> UniformRealDist;
    /// \def NormalRealDist
    /// \brief boost::normal_distribution<double>
    typedef boost::normal_distribution<double> NormalRealDist;
    /// \def UniformIntDist
    /// \brief boost::uniform_int<int>
    typedef boost::uniform_int<int> UniformIntDist;

    /// \def URealGen
    /// \brief boost::variate_generator with GeneratorType and UniformRealDist
    typedef boost::variate_generator<GeneratorType&, UniformRealDist > URealGen;
    /// \def NRealGen
    /// \brief boost::variate_generator with GeneratorType and NormalRealDist
    typedef boost::variate_generator<GeneratorType&, NormalRealDist > NRealGen;
    /// \def UIntGen
    /// \brief boost::variate_generator with GeneratorType and UniformIntDist
    typedef boost::variate_generator<GeneratorType&, UniformIntDist > UIntGen;

    /// \addtogroup gazebo_math
    /// \{

    /// \class Rand Rand.hh gzmath/gzmath.hh
    /// \brief Random number generator class
    class GZ_MATH_VISIBLE Rand
    {
      /// \brief Set the seed value.
      /// \param[in] _seed The seed used to initialize the randon number
      /// generator.
      public: static void SetSeed(uint32_t _seed);

      /// \brief Get the seed value.
      /// \return The seed value used to initialize the random number
      /// generator.
      public: static uint32_t GetSeed();

      /// \brief Get a double from a uniform distribution
      /// \param[in] _min Minimum bound for the random number
      /// \param[in] _max Maximum bound for the random number
      public: static double GetDblUniform(double _min = 0, double _max = 1);

      /// \brief Get a double from a normal distribution
      /// \param[in] _mean Mean value for the distribution
      /// \param[in] _sigma Sigma value for the distribution
      public: static double GetDblNormal(double _mean = 0, double _sigma = 1);

      /// \brief Get a integer from a uniform distribution
      /// \param[in] _min Minimum bound for the random number
      /// \param[in] _max Maximum bound for the random number
      public: static int GetIntUniform(int _min, int _max);

      /// \brief Get a double from a normal distribution
      /// \param[in] _mean Mean value for the distribution
      /// \param[in] _sigma Sigma value for the distribution
      public: static int GetIntNormal(int _mean, int _sigma);

      // The random number generator
      private: static GeneratorType *randGenerator;

      private: static uint32_t seed;
    };
    /// \}
  }
}
#endif


// Copyright (C) 2006-2009 Dmitry Bufistov and Andrey Parfenov

// 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)

#include <cassert>
#include <ctime>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_real.hpp>
#include <boost/graph/howard_cycle_ratio.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/connected_components.hpp>
#include <boost/graph/strong_components.hpp>
#include <boost/graph/random.hpp>
#include <boost/graph/erdos_renyi_generator.hpp>
#include <boost/random/linear_congruential.hpp>
#include <boost/random.hpp>
#include <boost/config.hpp>
#include <stack>
#include <iostream>
#include <vector>

/**
 * @author Dmitry Bufistov
 * @author Andrey Parfenov
 */

using namespace boost;

//==============================================================================
struct node_properties
{
  int component;
};
struct edge_properties
{
};
struct graph_properties
{
  int number_of_components;
};

// --------------------------------------
// typedef boost::adjacency_list<
//   boost::vecS,
//   boost::vecS,
//   boost::directedS,
//   node_properties,
//   edge_properties,
//   graph_properties> graph_t;

typedef adjacency_list<
    vecS, vecS, directedS,
    node_properties,
    property<
        edge_weight_t, double,
        property<edge_weight2_t, double> >,
    graph_properties
> graph_t;

template <typename TG>
void gen_rand_graph(TG &g, size_t nV, size_t nE)
{
    g.clear();
    mt19937 rng;
    rng.seed(uint32_t(time(0)));
    boost::generate_random_graph(g, nV, nE, rng, true, true);
    boost::uniform_real<> ur(-1,10);
    boost::variate_generator<boost::mt19937&, boost::uniform_real<> >   ew1rg(rng, ur);
    randomize_property<edge_weight_t>(g, ew1rg);
    boost::uniform_int<size_t> uint(1,5);
    boost::variate_generator<boost::mt19937&, boost::uniform_int<size_t> >      ew2rg(rng, uint);
    randomize_property<edge_weight2_t>(g, ew2rg);
}

typedef typename boost::graph_traits<graph_t>::vertex_descriptor vertex_t;
typedef typename boost::graph_traits<graph_t>::edge_descriptor edge_t;

graph_t graph_local;
std::vector<vertex_t> vertices;

int  nn = 100;
int e = 15;
std::vector<bool> visited(nn, false);
std::vector<int> r_index(nn, 0);
std::vector<bool> inComponent(nn, false);
std::stack<int> S;
int vindex = 0;
int c = 0; // component number

//==============================================================================
void print_graph(const graph_t &graph_local)
{
  std::cout << "graph_local:" << std::endl;
  auto edges = boost::edges(graph_local);
  for (auto it = edges.first; it != edges.second; ++it)
  {
    std::cout << boost::source(*it, graph_local) << " -> "
              << boost::target(*it, graph_local) << std::endl;
  }
}

void readSCC(graph_t graph_in, int nn)
{
  std::vector<int> cc(nn);
  int num = strong_components(graph_in, make_iterator_property_map(cc.begin(), get(boost::vertex_index, graph_in), cc[0]));

  std::cout << "Total number of components: " << num << std::endl;
  std::vector<int>::iterator i;
  for (i = cc.begin(); i != cc.end(); ++i)
    std::cout << "Vertex " << i - cc.begin()
              << " is in component " << *i << std::endl;
  //std::cout << "" << i - cc.begin();
}

void visit(int v)
{
  bool root = true;
  visited[v] = true;
  r_index[v] = vindex;
  vindex = vindex + 1;
  inComponent[v] = false;
  auto edges = boost::edges(graph_local);
  for (auto it = edges.first; it != edges.second; ++it)
  {
    int w = boost::target(*it, graph_local);
    int s = boost::source(*it, graph_local);

    if (v == s)
    {
      //std::cout << "\n Visit for node: " << v << ", edge: " << s << "->" << w << "\n";
      if (!visited[w])
      {
        visit(w);
      }

      for (auto i = r_index.begin(); i != r_index.end(); ++i)
        //bool check = !(inComponent[w]) && (r_index[w] < r_index[v]);
        if (!(inComponent[w]) && (r_index[w] < r_index[v]))
        {
          r_index[v] = r_index[w];
          root = false;
        }
    }
  }

  if (root)
  {
    inComponent[v] = true;
    while (!S.empty())
    {
      while ((!S.empty()) && (r_index[v] <= r_index[S.top()]))
      {
        int w = S.top();
        S.pop();
        r_index[w] = c;
        inComponent[w] = true;
      }
    }
    r_index[v] = c;
    c = c + 1;
  }
  else
  {
    S.push(v);
  }
}
typedef boost::erdos_renyi_iterator<boost::minstd_rand, graph_t> ERGen;

//==============================================================================
int main()
{

  const double epsilon = 0.02;
  // double min_cr, max_cr; ///Minimum and maximum cycle ratio
  // typedef std::vector<graph_traits<graph_t>::edge_descriptor> ccReal_t;
  // ccReal_t cc; ///critical cycle

  // graph_t tgr;
  // property_map<graph_t, vertex_index_t>::type vim = get(vertex_index, tgr);
  // property_map<graph_t, edge_weight_t>::type ew1 = get(edge_weight, tgr);
  // property_map<graph_t, edge_weight2_t>::type ew2 = get(edge_weight2, tgr);

  // gen_rand_graph(tgr, nn, e);
  // std::cout << "Vertices number: " << num_vertices(tgr) << std::endl;
  // std::cout << "Edges number: " << num_edges(tgr) << std::endl;

  std::cout<<"test";
  // graph_local = tgr;
  // Create graph with nn nodes and edges with probability p
  boost::minstd_rand gen;
  graph_t gr(ERGen(gen, nn, epsilon), ERGen(), nn);
  graph_local = gr;
  // print_graph(graph_local);
  //random generation;
  // boost::mt19937 rng;
  // boost::generate_random_graph(graph_local, nn, e, rng);
  // size_t e = boost::num_edges(graph_local);
  // size_t n = boost::num_vertices(graph_local);

  // boost implementation
  // graph_local[boost::graph_bundle].number_of_components =
  //     boost::connected_components(graph_local,
  //                                 boost::get(&node_properties::component, graph_local));
  readSCC(graph_local, nn);

  for (int i = 0; i < nn; i++)
  {
    std::cout<<"\n i="<<i<<"\n";
    if (!visited[i])
    {
      visit(i);
    }
  }

  std::cout << "\nComponents : ----------" << c << std::endl;

  for (auto i = r_index.begin(); i != r_index.end(); ++i)
    std::cout << *i << ' ';
  //std::cout << "\n ***************************************" << std::endl;
  //std::cout << "\nTrue boost graph_local implementation : \n----------------------" << std::endl;

  return 0;
}


//---------------------------------------------------------------------------//
//!
//! \file   Utility_PQLAQuadrature.hpp
//! \author Philip Britt
//! \brief  PQLA Direction Quadrature handler declaration
//!
//---------------------------------------------------------------------------//

#ifndef UTILITY_PQLA_QUADRATURE
#define UTILITY_PQLA_QUADRATURE

// Boost Includes
#include <boost/serialization/version.hpp>
#include <boost/serialization/export.hpp>
#include <boost/serialization/shared_ptr.hpp>

// FRENSIE includes
#include "Utility_Vector.hpp"
#include "Utility_Tuple.hpp"
#include "Utility_Array.hpp"
#include "Utility_ExplicitSerializationTemplateInstantiationMacros.hpp"
#include "Utility_SerializationHelpers.hpp"

namespace Utility{

struct SphericalTriangle
{
  /*! Vector that contains a tuple representing spherical triangle parameters.
   * \details First element of the tuple is an array that contains the
   * 2-norm direction representing a vertex of the triangle.
   * Second is the length of the spherical triangle side opposite from that vertex
   * (or angle that the 2 other vertices of the triangle make with each other).
   * Third is the angle made from the sides of the spherical triangle from that vertex.
   * Note their order does NOT matter so long as they are consistent with the above definition
   */
  std::vector<std::tuple<std::array<double, 3>, double, double>> triangle_parameter_vector;

  //! Area of the triangle
  double area;

    //! Processes spherical triangle information
  void computeAndStoreTriangleParameters(std::vector<std::array<double, 3>>& vertex_vector);

  // Serialize the data
  template<typename Archive>
  void serialize(Archive& ar, const unsigned version)
  {
    ar & BOOST_SERIALIZATION_NVP(triangle_parameter_vector);
    ar & BOOST_SERIALIZATION_NVP(area);
  }
};

class PQLAQuadrature
{

  public:

  //! Constructor
  PQLAQuadrature(unsigned quadrature_order);

  //! Destructor
  ~PQLAQuadrature()
  { /* ... */ }

  //! Find which triangle bin a direction vector is in
  size_t findTriangleBin(const std::array<double, 3>& direction) const;

  //! Find which triangle bin a direction vector is in
  size_t findTriangleBin(const double x_direction, const double y_direction, const double z_direction) const;

  //! Return the order of the quadrature
  unsigned getQuadratureOrder() const;

  //! Get the total number of triangles
  size_t getNumberOfTriangles() const;

  //! Get the area of a specific spherical triangle
  double getTriangleArea(const size_t triangle_index) const;

  /*! Get a random direction from within a spherical triangle (evenly distributed probability) - reference here
   * \details reference: Stratified Sampling of Spherical Triangles, James Arvo, SIGGRAPH '95
   */
  void sampleIsotropicallyFromTriangle(std::array<double, 3>& direction_vector,
                                       const size_t triangle_index) const;

  const std::vector<SphericalTriangle>& getSphericalTriangleVector() const;

  private:

  //! Default constructor (for archiving)
  PQLAQuadrature()
  { /* ... */ }

  //! Vector operation for the purpose of sampleIsotropicallyFromTriangle
  void isotropicSamplingVectorOperation(const std::array<double, 3>& vertex_1,
                                        const std::array<double, 3>& vertex_2,
                                        std::array<double, 3>& result_vector) const;

  //! Converts direction vector to 1-norm normalized vector
  void normalizeVectorToOneNorm(const std::array<double, 3>& direction_2_norm,
                                 std::array<double, 3>& direction_1_norm) const;

  //! Converts direction vector to 1-norm normalized vector
  void normalizeVectorToOneNorm(const double x_direction,
                                  const double y_direction,
                                  const double z_direction,
                                  std::array<double, 3>& direction_1_norm) const;

  //! Take lower bounding plane indices of direction vector to form triangle index
  size_t calculatePositiveTriangleBinIndex(const unsigned i_x, const unsigned i_y, const unsigned i_z) const;

  //! Take direction signs to calculate secondary index
  size_t findSecondaryIndex(const bool x_sign, const bool y_sign, const bool z_sign) const;

  //! Quadrature order
  unsigned d_quadrature_order;

  //! Vector that stores POSITIVE DOMAIN spherical triangles
  std::vector<SphericalTriangle> d_spherical_triangle_vector;

  //! Serialize the data
  template<typename Archive>
  void serialize(Archive& ar, const unsigned version);

  //! Declare the boost serialization access object as a friend
  friend class boost::serialization::access;

};

// Serialize the data
template<typename Archive>
void PQLAQuadrature::serialize(Archive& ar, const unsigned version)
{
  // Serialize the member data
  ar & BOOST_SERIALIZATION_NVP(d_quadrature_order);
  ar & BOOST_SERIALIZATION_NVP(d_spherical_triangle_vector);
}

} // end Utility namespace

BOOST_SERIALIZATION_CLASS_VERSION(PQLAQuadrature, Utility, 0);
BOOST_SERIALIZATION_CLASS_EXPORT_STANDARD_KEY(PQLAQuadrature, Utility);
EXTERN_EXPLICIT_CLASS_SERIALIZE_INST(Utility, PQLAQuadrature);

#endif // end UTILITY_PQLA_QUADRATURE

//---------------------------------------------------------------------------//
// end Utility_PQLADiscetization.hpp
//---------------------------------------------------------------------------//


#pragma once

#include "../option.hh"

#include "../../MotionModule/WalkModule/walkmodule.hh"

#include <Eigen/Core>
#include <Eigen/Geometry>

namespace bold
{
  class OdoWalkTo : public Option
  {
  public:
    OdoWalkTo(std::string const& id, std::shared_ptr<WalkModule> walkModule);

    void setTarget(Eigen::Vector2d targetPos, Eigen::Vector2d targetFaceDir, double maxDist);

    double hasTerminated() override;

    OptionVector runPolicy(rapidjson::Writer<rapidjson::StringBuffer>& writer) override;

  private:
    void updateProgress();

    std::shared_ptr<WalkModule> d_walkModule;
    Eigen::Vector2d d_targetPos;
    Eigen::Vector2d d_targetFaceDir;

    double d_maxDist;
    Eigen::Affine3d d_lastOdoReading;
    Eigen::Affine3d d_progress;
  };
}


///////////////////////////////////////////////////////////////////////////////
//  Copyright 2014 Anton Bikineev
//  Copyright 2014 Christopher Kormanyos
//  Copyright 2014 John Maddock
//  Copyright 2014 Paul Bristow
//  Distributed under 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)

#ifndef BOOST_MATH_HYPERGEOMETRIC_1F1_HPP
#define BOOST_MATH_HYPERGEOMETRIC_1F1_HPP

#include <boost/config.hpp>

#if defined(BOOST_NO_CXX11_AUTO_DECLARATIONS) || defined(BOOST_NO_CXX11_LAMBDAS) || defined(BOOST_NO_CXX11_UNIFIED_INITIALIZATION_SYNTAX)
# error "hypergeometric_1F1 requires a C++11 compiler"
#endif

#include <boost/math/policies/policy.hpp>
#include <boost/math/policies/error_handling.hpp>
#include <boost/math/special_functions/detail/hypergeometric_series.hpp>
#include <boost/math/special_functions/detail/hypergeometric_asym.hpp>
#include <boost/math/special_functions/detail/hypergeometric_rational.hpp>
#include <boost/math/special_functions/detail/hypergeometric_1F1_recurrence.hpp>
#include <boost/math/special_functions/detail/hypergeometric_1F1_by_ratios.hpp>
#include <boost/math/special_functions/detail/hypergeometric_pade.hpp>
#include <boost/math/special_functions/detail/hypergeometric_1F1_bessel.hpp>
#include <boost/math/special_functions/detail/hypergeometric_1F1_scaled_series.hpp>
#include <boost/math/special_functions/detail/hypergeometric_pFq_checked_series.hpp>
#include <boost/math/special_functions/detail/hypergeometric_1F1_addition_theorems_on_z.hpp>
#include <boost/math/special_functions/detail/hypergeometric_1F1_large_abz.hpp>
#include <boost/math/special_functions/detail/hypergeometric_1F1_small_a_negative_b_by_ratio.hpp>
#include <boost/math/special_functions/detail/hypergeometric_1F1_negative_b_regions.hpp>

namespace boost { namespace math { namespace detail {

   // check when 1F1 series can't decay to polynom
   template <class T>
   inline bool check_hypergeometric_1F1_parameters(const T& a, const T& b)
   {
      BOOST_MATH_STD_USING

         if ((b <= 0) && (b == floor(b)))
         {
            if ((a >= 0) || (a < b) || (a != floor(a)))
               return false;
         }

      return true;
   }

   template <class T, class Policy>
   T hypergeometric_1F1_divergent_fallback(const T& a, const T& b, const T& z, const Policy& pol, int& log_scaling)
   {
      BOOST_MATH_STD_USING
      const char* function = "hypergeometric_1F1_divergent_fallback<%1%>(%1%,%1%,%1%)";
      //
      // We get here if either:
      // 1) We decide up front that Tricomi's method won't work, or:
      // 2) We've called Tricomi's method and it's failed.
      //
      if (b > 0)
      {
         // Commented out since recurrence seems to always be better?
#if 0
         if ((z < b) && (a > -50))
            // Might as well use a recurrence in preference to z-recurrence:
            return hypergeometric_1F1_backward_recurrence_for_negative_a(a, b, z, pol, function, log_scaling);
         T z_limit = fabs((2 * a - b) / (sqrt(fabs(a))));
         int k = 1 + itrunc(z - z_limit);
         // If k is too large we destroy all the digits in the result:
         T convergence_at_50 = (b - a + 50) * k / (z * 50);
         if ((k > 0) && (k < 50) && (fabs(convergence_at_50) < 1) && (z > z_limit))
         {
            return boost::math::detail::hypergeometric_1f1_recurrence_on_z_minus_zero(a, b, T(z - k), k, pol, log_scaling);
         }
#endif
         if (z < b)
            return hypergeometric_1F1_backward_recurrence_for_negative_a(a, b, z, pol, function, log_scaling);
         else
            return hypergeometric_1F1_backwards_recursion_on_b_for_negative_a(a, b, z, pol, function, log_scaling);
      }
      else  // b < 0
      {
         if (a < 0)
         {
            if ((b < a) && (z < -b / 4))
               return hypergeometric_1F1_from_function_ratio_negative_ab(a, b, z, pol, log_scaling);
            else
            {
               //
               // Solve (a+n)z/((b+n)n) == 1 for n, the number of iterations till the series starts to converge.
               // If this is well away from the origin then it's probably better to use the series to evaluate this.
               // Note that if sqr is negative then we have no solution, so assign an arbitrarily large value to the
               // number of iterations.
               //
               bool can_use_recursion = (z - b + 100 < boost::math::policies::get_max_series_iterations<Policy>()) && (100 - a < boost::math::policies::get_max_series_iterations<Policy>());
               T sqr = 4 * a * z + b * b - 2 * b * z + z * z;
               T iterations_to_convergence = sqr > 0 ? T(0.5f * (-sqrt(sqr) - b + z)) : T(-a - b);
               if(can_use_recursion && ((std::max)(a, b) + iterations_to_convergence > -300))
                  return hypergeometric_1F1_backwards_recursion_on_b_for_negative_a(a, b, z, pol, function, log_scaling);
               //
               // When a < b and if we fall through to the series, then we get divergent behaviour when b crosses the origin
               // so ideally we would pick another method.  Otherwise the terms immediately after b crosses the origin may
               // suffer catestrophic cancellation....
               //
               if((a < b) && can_use_recursion)
                  return hypergeometric_1F1_backwards_recursion_on_b_for_negative_a(a, b, z, pol, function, log_scaling);
            }
         }
         else
         {
            //
            // Start by getting the domain of the recurrence relations, we get either:
            //   -1     Backwards recursion is stable and the CF will converge to double precision.
            //   +1     Forwards recursion is satble and the CF will converge to double precision.
            //    0     No man's land, we're not far enough away from the crossover point to get double precision from either CF.
            //
            // At higher than double precision we need to be further away from the crossover location to
            // get full converge, but it's not clear how much further - indeed at quad precision it's
            // basically impossible to ever get forwards iteration to work.  Backwards seems to work
            // OK as long as a > 1 whatever the precision tbough.
            //
            int domain = hypergeometric_1F1_negative_b_recurrence_region(a, b, z);
            if ((domain < 0) && ((a > 1) || (boost::math::policies::digits<T, Policy>() <= 64)))
               return hypergeometric_1F1_from_function_ratio_negative_b(a, b, z, pol, log_scaling);
            else if (domain > 0)
            {
               if (boost::math::policies::digits<T, Policy>() <= 64)
                  return hypergeometric_1F1_from_function_ratio_negative_b_forwards(a, b, z, pol, log_scaling);
               try
               {
                  return hypergeometric_1F1_checked_series_impl(a, b, z, pol, log_scaling);
               }
               catch (const evaluation_error&)
               {
                  //
                  // The series failed, try the recursions instead and hope we get at least double precision:
                  //
                  return hypergeometric_1F1_from_function_ratio_negative_b_forwards(a, b, z, pol, log_scaling);
               }
            }
            //
            // We could fall back to Tricomi's approximation if we're in the transition zone
            // betweeen the above two regions.  However, I've been unable to find any examples
            // where this is better than the series, and there are many cases where it leads to
            // quite grievous errors.
            /*
            else if (allow_tricomi)
            {
               T aa = a < 1 ? T(1) : a;
               if (z < fabs((2 * aa - b) / (sqrt(fabs(aa * b)))))
                  return hypergeometric_1F1_AS_13_3_7_tricomi(a, b, z, pol, log_scaling);
            }
            */
         }
      }

      // If we get here, then we've run out of methods to try, use the checked series which will
      // raise an error if the result is garbage:
      return hypergeometric_1F1_checked_series_impl(a, b, z, pol, log_scaling);
   }

   template <class T>
   bool is_convergent_negative_z_series(const T& a, const T& b, const T& z, const T& b_minus_a)
   {
      BOOST_MATH_STD_USING
      //
      // Filter out some cases we don't want first:
      //
      if((b_minus_a > 0) && (b > 0))
      {
         if (a < 0)
            return false;
      }
      //
      // Generic check: we have small initial divergence and are convergent after 10 terms:
      //
      if ((fabs(z * a / b) < 2) && (fabs(z * (a + 10) / ((b + 10) * 10)) < 1))
      {
         // Double check for divergence when we cross the origin on a and b:
         if (a < 0)
         {
            T n = 300 - floor(a);
            if (fabs((a + n) * z / ((b + n) * n)) < 1)
            {
               if (b < 0)
               {
                  T m = 3 - floor(b);
                  if (fabs((a + m) * z / ((b + m) * m)) < 1)
                     return true;
               }
               else
                  return true;
            }
         }
         else if (b < 0)
         {
            T n = 3 - floor(b);
            if (fabs((a + n) * z / ((b + n) * n)) < 1)
               return true;
         }
      }
      if ((b > 0) && (a < 0))
      {
         //
         // For a and z both negative, we're OK with some initial divergence as long as
         // it occurs before we hit the origin, as to start with all the terms have the
         // same sign.
         //
         // https://www.wolframalpha.com/input/?i=solve+(a%2Bn)z+%2F+((b%2Bn)n)+%3D%3D+1+for+n
         //
         T sqr = 4 * a * z + b * b - 2 * b * z + z * z;
         T iterations_to_convergence = sqr > 0 ? T(0.5f * (-sqrt(sqr) - b + z)) : T(-a + b);
         if (iterations_to_convergence < 0)
            iterations_to_convergence = 0.5f * (sqrt(sqr) - b + z);
         if (a + iterations_to_convergence < -50)
         {
            // Need to check for divergence when we cross the origin on a:
            if (a > -1)
               return true;
            T n = 300 - floor(a);
            if(fabs((a + n) * z / ((b + n) * n)) < 1)
               return true;
         }
      }
      return false;
   }

   template <class T>
   inline T cyl_bessel_i_shrinkage_rate(const T& z)
   {
      // Approximately the ratio I_10.5(z/2) / I_9.5(z/2), this gives us an idea of how quickly
      // the Bessel terms in A&S 13.6.4 are converging:
      if (z < -160)
         return 1;
      if (z < -40)
         return 0.75f;
      if (z < -20)
         return 0.5f;
      if (z < -7)
         return 0.25f;
      if (z < -2)
         return 0.1f;
      return 0.05f;
   }

   template <class T>
   inline bool hypergeometric_1F1_is_13_3_6_region(const T& a, const T& b, const T& z)
   {
      BOOST_MATH_STD_USING
      if(fabs(a) == 0.5)
         return false;
      if ((z < 0) && (fabs(10 * a / b) < 1) && (fabs(a) < 50))
      {
         T shrinkage = cyl_bessel_i_shrinkage_rate(z);
         // We want the first term not too divergent, and convergence by term 10:
         if ((fabs((2 * a - 1) * (2 * a - b) / b) < 2) && (fabs(shrinkage * (2 * a + 9) * (2 * a - b + 10) / (10 * (b + 10))) < 0.75))
            return true;
      }
      return false;
   }

   template <class T>
   inline bool hypergeometric_1F1_need_kummer_reflection(const T& a, const T& b, const T& z)
   {
      BOOST_MATH_STD_USING
      //
      // Check to see if we should apply Kummer's relation or not:
      //
      if (z > 0)
         return false;
      if (z < -1)
         return true;
      //
      // When z is small and negative, things get more complex.
      // More often than not we do not need apply Kummer's relation and the
      // series is convergent as is, but we do need to check:
      //
      if (a > 0)
      {
         if (b > 0)
         {
            return fabs((a + 10) * z / (10 * (b + 10))) < 1;  // Is the 10'th term convergent?
         }
         else
         {
            return true;  // Likely to be divergent as b crosses the origin
         }
      }
      else // a < 0
      {
         if (b > 0)
         {
            return false;  // Terms start off all positive and then by the time a crosses the origin we *must* be convergent.
         }
         else
         {
            return true;  // Likely to be divergent as b crosses the origin, but hard to rationalise about!
         }
      }
   }

   template <class T, class Policy>
   T hypergeometric_1F1_imp(const T& a, const T& b, const T& z, const Policy& pol, int& log_scaling)
   {
      BOOST_MATH_STD_USING // exp, fabs, sqrt

      static const char* const function = "boost::math::hypergeometric_1F1<%1%,%1%,%1%>(%1%,%1%,%1%)";

      if ((z == 0) || (a == 0))
         return T(1);

      // undefined result:
      if (!detail::check_hypergeometric_1F1_parameters(a, b))
         return policies::raise_domain_error<T>(
            function,
            "Function is indeterminate for negative integer b = %1%.",
            b,
            pol);

      // other checks:
      if (a == -1)
         return 1 - (z / b);

      const T b_minus_a = b - a;

      // 0f0 a == b case;
      if (b_minus_a == 0)
      {
         int scale = itrunc(z, pol);
         log_scaling += scale;
         return exp(z - scale);
      }
      // Special case for b-a = -1, we don't use for small a as it throws the digits of a away and leads to large errors:
      if ((b_minus_a == -1) && (fabs(a) > 0.5))
      {
         // for negative small integer a it is reasonable to use truncated series - polynomial
         if ((a < 0) && (a == ceil(a)) && (a > -50))
            return detail::hypergeometric_1F1_generic_series(a, b, z, pol, log_scaling, function);

         return (b + z) * exp(z) / b;
      }

      if ((a == 1) && (b == 2))
         return boost::math::expm1(z, pol) / z;

      if ((b - a == b) && (fabs(z / b) < policies::get_epsilon<T, Policy>()))
         return 1;
      //
      // Special case for A&S 13.3.6:
      //
      if (z < 0)
      {
         if (hypergeometric_1F1_is_13_3_6_region(a, b, z))
         {
            // a is tiny compared to b, and z < 0
            // 13.3.6 appears to be the most efficient and often the most accurate method.
            T r = boost::math::detail::hypergeometric_1F1_AS_13_3_6(b_minus_a, b, T(-z), a, pol, log_scaling);
            int scale = itrunc(z, pol);
            log_scaling += scale;
            return r * exp(z - scale);
         }
         if ((b < 0) && (fabs(a) < 1e-2))
         {
            //
            // This is a tricky area, potentially we have no good method at all:
            //
            if (b - ceil(b) == a)
            {
               // Fractional parts of a and b are genuinely equal, we might as well
               // apply Kummer's relation and get a truncated series:
               int scaling = itrunc(z);
               T r = exp(z - scaling) * detail::hypergeometric_1F1_imp<T>(b_minus_a, b, -z, pol, log_scaling);
               log_scaling += scaling;
               return r;
            }
            if ((b < -1) && (max_b_for_1F1_small_a_negative_b_by_ratio(z) < b))
               return hypergeometric_1F1_small_a_negative_b_by_ratio(a, b, z, pol, log_scaling);
            if ((b > -1) && (b < -0.5f))
            {
               // Recursion is meta-stable:
               T first = hypergeometric_1F1_imp(a, T(b + 2), z, pol);
               T second = hypergeometric_1F1_imp(a, T(b + 1), z, pol);
               return tools::apply_recurrence_relation_backward(hypergeometric_1F1_recurrence_small_b_coefficients<T>(a, b, z, 1), 1, first, second);
            }
            //
            // We've got nothing left but 13.3.6, even though it may be initially divergent:
            //
            T r = boost::math::detail::hypergeometric_1F1_AS_13_3_6(b_minus_a, b, T(-z), a, pol, log_scaling);
            int scale = itrunc(z, pol);
            log_scaling += scale;
            return r * exp(z - scale);
         }
      }
      //
      // Asymptotic expansion for large z
      // TODO: check region for higher precision types.
      // Use recurrence relations to move to this region when a and b are also large.
      //
      if (detail::hypergeometric_1F1_asym_region(a, b, z, pol))
      {
         int saved_scale = log_scaling;
         try
         {
            return hypergeometric_1F1_asym_large_z_series(a, b, z, pol, log_scaling);
         }
         catch (const evaluation_error&)
         {
         }
         //
         // Very occationally our convergence criteria don't quite go to full precision
         // and we have to try another method:
         //
         log_scaling = saved_scale;
      }

      if ((fabs(a * z / b) < 3.5) && (fabs(z * 100) < fabs(b)) && ((fabs(a) > 1e-2) || (b < -5)))
         return detail::hypergeometric_1F1_rational(a, b, z, pol);

      if (hypergeometric_1F1_need_kummer_reflection(a, b, z))
      {
         if (a == 1)
            return detail::hypergeometric_1F1_pade(b, z, pol);
         if (is_convergent_negative_z_series(a, b, z, b_minus_a))
         {
            if ((boost::math::sign(b_minus_a) == boost::math::sign(b)) && ((b > 0) || (b < -200)))
            {
               // Series is close enough to convergent that we should be OK,
               // In this domain b - a ~ b and since 1F1[a, a, z] = e^z 1F1[b-a, b, -z]
               // and 1F1[a, a, -z] = e^-z the result must necessarily be somewhere near unity.
               // We have to rule out b small and negative becuase if b crosses the origin early
               // in the series (before we're pretty much converged) then all bets are off.
               // Note that this can go badly wrong when b and z are both large and negative,
               // in that situation the series goes in waves of large and small values which
               // may or may not cancel out.  Likewise the initial part of the series may or may
               // not converge, and even if it does may or may not give a correct answer!
               // For example 1F1[-small, -1252.5, -1043.7] can loose up to ~800 digits due to
               // cancellation and is basically incalculable via this method.
               return hypergeometric_1F1_checked_series_impl(a, b, z, pol, log_scaling);
            }
         }
         // Let's otherwise make z positive (almost always)
         // by Kummer's transformation
         // (we also don't transform if z belongs to [-1,0])
         int scaling = itrunc(z);
         T r = exp(z - scaling) * detail::hypergeometric_1F1_imp<T>(b_minus_a, b, -z, pol, log_scaling);
         log_scaling += scaling;
         return r;
      }
      //
      // Check for initial divergence:
      //
      bool series_is_divergent = (a + 1) * z / (b + 1) < -1;
      if (series_is_divergent && (a < 0) && (b < 0) && (a > -1))
         series_is_divergent = false;   // Best off taking the series in this situation
      //
      // If series starts off non-divergent, and becomes divergent later
      // then it's because both a and b are negative, so check for later
      // divergence as well:
      //
      if (!series_is_divergent && (a < 0) && (b < 0) && (b > a))
      {
         //
         // We need to exclude situations where we're over the initial "hump"
         // in the series terms (ie series has already converged by the time
         // b crosses the origin:
         //
         //T fa = fabs(a);
         //T fb = fabs(b);
         T convergence_point = sqrt((a - 1) * (a - b)) - a;
         if (-b < convergence_point)
         {
            T n = -floor(b);
            series_is_divergent = (a + n) * z / ((b + n) * n) < -1;
         }
      }
      else if (!series_is_divergent && (b < 0) && (a > 0))
      {
         // Series almost always become divergent as b crosses the origin:
         series_is_divergent = true;
      }
      if (series_is_divergent && (b < -1) && (b > -5) && (a > b))
         series_is_divergent = false;  // don't bother with divergence, series will be OK

      //
      // Test for alternating series due to negative a,
      // in particular, see if the series is initially divergent
      // If so use the recurrence relation on a:
      //
      if (series_is_divergent)
      {
         if((a < 0) && (floor(a) == a) && (-a < policies::get_max_series_iterations<Policy>()))
            // This works amazingly well for negative integer a:
            return hypergeometric_1F1_backward_recurrence_for_negative_a(a, b, z, pol, function, log_scaling);
         //
         // In what follows we have to set limits on how large z can be otherwise
         // the Bessel series become large and divergent and all the digits cancel out.
         // The criteria are distinctly empiracle rather than based on a firm analysis
         // of the terms in the series.
         //
         if (b > 0)
         {
            T z_limit = fabs((2 * a - b) / (sqrt(fabs(a))));
            if ((z < z_limit) && hypergeometric_1F1_is_tricomi_viable_positive_b(a, b, z))
               return detail::hypergeometric_1F1_AS_13_3_7_tricomi(a, b, z, pol, log_scaling);
         }
         else  // b < 0
         {
            if (a < 0)
            {
               T z_limit = fabs((2 * a - b) / (sqrt(fabs(a))));
               //
               // I hate these hard limits, but they're about the best we can do to try and avoid
               // Bessel function internal failures: these will be caught and handled
               // but up the expense of this function call:
               //
               if (((z < z_limit) || (a > -500)) && ((b > -500) || (b - 2 * a > 0)) && (z < -a))
               {
                  //
                  // Outside this domain we will probably get better accuracy from the recursive methods.
                  //
                  if(!(((a < b) && (z > -b)) || (z > z_limit)))
                     return detail::hypergeometric_1F1_AS_13_3_7_tricomi(a, b, z, pol, log_scaling);
                  //
                  // When b and z are both very small, we get large errors from the recurrence methods
                  // in the fallbacks.  Tricomi seems to work well here, as does direct series evaluation
                  // at least some of the time.  Picking the right method is not easy, and sometimes this
                  // is much worse than the fallback.  Overall though, it's a reasonable choice that keeps
                  // the very worst errors under control.
                  //
                  if(b > -1)
                     return detail::hypergeometric_1F1_AS_13_3_7_tricomi(a, b, z, pol, log_scaling);
               }
            }
            //
            // We previosuly used Tricomi here, but it appears to be worse than
            // the recurrence-based algorithms in hypergeometric_1F1_divergent_fallback.
            /*
            else
            {
               T aa = a < 1 ? T(1) : a;
               if (z < fabs((2 * aa - b) / (sqrt(fabs(aa * b)))))
                  return detail::hypergeometric_1F1_AS_13_3_7_tricomi(a, b, z, pol, log_scaling);
            }*/
         }

         return hypergeometric_1F1_divergent_fallback(a, b, z, pol, log_scaling);
      }

      if (hypergeometric_1F1_is_13_3_6_region(b_minus_a, b, T(-z)))
      {
         // b_minus_a is tiny compared to b, and -z < 0
         // 13.3.6 appears to be the most efficient and often the most accurate method.
         return boost::math::detail::hypergeometric_1F1_AS_13_3_6(a, b, z, b_minus_a, pol, log_scaling);
      }
#if 0
      if ((a > 0) && (b > 0) && (a * z / b > 2))
      {
         //
         // Series is initially divergent and slow to converge, see if applying
         // Kummer's relation can improve things:
         //
         if (is_convergent_negative_z_series(b_minus_a, b, T(-z), b_minus_a))
         {
            int scaling = itrunc(z);
            T r = exp(z - scaling) * detail::hypergeometric_1F1_checked_series_impl(b_minus_a, b, T(-z), pol, log_scaling);
            log_scaling += scaling;
            return r;
         }

      }
#endif
      if ((a > 0) && (b > 0) && (a * z > 50))
         return detail::hypergeometric_1F1_large_abz(a, b, z, pol, log_scaling);

      if (b < 0)
         return detail::hypergeometric_1F1_checked_series_impl(a, b, z, pol, log_scaling);

      return detail::hypergeometric_1F1_generic_series(a, b, z, pol, log_scaling, function);
   }

   template <class T, class Policy>
   inline T hypergeometric_1F1_imp(const T& a, const T& b, const T& z, const Policy& pol)
   {
      BOOST_MATH_STD_USING // exp, fabs, sqrt
      int log_scaling = 0;
      T result = hypergeometric_1F1_imp(a, b, z, pol, log_scaling);
      //
      // Actual result will be result * e^log_scaling.
      //
#ifndef BOOST_NO_CXX11_THREAD_LOCAL
    static const thread_local int max_scaling = itrunc(boost::math::tools::log_max_value<T>()) - 2;
    static const thread_local T max_scale_factor = exp(T(max_scaling));
#else
    int max_scaling = itrunc(boost::math::tools::log_max_value<T>()) - 2;
      T max_scale_factor = exp(T(max_scaling));
#endif

      while (log_scaling > max_scaling)
      {
         result *= max_scale_factor;
         log_scaling -= max_scaling;
      }
      while (log_scaling < -max_scaling)
      {
         result /= max_scale_factor;
         log_scaling += max_scaling;
      }
      if (log_scaling)
         result *= exp(T(log_scaling));
      return result;
   }

   template <class T, class Policy>
   inline T log_hypergeometric_1F1_imp(const T& a, const T& b, const T& z, int* sign, const Policy& pol)
   {
      BOOST_MATH_STD_USING // exp, fabs, sqrt
      int log_scaling = 0;
      T result = hypergeometric_1F1_imp(a, b, z, pol, log_scaling);
      if (sign)
      *sign = result < 0 ? -1 : 1;
     result = log(fabs(result)) + log_scaling;
      return result;
   }

   template <class T, class Policy>
   inline T hypergeometric_1F1_regularized_imp(const T& a, const T& b, const T& z, const Policy& pol)
   {
      BOOST_MATH_STD_USING // exp, fabs, sqrt
      int log_scaling = 0;
      T result = hypergeometric_1F1_imp(a, b, z, pol, log_scaling);
      //
      // Actual result will be result * e^log_scaling / tgamma(b).
      //
    int result_sign = 1;
    T scale = log_scaling - boost::math::lgamma(b, &result_sign, pol);
#ifndef BOOST_NO_CXX11_THREAD_LOCAL
      static const thread_local T max_scaling = boost::math::tools::log_max_value<T>() - 2;
    static const thread_local T max_scale_factor = exp(max_scaling);
#else
    T max_scaling = boost::math::tools::log_max_value<T>() - 2;
    T max_scale_factor = exp(max_scaling);
#endif

      while (scale > max_scaling)
      {
         result *= max_scale_factor;
         scale -= max_scaling;
      }
      while (scale < -max_scaling)
      {
         result /= max_scale_factor;
     scale += max_scaling;
      }
      if (scale != 0)
         result *= exp(scale);
      return result * result_sign;
   }

} // namespace detail

template <class T1, class T2, class T3, class Policy>
inline typename tools::promote_args<T1, T2, T3>::type hypergeometric_1F1(T1 a, T2 b, T3 z, const Policy& /* pol */)
{
   BOOST_FPU_EXCEPTION_GUARD
      typedef typename tools::promote_args<T1, T2, T3>::type result_type;
   typedef typename policies::evaluation<result_type, Policy>::type value_type;
   typedef typename policies::normalise<
      Policy,
      policies::promote_float<false>,
      policies::promote_double<false>,
      policies::discrete_quantile<>,
      policies::assert_undefined<> >::type forwarding_policy;
   return policies::checked_narrowing_cast<result_type, Policy>(
      detail::hypergeometric_1F1_imp<value_type>(
         static_cast<value_type>(a),
         static_cast<value_type>(b),
         static_cast<value_type>(z),
         forwarding_policy()),
      "boost::math::hypergeometric_1F1<%1%>(%1%,%1%,%1%)");
}

template <class T1, class T2, class T3>
inline typename tools::promote_args<T1, T2, T3>::type hypergeometric_1F1(T1 a, T2 b, T3 z)
{
   return hypergeometric_1F1(a, b, z, policies::policy<>());
}

template <class T1, class T2, class T3, class Policy>
inline typename tools::promote_args<T1, T2, T3>::type hypergeometric_1F1_regularized(T1 a, T2 b, T3 z, const Policy& /* pol */)
{
   BOOST_FPU_EXCEPTION_GUARD
      typedef typename tools::promote_args<T1, T2, T3>::type result_type;
   typedef typename policies::evaluation<result_type, Policy>::type value_type;
   typedef typename policies::normalise<
      Policy,
      policies::promote_float<false>,
      policies::promote_double<false>,
      policies::discrete_quantile<>,
      policies::assert_undefined<> >::type forwarding_policy;
   return policies::checked_narrowing_cast<result_type, Policy>(
      detail::hypergeometric_1F1_regularized_imp<value_type>(
         static_cast<value_type>(a),
         static_cast<value_type>(b),
         static_cast<value_type>(z),
         forwarding_policy()),
      "boost::math::hypergeometric_1F1<%1%>(%1%,%1%,%1%)");
}

template <class T1, class T2, class T3>
inline typename tools::promote_args<T1, T2, T3>::type hypergeometric_1F1_regularized(T1 a, T2 b, T3 z)
{
   return hypergeometric_1F1_regularized(a, b, z, policies::policy<>());
}

template <class T1, class T2, class T3, class Policy>
inline typename tools::promote_args<T1, T2, T3>::type log_hypergeometric_1F1(T1 a, T2 b, T3 z, const Policy& /* pol */)
{
  BOOST_FPU_EXCEPTION_GUARD
    typedef typename tools::promote_args<T1, T2, T3>::type result_type;
  typedef typename policies::evaluation<result_type, Policy>::type value_type;
  typedef typename policies::normalise<
    Policy,
    policies::promote_float<false>,
    policies::promote_double<false>,
    policies::discrete_quantile<>,
    policies::assert_undefined<> >::type forwarding_policy;
  return policies::checked_narrowing_cast<result_type, Policy>(
    detail::log_hypergeometric_1F1_imp<value_type>(
      static_cast<value_type>(a),
      static_cast<value_type>(b),
      static_cast<value_type>(z),
      0,
      forwarding_policy()),
    "boost::math::hypergeometric_1F1<%1%>(%1%,%1%,%1%)");
}

template <class T1, class T2, class T3>
inline typename tools::promote_args<T1, T2, T3>::type log_hypergeometric_1F1(T1 a, T2 b, T3 z)
{
  return log_hypergeometric_1F1(a, b, z, policies::policy<>());
}

template <class T1, class T2, class T3, class Policy>
inline typename tools::promote_args<T1, T2, T3>::type log_hypergeometric_1F1(T1 a, T2 b, T3 z, int* sign, const Policy& /* pol */)
{
  BOOST_FPU_EXCEPTION_GUARD
    typedef typename tools::promote_args<T1, T2, T3>::type result_type;
  typedef typename policies::evaluation<result_type, Policy>::type value_type;
  typedef typename policies::normalise<
    Policy,
    policies::promote_float<false>,
    policies::promote_double<false>,
    policies::discrete_quantile<>,
    policies::assert_undefined<> >::type forwarding_policy;
  return policies::checked_narrowing_cast<result_type, Policy>(
    detail::log_hypergeometric_1F1_imp<value_type>(
      static_cast<value_type>(a),
      static_cast<value_type>(b),
      static_cast<value_type>(z),
      sign,
      forwarding_policy()),
    "boost::math::hypergeometric_1F1<%1%>(%1%,%1%,%1%)");
}

template <class T1, class T2, class T3>
inline typename tools::promote_args<T1, T2, T3>::type log_hypergeometric_1F1(T1 a, T2 b, T3 z, int* sign)
{
  return log_hypergeometric_1F1(a, b, z, sign, policies::policy<>());
}

  } } // namespace boost::math

#endif // BOOST_MATH_HYPERGEOMETRIC_HPP


// Boost.Geometry (aka GGL, Generic Geometry Library)
//
// Copyright (c) 2007-2015 Barend Gehrels, Amsterdam, the Netherlands.
//
// This file was modified by Oracle on 2015, 2016.
// Modifications copyright (c) 2015-2016, Oracle and/or its affiliates.
//
// Contributed and/or modified by Vissarion Fysikopoulos, on behalf of Oracle
//
// 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)

#include <algorithms/area/test_area.hpp>

#include <boost/geometry/geometries/geometries.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/multi_polygon.hpp>

#include <boost/geometry/io/wkt/wkt.hpp>

template <typename CT>
void test_all()
{
    typedef typename bg::model::d2::point_xy<CT> pt_crt;
    typedef typename bg::model::point<CT, 2, bg::cs::spherical_equatorial<bg::degree> > pt_sph;
    typedef typename bg::model::point<CT, 2, bg::cs::geographic<bg::degree> > pt_geo;

    typedef bg::model::multi_polygon<bg::model::polygon<pt_crt> > mp_crt;
    typedef bg::model::multi_polygon<bg::model::polygon<pt_sph> > mp_sph;
    typedef bg::model::multi_polygon<bg::model::polygon<pt_geo> > mp_geo;

    // mean Earth's radius^2
    double r2 = bg::math::sqr(bg::get_radius<0>(bg::srs::sphere<double>()));

    std::string poly = "MULTIPOLYGON(((0 0,0 7,4 2,2 0,0 0)))";
    test_geometry<mp_crt>(poly, 16.0);
    test_geometry<mp_sph>(poly, 197897454752.69489 / r2);
    test_geometry<mp_geo>(poly, 197022195785.33771);
}

int test_main( int , char* [] )
{
    test_all<double>();

    return 0;
}


// [[Rcpp::plugins(openmp)]]
#include <omp.h>
#include <Rcpp.h>
#include <RcppEigen.h>
#include <Eigen/Core>
#include <random>
#include "distributions.h"
#include "concurrentqueue.h"

// [[Rcpp::depends(RcppEigen)]]
using namespace Rcpp;
using namespace RcppEigen;
using namespace Eigen;
using Eigen::Matrix;
using Eigen::MatrixXd;
using Eigen::VectorXd;
using Eigen::SparseVector;
using Eigen::LLT;
using Eigen::Lower;
using Eigen::Map;
using Eigen::Upper;
typedef Map<MatrixXd> MapMatd;

template<typename Scalar>
struct scalar_normal_dist_op
{
  static std::mt19937 rng;                        // The uniform pseudo-random algorithm
  mutable std::normal_distribution<Scalar> norm; // gaussian combinator

  EIGEN_EMPTY_STRUCT_CTOR(scalar_normal_dist_op)

    template<typename Index>
    inline const Scalar operator() (Index, Index = 0) const { return norm(rng); }
    inline void seed(const uint64_t &s) { rng.seed(s); }
};

template<typename Scalar>
std::mt19937 scalar_normal_dist_op<Scalar>::rng;

template<typename Scalar>
struct exponential_functor
{
  exponential_functor(){}

  const Scalar operator()(const Scalar& x) const{ return exp_rng(x); }

};

inline MatrixXd AtA(const MapMatd& A) {
  int n(A.cols());
  return MatrixXd(n,n).setZero().selfadjointView<Lower>()
                      .rankUpdate(A.adjoint());
}

template<typename Scalar>
struct inv_gamma_functor
{
  inv_gamma_functor(const Scalar& vd):m_a(vd){}

  const Scalar operator()(const Scalar& x) const{ return inv_gamma_rate_rng(0.5 + 0.5*m_a,x); }
  Scalar  m_a;
};

template<typename Scalar>
struct gamma_functor
{
  gamma_functor(const Scalar& vd):m_a(vd){}

  const Scalar operator()(const Scalar& x) const{ return gamma_rng(0.5 + 0.5*m_a,x); }
  Scalar  m_a;
};

template<typename Scalar>
struct inv_gamma_functor_init
{
  inv_gamma_functor_init(){}

  const Scalar operator()(const Scalar& x) const{ return inv_gamma_rate_rng(0.5,x); }

};

template<typename Scalar>
struct inv_gamma_functor_init_v
{
  inv_gamma_functor_init_v(const Scalar& vd):m_a(vd){}

  const Scalar operator()(const Scalar& x) const{ return inv_gamma_rate_rng(0.5*m_a,m_a*x); }
  Scalar  m_a;
};

/*
* Bayes R sampler
* outputFile- The file in which the samples aftare burnin will be stored
* seed- random seed
* max_iterations- total of number of samples taken.
* burn_in - integer leq than max_iterations, number of samples used for burn in, after which, al samples will  be stored in the outputFile
* thinning- thinning regime, not implemented
* X- matrix of snp markers, or covariates of interest
* Y- vector of response variates, must have the same number of rows as X
* sigma0- variance of the zero-centered normal prior over the intercept
* v0E- degrees of  freedom of the prior inverse scaled chi-squared distribution over residues variance
* s02E - scale parameter of the prior inverse scaled chi-squared distribution over residues variance
* v0G- degrees of freedom of the prior inverse scaled chi-squared distribution over genetic effects variance
* s02G- scale parameter of the prior inverse scaled chi-squared distribution over genetic effects variance
*/
// [[Rcpp::export]]
void HorseshoeR(std::string outputFile, int seed, int max_iterations, int burn_in, int thinning, Eigen::MatrixXd X, Eigen::VectorXd Y,double A, double v0E, double s02E, double vL, double vT,double c2,double vC,double sC) {
  int flag;
  moodycamel::ConcurrentQueue<Eigen::VectorXd> q;
  flag=0;
  int N(Y.size());
  int M(X.cols());
  VectorXd components(M);

  ////////////validate inputs

  if(max_iterations < burn_in || max_iterations<1 || burn_in<1) //validations related to mcmc burnin and iterations
  {
    std::cout<<"error: burn_in has to be a positive integer and smaller than the maximum number of iterations ";
    return;
  }

  Eigen::initParallel();
  Eigen::setNbThreads(10);
  double sum_beta_sqr;

#pragma omp parallel num_threads(2) shared(flag,q,M,N)
{
#pragma omp sections
{

  {

    //mean and residual variables
    double mu; // mean or intercept
    double sigmaG; //genetic variance
    double sigmaE; // residuals variance

    //component variables
    VectorXd lambda(M);
    VectorXd v(M);
    VectorXd phi(M);
    VectorXd chi(M);
    double tau;
    double eta;
    //linear model variables
    MatrixXd beta(M,1); // effect sizes
    VectorXd y_tilde(N); // variable containing the adjusted residuals to exclude the effects of a given marker
    VectorXd epsilon(N); // variable containing the residuals

    //sampler variables
    VectorXd sample(2*M+4+N); // varible containg a sambple of all variables in the model, M marker effects, M component assigned to markers, sigmaE, sigmaG, mu, iteration number and Explained variance
    std::vector<int> markerI;
    for (int i=0; i<M; ++i) {
      markerI.push_back(i);
    }

    int marker;
    double acum;

    y_tilde.setZero();

    beta.setZero();
    tau=beta_rng(1,1);

    mu=0;

    v=(v.setOnes().array()).unaryExpr(inv_gamma_functor<double>(0));
    v.setOnes();
    //std::cout<< "initial v" << eta;
    lambda=v.unaryExpr(inv_gamma_functor_init_v<double>(vL));
    lambda.setOnes();

    std::chrono::high_resolution_clock::time_point t1 = std::chrono::high_resolution_clock::now();
    epsilon= Y.array() - mu - (X*beta).array();
    sigmaE=epsilon.squaredNorm()/N*0.5;

    eta=inv_gamma_rate_rng(0.5,1/(sigmaE*pow(A,2)));
    //eta=0.00001;
    std::cout<< "initial eta " << eta<<"\n";
    tau=(1.0/eta)*inv_gamma_rate_rng(0.5*vT,vT);

    // tau=1/A;
    std::cout<< "initial tau " << tau<<"\n";

    for(int iteration=0; iteration < max_iterations; iteration++){

      if(iteration>0)
        if( iteration % (int)std::ceil(max_iterations/10) ==0)
        {
          std::cout << "iteration: "<<iteration <<"\n";
          std::cout<< " tau " << tau<<"\n";
          std::cout<< " eta " << eta<<"\n";
          std::cout<< "sigmaE" << sigmaE<<"\n";
        }

        epsilon= epsilon.array()+mu;//  we substract previous value
        mu = norm_rng(epsilon.sum()/(double)N, sigmaE/(double)N); //update mu
        epsilon= epsilon.array()-mu;// we substract again now epsilon =Y-mu-X*beta

        std::random_shuffle(markerI.begin(), markerI.end());

        eta = inv_gamma_rate_rng(0.5+0.5*vT,(1.0/(sigmaE*A*A))+vT/tau);
        v=(vL/(lambda).array()+1.0).unaryExpr(inv_gamma_functor<double>(vL));
        for(int j=0; j < M; j++){

          marker= markerI[j];

          y_tilde= epsilon.array()+(X.col(marker)*beta(marker,0)).array();//now y_tilde= Y-mu-X*beta+ X.col(marker)*beta(marker)_old

          // std::cout<< muk;
          //we compute the denominator in the variance expression to save computations
          //denom=X.col(marker).squaredNorm()+(sigmaE/(tau*c2*lambda[marker]/(tau*lambda[marker]+c2)));
          //muk for the other components is computed according to equaitons
          //muk= (X.col(marker).cwiseProduct(y_tilde)).sum()/denom;
          //beta(marker,0)=norm_rng(muk,sigmaE/denom);
          beta(marker,0)=(X.col(marker).cwiseProduct(y_tilde)).sum()/(X.col(marker).squaredNorm()+(sigmaE/(tau*c2*lambda[marker]/(tau*lambda[marker]+c2))))+sqrt(sigmaE/(X.col(marker).squaredNorm()+(sigmaE/(tau*c2*lambda[marker]/(tau*lambda[marker]+c2)))))*norm_rng(0,1);

          epsilon=y_tilde-X.col(marker)*beta(marker,0);//now epsilon contains Y-mu - X*beta+ X.col(marker)*beta(marker)_old- X.col(marker)*beta(marker)_new

        }

        lambda=(vL*v.cwiseInverse()+(0.5*beta.cwiseProduct(beta)*(1.0/tau))).unaryExpr(inv_gamma_functor<double>(vL));
        //  if(iteration==0)
        //  std::cout<< " lambda " << lambda<<"\n";
        tau= inv_gamma_rate_rng(0.5*(M+vT),vT/eta+((0.5)*((beta.array().pow(2))/lambda.array()).sum()));

        //tau=A;
         c2=inv_gamma_rate_rng(0.5*vC+0.5*M,vC*sC*0.5+0.5*beta.squaredNorm());
        //  c2=sC;

        sigmaE=inv_scaled_chisq_rng(v0E+N,((epsilon).squaredNorm()+v0E*s02E)/(v0E+N));

        if(iteration >= burn_in)
        {
          if(iteration % thinning == 0){
            sample<< iteration,mu,beta,sigmaE,tau,lambda,epsilon;
            q.enqueue(sample);
          }

        }

    }

    std::chrono::high_resolution_clock::time_point t2 = std::chrono::high_resolution_clock::now();
    auto duration = std::chrono::duration_cast<std::chrono::seconds>( t2 - t1 ).count();
    std::cout << "duration: "<<duration << "s\n";
    flag=1;
  }
#pragma omp section
{
  bool queueFull;
  queueFull=0;
  std::ofstream outFile;
  outFile.open(outputFile);
  VectorXd sampleq(2*M+4+N);
  IOFormat CommaInitFmt(StreamPrecision, DontAlignCols, ", ", ", ", "", "", "", "");
  outFile<< "iteration,"<<"mu,";
  for(unsigned int i = 0; i < M; ++i){
    outFile << "beta[" << (i+1) << "],";

  }
  outFile<<"sigmaE,"<<"tau,";
  for(unsigned int i = 0; i < M; ++i){
    outFile << "lambda[" << (i+1) << "],";
  }
  for(unsigned int i = 0; i < N; ++i){
    outFile << "epsilon[" << (i+1) << "],";
  }
  outFile<<"\n";

  while(!flag ){
    if(q.try_dequeue(sampleq))
      outFile<< sampleq.transpose().format(CommaInitFmt) << "\n";
  }
}

}
}

}

/*** R
M=3000
N=2000
MT=200
B=matrix(rnorm(M,sd=sqrt(0.5/MT)),ncol=1)
  B[sample(1:M,M-MT),1]=0
X <- matrix(rnorm(M*N), N, M); var(X[,1])
  G <- X%*%B; var(G)
    Y=X%*%B+rnorm(N,sd=sqrt((1-var(G)))); var(Y)
    Y=scale(Y)
    X=scale(X)
    vT=1
    vL=1
    A=1500
    A=(1/sqrt(N))*A/(M-A)
    c2=1.0
    v0E=0.001
    s02E=0.001
    vC=10
    sC=10.0

    HorseshoeR("./test2.csv",1, 20000,10000 ,10,X, Y, A,  v0E, s02E,  vL,  vT, c2,vC,sC)
      library(readr)
     library(data.table)
      tmp <- fread("./test2.csv")
      tmp<-as.matrix(tmp)
      plot(B,colMeans(tmp[,grep("beta",colnames(tmp))]))
      summary(lm(B~colMeans(tmp[,grep("beta",colnames(tmp))])))
      lines(B,B)
      abline(h=0)
      G <- X%*%B;
    var(G)
      tmp<-as.data.frame(tmp)

      mean(tmp$sigmaE)
      plot(tmp$sigmaE)
      plot(tmp$mu)
      plot(tmp$tau)
      hist(as.matrix(tmp[,grep("beta",names(tmp))]))

      */