#ifndef STAN_MATH_PRIM_SCAL_PROB_BETA_RNG_HPP
#define STAN_MATH_PRIM_SCAL_PROB_BETA_RNG_HPP

#include <boost/math/special_functions/gamma.hpp>
#include <boost/random/gamma_distribution.hpp>
#include <boost/random/uniform_real_distribution.hpp>
#include <boost/random/variate_generator.hpp>
#include <stan/math/prim/scal/err/check_consistent_sizes.hpp>
#include <stan/math/prim/scal/err/check_less_or_equal.hpp>
#include <stan/math/prim/scal/err/check_nonnegative.hpp>
#include <stan/math/prim/scal/err/check_not_nan.hpp>
#include <stan/math/prim/scal/err/check_positive_finite.hpp>
#include <stan/math/prim/scal/fun/log1m.hpp>
#include <stan/math/prim/scal/fun/multiply_log.hpp>
#include <stan/math/prim/scal/fun/log_sum_exp.hpp>
#include <stan/math/prim/scal/fun/value_of.hpp>
#include <stan/math/prim/scal/fun/digamma.hpp>
#include <stan/math/prim/scal/fun/lgamma.hpp>
#include <stan/math/prim/scal/fun/lbeta.hpp>
#include <stan/math/prim/scal/fun/constants.hpp>
#include <stan/math/prim/scal/meta/include_summand.hpp>
#include <stan/math/prim/scal/fun/grad_reg_inc_beta.hpp>
#include <stan/math/prim/scal/fun/inc_beta.hpp>

namespace stan {
  namespace math {

    /**
     * Return a pseudorandom Beta variate with the supplied success and failure
     * parameters and specified random number generator.
     *
     * @tparam RNG class of random number generator
     * @param alpha positive finite success parameter
     * @param beta positive finite failure parameter
     * @param rng random number generator
     * @return Beta random variate
     * @throw std::domain_error if alpha or beta is nonpositive
     */
    template <class RNG>
    inline double
    beta_rng(double alpha,
             double beta,
             RNG& rng) {
      using boost::variate_generator;
      using boost::random::gamma_distribution;
      using boost::random::uniform_real_distribution;
      using std::log;
      using std::exp;
      static const char* function("beta_rng");
      check_positive_finite(function, "First shape parameter", alpha);
      check_positive_finite(function, "Second shape parameter", beta);

      // If alpha and beta are large, trust the usual ratio of gammas
      // method for generating beta random variables. If any parameter
      // is small, work in log space and use Marsaglia and Tsang's trick
      if (alpha > 1.0 && beta > 1.0) {
        variate_generator<RNG&, gamma_distribution<> >
          rng_gamma_alpha(rng, gamma_distribution<>(alpha, 1.0));
        variate_generator<RNG&, gamma_distribution<> >
          rng_gamma_beta(rng, gamma_distribution<>(beta, 1.0));
        double a = rng_gamma_alpha();
        double b = rng_gamma_beta();
        return a / (a + b);
      } else {
        variate_generator<RNG&, uniform_real_distribution<> >
          uniform_rng(rng, uniform_real_distribution<>(0.0, 1.0));
        variate_generator<RNG&, gamma_distribution<> >
          rng_gamma_alpha(rng, gamma_distribution<>(alpha + 1, 1.0));
        variate_generator<RNG&, gamma_distribution<> >
          rng_gamma_beta(rng, gamma_distribution<>(beta + 1, 1.0));
        double log_a = log(uniform_rng()) / alpha + log(rng_gamma_alpha());
        double log_b = log(uniform_rng()) / beta + log(rng_gamma_beta());
        double log_sum = log_sum_exp(log_a, log_b);
        return exp(log_a - log_sum);
      }
    }

  }
}
#endif


/*  Sirikata
 *  ExpIntegral.cpp
 *
 *  Copyright (c) 2010, Daniel Reiter Horn
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions are
 *  met:
 *  * Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 *  * Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 *  * Neither the name of Sirikata nor the names of its contributors may
 *    be used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
 * IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
 * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
 * PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER
 * OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
 * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
 * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
 * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

#include <sirikata/core/util/Platform.hpp>
#include "ExpIntegral.hpp"

#ifndef SIRIKATA_BAD_BOOST_ERF
#include <boost/math/special_functions/erf.hpp>
#endif

#include <iostream>
#include <string.h>

static double square(double x) {
    return x*x;
}

static double expFunction(double k,double x, double y, double u, double v) {
    return exp(-k*(square(x-u)+square(y-v)));
}
static double myerf(double value) {
#ifndef SIRIKATA_BAD_BOOST_ERF
    return boost::math::erf(value);
#else
    return 0;
#endif
}
static double integralExpFunction(double k, double xmin, double xmax, double ymin, double ymax, double umin, double umax, double vmin, double vmax) {
    double e=2.7182818284590451;
    double j=xmin;
    double l=xmax;
    double m=ymin;
    double n=ymax;
    double p=umin;
    double q=umax;
    double r=vmin;
    double s=vmax;
    double pi=3.1415926535897931;
    double sqrtloge=1;
    double loge=1;
    return (1./(4*k*log(e)))
        *pi*(  ( j - p)*myerf(sqrt(k)*(j - p)*sqrtloge)
             + (-l + p)*myerf(sqrt(k)*(l - p)*sqrtloge)
             + (-j + q)*myerf(sqrt(k)*(j - q)*sqrtloge)
             + ( l - q)*myerf(sqrt(k)*(l - q)*sqrtloge)
               + (exp(-k*square(j - p))
                  - exp(-k*square(j - q)))/(sqrt(k)*sqrt(pi)*sqrtloge) +
               (-exp(-k*square(l - p)) + exp(-k*square(l - q)))/(sqrt(k)*sqrt(pi)*sqrtloge))
        *(  ( m - r)*myerf(sqrt(k)*(m - r)*sqrtloge)
            + (-n + r)*myerf(sqrt(k)*(n - r)*sqrtloge)
            + (-m + s)*myerf(sqrt(k)*(m - s)*sqrtloge)
            + ( n - s)*myerf(sqrt(k)*(n - s)*sqrtloge)
            + (exp(-k*square(m - r)) - exp(-k*square(m - s)))/(sqrt(k)*sqrt(pi)*sqrtloge)
            + (-exp(-k*square(n - r)) + exp(-k*square(n - s)))/(sqrt(k)*sqrt(pi)*sqrtloge));
}
namespace Sirikata {
double integralExpFunction(double k, const Vector3d& xymin, const Vector3d& xymax, const Vector3d& uvmin, const Vector3d& uvmax){
    return ::integralExpFunction(k,xymin.x,xymax.x,xymin.y,xymax.y,uvmin.x,uvmax.x,uvmin.y,uvmax.y);
}
}
int maino (int argc, char**argv) {
    float k=atof(argv[1]);

    float xmin=-1;
    float xmax= 1;
    float ymin=-1;
    float ymax= 1;
    float arg=atof(argv[2]);
    float umin=-arg;
    float umax= arg;
    float vmin=-arg;
    float vmax= arg;

    std::cout << " Function evaluated at hundred: "<<
        expFunction(1,100,100,100,100)<<
        " Integration of function over a lot "<<integralExpFunction(k,
                                xmin,
                                xmax,
                                ymin,
                                ymax,
                                umin,
                                umax,
                                vmin,
                                vmax);
    return 0;
}


//==================================================================================================
/*!
  @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_ARCH_COMMON_DETAIL_SIMD_F_INVTRIG_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_DETAIL_SIMD_F_INVTRIG_HPP_INCLUDED

#include <boost/simd/function/horn.hpp>
#include <boost/simd/function/logical_and.hpp>
#include <boost/simd/constant/constant.hpp>
#include <boost/simd/constant/half.hpp>
#include <boost/simd/constant/mhalf.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/pi.hpp>
#include <boost/simd/constant/pio_2.hpp>
#include <boost/simd/detail/constant/pio_2lo.hpp>
#include <boost/simd/constant/pio_3.hpp>
#include <boost/simd/constant/pio_4.hpp>
#include <boost/simd/detail/constant/pio_4lo.hpp>
#include <boost/simd/constant/tan_3pio_8.hpp>
#include <boost/simd/constant/tanpio_8.hpp>
#include <boost/simd/constant/two.hpp>
#include <boost/simd/constant/twopio_3.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/function/abs.hpp>
#include <boost/simd/function/bitofsign.hpp>
#include <boost/simd/function/bitwise_xor.hpp>
#include <boost/simd/function/fma.hpp>
#include <boost/simd/function/ifnot_plus.hpp>
#include <boost/simd/function/if_plus.hpp>
#include <boost/simd/function/is_eqz.hpp>
#include <boost/simd/function/is_inf.hpp>
#include <boost/simd/function/dec.hpp>
#include <boost/simd/function/oneminus.hpp>
#include <boost/simd/function/inc.hpp>
#include <boost/simd/function/rec.hpp>
#include <boost/simd/function/sqr.hpp>
#include <boost/simd/function/sqrt.hpp>

namespace boost { namespace simd
{
  namespace detail
  {
    namespace bd =  boost::dispatch;
    namespace bs =  boost::simd;

  template < class A0 >
  struct invtrig_base<A0,tag::radian_tag,tag::simd_type, float>
  {
    static BOOST_FORCEINLINE A0 asin(A0 const& a0)
    {
      A0 sgn, x;
      x = bs::abs(a0);
      sgn = bs::bitofsign(a0);
      const auto x_larger_05 = x > bs::Half<A0>();
      A0 z = if_else(x_larger_05, bs::Half<A0>()*bs::oneminus(x), bs::sqr(x));
      x = if_else(x_larger_05, sqrt(z), x);
      // Remez polynomial of degree 4 for (asin(rx)-rx)/(rx*rx*rx) in [0, 0.25]
      // 2120752146 values (99.53%) within 0.0 ULPs
      //    9954286 values (0.47%)  within 0.5 ULPs
      // 4.0 cycles/element SSE4.2 g++-4.8
      A0 z1 = horn<A0,
        0x3e2aaae4,
        0x3d9980f6,
        0x3d3a3ec7,
        0x3cc617e3,
        0x3d2cb352
        > (z);
      z1 = bs::fma(z1, z*x, x);
      z = if_else(x_larger_05, bs::Pio_2<A0>()-(z1+z1), z1);
      return bs::bitwise_xor(z, sgn);
    }

    static BOOST_FORCEINLINE A0 acos(const A0& a0)
    {
      // 2130706432 values computed.
      // 1968272987 values (92.38%) within 0.0 ULPs
      //  162433445 values (7.62%)  within 0.5 ULPs
      // 8.5 cycles/element SSE4.2 g++-4.8
      A0 x = bs::abs(a0);
      auto x_larger_05 = is_greater(x, bs::Half<A0>());
      x  = if_else(x_larger_05, bs::sqrt(fma(bs::Mhalf<A0>(), x, bs::Half<A0>())), a0);
      x  = asin(x);
      x =  if_plus(x_larger_05, x, x);
      x  = bs::if_else(a0 < bs::Mhalf<A0>(), bs::Pi<A0>()-x, x);
      return bs::if_else(x_larger_05, x, bs::Pio_2<A0>()-x);
    }

    static BOOST_FORCEINLINE A0 atan(const A0& a0)
    {
      A0 absa0 =  bs::abs(a0);
      const A0 x  = kernel_atan(absa0, bs::rec(absa0));
      return bs::bitwise_xor(x, bs::bitofsign(a0));
    }

    static BOOST_FORCEINLINE A0 acot(const A0& a0)
    {
      A0 absa0 =  bs::abs(a0);
      const A0 x  = kernel_atan(bs::rec(absa0), absa0);
      return bs::bitwise_xor(x, bs::bitofsign(a0));
    }

    static BOOST_FORCEINLINE A0 kernel_atan(const A0&  x, const A0& recx)
    {
      //4278190076 values computed  in range: [-3.40282e+38, 3.40282e+38]
      //4257598358 values (99.52%)  within 0.0 ULPs
      //  20591718 values (0.48%)   within 0.5 ULPs

      //here x is positive
      const auto flag1 = x < Tan_3pio_8<A0>();
      const auto flag2 = bs::logical_and(x >= Constant<A0, 0x3ed413cd>(), flag1);
      A0 yy =  bs::if_zero_else(flag1, Pio_2<A0>());
      yy =  bs::if_else(flag2, Pio_4<A0>(), yy);
      A0 xx =   bs::if_else(flag1, x, -recx);
      xx =  bs::if_else(flag2, (bs::dec(x)/bs::inc(x)),xx);
      const A0 z = bs::sqr(xx);
      A0 z1 = horn<A0
        , 0xbeaaaa2aul  // -3.3333293e-01
        , 0x3e4c925ful  //  1.9991724e-01
        , 0xbe0e1b85ul  // -1.4031009e-01
        , 0x3da4f0d1ul  //  8.5460119e-02
        > (z);
      z1 = bs::fma(xx, bs::multiplies( z1, z), xx);
      z1 = if_plus(flag2, z1, Pio_4lo<A0>());
      z1 = ifnot_plus(flag1, z1, Pio_2lo<A0>());
      return yy+z1;
    }
  };
}
} }
#endif


#include "vtkReferenceFrameFilter.h"
#include <vtkImageData.h>
#include <vtkObjectFactory.h>
#include <vtkStreamingDemandDrivenPipeline.h>
#include <vtkInformationVector.h>
#include <vtkInformation.h>
#include <vtkDataObject.h>
#include <vtkSmartPointer.h>
#include <vtkPointData.h>
#include <vtkDataArray.h>
#include <vtkVector.h>
#include <Eigen\Core>
#include <Eigen\StdVector>
#include <Eigen\QR>
#include <vector>
#include <vtkFloatArray.h>

vtkStandardNewMacro(vtkReferenceFrameFilter);

// Creates a 2x2 matrix from 2 column vectors
static Eigen::Matrix2d make_Matrix2d(const Eigen::Vector2d& c0, const Eigen::Vector2d& c1)
{
	Eigen::Matrix2d M;
	M << c0.x(), c1.x(), c0.y(), c1.y();
	return M;
}

// Create a 2x2 matrix from row-wise components
static Eigen::Matrix2d make_Matrix2d(const double& m00, const double& m01, const double& m10, const double& m11)
{
	Eigen::Matrix2d M;
	M << m00, m01, m10, m11;
	return M;
}

// Returns value of Binomial Coefficient C(n, k)
static int binomialCoeff(int n, int k)
{
	int res = 1;
	if (k > n - k)
		k = n - k;
	for (int i = 0; i < k; ++i) {
		res *= (n - i);
		res /= (i + 1);
	}
	return res;
}

static int factorial(int n)
{
	int ret = 1;
	for (int i = 1; i <= n; ++i)
		ret *= i;
	return ret;
}

static void ComputeDisplacementSystemMatrixCoefficients(int i, int j, const Eigen::Vector2d& xx, const Eigen::Vector2d& vv, const Eigen::Matrix2d& J, Eigen::Matrix2d& f, Eigen::Matrix2d& g)
{
	f = make_Matrix2d(0, 0, 0, 0), g = make_Matrix2d(0, 0, 0, 0);
	Eigen::Matrix2d I = make_Matrix2d(1, 0, 0, 1);
	double x = xx.x(), y = xx.y(), u = vv.x(), v = vv.y();

	double x_i = pow(x, i), x_i1 = pow(x, i - 1);
	double y_j = pow(y, j), y_j1 = pow(y, j - 1);

	int fac_i = factorial(i), fac_i1 = factorial(i - 1);
	int fac_j = factorial(j), fac_j1 = factorial(j - 1);

	f -= x_i * y_j / (fac_i * fac_j) * J;
	if (i != 0)
		f += u * x_i1*y_j / (fac_i1 * fac_j) * I;
	if (j != 0)
		f += v * x_i*y_j1 / (fac_i * fac_j1) * I;

	g = x_i * y_j / (fac_i * fac_j) * I;
}

vtkReferenceFrameFilter::vtkReferenceFrameFilter() : NeighborhoodU(10), Invariance(AffineInvariance), UseSummedAreaTables(true), TaylorOrder(2),
FieldNameV(NULL), FieldNameVx(NULL), FieldNameVy(NULL), FieldNameVt(NULL)
{
	SetFieldNameV("v");
	SetFieldNameVx("vx");
	SetFieldNameVy("vy");
	SetFieldNameVt("vt");
}

int vtkReferenceFrameFilter::RequestData(vtkInformation *vtkNotUsed(request),
	vtkInformationVector **inputVector,
	vtkInformationVector *outputVector)
{
	using namespace Eigen;

	// Get the info objects
	vtkInformation *inInfo = inputVector[0]->GetInformationObject(0);
	vtkInformation *outInfo = outputVector->GetInformationObject(0);

	// Get the input and ouptut
	vtkImageData *input = vtkImageData::SafeDownCast(inInfo->Get(vtkDataObject::DATA_OBJECT()));
	vtkImageData *output = vtkImageData::SafeDownCast(outInfo->Get(vtkDataObject::DATA_OBJECT()));

	// Get the information on the domain
	int* dims = input->GetDimensions();
	double* spacing = input->GetSpacing();
	double* boundsMin = input->GetOrigin();

	// select the system matrix size depending on the chosen invariance
	int systemSize = 6;
	switch (Invariance) {
	default:
	case Objectivity:			systemSize = 6;		break;
	case SimilarityInvariance:	systemSize = 8;		break;
	case AffineInvariance:		systemSize = 12;	break;
	case Displacement:			systemSize = 2 * TaylorOrder*TaylorOrder + TaylorOrder * 6 + 4; break;
	}

	// read the input data and abort if data is not present!
	float* input_v = NULL, *input_vx = NULL, *input_vy = NULL, *input_vt = NULL;
	if (vtkFloatArray::SafeDownCast(input->GetPointData()->GetArray(FieldNameV)) && input->GetPointData()->GetArray(FieldNameV)->GetNumberOfComponents() == 2)
		input_v = vtkFloatArray::SafeDownCast(input->GetPointData()->GetArray(FieldNameV))->GetPointer(0);
	else { cout << "Field " << FieldNameV << " was not found or does not have 2 components!" << endl; return 0; }

	if (vtkFloatArray::SafeDownCast(input->GetPointData()->GetArray(FieldNameVx)) && input->GetPointData()->GetArray(FieldNameVx)->GetNumberOfComponents() == 2)
		input_vx = vtkFloatArray::SafeDownCast(input->GetPointData()->GetArray(FieldNameVx))->GetPointer(0);
	else { cout << "Field " << FieldNameVx << " was not found or does not have 2 components!" << endl; return 0; }

	if (vtkFloatArray::SafeDownCast(input->GetPointData()->GetArray(FieldNameVy)) && input->GetPointData()->GetArray(FieldNameVy)->GetNumberOfComponents() == 2)
		input_vy = vtkFloatArray::SafeDownCast(input->GetPointData()->GetArray(FieldNameVy))->GetPointer(0);
	else { cout << "Field " << FieldNameVy << " was not found or does not have 2 components!" << endl; return 0; }

	if (vtkFloatArray::SafeDownCast(input->GetPointData()->GetArray(FieldNameVt)) && input->GetPointData()->GetArray(FieldNameVt)->GetNumberOfComponents() == 2)
		input_vt = vtkFloatArray::SafeDownCast(input->GetPointData()->GetArray(FieldNameVt))->GetPointer(0);
	else { cout << "Field " << FieldNameVt << " was not found or does not have 2 components!" << endl; return 0; }

	// Create an image to write the result to and copy the input into it (just for initialization)
	vtkSmartPointer<vtkImageData> image = vtkSmartPointer<vtkImageData>::New();
	image->DeepCopy(input);

	vtkDataArray* output_v = image->GetPointData()->GetArray(FieldNameV);
	vtkDataArray* output_vx = image->GetPointData()->GetArray(FieldNameVx);
	vtkDataArray* output_vy = image->GetPointData()->GetArray(FieldNameVy);
	vtkDataArray* output_vt = image->GetPointData()->GetArray(FieldNameVt);

	// Iterate the time steps (in parallel)
#ifdef NDEBUG
#pragma omp parallel for schedule(dynamic,16)
#endif
	for (int it = 0; it < dims[2]; it++)
	{
		// declare matrices for every voxel of a slice
		std::vector<MatrixXd, aligned_allocator<MatrixXd> > _M(dims[0] * dims[1]);
		std::vector<MatrixXd, aligned_allocator<MatrixXd> > _MTM(dims[0] * dims[1]);
		std::vector<MatrixXd, aligned_allocator<MatrixXd> > _MTb(dims[0] * dims[1]);

		// setup the system matrix M and right hand side.
		for (int iy = 0; iy < dims[1]; iy++)
		{
			double y = boundsMin[1] + iy * spacing[1];
			for (int ix = 0; ix < dims[0]; ix++)
			{
				double x = boundsMin[0] + ix * spacing[0];
				int tupleIdx = it * dims[0] * dims[1] + iy * dims[0] + ix;

				// position, velocity and derivatives at the voxel
				Vector2d xx(x, y);
				Vector2d vv(input_v[tupleIdx * 2 + 0], input_v[tupleIdx * 2 + 1]);
				Vector2d dx(input_vx[tupleIdx * 2 + 0], input_vx[tupleIdx * 2 + 1]);
				Vector2d dy(input_vy[tupleIdx * 2 + 0], input_vy[tupleIdx * 2 + 1]);
				Vector2d dt(input_vt[tupleIdx * 2 + 0], input_vt[tupleIdx * 2 + 1]);

				// compute 90 degree rotated vectors, setup Jacobian and compute products
				Vector2d Xp(-xx.y(), xx.x());
				Vector2d Vp(-vv.y(), vv.x());
				Matrix2d J = make_Matrix2d(dx, dy);
				Vector2d Jxpvp = -J * Xp + Vp;
				Vector2d Jxv = -J * xx + vv;

				// setup matrix M
				MatrixXd M(2, systemSize);
				switch (Invariance)
				{
				default:
				case Objectivity:
					M(0, 0) = Jxpvp.x(); 	 M(0, 1) = dx.x(); M(0, 2) = dy.x();   M(0, 3) = 1; M(0, 4) = 0;   M(0, 5) = Xp.x();
					M(1, 0) = Jxpvp.y(); 	 M(1, 1) = dx.y(); M(1, 2) = dy.y();   M(1, 3) = 0; M(1, 4) = 1;   M(1, 5) = Xp.y();
					break;
				case SimilarityInvariance:
					M(0, 0) = Jxpvp.x(); 	 M(0, 1) = dx.x(); M(0, 2) = dy.x();   M(0, 3) = 1; M(0, 4) = 0;   M(0, 5) = Xp.x(); M(0, 6) = Jxv.x(); M(0, 7) = xx.x();
					M(1, 0) = Jxpvp.y(); 	 M(1, 1) = dx.y(); M(1, 2) = dy.y();   M(1, 3) = 0; M(1, 4) = 1;   M(1, 5) = Xp.y(); M(1, 6) = Jxv.y();	M(1, 7) = xx.y();
					break;
				case AffineInvariance:
					M(0, 0) = vv.x() - xx.x() * J(0, 0);	M(0, 1) = 0 - xx.x() * J(0, 1);	/**/  M(0, 2) = vv.y() - xx.y() * J(0, 0);  M(0, 3) = 0 - xx.y() * J(0, 1);  /**/  M(0, 4) = J(0, 0);  M(0, 5) = J(0, 1);  /**/  M(0, 6) = 1;  M(0, 7) = 0;  /**/  M(0, 8) = xx.x();  M(0, 9) = 0;       /**/  M(0, 10) = xx.y();  M(0, 11) = 0;
					M(1, 0) = 0 - xx.x() * J(1, 0);	M(1, 1) = vv.x() - xx.x() * J(1, 1);	/**/  M(1, 2) = 0 - xx.y() * J(1, 0);  M(1, 3) = vv.y() - xx.y() * J(1, 1);  /**/  M(1, 4) = J(1, 0);  M(1, 5) = J(1, 1);  /**/  M(1, 6) = 0;	M(1, 7) = 1; /**/  M(1, 8) = 0;       M(1, 9) = xx.x();  /**/  M(1, 10) = 0;       M(1, 11) = xx.y();
					break;
				case Displacement:
				{
					int index = 0;
					for (int m = 0; m <= TaylorOrder; ++m)
					{
						for (int i = m; i >= 0; --i)
						{
							int j = m - i;
							Matrix2d f, g;
							ComputeDisplacementSystemMatrixCoefficients(i, j, xx, vv, J, f, g);
							M(0, index * 4 + 0) = f(0, 0);		M(1, index * 4 + 0) = f(1, 0);
							M(0, index * 4 + 1) = f(0, 1);		M(1, index * 4 + 1) = f(1, 1);

							M(0, index * 4 + 2) = g(0, 0);		M(1, index * 4 + 2) = g(1, 0);
							M(0, index * 4 + 3) = g(0, 1);		M(1, index * 4 + 3) = g(1, 1);
							index += 1;
						}
					}
					break;
				}
				}

				// store MTM and MTb
				MatrixXd MT = M.transpose();
				_M[iy*dims[0] + ix] = M;
				_MTM[iy*dims[0] + ix] = MT * M;
				_MTb[iy*dims[0] + ix] = MT * dt * ((Invariance == Displacement) ? -1 : 1);
			}
		}

		// compute the prefix sum
		if (UseSummedAreaTables)
		{
			for (int iy = 0; iy < dims[1]; iy++)
				for (int ix = 0; ix < dims[0]; ix++)
				{
					if (ix > 0) {
						_MTM[iy*dims[0] + ix] += _MTM[iy*dims[0] + (ix - 1)];
						_MTb[iy*dims[0] + ix] += _MTb[iy*dims[0] + (ix - 1)];
					}
					if (iy > 0) {
						_MTM[iy*dims[0] + ix] += _MTM[(iy - 1)*dims[0] + ix];
						_MTb[iy*dims[0] + ix] += _MTb[(iy - 1)*dims[0] + ix];
					}
					if (ix > 0 && iy > 0) {
						_MTM[iy*dims[0] + ix] -= _MTM[(iy - 1)*dims[0] + (ix - 1)];
						_MTb[iy*dims[0] + ix] -= _MTb[(iy - 1)*dims[0] + (ix - 1)];
					}
				}
		}

		// solve the system for each pixel
		for (int iy = 0; iy < dims[1]; iy++)
		{
			double y = boundsMin[1] + iy * spacing[1];
			for (int ix = 0; ix < dims[0]; ix++)
			{
				double x = boundsMin[0] + ix * spacing[0];
				int tupleIdx = it * dims[0] * dims[1] + iy * dims[0] + ix;

				// corner indices of the neighborhood region
				int x1 = std::min(std::max(0, ix - NeighborhoodU - 1), dims[0] - 1);
				int y1 = std::min(std::max(0, iy - NeighborhoodU - 1), dims[1] - 1);
				int x2 = std::min(std::max(0, ix + NeighborhoodU), dims[0] - 1);
				int y2 = std::min(std::max(0, iy + NeighborhoodU), dims[1] - 1);

				// compute the sum of all matrices/vectors in neighborhood region
				MatrixXd MTM(systemSize, systemSize);
				MTM.setZero();
				VectorXd MTb(systemSize, 1);
				MTb.setZero();

				if (UseSummedAreaTables)
				{
					MTM = _MTM[y2*dims[0] + x2] + _MTM[y1*dims[0] + x1] - _MTM[y1*dims[0] + x2] - _MTM[y2*dims[0] + x1];
					MTb = _MTb[y2*dims[0] + x2] + _MTb[y1*dims[0] + x1] - _MTb[y1*dims[0] + x2] - _MTb[y2*dims[0] + x1];
				}
				else
				{
					for (int wy = y1; wy <= y2; ++wy)
						for (int wx = x1; wx <= x2; ++wx)
						{
							MTM += _MTM[wy*dims[0] + wx];
							MTb += _MTb[wy*dims[0] + wx];
						}
				}

				// solve for reference frame parameters
				VectorXd uu = MTM.fullPivHouseholderQr().solve(MTb);

				// compute new vector field in optimal frame
				Vector2d xx(x, y);
				Vector2d vv(input_v[tupleIdx * 2 + 0], input_v[tupleIdx * 2 + 1]);
				Vector2d dx(input_vx[tupleIdx * 2 + 0], input_vx[tupleIdx * 2 + 1]);
				Vector2d dy(input_vy[tupleIdx * 2 + 0], input_vy[tupleIdx * 2 + 1]);
				Vector2d dt(input_vt[tupleIdx * 2 + 0], input_vt[tupleIdx * 2 + 1]);
				Matrix2d J = make_Matrix2d(dx, dy);
				Vector2d vnew(0, 0);
				Matrix2d Jnew = make_Matrix2d(0, 0, 0, 0);
				Vector2d Xp(-xx.y(), xx.x());
				switch (Invariance)
				{
				case Objectivity:
					vnew = vv + Vector2d(uu(1), uu(2)) - uu(0) * Xp;
					Jnew = J + make_Matrix2d(0, uu(0), -uu(0), 0);
					break;
				case SimilarityInvariance:
					vnew = vv + Vector2d(uu(1), uu(2)) - uu(0) * Xp - uu(6) * xx;
					Jnew = J + make_Matrix2d(0, uu(0), -uu(0), 0) - make_Matrix2d(uu(6), 0, 0, uu(6));
					break;
				case AffineInvariance:
				{
					Matrix2d H1 = make_Matrix2d(-uu(0), -uu(2), -uu(1), -uu(3));
					Vector2d k1(uu(4), uu(5));
					vnew = vv + H1 * xx + k1;
					Jnew = J + H1;
					break;
				}
				case Displacement:
				{
					// construct velocity
					{
						Vector2d Ft(0, 0);
						int fac_m = 1;
						for (int m = 0; m <= TaylorOrder; ++m)
						{
							if (m > 1)
								fac_m *= m;

							Vector2d Ftpart(0, 0);
							for (int i = 0; i <= m; ++i)
							{
								int fu_i = i, fu_j = m - i;
								// general formular to get the linear index:  (i+j)*(i+j+1) + 2*j
								int linear_fu = (fu_i + fu_j)*(fu_i + fu_j + 1) + 2 * fu_j;
								double fu = uu(2 * linear_fu + 0);
								double fv = uu(2 * linear_fu + 1);

								int binom = binomialCoeff(m, i);
								Ftpart += binom * pow(xx.x(), i) * pow(xx.y(), m - i) * Vector2d(fu, fv);
							}
							Ft += Ftpart / fac_m;
						}
						vnew = vv + Ft;
					}
					// construct Jacobian
					{
						Matrix2d nablaFt = make_Matrix2d(0, 0, 0, 0);
						int fac_m = 1;
						for (int m = 1; m <= TaylorOrder; ++m)
						{
							Matrix2d nablaFtpart = make_Matrix2d(0, 0, 0, 0);
							for (int i = 0; i <= m - 1; ++i)
							{
								int fx_i = i + 1, fx_j = m - 1 - i;
								int fy_i = i, fy_j = m - i;
								// general formular to get the linear index:  (i+j)*(i+j+1) + 2*j
								int linear_fx = (fx_i + fx_j)*(fx_i + fx_j + 1) + 2 * fx_j;
								int linear_fy = (fy_i + fy_j)*(fy_i + fy_j + 1) + 2 * fy_j;
								double fxu = uu(2 * linear_fx + 0);
								double fxv = uu(2 * linear_fx + 1);
								double fyu = uu(2 * linear_fy + 0);
								double fyv = uu(2 * linear_fy + 1);

								int binom = binomialCoeff(m - 1, i);
								nablaFtpart += binom * pow(xx.x(), i) * pow(xx.y(), m - 1 - i) * make_Matrix2d(fxu, fyu, fxv, fyv);
							}
							nablaFt += nablaFtpart / fac_m;

							if (m > 1)
								fac_m *= m;
						}
						Jnew = J + nablaFt;
					}
					break;
				}
				}
				Vector2d vtnew = dt - _M[iy * dims[0] + ix] * uu * ((Invariance == Displacement) ? -1 : 1);

				// store the result
				output_v->SetTuple2(tupleIdx, vnew.x(), vnew.y());
				output_vx->SetTuple2(tupleIdx, Jnew(0, 0), Jnew(1, 0));
				output_vy->SetTuple2(tupleIdx, Jnew(0, 1), Jnew(1, 1));
				output_vt->SetTuple2(tupleIdx, vtnew.x(), vtnew.y());
			}
		}
	}

	// Copy the computed image to the output
	output->ShallowCopy(image);

	// Update the extent
	int extent[6];
	input->GetExtent(extent);
	output->SetExtent(extent);
	outInfo->Set(vtkStreamingDemandDrivenPipeline::UPDATE_EXTENT(), extent, 6);
	outInfo->Set(vtkStreamingDemandDrivenPipeline::WHOLE_EXTENT(), extent, 6);
	return 1;
}


/*    Copyright (c) 2010-2018, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 *
 *    References
 *      van der Ham, L. Interplanetary trajectory design using dynamical systems theory,
 *          MSc thesis, Delft University of Technology, Delft, The Netherlands, 2012.
 *      Mireles James, J.D. Celestial Mechanics Notes Set 4: The Circular Restricted Three Body
 *          Problem, 2006, http://www.math.utexas.edu/users/jjames/hw4Notes.pdf,
 *          last accessed: 18th May, 2012.
 *
 *    Notes
 *      WARNING: There seems to be a bug in the computation of the L3 location!
 *
 */

#include <cmath>
#include <stdexcept>

#include <boost/bind.hpp>

#include "Tudat/Astrodynamics/Gravitation/librationPoint.h"
#include "Tudat/Mathematics/BasicMathematics/functionProxy.h"

namespace tudat
{
namespace gravitation
{
namespace circular_restricted_three_body_problem
{

using namespace root_finders;
using namespace basic_mathematics;

//! Compute location of Lagrange libration point.
void LibrationPoint::computeLocationOfLibrationPoint(
        LagrangeLibrationPoints lagrangeLibrationPoint )
{
    using std::pow;
    using std::sqrt;

    // Set functions for Newton-Raphson based on collinear libration point passed as input
    // parameter, or computed locations directly of equilateral libration points.
    switch( lagrangeLibrationPoint )
    {
    case l1:
    {
        // Create an object containing the function of which we whish to obtain the root from.
        UnivariateProxyPointer rootFunction = std::make_shared< UnivariateProxy >(
                    std::bind( &LibrationPoint::computeL1LocationFunction, this, std::placeholders::_1 ) );

        // Add the first derivative of the root function.
        rootFunction->addBinding( -1, std::bind( &LibrationPoint::
                computeL1FirstDerivativeLocationFunction, this, std::placeholders::_1 ) );

        // Set position vector of L1 in Cartesian elements based on result of Newton-Raphson
        // root-finding algorithm.
        positionOfLibrationPoint_ << rootFinder->execute( rootFunction, 1.0 ), 0.0, 0.0;
    }
        break;

    case l2:
    {
        // Create an object containing the function of which we whish to obtain the root from.
        UnivariateProxyPointer rootFunction = std::make_shared< UnivariateProxy >(
                    std::bind( &LibrationPoint::computeL2LocationFunction, this, std::placeholders::_1 ) );

        // Add the first derivative of the root function.
        rootFunction->addBinding( -1, std::bind( &LibrationPoint::
                computeL2FirstDerivativeLocationFunction, this, std::placeholders::_1 ) );

        // Set position vector of L1 in Cartesian elements based on result of Newton-Raphson
        // root-finding algorithm.
        positionOfLibrationPoint_ << rootFinder->execute( rootFunction, 1.0 ), 0.0, 0.0;
    }
        break;

    case l3:
    {
        // Create an object containing the function of which we whish to obtain the root from.
        UnivariateProxyPointer rootFunction = std::make_shared< UnivariateProxy >(
                    std::bind( &LibrationPoint::computeL3LocationFunction, this, std::placeholders::_1 ) );

        // Add the first derivative of the root function.
        rootFunction->addBinding( -1, std::bind( &LibrationPoint::
                computeL3FirstDerivativeLocationFunction, this, std::placeholders::_1 ) );

        // Set position vector of L1 in Cartesian elements based on result of Newton-Raphson
        // root-finding algorithm.
        positionOfLibrationPoint_ << rootFinder->execute( rootFunction, -1.0 ), 0.0, 0.0;
    }
        break;

    case l4:

        // Set position vector of L4 in Cartesian elements.
        positionOfLibrationPoint_.x( ) = 0.5 - massParameter;
        positionOfLibrationPoint_.y( ) = 0.5 * sqrt( 3.0 );
        positionOfLibrationPoint_.z( ) = 0.0;

        break;

    case l5:

        // Set position vector of L5 in Cartesian elements.
        positionOfLibrationPoint_.x( ) = 0.5 - massParameter;
        positionOfLibrationPoint_.y( ) = -0.5 * sqrt( 3.0 );
        positionOfLibrationPoint_.z( ) = 0.0;

        break;

    default:

        throw std::runtime_error(
                            "The Lagrange libration point requested does not exist." );
    };
}

} // namespace circular_restricted_three_body_problem
} // namespace gravitation
} // namespace tudat


#include <mpi.h>
#include <iostream>
#include <iomanip>
#include <armadillo>
using namespace std;
using namespace arma;

double ** CreateMatrix(int m, int n){
  double ** mat;
  mat = new double*[m];
  for(int i=0;i<m;i++){
    mat[i] = new double[n];
    for(int j=0;j<m;j++)
      mat[i][j] = 0.0;
  }
  return mat;
}

void DestroyMatrix(double ** mat, int m, int n){
  for(int i=0;i<m;i++)
    delete[] mat[i];
  delete[] mat;
}

int Jacobi_P(int, int, int, double **, double *, double *, double);

int main(int argc, char * argv[]){
  int i,j, N = 20;
  double **A,*x,*q;
  int totalnodes,mynode;

  MPI_Init(&argc,&argv);
  MPI_Comm_size(MPI_COMM_WORLD, &totalnodes);
  MPI_Comm_rank(MPI_COMM_WORLD, &mynode);

  if(mynode==0){
    A = CreateMatrix(N,N);
    x = new double[N];
    q = new double[N];

    for(i=0;i<N;i++){
      q[i] = i+1;
      A[i][i] = -2.0;
      if(i<N-1){
        A[i][i+1] = 1.0;
        A[i+1][i] = 1.0;
      }
    }
  }
  Jacobi_P(mynode,totalnodes,N,A,x,q,1.0e-14);
  if(mynode==0){
    for(i=0;i<N;i++)
      cout << x[i] << endl;
    DestroyMatrix(A,N,N);
    delete[] x;
    delete[] q;
  }
  MPI_Finalize();
}

int Jacobi_P(int mynode, int numnodes, int N, double **A, double *x, double *b, double abstol){
  int i,j,k,i_global;
  int maxit = 100000;
  int rows_local,local_offset,last_rows_local,*count,*displacements;
  double sum1,sum2,*xold;
  double error_sum_local, error_sum_global;
  MPI_Status status;

  rows_local = (int) floor((double)N/numnodes);
  local_offset = mynode*rows_local;
  if(mynode == (numnodes-1))
    rows_local = N - rows_local*(numnodes-1);

  /*Distribute the Matrix and R.H.S. among the processors */
  if(mynode == 0){
    for(i=1;i<numnodes-1;i++){
      for(j=0;j<rows_local;j++)
        MPI_Send(A[i*rows_local+j],N,MPI_DOUBLE,i,j,MPI_COMM_WORLD);
      MPI_Send(b+i*rows_local,rows_local,MPI_DOUBLE,i,rows_local,
               MPI_COMM_WORLD);
    }
    last_rows_local = N-rows_local*(numnodes-1);
    for(j=0;j<last_rows_local;j++)
      MPI_Send(A[(numnodes-1)*rows_local+j],N,MPI_DOUBLE,numnodes-1,j,
               MPI_COMM_WORLD);
    MPI_Send(b+(numnodes-1)*rows_local,last_rows_local,MPI_DOUBLE,numnodes-1,
             last_rows_local,MPI_COMM_WORLD);
  }
  else{
    A = CreateMatrix(rows_local,N);
    x = new double[rows_local];
    b = new double[rows_local];
    for(i=0;i<rows_local;i++)
      MPI_Recv(A[i],N,MPI_DOUBLE,0,i,MPI_COMM_WORLD,&status);
    MPI_Recv(b,rows_local,MPI_DOUBLE,0,rows_local,MPI_COMM_WORLD,&status);
  }

  xold = new double[N];
  count = new int[numnodes];
  displacements = new int[numnodes];

  //set initial guess to all 1.0
  for(i=0; i<N; i++){
    xold[i] = 1.0;
  }

  for(i=0;i<numnodes;i++){
    count[i] = (int) floor((double)N/numnodes);
    displacements[i] = i*count[i];
  }
  count[numnodes-1] = N - ((int)floor((double)N/numnodes))*(numnodes-1);

  for(k=0; k<maxit; k++){
    error_sum_local = 0.0;
    for(i = 0; i<rows_local; i++){
      i_global = local_offset+i;
      sum1 = 0.0; sum2 = 0.0;
      for(j=0; j < i_global; j++)
        sum1 = sum1 + A[i][j]*xold[j];
      for(j=i_global+1; j < N; j++)
        sum2 = sum2 + A[i][j]*xold[j];

      x[i] = (-sum1 - sum2 + b[i])/A[i][i_global];
      error_sum_local += (x[i]-xold[i_global])*(x[i]-xold[i_global]);
    }

    MPI_Allreduce(&error_sum_local,&error_sum_global,1,MPI_DOUBLE,
                  MPI_SUM,MPI_COMM_WORLD);
    MPI_Allgatherv(x,rows_local,MPI_DOUBLE,xold,count,displacements,
                   MPI_DOUBLE,MPI_COMM_WORLD);

    if(sqrt(error_sum_global)<abstol){
      if(mynode == 0){
        for(i=0;i<N;i++)
          x[i] = xold[i];
      }
      else{
        DestroyMatrix(A,rows_local,N);
        delete[] x;
        delete[] b;
      }
      delete[] xold;
      delete[] count;
      delete[] displacements;
      return k;
    }
  }

  cerr << "Jacobi: Maximum Number of Interations Reached Without Convergence\n";
  if(mynode == 0){
    for(i=0;i<N;i++)
      x[i] = xold[i];
  }
  else{
    DestroyMatrix(A,rows_local,N);
    delete[] x;
    delete[] b;
  }
  delete[] xold;
  delete[] count;
  delete[] displacements;

  return maxit;
}


/*
 * Copyright (c) 2019 Opticks Team. All Rights Reserved.
 *
 * This file is part of Opticks
 * (see https://bitbucket.org/simoncblyth/opticks).
 *
 * 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.
 */

#include <iomanip>
#include <iostream>
#include <functional>
#include <boost/math/tools/roots.hpp>

template <class T>
struct fn
{
    T operator()(const T& x )
    {
        return (x-2.)*(x-5.) ;
    }
};

template <class T>
struct tolerance
{
    bool operator()(const T& min, const T& max )
    {
        return (max - min) < 0.001 ;
    }
};

int main()
{

   fn<float> f ;
   tolerance<float> tol ;

   float min = 1 ;
   float max = 3 ;

   std::pair<float, float> r = boost::math::tools::bisect(f, min, max, tol );

   std::cout
      << " r " << std::setw(15) << std::fixed << std::setprecision(4) << r.first
      << " " << r.second
      << std::endl
      ;

    return 0 ;
}


#pragma once

#include <algorithm>
#include <cassert>
#include <cmath>
#include <cstddef>
#include <string>
#include <tuple>
#include <type_traits>
#include <utility>

#include <boost/functional/hash.hpp>

#include <common/traits.hpp>
#include <common/types.hpp>

namespace vitamine
{
	template <typename T, typename Tag = void>
	struct Vector2XZ
	{
		using ComponentType = T;

		T x, z;

		Vector2XZ() noexcept = default;
		constexpr Vector2XZ(T x, T z) noexcept : x(x), z(z) {}

		[[nodiscard]]
		bool withinOrdered(Vector2XZ from, Vector2XZ to) const
		{
			assert(from.x <= to.x);
			assert(from.z <= to.z);
			return x >= from.x && x <= to.x
			    && z >= from.z && z <= to.z;
		}

		template <typename U = T>
		[[nodiscard]]
		constexpr T lengthSquared() const
		{
			auto x = static_cast<U>(this->x);
			auto z = static_cast<U>(this->z);
			return x * x + z * z;
		}

		template <typename F = Float64>
		[[nodiscard]]
		F length() const
		{
			using std::sqrt;
			return sqrt(static_cast<F>(lengthSquared()));
		}
	};

	template <typename T, typename Tag = void>
	struct Vector3XYZ
	{
		using ComponentType = T;

		T x, y, z;

		Vector3XYZ() noexcept = default;
		constexpr Vector3XYZ(T x, T y, T z) noexcept : x(x), y(y), z(z) {}

		[[nodiscard]]
		bool withinOrdered(Vector3XYZ from, Vector3XYZ to)
		{
			assert(from.x <= to.x);
			assert(from.y <= to.y);
			assert(from.z <= to.z);
			return x >= from.x && x <= to.x
			    && y >= from.y && y <= to.y
			    && z >= from.z && z <= to.z;
		}

		template <typename U = T>
		[[nodiscard]]
		constexpr T lengthSquared() const
		{
			auto x = static_cast<U>(this->x);
			auto y = static_cast<U>(this->y);
			auto z = static_cast<U>(this->z);
			return x * x + y * y + z * z;
		}

		template <typename F = Float64>
		[[nodiscard]]
		F length() const
		{
			using std::sqrt;
			return sqrt(static_cast<F>(lengthSquared()));
		}
	};

	static_assert(std::is_trivial_v<Vector2XZ <Int>>);
	static_assert(std::is_trivial_v<Vector3XYZ<Int>>);

	static_assert(sizeof(Vector2XZ <Int>) == 2 * sizeof(Int));
	static_assert(sizeof(Vector3XYZ<Int>) == 3 * sizeof(Int));

	template <typename T, typename Tag>
	constexpr bool operator==(Vector2XZ<T, Tag> a, Vector2XZ<T, Tag> b)
	{
		return a.x == b.x && a.z == b.z;
	}

	template <typename T, typename Tag>
	constexpr bool operator==(Vector3XYZ<T, Tag> a, Vector3XYZ<T, Tag> b)
	{
		return a.x == b.x && a.y == b.y && a.z == b.z;
	}

	template <typename T, typename Tag>
	constexpr bool operator!=(Vector2XZ<T, Tag> a, Vector2XZ<T, Tag> b)
	{
		return !(a == b);
	}

	template <typename T, typename Tag>
	constexpr bool operator!=(Vector3XYZ<T, Tag> a, Vector3XYZ<T, Tag> b)
	{
		return !(a == b);
	}

	template <typename T, typename Tag>
	constexpr Vector2XZ<T, Tag>& operator+=(Vector2XZ<T, Tag>& a, Vector2XZ<T, Tag> b)
	{
		a.x += b.x;
		a.z += b.z;
		return a;
	}

	template <typename T, typename Tag>
	constexpr Vector3XYZ<T, Tag>& operator+=(Vector3XYZ<T, Tag>& a, Vector3XYZ<T, Tag> b)
	{
		a.x += b.x;
		a.y += b.y;
		a.z += b.z;
		return a;
	}

	template <typename T, typename Tag>
	constexpr Vector2XZ<T, Tag>& operator-=(Vector2XZ<T, Tag>& a, Vector2XZ<T, Tag> b)
	{
		a.x -= b.x;
		a.z -= b.z;
		return a;
	}

	template <typename T, typename Tag>
	constexpr Vector3XYZ<T, Tag>& operator-=(Vector3XYZ<T, Tag>& a, Vector3XYZ<T, Tag> b)
	{
		a.x -= b.x;
		a.y -= b.y;
		a.z -= b.z;
		return a;
	}

	template <typename T, typename Tag>
	constexpr Vector2XZ<T, Tag> operator+(Vector2XZ<T, Tag> a, Vector2XZ<T, Tag> b)
	{
		return {a.x + b.x, a.z + b.z};
	}

	template <typename T, typename Tag>
	constexpr Vector3XYZ<T, Tag> operator+(Vector3XYZ<T, Tag> a, Vector3XYZ<T, Tag> b)
	{
		return {a.x + b.x, a.y + b.y, a.z + b.z};
	}

	template <typename T, typename Tag>
	constexpr Vector2XZ<T, Tag> operator-(Vector2XZ<T, Tag> a, Vector2XZ<T, Tag> b)
	{
		return {a.x - b.x, a.z - b.z};
	}

	template <typename T, typename Tag>
	constexpr Vector3XYZ<T, Tag> operator-(Vector3XYZ<T, Tag> a, Vector3XYZ<T, Tag> b)
	{
		return {a.x - b.x, a.y - b.y, a.z - b.z};
	}

	template <typename T, typename Tag>
	constexpr Vector2XZ<T, Tag> operator*(Vector2XZ<T, Tag> v, typename Identity<T>::Type n)
	{
		return {v.x * n, v.z * n};
	}

	template <typename T, typename Tag>
	constexpr Vector3XYZ<T, Tag> operator*(Vector3XYZ<T, Tag> v, typename Identity<T>::Type n)
	{
		return {v.x * n, v.y * n, v.z * n};
	}

	template <typename T, typename Tag>
	constexpr Vector2XZ<T, Tag> operator*(typename Identity<T>::Type n, Vector2XZ<T, Tag> v)
	{
		return {n * v.x, n * v.z};
	}

	template <typename T, typename Tag>
	constexpr Vector3XYZ<T, Tag> operator*(typename Identity<T>::Type n, Vector3XYZ<T, Tag> v)
	{
		return {n * v.x, n * v.y, n * v.z};
	}

	template <typename T, typename Tag>
	constexpr Vector2XZ<T, Tag> operator/(Vector2XZ<T, Tag> v, typename Identity<T>::Type n)
	{
		return {v.x / n, v.z / n};
	}

	template <typename T, typename Tag>
	constexpr Vector3XYZ<T, Tag> operator/(Vector3XYZ<T, Tag> v, typename Identity<T>::Type n)
	{
		return {v.x / n, v.y / n, v.z / n};
	}

	template <typename T, typename Tag>
	constexpr Vector2XZ<T, Tag> operator/(typename Identity<T>::Type n, Vector2XZ<T, Tag> v)
	{
		return {n / v.x, n / v.z};
	}

	template <typename T, typename Tag>
	constexpr Vector3XYZ<T, Tag> operator/(typename Identity<T>::Type n, Vector3XYZ<T, Tag> v)
	{
		return {n / v.x, n / v.y, n / v.z};
	}

	template <typename T, typename Tag>
	std::string toString(Vector2XZ<T, Tag> v)
	{
		return '(' + std::to_string(v.x) + ',' + ' ' + std::to_string(v.z) + ')';
	}

	template <typename T, typename Tag>
	std::string toString(Vector3XYZ<T, Tag> v)
	{
		return '(' + std::to_string(v.x) + ',' + ' ' + std::to_string(v.y) + ',' + ' ' + std::to_string(v.z) + ')';
	}
}

namespace std
{
	template <typename T, typename Tag>
	struct hash<vitamine::Vector2XZ<T, Tag>>
	{
		std::size_t operator()(vitamine::Vector2XZ<T, Tag> v) const noexcept
		{
			return boost::hash_value(std::tie(v.x, v.z));
		}
	};

	template <typename T, typename Tag>
	struct hash<vitamine::Vector3XYZ<T, Tag>>
	{
		std::size_t operator()(vitamine::Vector3XYZ<T, Tag> v) const noexcept
		{
			return boost::hash_value(std::tie(v.x, v.y, v.z));
		}
	};

	template <typename T, typename Tag>
	struct less<vitamine::Vector2XZ<T, Tag>>
	{
		constexpr bool operator()(vitamine::Vector2XZ<T, Tag> a, vitamine::Vector2XZ<T, Tag> b) const noexcept
		{
			if(a.x < b.x)
				return true;

			if(a.x > b.x)
				return false;

			return a.z < b.z;
		}
	};

	template <typename T, typename Tag>
	struct less<vitamine::Vector3XYZ<T, Tag>>
	{
		constexpr bool operator()(vitamine::Vector3XYZ<T, Tag> a, vitamine::Vector3XYZ<T, Tag> b) const noexcept
		{
			if(a.x < b.x)
				return true;

			if(a.x > b.x)
				return false;

			if(a.y < b.y)
				return true;

			if(a.y > b.y)
				return false;

			return a.z < b.z;
		}
	};
}


#include <iostream>
#include <vector>
#include <queue>
#include <limits>
#include <cmath>

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/push_relabel_max_flow.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>

typedef boost::adjacency_list_traits<boost::vecS, boost::vecS, boost::directedS> traits;
typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::directedS, boost::no_property,
    boost::property<boost::edge_capacity_t, long,
        boost::property<boost::edge_residual_capacity_t, long,
            boost::property<boost::edge_reverse_t, traits::edge_descriptor>>>> graph;

typedef traits::vertex_descriptor vertex_desc;
typedef traits::edge_descriptor edge_desc;
typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS,
  boost::no_property, boost::property<boost::edge_weight_t, int> >      weighted_graph;
typedef boost::property_map<weighted_graph, boost::edge_weight_t>::type weight_map;

// Custom edge adder class, highly recommended
class edge_adder {
  graph &G;

 public:
  explicit edge_adder(graph &G) : G(G) {}

  void add_edge(int from, int to, long capacity) {
    auto c_map = boost::get(boost::edge_capacity, G);
    auto r_map = boost::get(boost::edge_reverse, G);
    const auto e = boost::add_edge(from, to, G).first;
    const auto rev_e = boost::add_edge(to, from, G).first;
    c_map[e] = capacity;
    c_map[rev_e] = 0; // reverse edge has no capacity!
    r_map[e] = rev_e;
    r_map[rev_e] = e;
  }
};

using namespace std;

struct edge {
  int u;
  int v;
  int length;
  int width;
};

// Strategy:
// - Find all edges that are part of some shortest path
// - Run a max-flow algorithm on those edges
void solve() {
  int n, m, s, f;
  cin >> n >> m >> s >> f;

  // Dijkstra Graph
  weighted_graph G(n);

  // Read neighbors
  vector<edge> edges;
  edges.reserve(m);
  int u, v, length, width;
  for (int i = 0; i < m; ++i) {
    cin >> u >> v >> width >> length;
    boost::add_edge(u, v, length, G);
    edges.push_back({u, v, length, width});
  }

  // Run dijkstra
  std::vector<int> d(n);
  boost::dijkstra_shortest_paths(G, s, boost::distance_map(boost::make_iterator_property_map( d.begin(), boost::get(boost::vertex_index, G))));

  // Flow graph
  graph gFlow(n);
  edge_adder adder(gFlow);

  // Find edges on a shortest path
  vector<edge> shortestPathEdges;
  for (auto e : edges) {
    int diff = d[e.v] - d[e.u];
    if (e.length == diff) {
      adder.add_edge(e.u, e.v, e.width);
    }
    else if (e.length == -diff) {
      adder.add_edge(e.v, e.u, e.width);
    }
  }

  // Compute flow
  long flow = boost::push_relabel_max_flow(gFlow, s, f);
  cout << flow << endl;
}

int main() {
  ios_base::sync_with_stdio(false);
  int t; cin >> t;
  while (t--)
  {
    solve();
  }
  return 0;
}


/*
 * 这是 TJU Robomasters 上位机源码，未经管理层允许严禁传播给其他人（包括队内以及队外）
 *
 * 该文件包含各种预测模型，并封装到了以抽象类Predictor类为基类的类中
 */

#pragma once

#include <opencv2/opencv.hpp>
#include <Eigen/Dense>
#include "util.hpp"
#include <atomic>
#include "KalmanFilter.hpp"
using namespace cv;

typedef struct _prd_path_pt{

    Point3f worldPosition;
    Point2f targetPTZAngle;
    Point2f selfWorldPosition;
    double duration;

    _prd_path_pt(Point3d _worldp,Point2f _tgtAngle,Point2f _sfworldp,float _dur)
    {
        worldPosition = _worldp;
        targetPTZAngle = _tgtAngle;
        selfWorldPosition = _sfworldp;
        duration = _dur;
    }

    _prd_path_pt()
    { }

}PredictionPathPoint;

class Predictor
{
public:
    // 添加一个轨迹点，该函数应该能够自主甄别轨迹点是否与之前追踪的目标一样，如果不是
    // 预测器自动清空历史数据重新开始预测
    virtual void AddPredictPoint(PredictionPathPoint ppp) = 0;

    // 预测一段时间之后目标的位置
    virtual void Predict(double prdTime) = 0;

    // 主动清除历史记录
    virtual void ClearHistory() = 0;
};
// 线性预测器，由历史数据给出线性的预测（假设目标匀速直线运动）
class KFCPredictor : public Predictor
{
public:
    static const int HistorySize = 6;
    Mat P, Q, H, R, F, K, x_;
    int xstate = 0;
    PredictionPathPoint present;
    PredictionPathPoint lastpoint;

    KFCPredictor()
    {
        Mat P_in = Mat::eye(6,6,CV_32FC1);
        Mat Q_in = Mat::eye(6,6,CV_32FC1);
        Mat H_in = Mat::eye(6,6,CV_32FC1);
        Mat R_in = Mat::eye(6,6,CV_32FC1);
        P = P_in;
        Q = Q_in;
        H = H_in;
        R = R_in;
        lastpoint.targetPTZAngle=Point2f(0,0);
    }

    KFCPredictor(Mat P_in, Mat Q_in, Mat H_in, Mat R_in)
    {
        P = P_in;
        Q = Q_in;
        H = H_in;
        R = R_in;
        lastpoint.targetPTZAngle=Point2f(0,0);
    }

    void AddPredictPoint(PredictionPathPoint ppp)
    {
        if (Length(ppp.targetPTZAngle - lastpoint.targetPTZAngle) > 10)
            ClearHistory();
        present = ppp;
    }

    void ClearHistory()
    {
        xstate = 0;
        P = Mat::eye(6,6,CV_32FC1);
    };

    void FirstFind()
    {
        p_tx_old = present.worldPosition.x;
        p_ty_old = present.worldPosition.y;
        p_tz_old = present.worldPosition.z;
        lastpoint = present;
    }

    void FirstSetFilter()
    {
        //将视觉根据图像计算出的装甲板在相机坐标系下的位置传给预测类中的worldposition
        //以当前的计算作为根据上一次计算进行预测的观测值
        double t = present.duration;
        float v_tx_now = (present.worldPosition.x - p_tx_old)/t;
        float v_ty_now =(present.worldPosition.y - p_ty_old)/t;
        float v_tz_now = (present.worldPosition.z - p_tz_old)/t;
        x_ = (Mat_<float>(6,1) <<
                   present.worldPosition.x, present.worldPosition.y, present.worldPosition.z,
                   v_tx_now, v_ty_now, v_tz_now
                   );
        xstate = 1;
        p_tx_old = present.worldPosition.x;
        p_ty_old = present.worldPosition.y;
        p_tz_old = present.worldPosition.z;
        lastpoint = present;
    }
    void ContinueSetFilter()
    {
        double t = present.duration;
        float v_tx_now = (present.worldPosition.x - p_tx_old)/t;
        float v_ty_now = (present.worldPosition.y - p_ty_old)/t;
        float v_tz_now = (present.worldPosition.z - p_tz_old)/t;

        Mat z = (Mat_<float>(6,1) <<
                   present.worldPosition.x, present.worldPosition.y, present.worldPosition.z,
                   v_tx_now, v_ty_now, v_tz_now
                   );

        Predict(t);
        update(z);

        p_tx_old = x_.at<float>(0, 0);
        p_ty_old = x_.at<float>(1, 0);
        p_tz_old = x_.at<float>(2, 0);
        lastpoint = present;
    }

    void Predict(double prdTime)
    {
        double t = prdTime;
        F = (Mat_<float>(6,6) <<
                   1.0, 0.0, 0.0, t, 0.0, 0.0,
                   0.0, 1.0, 0.0, 0.0, t, 0.0,
                   0.0, 0.0, 1.0, 0.0, 0.0, t,
                   0.0, 0.0, 0.0, 1.0, 0.0, 0.0,
                   0.0, 0.0, 0.0, 0.0, 1.0,0.0,
                   0.0, 0.0, 0.0, 0.0, 0.0, 1.0
                   );
        //起
        x_ = F * x_;
        // return Point3f(x_.at<float>(0,0),x_.at<float>(0,1),x_.at<float>(0,2));
    }

    void update(Mat z)
    {
        //按
        P = F*P*F.t() + Q;
        //顿
        Mat S = H*P*H.t() + R;
        K = P*H.t()*S.inv();
        //挫
        Mat y = z - H*x_;
        x_ = x_ + (K*y);
        //下笔风雷
        Mat I = Mat::eye(6,6,CV_32FC1);
        P = (I - K*H)*P;
    }
protected:
    float p_tx_old;                  //位置保留量
    float p_ty_old;
    float p_tz_old;
};

class AnglePredictor : public Predictor
{
public:

    Mat P, Q, H, R, F, K, x_;
    int xstate = 0;
    PredictionPathPoint present;
    PredictionPathPoint lastpoint;

    AnglePredictor()
    {
        Mat P_in = Mat::eye(4,4,CV_32FC1);
        Mat Q_in = (Mat_<float>(4,4) <<
                   5,0,0,0,
                   0,1,0,0,
                   0,0,5,0,
                   0,0,0,1
                   );
        Mat H_in = (Mat_<float>(2,4) <<
                   1,0,0,0,
                   0,1,0,0
                   );
        Mat R_in = (Mat_<float>(2,2) <<
                   200,0,
                   0,200
                   );
        P = P_in;
        Q = Q_in;
        H = H_in;
        R = R_in;
        lastpoint.targetPTZAngle=Point2f(0,0);
    }

    AnglePredictor(Mat P_in, Mat Q_in, Mat H_in, Mat R_in)
    {
        P = P_in;
        Q = Q_in;
        H = H_in;
        R = R_in;
        lastpoint.targetPTZAngle=Point2f(0,0);
    }

    void AddPredictPoint(PredictionPathPoint ppp)
    {
        if (Length(ppp.targetPTZAngle - lastpoint.targetPTZAngle) > 10)
            ClearHistory();
        present = ppp;
    }

    void ClearHistory()
    {
        xstate = 0;
        P = Mat::eye(4,4,CV_32FC1);
    };

    void FirstFind()
    {

        lastpoint = present;
    }

    void FirstSetFilter()
    {
        double t = present.duration;
        float vx = (present.targetPTZAngle.x - lastpoint.targetPTZAngle.x)/t;
        float vy =(present.targetPTZAngle.y - lastpoint.targetPTZAngle.y)/t;

        x_ = (Mat_<float>(4,1) <<
                   present.targetPTZAngle.x, present.targetPTZAngle.y,
                   vx, vy
                   );
        xstate = 1;

        lastpoint = present;
    }
    void ContinueSetFilter()
    {
        double t = present.duration;
        float vx = (present.targetPTZAngle.x - lastpoint.targetPTZAngle.x)/t;
        float vy =(present.targetPTZAngle.y - lastpoint.targetPTZAngle.y)/t;
        Mat z = (Mat_<float>(2,1) <<
                   present.targetPTZAngle.x, present.targetPTZAngle.y
                   );

        Predict(t);
        update(z);
        // cout << "presemtangle: " << present.targetPTZAngle << " lastangle: " << lastpoint.targetPTZAngle << endl;
        // cout << "speedx: " << (present.targetPTZAngle.x-lastpoint.targetPTZAngle.x)/present.duration << endl;
        // cout << "speedy: " << (present.targetPTZAngle.y-lastpoint.targetPTZAngle.y)/present.duration << endl;
        lastpoint = present;
    }

    void Predict(double prdTime)
    {
        double t = prdTime;
        F = (Mat_<float>(4,4) <<
                   1.0, 0.0, t, 0.0,
                   0.0, 1.0, 0.0, t,
                   0.0, 0.0, 1.0, 0.0,
                   0.0, 0.0, 0.0, 1.0
                   );
        //起
        x_ = F * x_;

        //return Point2f(myx_.at<float>(0,0),myx_.at<float>(0,1));
    }
    Point2f PredictReal(double prdTime)
    {
        double t = prdTime;
        F = (Mat_<float>(4,4) <<
                   1.0, 0.0, t, 0.0,
                   0.0, 1.0, 0.0, t,
                   0.0, 0.0, 1.0, 0.0,
                   0.0, 0.0, 0.0, 1.0
                   );
        //起

        Mat myx_ = F * x_;
        return Point2f(myx_.at<float>(0,0),myx_.at<float>(0,1));
    }

    void update(Mat z)
    {
        //按
        P = F*P*F.t() + Q;
        //顿
        Mat S = H*P*H.t() + R;
        K = P*H.t()*S.inv();
        //挫
        Mat y = z - H*x_;
        x_ = x_ + (K*y);
        //下笔风雷
        Mat I = Mat::eye(4,4,CV_32FC1);
        P = (I - K*H)*P;
    }
};

class KalmanPredictor{
public:
KalmanPredictor(){

	Eigen::MatrixXd A(stateSize, stateSize);
	A << 1, 0, 1, 0,
		0, 1, 0, 1,
		0, 0, 1, 0,
		0, 0, 0, 1 ;

	Eigen::MatrixXd H(measureSize, stateSize);
	H << 2, 0, 0, 0,
		0, 2, 0, 0,
		0, 0, 1, 0,
		0, 0, 0, 1;

	Eigen::MatrixXd P(stateSize, stateSize);
	P << 1, 0, 0, 0,
		0, 1, 0, 0,
		0, 0, 1, 0,
		0, 0, 0, 1;

	Eigen::MatrixXd Q(stateSize, stateSize);
	Q << 1, 0, 0, 0,
		0, 1, 0, 0,
		0, 0, 1, 0,
		0, 0, 0, 1;

	Eigen::MatrixXd R(measureSize, measureSize);
	R << 900, 0, 0, 0,
		0, 900, 0, 0,
		0, 0, 700, 0,
		0, 0, 0, 700;

	KF.init(stateSize, measureSize, A, P, R, Q, H);

	x.resize(stateSize);
	x << 0, 0, 0, 0;
}
public:
	Eigen::MatrixXd F;
	Eigen::VectorXd x;
    cv::Point2f predict(float yaw, float pitch,float distance, float deltatime,int bulletSpeed) {
	    return kalmanPredict(yaw, pitch, (double)deltatime,distance/bulletSpeed * 0.001);
	}

	cv::Point2f kalmanPredict(float yaw, float pitch, double deltatime,float time){
	if (abs(x(0) - yaw) > 200) {
		x(0) = (double)yaw;
		yawSpeed = 0;
		x(2) = yawSpeed;
	}
	if (abs(x(2) - pitch) > 200) {
		x(1) = (double)pitch;
		pitchSpeed = 0;
		x(3) = pitchSpeed;
	}

	yawSpeed = (yaw - lastYaw) / deltatime;
	pitchSpeed = (pitch - lastPitch) / deltatime;
	if(abs(yawSpeed) > 50){
		yawSpeed = 0;
	}
	if(abs(pitchSpeed > 20)){
		pitchSpeed = 0;
	}
	Eigen::VectorXd z(measureSize);
	Eigen::VectorXd output;
	F = Eigen::MatrixXd(4, 4);
	cout << "hello3"<<endl;
    F << 1.0,0.0,(double)deltatime,0.0,
        0.0,1.0,0.0,(double)deltatime,
        0.0,0.0,1.0,0.0,
        0.0,0.0,0.0,1.0;
	z << (double)yaw, (double)pitch, (double)yawSpeed, (double)pitchSpeed;
	KF.predict(x,F);
	KF.update(x, z);
	lastYaw = yaw;
	lastPitch = pitch;
	//cout << " X:" << x(0) << " v:" << x(2) << endl;
	double predictYaw = x(0) + (time + 0.35) * x(2);
	double predictPitch = x(1) + (time + 0.35) * x(3);

	return cv::Point2f((float)predictYaw, (float)predictPitch);
}
private:
	EigenKalman::KalmanFilter KF;
	double lastYaw;
	double lastPitch;
	double yawSpeed = 0;
	double pitchSpeed = 0;
	int measureSize = 4;
	int stateSize = 4;
public:
	atomic<int> shootSpeed;//发射速度 单位;米/秒
};


#include <iostream>
#include <boost/concept_check.hpp>
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_edge.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/core/optimization_algorithm_dogleg.h>
#include <random>
#include <chrono>
#include <Eigen/Core>
#include <opencv2/core/core.hpp>
#include <cmath>

using namespace std;
using namespace g2o;

class hello_vertex: public BaseVertex<1, Eigen::Matrix<double,1,1>> {//数据类型要以Eigen::Matrix给出,因为需要转置操作
          /**
         * update the position of the node from the parameters in v.
         * Implement in your class!
         */
	//optimizable_graph.h
        virtual void oplusImpl(const double* v) {
		cout << "v: " << *v << endl;
		_estimate(0,0) += *v;				//_estimate的类型即为继承模板时传入的类型
		cout << "_estimate: " << _estimate(0,0) << endl;
	}

        //! sets the node to the origin (used in the multilevel stuff)
        //optimizable_graph.h
        virtual void setToOriginImpl() {
		cout << "setToOriginImpl " << _estimate(0,0)  << endl;
		_estimate(0,0) = 0;
	}

	//! read the vertex from a stream, i.e., the internal state of the vertex
        virtual bool read(std::istream& is) {}
        //! write the vertex to a stream
        virtual bool write(std::ostream& os) const {}
};

class hello_edge: public BaseUnaryEdge<1, double, hello_vertex> {
	// computes the error of the edge and stores it in an internal structure
	//optimizable_graph.h
        virtual void computeError() {
		const hello_vertex* v = static_cast<const hello_vertex*> (_vertices[0]);
		const Eigen::Matrix<double,1,1> est = v->estimate();
		//typedef Eigen::Matrix<double, D, 1, Eigen::ColMajor> ErrorVector;
		//_error = est - (Eigen::Matrix<double, 1, 1> )(10);
		cout << "computeError: " ;
		//_error(0,0) = _measurement - est(0,0);
		//此处误差的定义影响后续解析求导时Jacob矩阵前边是否添加负号
		_error(0,0) = -_measurement + (est(0,0) + 1)*(est(0,0) + 1);
		cout << _error(0,0) << endl;
	}

	virtual void linearizeOplus() {
		const hello_vertex* v = static_cast<const hello_vertex*> (_vertices[0]);
		const Eigen::Matrix<double,1,1> est = v->estimate();
		//需要提供负梯度,与Ceres相同
		//之所以是负梯度,因为err = _measurement - f(_estimate)
		//因此为负的
		_jacobianOplusXi(0,0) = 2 * (est(0,0) + 1);
		cout << "_jacobianOplusXi: " << _jacobianOplusXi << endl;
	}

	//! read the vertex from a stream, i.e., the internal state of the vertex
        virtual bool read(std::istream& is) {}
        //! write the vertex to a stream
        virtual bool write(std::ostream& os) const {}
};
/*
 *  default_random_engine generator;
 *  normal_distribution<double> distribution(0.0,0.5);
 *  distribution(generator);
 */

int main(int argc, char** argv)
{

	// 构建图优化，先设定g2o
	typedef g2o::BlockSolver< g2o::BlockSolverTraits<1,1> > Block;  // 每个误差项优化变量维度为3，误差值维度为1
	Block::LinearSolverType* linearSolver = new g2o::LinearSolverDense<Block::PoseMatrixType>(); // 线性方程求解器
	Block* solver_ptr = new Block( linearSolver );      // 矩阵块求解器
	// 梯度下降方法，从GN, LM, DogLeg 中选
	g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( solver_ptr );
	// g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton( solver_ptr );
	// g2o::OptimizationAlgorithmDogleg* solver = new g2o::OptimizationAlgorithmDogleg( solver_ptr );
	g2o::SparseOptimizer optimizer;     // 图模型
	optimizer.setAlgorithm( solver );   // 设置求解器
	optimizer.setVerbose( true );       // 打开调试输出

	// 往图中增加顶点
	hello_vertex* v = new hello_vertex();
	v->setEstimate((Eigen::Matrix<double,1,1>) 100);
	v->setId(0);
	optimizer.addVertex( v );

	//加边
	hello_edge* edge = new hello_edge;
	edge->setId(0);
	edge->setVertex( 0, v );                // 设置连接的顶点
	edge->setMeasurement(66.6);      // 观测数值
	edge->setInformation(Eigen::Matrix<double,1,1>::Identity()*0.25); // 信息矩阵：协方差矩阵之逆
	optimizer.addEdge( edge );

	// 执行优化
	cout<<"start optimization"<<endl;
	chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
	optimizer.initializeOptimization();
	optimizer.optimize(100);
	chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
	chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>( t2-t1 );
	cout<<"solve time cost = "<<time_used.count()<<" seconds. "<<endl;

	// 输出优化值
	Eigen::Matrix<double,1,1> abc_estimate = v->estimate();
	cout<<"estimated value: "<<abc_estimate.transpose()<<endl;

	return 0;
}


// g++ -O3 -std=c++17 -fopenmp stifness_assembly.cpp -I"/usr/include/tbb"
// -I"/usr/include/eigen3" && ./a.out

// // #include <tbb/task_scheduler_init.h>
// #include <tbb/tbb.h>

// clang-format off
// Eigen::initParallel(); //TODO try this

#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <iostream>
#include <omp.h>
#include <tbb/tbb.h>

int main() {
  double t1 = omp_get_wtime();

  auto const n = 10000;   // number of nodes
  auto const n_el = 20; // number of elements

  Eigen::MatrixXd Kdense = Eigen::MatrixXd::Zero(n, n); // global stiffness matrix
  Eigen::MatrixXd Ke = Eigen::MatrixXd::Ones(n_el, n_el); // elemental stiffness
#pragma omp declare reduction(+ : Eigen::MatrixXd : omp_out =  omp_out + omp_in) \
                initializer(omp_priv=Eigen::MatrixXd::Zero(n,n))

#pragma omp parallel for reduction(+ : Kdense)
  for (int i = 0; i < n - n_el + 1; i++) {
    Kdense.block(i, i, n_el, n_el) += Ke;
  }

  double t2 = omp_get_wtime();
  std::cout << Kdense.block(n - n_el, n - n_el, n_el, n_el) << std::endl;
  std::cout << "/* message */" << t2 - t1 << '\n';

  using namespace tbb;
    std::vector<double> a(20);
    tbb::parallel_for( size_t(0), 10,1,  [&]( size_t i ) {
        a.at(i)=i; return;
    } );

}

// Eigen::SparseMatrix<double, Eigen::RowMajor> K(n, n);
// K.coeffs() = 0.0;
// auto Ke = Eigen::MatrixXd::Ones(n_el, n_el);

// #pragma omp critical
//     {
//       for (auto b = 0; b < dofs.size(); b++) {
//         for (auto a = 0; a < dofs.size(); a++) {
// #pragma omp critical
//           K.coeffRef(int(dofs[a]), int(dofs[b])) += Ke(a, b);
//         }
// }
// }


#include <iostream>
#include <cmath>
#include <algorithm>
#include <vector>
#include <string>
#include <boost/lexical_cast.hpp>
#include <boost/multiprecision/cpp_int.hpp>
using namespace std;

class Radix {
private:
    const char* s;
    int a[128];
public:
    Radix(const char* s = "0123456789ABCDEF") : s(s) {
        int i;
        for(i = 0; s[i]; ++i)
            a[(int)s[i]] = i;
    }
    std::string to(long long p, int q) {
        int i;
        if(!p)
            return "0";
        char t[64] = { };
        for(i = 62; p; --i) {
            t[i] = s[p % q];
            p /= q;
        }
        return std::string(t + i + 1);
    }
    std::string to(const std::string& t, int p, int q) {
        return to(to(t, p), q);
    }
    long long to(const std::string& t, int p) {
        int i;
        long long sm = a[(int)t[0]];
        for(i = 1; i < (int)t.length(); ++i)
            sm = sm * p + a[(int)t[i]];
        return sm;
    }
};
int main() {
    int k; cin >> k;
    string a, b; cin >> a >> b;
    Radix r;
    boost::multiprecision::cpp_int x = boost::lexical_cast<boost::multiprecision::cpp_int>(r.to(a, k));
    boost::multiprecision::cpp_int y = boost::lexical_cast<boost::multiprecision::cpp_int>(r.to(b, k));
    cout << x * y << endl;
}


//Code developed and used to produce the results of
//
//  Rhoads, D., Solé-Ribalta, A., González, M. C., & Borge-Holthoefer, J. (2020).
//  Planning for sustainable Open Streets in pandemic cities.
//  arXiv preprint arXiv:2009.12548.
//
//Please cite the above paper in any works using or derived
//from this software

#define TRUE 1
#define FALSE 0
#define DEBUG 0
#include "mex.h"
#define MAX_COLA 65001
#define MAX_NODES 65000
typedef unsigned long int node_id_type;
#include "predQueueCircStatic.h"
#include <string.h>
#include <math.h>
#include "matrix.h"
#include <boost/heap/fibonacci_heap.hpp>

char outputString[150];

//returns the value of a half-normal PDF with sigma=855 at x
double halfNormalPDF(double x){

    //855 is the sigma that corresponds to a situation where
    //95% of trips fall within a distance of 1667 meters

    return (sqrt(2)/(855*(sqrt(M_PI)))) * exp(-pow(x,2)/(2*pow(855,2)));

}

//returns one row of the estimated OD matrix,
//corresponding to the number of trips from s
//to all other nodes j
void getOdRow(
    node_id_type s,
    double * vStores,
    double * vPop,
    double * vCoords,
    node_id_type sizeSupra,
    double * attraction_vectPr,
    double * OD_rowPr
){

    double xi, yi, xj, yj, dist, mass, rowSum;

    memset( attraction_vectPr, 0, sizeof(double)*sizeSupra );
    memset( OD_rowPr, 0, sizeof(double)*sizeSupra );

    //x and y coordinates of node s
    xi = vCoords[s];
    yi = vCoords[sizeSupra + s];

    //keep track of a sum of the row, to normalize later
    rowSum = 0;
    //for all nodes j in the network...
    for (node_id_type j=0; j<sizeSupra; j++){
        if (s != j ){

            //x and y coordinates of node j
            xj = vCoords[j];
            yj = vCoords[sizeSupra + j];

            //Calculate distance between s and j
            dist = sqrt(pow(xi-xj,2) + pow(yi-yj,2));

            //mass = # stores at j * halfnorm(distance_sj)
            mass = vStores[j] * halfNormalPDF(dist);
            attraction_vectPr[j] = mass;
            rowSum = rowSum + mass;

        }
        else {
            attraction_vectPr[j] = 0;
        }
    }

    for (node_id_type j=0; j<sizeSupra; j++){
        //OD_row = population at source * normalized mass of j
        OD_rowPr[j] = vPop[s]*(attraction_vectPr[j] / rowSum);
    }
}

void printMatrix(double * P, int rows, int cols);
void printMatrixInverted(double * P, int rows, int cols);
void printSparseMatrix(int n, double * pr, size_t * ir, size_t * jc);
//to use the heaps
struct dk_Node{
    node_id_type nodeId;
    double distance;
    dk_Node(node_id_type id, double dist) : nodeId(id),distance(dist) { } //<-- this is a constructor
};
struct compare_dk_Node{
    bool operator()(const dk_Node& n1, const dk_Node& n2) const{
        return n1.distance > n2.distance;
    }
};

//prhs[0] -> Adjacency Matrix in time
//prhs[1] -> N nodes
//prhs[2] -> N layers = 1
//prhs[3] -> vector s of services per node
//prhs[4] -> vector p of population per node
//prhs[5] -> vector c of node coordinates x,y

//plhs[0] -> Node betweenness
//plhs[1] -> Directed edge betweenness
//plhs[2] -> Num at start node CHECK
//plhs[3] -> Num at end node CHECK

void mexFunction( int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[] ){

    //output matrices
    double tolerance = 0.000000001;
    node_id_type numNodes = 0, numLayers = 0;
    node_id_type sizeSupra = 0;
    mwSignedIndex dimsP[3];
    mxArray *rhs[1], *lhs[1];

    // get and check inputs
    if (nrhs != 6) mexErrMsgTxt( "Only 6 input arguments allowed." );
    if (nlhs > 2) mexErrMsgTxt( "Only 2 output argument allowed." );
    sizeSupra = mxGetN( prhs[0] );
    if (mxGetM( prhs[0] ) != sizeSupra) mexErrMsgTxt( "Input matrix G needs to be square." );
    if(mxIsSparse(prhs[0])==0) mexErrMsgTxt( "Input matrix must be sparse" );

    numNodes = mxGetPr(prhs[1])[0];
    numLayers = mxGetPr(prhs[2])[0];

    double * vStores = mxGetPr(prhs[3]);
    double * vPop = mxGetPr(prhs[4]);
    double * vCoords = mxGetPr(prhs[5]);

    mxArray * attraction_vect = mxCreateDoubleMatrix( sizeSupra, 1, mxREAL );
    mxArray * OD_row = mxCreateDoubleMatrix( sizeSupra, 1, mxREAL );
    double * attraction_vectPr = mxGetPr(attraction_vect);
    double * OD_rowPr = mxGetPr(OD_row);

    if(numLayers != 1){
        mexErrMsgTxt("This code is not prepared to work in multiplex networks" );
    }
    if((numNodes*numLayers)!=sizeSupra) mexErrMsgTxt( "Incorrect size of the input matrix" );

    rhs[0] = mxDuplicateArray(prhs[0]);
    mexCallMATLAB(1, lhs, 1, rhs, "'");
    mxDestroyArray(rhs[0]);

    double *Gpr = mxGetPr(lhs[0]);
    size_t *Gir = mxGetIr(lhs[0]);
    size_t *Gjc = mxGetJc(lhs[0]);

    double edgeWeight = 0;
    double min_dist = -1;

    long int sIndEdges;
    long int eIndEdges;
    long int nInd;
    node_id_type v,s,w,min_v;
    double numPacketReachDest;
    double * C_B = 0;
    double * spEBW = 0;
    staticQueue * P = NULL;
    double * sigma = 0;
    double * sigmaOrig = 0;
    double * d = 0;
    double * delta = 0;
    double * visited = 0;
    staticQueue S;

    //to store the nodes and the distance to them
    boost::heap::fibonacci_heap< dk_Node, boost::heap::compare<compare_dk_Node> > dk_heap;
    boost::heap::fibonacci_heap< dk_Node, boost::heap::compare<compare_dk_Node> >::handle_type heapHandles[MAX_NODES];

    plhs[0] = mxCreateDoubleMatrix( sizeSupra, 1, mxREAL );
    C_B = mxGetPr(plhs[0]);
    plhs[1] = mxCreateDoubleMatrix( numLayers*numNodes, numLayers*numNodes, mxREAL );
    spEBW = mxGetPr(plhs[1]);

    visited = (double *)mxMalloc(sizeof(double)*sizeSupra);
    sigma = (double *)mxMalloc(sizeof(double)*sizeSupra);
    sigmaOrig = (double *)mxMalloc(sizeof(double)*sizeSupra);
    d = (double *)mxMalloc(sizeof(double)*sizeSupra);
    P = (staticQueue *)mxMalloc(sizeof(staticQueue)*sizeSupra);
    delta = (double *)mxMalloc(sizeof(double)*sizeSupra);

    memset( C_B, 0, sizeof(double)*sizeSupra );
    memset( spEBW, 0, sizeof(double)*sizeSupra );

    for( s = 0 ; s<numNodes; s++){

        sprintf(outputString,"fprintf('Source = %lu\\n');",s+1);

        mexEvalString(outputString);

        getOdRow(s, vStores, vPop, vCoords, sizeSupra, attraction_vectPr, OD_rowPr);

        //initializations
        create(&S);

        for( w = 0 ; w<sizeSupra ; w++){
            create(&P[w]);
            sigma[w] = 0;
            d[w] = mxGetInf();
            visited[w] = 0;

            //store the handle in a table then use the table to access the handle
            heapHandles[w] = dk_heap.push(dk_Node(w,d[w]));
        }
        d[s] = 0;
        (*heapHandles[s]).distance = 0; dk_heap.decrease(heapHandles[s]);
        sigma[s] = 1;

        //end initializations

        min_v = dk_heap.top().nodeId;
        min_dist = dk_heap.top().distance;
        dk_heap.pop();

        while( min_dist < mxGetInf()){
        //Min_V := S;
        //Min_Dist := 0.0;
        //while Min_Dist /= Plus_Infinity loop

            if(DEBUG){
                sprintf(outputString,"fprintf('min_v %lu, min_dist = %f\\n');",min_v+1,min_dist);
                mexEvalString(outputString);
            }

            v = min_v;

            visited[v] = 1;
            add(&S,v);

            if(DEBUG){
                sprintf(outputString,"fprintf('Exploring %lu\\n');",v+1);
                mexEvalString(outputString);
            }

            //for each neigbor of v
            sIndEdges = Gjc[v];
            eIndEdges = Gjc[v+1] - 1;

            if(DEBUG){
                sprintf(outputString,"fprintf('\\tmin_v = %lu, min_dist = %f\\n');",min_v+1, min_dist);
                mexEvalString(outputString);
            }

            for( nInd = sIndEdges ; nInd<=eIndEdges ; nInd++ ){
            //while Has_Next(El) loop
                w = Gir[nInd];
                edgeWeight = Gpr[nInd];

                if(DEBUG){
                    sprintf(outputString,"fprintf('\tEdge from w=%lu to w=%lu and edgeWeight = %f\\n');",v+1, w+1, edgeWeight);
                    mexEvalString(outputString);
                }

                if(DEBUG){
                    sprintf(outputString,"fprintf('\\tedges out %lu\\n');",w);
                    mexEvalString(outputString);
                }

                if (v != w){
                //if V /= W then
                    if(d[w] > d[v] + edgeWeight){ // new path to the vertex
                        d[w] = d[v] + edgeWeight;

                        (*heapHandles[w]).distance = d[w]; dk_heap.decrease(heapHandles[w]);

                        sigma[w] = sigma[v];
                        create(&P[w]);
                        add(&P[w],v);

                        if(DEBUG){
                            sprintf(outputString,"fprintf('\\t\\tnew path to vertex %lu with dist  = %f\\n');",w+1, d[w]);
                            mexEvalString(outputString);
                        }
                    }else if( fabs(d[w] - d[v] - edgeWeight) < tolerance){ // the current path is of equal length
                        sigma[w] = sigma[w] + sigma[v];
                        add(&P[w],v);

                        if(DEBUG){
                            sprintf(outputString,"fprintf('\\t\\tpath to equal length to %lu , path difference  =%f\\n');",w+1, fabs(d[w] - d[v] - edgeWeight));
                            mexEvalString(outputString);
                            sprintf(outputString,"fprintf('\\t\\told length = %f, dist to %lu = %f and edgeweight = %f\\n');",d[w],v+1,d[v],edgeWeight);
                            mexEvalString(outputString);
                        }
                    }
                 }
                 //end if;
            }
            //end loop;

            if(!dk_heap.empty()){
                min_v = dk_heap.top().nodeId;
                min_dist = dk_heap.top().distance;
                dk_heap.pop();
            }else{
                min_v = -1;
                min_dist = mxGetInf();
            }
        }

        //keep an original copy of the sigma
        //memcpy(sigmaOrig,sigma,sizeof(double)*sizeSupra);
        for( w = 0 ; w<sizeSupra ; w++){
            delta[w] = 0;

        }

        while(!empty(&S)){

            w = pop(&S);

            if(DEBUG){
                sprintf(outputString,"fprintf('explore vertex w = %lu\\n');",w+1);
                mexEvalString(outputString);
            }

            if(DEBUG){
                sprintf(outputString,"fprintf('Distance from (s=%lu) to (w=%lu) = %f\\n');",s+1,w+1,d[w]);
                mexEvalString(outputString);
            }

            while(!empty(&P[w])){

                v = pop(&P[w]);

                if(DEBUG){
                    sprintf(outputString,"fprintf('\\tvertex %lu has predecessor %lu\\n');",w+1,v+1);
                    mexEvalString(outputString);
                }

                if(DEBUG){
                    sprintf(outputString,"fprintf('\\t\\tPred: (v=%lu)\\n');",v);
                    mexEvalString(outputString);
                    sprintf(outputString,"fprintf('\\t\\tsigma[v=%lu]=%i/sigma[w=%lu]=%i,delta[w=%lu]=%f\\n');",v,(int)sigma[v],w,(int)sigma[w],w,delta[w]);
                    mexEvalString(outputString);
                }

                //--> Aquí la OD matrix

                numPacketReachDest = OD_rowPr[w];

                delta[v] = delta[v] + (sigma[v]/sigma[w])*(numPacketReachDest + delta[w]);

                if(v != s){

                    spEBW[v + sizeSupra*w] = spEBW[v + sizeSupra*w] + (sigma[v]/sigma[w])*(numPacketReachDest + delta[w]);

                }
                if(v == s){

                    spEBW[v + sizeSupra*w] = spEBW[v + sizeSupra*w] + (sigma[v]/sigma[w])*(numPacketReachDest + delta[w]);

                }

            }

            if(w!=s){

                C_B[w] = C_B[w] + delta[w];  /// aqui la delta d'ha de normalizar no es poden sumar les betweenness
                                                               // de les differents capes simplement

                if(DEBUG){
                    sprintf(outputString,"fprintf('\\t\\tBetweenness[w(%lu)]=%f\\n');",w,C_B[w]);
                    mexEvalString(outputString);
                }

            }

        }

    }

    mxFree(sigma);
    mxFree(sigmaOrig);
    mxFree(d);
    mxFree(P);
    mxFree(delta);
    mxFree(visited);
}

void printMatrix(double * P, int rows, int cols){
    int rowIndex = 0, columnIndex = 0;

    //print the matrix
    for(rowIndex = 0 ; rowIndex < rows; rowIndex++){
        for(columnIndex = 0 ; columnIndex < cols; columnIndex++){
            mexPrintf("%1.4f ",P[rowIndex*cols + columnIndex]);
        }
        mexPrintf("\n");
    }
}

void printMatrixInverted(double * P, int rows, int cols){
    int rowIndex = 0, columnIndex = 0;

    //print the matrix
    for(rowIndex = 0 ; rowIndex < rows; rowIndex++){
        for(columnIndex = 0 ; columnIndex < cols; columnIndex++){
            mexPrintf("%1.4f ",P[columnIndex*rows + rowIndex]);
        }
        mexPrintf("\n");
    }
}

void printSparseMatrix(int n, double * pr, size_t * ir, size_t * jc){
    size_t nrow;

    mexPrintf("This is from the .cpp");

    for (int y=0; y<n; y++){
        nrow = jc[y+1] - jc[y];
        for (int x = 0; x<nrow; x++){
            mexPrintf("      (%d,%d)    %g\n", (*ir++)+1,y+1,*pr++);
        }
    }
}


/**
 * \file dcs/math/detail/float.hpp
 *
 * \brief Utilities for floating-point comparison.
 *
 * \author Marco Guazzone (marco.guazzone@gmail.com)
 *
 * <hr/>
 *
 * Copyright 2012 Marco Guazzone (marco.guazzone@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
 *
 * 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 DCS_MATH_DETAIL_FLOAT_HPP
#define DCS_MATH_DETAIL_FLOAT_HPP

#include <algorithm>
#include <boost/type_traits/is_floating_point.hpp>
#include <boost/utility/enable_if.hpp>
//#include <cfloat>
#include <cmath>

namespace dcs { namespace math { namespace detail {

/// See also:
/// - http://www.cygnus-software.com/papers/comparingfloats/comparingfloats.htm
/// - http://www.petebecker.com/js/js200012.html
/// - http://code.google.com/p/googletest/source/browse/trunk/include/gtest/internal/gtest-internal.h
/// - http://www.parashift.com/c++-faq-lite/newbie.html#faq-29.16
/// - http://adtmag.com/articles/2000/03/16/comparing-floats-how-to-determine-if-floating-quantities-are-close-enough-once-a-tolerance-has-been.aspx
/// - http://www.boost.org/doc/libs/1_47_0/libs/test/doc/html/utf/testing-tools/floating_point_comparison.html
/// - http://learningcppisfun.blogspot.com/2010/04/comparing-floating-point-numbers.html
/// - http://floating-point-gui.de/errors/comparison/
/// - https://randomascii.wordpress.com/2012/02/25/comparing-floating-point-numbers-2012-edition/
/// .

/**
 * \brief x is approximately equal to y.
 *
 * Inspired by [1]:
 * \f[
 *  $x \approx y\,\text{ if and only if } |y-x|\le\epsilon\max(|x|,|y|)
 * \f]
 *
 * References:
 * -# Knuth, "The Art of Computer Programming: Vol.2" 3rd Ed, 1998, Sec. 4.2.2.
 * .
 */
template <typename T>
inline
typename ::boost::enable_if<
	::boost::is_floating_point<T>,
	bool
>::type approximately_equal(T x, T y, T tol)
{
	// Try first with standard comparison (handles the case when both x and y are zero)
	if (x == y)
	{
		return true;
	}

	// Handle degenerate cases
	if (::std::isnan(x) || ::std::isnan(y))
	{
		// According to IEEE, NaN are different even by itself
		return false;
	}
	if (::std::isinf(x) && ::std::isinf(y))
	{
		// According to IEEE, Infinite operands of the same sign shall compare equal
		return true;
	}
	if ((::std::isinf(x) && ::std::isfinite(y)) || (::std::isfinite(x) && ::std::isinf(y)))
	{
		// Infinity vs non-infinite operands are different
		return false;
	}

	return ::std::abs(x-y) <= (::std::max(::std::abs(x), ::std::abs(y))*tol);
}

/**
 * \brief x is definitely equal to y.
 *
 * Inspired by [1]:
 * \f[
 *  $x \sim y\,\text{ if and only if } |y-x|\le\epsilon\min(|x|,|y|)
 * \f]
 *
 * References:
 * -# Knuth, "The Art of Computer Programming: Vol.2" 3rd Ed, 1998, Sec. 4.2.2.
 * .
 */
template <typename T>
inline
typename ::boost::enable_if<
	::boost::is_floating_point<T>,
	bool
>::type essentially_equal(T x, T y, T tol)
{
	// Try first with standard comparison (handles the case when both x and y are zero)
	if (x == y)
	{
		return true;
	}
	// Handle degenerate cases
	if (::std::isnan(x) || ::std::isnan(y))
	{
		// According to IEEE, NaN are different even by itself
		return false;
	}
	if (::std::isinf(x) && ::std::isinf(y))
	{
		// According to IEEE, Infinite operands of the same sign shall compare equal
		return true;
	}
	if ((::std::isinf(x) && ::std::isfinite(y)) || (::std::isfinite(x) && ::std::isinf(y)))
	{
		// Infinity vs non-infinite operands are different
		return false;
	}

	return ::std::abs(x-y) <= (::std::min(::std::abs(x), ::std::abs(y))*tol);
}

/**
 * \brief x is definitely greater than y.
 *
 * Inspired by [1]:
 * \f[
 *  $x \succ y\,\text{ if and only if } x-y > \epsilon\max(|x|,|y|)
 * \f]
 *
 * References:
 * -# Knuth, "The Art of Computer Programming: Vol.2" 3rd Ed, 1998, Sec. 4.2.2.
 * .
 */
template <typename T>
inline
typename ::boost::enable_if<
	::boost::is_floating_point<T>,
	bool
>::type definitely_greater(T x, T y, T tol)
{
	// Try first with standard comparison
	if (x <= y)
	{
		return false;
	}

	// Handle degenerate cases
	if (::std::isnan(x) || ::std::isnan(y))
	{
		// According to IEEE, NaN are different even by itself
		return false;
	}
	if (::std::isinf(x) && ::std::isfinite(y))
	{
		return true;
	}
	if (::std::isfinite(x) && ::std::isinf(y))
	{
		return false;
	}

	return (x-y) > (::std::max(::std::abs(x), ::std::abs(y))*tol);
}

/**
 * \brief x is definitely less than y.
 *
 * Inspired by [1]:
 * \f[
 *  $x \prec y\,\text{ if and only if } y-x > \epsilon\max(|x|,|y|)
 * \f]
 *
 * References:
 * -# Knuth, "The Art of Computer Programming: Vol.2" 3rd Ed, 1998, Sec. 4.2.2.
 * .
 */
template <typename T>
inline
typename ::boost::enable_if<
	::boost::is_floating_point<T>,
	bool
>::type definitely_less(T x, T y, T tol)
{
	// Try first with standard comparison
	if (x >= y)
	{
		return false;
	}

	// Handle degenerate cases
	if (::std::isnan(x) || ::std::isnan(y))
	{
		// According to IEEE, NaN are different even by itself
		return false;
	}
	if (::std::isinf(x) && ::std::isfinite(y))
	{
		return false;
	}
	if (::std::isfinite(x) && ::std::isinf(y))
	{
		return true;
	}

	return (y-x) > (::std::max(::std::abs(x), ::std::abs(y))*tol);
}

}}} // Namespace dcs::math::detail

#endif // DCS_MATH_DETAIL_FLOAT_HPP


#include <iostream>
#include <Eigen/Dense>

using namespace Eigen;

int main() {
  MatrixXd m(2, 2);
  m(0, 0) = 3;
  m(1, 0) = 2.5;
  m(0, 1) = -1;
  m(1, 1) = m(1, 0) + m(0, 1);
  std::cout << "Here is the matrix m:\n" << m << std::endl;
  VectorXd v(2);
  v(0) = 4;
  v(1) = v(0) - 1;
  std::cout << "Here is the vector v:\n" << v << std::endl;
}


#include <iostream>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/algorithm/minmax.hpp>
#include <vector>
#include <omp.h>

#include "dbscan.h"

namespace clustering {
DBSCAN::ClusterData DBSCAN::gen_cluster_data( size_t features_num, size_t elements_num )
{
    DBSCAN::ClusterData cl_d( elements_num, features_num );

    for ( size_t i = 0; i < elements_num; ++i ) {
        for ( size_t j = 0; j < features_num; ++j ) {
            cl_d( i, j ) = ( -1.0 + rand() * ( 2.0 ) / RAND_MAX );
        }
    }

    return cl_d;
}

DBSCAN::FeaturesWeights DBSCAN::std_weights( size_t s )
{
    // num cols
    DBSCAN::FeaturesWeights ws( s );

    for ( size_t i = 0; i < s; ++i ) {
        ws( i ) = 1.0;
    }

    return ws;
}

DBSCAN::DBSCAN()
{
}

static int num_threads_or_default( int nt )
{
    if ( !nt ) {
        return omp_get_max_threads();
    }
    return nt;
}

void DBSCAN::init( double eps, size_t min_elems, int num_threads )
{
    m_eps = eps;
    m_min_elems = min_elems;
    m_num_threads = num_threads_or_default( num_threads );
}

DBSCAN::DBSCAN( double eps, size_t min_elems, int num_threads )
    : m_eps( eps )
    , m_min_elems( min_elems )
    , m_num_threads( num_threads_or_default( num_threads ) )
    , m_dmin( 0.0 )
    , m_dmax( 0.0 )
{
    reset();
}

DBSCAN::~DBSCAN()
{
}

void DBSCAN::reset()
{
    m_labels.clear();
}

void DBSCAN::prepare_labels( size_t s )
{
    m_labels.resize( s );

    for ( auto& l : m_labels ) {
        l = -1;
    }
}

const DBSCAN::DistanceMatrix DBSCAN::calc_dist_matrix( const DBSCAN::ClusterData& C, const DBSCAN::FeaturesWeights& W )
{
    DBSCAN::ClusterData cl_d = C;

    omp_set_dynamic( 0 );
    omp_set_num_threads( m_num_threads );
#pragma omp parallel for
    for ( size_t i = 0; i < cl_d.size2(); ++i ) {
        ublas::matrix_column< DBSCAN::ClusterData > col( cl_d, i );

        const auto r = minmax_element( col.begin(), col.end() );

        double data_min = *r.first;
        double data_range = *r.second - *r.first;

        if ( data_range == 0.0 ) {
            data_range = 1.0;
        }

        const double scale = 1 / data_range;
        const double min = -1.0 * data_min * scale;

        col *= scale;
        col.plus_assign( ublas::scalar_vector< typename ublas::matrix_column< DBSCAN::ClusterData >::value_type >( col.size(), min ) );
    }

    // rows x rows
    DBSCAN::DistanceMatrix d_m( cl_d.size1(), cl_d.size1() );
    ublas::vector< double > d_max( cl_d.size1() );
    ublas::vector< double > d_min( cl_d.size1() );

    omp_set_dynamic( 0 );
    omp_set_num_threads( m_num_threads );
#pragma omp parallel for
    for ( size_t i = 0; i < cl_d.size1(); ++i ) {
        for ( size_t j = i; j < cl_d.size1(); ++j ) {
            d_m( i, j ) = 0.0;

            if ( i != j ) {
                ublas::matrix_row< DBSCAN::ClusterData > U( cl_d, i );
                ublas::matrix_row< DBSCAN::ClusterData > V( cl_d, j );

                int k = 0;
                for ( const auto e : ( U - V ) ) {
                    d_m( i, j ) += fabs( e ) * W[k++];
                }

                d_m( j, i ) = d_m( i, j );
            }
        }

        const auto cur_row = ublas::matrix_row< DBSCAN::DistanceMatrix >( d_m, i );
        const auto mm = minmax_element( cur_row.begin(), cur_row.end() );

        d_max( i ) = *mm.second;
        d_min( i ) = *mm.first;
    }

    m_dmin = *( min_element( d_min.begin(), d_min.end() ) );
    m_dmax = *( max_element( d_max.begin(), d_max.end() ) );

    m_eps = ( m_dmax - m_dmin ) * m_eps + m_dmin;

    return d_m;
}

DBSCAN::Neighbors DBSCAN::find_neighbors( const DBSCAN::DistanceMatrix& D, uint32_t pid )
{
    Neighbors ne;

    for ( uint32_t j = 0; j < D.size1(); ++j ) {
        if ( D( pid, j ) <= m_eps ) {
            ne.push_back( j );
        }
    }
    return ne;
}

void DBSCAN::dbscan( const DBSCAN::DistanceMatrix& dm )
{
    std::vector< uint8_t > visited( dm.size1() );

    uint32_t cluster_id = 0;

    for ( uint32_t pid = 0; pid < dm.size1(); ++pid ) {
        if ( !visited[pid] ) {
            visited[pid] = 1;

            Neighbors ne = find_neighbors( dm, pid );

            if ( ne.size() >= m_min_elems ) {
                m_labels[pid] = cluster_id;

                for ( uint32_t i = 0; i < ne.size(); ++i ) {
                    uint32_t nPid = ne[i];

                    if ( !visited[nPid] ) {
                        visited[nPid] = 1;

                        Neighbors ne1 = find_neighbors( dm, nPid );

                        if ( ne1.size() >= m_min_elems ) {
                            for ( const auto& n1 : ne1 ) {
                                ne.push_back( n1 );
                            }
                        }
                    }

                    if ( m_labels[nPid] == -1 ) {
                        m_labels[nPid] = cluster_id;
                    }
                }

                ++cluster_id;
            }
        }
    }
}

void DBSCAN::fit( const DBSCAN::ClusterData& C )
{
    const DBSCAN::FeaturesWeights W = DBSCAN::std_weights( C.size2() );
    wfit( C, W );
}
void DBSCAN::fit_precomputed( const DBSCAN::DistanceMatrix& D )
{
    prepare_labels( D.size1() );
    dbscan( D );
}

void DBSCAN::wfit( const DBSCAN::ClusterData& C, const DBSCAN::FeaturesWeights& W )
{
    prepare_labels( C.size1() );
    const DBSCAN::DistanceMatrix D = calc_dist_matrix( C, W );
    dbscan( D );
}

const DBSCAN::Labels& DBSCAN::get_labels() const
{
    return m_labels;
}

std::ostream& operator<<( std::ostream& o, DBSCAN& d )
{
    o << "[ ";
    for ( const auto& l : d.get_labels() ) {
        o << " " << l;
    }
    o << " ] " << std::endl;

    return o;
}
}


#include <iostream>
#include "ArchivoTrain.h"
#include "Imagen.h"
#include "Trabajador.h"
#include "config.h"
#include "ArchivoNeighbors.h"
#include <set>
#include "ActiveSet.h"
#include <fstream>
#include <Eigen/Eigenvalues>
#include <algorithm>
#include <iterator>
/**
 * Necesito que el archivo venga en
 * "exactamente" el mismo formato que
 * el train de kaggle, se detecta la
 * cantidad de dimensiones
 * */
//weinberger 09 - páginas 33 a 35
using namespace std;

void filtrarNegativos(Matriz& m){
	for(int i = 0; i < m.rows(); i++){
		if(m(i,i)<0){
			m(i,i)=0;
		}
	}
}

void obtenerSalieron(	ConjuntoActiveSets& salieron,
						ConjuntoActiveSets& activos,
						ConjuntoActiveSets& anterior){
	//salieron = anterior - activos
	set_difference(	anterior.begin(),anterior.end(),
					activos.begin(),activos.end(),
					inserter(salieron,salieron.end()),
					CompararActiveSet()
	);
}
void obtenerEntraron(	ConjuntoActiveSets& entraron,
						ConjuntoActiveSets& activos,
						ConjuntoActiveSets& anterior){
	//entraron = activos - anterior
	set_difference(	activos.begin(),activos.end(),
					anterior.begin(),anterior.end(),
					inserter(entraron,entraron.end()),
					CompararActiveSet()
	);
	//notese que los argumentos están invertidos
}

int main(int argc, char* argv[]){
	if(argc==1){
		cout<<"Y los argumentos?"<<endl;
	}else{
		if(argv[1][0]=='n'){
			cout<<"Recalculando target neighbours"<<endl;
			ArchivoTrain archivo;
			archivo.conectarTargetNeighbors(5,0,1000);
			archivo.guardarTargetNeighbours(0,1000);
		}else if(argv[1][0]=='m'){
			ofstream off("M.dat");
			cout<<"Voy a calcular la matriz M"<<endl;
			ArchivoTrain archivo;
			ArchivoNeighbors neighbors(archivo);

			Matriz m,g;
			m.setIdentity();//initialize with the identity matrix
			ConjuntoActiveSets posibles_activos, activos, anteriores;//initialize active sets
			//neighbors.calcularG0(g);//initialize gradient

			for(int i = 0; i<2; i++){//while not converged do
				cout<<"Iniciando nueva iteracion:"<<i<<endl;
				if(i==0){//if mod(i,10)==0 || casi convergencia then
					posibles_activos.clear();
					neighbors.agregarActiveSets(posibles_activos);//compute Nt+1 exactly
					//(es inviable hacer la búsqueda de active sets con la distancia Mahalanobis)
					activos = posibles_activos;//update active set
				}else{
					anteriores = activos;
					neighbors.filtrarActivos(posibles_activos, activos,m);
					//compute Nt+1 only search active set
				}
				cout<<"Posibles activos:"<<activos.size()<<endl;

				cout<<"Voy a calcular la G nueva"<<endl;

				ConjuntoActiveSets salieron, entraron;
				obtenerSalieron(salieron,activos,anteriores);
				obtenerEntraron(entraron,activos,anteriores);
				cout<<"Ya tengo los que salientraron"<<endl;

				Matriz gResta,gSuma;
				neighbors.calcularSegundoTerminoGradiente(gResta,salieron);
				neighbors.calcularSegundoTerminoGradiente(gSuma,entraron);
				g+=g-gResta+gSuma;//sumaresta de gradientes

				cout<<"Descmponiendo las matrices"<<endl;

				Matriz m_def = m-0.1*g;//Mt-alfa*Gt+1
				Eigen::EigenSolver<Matriz> solver(m_def);
				Matriz delta = solver.pseudoEigenvalueMatrix();
				Matriz v = solver.pseudoEigenvectors();
				filtrarNegativos(delta);//proyección sobre semidefinidas positivas
				m=v*delta*v.transpose();//take gradient step

			}

			off<<m;//output Mt

		}else if(argv[1][0]=='g'){
			ofstream off("G0.dat");
			cout<<"Voy a calcular la matriz G0"<<endl;
			ArchivoTrain archivo;
			ArchivoNeighbors neighbors(archivo);
			Matriz g0;
			neighbors.calcularG0(g0);
			cout<<"G0 es "<<g0<<endl;
			off<<g0;
		}else if(argv[1][0]=='a'){
			ofstream off("G0.dat");
			cout<<"Voy a calcular los ActiveSets"<<endl;
			Matriz m,g0;
			m.setIdentity();
			ArchivoTrain archivo;
			ArchivoNeighbors neighbors(archivo);

			ConjuntoActiveSets caca;
			neighbors.agregarActiveSets(caca);
			cout<<"Activesets: "<<caca.size()<<endl;
		}
	}
	return 0;
}


#include "average_case_error.hpp"

#include <cudd_helpers.hpp>
#include <boost/multiprecision/cpp_int.hpp>

namespace abo::error_metrics {

    boost::multiprecision::cpp_dec_float_100 average_value(const std::vector<BDD> &f) {

        boost::multiprecision::uint256_t sum = 0;
        const boost::multiprecision::uint256_t one = 1;

        int max_support_size = 0;
        for (const auto &bdd : f) {
            max_support_size = std::max(max_support_size, bdd.SupportSize());
        }

        for (unsigned int i = 0;i<f.size();i++) {
            // TODO: CountMinterms returns a double, might loose precision
            double minterms = f[i].CountMinterm(max_support_size);
            sum += (one << i) * boost::multiprecision::uint256_t(minterms);
        }

        return boost::multiprecision::cpp_dec_float_100(sum) /
                boost::multiprecision::cpp_dec_float_100(one << max_support_size);
    }

    boost::multiprecision::cpp_dec_float_100 mean_squared_value(const std::vector<BDD> &f) {

        boost::multiprecision::uint256_t sum = 0;
        const boost::multiprecision::uint256_t one = 1;

        int max_support_size = 0;
        for (const auto &bdd : f) {
            max_support_size = std::max(max_support_size, bdd.SupportSize());
        }

        for (unsigned int i = 0;i<f.size();i++) {
            for (unsigned int b = i;b<f.size();b++) {
                // cudd handles the case i = b efficiently
                BDD both = f[i] & f[b];
                double minterms = both.CountMinterm(max_support_size);
                // most terms (except for i == b), appear twice when factoring the square
                boost::multiprecision::uint256_t doubling_factor = i == b ? 1 : 2;
                sum += (one << i) * (one << b) * boost::multiprecision::uint256_t(minterms) * doubling_factor;
            }
        }

        return boost::multiprecision::cpp_dec_float_100(sum) /
                boost::multiprecision::cpp_dec_float_100(one << max_support_size);
    }

    boost::multiprecision::cpp_dec_float_100 average_case_error(const Cudd &mgr, const std::vector<BDD> &f, const std::vector<BDD> &f_hat,
                                                                const util::NumberRepresentation num_rep) {

        std::vector<BDD> absolute_difference = abo::util::bdd_absolute_difference(mgr, f, f_hat, num_rep);
        return average_value(absolute_difference);
    }

    boost::multiprecision::cpp_dec_float_100 mean_squared_error(const Cudd &mgr, const std::vector<BDD> &f, const std::vector<BDD> &f_hat,
                                                                const util::NumberRepresentation num_rep) {

        std::vector<BDD> absolute_difference = abo::util::bdd_absolute_difference(mgr, f, f_hat, num_rep);
        return mean_squared_value(absolute_difference);
    }

    boost::multiprecision::cpp_dec_float_100 average_case_error_add(const Cudd &mgr, const std::vector<BDD> &f, const std::vector<BDD> &f_hat,
                                                                    const NumberRepresentation num_rep) {

        ADD diff = abo::util::absolute_difference_add(mgr, f, f_hat, num_rep);
        std::vector<std::pair<double, unsigned long>> terminal_values = abo::util::add_terminal_values(diff);

        boost::multiprecision::uint256_t sum = 0;
        boost::multiprecision::uint256_t path_sum = 0;
        for (auto p : terminal_values) {
            sum += boost::multiprecision::uint256_t(p.first) * p.second;
            path_sum += p.second;
        }
        return boost::multiprecision::cpp_dec_float_100(sum) /
                boost::multiprecision::cpp_dec_float_100(path_sum);
    }

    boost::multiprecision::cpp_dec_float_100 mean_squared_error_add(const Cudd &mgr, const std::vector<BDD> &f, const std::vector<BDD> &f_hat,
                                                                    const NumberRepresentation num_rep) {

        ADD diff = abo::util::absolute_difference_add(mgr, f, f_hat, num_rep);
        std::vector<std::pair<double, unsigned long>> terminal_values = abo::util::add_terminal_values(diff);

        boost::multiprecision::uint256_t sum = 0;
        boost::multiprecision::uint256_t path_sum = 0;
        for (auto p : terminal_values) {
            boost::multiprecision::uint256_t value(p.first);
            sum += value * value * p.second;
            path_sum += p.second;
        }
        return boost::multiprecision::cpp_dec_float_100(sum) /
                boost::multiprecision::cpp_dec_float_100(path_sum);
    }

}