#include <bspline_fiting.hh>
#include <Geo/iterate.hh>

#pragma warning( push )
#pragma warning( disable : 4714 )
#include <Eigen/Dense>

#include <fstream>
#include <vector>

namespace Geo {

template<size_t dimT>
struct BsplineFitting : public IBsplineFitting<dimT>
{
  bool init(const size_t _deg,
    const std::vector<double>& _knots, const IFunction& _f)
  {
    if (_knots.empty())
      return false;
    auto extn = (_knots.back() - _knots.front()) / 2;
    knots_.push_back(_knots.front() - extn);
    knots_.insert(knots_.end(), _knots.begin(), _knots.end());
    knots_.push_back(_knots.back() + extn);
    deg_ = _deg;
    f_ = &_f;
    X_.clear();
    A_.clear();
    B_.clear();
    return true;
  }

  void set_parameter_correction_iterations(size_t _itr_nmbr) { itr_nmbr_ = _itr_nmbr; }
  void set_favour_boundaries(const bool _fvr_bndr) { fvr_bndr_ = _fvr_bndr; }
  void set_samples_per_interval(const size_t _smpl_per_intrvl)
  {
    smpl_per_intrvl_ = _smpl_per_intrvl;
  }

  void compute();
  const std::vector<VectorD<dimT>>& X() const { return X_; }
  VectorD<dimT> eval(const double _t);
private:
  void find_equations();
  double N(size_t _i, const size_t _k, const double _t);
  void add_equation(const double _t, const double _wi);

  std::vector<std::vector<double>> A_;
  std::vector<double> knots_;
  std::vector<VectorD<dimT>> B_;
  std::vector<VectorD<dimT>> X_;
  size_t deg_ = 0;
  const IFunction* f_ = nullptr;
  size_t itr_nmbr_ = 0;
  bool fvr_bndr_ = true;
  size_t smpl_per_intrvl_ = 4;
};

template<size_t dimT>
double BsplineFitting<dimT>::N(size_t _i, const size_t _p, const double _t)
{
  if (_p == 0)
  {
    if (_t < knots_[_i] || _t >= knots_[_i + 1])
      return 0;
    return 1;
  }
  double res = 0;

  auto b = N(_i, _p - 1, _t);
  if (b != 0)
    res += b * (_t - knots_[_i]) / (knots_[_i + _p] - knots_[_i]);

  b = N(_i + 1, _p - 1, _t);
  if (b != 0)
  {
    auto end_kn = knots_[_i + _p + 1];
    res += b * (end_kn - _t) / (end_kn - knots_[_i + 1]);
  }
  return res;
}

template<size_t dimT>
void BsplineFitting<dimT>::add_equation(const double _t, const double _wi)
{
  A_.emplace_back();
  const auto wi_sqr = sqrt(_wi);
  for (int j = 0; j < knots_.size() - deg_ - 1; ++j)
    A_.back().push_back(N(j, deg_, _t) * wi_sqr);

  VectorD<dimT> pt_crv;
  if (X_.empty() || (_t == knots_[1]) || (_t == knots_[knots_.size() - 2]))
    pt_crv = f_->evaluate(_t);
  else
    pt_crv = f_->closest_point(eval(_t), _t);
  B_.emplace_back(pt_crv * wi_sqr);
};

template<size_t dimT>
void BsplineFitting<dimT>::find_equations()
{
  A_.clear();
  B_.clear();
  if (fvr_bndr_)
  {
    double w_prev = 0;
    const auto last_idx = knots_.size() - 2;
    for (size_t i = 2; i <= last_idx; ++i)
    {
      auto dw = knots_[i] - knots_[i - 1];
      if (dw <= 0)
        continue;
      const double step = 1. / smpl_per_intrvl_;
      const double w_step = dw * step;
      auto wi = w_prev + w_step / 2;
      for (double x = 0; x < 1; x += step)
      {
        auto t = knots_[i - 1] * (1 - x) + knots_[i] * x;
        add_equation(t, wi);
        wi = w_step;
      }
      w_prev = w_step / 2;
    }
    add_equation(knots_[last_idx], w_prev);
  }
  else
  {
    const auto last_idx = knots_.size() - 2;
    for (size_t i = 2; i <= last_idx; ++i)
    {
      auto dw = knots_[i] - knots_[i - 1];
      if (dw <= 0)
        continue;
      const double step = 1. / smpl_per_intrvl_;
      const double wi = dw * step;
      for (double x = step / 2; x < 1; x += step)
        add_equation(knots_[i - 1] * (1 - x) + knots_[i] * x, wi);
    }
  }
}

template<size_t dimT>
void BsplineFitting<dimT>::compute()
{
  for (size_t iter = 0; iter <= itr_nmbr_; ++iter)
  {
    find_equations();
    const auto row_nmbr = A_.size();
    const auto col_nmbr = A_.front().size();
    Eigen::MatrixXd A(row_nmbr, col_nmbr);
    for (int i = 0; i < row_nmbr; ++i)
    {
      for (int j = 0; j < col_nmbr; ++j)
        A(i, j) = A_[i][j];
    }
    const auto& jsvd =
      A.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV);
    for (int j = 0; j < B_[0].size(); ++j)
    {
      Eigen::VectorXd B(B_.size());
      for (int i = 0; i < row_nmbr; ++i)
        B(i) = B_[i][j];
      Eigen::VectorXd res = jsvd.solve(B);
      X_.resize(col_nmbr);
      for (int i = 0; i < col_nmbr; ++i)
        X_[i][j] = res(i);
    }
  }
}

template<size_t dimT>
VectorD<dimT> BsplineFitting<dimT>::eval(const double _t)
{
  VectorD<dimT> res = { 0 };
  for (int i = 0; i < X_.size(); ++i)
    res += N(i, deg_, _t) * X_[i];
  return res;
}

template<size_t dimT>
std::shared_ptr<IBsplineFitting<dimT>> IBsplineFitting<dimT>::make()
{
  return std::make_shared<BsplineFitting<dimT>>();
}

template struct IBsplineFitting<2>;
template struct IBsplineFitting<3>;

}//namespace Geo


// linpack_unchained.cpp: system solver benchmark with exact solution
// This program completes the design goal of LINPACK, that is,
// create an idealized problem where both the input values and the correct answer
// are expressible in the numerical vocabulary of the computing environment.
//
// Copyright (C) 2017-2020 Stillwater Supercomputing, Inc.
//
// This file is part of the HPRBLAS project, which is released under an MIT Open Source license.
#pragma warning(disable : 4996)
#include "common.hpp"

#include <iostream>
#include <typeinfo>

#include <boost/numeric/mtl/mtl.hpp>
#include <boost/numeric/itl/itl.hpp>

// if you need to configure the posit number system, do it before including <hprblas>
#include <hprblas>
// matrix generators
#include <generators/matrix_generators.hpp>

using namespace std;
using namespace sw::universal;

#define BACKEND_MTL 0
#define BACKEND_EIGEN 1

#if defined(BACKEND_MTL)
#if ARITHMETIC_POSIT
using Tensor = mtl::Tensor<  posit<nbits, es> >;
#elif ARITHMETIC_INT8
using Tensor = mtl::Tensor< uint8_t >;
#elif ARITHMETIC_FP16
using Tensor = mtl::Tensor< fp16 >;
#endif
#elif defined(BACKEND_EIGEN)
#if ARITHMETIC_POSIT
using Tensor = Eigen::Tensor<  posit<nbits, es> >;
#elif ARITHMETIC_INT8
using Tensor = Eigen::Tensor< uint8_t >;
#elif ARITHMETIC_FP16
using Tensor = Eigen::Tensor< fp16 >;
#endif
#endif

int main(int argc, char** argv)
try {
	const size_t nbits = 32;
	const size_t es = 2;

	{
		using Scalar = posit<nbits, es>;
		using Vector = mtl::dense_vector< Scalar >;
		using Matrix = mtl::dense2D< Scalar >;

		constexpr size_t N = 100;
		Matrix A(N, N);
		Vector r(N), rtilde(N);
		Vector x(N, 1.0), b(N);
		// generate a uniform random matrix
		//sw::hprblas::uniform_rand_sorted(A);
		sw::hprblas::uniform_rand(A, -100.0, 100.0);

#define MANUAL 0
#if MANUAL
		b = A * x;
		x = --posit<nbits, es>(1); /// 1 - eps
		r = b - A * x;
		rtilde = r;
		Scalar rho = sw::hprblas::fused_dot<Vector, nbits, es>(rtilde, r);
		cout << "rho: " << double(rho) << endl;
		sw::hprblas::printVector(cout, "r: ", r);

#else
		// Create an ILU(0) preconditioner
		//itl::pc::ilu_0<Matrix>    P(A);   // <-- this does a LU decomposition with pivoting
		itl::pc::identity<Matrix>	P(A);

		// Set b such that x == 1 is solution; start with x == 0
		b = A * x; x = 0;

		// Termination criterion: r < 1e-6 * b or N iterations
		itl::noisy_iteration<Scalar>       iter(b, 500, 1.e-6);

		try {
			// Solve Ax == b with left preconditioner P
			itl::bicgstab(A, x, b, P, iter);
		}
		catch (const std::runtime_error& e) {
			cerr << "caught solver exception: " << e.what() << endl;
		}
		catch (...) {
			cerr << "caught unknown exception using bicgstab" << endl;
		}

		return 0;

#endif

		if (b != x) {
			typedef typename mtl::Collection<Matrix>::size_type     size_type;
			posit<nbits, es> p, one(1);
			for (size_type r = 0; r < num_rows(A); ++r) {
				sw::universal::quire<nbits, es> q, qt;
				for (size_type c = 0; c < num_cols(A); ++c) {
					p = A[r][c];
					q += quire_mul(one, p);
					qt.reset();
					qt += quire_mul(one, p);
					cout << qt << endl;
				}
				cout << q << endl;

				convert(q.to_value(), p);
				qt.reset();
				qt += quire_mul(posit<nbits, es>(1.0), p);
				cout << qt << endl << endl;
			}
		}
		else {
			cout << "Solution vector:\n" << x << endl;
		}
	}

	return 0;

	{
		using Matrix = mtl::dense2D<float>;
		using Vector = mtl::dense_vector<float>;
		Matrix  A(4, 4), L(4, 4), U(4, 4), LU(4, 4);
		Vector	x(4), b(4), xx(4);
		double 	c = 1.0;

		sw::hprblas::uniform_rand(A);  // uniform random with values between [0,1]
		LU = A;
		lu(LU);
		cout << A << endl;
		cout << LU << endl;
	}

	int nrOfFailedTestCases = 0;
	return nrOfFailedTestCases > 0 ? EXIT_FAILURE : EXIT_SUCCESS;
}
catch (char const* msg) {
	std::cerr << "caught ad hoc exception: " << msg << std::endl;
	return EXIT_FAILURE;
}
catch (const posit_arithmetic_exception& err) {
	std::cerr << "caught posit_arithmetic_exception: " << err.what() << std::endl;
	return EXIT_FAILURE;
}
catch (const posit_internal_exception& err) {
	std::cerr << "caught posit_internal_exception: " << err.what() << std::endl;
	return EXIT_FAILURE;
}
catch (const quire_exception& err) {
	std::cerr << "caught quire exception: " << err.what() << std::endl;
	return EXIT_FAILURE;
}
catch (const mtl::domain_error& err) {
	std::cerr << "caught linear algebra domain exception: " << err.what() << std::endl;
	return EXIT_FAILURE;
}
catch (const itl::search_space_exhaustion& err) {
	std::cerr << "caught iterative solver domain exception: " << err.what() << std::endl;
	return EXIT_FAILURE;
}
catch (const std::runtime_error& err) {
	std::cerr << "caught runtime_error: " << err.what() << std::endl;
	return EXIT_FAILURE;
}
catch (...) {
	std::cerr << "caught unknown exception" << std::endl;
	return EXIT_FAILURE;
}


// --------------------------------------------------------------
// SL 2019-02-25
// A simple, easily hackable topopt code, directly inspired
// from "A 99 line topology optimization code written in MATLAB"
// MIT-license
// (c) Sylvain Lefebvre, https://github.com/sylefeb
// --------------------------------------------------------------
/*

This code is directly inspired from the fantastic paper

A 99 line topology optimization code written in MATLAB
Structural and Multidisciplinary Optimization 21(2), 2001, pp. 120-127
by Ole Sigmund.

http://www.topopt.mek.dtu.dk/Apps-and-software/A-99-line-topology-optimization-code-written-in-MATLAB

I simply re-implemented it in C/C++ using LibSL-small and Eigen.
And yes, it takes more than 99 lines of C/C++ ;-)

Notations
---------

- dens : optimized densities
- nelx : number of elements along X axis (nelx+1 corners)
- nely : number of elements along Y axis (nely+1 corners)
- dc   : compliance gradient
- KE   : stiffness matrix for a single element

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

#include <LibSL/LibSL.h>
#include <Eigen/Sparse>

#include <iostream>
#include <ctime>
#include <cmath>
#include <set>
#include <limits>

// --------------------------------------------------------------

using namespace std;

// --------------------------------------------------------------

LIBSL_WIN32_FIX;

// --------------------------------------------------------------

// stiffness matrix for a unit square element
Array2D<double> KE;

// Computes the stiffness matrix of a single, unit square element
// (forward declaration, code at the end)
void lk(Array2D<double>& _KE);

// --------------------------------------------------------------

// matrix vector multiply
template <typename T>
void mv_mul(const Array2D<T>& A, const Array<T>& v, Array<T>& _res)
{
  _res.allocate(v.size());
  ForIndex(l, A.ysize()) {
    T al = 0;
    ForIndex(c, A.xsize()) {
      al = al + A.at(c, l) * v[c];
    }
    _res[l] = al;
  }
}

// vector-vector multiply
template <typename T>
T vv_mul(const Array<T>& a, const Array<T>& b)
{
  T res = 0;
  ForIndex(i, a.size()) {
    res = res + a[i] * b[i];
  }
  return res;
}

// --------------------------------------------------------------

// Applies optimality criterion (OC) to update densities (x) from compliance gradient (dc).
// Maintains the volume fraction
void OC(int nelx, int nely, Array2D<double>& _dens, double volfrac, const Array2D<double>& dc)
{
  double move = 0.2;
  Array2D<double> dens_new(_dens.xsize(), _dens.ysize());
  double vtot = 0.0;
  double l1 = 0, l2 = 100000;
  while (l2 - l1 > 1e-3) { // bisection search for volume preservation
    double lmid = 0.5*(l2 + l1);
    vtot = 0.0;
    ForArray2D(dens_new, c, l) {
      // OC term
      double Be = (-dc.at(c, l)) / lmid;
      dens_new.at(c, l) = max(0.02,
        max(_dens.at(c, l) - move,
          min(1.0,
            min(_dens.at(c, l) + move, _dens.at(c, l) * sqrt(Be))
          )
        )
      );
      vtot += dens_new.at(c, l);
    }
    if (vtot - volfrac * nelx * nely > 0.0) {
      l1 = lmid;
    } else {
      l2 = lmid;
    }
  }
  cerr << "Vol frac in result  " << vtot / (nelx* nely) << endl;
  _dens = dens_new;
}

// --------------------------------------------------------------

// Filters the result to prevent the 'checkerboard effect' due to the solver
// attempting to produce infinitely small 'bubbles' (composite).
void filter(int nelx, int nely, double rmin, const Array2D<double>& dens, Array2D<double>& _dc)
{
  Array2D<double> dcn(nelx, nely);
  dcn.fill(0);
  ForArray2D(dcn, i, j) {
    double sum = 0.0;
    ForRange(l, max(j - round(rmin), 0), min(j + round(rmin), nely - 1)) {
      ForRange(k, max(i - round(rmin), 0), min(i + round(rmin), nelx - 1)) {
        double fac = rmin - sqrt((double)(i - k)*(i - k) + (double)(j - l)*(j - l));
        sum += max(0.0f, fac);
        dcn.at(i, j) = dcn.at(i, j) + max(0.0f, fac) * dens.at(k, l) * _dc.at(k, l);
      }
    }
    dcn.at(i, j) = dcn.at(i, j) / (dens.at(i, j)*sum);
  }
  _dc = dcn;
}

// --------------------------------------------------------------

// Straightforward linear finite element solver
// The degrees of freedom (variables) are the corners of the elements, times two (x and y coordinates)
// Each corner at (cx,cy) is associated to two ids:
//   - grid id computed cx + (nelx + 1) * cy
//   - variable id which identifies it in the sparse system of equations
// The x/y coordinates are at respectively id*2+0 and id*2+1
void FE(int nelx, int nely, const Array2D<double>& dens, double penal, Array2D<v2f>& _U)
{
  // variables to lock: there are attachement points of the structure
  set<int> locked;
  // -> here we attach the left third of the bottom row (x in  [0,(nelx + 1) / 3 - 1] , y = nely
  ForIndex(x, (nelx + 1) / 3) {
    locked.insert((x + (nelx + 1) * nely) * 2 + 0); // x
    locked.insert((x + (nelx + 1) * nely) * 2 + 1); // y
  }
  // mapping between variable and ids
  Array<int>   grid2var(2 * (nelx + 1)*(nely + 1));
  Array<int>   var2grid(2 * (nelx + 1)*(nely + 1) - (int)locked.size());
  grid2var.fill(-1);
  var2grid.fill(-1);
  int varid = 0;
  ForIndex(gridid, 2 * (nelx + 1)*(nely + 1)) {
    if (locked.find(gridid) == locked.end()) {
      grid2var[gridid] = varid;
      var2grid[varid]  = gridid;
      varid++;
    }
  }
  sl_assert(varid == 2 * (nelx + 1)*(nely + 1) - (int)locked.size());
  // matrix A (FE equations)
  std::vector<Eigen::Triplet<double> > coefficients;
  ForIndex(x, nelx) {
    ForIndex(y, nely) {
      int p00 = (x + (y)* (nelx + 1));
      int p01 = (x + (y + 1) * (nelx + 1));
      int p11 = ((x + 1) + (y + 1) * (nelx + 1));
      int p10 = ((x + 1) + (y)* (nelx + 1));
      Array<int> corners(8);
      corners[0] = p00 * 2 + 0;
      corners[1] = p00 * 2 + 1;
      corners[2] = p10 * 2 + 0;
      corners[3] = p10 * 2 + 1;
      corners[4] = p11 * 2 + 0;
      corners[5] = p11 * 2 + 1;
      corners[6] = p01 * 2 + 0;
      corners[7] = p01 * 2 + 1;
      double pow_x = pow(dens.at(x, y), penal);
      // add coefficients only for non-locked variables
      ForIndex(k, 8) {
        if (grid2var[corners[k]] > -1) {
          ForIndex(l, 8) {
            if (grid2var[corners[l]] > -1) {
              coefficients.push_back(Eigen::Triplet<double>(grid2var[corners[k]], grid2var[corners[l]],
                KE.at(l, k) * pow_x
                ));
            }
          }
        }
      }
    }
  }
  Eigen::SparseMatrix<double> A(
    2 * (nelx + 1)*(nely + 1) - locked.size(),
    2 * (nelx + 1)*(nely + 1) - locked.size());
  A.setFromTriplets(coefficients.begin(), coefficients.end());
  // vector b, contains external forces
  Eigen::VectorXd b = Eigen::VectorXd(2 * (nelx + 1)*(nely + 1) - locked.size());
  ForIndex(i, b.size()) {
    b[i] = 0.0;
  }
  // -> here we apply external forces at two specific points (near the image top)
  b[grid2var[(nelx / 2     + 24 * (nelx + 1)) * 2 + 1]] = -1;
  b[grid2var[(nelx * 5 / 6 + 16 * (nelx + 1)) * 2 + 1]] = -1;
  //                                         ^^^^^^^ force in y direction
  // solver
  cerr << "solving ...";
  Eigen::SparseLU<Eigen::SparseMatrix<double> > solver(A);
  Eigen::VectorXd result = solver.solve(b);
  cerr << " done.\n";
  // store computed displacement
  _U.allocate(nelx + 1, nely + 1);
  ForIndex(varid, result.size()) {
    int gridid = var2grid[varid];
    int x = (gridid / 2) % (nelx + 1);
    int y = (gridid / 2) / (nelx + 1);
    int c = (gridid & 1);
    _U.at(x, y)[c] = (float)result[varid];
  }
}

// --------------------------------------------------------------

// Executes the global optimization loop until 'convergence' (change below threshold)
// volfrac is the target volume fraction in [0-1]
// penal   is the density penality (typically 3)
// rmin    controls the filter size
void topopt(int nelx, int nely, double volfrac, double penal, double rmin)
{
  ImageFloat1     img(nelx, nely);  // for image output
  Array2D<double> dens(nelx, nely); // optimized density grid
  Array2D<double> dens_old;         // result from previous iteration
  Array2D<double> dc(nelx, nely);   // compliance gradient

  // initialization with a random field of selected volume fraction (volfrac)
  double tot = 0;
  ForArray2D(dens, i, j) {
    dens.at(i, j) = rnd();
    tot += dens.at(i, j);
  }
  tot = tot / (nelx*nely);
  ForArray2D(dens, i, j) {
    dens.at(i, j) = volfrac * dens.at(i, j) / tot;
  }
  int loop = 0;
  double change = 1;
  while (change > 0.01) {

    loop++;
    dens_old = dens;

    // solve for displacement (finite element solution)
    Array2D<v2f> U;
    FE(nelx, nely, dens, penal, /*out*/ U);

    // option to output displacement magnitudes
    if (0) {
      ImageFloat1 img;
      img.pixels().allocate(nelx + 1, nely + 1);
      ForImage((&img), i, j) {
        img.pixel(i, j) = length(U.at(i, j));
      }
      img.remap(0.0f, 255.0f);
      saveImage(sprint("%03d_FE.tga", loop), img.cast<ImageL8>());
    }

    // compute compliance and gradient
    // -> for each element
    double c_comp = 0.0;
    ForIndex(ely, nely) {
      ForIndex(elx, nelx) {
        // get displacement for the element
        Array<double> corners(8);
        corners[0] = U.at(elx, ely)[0];
        corners[1] = U.at(elx, ely)[1];
        corners[2] = U.at(elx + 1, ely)[0];
        corners[3] = U.at(elx + 1, ely)[1];
        corners[4] = U.at(elx + 1, ely + 1)[0];
        corners[5] = U.at(elx + 1, ely + 1)[1];
        corners[6] = U.at(elx, ely + 1)[0];
        corners[7] = U.at(elx, ely + 1)[1];
        // compute compliance
        Array<double> KE_Ue;
        mv_mul(KE, corners, /*out*/KE_Ue);
        v2d Ue_KE_Ue = vv_mul(KE_Ue, corners);
        double compliance = Ue_KE_Ue[0] + Ue_KE_Ue[1];
        double comp       = pow(dens.at(elx, ely), penal) * compliance;
        c_comp += comp;
        // gradient
        double dcomp    = -penal * pow(dens.at(elx, ely), penal - 1.0f) * compliance;
        dc.at(elx, ely) = dcomp;
      }
    }

    // filtering
    filter(nelx, nely, rmin, dens, dc/*out*/);

    // OC method
    OC(nelx, nely, dens/*out*/, volfrac, dc);

#if 0
    // For fun: kills a circle of density (obstacle)
    ForArray2D(dens, i, j) {
      if ( length(v2f((float)i, (float)j) - v2f((float)nelx/2, (float)nely/2)) < nelx/6.0f) {
        dens.at(i, j) = 0.01;
      }
    }
#endif

    // compute max change
    change = 0.0;
    ForArray2D(dens, i, j) {
      change = max(change, abs(dens.at(i, j) - dens_old.at(i, j)));
    }
    // output iteration stats
    cerr << sprint(" Loop %d, compliance = %f, change = %f\n", loop, c_comp, change);
    // output image
    ForImage((&img), i, j) {
      img.pixel(i,j) = 255.0f * (1.0f - (float)pow(dens.at(i,j),penal));
    }
    saveImage(sprint("%03d_struct.tga", loop), img.cast<ImageL8>());

  }

}

// --------------------------------------------------------------

// Program entry point.
int main(int argc, char **argv)
{
  try {

    // generate stiffness matrix for unit element
    lk(KE);

    // go ahead!
    topopt(256, 96, 0.3f, 3.0f, 1.2f);

  } catch (Fatal& e) {
    cerr << Console::red << e.message() << Console::gray << endl;
    return (-1);
  }

  return (0);
}

// --------------------------------------------------------------

// Computes the stiffness matrix of a single, unit square element
void lk(Array2D<double>& _KE)
{
  double E = 1.0f;
  double nu = 0.3f;
  double k[8];
  k[0] = 1.0f / 2.0 - nu / 6.0;
  k[1] = 1.0f / 8.0 + nu / 8.0;
  k[2] = -1.0f / 4.0 - nu / 12.0;
  k[3] = -1.0f / 8.0 + 3.0*nu / 8.0;
  k[4] = -1.0f / 4.0 + nu / 12.0;
  k[5] = -1.0f / 8.0 - nu / 8.0;
  k[6] = nu / 6.0;
  k[7] = 1.0f / 8.0 - 3.0*nu / 8.0;
  _KE.allocate(8, 8);

  _KE.at(0, 0) = E / (1.0 - nu * nu) * k[0];
  _KE.at(1, 0) = E / (1.0 - nu * nu) * k[1];
  _KE.at(2, 0) = E / (1.0 - nu * nu) * k[2];
  _KE.at(3, 0) = E / (1.0 - nu * nu) * k[3];
  _KE.at(4, 0) = E / (1.0 - nu * nu) * k[4];
  _KE.at(5, 0) = E / (1.0 - nu * nu) * k[5];
  _KE.at(6, 0) = E / (1.0 - nu * nu) * k[6];
  _KE.at(7, 0) = E / (1.0 - nu * nu) * k[7];

  _KE.at(0, 1) = E / (1.0 - nu * nu) * k[1];
  _KE.at(1, 1) = E / (1.0 - nu * nu) * k[0];
  _KE.at(2, 1) = E / (1.0 - nu * nu) * k[7];
  _KE.at(3, 1) = E / (1.0 - nu * nu) * k[6];
  _KE.at(4, 1) = E / (1.0 - nu * nu) * k[5];
  _KE.at(5, 1) = E / (1.0 - nu * nu) * k[4];
  _KE.at(6, 1) = E / (1.0 - nu * nu) * k[3];
  _KE.at(7, 1) = E / (1.0 - nu * nu) * k[2];

  _KE.at(0, 2) = E / (1.0 - nu * nu) * k[2];
  _KE.at(1, 2) = E / (1.0 - nu * nu) * k[7];
  _KE.at(2, 2) = E / (1.0 - nu * nu) * k[0];
  _KE.at(3, 2) = E / (1.0 - nu * nu) * k[5];
  _KE.at(4, 2) = E / (1.0 - nu * nu) * k[6];
  _KE.at(5, 2) = E / (1.0 - nu * nu) * k[3];
  _KE.at(6, 2) = E / (1.0 - nu * nu) * k[4];
  _KE.at(7, 2) = E / (1.0 - nu * nu) * k[1];

  _KE.at(0, 3) = E / (1.0 - nu * nu) * k[3];
  _KE.at(1, 3) = E / (1.0 - nu * nu) * k[6];
  _KE.at(2, 3) = E / (1.0 - nu * nu) * k[5];
  _KE.at(3, 3) = E / (1.0 - nu * nu) * k[0];
  _KE.at(4, 3) = E / (1.0 - nu * nu) * k[7];
  _KE.at(5, 3) = E / (1.0 - nu * nu) * k[2];
  _KE.at(6, 3) = E / (1.0 - nu * nu) * k[1];
  _KE.at(7, 3) = E / (1.0 - nu * nu) * k[4];

  _KE.at(0, 4) = E / (1.0 - nu * nu) * k[4];
  _KE.at(1, 4) = E / (1.0 - nu * nu) * k[5];
  _KE.at(2, 4) = E / (1.0 - nu * nu) * k[6];
  _KE.at(3, 4) = E / (1.0 - nu * nu) * k[7];
  _KE.at(4, 4) = E / (1.0 - nu * nu) * k[0];
  _KE.at(5, 4) = E / (1.0 - nu * nu) * k[1];
  _KE.at(6, 4) = E / (1.0 - nu * nu) * k[2];
  _KE.at(7, 4) = E / (1.0 - nu * nu) * k[3];

  _KE.at(0, 5) = E / (1.0 - nu * nu) * k[5];
  _KE.at(1, 5) = E / (1.0 - nu * nu) * k[4];
  _KE.at(2, 5) = E / (1.0 - nu * nu) * k[3];
  _KE.at(3, 5) = E / (1.0 - nu * nu) * k[2];
  _KE.at(4, 5) = E / (1.0 - nu * nu) * k[1];
  _KE.at(5, 5) = E / (1.0 - nu * nu) * k[0];
  _KE.at(6, 5) = E / (1.0 - nu * nu) * k[7];
  _KE.at(7, 5) = E / (1.0 - nu * nu) * k[6];

  _KE.at(0, 6) = E / (1.0 - nu * nu) * k[6];
  _KE.at(1, 6) = E / (1.0 - nu * nu) * k[3];
  _KE.at(2, 6) = E / (1.0 - nu * nu) * k[4];
  _KE.at(3, 6) = E / (1.0 - nu * nu) * k[1];
  _KE.at(4, 6) = E / (1.0 - nu * nu) * k[2];
  _KE.at(5, 6) = E / (1.0 - nu * nu) * k[7];
  _KE.at(6, 6) = E / (1.0 - nu * nu) * k[0];
  _KE.at(7, 6) = E / (1.0 - nu * nu) * k[5];

  _KE.at(0, 7) = E / (1.0 - nu * nu) * k[7];
  _KE.at(1, 7) = E / (1.0 - nu * nu) * k[2];
  _KE.at(2, 7) = E / (1.0 - nu * nu) * k[1];
  _KE.at(3, 7) = E / (1.0 - nu * nu) * k[4];
  _KE.at(4, 7) = E / (1.0 - nu * nu) * k[3];
  _KE.at(5, 7) = E / (1.0 - nu * nu) * k[6];
  _KE.at(6, 7) = E / (1.0 - nu * nu) * k[5];
  _KE.at(7, 7) = E / (1.0 - nu * nu) * k[0];
}

// --------------------------------------------------------------


/* ----------------------------------------------------------------------------
 * Copyright 2020, Jesus Tordesillas Torres, Aerospace Controls Laboratory
 * Massachusetts Institute of Technology
 * All Rights Reserved
 * Authors: Jesus Tordesillas, et al.
 * See LICENSE file for the license information
 * -------------------------------------------------------------------------- */

// Continuous version, and with polytope constraints.

#include "gurobi_c++.h"
#include <sstream>
#include <Eigen/Dense>
#include <type_traits>
using namespace std;

template <typename T>
GRBQuadExpr GetNorm2(const std::vector<T>& x)  // Return the squared norm of a vector
{
  GRBQuadExpr result = 0;
  for (int i = 0; i < x.size(); i++)
  {
    result = result + x[i] * x[i];
  }
  return result;
}

std::vector<GRBLinExpr> MatrixMultiply(const std::vector<std::vector<double>>& A, const std::vector<GRBVar>& x)
{
  std::vector<GRBLinExpr> result;

  for (int i = 0; i < A.size(); i++)
  {
    GRBLinExpr lin_exp = 0;
    for (int m = 0; m < x.size(); m++)
    {
      lin_exp = lin_exp + A[i][m] * x[m];
    }
    result.push_back(lin_exp);
  }
  return result;
}

std::vector<GRBLinExpr> MatrixMultiply(const std::vector<std::vector<double>>& A, const std::vector<GRBLinExpr>& x)
{
  std::vector<GRBLinExpr> result;

  for (int i = 0; i < A.size(); i++)
  {
    GRBLinExpr lin_exp = 0;
    for (int m = 0; m < x.size(); m++)
    {
      lin_exp = lin_exp + A[i][m] * x[m];
    }
    result.push_back(lin_exp);
  }
  return result;
}

template <typename T>  // Overload + to sum Elementwise std::vectors
std::vector<T> operator+(const std::vector<T>& a, const std::vector<T>& b)
{
  assert(a.size() == b.size());

  std::vector<T> result;
  result.reserve(a.size());

  std::transform(a.begin(), a.end(), b.begin(), std::back_inserter(result), std::plus<T>());
  return result;
}

template <typename T>  // Overload - to substract Elementwise std::vectors
std::vector<T> operator-(const std::vector<T>& a, const std::vector<T>& b)
{
  assert(a.size() == b.size());

  std::vector<T> result;
  result.reserve(a.size());

  std::transform(a.begin(), a.end(), b.begin(), std::back_inserter(result), std::minus<T>());
  return result;
}

std::vector<GRBLinExpr> operator-(const std::vector<GRBVar>& x, const std::vector<double>& b)
{
  std::vector<GRBLinExpr> result;
  for (int i = 0; i < x.size(); i++)
  {
    GRBLinExpr tmp = x[i] - b[i];
    result.push_back(tmp);
  }
  return result;
}

template <typename T>
std::vector<T> eigenVector2std(const Eigen::Matrix<T, -1, 1>& x)
{
  std::vector<T> result = 0;
  for (int i = 0; i < x.rows(); i++)
  {
    result.push_back(x(i, 1));
  }
  return result;
}

template <typename T>
std::vector<std::vector<T>> eigenMatrix2std(const Eigen::Matrix<T, -1, -1>& x)
{
  std::vector<std::vector<T>> result;

  for (int i = 0; i < x.rows(); i++)
  {
    std::vector<T> row;
    for (int j = 0; j < x.cols(); j++)
    {
      row.push_back(x(i, j));
    }
    result.push_back(row);
  }
  return result;
}

template <typename T>
std::vector<T> GetColumn(std::vector<std::vector<T>> x, int column)
{
  std::vector<T> result;

  for (int i = 0; i < x.size(); i++)
  {
    result.push_back(x[i][column]);
  }
  return result;
}

GRBLinExpr getPos(int t, double tau, int ii, bool solved, std::vector<std::vector<GRBVar>> x)
{
  if (solved == true)
  {
    GRBLinExpr pos = (x[t][0 + ii].get(GRB_DoubleAttr_X)) * tau * tau * tau +
                     (x[t][3 + ii].get(GRB_DoubleAttr_X)) * tau * tau + (x[t][6 + ii].get(GRB_DoubleAttr_X)) * tau +
                     (x[t][9 + ii].get(GRB_DoubleAttr_X));
    return pos;
  }
  else
  {
    GRBLinExpr pos = x[t][0 + ii] * tau * tau * tau + x[t][3 + ii] * tau * tau + x[t][6 + ii] * tau + x[t][9 + ii];
    return pos;
  }
}

GRBLinExpr getVel(int t, double tau, int ii, bool solved, std::vector<std::vector<GRBVar>> x)
{  // t is the segment, tau is the time inside a specific segment (\in[0,dt], i is the axis)
  if (solved == true)
  {
    GRBLinExpr vel = (3 * x[t][0 + ii].get(GRB_DoubleAttr_X)) * tau * tau +
                     (2 * x[t][3 + ii].get(GRB_DoubleAttr_X)) * tau + (x[t][6 + ii].get(GRB_DoubleAttr_X));
    return vel;
  }
  else
  {
    GRBLinExpr vel = 3 * x[t][0 + ii] * tau * tau + 2 * x[t][3 + ii] * tau + x[t][6 + ii];
    return vel;
  }
}

GRBLinExpr getAccel(int t, double tau, int ii, bool solved, std::vector<std::vector<GRBVar>> x)
{  // t is the segment, tau is the time inside a specific segment(\in[0, dt], i is the axis)
  if (solved == true)
  {
    GRBLinExpr accel = (6 * x[t][0 + ii].get(GRB_DoubleAttr_X)) * tau + 2 * x[t][3 + ii].get(GRB_DoubleAttr_X);
    return accel;
  }
  else
  {
    GRBLinExpr accel = 6 * x[t][0 + ii] * tau + 2 * x[t][3 + ii];
    return accel;
  }
}

GRBLinExpr getJerk(int t, double tau, int ii, bool solved, std::vector<std::vector<GRBVar>> x)
{  // t is the segment, tau is the time inside a specific segment (\in[0,dt], i is the axis)
  if (solved == true)
  {
    GRBLinExpr jerk = 6 * x[t][0 + ii].get(GRB_DoubleAttr_X);  // Note that here tau doesn't appear (makes sense)
    return jerk;
  }
  else
  {
    GRBLinExpr jerk = 6 * x[t][0 + ii];  // Note that here tau doesn't appear (makes sense)
    return jerk;
  }
}

int main(int argc, char* argv[])
{
  GRBEnv* env = 0;
  GRBVar* open = 0;
  GRBVar** transport = 0;
  int transportCt = 0;
  try
  {
    // Model
    env = new GRBEnv();
    GRBModel m = GRBModel(*env);
    m.set(GRB_StringAttr_ModelName, "planning");

    int N = 10;
    double umax = 5;
    double amax = 3;
    double vmax = 5;
    /*    double q = 20000000000;*/
    double dt = 5.0 / N;
    /*    double dt2 = dt * dt / 2.0;
        double dt3 = dt * dt * dt / 6.0;
        std::vector<std::string> states = { "x", "y", "z", "vx", "vy", "vz", "ax", "ay", "az" };
        std::vector<std::string> inputs = { "jx", "jy", "jz" };*/

    std::vector<std::string> coeff = { "ax", "ay", "az", "bx", "by", "bz", "cx", "cy", "cz", "dx", "dy", "dz" };

    std::vector<double> x0 = { 5, 11.5, 0.5, 0, 0, 0, 0, 0, 0 };

    std::vector<double> xf = { 14, 5, 2.5, 0, 0, 0, 0, 0, 0 };

    // std::cout << "here1" << std::endl;

    std::vector<std::vector<GRBVar>> x;
    std::vector<std::vector<GRBVar>> u;

    for (int t = 0; t < N + 1; t++)
    {
      std::vector<GRBVar> row_t;
      for (int i = 0; i < 12; i++)
      {
        row_t.push_back(m.addVar(-GRB_INFINITY, GRB_INFINITY, 0, GRB_CONTINUOUS, coeff[i] + std::to_string(t)));
      }
      x.push_back(row_t);
    }

    // std::cout << "here2" << std::endl;

    /*    for (int i = 0; i < 3; i++)
        {
          std::vector<GRBVar> row_i;
          for (int t = 0; t < N; t++)
          {
            row_i.push_back(m.addVar(-umax, umax, 0, GRB_CONTINUOUS, inputs[i] + std::to_string(t)));
          }
          u.push_back(row_i);
        }*/

    // Constraints x_t+1=Ax_t+Bu_t
    for (int t = 0; t < N - 1; t++)
    {
      for (int i = 0; i < 3; i++)
      {
        m.addConstr(getPos(t, dt, i, false, x) == getPos(t + 1, 0, i, false, x));      // Continuity in position
        m.addConstr(getVel(t, dt, i, false, x) == getVel(t + 1, 0, i, false, x));      // Continuity in velocity
        m.addConstr(getAccel(t, dt, i, false, x) == getAccel(t + 1, 0, i, false, x));  // Continuity in acceleration
      }
    }

    // std::cout << "here3" << std::endl;
    // Constraint x0==x_initial

    for (int i = 0; i < 3; i++)
    {
      m.addConstr(getPos(0, 0, i, false, x) == x0[i]);        // Initial position
      m.addConstr(getVel(0, 0, i, false, x) == x0[i + 3]);    // Initial velocity
      m.addConstr(getAccel(0, 0, i, false, x) == x0[i + 6]);  // Initial acceleration}
    }

    //  std::cout << "here4" << std::endl;

    // Constraint xT==x_final
    for (int i = 0; i < 3; i++)
    {
      m.addConstr(getPos(N - 1, dt, i, false, x) - xf[i] <= 0.2);   // Final position
      m.addConstr(getPos(N - 1, dt, i, false, x) - xf[i] >= -0.2);  // Final position

      m.addConstr(getVel(N - 1, dt, i, false, x) - xf[i + 3] <= 0.2);   // Final velocity
      m.addConstr(getVel(N - 1, dt, i, false, x) - xf[i + 3] >= -0.2);  // Final velocity

      m.addConstr(getAccel(N - 1, dt, i, false, x) - xf[i + 6] <= 0.2);   // Final acceleration
      m.addConstr(getAccel(N - 1, dt, i, false, x) - xf[i + 6] >= -0.2);  // Final acceleration
    }

    // std::cout << "here5" << std::endl;
    // Constraint v<=vmax, a<=amax, u<=umax
    for (int t = 0; t < N - 1; t++)
    {
      for (int i = 0; i < 3; i++)
      {
        m.addConstr(getVel(t, dt, i, false, x) <= vmax);
        m.addConstr(getVel(t, dt, i, false, x) >= -vmax);

        m.addConstr(getAccel(t, dt, i, false, x) <= amax);
        m.addConstr(getAccel(t, dt, i, false, x) >= -amax);

        m.addConstr(getJerk(t, dt, i, false, x) <= umax);
        m.addConstr(getJerk(t, dt, i, false, x) >= -umax);
      }
    }

    //  std::cout << "here6" << std::endl;

    /*    Eigen::Matrix<double, -1, 3> A1;
        Eigen::Matrix<double, -1, 3> A2;
        Eigen::Matrix<double, -1, 3> A3;*/

    Eigen::MatrixXd A1(12, 3);
    Eigen::MatrixXd A2(10, 3);
    Eigen::MatrixXd A3(10, 3);

    Eigen::VectorXd b1(12);
    Eigen::VectorXd b2(10);
    Eigen::VectorXd b3(10);

    A1 << -0.0990887, 0.994031, -0.0456529,  ////////////////////////////////////
        -0.11874, 0.992494, 0.0292636,       ////////////////////////////////////
        0.315838, 0.947835, -0.0430724,      ////////////////////////////////////
        0.279625, 0.956348, 0.0849006,       ////////////////////////////////////
        0.0379941, -0.999235, -0.00925698,   ////////////////////////////////////
        0.031154, -0.999406, 0.0147274,      ////////////////////////////////////
        0, -1, 0,                            ////////////////////////////////////
        -0, 1, -0,                           ////////////////////////////////////
        0.95448, 0, 0.298275,                ////////////////////////////////////
        -0.95448, -0, -0.298275,             ////////////////////////////////////
        0.298275, 0, -0.95448,               ////////////////////////////////////
        -0.298275, -0, 0.95448;              ////////////////////////////////////

    std::cout << "here6.5" << std::endl;

    b1 << 11.2113,
        11.1351,   ////////////////////////////////////
        15.1733,   ////////////////////////////////////
        15.1708,   ////////////////////////////////////
        -9.26336,  ////////////////////////////////////
        -9.27607,  ////////////////////////////////////
        -9.5,      ////////////////////////////////////
        13.5,      ////////////////////////////////////
        14.3031,   ////////////////////////////////////
        -3.92154,  ////////////////////////////////////
        2.01413,   ////////////////////////////////////
        -0.014135;

    A1 << -0.0990887, 0.994031, -0.0456529,  ////////////////////////////////////
        -0.11874, 0.992494, 0.0292636,       ////////////////////////////////////
        0.315838, 0.947835, -0.0430724,      ////////////////////////////////////
        0.279625, 0.956348, 0.0849006,       ////////////////////////////////////
        0.0379941, -0.999235, -0.00925698,   ////////////////////////////////////
        0.031154, -0.999406, 0.0147274,      ////////////////////////////////////
        0, -1, 0,                            ////////////////////////////////////
        -0, 1, -0,                           ////////////////////////////////////
        0.95448, 0, 0.298275,                ////////////////////////////////////
        -0.95448, -0, -0.298275,             ////////////////////////////////////
        0.298275, 0, -0.95448,               ////////////////////////////////////
        -0.298275, -0, 0.95448;              ////////////////////////////////////

    A2 << -0.199658, 0.976518, 0.0809297,  ////////////////////////////////////
        -0.166117, 0.983608, -0.0701482,   ////////////////////////////////////
        -0.358298, -0.933434, 0.0179951,   ////////////////////////////////////
        -0.365568, -0.928824, -0.0603848,  ////////////////////////////////////
        -0.707107, -0.707107, 0,           ////////////////////////////////////
        0.707107, 0.707107, -0,            ////////////////////////////////////
        0.485071, -0.485071, -0.727607,    ////////////////////////////////////
        -0.485071, 0.485071, 0.727607,     ////////////////////////////////////
        -0.514496, 0.514496, -0.685994,    ////////////////////////////////////
        0.514496, -0.514496, 0.685994;     ////////////////////////////////////

    b2 << 9.06362,  ////////////////////////////////////
        9.21024,    ////////////////////////////////////
        -13.5612,   ////////////////////////////////////
        -13.7295,   ////////////////////////////////////
        -15.3241,   ////////////////////////////////////
        19.3241,    ////////////////////////////////////
        1.60634,    ////////////////////////////////////
        2.45521,    ////////////////////////////////////
        -1.82973,   ////////////////////////////////////
        3.82973;    ////////////////////////////////////

    A3 << -0.999958, 0.00342454, 0.00852636,  ////////////////////////////////////
        -0.999832, 0.00363672, -0.0179664,    ////////////////////////////////////
        -0.999778, -0.0204566, 0.00504416,    ////////////////////////////////////
        -0.999564, -0.0227306, -0.0188383,    ////////////////////////////////////
        -1, -0, 0,                            ////////////////////////////////////
        1, 0, -0,                             ////////////////////////////////////
        0, -0.98387, 0.178885,                ////////////////////////////////////
        -0, 0.98387, -0.178885,               ////////////////////////////////////
        0, -0.178885, -0.98387,               ////////////////////////////////////
        -0, 0.178885, 0.98387;

    b3 << -12.7365,  ////////////////////////////////////
        -12.7834,    ////////////////////////////////////
        -12.9236,    ////////////////////////////////////
        -12.9824,    ////////////////////////////////////
        -12,         ////////////////////////////////////
        16,          ////////////////////////////////////
        -3.47214,    ////////////////////////////////////
        11.0623,     ////////////////////////////////////
        -2.3541,     ////////////////////////////////////
        4.3541;
    std::cout << "here6.7" << std::endl;

    std::vector<std::vector<double>> A1std = eigenMatrix2std(A1);
    std::vector<std::vector<double>> A2std = eigenMatrix2std(A2);
    std::vector<std::vector<double>> A3std = eigenMatrix2std(A3);

    /*    std::vector<std::vector<double>> b1std = eigenMatrix2std(b1);
        std::vector<std::vector<double>> b2std = eigenMatrix2std(b2);
        std::vector<std::vector<double>> b3std = eigenMatrix2std(b3);*/

    // m.update();

    std::cout << "here7" << std::endl;
    std::vector<std::vector<GRBVar>> b;
    for (int t = 0; t < N + 1; t++)
    {
      std::vector<GRBVar> row;
      for (int i = 0; i < 3; i++)  // For the three polytopes
      {
        GRBVar variable =
            m.addVar(-GRB_INFINITY, GRB_INFINITY, 0, GRB_BINARY, "s" + std::to_string(i) + "_" + std::to_string(t));
        row.push_back(variable);
      }
      b.push_back(row);
    }

    std::cout << "here8" << std::endl;

    // If is 1 --> in that polytope

    for (int t = 0; t < N; t++)
    {
      GRBLinExpr sum = 0;
      for (int col = 0; col < b[0].size(); col++)
      {
        sum = sum + b[t][col];
      }
      m.addConstr(sum == 1);

      std::vector<GRBLinExpr> pos = { getPos(t, 0, 0, false, x), getPos(t, 0, 1, false, x), getPos(t, 0, 2, false, x) };

      for (int i = 0; i < b1.rows(); i++)
      {
        m.addGenConstrIndicator(b[t][0], 1, MatrixMultiply(A1std, pos)[i], '<',
                                b1[i]);  // If b[t,0]==1, then...
      }
      for (int i = 0; i < b2.rows(); i++)
      {
        m.addGenConstrIndicator(b[t][1], 1, MatrixMultiply(A2std, pos)[i], '<',
                                b2[i]);  // If b[t,1]==1, then...
      }
      for (int i = 0; i < b3.rows(); i++)
      {
        m.addGenConstrIndicator(b[t][2], 1, MatrixMultiply(A3std, pos)[i], '<',
                                b3[i]);  // If b[t,2]==1, then...
      }
    }
    std::cout << "here9" << std::endl;

    GRBQuadExpr control_cost = 0;
    for (int t = 0; t < N; t++)
    {
      std::vector<GRBLinExpr> ut = { getJerk(t, 0, 0, false, x), getJerk(t, 0, 1, false, x),
                                     getJerk(t, 0, 2, false, x) };
      control_cost = control_cost + GetNorm2(ut);
    }

    std::cout << "here10" << std::endl;

    /*    GRBQuadExpr final_state_cost = 0;
        std::vector<GRBVar> xFinal = GetColumn(x, N);
        // std::vector<GRBLinExpr> prueba = xFinal - xf;
        final_state_cost = GetNorm2(xFinal - xf);
        final_state_cost = q * final_state_cost;*/

    m.setObjective(control_cost, GRB_MINIMIZE);

    // Solve*/
    m.update();
    std::cout << "here11" << std::endl;
    m.write("debug.lp");
    m.optimize();
    std::cout << "here12" << std::endl;

    std::cout << "\nOBJECTIVE: " << m.get(GRB_DoubleAttr_ObjVal) << std::endl;
    std::cout << "Positions X:" << std::endl;
    for (int t = 0; t < N + 1; t++)
    {
      std::cout << getPos(t, 0, 0, true, x) << std::endl;
    }
  }

  catch (GRBException e)
  {
    cout << "Error code = " << e.getErrorCode() << endl;
    cout << e.getMessage() << endl;
  }
  /*catch (...)
  {
    cout << "Exception during optimization" << endl;
  }*/

  delete env;
  return 0;
}


#include <iostream>
#include <vector>
#include <tuple>
#include <algorithm>

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/cycle_canceling.hpp>
#include <boost/graph/push_relabel_max_flow.hpp>
#include <boost/graph/successive_shortest_path_nonnegative_weights.hpp>
#include <boost/graph/find_flow_cost.hpp>

// Graph Type with nested interior edge properties for Cost Flow Algorithms
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,
                boost::property <boost::edge_weight_t, long> > > > > graph;
typedef boost::graph_traits<graph>::edge_descriptor             edge_desc;
typedef boost::graph_traits<graph>::out_edge_iterator           out_edge_it;

// Custom edge adder class
class edge_adder {
 graph &G;

 public:
  explicit edge_adder(graph &G) : G(G) {}
  void add_edge(int from, int to, long capacity, long cost) {
    //std::cout << from << " -> " << to << ": capacity = " << capacity << ", cost = " << cost << std::endl;
    auto c_map = boost::get(boost::edge_capacity, G);
    auto r_map = boost::get(boost::edge_reverse, G);
    auto w_map = boost::get(boost::edge_weight, G); // new!
    const edge_desc e = boost::add_edge(from, to, G).first;
    const edge_desc 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;
    w_map[e] = cost;   // new assign cost
    w_map[rev_e] = -cost;   // new negative cost
  }
};

using namespace std;

struct elephant {
  int x, y, cost, capacity;
};

// Compute max flow min cost given a limit for the flow
tuple<int, int> flowAndCost(int n, int start, int end, vector<elephant> &elephants, int flowLimit, int budget) {
  graph G(n);
  edge_adder adder(G);
  auto c_map = boost::get(boost::edge_capacity, G);
  auto rc_map = boost::get(boost::edge_residual_capacity, G);

  for (auto e : elephants) {
    adder.add_edge(e.x, e.y, e.capacity, e.cost);
  }

  int source = boost::add_vertex(G);
  int target = boost::add_vertex(G);
  adder.add_edge(source, start, flowLimit, 0);
  adder.add_edge(end, target, flowLimit, 0);

  boost::successive_shortest_path_nonnegative_weights(G, source, target);
  int flow = 0;
  out_edge_it e, eend;
  for(boost::tie(e, eend) = boost::out_edges(boost::vertex(source,G), G); e != eend; ++e) {
      flow += c_map[*e] - rc_map[*e];
  }
  int cost = boost::find_flow_cost(G);
  return {flow, cost};
}

// Check if it is feasible to transport numSuitcases suitcases
bool isFeasible(int n, int start, int end, vector<elephant> &elephants, int numSuitcases, int budget) {
  int flow, cost;
  tie(flow, cost) = flowAndCost(n, start, end, elephants, numSuitcases, budget);

  if (flow < numSuitcases) {
    return false;
  }
  return cost <= budget;
}

// Strategy:
// - Binary search to find largest feasible number
void solve() {
  int n, m, budget, start, end;
  cin >> n >> m >> budget >> start >> end;

  int x, y, cost, capacity;
  vector<elephant> elephants;
  elephants.reserve(m);
  for (int i = 0; i < m; ++i) {
    cin >> x >> y >> cost >> capacity;
    elephants.push_back({x, y, cost, capacity});
  }

  // Find an upper bound (what is the max flow?)
  int maxFlow = get<0>(flowAndCost(n, start, end, elephants, numeric_limits<int>::max(), budget));

  // Binary search for highest feasible number of elephants
  int a = 0;
  int b = maxFlow;
  while (a != b) {
    int m = a + (b - a + 1) / 2;

    if (isFeasible(n, start, end, elephants, m, budget)) {
      a = m;
    }
    else {
      b = m - 1;
    }
  }

  cout << a << endl;
}

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


#ifndef MATHTOOLBOX_CLASSICAL_MDS_HPP
#define MATHTOOLBOX_CLASSICAL_MDS_HPP

#include <Eigen/Core>
#include <Eigen/Eigenvalues>

namespace mathtoolbox
{
    /// \brief Compute low-dimensional embedding by using classical multi-dimensional scaling (MDS)
    ///
    /// \param D Distance (dissimilarity) matrix
    ///
    /// \param target_dim Target dimensionality
    ///
    /// \return Coordinate matrix whose i-th column corresponds to the embedded coordinates of the i-th entry
    Eigen::MatrixXd ComputeClassicalMds(const Eigen::MatrixXd& D, const unsigned taregt_dim);
} // namespace mathtoolbox

#endif // MATHTOOLBOX_CLASSICAL_MDS_HPP


//####### Test module for utilites ####################################

//Define Module name
 #define BOOST_TEST_MODULE "utilites"

//Will automatically define a main for this test
 #define BOOST_TEST_DYN_LINK

 //Include Boost unit tests library & library for floating point comparison
 #include <boost/test/unit_test.hpp>
 #include <boost/test/floating_point_comparison.hpp>

//Units choice. Not relevant here, but avoids compile-time warning
#define PXRMP_USE_SI_UNITS

#include "utilities.hpp"

using namespace picsar::multi_physics;

// ------------- Tests --------------

// ------------- Tests --------------

//Tolerance for double precision calculations
const double double_tolerance = 1.0e-10;

//Tolerance for single precision calculations
const float float_tolerance = 1.0e-4;

//Templated tolerance
template <typename T>
T tolerance()
{
    if(std::is_same<T,float>::value)
        return float_tolerance;
    else
        return double_tolerance;
}

//Test generate_lin_spaced_vec generic
template<typename _WHATEVER>
void test_generate_lin_spaced_vec(_WHATEVER min, _WHATEVER max, size_t size)
{
    std::vector<_WHATEVER> vv =
        generate_lin_spaced_vec<_WHATEVER>(min, max, size);

    BOOST_CHECK_EQUAL(vv.size(), size);
    BOOST_CHECK_EQUAL(vv.front(), min);
    BOOST_CHECK_EQUAL(vv.back(), max);

    _WHATEVER expdiff =(max-min)/(size-1) ;

    for(size_t i = 1; i < vv.size(); ++i){
        _WHATEVER diff = vv[i]-vv[i-1];
        BOOST_CHECK_SMALL((diff-expdiff)/expdiff, tolerance<_WHATEVER>());
    }
}

//Test generate_lin_spaced_vec generic with double precision
BOOST_AUTO_TEST_CASE( test_generate_lin_spaced_vec_double_1 )
{
    test_generate_lin_spaced_vec<double>(-73, 112, 500);
}

//Test generate_lin_spaced_vec generic with single precision
BOOST_AUTO_TEST_CASE( test_generate_lin_spaced_vec_single_1 )
{
    test_generate_lin_spaced_vec<float>(-73, 112, 500);
}

//Test generate_log_spaced_vec generic
template<typename _WHATEVER>
void test_generate_log_spaced_vec(_WHATEVER min, _WHATEVER max, size_t size)
{
    std::vector<_WHATEVER> vv =
        generate_log_spaced_vec<_WHATEVER>(min, max, size);

    BOOST_CHECK_EQUAL(vv.size(), size);
    BOOST_CHECK_EQUAL(vv.front(), min);
    BOOST_CHECK_EQUAL(vv.back(), max);

    _WHATEVER mul = pow(max/min, static_cast<_WHATEVER>(1.0/(size-1)));

    for(size_t i = 1; i < vv.size(); ++i){
        _WHATEVER ratio = vv[i]/vv[i-1];
        BOOST_CHECK_SMALL((ratio-mul)/mul, tolerance<_WHATEVER>());
    }
}

//Test generate_log_spaced_vec generic with double precision
BOOST_AUTO_TEST_CASE( test_generate_log_spaced_vec_double_1 )
{
    test_generate_log_spaced_vec<double>(0.01, 100.0, 200);
}

//Test generate_log_spaced_vec generic with single precision
BOOST_AUTO_TEST_CASE( test_generate_log_spaced_vec_single_1 )
{
    test_generate_log_spaced_vec<float>(0.01, 100.0, 200);
}


#include <functional>
#include <iomanip>
#include <iostream>
#include <string>
#include <tuple>
#include <utility/include_all.h>
#include <utility/iterator.h>
#include <utility/math.h>
#include <utility/unit_math.h>
#include <vector>

auto generateHexGrid(float h, float r, bool center = true, float scale = 1.0f) {
	float H = h * kernelSize();
	auto gen_position = [&](auto r, int32_t i, int32_t j, int32_t k) {
		float4 initial{ 2.0f * i + ((j + k) % 2), sqrt(3.f) * (j + 1.0f / 3.0f * (k % 2)), 2.0f * sqrt(6.0f) / 3.0f * k, h / r };
		return initial * r;
	};
	int32_t requiredSlices_x = (int32_t)math::ceilf(scale * H / r);
	int32_t requiredSlices_y = (int32_t)math::ceilf(scale * H / (sqrt(3.0f) * r));
	int32_t requiredSlices_z = (int32_t)math::ceilf(scale * H / r * 3.0f / (sqrt(6.0f) * 2.0f));

	std::vector<float4> positions;
	for (int32_t x_it = -requiredSlices_x; x_it <= requiredSlices_x; x_it++)
		for (int32_t y_it = -requiredSlices_y; y_it <= requiredSlices_y; y_it++)
			for (int32_t z_it = -requiredSlices_z; z_it <= requiredSlices_z; z_it++)
				if (center || (!center && (x_it != 0 || y_it != 0 || z_it != 0)))
					positions.push_back(gen_position(r, x_it, y_it, z_it));
	return positions;
}
#define CALC_CONSTANTS
#ifdef CALC_CONSTANTS
constexpr auto volume = 1.f;
auto radius = powf(volume, 1.f / 3.f) * PI4O3_1;
auto h = support_from_volume(volume);
auto H = h * kernelSize();
auto getPacking() {
	int32_t it = 0;
	auto spacing = math::brentsMethod(
		[&](auto r) {
		auto positions = generateHexGrid(h, r, true, 1.0f);
		auto positionsL = generateHexGrid(h, r, true, 2.0f);
		float error = 0.0f;
		for (const auto& pos : positions) {
			float density = -1.0f;
			for (const auto& posL : positionsL)
				density += volume * spline4_kernel(posL, pos);
			error += density;
		}
		std::cout << r << "[" << it++ << "] -> " << error << std::endl;
		return error;
	},
		radius * 0.75f, radius * 8.0f, 1e-5f, 100);
	return spacing;
}
auto spacing = getPacking();
#else
constexpr auto H = 0x1.2487b0p+1f;
constexpr auto h = 0x1.407358p+0f;
constexpr auto r = 0x1.e8ec8ap-3f;
constexpr auto V = 0x1.000000p+0f;
constexpr auto s = 0x1.1ece3cp-1f;
#endif

constexpr auto lutSize = 1024;
constexpr auto integralSize = 16*1024;

template<typename C>
auto generateLUT(C&& func) {
	float4 c{ 0.f,0.f,0.f,h };
	using res_t = double;// decltype(func(c, std::declval<float4>()));
	std::array<res_t, lutSize> LUT;
	float dd = 2.f * H / ((float)lutSize - 1);
	for (auto di = 0; di < lutSize; ++di) {
		auto n = integralSize;
		double dh = H / ((double)n);
		double d = H - dd * (double)(di);

		res_t integral = vector_t<double, math::dimension_v<res_t>>::zero();
#pragma omp parallel for reduction(+ : integral)
		for (auto ni = 0; ni < n; ++ni) {
			double xl = dh * (double)ni;
			double xh = dh * (double)ni + dh;

			float4 p{ (float) xl + 0.5f * (float)dh, 0.f, 0.f, h };

			double hl = math::clamp(xl - d, 0.f, 2.f * xl);
			double hh = math::clamp(xh - d, 0.f, 2.f * xh);

			double Vl = CUDART_PI_F * hl * hl / 3.f * (3.f * xl - hl);
			double Vh = CUDART_PI_F * hh * hh / 3.f * (3.f * xh - hh);

			double dV = Vh - Vl;

			integral += dV * math::castTo<typename vector_t<double, math::dimension_v<res_t>>::type>(func(c, p, (p.x - d) / H));
		}
		LUT[di] = integral;
	}
	std::reverse(LUT.begin(), LUT.end());
	return LUT;
}

constexpr float t = 0.0001f;
template<typename C>
auto gradientLUT(C&& func) {
	float4 c{ 0.f,0.f,0.f,h };
	using res_t = double;// decltype(func(c, std::declval<float4>()));
	std::array<res_t, lutSize> LUT;
	double dd = 2.f * H / ((double)lutSize - 1);
	for (auto di = 0; di < lutSize; ++di) {
		constexpr auto n = integralSize;
		double dh = H / ((double)n);
		double d = H - dd * (double)(di);

		res_t integralp = vector_t<double, math::dimension_v<res_t>>::zero();
#pragma omp parallel for reduction(+ : integralp)
		for (auto ni = 0; ni < n; ++ni) {
			double xl = dh * (double)ni;
			double xh = dh * (double)ni + dh;

			float4 p{ (float) xl + 0.5f * (float) dh, 0.f, 0.f, h };

			double hl = math::clamp(xl - d + t, 0., 2. * xl);
			double hh = math::clamp(xh - d + t, 0., 2. * xh);

			double Vl = CUDART_PI * hl * hl / 3. * (3. * xl - hl);
			double Vh = CUDART_PI * hh * hh / 3. * (3. * xh - hh);

			double dV = Vh - Vl;

			integralp += dV * math::castTo<typename vector_t<double, math::dimension_v<res_t>>::type>(func(c, p, (p.x - d - t) / H));
		}
		res_t integraln = vector_t<double, math::dimension_v<res_t>>::zero();
#pragma omp parallel for reduction(+ : integraln)
		for (auto ni = 0; ni < n; ++ni) {
			double xl = dh * (double)ni;
			double xh = dh * (double)ni + dh;

			float4 p{ (float)xl + 0.5f * (float)dh , 0.f, 0.f, h };

			double hl = math::clamp(xl - d - t, 0., 2. * xl);
			double hh = math::clamp(xh - d - t, 0., 2. * xh);

			double Vl = CUDART_PI_F * hl * hl / 3. * (3. * xl - hl);
			double Vh = CUDART_PI_F * hh * hh / 3. * (3. * xh - hh);

			double dV = Vh - Vl;

			integraln += dV * math::castTo<typename vector_t<double, math::dimension_v<res_t>>::type>(func(c, p, (p.x - d + t) / H));
		}
		LUT[di] = (integralp - integraln) / (2.0 * t);
	}
	std::reverse(LUT.begin(), LUT.end());
	return LUT;
}
template<typename T>
auto lookup (const std::array<T, lutSize> LUT, float x) {
	auto xRel = ((x + H) / (2.f * H)) * ((float)lutSize - 1.f);
	auto xL = math::floorf(xRel);
	auto xH = math::ceilf(xRel);
	auto xD = xRel - xL;
	int32_t xLi = math::clamp(static_cast<int32_t>(xL), 0, lutSize - 1);
	int32_t xHi = math::clamp(static_cast<int32_t>(xH), 0, lutSize - 1);
	auto lL = LUT[xLi];
	auto lH = LUT[xHi];
	return lL * xD + (1.f - xD) * lH;
};

#include <config/config.h>
#include <fstream>
#ifdef _WIN32
#include <experimental/filesystem>
namespace fs = std::experimental::filesystem;
#else
#include <boost/filesystem.hpp>
namespace fs = boost::filesystem;
#endif

auto writeLUT(const std::string& name, const std::string& type, const std::array<double, lutSize>& LUT) {
	fs::path bin_dir(sourceDirectory);
	auto file = bin_dir / "cfg" / name;
	file.replace_extension("lut");

	if (fs::exists(file)) {
		if (fs::exists(__FILE__)) {
			auto input_ts = fs::last_write_time(__FILE__);
			auto output_ts = fs::last_write_time(file);
			if (input_ts <= output_ts) {
				return;
			}
		}
	}
	std::cout << "Writing " << file.string() << std::endl;

	std::ofstream output(file.string());
	//output << "std::vector<" << type << "> " << name << "{ ";
	int32_t ctr = 0;
	for (auto v : LUT) {
		output << std::scientific << std::setprecision(std::numeric_limits<float>::digits10 + 1)
			<< static_cast<float>(v) << " ";
	}
	//output << "};" << std::endl;
	output.close();
}

auto sphericalGradientIntegral(const std::function<double(float4, float4, float)>& func, int32_t phiSlices, int32_t thetaSlices, int32_t radiusSteps) {
	float4 c{ 0.f,0.f,0.f,h };
	using res_t = double;
	std::array<res_t, lutSize> LUT;
	double dd = 2.f * H / ((double)lutSize - 1);
	for (auto di = 0; di < lutSize; ++di) {
		auto n = radiusSteps;
		double dh = H / ((double)n);
		double d = H - dd * (double)(di);
		double dTheta = (2.0 * CUDART_PI) / (double)thetaSlices;
		double dPhi = (CUDART_PI) / (double)phiSlices;

		res_t integralp = vector_t<double, math::dimension_v<res_t>>::zero();
#pragma omp parallel for
		for (auto iR = 0; iR < radiusSteps; ++iR) {
			double xl = dh * (double)iR;
			double xh = dh * (double)iR + dh;
			float r = (float) xl + 0.5f * (float)dh;
			double Vl = 4.0 / 3.0 * CUDART_PI * xl * xl * xl;
			double Vh = 4.0 / 3.0 * CUDART_PI * xh * xh * xh;

			double dV = (Vh - Vl) / (double)thetaSlices / (double)phiSlices;
			res_t thetaSum = vector_t<double, math::dimension_v<res_t>>::zero();
			for (auto iTheta = 0.0; iTheta < 2.0 * CUDART_PI; iTheta += dTheta) {
				res_t phiSum = vector_t<double, math::dimension_v<res_t>>::zero();
				for (auto iPhi = 0.0; iPhi < CUDART_PI; iPhi += dPhi) {
					double theta = iTheta + dTheta * 0.5;
					double phi = iPhi + dPhi * 0.5;
					double x = r * cos(theta) * sin(phi);
					double y = r * sin(theta) * sin(phi);
					double z = r * cos(phi);
					if (x < d + t)
						continue;
					float4 p{ (float)x, (float)y, (float)z, h };
					phiSum += dV * math::castTo<typename vector_t<double, math::dimension_v<res_t>>::type>(func(c, p, d + t));
				}
				thetaSum += phiSum;
			}
			integralp += thetaSum;
		}
		res_t integraln = vector_t<double, math::dimension_v<res_t>>::zero();
#pragma omp parallel for
		for (auto iR = 0; iR < radiusSteps; ++iR) {
			double xl = dh * (double)iR;
			double xh = dh * (double)iR + dh;
			float r = (float)xl + 0.5f * (float)dh;
			double Vl = 4.0 / 3.0 * CUDART_PI * xl * xl * xl;
			double Vh = 4.0 / 3.0 * CUDART_PI * xh * xh * xh;

			double dV = (Vh - Vl) / (double)thetaSlices / (double)phiSlices;
			res_t thetaSum = vector_t<double, math::dimension_v<res_t>>::zero();
			for (auto iTheta = 0.0; iTheta < 2.0 * CUDART_PI; iTheta += dTheta) {
				res_t phiSum = vector_t<double, math::dimension_v<res_t>>::zero();
				for (auto iPhi = 0.0; iPhi < CUDART_PI; iPhi += dPhi) {
					double theta = iTheta + dTheta * 0.5;
					double phi = iPhi + dPhi * 0.5;
					double x = r * cos(theta) * sin(phi);
					double y = r * sin(theta) * sin(phi);
					double z = r * cos(phi);
					if (x < d - t)
						continue;
					float4 p{ (float)x, (float)y, (float)z, h };
					phiSum += dV * math::castTo<typename vector_t<double, math::dimension_v<res_t>>::type>(func(c, p, d - t));
				}
				thetaSum += phiSum;
			}
			integraln += thetaSum;
		}
		LUT[di] = (integralp - integraln) / (2.0 * t);
	}
	std::reverse(LUT.begin(), LUT.end());
	return LUT;
}

void progressBar(int32_t frame, int32_t frameTarget, float progress) {
	std::ios cout_state(nullptr);
	cout_state.copyfmt(std::cout);
	static auto startOverall = std::chrono::high_resolution_clock::now();
	static auto startFrame = startOverall;
	static auto lastTime = startOverall;
	static int32_t lastFrame = frame;
	//if (frame != lastFrame) {
	//	lastFrame = frame;
	if(frame == 0)
		startFrame = std::chrono::high_resolution_clock::now();
	//}
	auto now = std::chrono::high_resolution_clock::now();
	lastTime = now;
	int barWidth = 128;
	std::cout << "Generating " << std::setw(4) << frame;
	if (frameTarget != -1)
		std::cout << "/" << std::setw(4) << frameTarget;
	std::cout << " [";
	int pos = barWidth * progress;
	for (int i = 0; i < barWidth; ++i) {
		if (i < pos) std::cout << "=";
		else if (i == pos) std::cout << ">";
		else std::cout << " ";
	}
	std::cout << "] " << std::setw(3) << int(progress * 100.0) << " ";
	auto dur = std::chrono::duration_cast<std::chrono::milliseconds>(now - startFrame);
	if (dur.count() < 100 || progress < 1e-3f) {
		std::cout << " ---/---s  ";
	}
	else {
		auto totalTime = ((float)std::chrono::duration_cast<std::chrono::microseconds>(now - startFrame).count()) / 1000.f / 1000.f;
		std::cout << std::fixed << std::setprecision(0) << " " << std::setw(3) << totalTime << "/" << std::setw(3) << (totalTime / progress) << "s  ";
	}
	std::cout << "\r";
	std::cout.flush();
	std::cout.copyfmt(cout_state);
}

auto sphericalIntegral(const std::function<double(float4, float4, float)>& func, int32_t phiSlices, int32_t thetaSlices, int32_t radiusSteps) {
	float4 c{ 0.f,0.f,0.f,h };
	using res_t = double;
	std::array<res_t, lutSize> LUT;
	double dd = 2.f * H / ((double)lutSize - 1);
	//std::vector<double> thetaSum(thetaSlices);
	//std::vector<double> phiSum(phiSlices);
	for (auto di = 0; di < lutSize; ++di) {
		progressBar(di, lutSize, (double)di / (double)lutSize);
		auto n = radiusSteps;
		double dh = H / ((double)n);
		double d = -H + dd * (double)(di);
		double dTheta = (2.0 * CUDART_PI) / (double)thetaSlices;
		double dPhi = (CUDART_PI) / (double)phiSlices;

		res_t integralp = vector_t<double, math::dimension_v<res_t>>::zero();
		double dVSum = 0.0;
#pragma omp parallel for
		for (auto iR = 0; iR < radiusSteps; ++iR) {
			double xl = dh * (double)iR;
			double xh = dh * (double)iR + dh;
			float r = (float)xl + 0.5f * (float)dh;
			double Vl = 4.0 / 3.0 * CUDART_PI * xl * xl * xl;
			double Vh = 4.0 / 3.0 * CUDART_PI * xh * xh * xh;

			double dV = (Vh - Vl) / (double)thetaSlices / (double)phiSlices;
			res_t thetaSum = vector_t<double, math::dimension_v<res_t>>::zero();
			for (auto iTheta = 0; iTheta < thetaSlices; iTheta++) {
				res_t phiSum = vector_t<double, math::dimension_v<res_t>>::zero();
				for (auto iPhi = 0; iPhi < phiSlices; iPhi++) {
					double theta = ((double)iTheta) * dTheta;// +dTheta * 0.5;
					double phi = ((double)iPhi) * dPhi;// +dPhi * 0.5;
					double x = r * cos(theta) * sin(phi);
					double y = r * sin(theta) * sin(phi);
					double z = r * cos(phi);
					if (x < d)
						phiSum += 0.0;
					else {
						float4 p{ (float)x, (float)y, (float)z, h };
						phiSum += math::castTo<typename vector_t<double, math::dimension_v<res_t>>::type>(func(c, p, (x - d) / (1.0 * H)));
					}
				}
				//std::sort(phiSum.begin(), phiSum.end());
				thetaSum += phiSum;
			}
			//std::sort(thetaSum.begin(), thetaSum.end());
			//integralp += dV * std::reduce(thetaSum.begin(), thetaSum.end());
			integralp += dV * thetaSum;
		}
		LUT[di] = integralp;
		//break;
	}
	std::cout << std::endl;
	//std::reverse(LUT.begin(), LUT.end());
	return LUT;
}

auto approximateGradient(const std::array<double, lutSize>& LUT, const std::array<double, lutSize>& vLUT) {
	std::array<double, lutSize> gLUT;
	for (auto& e : gLUT) e = 0.0;
	for (int32_t i = 0; i < lutSize - 1; ++i) {
		auto vi1 = 1.f; //vLUT[i + 1];
		auto vi = 1.f; //vLUT[i];
		vi1 = vi1 < 1e-5f ? vi : vi1;

		gLUT[i] = (LUT[i + 1] / vi1 - LUT[i] / vi) / (2.0 * H / (double)lutSize) * vi;
	}
	gLUT[lutSize - 1] = gLUT[lutSize - 2];
	return gLUT;
}

auto smoothLUT(const std::array<double, lutSize>& LUT) {
	std::array<double, lutSize> gLUT;
	gLUT = LUT;
	for (int32_t i = 1; i < lutSize - 1; ++i) {
		if(LUT[i-1] < LUT[i])
			gLUT[i] = (LUT[i - 1] + LUT[i + 1])*0.5;
	}
	return gLUT;
}

#include <omp.h>
int main(int32_t argc, char** argv) {
	omp_set_num_threads(12);
#ifdef CALC_CONSTANTS
	std::ios cout_state(nullptr);
	cout_state.copyfmt(std::cout);
	std::cout << std::hexfloat << "H = " << H << std::endl;
	std::cout << std::hexfloat << "h = " << h << std::endl;
	std::cout << std::hexfloat << "radius = " << radius << std::endl;
	std::cout << std::hexfloat << "volume = " << volume << std::endl;
	std::cout << std::hexfloat << "spacing = " << spacing << std::endl;
	std::cout.copyfmt(cout_state);
	std::cout << "H = " << H << std::endl;
	std::cout << "h = " << h << std::endl;
	std::cout << "radius = " << radius << std::endl;
	std::cout << "volume = " << volume << std::endl;
	std::cout << "spacing = " << spacing << std::endl;
	std::cout.copyfmt(cout_state);
#endif
	std::cout << "Running LUT generation code." << std::endl;
//#ifdef WIN32
//	HWND console = GetConsoleWindow();
//	RECT _r;
//	GetWindowRect(console, &_r);
//	MoveWindow(console, 0, 0, 1920, 1200, TRUE);
//#endif
	//std::cout << "H = " << H << std::endl;
	//std::cout << "h = " << h << std::endl;
	//std::cout << "radius = " << r << std::endl;
	//std::cout << "volume = " << V << std::endl;
	//std::cout << "spacing = " << s << std::endl;

	//std::cout << "Spherical Integral: " <<  << std::endl;
	//std::cout << "Analytical: " << CUDART_PI * 4.0 / 3.0 * H * H * H << std::endl;

	//std::array densityLUT = sphericalIntegral([](float4 c, float4 p, float d) {return kernel(c, p); }, 128, 128, 128);
	//std::cout << "Generated density LUT" << std::endl;
	float dFactor = 0.f;// 0.f;// 1.5f;
	std::array densityLUT = generateLUT([&](float4 c, float4 p, float d) { return (1.f + dFactor * d) * kernel(c, p); });
	writeLUT("density", "float", densityLUT);

	auto lookup = [&](auto x) {
		float xRel = ((x + 1.f) / 2.f)* ((float)lutSize - 1.f);
		auto xL = math::floorf(xRel);
		auto xH = math::ceilf(xRel);
		auto xD = xRel - xL;
		int32_t xLi = math::clamp(static_cast<int32_t>(xL), 0, lutSize - 1);
		int32_t xHi = math::clamp(static_cast<int32_t>(xH), 0, lutSize - 1);
		auto lL = densityLUT[xLi];
		auto lH = densityLUT[xHi];
		auto val = lL * xD + (1.f - xD) * lH;
		return val;
	};

	auto findX = [&](auto x) {
		float f = -1.f;
		int32_t n = 8;
		for (int32_t n = 1; n < 11; ++n) {
			auto fx = lookup(f);
			//std::cout << "Starting at " << f << " : " << fx << " with dx = " << powf(0.5f, (float)n) << std::endl;
			while (n % 2 == 1 ? fx > x + 0.001f : fx < x - 0.001f){
				f += (n % 2 == 1 ? 1.f : -1.f) * powf(0.5f, (float)n);
				fx = lookup(f);
				//std::cout << f << " -> " << fx << std::endl;
			}
		}
		//std::cout << f << " - " << lookup(f) << " <-> " << x << std::endl;
		return f;
	};
	std::array<double, lutSize> offsetLUT;
	for (int32_t i = 0; i < lutSize; ++i) {
		float f = (float) i / (float)lutSize;
		offsetLUT[i] = /*0.24509788f*/ -0.f* findX(f);
	}
	//std::reverse(offsetLUT.begin(), offsetLUT.end());
	writeLUT("offsetLUT", "float", offsetLUT);
	//findX(0.5f);

	std::array spline2LUT = generateLUT([&](float4 c, float4 p, float d) { return (1.f + dFactor * d) * (1.f + dFactor * d) *  math::dot3(gradient(c, p), gradient(c, p)); });
	writeLUT("spline2", "float", spline2LUT);
	std::array spikyLUT = generateLUT([&](float4 c, float4 p, float d) { return (1.f + dFactor * d) * PressureKernel<kernel_kind::spline4>::value(c,p); });
	writeLUT("spiky", "float", spikyLUT);
	std::array splineLUT = generateLUT([&](float4 c, float4 p, float d) { return (1.f + dFactor * d) *  kernel(c, p); });
	writeLUT("spline", "float", splineLUT);
	std::array cohesionLUT = generateLUT([&](float4 c, float4 p, float d) { return (1.f + dFactor * d) *  Kernel<kernel_kind::cohesion>::value(c, p).x; });
	for (int32_t i = 1; i < cohesionLUT.size(); ++i) {
		cohesionLUT[i] = abs(cohesionLUT[i]) < 1e-12 ? cohesionLUT[i - 1] : cohesionLUT[i];
	}
	writeLUT("cohesion", "float", cohesionLUT);
	std::array adhesionLUT = generateLUT([&](float4 c, float4 p, float d) { return (1.f + dFactor * d) *  Kernel<kernel_kind::adhesion>::value(c, p).x; });
	for (int32_t i = 1; i < adhesionLUT.size(); ++i) {
		adhesionLUT[i] = abs(adhesionLUT[i]) < 1e-12 ? adhesionLUT[i - 1] : adhesionLUT[i];
	}
	writeLUT("adhesion", "float", adhesionLUT);

	std::array volumeLUT = generateLUT([&](float4 c, float4 p, float d) { return (1.f + dFactor * d); });
	//for (auto& v : volumeLUT)
	//	v = 1.f;
	writeLUT("volume", "float", volumeLUT);

	////std::cout << adhesion0  << std::endl;
	//std::array spikyGradientLUT = generateLUT([&](float4 c, float4 p, float d) {
	//	return -((1.f + dFactor * d) *  SpikyKernel<kernel_kind::spline4>::gradient(c, p).x); });

	std::array spikyGradientLUT = approximateGradient(spikyLUT, volumeLUT);
	for (int32_t i = 1; i < spikyGradientLUT.size(); ++i) {
		//spikyGradientLUT[i] = fabsf(spikyGradientLUT[i]) < 1e-12f ? spikyGradientLUT[i - 1] : spikyGradientLUT[i];
	}
	//std::array spikyGradientLUT = generateLUT([&](float4 c, float4 p, float d) { return (1.f) * SpikyKernel<kernel_kind::spline4>::gradient(c, p).x ; });
	//for (auto& v : spikyGradientLUT)
		//v = -v;// math::max(std::decay_t<decltype(v)>{0.f}, v);
	writeLUT("spikyGradient", "float", spikyGradientLUT);
	std::array splineGradientLUT = approximateGradient(splineLUT, volumeLUT);
	for (int32_t i = 1; i < splineGradientLUT.size(); ++i) {
		//splineGradientLUT[i] = fabsf(splineGradientLUT[i]) < 1e-12f ? splineGradientLUT[i - 1] : splineGradientLUT[i];
	}
	//std::array splineGradientLUT = generateLUT([&](float4 c, float4 p, float d) { return (1.f ) * gradient(c, p).x;  });
	//for (auto& v : splineGradientLUT)
	//	v = -v;// math::max(std::decay_t<decltype(v)>{0.f}, v);
	writeLUT("splineGradient", "float", splineGradientLUT);

	//std::array densityLUT = sphericalIntegral([&](float4 c, float4 p, float d) {return kernel(c, p); }, 127, 127, 127);
	auto chi = [&](auto d) {
		return 1.f + dFactor * d;
		//return math::clamp(1.0 / lookup(densityLUT, -d * HforV1) + d,1.0,4.0);
	};
	//densityLUT = smoothLUT(densityLUT);
	//std::cout << "Generated density LUT" << std::endl;
	//writeLUT("density", "float", densityLUT);

	//std::array splineLUTN = sphericalIntegral([&](float4 c, float4 p, float d) {return chi(d) * kernel(c, p); }, 127, 127, 127);
	//splineLUTN = smoothLUT(splineLUTN);
	//std::cout << "Generated spline LUT" << std::endl;
	//writeLUT("splinePolar", "float", splineLUTN);
	//std::array splineGradientLUTN = approximateGradient(splineLUTN);
	//std::cout << "Generated spline Gradient LUT" << std::endl;
	//writeLUT("splineGradientPolar", "float", splineGradientLUTN);

	//std::array spikyLUT = sphericalIntegral([&](float4 c, float4 p, float d) {return chi(d) * SpikyKernel<kernel_kind::spline4>::value(c, p); }, 128, 128, 128);
	//spikyLUT = smoothLUT(spikyLUT);
	//std::cout << "Generated spiky LUT" << std::endl;
	//writeLUT("spiky", "float", spikyLUT);
	//std::array spikyGradientLUT = approximateGradient(spikyLUT);
	//std::cout << "Generated spiky Gradient LUT" << std::endl;
	//writeLUT("spikyGradient", "float", spikyGradientLUT);
	//std::array cohesionLUT = sphericalIntegral([&](float4 c, float4 p, float d) {return chi(d) * Kernel<kernel_kind::cohesion>::value(c, p).x; }, 128, 128, 128);
	//std::cout << "Generated cohesion LUT" << std::endl;
	//writeLUT("cohesion", "float", cohesionLUT);
	//std::array adhesionLUT = sphericalIntegral([&](float4 c, float4 p, float d) {return chi(d) * Kernel<kernel_kind::adhesion>::value(c, p).x; }, 128, 128, 128);
	//auto adhesion0 = lookup(adhesionLUT, -HforV1);
	//for (auto& v : adhesionLUT)
	//	v /= adhesion0;
	//std::cout << "Generated adhesion LUT" << std::endl;
	//writeLUT("adhesion", "float", adhesionLUT);
	//std::array volumeLUT = sphericalIntegral([&](float4 c, float4 p, float d) {return 1.0; }, 128, 128, 128);
	//std::cout << "Generated volume LUT" << std::endl;
	//writeLUT("volume", "float", volumeLUT);

	//float dh = 0.001f;
	//float4 dx{ dh,0.f,0.f,0.f };
	//float4 dy{ 0.f,dh,0.f,0.f };
	//float4 dz{ 0.f,0.f,dh,0.f };

	//std::array spikyLUT = generateLUT([&](float4 c, float4 p) { return SpikyKernel<kernel_kind::spline4>::value(c, p); });
	//std::array splineLUT = generateLUT([&](float4 c, float4 p) { return kernel(c, p); });
	//std::array cohesionLUT = generateLUT([&](float4 c, float4 p) { return Kernel<kernel_kind::cohesion>::value(c, p).x; });
	//std::array adhesionLUT = generateLUT([&](float4 c, float4 p) { return Kernel<kernel_kind::adhesion>::value(c, p).x; });
	//std::array spikyGradientLUT = gradientLUT([&](float4 c, float4 p) { return SpikyKernel<kernel_kind::spline4>::value(c, p); });
	//std::array splineGradientLUT = gradientLUT([&](float4 c, float4 p) { return kernel(c, p); });
	//std::array volumeLUT = gradientLUT([&](float4 c, float4 p) { return 1.f; });

	//for (int32_t i = 0; i < lutSize; ++i) {
	//	std::cout << i << "\t" << splineLUT[i] << " - " << splineLUT2[i] << " -> " << splineLUT[i] / splineLUT2[i] << std::endl;
	//}

	//for (float x = -H; x <= H; x += H / 16.f) {
	//	std::cout << x << "\t" << lookup(splineLUT, x) << " @ " << lookup(splineLUT2, x) << std::endl;
	//}

	//for (float x = -H; x <= H; x += H / 16.f) {
	//	std::cout << x << "\t" << lookup(splineLUT, x) << " @ " << lookup(splineGradientLUT, x) << " -> " << lookup(splineLUT, x + dh) << " : " << lookup(splineLUT, x) + dh * lookup(splineGradientLUT, x) << std::endl;
	//}

	//for (auto& v : splineGradientLUT)
	//	v = -v;// math::max(std::decay_t<decltype(v)>{0.f}, v);
	//for (auto& v : spikyGradientLUT)
	//	v = -v;// math::max(std::decay_t<decltype(v)>{0.f}, v);
	//for (auto& v : cohesionLUT)
	//	v = -v;// math::max(std::decay_t<decltype(v)>{0.f}, v);
	////std::cout << adhesion0  << std::endl;

	//auto x0 = lookup(splineLUT, 0.0f);
	//auto xp = lookup(splineLUT, 0.0f + t);
	//auto xn = lookup(splineLUT, 0.0f - t);
	//auto xnum = (xp - xn) / (2.f * t);
	//auto xint = lookup(splineGradientLUT, 0.f);
	//std::cout << x0 << " -> " << xp << std::endl;
	//std::cout << xnum << " <-> " << xint << std::endl;

	//float evalP = 0.f * H;
	//std::cout << "Spherical integral" << std::endl;
	//std::cout << "Kernel:   " << lookup(splineLUT, evalP) << std::endl;
	//std::cout << "Gradient: " << lookup(splineGradientLUT, evalP) << std::endl;

	//float4 c{ 0.f,0.f,0.f,h };
	//double sumd = 0.0;
	//double4 gradSumd{ 0.0,0.0,0.0,0.0 };
	//double volume = 0.f;
	//constexpr auto trapz = 512;
	//constexpr auto dt = 2.0 * (double) H / ((double)trapz);
	//constexpr auto dV = dt * dt * dt;
	//auto trapH = support_from_volume(dV);
	//for (int32_t xi = -trapz / 2; xi <= trapz / 2; ++xi) {
	//	for (int32_t yi = -trapz / 2; yi <= trapz / 2; ++yi) {
	//		for (int32_t zi = -trapz / 2; zi <= trapz / 2; ++zi) {
	//			float4 p{ (float)dt * (float)xi, (float)dt * (float)yi, (float)dt * (float)zi, h };
	//			if (p.x > evalP)
	//				continue;
	//			sumd += (double) kernel(c, p) * dV;
	//			gradSumd += math::castTo<double4>(gradient(c, p)) * dV;
	//			volume += kernel(c, p) > 0.f ? dV : 0.f;
	//		}
	//	}
	//}
	//std::cout << "Trapz" << std::endl;
	//std::cout << "Kernel:   " << sumd << std::endl;
	//std::cout << "Gradient: " << gradSumd << std::endl;

	//getchar();
}


//
// Created by tkhamvilai on 7/16/19.
//

#ifndef DIST_MILP_MATHHELPERFUNCTIONS_HPP
#define DIST_MILP_MATHHELPERFUNCTIONS_HPP

#include <vector>
#include <algorithm>
#include <Eigen/Dense>

bool MathHelperFunctions_isSubset(std::vector<int> arr1, std::vector<int> arr2);
int MathHelperFunctions_norm_of_difference(int i, int j, Eigen::MatrixXi mat);
int MathHelperFunctions_Intersection(std::vector<int> arr1, std::vector<int> arr2);

#endif //DIST_MILP_MATHHELPERFUNCTIONS_HPP


/*
	Copyright (C) 2003-2014 by David White <davewx7@gmail.com>

	This software is provided 'as-is', without any express or implied
	warranty. In no event will the authors be held liable for any damages
	arising from the use of this software.

	Permission is granted to anyone to use this software for any purpose,
	including commercial applications, and to alter it and redistribute it
	freely, subject to the following restrictions:

	   1. The origin of this software must not be misrepresented; you must not
	   claim that you wrote the original software. If you use this software
	   in a product, an acknowledgement in the product documentation would be
	   appreciated but is not required.

	   2. Altered source versions must be plainly marked as such, and must not be
	   misrepresented as being the original software.

	   3. This notice may not be removed or altered from any source
	   distribution.
*/

#pragma once

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

#include "geometry.hpp"

template<typename T>
geometry::Point<T> rotate_point_around_origin(T x1, T y1, float alpha, bool round)
{
	geometry::Point<T> beta;

	/*   //we actually don't need the initial theta and radius.  This is why:
	x2 = R * (cos(theta) * cos(alpha) + sin(theta) * sin(alpha))
	y2 = R * (sin(theta) * cos(alpha) + cos(theta) * sin(alpha));
	but
	R * (cos(theta)) = x1
	R * (sin(theta)) = x2
	this collapses the above to:  */

	float c1 = x1 * cos(alpha) - y1 * sin(alpha);
	float c2 = y1 * cos(alpha) + x1 * sin(alpha);

	beta.x = static_cast<T>(round ? boost::math::round(c1) : c1);
	beta.y = static_cast<T>(round ? boost::math::round(c2) : c2);

	return beta;
}

template<typename T>
geometry::Point<T> rotate_point_around_origin_with_offset(T x1, T y1, float alpha, T u1, T v1, bool round=true)
{
	geometry::Point<T> beta = rotate_point_around_origin(x1 - u1, y1 - v1, alpha, round);

	beta.x += u1;
	beta.y += v1;

	return beta;
}

void rotate_rect(short center_x, short center_y, float rotation, short* rect_vertexes);
void rotate_rect(float center_x, float center_y, float rotation, float* rect_vertexes);
void rotate_rect(const rect& r, float angle, short* output);


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

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

 * See LICENSE for the license information

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

/**
 * @file    Rot3M.cpp
 * @brief   Rotation (internal: 3*3 matrix representation*)
 * @author  Alireza Fathi
 * @author  Christian Potthast
 * @author  Frank Dellaert
 * @author  Richard Roberts
 */

#include <gtsam/config.h> // Get GTSAM_USE_QUATERNIONS macro

#ifndef GTSAM_USE_QUATERNIONS

#include <gtsam/geometry/Rot3.h>
#include <boost/math/constants/constants.hpp>
#include <cmath>

using namespace std;

namespace gtsam {

static const Matrix3 I3 = Matrix3::Identity();

/* ************************************************************************* */
Rot3::Rot3() : rot_(Matrix3::Identity()) {}

/* ************************************************************************* */
Rot3::Rot3(const Point3& col1, const Point3& col2, const Point3& col3) {
  rot_.col(0) = col1.vector();
  rot_.col(1) = col2.vector();
  rot_.col(2) = col3.vector();
}

/* ************************************************************************* */
Rot3::Rot3(double R11, double R12, double R13,
    double R21, double R22, double R23,
    double R31, double R32, double R33) {
    rot_ << R11, R12, R13,
        R21, R22, R23,
        R31, R32, R33;
}

/* ************************************************************************* */
Rot3::Rot3(const Matrix3& R) {
  rot_ = R;
}

/* ************************************************************************* */
Rot3::Rot3(const Matrix& R) {
  if (R.rows()!=3 || R.cols()!=3)
    throw invalid_argument("Rot3 constructor expects 3*3 matrix");
  rot_ = R;
}

///* ************************************************************************* */
//Rot3::Rot3(const Matrix3& R) : rot_(R) {}

/* ************************************************************************* */
Rot3::Rot3(const Quaternion& q) : rot_(q.toRotationMatrix()) {}

/* ************************************************************************* */
Rot3 Rot3::Rx(double t) {
  double st = sin(t), ct = cos(t);
  return Rot3(
      1,  0,  0,
      0, ct,-st,
      0, st, ct);
}

/* ************************************************************************* */
Rot3 Rot3::Ry(double t) {
  double st = sin(t), ct = cos(t);
  return Rot3(
      ct, 0, st,
      0, 1,  0,
      -st, 0, ct);
}

/* ************************************************************************* */
Rot3 Rot3::Rz(double t) {
  double st = sin(t), ct = cos(t);
  return Rot3(
      ct,-st, 0,
      st, ct, 0,
      0,  0, 1);
}

/* ************************************************************************* */
// Considerably faster than composing matrices above !
Rot3 Rot3::RzRyRx(double x, double y, double z) {
  double cx=cos(x),sx=sin(x);
  double cy=cos(y),sy=sin(y);
  double cz=cos(z),sz=sin(z);
  double ss_ = sx * sy;
  double cs_ = cx * sy;
  double sc_ = sx * cy;
  double cc_ = cx * cy;
  double c_s = cx * sz;
  double s_s = sx * sz;
  double _cs = cy * sz;
  double _cc = cy * cz;
  double s_c = sx * cz;
  double c_c = cx * cz;
  double ssc = ss_ * cz, csc = cs_ * cz, sss = ss_ * sz, css = cs_ * sz;
  return Rot3(
      _cc,- c_s + ssc,  s_s + csc,
      _cs,  c_c + sss, -s_c + css,
      -sy,        sc_,        cc_
  );
}

/* ************************************************************************* */
Rot3 Rot3::rodriguez(const Vector& w, double theta) {
  // get components of axis \omega
  double wx = w(0), wy=w(1), wz=w(2);
  double wwTxx = wx*wx, wwTyy = wy*wy, wwTzz = wz*wz;
#ifndef NDEBUG
  double l_n = wwTxx + wwTyy + wwTzz;
  if (std::abs(l_n-1.0)>1e-9) throw domain_error("rodriguez: length of n should be 1");
#endif

  double c = cos(theta), s = sin(theta), c_1 = 1 - c;

  double swx = wx * s, swy = wy * s, swz = wz * s;
  double C00 = c_1*wwTxx, C01 = c_1*wx*wy, C02 = c_1*wx*wz;
  double                  C11 = c_1*wwTyy, C12 = c_1*wy*wz;
  double                                   C22 = c_1*wwTzz;

  return Rot3(
        c + C00, -swz + C01,  swy + C02,
      swz + C01,    c + C11, -swx + C12,
     -swy + C02,  swx + C12,    c + C22);
}

/* ************************************************************************* */
Rot3 Rot3::compose (const Rot3& R2,
    boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
  if (H1) *H1 = R2.transpose();
  if (H2) *H2 = I3;
  return *this * R2;
}

/* ************************************************************************* */
Rot3 Rot3::operator*(const Rot3& R2) const {
  return Rot3(Matrix3(rot_*R2.rot_));
}

/* ************************************************************************* */
Rot3 Rot3::inverse(boost::optional<Matrix&> H1) const {
  if (H1) *H1 = -rot_;
  return Rot3(Matrix3(rot_.transpose()));
}

/* ************************************************************************* */
Rot3 Rot3::between (const Rot3& R2,
    boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
  if (H1) *H1 = -(R2.transpose()*rot_);
  if (H2) *H2 = I3;
  return Rot3(Matrix3(rot_.transpose()*R2.rot_));
}

/* ************************************************************************* */
Point3 Rot3::rotate(const Point3& p,
    boost::optional<Matrix&> H1,  boost::optional<Matrix&> H2) const {
  if (H1 || H2) {
      if (H1) *H1 = rot_ * skewSymmetric(-p.x(), -p.y(), -p.z());
      if (H2) *H2 = rot_;
    }
  return Point3(rot_ * p.vector());
}

/* ************************************************************************* */
// Log map at identity - return the canonical coordinates of this rotation
Vector3 Rot3::Logmap(const Rot3& R) {

  static const double PI = boost::math::constants::pi<double>();

  const Matrix3& rot = R.rot_;
  // Get trace(R)
  double tr = rot.trace();

  // when trace == -1, i.e., when theta = +-pi, +-3pi, +-5pi, etc.
  // we do something special
  if (std::abs(tr+1.0) < 1e-10) {
    if(std::abs(rot(2,2)+1.0) > 1e-10)
      return (PI / sqrt(2.0+2.0*rot(2,2) )) *
          Vector3(rot(0,2), rot(1,2), 1.0+rot(2,2));
    else if(std::abs(rot(1,1)+1.0) > 1e-10)
      return (PI / sqrt(2.0+2.0*rot(1,1))) *
          Vector3(rot(0,1), 1.0+rot(1,1), rot(2,1));
    else // if(std::abs(R.r1_.x()+1.0) > 1e-10)  This is implicit
      return (PI / sqrt(2.0+2.0*rot(0,0))) *
          Vector3(1.0+rot(0,0), rot(1,0), rot(2,0));
  } else {
    double magnitude;
    double tr_3 = tr-3.0; // always negative
    if (tr_3<-1e-7) {
      double theta = acos((tr-1.0)/2.0);
      magnitude = theta/(2.0*sin(theta));
    } else {
      // when theta near 0, +-2pi, +-4pi, etc. (trace near 3.0)
      // use Taylor expansion: magnitude \approx 1/2-(t-3)/12 + O((t-3)^2)
      magnitude = 0.5 - tr_3*tr_3/12.0;
    }
    return magnitude*Vector3(
        rot(2,1)-rot(1,2),
        rot(0,2)-rot(2,0),
        rot(1,0)-rot(0,1));
  }
}

/* ************************************************************************* */
Rot3 Rot3::retractCayley(const Vector& omega) const {
  const double x = omega(0), y = omega(1), z = omega(2);
  const double x2 = x * x, y2 = y * y, z2 = z * z;
  const double xy = x * y, xz = x * z, yz = y * z;
  const double f = 1.0 / (4.0 + x2 + y2 + z2), _2f = 2.0 * f;
  return (*this)
      * Rot3((4 + x2 - y2 - z2) * f, (xy - 2 * z) * _2f, (xz + 2 * y) * _2f,
          (xy + 2 * z) * _2f, (4 - x2 + y2 - z2) * f, (yz - 2 * x) * _2f,
          (xz - 2 * y) * _2f, (yz + 2 * x) * _2f, (4 - x2 - y2 + z2) * f);
}

/* ************************************************************************* */
Rot3 Rot3::retract(const Vector& omega, Rot3::CoordinatesMode mode) const {
  if(mode == Rot3::EXPMAP) {
    return (*this)*Expmap(omega);
  } else if(mode == Rot3::CAYLEY) {
    return retractCayley(omega);
  } else if(mode == Rot3::SLOW_CAYLEY) {
    Matrix Omega = skewSymmetric(omega);
    return (*this)*CayleyFixed<3>(-Omega/2);
  } else {
    assert(false);
    exit(1);
  }
}

/* ************************************************************************* */
Vector3 Rot3::localCoordinates(const Rot3& T, Rot3::CoordinatesMode mode) const {
  if(mode == Rot3::EXPMAP) {
    return Logmap(between(T));
  } else if(mode == Rot3::CAYLEY) {
    // Create a fixed-size matrix
    Eigen::Matrix3d A(between(T).matrix());
    // Mathematica closed form optimization (procrastination?) gone wild:
    const double a=A(0,0),b=A(0,1),c=A(0,2);
    const double d=A(1,0),e=A(1,1),f=A(1,2);
    const double g=A(2,0),h=A(2,1),i=A(2,2);
    const double di = d*i, ce = c*e, cd = c*d, fg=f*g;
    const double M = 1 + e - f*h + i + e*i;
    const double K = 2.0 / (cd*h + M + a*M -g*(c + ce) - b*(d + di - fg));
    const double x = (a * f - cd + f) * K;
    const double y = (b * f - ce - c) * K;
    const double z = (fg - di - d) * K;
    return -2 * Vector3(x, y, z);
  } else if(mode == Rot3::SLOW_CAYLEY) {
    // Create a fixed-size matrix
    Eigen::Matrix3d A(between(T).matrix());
    // using templated version of Cayley
    Eigen::Matrix3d Omega = CayleyFixed<3>(A);
    return -2*Vector3(Omega(2,1),Omega(0,2),Omega(1,0));
  } else {
    assert(false);
    exit(1);
  }
}

/* ************************************************************************* */
Matrix3 Rot3::matrix() const {
  return rot_;
}

/* ************************************************************************* */
Matrix3 Rot3::transpose() const {
  return rot_.transpose();
}

/* ************************************************************************* */
Point3 Rot3::r1() const { return Point3(rot_.col(0)); }

/* ************************************************************************* */
Point3 Rot3::r2() const { return Point3(rot_.col(1)); }

/* ************************************************************************* */
Point3 Rot3::r3() const { return Point3(rot_.col(2)); }

/* ************************************************************************* */
Quaternion Rot3::toQuaternion() const {
  return Quaternion(rot_);
}

/* ************************************************************************* */

} // namespace gtsam

#endif


#include <stan/math/fwd/scal.hpp>
#include <gtest/gtest.h>
#include <boost/math/special_functions/gamma.hpp>
#include <boost/math/special_functions/digamma.hpp>
#include <math/fwd/scal/fun/nan_util.hpp>

TEST(AgradFwdTgamma, Fvar) {
  using boost::math::digamma;
  using stan::math::fvar;
  using stan::math::tgamma;

  fvar<double> x(0.5, 1.0);
  fvar<double> a = tgamma(x);
  EXPECT_FLOAT_EQ(tgamma(0.5), a.val_);
  EXPECT_FLOAT_EQ(tgamma(0.5) * digamma(0.5), a.d_);
}

struct tgamma_fun {
  template <typename T0>
  inline T0 operator()(const T0& arg1) const {
    return tgamma(arg1);
  }
};

TEST(AgradFwdTgamma, tgamma_NaN_0) {
  tgamma_fun tgamma_;
  test_nan_fwd(tgamma_, false);
}


#ifndef _TEST_FIXTURES_H_
#define _TEST_FIXTURES_H_
#include <iostream>
#include <iomanip>
#include <vector>
#include <cmath>
//#include "mxn_indexer.hpp"
#include <boost/static_assert.hpp>
#include "boost/multi_array.hpp"

//http://www.boost.org/doc/libs/1_55_0/libs/multi_array/doc/user.html
//http://stackoverflow.com/questions/2168082/how-to-rewrite-array-from-row-order-to-column-order
#include "image_stack_utils.h"
#include "test_algorithms.hpp"

namespace anyfold {

template <unsigned short KernelDimSize = 3,
	  unsigned ImageDimSize = 8
	  >
struct convolutionFixture3D
{

  const unsigned    image_size_   ;
  std::vector<int>  image_shape_                             ;
  std::vector<int>  padded_image_shape_                             ;

  image_stack       image_                           ;
  image_stack       padded_image_                           ;
  image_stack       padded_one_                           ;
  image_stack       padded_output_                           ;

  image_stack       image_folded_by_horizontal_             ;
  image_stack       image_folded_by_vertical_               ;
  image_stack       image_folded_by_depth_                  ;
  image_stack       image_folded_by_all1_                   ;

  image_stack       padded_image_folded_by_horizontal_             ;
  image_stack       padded_image_folded_by_vertical_               ;
  image_stack       padded_image_folded_by_depth_                  ;
  image_stack       padded_image_folded_by_all1_                   ;

  const unsigned    kernel_size_  ;
  std::vector<int>  kernel_dims_                            ;
  std::vector<int>  asymm_kernel_dims_                            ;

  image_stack       trivial_kernel_                         ;
  image_stack       identity_kernel_                        ;
  image_stack       vertical_kernel_                        ;
  image_stack       horizontal_kernel_                        ;
  image_stack       depth_kernel_                           ;
  image_stack       all1_kernel_                           ;

  BOOST_STATIC_ASSERT(KernelDimSize % 2 != 0);

public:

  convolutionFixture3D():
    image_size_                             ((unsigned)std::pow(ImageDimSize,3)),
    image_shape_                             (3,ImageDimSize),
    padded_image_shape_                             (3,ImageDimSize+2*(KernelDimSize/2)),
    image_                           (boost::extents[ImageDimSize][ImageDimSize][ImageDimSize]),
    padded_image_                           (boost::extents[ImageDimSize+2*(KernelDimSize/2)][ImageDimSize+2*(KernelDimSize/2)][ImageDimSize+2*(KernelDimSize/2)]),
    padded_one_                           (boost::extents[ImageDimSize+2*(KernelDimSize/2)][ImageDimSize+2*(KernelDimSize/2)][ImageDimSize+2*(KernelDimSize/2)]),
    padded_output_                           (boost::extents[ImageDimSize+2*(KernelDimSize/2)][ImageDimSize+2*(KernelDimSize/2)][ImageDimSize+2*(KernelDimSize/2)]),
    image_folded_by_horizontal_             (boost::extents[ImageDimSize][ImageDimSize][ImageDimSize]),
    image_folded_by_vertical_               (boost::extents[ImageDimSize][ImageDimSize][ImageDimSize]),
    image_folded_by_depth_                  (boost::extents[ImageDimSize][ImageDimSize][ImageDimSize]),
    image_folded_by_all1_                   (boost::extents[ImageDimSize][ImageDimSize][ImageDimSize]),
    padded_image_folded_by_horizontal_             (boost::extents[ImageDimSize+2*(KernelDimSize/2)][ImageDimSize+2*(KernelDimSize/2)][ImageDimSize+2*(KernelDimSize/2)]),
    padded_image_folded_by_vertical_               (boost::extents[ImageDimSize+2*(KernelDimSize/2)][ImageDimSize+2*(KernelDimSize/2)][ImageDimSize+2*(KernelDimSize/2)]),
    padded_image_folded_by_depth_                  (boost::extents[ImageDimSize+2*(KernelDimSize/2)][ImageDimSize+2*(KernelDimSize/2)][ImageDimSize+2*(KernelDimSize/2)]),
    padded_image_folded_by_all1_                   (boost::extents[ImageDimSize+2*(KernelDimSize/2)][ImageDimSize+2*(KernelDimSize/2)][ImageDimSize+2*(KernelDimSize/2)]),
    kernel_size_                            ((unsigned)std::pow(KernelDimSize,3)),
    kernel_dims_                            (3,KernelDimSize),
    asymm_kernel_dims_                            (3,KernelDimSize),
    trivial_kernel_                         (boost::extents[KernelDimSize][KernelDimSize][KernelDimSize]),
    identity_kernel_                        (boost::extents[KernelDimSize][KernelDimSize][KernelDimSize]),
    vertical_kernel_                        (boost::extents[KernelDimSize][KernelDimSize][KernelDimSize]),
    horizontal_kernel_                        (boost::extents[KernelDimSize][KernelDimSize][KernelDimSize]),
    depth_kernel_                           (boost::extents[KernelDimSize][KernelDimSize][KernelDimSize]),
    all1_kernel_                           (boost::extents[KernelDimSize][KernelDimSize][KernelDimSize])
  {

    //FILL KERNELS
    const unsigned halfKernel  = KernelDimSize/2u;

    std::fill(trivial_kernel_.data()       ,trivial_kernel_.data()       +  kernel_size_                           ,0.f);
    std::fill(identity_kernel_.data()      ,identity_kernel_.data()      +  kernel_size_                           ,0.f);
    std::fill(vertical_kernel_.data()      ,vertical_kernel_.data()      +  kernel_size_                           ,0.f);
    std::fill(depth_kernel_.data()         ,depth_kernel_.data()         +  kernel_size_                           ,0.f);
    std::fill(all1_kernel_.data()         ,all1_kernel_.data()         +  kernel_size_                           ,1.f);
    std::fill(horizontal_kernel_.data()      ,horizontal_kernel_.data()      +  kernel_size_                           ,0.f);

    identity_kernel_[KernelDimSize/2][KernelDimSize/2][KernelDimSize/2]=1.;

    for(unsigned int index = 0;index<KernelDimSize;++index){
      horizontal_kernel_[index][halfKernel][halfKernel] = float(index+1);
      vertical_kernel_[halfKernel][index][halfKernel] = float(index+1);
      depth_kernel_   [halfKernel][halfKernel][index] = float(index+1);
    }

    //FILL IMAGES
    unsigned padded_image_axis = ImageDimSize+2*halfKernel;
    unsigned padded_image_size = std::pow(padded_image_axis,3);
    std::fill(padded_image_.data(),  padded_image_.data()  +  padded_image_size,  0.f  );
    std::fill(padded_one_.data(),  padded_one_.data()  +  padded_image_size,  0.f  );
    std::fill(padded_output_.data(),  padded_output_.data()  +  padded_image_size,  0.f  );
    std::fill(padded_image_folded_by_horizontal_.data(),  padded_image_folded_by_horizontal_.data()  +  padded_image_size,  0.f  );
    std::fill(padded_image_folded_by_vertical_.data(),  padded_image_folded_by_vertical_.data()  +  padded_image_size,  0.f  );
    std::fill(padded_image_folded_by_depth_.data(),  padded_image_folded_by_depth_.data()  +  padded_image_size,  0.f  );
    std::fill(padded_image_folded_by_all1_.data(),  padded_image_folded_by_all1_.data()  +  padded_image_size,  0.f  );

    padded_one_[padded_image_axis/2][padded_image_axis/2][padded_image_axis/2] = 1.f;

    for (unsigned pixel = 0; pixel < image_.num_elements(); ++pixel)
      image_.data()[pixel] = float(pixel);

    //PADD THE IMAGE FOR CONVOLUTION
    range axis_subrange = range(halfKernel,halfKernel+ImageDimSize);
    image_stack_view padded_image_original = padded_image_[ boost::indices[axis_subrange][axis_subrange][axis_subrange] ];
    padded_image_original = image_;

    padded_image_folded_by_horizontal_  = padded_image_;
    padded_image_folded_by_vertical_    = padded_image_;
    padded_image_folded_by_depth_       = padded_image_;
    padded_image_folded_by_all1_        = padded_image_;

    //PREPARE ASYMM IMAGES
    std::vector<unsigned> symm_offsets(3);
    std::fill(symm_offsets.begin(), symm_offsets.end(), halfKernel);

    //CONVOLVE
    convolve(padded_image_, horizontal_kernel_, padded_image_folded_by_horizontal_, symm_offsets);
    convolve(padded_image_, vertical_kernel_, padded_image_folded_by_vertical_, symm_offsets);
    convolve(padded_image_, depth_kernel_, padded_image_folded_by_depth_, symm_offsets);
    convolve(padded_image_, all1_kernel_, padded_image_folded_by_all1_, symm_offsets);

    //EXTRACT NON-PADDED CONTENT FROM CONVOLVED IMAGE STACKS
    image_folded_by_horizontal_  = padded_image_folded_by_horizontal_[ boost::indices[axis_subrange][axis_subrange][axis_subrange] ];
    image_folded_by_vertical_    = padded_image_folded_by_vertical_[ boost::indices[axis_subrange][axis_subrange][axis_subrange] ];
    image_folded_by_depth_       = padded_image_folded_by_depth_[ boost::indices[axis_subrange][axis_subrange][axis_subrange] ];
    image_folded_by_all1_        = padded_image_folded_by_all1_[ boost::indices[axis_subrange][axis_subrange][axis_subrange] ];

  }

   virtual ~convolutionFixture3D()  {

  };

  static const unsigned image_axis_size = ImageDimSize;
  static const unsigned kernel_axis_size = KernelDimSize;

};

typedef convolutionFixture3D<> default_3D_fixture;

}

#endif


///////////////////////////////////////////////////////////////////////////////
//  Copyright 2020 John Maddock. Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#include <nil/crypto3/multiprecision/mpfr.hpp>
#include <boost/math/special_functions/next.hpp>
#include "test.hpp"

using namespace nil::crypto3::multiprecision;

int main() {
    mpfr_float_50 half = 0.5;
    mpfr_float_50 under_half = boost::math::float_prior(half);
    BOOST_CHECK_NE(half, under_half);
    int e1, e2;
    mpfr_float_50 norm1, norm2;
    norm1 = frexp(half, &e1);
    norm2 = frexp(under_half, &e2);
    BOOST_CHECK_EQUAL(norm1, half);
    BOOST_CHECK_EQUAL(e1, 0);
    BOOST_CHECK_GT(1, norm2);
    BOOST_CHECK_LE(0.5, norm2);
    BOOST_CHECK_EQUAL(e2, -1);
    return boost::report_errors();
}


//==================================================================================================
/*!
  @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_FUNCTION_COTPI_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_COTPI_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

  /*!
    @ingroup group-trigonometric
    This function object returns the cotangent of input in
    \f$\pi\f$ multiples: \f$\cos(\pi x)/sin(\pi x)\f$.

    @par Header <boost/simd/function/cotpi.hpp>

    @par Note

      As most other trigonometric function cotd can be called
      with a second optional parameter  which is a tag on speed
      and accuracy (see @ref cos for further details)

    @see cos, sin, tan, cot, cotpi

    @par Example:

      @snippet cotpi.cpp cotpi

    @par Possible output:

      @snippet cotpi.txt cotpi

  **/
  IEEEValue cotpi(IEEEValue const& x);
} }
#endif

#include <boost/simd/function/scalar/cotpi.hpp>
#include <boost/simd/function/simd/cotpi.hpp>

#endif