/**
 *	@author Marek Solony (isolony(at)fit.vutbr.cz)
 *	@date 2014
 *	@brief Convertor from Bundler output (.out) into graph file for SLAM++
 *		details about .out format can be found at: http://www.cs.cornell.edu/~snavely/bundler/bundler-v0.4-manual.html#S6
 */

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <string.h>
#include <iostream>
#include <fstream>
#include <vector>
#include <map>
#include <Eigen/Dense>

#define PI 3.14159265359

using namespace std;

int counter = 0;
std::map<int, int> table;

static void Quat_to_AxisAngle(const Eigen::Quaterniond &r_quat, Eigen::Vector3d &r_axis_angle)
{
	double f_half_angle = /*(r_quat.w() <= 0)? asin(r_quat.vec().norm()) :*/ acos(r_quat.w()); // 0 .. pi

	if(f_half_angle < 1e-12)
		r_axis_angle = Eigen::Vector3d(0, 0, 0); // lim(sin(x) / x) for x->0 equals 1, we're therefore multiplying a null vector by 1
	else {
		double f_angle = 2 * ((r_quat.w() <= 0)? f_half_angle - M_PI : f_half_angle);
		r_axis_angle = r_quat.vec() * (f_angle / sin(f_half_angle));
	}
}
static void AxisAngle_to_Quat(const Eigen::Vector3d &r_axis_angle, Eigen::Quaterniond &r_quat)
{
	double f_angle = r_axis_angle.norm();
	if(f_angle < 1e-12)
		r_quat = Eigen::Quaterniond(1, 0, 0, 0); // cos(0) = 1
	else {
		//_ASSERTE(f_angle <= M_PI); // sometimes broken
		f_angle = fmod(f_angle, M_PI * 2);
		double q = (sin(f_angle * .5) / f_angle);
		r_quat = Eigen::Quaterniond(cos(f_angle * .5), r_axis_angle(0) * q,
			r_axis_angle(1) * q, r_axis_angle(2) * q);
		r_quat.normalize();
	}
}

int main(int argc, char *argv[]) {
	char edge[64];
	int v[2];
	float pos[3];
	int col[3];
	float rot[4];
	float conv[21];

	std::vector<Eigen::VectorXd> pts3d;
	std::vector<Eigen::VectorXd> cam;
	std::vector<Eigen::VectorXd> observation;

	if(argc == 3)
	{
		FILE * pFile = fopen (argv[1],"r+");

		std::ofstream wFile;
		wFile.open(argv[2]);

		std::cout << "bundler -> graph" << std::endl;
		//scan n cams and n points
		int cams, points;
		fscanf (pFile, "%d %d", &cams, &points);

		std::cout << "cams: " << cams << std::endl;
		std::cout << "points: " << points << std::endl;
		//load cams
		std::vector<int> camdxs;
		for(int a = 0; a < cams; a++) {
			Eigen::VectorXd v1(15);

			camdxs.push_back(-1);

			for(int b = 0; b < 15; b++) {
				fscanf (pFile, "%f", &conv[0] );
				v1(b) = conv[0];
			}
			cam.push_back(v1);
		}
		//load points and observations
		for(int a = 0; a < points; a++) {
			if(a % 100000 == 0)
				std::cout << a << std::endl;

			Eigen::VectorXd v1(4);
			fscanf (pFile, "%f %f %f", &pos[0], &pos[1], &pos[2] );
			v1(0) = pos[0];
			v1(1) = pos[1];
			v1(2) = pos[2];
			fscanf (pFile, "%d %d %d", &col[0], &col[1], &col[2] );

			int refs;
			fscanf (pFile, "%d", &refs );

			v1(3) = refs;
			pts3d.push_back(v1);

			for(int b = 0; b < refs; b++) {
				Eigen::VectorXd v2(4);
				v2(1) = a;	//which point?
				fscanf (pFile, "%d %d %f %f", &v[1], &v[0], &pos[0], &pos[1]);

				camdxs.at(v[1]) = v[1];

				v2(0) = v[1];
				v2(2) = pos[0];
				v2(3) = pos[1];

				observation.push_back(v2);
			}
		}

		std::cout << "cams " << cam.size() << std::endl;
		std::cout << "pts " << pts3d.size() << std::endl;
		//DUMP THE DATASET TO GRAPH FORMAT
		for (int a = 0; a < cam.size(); a++) {
			//convert rotation matrix to inverse quaternion
			Eigen::Matrix3d rot;
			rot << (cam[a])(3), (cam[a])(4), (cam[a])(5),
				(cam[a])(6), (cam[a])(7), (cam[a])(8),
				(cam[a])(9), (cam[a])(10), (cam[a])(11);
			//invert
			rot = rot.inverse().eval();

			//to quat
			Eigen::Quaternion<double> quat(rot);

			Eigen::Vector3d t_vec((cam[a])(12), (cam[a])(13), (cam[a])(14));
			//rotate
			Eigen::Vector3d c = -(quat * (t_vec));

			//store to file
			wFile << "VERTEX_CAM " << a << " " << c(0) << " " << c(1) << " " << c(2) << " " <<
				quat.x() << " " << quat.y() << " " << quat.z() << " " << quat.w() << " " <<
				(cam[a])(0) << " " << (cam[a])(0) << " " << 0 << " " << 0 << " " << 0 << std::endl;
		}

		int obs_count = 0;
		for (int a = 0; a < pts3d.size(); a++) {
			wFile << "VERTEX_XYZ " << a + cam.size() << " " <<
				(pts3d[a])(0) << " " << (pts3d[a])(1) << " " << (pts3d[a])(2) << std::endl;

			for(int b = 0; b < (pts3d[a])(3); b++) {
				wFile << "EDGE_PROJECT_P2MC " << a + cam.size() << " " << (observation[obs_count])(0) << " " <<
				-(observation[obs_count])(2) << " " << -(observation[obs_count])(3) << " 1 0 1" << std::endl;

				camdxs.at((observation[b])(0)) = (observation[b])(0);

				obs_count ++;
			}
		}

	//pFile.close();
	wFile.close();
	std::cout << "done " << cams << std::endl;
	} else {
		std::cout << "Convertor from Bundler output (.out) into graph file for SLAM++" << std::endl <<
				"details about .out format can be found at: http://www.cs.cornell.edu/~snavely/bundler/bundler-v0.4-manual.html#S6" << std::endl << std::endl;
		std::cout << "Usage: bundler2graph <input .out file> <output .graph filename>" << std::endl;
	}

	return 0;
}


// Boost.Geometry

// Copyright (c) 2017 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 <geometry_test_common.hpp>

#include <boost/concept_check.hpp>

#include <boost/geometry/srs/srs.hpp>
#include <boost/geometry/algorithms/assign.hpp>
#include <boost/geometry/geometries/point.hpp>
#include <boost/geometry/strategies/strategies.hpp>

#include <test_common/test_point.hpp>

#ifdef HAVE_TTMATH
#  include <boost/geometry/extensions/contrib/ttmath_stub.hpp>
#endif

typedef bg::srs::spheroid<double> stype;

typedef bg::strategy::andoyer andoyer_formula;
typedef bg::strategy::thomas thomas_formula;
typedef bg::strategy::vincenty vincenty_formula;

template <typename P>
bool non_precise_ct()
{
    typedef typename bg::coordinate_type<P>::type ct;
    return boost::is_integral<ct>::value || boost::is_float<ct>::value;
}

template <typename P1, typename P2, typename FormulaPolicy>
void test_distance(double lon1, double lat1, double lon2, double lat2)
{
    typedef typename bg::promote_floating_point
        <
            typename bg::select_calculation_type<P1, P2, void>::type
        >::type calc_t;

    calc_t tolerance = non_precise_ct<P1>() || non_precise_ct<P2>() ?
                       5.0 : 0.001;

    P1 p1;
    P2 p2;

    bg::assign_values(p1, lon1, lat1);
    bg::assign_values(p2, lon2, lat2);

    // Test strategy that implements meridian distance against formula
    // that implements general distance
    // That may change in the future but in any case these calls must return
    // the same result

    calc_t dist_formula = FormulaPolicy::template inverse
            <
                double, true, false, false, false, false
            >::apply(lon1 * bg::math::d2r<double>(),
                     lat1 * bg::math::d2r<double>(),
                     lon2 * bg::math::d2r<double>(),
                     lat2 * bg::math::d2r<double>(),
                     stype()).distance;

    bg::strategy::distance::geographic<FormulaPolicy, stype> strategy;
    calc_t dist_strategy = strategy.apply(p1, p2);
    BOOST_CHECK_CLOSE(dist_formula, dist_strategy, tolerance);
}

template <typename P1, typename P2, typename FormulaPolicy>
void test_distance_reverse(double lon1, double lat1,
                           double lon2, double lat2)
{
    test_distance<P1, P2, FormulaPolicy>(lon1, lat1, lon2, lat2);
    test_distance<P1, P2, FormulaPolicy>(lon2, lat2, lon1, lat1);
}

template <typename P1, typename P2, typename FormulaPolicy>
void test_meridian()
{
    test_distance_reverse<P1, P2, FormulaPolicy>(0., 70., 0., 80.);
    test_distance_reverse<P1, P2, FormulaPolicy>(0, 70, 0., -80.);
    test_distance_reverse<P1, P2, FormulaPolicy>(0., -70., 0., 80.);
    test_distance_reverse<P1, P2, FormulaPolicy>(0., -70., 0., -80.);

    test_distance_reverse<P1, P2, FormulaPolicy>(0., 70., 180., 80.);
    test_distance_reverse<P1, P2, FormulaPolicy>(0., 70., 180., -80.);
    test_distance_reverse<P1, P2, FormulaPolicy>(0., -70., 180., 80.);
    test_distance_reverse<P1, P2, FormulaPolicy>(0., -70., 180., -80.);

    test_distance_reverse<P1, P2, FormulaPolicy>(350., 70., 170., 80.);
    test_distance_reverse<P1, P2, FormulaPolicy>(350., 70., 170., -80.);
    test_distance_reverse<P1, P2, FormulaPolicy>(350., -70., 170., 80.);
    test_distance_reverse<P1, P2, FormulaPolicy>(350., -70., 170., -80.);
}

template <typename P>
void test_all()
{
    test_meridian<P, P, andoyer_formula>();
    test_meridian<P, P, thomas_formula>();
    test_meridian<P, P, vincenty_formula>();
}

int test_main(int, char* [])
{
    test_all<bg::model::point<double, 2, bg::cs::geographic<bg::degree> > >();
    test_all<bg::model::point<float, 2, bg::cs::geographic<bg::degree> > >();
    test_all<bg::model::point<int, 2, bg::cs::geographic<bg::degree> > >();

    return 0;
}


#ifndef AMPAR_GILLESPIE_CLASS_HPP_INCLUDED
#define AMPAR_GILLESPIE_CLASS_HPP_INCLUDED

#include <iostream>
#include <cmath>
#include <vector>
#include <ctime>

#include <boost/random/uniform_real.hpp>
#include <boost/random/variate_generator.hpp>
#include <boost/generator_iterator.hpp>
#include <boost/random/linear_congruential.hpp>

#include "stl_vector_operation_functions.hpp"

class ampar_gillespie_class
{
   private:
      unsigned int seed;
      boost::minstd_rand rng;   //Boost random number generator
      boost::uniform_real<double> uni_dist;   //Uniform random number distribution which produces values b/w 0 and 1 (0 inclusive, 1 exlusive).
      boost::variate_generator<boost::minstd_rand&, boost::uniform_real< double> > uni_rand;// variate generator
      //--------------
      double kf1 = 4.59E06;    //M^-1 S^-1
      double kb1 = 4.26E03;    //S^-1
      double kf2 = 28.4E06;    //M^-1 * S^-1
      double kb2 = 3.26E03;    //S^-1
      double kf3 = 1.27E06;    //M^-1 S^-1
      double kb3 = 45.7;       //S^-1
      double a0 = 4.24E03;     //S^-1
      double b0 = 900.0;       //S^-1
      double a1 = 2.89E03;     //S^-1
      double b1 = 39.2;        //S^-1
      double a2 = 172.0;       //S^-1
      double b2 = 0.727;       //S^-1
      double a3 = 17.7;        //S^-1
      double b3 = 4.0;         //S^-1
      double a4 = 16.8;        //S^-1
      double b4 = 190.4;       //S^-1
      double Volume = 1.0;
   public:
      const double avagadro = 6.022e+23;
      const int Nstates = 8;
      const int Nreactions = 16;
      double r1,r2;
      //Construct vectors
      std::vector<int> X;      // A vector to hold the initial number of the receptors in each states [Glu,C0,C1,C2,C3,C4,C5,Open]
      std::vector<double> C;  //A vector to hold the Cmew values
      std::vector<double> H;  //A vector to hold the Hmew values
      //Class constructorsampar_gillespie_class(unsigned int seed_ = std::time(0), std::vector<int> X_):   seed(seed_),
       ampar_gillespie_class(std::vector<int> X_, unsigned int seed_=std::time(0)):   seed(seed_),
                                                                        uni_dist(boost::uniform_real<double>(0,1)),
                                                                        uni_rand(boost::variate_generator<boost::minstd_rand&, boost::uniform_real<double> >(rng, uni_dist)),
                                                                        X(std::move(X_))
      {
         rng.seed(seed);
         C = {kf1*Volume,kb1,kf2*Volume,kb2,a1,b1,kf3*Volume,kb3,a2,b2,a4,b4,a0,b0,b3,a3};
         H = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
      }
      void updateX (const int rx_id);
      void updateH (void);
};

/* Function update H_mew vector with the current number of molecules in each state from the state vector X */
void ampar_gillespie_class::updateH(void)
{
   //Genrate two random numbers from the distribution by means of STL generator interface.
   r1 = uni_rand();
   r2 = uni_rand();

   H[0] = X[0] * X[1];
   H[1] = X[2];
   H[2] = X[2] * X[0];
   H[3] = X[3];
   H[4] = X[2];
   H[5] = X[4];
   H[6] = X[4] * X[0];
   H[7] = X[5];
   H[8] = X[3];
   H[9] = X[5];
   H[10] = X[5];
   H[11] = X[6];
   H[12] = X[3];
   H[13] = X[7];
   H[14] = X[6];
   H[15] = X[7];
}
//Function update the state vector X for a given reaction rx_id
void ampar_gillespie_class::updateX(const int rx_id)
{
   //std::cout << "Reaction " << rx_id << " X in " << X;
   switch(rx_id)
   {
      case 0:
      {
         if((X[0] > 0) && (X[1] > 0)){
            --X[0]; --X[1];
            ++X[2];
         }
         break;
      }
      case 1:
      {
         if(X[2] > 0){
         --X[2];
         ++X[0]; ++X[1];
         }
         break;
      }
      case 2:
      {
         if((X[2] > 0) && (X[0] > 0)){
         --X[2]; --X[0];
         ++X[3];
         }
         break;
      }
      case 3:
      {
         if(X[3] > 0){
         --X[3];
         ++X[2]; ++X[0];
         }
         break;
      }
      case 4:
      {
         if(X[2] > 0){
         --X[2];
         ++X[4];
         }
         break;
      }
      case 5:
      {
         if(X[4] > 0){
         --X[4];
         ++X[2];
         }
         break;
      }
      case 6:
      {
         if((X[4] > 0) && (X[0] > 0)){
         --X[4];
         --X[0];
         ++X[5];
         }
         break;
      }
      case 7:
      {
         if(X[5] > 0){
         --X[5];
         ++X[4];
         ++X[0];
         }
         break;
      }
      case 8:
      {
         if(X[3] > 0){
         --X[3];
         ++X[5];
         }
         break;
      }
      case 9:
      {
         if(X[5] > 0){
         --X[5];
         ++X[3];
         }
         break;
      }
      case 10:
      {
         if(X[5] > 0){
         --X[5];
         ++X[6];
         }
         break;
      }
      case 11:
      {
         if(X[6] > 0){
         --X[6];
         ++X[5];
         }
         break;
      }
      case 12:
      {
         if(X[3] > 0){
         --X[3];
         ++X[7];
         }
         break;
      }
      case 13:
      {
         if(X[7] > 0){
         --X[7];
         ++X[3];
         }
         break;
      }
      case 14:
      {
         if(X[6] > 0){
         --X[6];
         ++X[7];
         }
         break;
      }
      case 15:
      {
         if(X[7] > 0){
         --X[7];
         ++X[6];
         }
         break;
      }
   }
   for(size_t i=0; i<X.size();++i){
      if(X[i] < 0){
         X[i] = 0;
      }
   }
}

#endif // AMPAR_GILLESPIE_CLASS_HPP_INCLUDED


#include "geomutils.h"

#include <stdint.h>
#include <algorithm>
#include <cassert>

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include <boost/geometry/geometries/adapted/boost_tuple.hpp>

BOOST_GEOMETRY_REGISTER_BOOST_TUPLE_CS(cs::cartesian)

std::vector<int> sortEdges(const std::vector<std::pair<int, int>> &edges, const std::vector<double> &weights) {
    std::vector<int> order(edges.size());
    for(int i = 0;i < order.size();i ++) {
        order[i] = i;
    }

    std::sort(order.begin(),order.end(), [weights](int i, int j){
        return (weights[i] < weights[j]);
    });
    return order;
}

void findAngle(const Point &p1, const Point &p2, Transformation &t) {
    Point vec(p2.x - p1.x,p2.y - p1.y);
    double n = len(vec);
    vec.x /= n;
    vec.y /= n;
    t.cos = vec.x;
    t.sin = std::sqrt(1 - t.cos * t.cos);
    if(vec.y >= 0) {
        t.sin = -t.sin;
    }
}

void computeConvexHull(Polygon &pts, Polygon &chull) {
    chull.clear();
    if(pts.size() == 1) {
        chull.push_back(pts[0]);
        chull.push_back(pts[0]);
        return;
    } else if(pts.size() == 2) {
        chull.push_back(pts[0]);
        chull.push_back(pts[1]);
        chull.push_back(pts[0]);
        return;
    }

    typedef boost::tuple<double, double> point;
    typedef boost::geometry::model::multi_point<point> mpoints;
    typedef boost::geometry::model::polygon<point> polygon;

    mpoints mpts;

    for(int i = 0;i < pts.size();i ++) {
        boost::geometry::append(mpts,point(pts[i].x,pts[i].y));
    }
    polygon hull;

    // Polygon is closed
    boost::geometry::convex_hull(mpts, hull);
    for(auto pt : hull.outer()) {
        chull.push_back(Point(pt.get<0>(), pt.get<1>()));
    }
}


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)

**/
//==================================================================================================
#ifndef BOOST_SIMD_IEEE_HPP_INCLUDED
#define BOOST_SIMD_IEEE_HPP_INCLUDED

namespace boost { namespace simd
{
  /*!
    @ingroup group-functions
    @defgroup group-ieee Ieee functions

   These functions provide scalar and SIMD algorithms for inspecting, generating or
   decomposing IEEE 754 floating point numbers.

   Operations such as exponent and mantissa
   extraction, floating point modulo, IEEE bit patterns manipulation and
   magnitude comparison are provided.

<center>
 |                      |                  |                  |                 |                 |
 |:--------------------:|:----------------:|:----------------:|:---------------:|:---------------:|
 | @ref bitfloating     | @ref bitinteger  | @ref bitofsign   | @ref copysign   | @ref eps        |
 | @ref exponentbits    | @ref exponent    | @ref fpclassify  | @ref frac       | @ref frexp      |
 | @ref ifrexp          | @ref ilogb       | @ref ldexp       | @ref mantissa   | @ref maxmag     |
 | @ref maxnum          | @ref maxnummag   | @ref minmag      | @ref minnum     | @ref minnummag  |
 | @ref modf            | @ref negate      | @ref negatenz    | @ref nextafter  | @ref next       |
 | @ref nextpow2        | @ref predecessor | @ref prev        | @ref safe_max   | @ref safe_min   |
 | @ref saturate        | @ref sign        | @ref signnz      | @ref successor  | @ref ulpdist    |
 | @ref ulp             |                  |                  |                 |                 |
</center>
  **/

} }

#include <boost/simd/function/bitfloating.hpp>
#include <boost/simd/function/bitinteger.hpp>
#include <boost/simd/function/bitofsign.hpp>
#include <boost/simd/function/copysign.hpp>
#include <boost/simd/function/eps.hpp>
#include <boost/simd/function/exponentbits.hpp>
#include <boost/simd/function/exponent.hpp>
#include <boost/simd/function/fpclassify.hpp>
#include <boost/simd/function/frac.hpp>
#include <boost/simd/function/frexp.hpp>
#include <boost/simd/function/ifrexp.hpp>
#include <boost/simd/function/ilogb.hpp>
#include <boost/simd/function/ldexp.hpp>
#include <boost/simd/function/mantissa.hpp>
#include <boost/simd/function/maxmag.hpp>
#include <boost/simd/function/maxnum.hpp>
#include <boost/simd/function/maxnummag.hpp>
#include <boost/simd/function/minmag.hpp>
#include <boost/simd/function/minnum.hpp>
#include <boost/simd/function/minnummag.hpp>
#include <boost/simd/function/modf.hpp>
#include <boost/simd/function/negate.hpp>
#include <boost/simd/function/negatenz.hpp>
#include <boost/simd/function/nextafter.hpp>
#include <boost/simd/function/next.hpp>
#include <boost/simd/function/nextpow2.hpp>
#include <boost/simd/function/predecessor.hpp>
#include <boost/simd/function/prev.hpp>
#include <boost/simd/function/safe_max.hpp>
#include <boost/simd/function/safe_min.hpp>
#include <boost/simd/function/saturate.hpp>
#include <boost/simd/function/sbits.hpp>
#include <boost/simd/function/sign.hpp>
#include <boost/simd/function/signnz.hpp>
#include <boost/simd/function/splat.hpp>
#include <boost/simd/function/successor.hpp>
#include <boost/simd/function/ulpdist.hpp>
#include <boost/simd/function/ulp.hpp>

#endif


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2012 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2012 Mateusz Loskot, London, UK.

// 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)
//
// Point Example - showing different type of points

#include <iostream>

#include <boost/geometry/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/adapted/boost_tuple.hpp>
#include <boost/geometry/geometries/adapted/c_array.hpp>
#include <boost/geometry/geometries/adapted/boost_array.hpp>
#include <boost/geometry/geometries/adapted/boost_polygon/point.hpp>

BOOST_GEOMETRY_REGISTER_C_ARRAY_CS(cs::cartesian)
BOOST_GEOMETRY_REGISTER_BOOST_ARRAY_CS(cs::cartesian)
BOOST_GEOMETRY_REGISTER_BOOST_TUPLE_CS(cs::cartesian)

int main()
{
    using namespace boost::geometry;

    // Boost.Geometry contains several point types:
    // 1: its own generic type
    model::point<double, 2, cs::cartesian> pt1;

    // 2: its own type targetted to Cartesian (x,y) coordinates
    model::d2::point_xy<double> pt2;

    // 3: it supports Boost tuple's
    boost::tuple<double, double> pt3;

    // 4: it supports normal arrays
    double pt4[2];

    // 5: it supports arrays-as-points from Boost.Array
    boost::array<double, 2> pt5;

    // 6: it supports points from Boost.Polygon
    boost::polygon::point_data<double> pt6;

    // 7: in the past there was a typedef point_2d
    //    But users are now supposted to do that themselves:
    typedef model::d2::point_xy<double> point_2d;
    point_2d pt7;

    // 7: there are more variants, and you can create your own.
    //    (see therefore the custom_point example)

    // All these types are handled the same way. We show here
    // assigning them and calculating distances.
    assign_values(pt1, 1, 1);
    assign_values(pt2, 2, 2);
    assign_values(pt3, 3, 3);
    assign_values(pt4, 4, 4);
    assign_values(pt5, 5, 5);
    assign_values(pt6, 6, 6);
    assign_values(pt7, 7, 7);

    double d1 = distance(pt1, pt2);
    double d2 = distance(pt3, pt4);
    double d3 = distance(pt5, pt6);
    std::cout << "Distances: "
        << d1 << " and " << d2 << " and " << d3 << std::endl;

    // (in case you didn't note, distances can be calculated
    //  from points with different point-types)

    // Several ways of construction and setting point values
    // 1: default, empty constructor, causing no initialization at all
    model::d2::point_xy<double> p1;

    // 2: as shown above, assign_values
    model::d2::point_xy<double> p2;
    assign_values(p2, 1, 1);

    // 3: using "set" function
    //    set uses the concepts behind, such that it can be applied for
    //    every point-type (like assign_values)
    model::d2::point_xy<double> p3;
    set<0>(p3, 1);
    set<1>(p3, 1);
    // set<2>(p3, 1); //will result in compile-error

    // 3: for any point type, and other geometry objects:
    //    there is the "make" object generator
    //    (this one requires to specify the point-type).
    model::d2::point_xy<double> p4 = make<model::d2::point_xy<double> >(1,1);

    // 5: for the d2::point_xy<...> type only: constructor with two values
    model::d2::point_xy<double> p5(1,1);

    // 6: for boost tuples you can of course use make_tuple

    // Some ways of getting point values

    // 1: using the "get" function following the concepts behind
    std::cout << get<0>(p2) << "," << get<1>(p2) << std::endl;

    // 2: for point_xy only
    std::cout << p2.x() << "," << p2.y() << std::endl;

    // 3: using boost-tuples you of course can boost-tuple-methods
    std::cout << pt3.get<0>() << "," << pt3.get<1>() << std::endl;

    // 4: Boost.Geometry supports various output formats, e.g. DSV
    //    (delimiter separated values)
    std::cout << dsv(pt3) << std::endl;

    // There are 3-dimensional points too
    model::point<double, 3, cs::cartesian> d3a, d3b;
    assign_values(d3a, 1, 2, 3);
    assign_values(d3b, 4, 5, 6);
    d3 = distance(d3a, d3b);

    // Other examples show other types of points, geometries and more algorithms

    return 0;
}


#define BOOST_TEST_MAIN
#include <boost/test/unit_test.hpp>
#include <occutils/Line.hxx>
#include <occutils/Direction.hxx>
#include <occutils/PrintOCC.hxx>
#include <occutils/Equality.hxx>

BOOST_AUTO_TEST_CASE( LineParallel2d )
{
    // A line should be parallel to itself
    gp_Lin2d linX(gp_Pnt2d(0, 0), OCCUtils::Direction::X2d());
    BOOST_CHECK(OCCUtils::Line::IsParallel(linX, linX));

    // A line should be parallel to its reverse line
    gp_Lin2d linMinusX = linX.Reversed();
    BOOST_CHECK(OCCUtils::Line::IsParallel(linX, linMinusX));
    BOOST_CHECK(OCCUtils::Line::IsParallel(linMinusX, linX));

    // Two perpendicular lines should not be parallel
    gp_Lin2d linY(gp_Pnt2d(0, 0), OCCUtils::Direction::Y2d());
    BOOST_CHECK(!OCCUtils::Line::IsParallel(linX, linY));
    BOOST_CHECK(!OCCUtils::Line::IsParallel(linY, linX));
    BOOST_CHECK(!OCCUtils::Line::IsParallel(linMinusX, linY));
    BOOST_CHECK(!OCCUtils::Line::IsParallel(linY, linMinusX));
}

BOOST_AUTO_TEST_CASE( LineIntersection2D )
{
    // Intersect between a line and itself has infinite points
    gp_Lin2d lin1(gp_Pnt2d(0, 0), OCCUtils::Direction::X2d());
    auto result = OCCUtils::Line::Intersection(lin1, lin1);
    BOOST_CHECK(!result.has_value());

    // Two intersecting lines should have an intersection point
    gp_Lin2d lin2(gp_Pnt2d(0, -1), OCCUtils::Direction::Y2d());
    result = OCCUtils::Line::Intersection(lin1, lin2);
    BOOST_CHECK(result.has_value());
    BOOST_CHECK_EQUAL(result.value(), gp_Pnt2d(0, 0));

    // Two more intersecting lines that do not intersect @ origin
    gp_Lin2d lin3(gp_Pnt2d(0, 1), OCCUtils::Direction::X2d());
    result = OCCUtils::Line::Intersection(lin2, lin3);
    BOOST_CHECK(result.has_value());
    BOOST_CHECK_EQUAL(result.value(), gp_Pnt2d(0, 1));

    // ... same as last test but argument order inverted (same result)
    result = OCCUtils::Line::Intersection(lin3, lin2);
    BOOST_CHECK(result.has_value());
    BOOST_CHECK_EQUAL(result.value(), gp_Pnt2d(0, 1));
}


// Boost.Geometry (aka GGL, Generic Geometry Library)
// Unit Test

// Copyright (c) 2021 Barend Gehrels, Amsterdam, the Netherlands.

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

#define BOOST_GEOMETRY_NO_ROBUSTNESS

#include <boost/multiprecision/cpp_bin_float.hpp>
#include <boost/multiprecision/cpp_dec_float.hpp>

#include <geometry_test_common.hpp>
#include <algorithms/overlay/multi_overlay_cases.hpp>

#include <boost/geometry/algorithms/correct.hpp>
#include <boost/geometry/algorithms/union.hpp>
#include <boost/geometry/io/wkt/wkt.hpp>
#include <boost/geometry/geometries/geometries.hpp>

#include <algorithms/overlay/multi_overlay_cases.hpp>

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

#include <boost/geometry/util/rational.hpp>

#include <set>

enum class exclude { all, rectangular, diagonal, hard, fp };

template <typename Geometry, typename Expected>
void test_one(std::string const& case_id,
              std::string const& wkt1, std::string const& wkt2,
              bool debug,
              Expected const& expected_area,
              Expected const& expected_max = -1)
{
    using coor_t = typename bg::coordinate_type<Geometry>::type;
    Geometry g1, g2, clip;

    bg::read_wkt(wkt1, g1);
    bg::read_wkt(wkt2, g2);

    bg::correct(g1);
    bg::correct(g2);

    bg::union_(g1, g2, clip);

    auto const area = bg::area(clip);
    if (debug)
    {
      std::cout << "AREA: " << std::setprecision(64) << area
                << " expected " << expected_area
                << " types coordinate " << string_from_type<coor_t>::name()
                << " area " << typeid(decltype(area)).name()
                << " expected " << typeid(Expected).name()
                << " size " << sizeof(coor_t)
                << std::endl;
    }

    // Check areas, they always have to be specified in integer for this test
    // and therefore the checking (including a tolerance) is different
    bool const ok = expected_max == -1
                    ? bg::math::equals(area, expected_area)
                    : bg::math::larger_or_equals(area, expected_area)
                      && bg::math::smaller_or_equals(area, expected_max);
    BOOST_CHECK_MESSAGE(ok,
            "union: " << case_id
            << " area: expected: " << expected_area
            << " detected: " << area
            << " type: " << (string_from_type<coor_t>::name())
            << " (" << (typeid(coor_t).name()) << ")");
}

template <typename Point>
void test_areal(std::set<exclude> const& exclude = {}, bool debug = false)
{
    using polygon = bg::model::polygon<Point>;
    using multi_polygon = bg::model::multi_polygon<polygon>;

    // Intended tests: only 3:
    // - simple case having only horizontal/vertical lines ("rectangular")
    // - simple case on integer grid but also having diagonals ("diagonal")
    // - case going wrong for <float> ("hard")

    if (exclude.count(exclude::rectangular)
        + exclude.count(exclude::all) == 0)
    {
        test_one<multi_polygon>("case_multi_rectangular",
            case_multi_rectangular[0], case_multi_rectangular[1], debug, 33125);
    }
    if (exclude.count(exclude::diagonal)
        + exclude.count(exclude::all) == 0)
    {
        test_one<multi_polygon>("case_multi_diagonal",
            case_multi_diagonal[0], case_multi_diagonal[1], debug, 5350);
    }
    if (exclude.count(exclude::hard)
        + exclude.count(exclude::fp)
        + exclude.count(exclude::all) == 0)
    {
        test_one<multi_polygon>("case_multi_hard",
            case_multi_hard[0], case_multi_hard[1], debug, 21, 23);
    }
}

int test_main(int, char* [])
{
    namespace bm = boost::multiprecision;

    using bg::model::d2::point_xy;

#if ! defined(BOOST_GEOMETRY_TEST_ONLY_ONE_TYPE)
    // Standard floating point types
    test_areal<point_xy<float>>({exclude::hard});
    test_areal<point_xy<double>>({});
    test_areal<point_xy<long double>>({});

    // Standard integer types
    test_areal<point_xy<std::int16_t>>({exclude::fp});
    test_areal<point_xy<std::int32_t>>({exclude::fp});
#endif
    test_areal<point_xy<std::int64_t>>({exclude::fp});

    // Boost multi precision (integer)
    test_areal<point_xy<bm::int128_t>>({exclude::fp});
#if ! defined(BOOST_GEOMETRY_TEST_ONLY_ONE_TYPE)
    test_areal<point_xy<bm::checked_int128_t>>({exclude::fp});
#endif

    // Boost multi precision (floating point)
#if ! defined(BOOST_GEOMETRY_TEST_ONLY_ONE_TYPE)
    test_areal<point_xy<bm::number<bm::cpp_bin_float<5>>>>();
    test_areal<point_xy<bm::number<bm::cpp_bin_float<10>>>>();
    test_areal<point_xy<bm::number<bm::cpp_bin_float<50>>>>();
#endif
    test_areal<point_xy<bm::number<bm::cpp_bin_float<100>>>>();

    test_areal<point_xy<bm::number<bm::cpp_dec_float<50>>>>({});

    // Boost multi precision (rational)
    test_areal<point_xy<bm::cpp_rational>>({exclude::fp});
#if ! defined(BOOST_GEOMETRY_TEST_ONLY_ONE_TYPE)
    test_areal<point_xy<bm::checked_cpp_rational>>({exclude::fp});
#endif

    // Boost multi precision float128 wrapper, is currently NOT supported
    // and it is limited to certain compilers anyway
    // test_areal<point_xy<bm::float128>>();

    // Boost rational (tests compilation)
    // (the rectangular case is correct; other input might give wrong results)
    // The int16 version throws a <zero denominator> exception
#if ! defined(BOOST_GEOMETRY_TEST_ONLY_ONE_TYPE)
    test_areal<point_xy<boost::rational<std::int16_t>>>({exclude::all});
    test_areal<point_xy<boost::rational<std::int32_t>>>({exclude::fp});
#endif
    test_areal<point_xy<boost::rational<std::int64_t>>>({exclude::fp});

    return 0;
}


#ifndef STAN_MATH_PRIM_MAT_FUN_VARIANCE_HPP
#define STAN_MATH_PRIM_MAT_FUN_VARIANCE_HPP

#include <stan/math/prim/arr/err/check_nonzero_size.hpp>
#include <stan/math/prim/mat/fun/Eigen.hpp>
#include <stan/math/prim/mat/fun/mean.hpp>
#include <boost/math/tools/promotion.hpp>
#include <vector>

namespace stan {
namespace math {

/**
 * Returns the sample variance (divide by length - 1) of the
 * coefficients in the specified standard vector.
 * @param v Specified vector.
 * @return Sample variance of vector.
 * @throws std::domain_error if the size of the vector is less
 * than 1.
 */
template <typename T>
inline return_type_t<T> variance(const std::vector<T>& v) {
  check_nonzero_size("variance", "v", v);
  if (v.size() == 1) {
    return 0.0;
  }
  T v_mean(mean(v));
  T sum_sq_diff(0);
  for (size_t i = 0; i < v.size(); ++i) {
    T diff = v[i] - v_mean;
    sum_sq_diff += diff * diff;
  }
  return sum_sq_diff / (v.size() - 1);
}

/**
 * Returns the sample variance (divide by length - 1) of the
 * coefficients in the specified column vector.
 * @param m Specified vector.
 * @return Sample variance of vector.
 */
template <typename T, int R, int C>
inline return_type_t<T> variance(const Eigen::Matrix<T, R, C>& m) {
  check_nonzero_size("variance", "m", m);

  if (m.size() == 1) {
    return 0.0;
  }
  return_type_t<T> mn(mean(m));
  return_type_t<T> sum_sq_diff(0);
  for (int i = 0; i < m.size(); ++i) {
    return_type_t<T> diff = m(i) - mn;
    sum_sq_diff += diff * diff;
  }
  return sum_sq_diff / (m.size() - 1);
}

}  // namespace math
}  // namespace stan
#endif


//==================================================================================================
/*
  Copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================

#include <boost/simd/function/multiplies.hpp>
#include <boost/simd/function/plus.hpp>
#include <boost/simd/function/splat.hpp>
#include <boost/simd/pack.hpp>

//! [scalar-dot]
template <typename Value>
Value dot(Value* first1, Value* last1, Value* first2)
{
  Value v(0);

  for (; first1 < last1; ++first1, ++first2) {
    v += (*first1) * (*first2);
  }

  return v;
}
//! [scalar-dot]


#pragma once
#include <vector>
#include <random>
#include <Eigen/Core>

namespace mdk {
    /**
     * A random number generator. We use two versions: a legacy version taken
     * from Fortran, and a modern version. Aside from sampling from [0, 1], we
     * add sampling from [a, b], N(0, 1), N(mu, sigma^2) or points on S^2.
     */
    class Random {
#ifdef LEGACY_MODE
    private:
        static constexpr int
            im1 = 2147483563,
            im2 = 2147483399,
            imm1 = im1-1,
            ia1 = 40014,
            ia2 = 40692,
            iq1 = 53668,
            iq2 = 52774,
            ir1 = 12211,
            ir2 = 3791,
            ntab = 32,
            ndiv = 1+imm1/ntab;

        static constexpr double
            eps = 1.2e-7,
            rnmx = 1.-eps,
            am = (float) 1.0/im1;

        int iy = 0, idum = -448, idum2 = 123456789;
        int iv[ntab];
    public:
        Random(int seed) {
            idum = -seed;
            for (auto& x: iv) x = 0;
        }
        inline double uniform() {
            int k, j;
            if (idum <= 0) {
                idum2 = idum = std::max(-idum, 1);
                for (j = ntab + 7; j >= 0; --j) {
                    k = idum / iq1;
                    idum = ia1 * (idum - k * iq1) - k * ir1;
                    if (idum < 0) idum += im1;
                    if (j < ntab) iv[j] = idum;
                }
                iy = iv[0];
            }

            k = idum / iq1;
            idum = ia1 * (idum - k * iq1) - k * ir1;
            if (idum < 0) idum += im1;

            k = idum2 / iq2;
            idum2 = ia2 * (idum2 - k * iq2) - k * ir2;
            if (idum2 < 0) idum2 += im2;

            j = iy / ndiv;
            iy = iv[j] - idum2;
            iv[j] = idum;
            if (iy < 1) iy += imm1;

            return std::min(am * iy, rnmx);
        }
#else
    private:
        uint64_t state;
        Random() = default;

    public:
        Random(int seed) {
            assert (seed > 0);
            state = seed;

            // Shuffle the state a bit
            for (int i = 0; i < 100; ++i) uniform();
        }

        inline double uniform() {
            static const double inv = 1.0 / (double) (1ull << 32);
            uint64_t result = state * 0xd989bcacc137dcd5ull;
            state ^= state >> 11;
            state ^= state << 31;
            state ^= state >> 18;
            double res = (uint32_t) (result >> 32ull);
            return res * inv;
        }

        /* This function is used to create a new random object
           that won't generate similar values.
           Uses splitmix64 to create new state */
        Random getNewRandom() {
            uint64_t result = state;
            result = (result ^ (result >> 30)) * 0xBF58476D1CE4E5B9;
            result = (result ^ (result >> 27)) * 0x94D049BB133111EB;
            result = result ^ (result >> 31);
            Random rng;
            rng.state = result;
            return rng;
        }

#endif
        Random(Random const& oth) = default;

        inline double uniform(double a, double b) {
            return a + (b - a) * uniform();
        }

        inline double normal() {
            double r1 = uniform();
            double r2 = uniform();
            return sqrt(-2.0  * log(r1)) * cos(2.0 * M_PI * r2);
        }

        inline std::pair<double, double> two_normals() {
            double r1 = uniform();
            double r2 = uniform();
            double r = sqrt(-2.0  * log(r1));
            double ang = 2.0 * M_PI * r2;
            return {r * cos(ang), r * sin(ang)};
        }

        inline double normal(double mu, double sigma) {
            return mu + sigma * normal();
        }

        inline Eigen::Vector3d sphere() {
            Eigen::Vector3d r { normal(), normal(), normal() };
            return r.normalized();
        }
    };
}