/***********************************************************************/
/**                                                                    */
/** vector2d.hpp                                                       */
/**                                                                    */
/** Copyright (c) 2016, Service Robotics Lab.                          */
/**                     http://robotics.upo.es                         */
/**                                                                    */
/** All rights reserved.                                               */
/**                                                                    */
/** Authors:                                                           */
/** Ignacio Perez-Hurtado (maintainer)                                 */
/** Jesus Capitan                                                      */
/** Fernando Caballero                                                 */
/** Luis Merino                                                        */
/**                                                                    */
/** This software may be modified and distributed under the terms      */
/** of the BSD license. See the LICENSE file for details.              */
/**                                                                    */
/** http://www.opensource.org/licenses/BSD-3-Clause                    */
/**                                                                    */
/***********************************************************************/

#ifndef _VECTOR2D_HPP_
#define _VECTOR2D_HPP_

#include <iostream>
#include <cmath>
//#include <geometry_msgs/Point.h>
#include <boost/functional/hash.hpp>

#include "angle.hpp"

namespace utils
{
class Vector2d
{
public:
  Vector2d() : x(0), y(0)
  {
  }
  Vector2d(double x, double y) : x(x), y(y)
  {
  }
  virtual ~Vector2d()
  {
  }
  double operator()(int index) const
  {
    return index == 0 ? x : y;
  }
  double operator[](int index) const
  {
    return index == 0 ? x : y;
  }
  bool operator==(const Vector2d& other) const
  {
    return x == other.x && y == other.y;
  }
  bool operator<(const Vector2d& other) const
  {
    return x < other.x || (x == other.x && y < other.y);
  }
  double getX() const
  {
    return x;
  }
  double getY() const
  {
    return y;
  }

  /*geometry_msgs::Point toPoint() const
  {
    geometry_msgs::Point p;
    p.x = x;
    p.y = y;
    p.z = 0;
    return p;
  }*/

  Vector2d& set(double x, double y)
  {
    Vector2d::x = x;
    Vector2d::y = y;
    return *this;
  }
  Vector2d& setX(double x)
  {
    Vector2d::x = x;
    return *this;
  }

  Vector2d& setY(double y)
  {
    Vector2d::y = y;
    return *this;
  }

  Vector2d& incX(double inc_x)
  {
    x += inc_x;
    return *this;
  }

  Vector2d& incY(double inc_y)
  {
    y += inc_y;
    return *this;
  }

  Vector2d& inc(double inc_x, double inc_y)
  {
    x += inc_x;
    y += inc_y;
    return *this;
  }

  const Angle angle() const
  {
    return Angle::fromRadian(std::atan2(y, x));
  }

  Angle angleTo(const Vector2d& other) const
  {
    return other.angle() - angle();
  }

  double squaredNorm() const
  {
    return x * x + y * y;
  }

  double norm() const
  {
    return std::sqrt(squaredNorm());
  }

  double dot(const Vector2d& other) const
  {
    return x * other.x + y * other.y;
  }

  Vector2d& normalize()
  {
    double n = norm();
    if (n > 0)
    {
      x /= n;
      y /= n;
    }
    return *this;
  }

  Vector2d normalized() const
  {
    Vector2d v(*this);
    v.normalize();
    return v;
  }

  Vector2d& operator*=(double scalar)
  {
    x *= scalar;
    y *= scalar;
    return *this;
  }
  Vector2d operator*(double scalar) const
  {
    return Vector2d(x * scalar, y * scalar);
  }

  Vector2d& operator/=(double scalar)
  {
    x /= scalar;
    y /= scalar;
    return *this;
  }
  Vector2d operator/(double scalar) const
  {
    return Vector2d(x / scalar, y / scalar);
  }

  Vector2d leftNormalVector() const
  {
    return Vector2d(-y, x);
  }

  Vector2d rightNormalVector() const
  {
    return Vector2d(y, -x);
  }

  Vector2d& operator+=(const Vector2d& other)
  {
    set(x + other.x, y + other.y);
    return *this;
  }
  Vector2d operator+(const Vector2d& other) const
  {
    return Vector2d(x + other.x, y + other.y);
  }
  Vector2d& operator-=(const Vector2d& other)
  {
    set(x - other.x, y - other.y);
    return *this;
  }
  Vector2d operator-(const Vector2d& other) const
  {
    return Vector2d(x - other.x, y - other.y);
  }
  Vector2d operator-() const
  {
    return Vector2d(-x, -y);
  }

  static const Vector2d& Zero()
  {
    static Vector2d zero;
    return zero;
  }

private:
  double x;
  double y;
};
}

inline utils::Vector2d operator*(double scalar, const utils::Vector2d& v)
{
  utils::Vector2d w(v);
  w *= scalar;
  return w;
}

namespace std
{
inline ostream& operator<<(ostream& stream, const utils::Vector2d& v)
{
  stream << "(" << v.getX() << "," << v.getY() << ")";
  return stream;
}

template <>
struct hash<utils::Vector2d>
{
  size_t operator()(const utils::Vector2d& v) const
  {
    using boost::hash_value;
    using boost::hash_combine;
    std::size_t seed = 0;
    hash_combine(seed, hash_value(v[0]));
    hash_combine(seed, hash_value(v[1]));
    return seed;
  }
};
}

#endif


#include <PCP/Common/Option.h>
#include <PCP/Common/Log.h>
#include <PCP/Common/Progress.h>
#include <PCP/Common/String.h>

#include <PCP/Geometry/Geometry.h>
#include <PCP/Geometry/PLY.h>

#include <PCP/SpacePartitioning/KdTree.h>

#include <Ponca/Fitting>

#include <Eigen/Eigenvalues>

using namespace pcp;

using WeightKernel = Ponca::SmoothWeightKernel<Scalar>;
using WeightFunc   = Ponca::DistWeightFunc<ConstPoint, WeightKernel>;
using SphereFit    = Ponca::Basket<ConstPoint, WeightFunc, Ponca::OrientedSphereFit>;

int main(int argc, char *argv[])
{
    Option opt(argc, argv);
    const String in_input  = opt.get_string("input",  "i").set_required();
    const String in_output = opt.get_string("output", "o").set_default("output");
    const Scalar in_scale  = opt.get_float( "scale"     ).set_default(0.01);
    const int    in_iter   = opt.get_int(   "iter"      ).set_default(30);
    const int    in_every  = opt.get_int(   "every"     ).set_default(30);

    bool ok = opt.ok();
    if(!ok) return 1;
    info() << opt;

    Geometry g;
    ok = PLY::load(in_input, g);
    if(!ok) return 1;
    PCP_ASSERT(g.has_normals());
    Geometry g2 = g;

    const auto aabb = g.aabb();
    const auto aabb_diag = aabb.diagonal().norm();
    const auto radius = in_scale * aabb_diag;
    info() << "radius = " << radius;

    auto prog = Progress(in_iter);
    const int digits = std::to_string(in_iter).size();

    for(int iter=1; iter<=in_iter; ++iter)
    {
        g.build_kdtree();

        #pragma omp parallel for
        for(int i=0; i<g.size(); ++i)
        {
            SphereFit fit;
            fit.setWeightFunc(WeightFunc(radius));
            fit.init(g[i]);

            for(int j : g.kdtree().range_neighbors(g[i], radius))
            {
                fit.addNeighbor(g.at(j));
            }

            const auto status = fit.finalize();

            if(status == Ponca::STABLE)
            {
                // projection on the sphere
                g2.point(i) = fit.project(g[i]);
                g2.normal(i) = fit.primitiveGradient(g2.point(i)).normalized();

                // re-orient if necessary
                if(g2.normal(i).dot(g.normal(i)) < 0) g2.normal(i) *= -1;
            }
            else
            {
                warning() << "Unstable fit at point " << i;
            }
        }
        std::swap(g, g2);

        // save
        if(iter % in_every == 0 || iter == in_iter)
        {
            PLY::save(in_output + "_" + str::to_string(iter,digits) + ".ply", g, false);
        }

        ++prog;
    }

    return 0;
}


// remainder_tree_array.cpp : This file contains the 'main' function. Program execution begins and ends there.
//

#include <iostream>
#include <chrono>
#include <cmath>
#include <cassert>
#include <random>
#include <vector>
#include <NTL/ZZ.h>
#include <NTL/vector.h>

using namespace std;
using namespace std::chrono;
using namespace NTL;
/*
 * To compile and run in Linux:
 * g++ -o remainder_tree_array remainder_tree_array.cpp -lntl -lgmp -pthread
 * ./remainder_tree_array
 *
 */

/*
 * Returns C[], an array of residues A0 mod m0, A0A1 mod m0m1, etc.
 * A: array of A0, A1, ...
 * m: array of m0, m1, ...
 */
void remainder_tree(Vec<ZZ> &C, Vec<ZZ> &A, Vec<ZZ> &m);
void print_tree(Vec<ZZ> &tree);
void complexity_graph(int N, int d);

int main()
{

	//complexity_graph(1<<20, 1);

	int bound = 1<<20;

	// Testing Wilson's Theorem
	Vec<ZZ> A;
	A.SetLength(bound);
	Vec<ZZ> m;
	m.SetLength(bound);

	for(int i = 0; i < bound; i++){
		A[i] = i+1;
		m[i] = ProbPrime(ZZ(i+1)) ? i+1 : 1;
	}

	/*for(int i = 0; i < A.length(); i++){
		cout << A[i] << " ";
	}
	cout << endl;

	for(int i = 0; i < m.length(); i++){
		cout << m[i] << " ";
	}
	cout << endl;
	*/
	Vec<ZZ> C;
	C.SetLength(bound);

	remainder_tree(C, A, m);

	/*for(int i = 0; i < C.length(); i++){
		cout << C[i] << " ";
	}
	cout << endl;
	*/

}

/*
 * Original Remainder Tree method
 * No space optimizations
 */

void remainder_tree(Vec<ZZ> &C, Vec<ZZ> &A, Vec<ZZ> &m) {
	// Assert that lengths of A, m, C match
	assert(A.length() == m.length());
	assert(A.length() == C.length());

	// Set N = length of input arrays
	int N = A.length();

	// Don't do anything if arrays are trivial
	if(N == 0){
		return;
	}

	// Index of leaf at the bottom left
	int leftmost = 1 << ((int)ceil(log2(N)));

	// Declare trees (always of length 2N for any N)
	Vec<ZZ> ATree;
	ATree.SetLength(2 * N);
	Vec<ZZ> mTree;
	mTree.SetLength(2 * N);
	Vec<ZZ> CTree;
	CTree.SetLength(2 * N);

	/*
	 * For example when N=11 the leaves are in this order:
	 *     / \       /\   /\    /\
	 *    /   \     /  7 8  9 10  11
	 *   /\   /\   /\
	 *  1  2 3  4 5  6
	 *
	 */

	// Initialize the leaves in ATree and mTree
	for (int i = leftmost; i < 2 * N; i++) { // leaves on lowest layer
		ATree[i] = A[i - leftmost];
		mTree[i] = m[i - leftmost];
	}
	for (int i = N; i < leftmost; i++) { // leaves on second lowest layer
		ATree[i] = A[i + N - leftmost];
		mTree[i] = m[i + N - leftmost];
	}

	// Calculate the rest of the product tree mTree
	for (int i = N - 1; i > 0; i--) {
		mTree[i] = mTree[2 * i] * mTree[2 * i + 1]; // parent is product of leaves
	}

	// Calculate the rest of the product tree aTree, taking mod mTree[1] = m[0]*...*m[N-1]
	for(int i = N - 1; i > 0; i--) {
		if ((i & (i+1)) != 0) { // Don't do calculation if on a node in right-most branch
			ATree[i] = (ATree[2 * i] * ATree[2 * i + 1]) % mTree[1]; // parent is product of leaves mod mTree[1]
		}
	}

	// Calculate accumulating remainder tree
	CTree[1] = 1;
	for (int i = 1; i < N; i++) {
		CTree[2 * i] = CTree[i] % mTree[2 * i]; // Left branch
		CTree[2 * i + 1] = (CTree[i] * ATree[2 * i]) % mTree[2 * i + 1]; // Right branch
	}

	//print_tree(ATree);
	//print_tree(mTree);
	//print_tree(CTree);

	for (int i = leftmost; i < 2 * N; i++) {
		C[i - leftmost] = CTree[i];
	}
	for (int i = N; i < leftmost; i++) {
		C[i + N - leftmost] = CTree[i];
	}

	return;
}

/*
 * Prints a tree given in Vec<ZZ> form
 */
void print_tree(Vec<ZZ> &tree){
	int top = 1;
	int counter = 0;
	for(int i = 1; i < tree.length(); i++){
		cout << tree[i] << " ";
		counter++;
		if (counter == top){
			cout << endl;
			counter = 0;
			top *= 2;
		}
	}
	cout << endl;
}

/*
 * Gives data points on size of input vs. computation time.
 * N = max size of data, d = number of data points
 */

void complexity_graph(int N, int d){
	vector<int> x;
	vector<int> y;

	int interval = N/d;
	int B = 0;
	while(B <= N){

		int testSize = B;
		int numSize = B;

		Vec<ZZ> test_A;
		test_A.SetLength(testSize);
		Vec<ZZ> test_m;
		test_m.SetLength(testSize);
		for (int i = 0; i < testSize; i++) {
			test_A[i] = rand() % numSize + 1;
			test_m[i] = rand() % numSize + 1;
		}
		/*
		for (int i = 0; i < testSize; i++) {
			cout << test_A[i] << " ";
		}
		cout << endl;

		for (int i = 0; i < testSize; i++) {
			cout << test_m[i] << " ";
		}
		cout << endl;
		*/

		uint64_t start = duration_cast<milliseconds>(system_clock::now().time_since_epoch()).count();
		Vec<ZZ> test_C;
		test_C.SetLength(testSize);
		remainder_tree(test_C, test_A, test_m);

		/*for (int i = 0; i < testSize; i++) {
			cout << test_C[i] << " ";
		}
		cout << endl;
		*/

		uint64_t end = duration_cast<milliseconds>(system_clock::now().time_since_epoch()).count();

		x.push_back(B);
		y.push_back(end-start);

		B += interval;
	}

	for(int i = 0; i < x.size(); i++){
		cout << x[i] << ", ";
	}
	cout << endl;

	for(int i = 0; i < y.size(); i++){
		cout << y[i] << ", ";
	}
	cout << endl;

}

// Run program: Ctrl + F5 or Debug > Start Without Debugging menu
// Debug program: F5 or Debug > Start Debugging menu

// Tips for Getting Started:
//   1. Use the Solution Explorer window to add/manage files
//   2. Use the Team Explorer window to connect to source control
//   3. Use the Output window to see build output and other messages
//   4. Use the Error List window to view errors
//   5. Go to Project > Add New Item to create new code files, or Project > Add Existing Item to add existing code files to the project
//   6. In the future, to open this project again, go to File > Open > Project and select the .sln file


/******************************************************************************\
* Author: Matthew Beauregard Smith                                             *
* Affiliation: The University of Texas at Austin                               *
* Department: Oden Institute and Institute for Cellular and Molecular Biology  *
* PI: Edward Marcotte                                                          *
* Project: Protein Fluorosequencing                                            *
\******************************************************************************/

// Boost unit test framework (recommended to be the first include):
#include <boost/test/unit_test.hpp>

// File under test:
#include "log-normal-distribution-fitter.h"

// Standard C++ library headers:
#include <cmath>

// Local project headers:
#include "common/error-model.h"

namespace whatprot {

namespace {
using boost::unit_test::tolerance;
using std::log;
using std::sqrt;
const double TOL = 0.000000001;
}  // namespace

BOOST_AUTO_TEST_SUITE(hmm_suite)
BOOST_AUTO_TEST_SUITE(fit_suite)
BOOST_AUTO_TEST_SUITE(log_normal_distribution_fitter_suite)

BOOST_AUTO_TEST_CASE(constructor_test, *tolerance(TOL)) {
    LogNormalDistributionFitter lndf;
    BOOST_TEST(lndf.w_sum_log_x_over_n == 0.0);
    BOOST_TEST(lndf.w_sum_log_x_over_n_sq == 0.0);
    BOOST_TEST(lndf.total_weight == 0.0);
}

BOOST_AUTO_TEST_CASE(add_sample_once_n_eq_0_test, *tolerance(TOL)) {
    LogNormalDistributionFitter lndf;
    double x = 1.277;
    int n = 0;
    double w = 0.98;
    lndf.add_sample(x, n, w);
    BOOST_TEST(lndf.w_sum_log_x_over_n == 0.0);
    BOOST_TEST(lndf.w_sum_log_x_over_n_sq == 0.0);
    BOOST_TEST(lndf.total_weight == 0.0);
}

BOOST_AUTO_TEST_CASE(add_sample_once_x_eq_0_test, *tolerance(TOL)) {
    LogNormalDistributionFitter lndf;
    double x = 0.0;
    int n = 1;
    double w = 0.98;
    lndf.add_sample(x, n, w);
    BOOST_TEST(lndf.w_sum_log_x_over_n == 0.0);
    BOOST_TEST(lndf.w_sum_log_x_over_n_sq == 0.0);
    BOOST_TEST(lndf.total_weight == 0.0);
}

BOOST_AUTO_TEST_CASE(add_sample_once_n_eq_1_test, *tolerance(TOL)) {
    LogNormalDistributionFitter lndf;
    double x = 1.277;
    int n = 1;
    double w = 0.98;
    lndf.add_sample(x, n, w);
    BOOST_TEST(lndf.w_sum_log_x_over_n == log(x / (double)n) * w);
    BOOST_TEST(lndf.w_sum_log_x_over_n_sq
               == log(x / (double)n) * log(x / (double)n) * w);
    BOOST_TEST(lndf.total_weight == w);
}

BOOST_AUTO_TEST_CASE(add_sample_once_n_gt_1_test, *tolerance(TOL)) {
    LogNormalDistributionFitter lndf;
    double x = 3.1415928;
    int n = 3;
    double w = 0.979;
    lndf.add_sample(x, n, w);
    BOOST_TEST(lndf.w_sum_log_x_over_n == log(x / (double)n) * w);
    BOOST_TEST(lndf.w_sum_log_x_over_n_sq
               == log(x / (double)n) * log(x / (double)n) * w);
    BOOST_TEST(lndf.total_weight == w);
}

BOOST_AUTO_TEST_CASE(add_sample_twice_n_eq_1_test, *tolerance(TOL)) {
    LogNormalDistributionFitter lndf;
    double x1 = 1.277;
    int n1 = 1;
    double w1 = 0.98;
    double x2 = 1.166;
    int n2 = 1;
    double w2 = 0.49;
    lndf.add_sample(x1, n1, w1);
    lndf.add_sample(x2, n2, w2);
    BOOST_TEST(lndf.w_sum_log_x_over_n
               == log(x1 / (double)n1) * w1 + log(x2 / (double)n2) * w2);
    BOOST_TEST(lndf.w_sum_log_x_over_n_sq
               == log(x1 / (double)n1) * log(x1 / (double)n1) * w1
                          + log(x2 / (double)n2) * log(x2 / (double)n2) * w2);
    BOOST_TEST(lndf.total_weight == w1 + w2);
}

BOOST_AUTO_TEST_CASE(add_sample_twice_n_gt_1_test, *tolerance(TOL)) {
    LogNormalDistributionFitter lndf;
    double x1 = 3.43;
    int n1 = 3;
    double w1 = 0.98;
    double x2 = 4.91;
    int n2 = 5;
    double w2 = 0.49;
    lndf.add_sample(x1, n1, w1);
    lndf.add_sample(x2, n2, w2);
    BOOST_TEST(lndf.w_sum_log_x_over_n
               == log(x1 / (double)n1) * w1 + log(x2 / (double)n2) * w2);
    BOOST_TEST(lndf.w_sum_log_x_over_n_sq
               == log(x1 / (double)n1) * log(x1 / (double)n1) * w1
                          + log(x2 / (double)n2) * log(x2 / (double)n2) * w2);
    BOOST_TEST(lndf.total_weight == w1 + w2);
}

BOOST_AUTO_TEST_CASE(get_type_test, *tolerance(TOL)) {
    LogNormalDistributionFitter lndf;
    BOOST_TEST(lndf.get_type() == DistributionType::LOGNORMAL);
}

BOOST_AUTO_TEST_CASE(get_mu_one_sample_test, *tolerance(TOL)) {
    LogNormalDistributionFitter lndf;
    double x1 = 3.43;
    int n1 = 3;
    double w1 = 0.98;
    lndf.add_sample(x1, n1, w1);
    BOOST_TEST(lndf.get_mu() == log(x1 / (double)n1));
}

BOOST_AUTO_TEST_CASE(get_mu_two_samples_test, *tolerance(TOL)) {
    LogNormalDistributionFitter lndf;
    double x1 = 3.43;
    int n1 = 3;
    double w1 = 0.98;
    double x2 = 4.91;
    int n2 = 5;
    double w2 = 0.49;
    lndf.add_sample(x1, n1, w1);
    lndf.add_sample(x2, n2, w2);
    BOOST_TEST(lndf.get_mu()
               == (log(x1 / (double)n1) * w1 + log(x2 / (double)n2) * w2)
                          / (w1 + w2));
}

BOOST_AUTO_TEST_CASE(get_sigma_one_sample_test, *tolerance(TOL)) {
    LogNormalDistributionFitter lndf;
    double x1 = 3.43;
    int n1 = 3;
    double w1 = 0.98;
    lndf.add_sample(x1, n1, w1);
    double mu = lndf.get_mu();
    BOOST_TEST(lndf.get_sigma()
               == sqrt((log(x1 / n1) - mu) * (log(x1 / n1) - mu)));
}

BOOST_AUTO_TEST_CASE(get_sigma_two_samples_test, *tolerance(TOL)) {
    LogNormalDistributionFitter lndf;
    double x1 = 3.43;
    int n1 = 3;
    double w1 = 0.98;
    double x2 = 4.91;
    int n2 = 5;
    double w2 = 0.49;
    lndf.add_sample(x1, n1, w1);
    lndf.add_sample(x2, n2, w2);
    double mu = lndf.get_mu();
    BOOST_TEST(lndf.get_sigma()
               == sqrt(((log(x1 / n1) - mu) * (log(x1 / n1) - mu) * w1
                        + (log(x2 / n2) - mu) * (log(x2 / n2) - mu) * w2)
                       / (w1 + w2)));
}

BOOST_AUTO_TEST_SUITE_END()  // log_normal_distribution_fitter_suite
BOOST_AUTO_TEST_SUITE_END()  // fit_suite
BOOST_AUTO_TEST_SUITE_END()  // hmm_suite

}  // namespace whatprot


/*!
 * @file dirichlet_bc.hpp
 * @brief Contains implementation of Dirichlet boundary conditions.
 * @author Konrad Simon
 * @date August 2019
 */

#ifndef INCLUDE_DIRICHLET_BC_HPP_
#define INCLUDE_DIRICHLET_BC_HPP_

// Deal.ii
#include <deal.II/base/function.h>

// STL
#include <cmath>
#include <fstream>

// My Headers
#include "coefficients.h"

namespace Coefficients
{
using namespace dealii;

/*!
 * @class DirichletBC
 * @brief Class implements scalar Dirichlet conditions.
 */
template <int dim>
class DirichletBC : public Function<dim>
{
public:
	DirichletBC() : Function<dim>() {}

	virtual double value(const Point<dim> &p,
						const unsigned int component = 0) const override;
	virtual void value_list(const std::vector<Point<dim>> &points,
								std::vector<double>  &values,
								const unsigned int component = 0) const override;
};

template <int dim>
double
DirichletBC<dim>::value(const Point<dim>& p,
							   const unsigned int /*component*/) const
{
	double return_value = (p(0)-0.5) * (p(0)-0.5) + (p(1)-0.5) * (p(1)-0.5);

	return return_value;
}

template <int dim>
void
DirichletBC<dim>::value_list(const std::vector<Point<dim>> &points,
								std::vector<double>  &values,
								const unsigned int /*component = 0*/) const
{
	Assert (points.size() == values.size(),
			ExcDimensionMismatch (points.size(), values.size()) );

	for ( unsigned int p=0; p<points.size(); ++p)
	{
		values[p] = (points[p](0)-0.5) * (points[p](0)-0.5) + (points[p](1)-0.5) * (points[p](1)-0.5);
	} // end ++p
}

} // end namespace Coefficients

#endif /* INCLUDE_DIRICHLET_BC_HPP_ */


//
// Copyright (c) 2019 INRIA
//

#include <pinocchio/autodiff/casadi.hpp>

#include <pinocchio/math/quaternion.hpp>
#include <pinocchio/spatial/se3.hpp>
#include <pinocchio/spatial/motion.hpp>

#include <boost/variant.hpp> // to avoid C99 warnings

#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>

BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)

BOOST_AUTO_TEST_CASE(test_se3)
{
  typedef pinocchio::SE3Tpl<casadi::SX> SE3;
  SE3 M1 = SE3::Identity();
  SE3 M2 = SE3::Random();

  SE3 M3 = M2 * M1;
  SE3 M1inv = M1.inverse();
}

BOOST_AUTO_TEST_CASE(test_motion)
{
  typedef pinocchio::MotionTpl<casadi::SX> Motion;
  Motion v1 = Motion::Zero();
  Motion v2 = Motion::Random();

  Motion v3 = v1 + v2;
}

BOOST_AUTO_TEST_CASE(test_quaternion)
{
  typedef pinocchio::SE3Tpl<casadi::SX> SE3AD;
  typedef pinocchio::SE3 SE3;

  SE3AD ad_M;
  SE3AD::Matrix3 & ad_rot = ad_M.rotation();

  casadi::SX cs_rot = casadi::SX::sym("rot", 3,3);
  pinocchio::casadi::copy(cs_rot,ad_rot);

  SE3AD::Quaternion ad_quat;
  pinocchio::quaternion::assignQuaternion(ad_quat,ad_rot);

  casadi::SX cs_quat(4,1);
  pinocchio::casadi::copy(ad_quat.coeffs(),cs_quat);

  casadi::Function eval_quat("eval_quat",
                             casadi::SXVector {cs_rot},
                             casadi::SXVector {cs_quat});

  for(int k = 0; k < 1e4; ++k)
  {
    SE3 M(SE3::Random());
    SE3::Quaternion quat_ref(M.rotation());

    casadi::DM vec_rot(3,3);
    pinocchio::casadi::copy(M.rotation(),vec_rot);

    casadi::DM quat_res = eval_quat(casadi::DMVector {vec_rot})[0];
    SE3::Quaternion quat_value;

    quat_value.coeffs() = Eigen::Map<Eigen::Vector4d>(static_cast< std::vector<double> >(quat_res).data());
    BOOST_CHECK(pinocchio::quaternion::defineSameRotation(quat_value,quat_ref));
//    if(not quat_value.coeffs().isApprox(quat_ref.coeffs()))
//    {
//      std::cout << "quat_value: " << quat_value.coeffs().transpose() << std::endl;
//      std::cout << "quat_ref: " << quat_ref.coeffs().transpose() << std::endl;
//    }
  }

}

BOOST_AUTO_TEST_SUITE_END()


#define BOOST_TEST_MAIN
#include <boost/test/included/unit_test.hpp>
#include <srook/tmpl/vt/concat.hpp>
#include <srook/tmpl/vt/foldl.hpp>
#include <srook/tmpl/vt/foldr.hpp>
#include <srook/tmpl/vt/reverse.hpp>
#include <srook/tmpl/vt/max.hpp>
#include <srook/tmpl/vt/filterD.hpp>
#include <srook/tmpl/vt/bind.hpp>
#include <srook/tmpl/vt/tt_proxy.hpp>
#include <srook/tmpl/vt/sort/qsort.hpp>
#include <srook/type_traits/is_same.hpp>
#include <srook/cstdint.hpp>

BOOST_AUTO_TEST_SUITE(srook_tmpl_vt_fold_test)

template <class L, class... R>
struct snoc : srook::tmpl::vt::concat<R..., L> {};

template <class Li, class X>
struct f
    : srook::tmpl::vt::concat<
        SROOK_DEDUCED_TYPENAME srook::tmpl::vt::concat<
            SROOK_DEDUCED_TYPENAME srook::tmpl::vt::filterD<srook::tmpl::vt::bind<srook::tmpl::vt::tt_proxy<srook::tmpl::vt::lt>::template type, srook::tmpl::vt::placeholders::_1, X>, Li>::type, X
        >::type,
        SROOK_DEDUCED_TYPENAME srook::tmpl::vt::filterD<srook::tmpl::vt::bind<srook::tmpl::vt::geq, srook::tmpl::vt::placeholders::_1, X>, Li>::type
    > {};

BOOST_AUTO_TEST_CASE(fold_test1)
{
    typedef srook::tmpl::vt::packer<srook::uint32_t, srook::uint8_t, srook::uint64_t, srook::uint16_t> type;

    // reverse
    typedef SROOK_DEDUCED_TYPENAME srook::tmpl::vt::foldr<snoc, srook::tmpl::vt::packer<>, type>::type rev;
    SROOK_ST_ASSERT(srook::is_same<SROOK_DEDUCED_TYPENAME srook::tmpl::vt::reverse<type>::type, rev>::value);

    // deriving the maximum type where `maximum` is defined by <srook/tmpl/vt/compare.hpp>
    typedef SROOK_DEDUCED_TYPENAME srook::tmpl::vt::foldr<srook::tmpl::vt::max, srook::tmpl::vt::packer<>, type>::type maxi;
    SROOK_ST_ASSERT(srook::is_same<srook::uint64_t, maxi>::value);

    // insertion sort with foldl
    typedef SROOK_DEDUCED_TYPENAME srook::tmpl::vt::foldl<f, srook::tmpl::vt::packer<>, type>::type isorted;
    SROOK_ST_ASSERT(srook::is_same<SROOK_DEDUCED_TYPENAME srook::tmpl::vt::qsort<type>::type, isorted>::value);
}

BOOST_AUTO_TEST_SUITE_END()


#ifndef __KALMANFILTER_HPP__
#define __KALMANFILTER_HPP__

#include <Eigen/Dense>
#include <utility>

class KalmanFilter {
 public:
 	struct KalmanState {
 		Eigen::VectorXd state;
 		Eigen::MatrixXd errorCovariance;
 	};

 	KalmanFilter(
    const int pStateDimensionality,
    const int pMeasurementDimensionality);

  void setNaturalModel(const Eigen::MatrixXd& pNewModel);
  void setControlModel(const Eigen::MatrixXd& pNewModel);
  void setTransitionModel(const Eigen::MatrixXd& pNewModel);
  void setStateNoiseCovariance(const Eigen::MatrixXd& pNewCovariance);
  void setMeasurementNoiseCovariance(const Eigen::MatrixXd& pNewCovariance);

	KalmanState KalmanFilterIteration(
		const KalmanState& pPreviousState,
		const Eigen::MatrixXd& pMeasurementVector,
		const Eigen::VectorXd& pControlVector);

 	Eigen::MatrixXd computeStatePrediction(
 		const Eigen::VectorXd& pPreviousState,
 		const Eigen::VectorXd& pControlVector);

 	Eigen::MatrixXd computeObservationPrediction(
 		const Eigen::VectorXd& pStatePrediction);

 	Eigen::MatrixXd computeErrorCovariancePrediction(
 		const Eigen::MatrixXd& pPreviousPredictionCovariance);

 	Eigen::MatrixXd computeKalmanGainFactor(
 		const Eigen::MatrixXd& pPredictionCovarianceEstimate);

 	Eigen::MatrixXd computeStateEstimate(
 		const Eigen::VectorXd& pStatePrediction,
 		const Eigen::MatrixXd& pKalmanGainFactor,
 		const Eigen::VectorXd& pMeasurementVector,
 		const Eigen::VectorXd& pObservationPrediction);

 	Eigen::MatrixXd computeErrorCovariance(
 		const Eigen::MatrixXd& pPredictionCovarianceEstimate,
 		const Eigen::MatrixXd& pKalmanGainFactor);

 private:
  int mStateDimensionality;       // aka n
  int mMeasurementDimensionality; // aka m
  Eigen::MatrixXd mNaturalModel;  // aka A; n x n; describes how the system
                                  // evolves naturally, i.e. without controls
                                  // or noise
  Eigen::MatrixXd mControlModel;  // aka B; n x n; describes how controls alter
                                  // the system
  Eigen::MatrixXd mTransitionModel; // aka H; m x n; describes how to map from
                                    // a state to an observation
  Eigen::VectorXd mStateNoise;    // aka \epsilon; n x 1; has covariance Q
  Eigen::MatrixXd mStateNoiseCovariance;  // aka Q;
  Eigen::VectorXd mMeasurementNoise; // aka \sigma; m x 1; has covariance R
  Eigen::MatrixXd mMeasurementNoiseCovariance;  // aka R; m x m
};

#endif //__KALMANFILTER_HPP__


// Boost.Geometry

// Copyright (c) 2015-2017 Oracle and/or its affiliates.

// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GEOMETRY_FORMULAS_THOMAS_INVERSE_HPP
#define BOOST_GEOMETRY_FORMULAS_THOMAS_INVERSE_HPP

#include <boost/math/constants/constants.hpp>

#include <boost/geometry/core/radius.hpp>

#include <boost/geometry/util/condition.hpp>
#include <boost/geometry/util/math.hpp>

#include <boost/geometry/formulas/differential_quantities.hpp>
#include <boost/geometry/formulas/flattening.hpp>
#include <boost/geometry/formulas/result_inverse.hpp>

namespace boost { namespace geometry { namespace formula
{

/*!
\brief The solution of the inverse problem of geodesics on latlong coordinates,
       Forsyth-Andoyer-Lambert type approximation with second order terms.
\author See
    - Technical Report: PAUL D. THOMAS, MATHEMATICAL MODELS FOR NAVIGATION SYSTEMS, 1965
      http://www.dtic.mil/docs/citations/AD0627893
    - Technical Report: PAUL D. THOMAS, SPHEROIDAL GEODESICS, REFERENCE SYSTEMS, AND LOCAL GEOMETRY, 1970
      http://www.dtic.mil/docs/citations/AD0703541
*/
template <
    typename CT,
    bool EnableDistance,
    bool EnableAzimuth,
    bool EnableReverseAzimuth = false,
    bool EnableReducedLength = false,
    bool EnableGeodesicScale = false
>
class thomas_inverse
{
    static const bool CalcQuantities = EnableReducedLength || EnableGeodesicScale;
    static const bool CalcAzimuths = EnableAzimuth || EnableReverseAzimuth || CalcQuantities;
    static const bool CalcFwdAzimuth = EnableAzimuth || CalcQuantities;
    static const bool CalcRevAzimuth = EnableReverseAzimuth || CalcQuantities;

public:
    typedef result_inverse<CT> result_type;

    template <typename T1, typename T2, typename Spheroid>
    static inline result_type apply(T1 const& lon1,
                                    T1 const& lat1,
                                    T2 const& lon2,
                                    T2 const& lat2,
                                    Spheroid const& spheroid)
    {
        result_type result;

        // coordinates in radians

        if ( math::equals(lon1, lon2) && math::equals(lat1, lat2) )
        {
            return result;
        }

        CT const c0 = 0;
        CT const c1 = 1;
        CT const c2 = 2;
        CT const c4 = 4;

        CT const pi_half = math::pi<CT>() / c2;
        CT const f = formula::flattening<CT>(spheroid);
        CT const one_minus_f = c1 - f;

//        CT const tan_theta1 = one_minus_f * tan(lat1);
//        CT const tan_theta2 = one_minus_f * tan(lat2);
//        CT const theta1 = atan(tan_theta1);
//        CT const theta2 = atan(tan_theta2);

        CT const theta1 = math::equals(lat1, pi_half) ? lat1 :
                          math::equals(lat1, -pi_half) ? lat1 :
                          atan(one_minus_f * tan(lat1));
        CT const theta2 = math::equals(lat2, pi_half) ? lat2 :
                          math::equals(lat2, -pi_half) ? lat2 :
                          atan(one_minus_f * tan(lat2));

        CT const theta_m = (theta1 + theta2) / c2;
        CT const d_theta_m = (theta2 - theta1) / c2;
        CT const d_lambda = lon2 - lon1;
        CT const d_lambda_m = d_lambda / c2;

        CT const sin_theta_m = sin(theta_m);
        CT const cos_theta_m = cos(theta_m);
        CT const sin_d_theta_m = sin(d_theta_m);
        CT const cos_d_theta_m = cos(d_theta_m);
        CT const sin2_theta_m = math::sqr(sin_theta_m);
        CT const cos2_theta_m = math::sqr(cos_theta_m);
        CT const sin2_d_theta_m = math::sqr(sin_d_theta_m);
        CT const cos2_d_theta_m = math::sqr(cos_d_theta_m);
        CT const sin_d_lambda_m = sin(d_lambda_m);
        CT const sin2_d_lambda_m = math::sqr(sin_d_lambda_m);

        CT const H = cos2_theta_m - sin2_d_theta_m;
        CT const L = sin2_d_theta_m + H * sin2_d_lambda_m;
        CT const cos_d = c1 - c2 * L;
        CT const d = acos(cos_d);
        CT const sin_d = sin(d);

        CT const one_minus_L = c1 - L;

        if ( math::equals(sin_d, c0)
          || math::equals(L, c0)
          || math::equals(one_minus_L, c0) )
        {
            return result;
        }

        CT const U = c2 * sin2_theta_m * cos2_d_theta_m / one_minus_L;
        CT const V = c2 * sin2_d_theta_m * cos2_theta_m / L;
        CT const X = U + V;
        CT const Y = U - V;
        CT const T = d / sin_d;
        CT const D = c4 * math::sqr(T);
        CT const E = c2 * cos_d;
        CT const A = D * E;
        CT const B = c2 * D;
        CT const C = T - (A - E) / c2;

        CT const f_sqr = math::sqr(f);
        CT const f_sqr_per_64 = f_sqr / CT(64);

        if ( BOOST_GEOMETRY_CONDITION(EnableDistance) )
        {
            CT const n1 = X * (A + C*X);
            CT const n2 = Y * (B + E*Y);
            CT const n3 = D*X*Y;

            CT const delta1d = f * (T*X-Y) / c4;
            CT const delta2d = f_sqr_per_64 * (n1 - n2 + n3);

            CT const a = get_radius<0>(spheroid);

            //result.distance = a * sin_d * (T - delta1d);
            result.distance = a * sin_d * (T - delta1d + delta2d);
        }

        if ( BOOST_GEOMETRY_CONDITION(CalcAzimuths) )
        {
            // NOTE: if both cos_latX == 0 then below we'd have 0 * INF
            // it's a situation when the endpoints are on the poles +-90 deg
            // in this case the azimuth could either be 0 or +-pi
            // but above always 0 is returned

            CT const F = c2*Y-E*(c4-X);
            CT const M = CT(32)*T-(CT(20)*T-A)*X-(B+c4)*Y;
            CT const G = f*T/c2 + f_sqr_per_64 * M;

            // TODO:
            // If d_lambda is close to 90 or -90 deg then tan(d_lambda) is big
            // and F is small. The result is not accurate.
            // In the edge case the result may be 2 orders of magnitude less
            // accurate than Andoyer's.
            CT const tan_d_lambda = tan(d_lambda);
            CT const Q = -(F*G*tan_d_lambda) / c4;
            CT const d_lambda_m_p = (d_lambda + Q) / c2;
            CT const tan_d_lambda_m_p = tan(d_lambda_m_p);

            CT const v = atan2(cos_d_theta_m, sin_theta_m * tan_d_lambda_m_p);
            CT const u = atan2(-sin_d_theta_m, cos_theta_m * tan_d_lambda_m_p);

            CT const pi = math::pi<CT>();

            if (BOOST_GEOMETRY_CONDITION(EnableAzimuth))
            {
                CT alpha1 = v + u;
                if (alpha1 > pi)
                {
                    alpha1 -= c2 * pi;
                }

                result.azimuth = alpha1;
            }

            if (BOOST_GEOMETRY_CONDITION(EnableReverseAzimuth))
            {
                CT alpha2 = pi - (v - u);
                if (alpha2 > pi)
                {
                    alpha2 -= c2 * pi;
                }

                result.reverse_azimuth = alpha2;
            }
        }

        if (BOOST_GEOMETRY_CONDITION(CalcQuantities))
        {
            typedef differential_quantities<CT, EnableReducedLength, EnableGeodesicScale, 2> quantities;
            quantities::apply(lon1, lat1, lon2, lat2,
                              result.azimuth, result.reverse_azimuth,
                              get_radius<2>(spheroid), f,
                              result.reduced_length, result.geodesic_scale);
        }

        return result;
    }
};

}}} // namespace boost::geometry::formula

#endif // BOOST_GEOMETRY_FORMULAS_THOMAS_INVERSE_HPP


/*
Part of the Fluid Corpus Manipulation Project (http://www.flucoma.org/)
Copyright 2017-2019 University of Huddersfield.
Licensed under the BSD-3 License.
See license.md file in the project root for full license information.
This project has received funding from the European Research Council (ERC)
under the European Union’s Horizon 2020 research and innovation programme
(grant agreement No 725899).
*/

#pragma once

#include "DCT.hpp"
#include "../util/AlgorithmUtils.hpp"
#include "../util/FluidEigenMappings.hpp"
#include "../util/PeakDetection.hpp"
#include "../../data/FluidIndex.hpp"
#include "../../data/TensorTypes.hpp"
#include <Eigen/Eigen>
#include <cmath>

namespace fluid {
namespace algorithm {

class CepstrumF0
{

public:
  using ArrayXd = Eigen::ArrayXd;

  CepstrumF0(index maxSize) : mCepstrumStorage(maxSize) {}

  void init(index size)
  {
    // avoid allocation of maxSize^2 at constructor
    mDCT = DCT(size, size);
    mDCT.init(size, size);

    mCepstrum = mCepstrumStorage.segment(0, size);
    mCepstrum.setZero();
  }

  void processFrame(const RealVectorView& input, RealVectorView output,
                    double minFreq, double maxFreq, double sampleRate)
  {
    using namespace Eigen;
    using namespace std;
    PeakDetection pd;

    ArrayXd mag = _impl::asEigen<Array>(input);
    ArrayXd logMag = mag.max(epsilon).log();
    double  pitch = 0;
    double  confidence = 0;
    index   minBin = min<index>(lrint(sampleRate / maxFreq), mag.size());
    index   maxBin = min<index>(lrint(sampleRate / minFreq), mag.size());

    mDCT.processFrame(logMag, mCepstrum);

    if (maxBin > minBin)
    {
      auto seg = mCepstrum.segment(minBin, maxBin - minBin);
      auto vec = pd.process(mCepstrum.segment(minBin, maxBin - minBin), 1,
                            seg.minCoeff());
      if (vec.size() > 0)
      {
        pitch = sampleRate / (vec[0].first + minBin);
        confidence = vec[0].second / mCepstrum[0];
      }
    }
    output(0) = pitch;
    output(1) = min(abs(confidence), 1.0);
  }

private:
  DCT     mDCT{0, 0};
  ArrayXd mCepstrumStorage;
  ArrayXd mCepstrum;
};
} // namespace algorithm
} // namespace fluid


/**
 * @file stableevaluationatapoint.cc
 * @brief NPDE homework StableEvaluationAtAPoint
 * @author Amélie Loher, Erick Schulz & Philippe Peter
 * @date 29.11.2021
 * @copyright Developed at ETH Zurich
 */

#include "stableevaluationatapoint.h"

#include <lf/base/base.h>
#include <lf/fe/fe.h>
#include <lf/geometry/geometry.h>
#include <lf/mesh/mesh.h>
#include <lf/quad/quad.h>
#include <lf/uscalfe/uscalfe.h>

#include <Eigen/Core>
#include <algorithm>
#include <cmath>
#include <iostream>
#include <memory>

namespace StableEvaluationAtAPoint {

double MeshSize(const std::shared_ptr<const lf::mesh::Mesh> &mesh_p) {
  double mesh_size = 0.0;
  // Find maximal edge length
  for (const lf::mesh::Entity *edge : mesh_p->Entities(1)) {
    // Compute the length of the edge
    double edge_length = lf::geometry::Volume(*(edge->Geometry()));
    mesh_size = std::max(edge_length, mesh_size);
  }
  return mesh_size;
}

Eigen::Vector2d OuterNormalUnitSquare(Eigen::Vector2d x) {
  // Use shortcut: x is on the unit square
  if (x(0) > x(1) && x(0) < 1.0 - x(1)) {
    return Eigen::Vector2d(0.0, -1.0);
  }
  if (x(0) > x(1) && x(0) > 1.0 - x(1)) {
    return Eigen::Vector2d(1.0, 0.0);
  }
  if (x(0) < x(1) && x(0) > 1.0 - x(1)) {
    return Eigen::Vector2d(0.0, 1.0);
  }
  return Eigen::Vector2d(-1.0, 0.0);
}

double FundamentalSolution::operator()(Eigen::Vector2d y) {
  LF_ASSERT_MSG(x_ != y, "G not defined for these coordinates!");
  return -1.0 / (2.0 * M_PI) * std::log((x_ - y).norm());
}

Eigen::Vector2d FundamentalSolution::grad(Eigen::Vector2d y) {
  LF_ASSERT_MSG(x_ != y, "G not defined for these coordinates!");
  return (x_ - y) / (2.0 * M_PI * (x_ - y).squaredNorm());
}

double PointEval(std::shared_ptr<const lf::mesh::Mesh> mesh_p) {
  double error = 0.0;
#if SOLUTION
  const auto u = [](Eigen::Vector2d x) -> double {
    Eigen::Vector2d one(1.0, 0.0);
    return std::log((x + one).norm());
  };
  const auto gradu = [](Eigen::Vector2d x) -> Eigen::Vector2d {
    Eigen::Vector2d one(1.0, 0.0);
    return (x + one) / (x + one).squaredNorm();
  };
  // Define a Functor for the dot product of grad u(x) * n(x)
  const auto gradu_dot_n = [gradu](const Eigen::Vector2d x) -> double {
    // Determine the normal vector n on the unit square
    Eigen::Vector2d n = OuterNormalUnitSquare(x);
    return gradu(x).dot(n);
  };

  // Compute right hand side
  const Eigen::Vector2d x(0.3, 0.4);
  const double rhs = PSL(mesh_p, gradu_dot_n, x) - PDL(mesh_p, u, x);
  // Compute the error
  error = std::abs(u(x) - rhs);
#else
  //====================
  // Your code goes here
  //====================
#endif
  return error;
}

double Psi::operator()(Eigen::Vector2d y) {
  const double c = M_PI / (0.5 * std::sqrt(2) - 1.0);
  const double dist = (y - center_).norm();

  if (dist <= 0.25 * std::sqrt(2)) {
    return 0.0;
  } else if (dist >= 0.5) {
    return 1.0;
  } else {
    return std::pow(std::cos(c * (dist - 0.5)), 2);
  }
}

Eigen::Vector2d Psi::grad(Eigen::Vector2d y) {
  double c = M_PI / (0.5 * std::sqrt(2) - 1.0);
  double dist = (y - center_).norm();

  if (dist <= 0.25 * std::sqrt(2)) {
    return Eigen::Vector2d(0.0, 0.0);

  } else if (dist >= 0.5) {
    return Eigen::Vector2d(0.0, 0.0);
  } else {
    return -2.0 * std::cos(c * (dist - 0.5)) * std::sin(c * (dist - 0.5)) *
           (c / dist) * (y - center_);
  }
}

double Psi::lapl(Eigen::Vector2d y) {
  double c = M_PI / (0.5 * std::sqrt(2) - 1.0);
  double c2 = c * c;
  double dist = (y - center_).norm();

  if (dist <= 0.25 * std::sqrt(2)) {
    return 0.0;
  } else if (dist >= 0.5) {
    return 0.0;
  } else {
    double sineval = std::sin(c * (dist - 0.5));
    double coseval = std::cos(c * (dist - 0.5));
    return 2 * c2 * sineval * sineval - 2 * c2 * coseval * coseval -
           2 * c * sineval * coseval / dist;
  }
}

double Jstar(std::shared_ptr<lf::uscalfe::FeSpaceLagrangeO1<double>> fe_space,
             Eigen::VectorXd uFE, const Eigen::Vector2d x) {
  double val = 0.0;
  Psi psi(Eigen::Vector2d(0.5, 0.5));
  FundamentalSolution G(x);
#if SOLUTION

  // Mesh covering a unit square domain
  std::shared_ptr<const lf::mesh::Mesh> mesh = fe_space->Mesh();
  // Use midpoint quadrature rule
  const lf::quad::QuadRule qr = lf::quad::make_TriaQR_MidpointRule();
  // Quadrature points
  const Eigen::MatrixXd zeta_ref{qr.Points()};
  // Quadrature weights
  const Eigen::VectorXd w_ref{qr.Weights()};
  // Number of quadrature points
  const lf::base::size_type P = qr.NumPoints();
  // Create mesh function to be evaluated at the quadrature points
  auto uFE_mf = lf::fe::MeshFunctionFE(fe_space, uFE);

  // Loop over all cells
  for (const lf::mesh::Entity *entity : mesh->Entities(0)) {
    // Standard way to apply a local quadrature rule
    const lf::geometry::Geometry &geo{*entity->Geometry()};
    // Quadrature points on actual cell
    const Eigen::MatrixXd zeta{geo.Global(zeta_ref)};
    const Eigen::VectorXd gram_dets{geo.IntegrationElement(zeta_ref)};
    // Values of finite element function on all quadrature points
    auto u_vals = uFE_mf(*entity, zeta_ref);

    // Quadrature loop
    for (int l = 0; l < P; l++) {
      const double w = w_ref[l] * gram_dets[l];
      val += w * (-u_vals[l]) *
             (2.0 * (G.grad(zeta.col(l))).dot(psi.grad(zeta.col(l))) +
              G(zeta.col(l)) * psi.lapl(zeta.col(l)));
    }
  }
#else
  //====================
  // Your code goes here
  //====================
#endif
  return val;
}

double StablePointEvaluation(
    std::shared_ptr<lf::uscalfe::FeSpaceLagrangeO1<double>> fe_space,
    Eigen::VectorXd uFE, const Eigen::Vector2d x) {
  double res = 0.0;

  Eigen::Vector2d center(0.5, 0.5);
  if ((x - center).norm() <= 0.25) {
    res = Jstar(fe_space, uFE, x);
  } else {
    std::cerr << "The point does not fulfill the assumptions" << std::endl;
  }

  return res;
}

double EvaluateFEFunction(
    std::shared_ptr<lf::uscalfe::FeSpaceLagrangeO1<double>> fe_space,
    const Eigen::VectorXd &uFE, Eigen::Vector2d global, double tol) {
  // Extract mesh
  auto mesh_p = fe_space->Mesh();
  // wrap coefficient vector into a FE mesh-function
  lf::fe::MeshFunctionFE mf(fe_space, uFE);

  for (const lf::mesh::Entity *entity_p : mesh_p->Entities(0)) {
    LF_ASSERT_MSG(lf::base::RefEl::kTria() == entity_p->RefEl(),
                  "Function only defined for triangular cells");

    // compute geometric information about the cell
    const lf::geometry::Geometry *geo_p = entity_p->Geometry();
    Eigen::MatrixXd corners = lf::geometry::Corners(*geo_p);

    // transform global coordinates to local coordinates on the cell
    Eigen::Matrix2d A;
    A << corners.col(1) - corners.col(0), corners.col(2) - corners.col(0);
    Eigen::Vector2d b;
    b << global - corners.col(0);
    Eigen::Vector2d loc = A.fullPivLu().solve(b);

    // evaluate meshfunction, if local coordinates lie in the reference triangle
    if (loc(0) >= 0 - tol && loc(1) >= 0 - tol && loc(0) + loc(1) <= 1 + tol) {
      return mf(*entity_p, loc)[0];
    }
  }
  return 0.0;
}

}  // namespace StableEvaluationAtAPoint


#include <Eigen/Core>
#include <iostream>
using namespace Eigen;
using namespace std;

template<typename Derived>
Eigen::Block<Derived, 2, 2>
topLeft2x2Corner(MatrixBase<Derived>& m)
{
  return Eigen::Block<Derived, 2, 2>(m.derived(), 0, 0);
}

template<typename Derived>
const Eigen::Block<const Derived, 2, 2>
topLeft2x2Corner(const MatrixBase<Derived>& m)
{
  return Eigen::Block<const Derived, 2, 2>(m.derived(), 0, 0);
}

int main(int, char**)
{
  Matrix3d m = Matrix3d::Identity();
  cout << topLeft2x2Corner(4*m) << endl; // calls the const version
  topLeft2x2Corner(m) *= 2;              // calls the non-const version
  cout << "Now the matrix m is:" << endl << m << endl;
  return 0;
}


#ifndef __ECTS_CONTROL_ALGORITHM__
#define __ECTS_CONTROL_ALGORITHM__

#include <ros/ros.h>
#include <Eigen/Dense>
#include <math.h>
#include <kdl/chainjnttojacsolver.hpp>
#include <limits>
#include <stdexcept>
#include <generic_control_toolbox/matrix_parser.hpp>
#include <generic_control_toolbox/kdl_manager.hpp>
#include <cmath>

namespace folding_algorithms{
typedef Eigen::Matrix<double, 6, 6> Matrix6d;
typedef Eigen::Matrix<double, 6, 7> Matrix67d;
typedef Eigen::Matrix<double, 12, 12> Matrix12d;
typedef Eigen::Matrix<double, 14, 14> Matrix14d;
typedef Eigen::Matrix<double, 6, 14> MatrixECTSr;
typedef Eigen::Matrix<double, 12, 14> MatrixECTS;
typedef Eigen::Vector3d Vector3d;
typedef Eigen::Matrix<double, 7, 1> Vector7d;
typedef Eigen::Matrix<double, 6, 1> Vector6d;
typedef Eigen::Matrix<double, 12, 1> Vector12d;
typedef Eigen::Matrix<double, 14, 1> Vector14d;

  /**
    Class that implements an extended cooperative task space (ECTS) control algorithm.

    Hardcoded dimensions to fit two 7 DOF manipulators.
  **/
  class ECTSController
  {
  public:
    /**
      Construct an ECTS controller.

      @param rod_eef key for the rod end-effector in the kdl_manager.
      @param surface_eef key for the surface_eef in the kdl_manager.
      @param kdl_manager Pointer to the kdl manager object, which is assumed to have been initialized with the relevant kinematic chains.
    **/
    ECTSController(const std::string &rod_eef, const std::string &surface_eef, std::shared_ptr<generic_control_toolbox::KDLManager> kdl_manager);
    ~ECTSController();

    /**
      Computes the ECTS reference joint velocities for the two manipulators.

      @param state The dual-arm manipulator joint state.
      @param ri The virtual sticks connecting the manipulators end-effectors to the task C-Frame.
      @param twist_a The commanded absolute motion twist.
      @param twist_r The commanded relative motion twist.
      @return The 14 dimensional joint velocities vector.
    **/
    Vector14d control(const sensor_msgs::JointState &state, const Vector3d &r1, const Vector3d &r2, const Vector6d &twist_a, const Vector6d &twist_r) const;

    /**
      Return the current alpha value that determines the degree of colaboration between arms.
    **/
    double getAlpha() const;

    /**
      Sets the alpha value.

      @param alpha The desired ECTS alpha value.
      @throw logic_error if alpha outside range [0, 1]
    **/
    void setAlpha(double alpha);

  private:
    ros::NodeHandle nh_;
    double alpha_, damping_;
    int beta_;
    std::string rod_eef_, surface_eef_;
    generic_control_toolbox::MatrixParser matrix_parser_;
    std::shared_ptr<generic_control_toolbox::KDLManager> kdl_manager_;

    /**
      Computes the ECTS jacobian that maps joints to task space twists.

      @param state The dual-arm manipulator joint state.
      @param r_i virtual stick connecting eef i to task frame
      @return The ECTS jacobian.
    **/
    MatrixECTS computeECTSJacobian(const sensor_msgs::JointState &state, const Vector3d &r_1, const Vector3d &r_2) const;

    /**
      Loads the ECTS controller parameters from the ros parameter server.

      @throw logic_error for parameters with unsuitable values
      @return True in case of success, False if some parameter is not available.
    **/
    bool getParams();
  };
}
#endif


#include <math.h>
#include <stdbool.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <iostream>
#include <armadillo>
#include <cmath>
#include <cstdlib>
#include <iomanip>
#include <time.h>
#include "func.hpp"
using namespace std;

const int max_iter = 200;
const double err_norm = 1e-14;

arma::vec Iterator(long long unsigned int n, arma::vec iter_values, double alpha);
arma::mat derivative_matrix(long long unsigned int n, double x2, double p1, double alpha);
arma::vec method_newton(long long unsigned int n, arma::mat D, double x2, double p1, double alpha);
arma::vec runge_kutta_5(long long unsigned int n, double x1, double x2, double p1, double p2, double alpha);
arma::vec ini_k(arma::vec Y, double alpha, double t, double h);
arma::mat inverse_matrix(arma::mat D);
double fedorenko_norm(arma::mat D, arma::vec iter_values, long long unsigned int n, double alpha);

arma::vec Iterator(long long unsigned int n, arma::vec iter_values, double alpha) {
    arma::mat d_matrix(static_cast<arma::uword>(2), static_cast<arma::uword>(2), arma::fill::zeros);
    double norm;

    for (int iter = 0; iter < max_iter; iter++){
        d_matrix = derivative_matrix(n, iter_values(0), iter_values(1), alpha);
        norm = fedorenko_norm(d_matrix, iter_values, n, alpha);
        if (norm < err_norm) {
            cout << setprecision(2) << defaultfloat << "step = " << iter << endl;
			cout << setprecision(2) << defaultfloat << "alpha = " << alpha << endl;
			cout << "------" << endl;
            cout << setprecision(13) << fixed << "x2(0) = " << iter_values(0) << endl;
			cout << setprecision(13) << fixed << "p1(0) = " << iter_values(1) << endl;
            cout << scientific << "norm = " << norm << endl << endl;
            return iter_values;
        }
        d_matrix = inverse_matrix(d_matrix);
        iter_values = method_newton(n, d_matrix, iter_values(0), iter_values(1), alpha);
    }

    return iter_values;
}

arma::mat derivative_matrix(long long unsigned int n, double x2, double p1, double alpha) {
    arma::vec value(n + 1, arma::fill::zeros);
    arma::mat d_matrix(static_cast<arma::uword>(2), static_cast<arma::uword>(2), arma::fill::zeros);
    double h = 1e-10;

    value = runge_kutta_5(n, 1.0, x2 + h, p1, 0.0, alpha);
    d_matrix(0, 0) += value(n - 1);
    d_matrix(1, 0) += value(n);
    value = runge_kutta_5(n, 1.0, x2 - h, p1, 0.0, alpha);
    d_matrix(0, 0) -= value(n - 1);
	d_matrix(1, 0) -= value(n);
    value = runge_kutta_5(n, 1.0, x2, p1 + h, 0.0, alpha);
    d_matrix(0, 1) += value(n - 1);
    d_matrix(1, 1) += value(n);
	value = runge_kutta_5(n, 1.0, x2, p1 - h, 0.0, alpha);
    d_matrix(0, 1) -= value(n - 1);
    d_matrix(1, 1) -= value(n);

    d_matrix = d_matrix/(2.0 * h);
    return d_matrix;
}

double fedorenko_norm(arma::mat d_matrix, arma::vec iter_values, long long unsigned int n, double alpha) {
    arma::vec value(n + 1, arma::fill::zeros);

    value = runge_kutta_5(n, 1.0, iter_values(0), iter_values(1), 0.0, alpha);

    return sqrt((value(n - 1)*value(n - 1)/(d_matrix(0,0)*d_matrix(0,0) + d_matrix(0,1)*d_matrix(0,1))) + (value(n)*value(n)/(d_matrix(1,0)*d_matrix(1,0) + d_matrix(1,1)*d_matrix(1,1))));
}

arma::mat inverse_matrix(arma::mat d_matrix) {
    double temp = 0.0;
    arma::mat value_matrix(static_cast<arma::uword>(2), static_cast<arma::uword>(2), arma::fill::zeros);

    temp = 1/((d_matrix(0,0)*d_matrix(1,1)) - (d_matrix(0,1)*d_matrix(1,0)));
    value_matrix(0, 0) = temp*d_matrix(1, 1);
    value_matrix(0, 1) = - temp*d_matrix(0, 1);
    value_matrix(1, 0) = - temp*d_matrix(1, 0);
    value_matrix(1, 1) = temp*d_matrix(0, 0);
    return value_matrix;
}

arma::vec method_newton(long long unsigned int n, arma::mat D, double x2, double p1, double alpha) {
    arma::vec value(n + 1, arma::fill::zeros);
    arma::vec iter_values(2, arma::fill::zeros);

    value = runge_kutta_5(n, 1.0, x2, p1, 0.0, alpha);
    iter_values(0) = x2 - D(0, 0) * value(n-1) - D(0, 1) * value(n);
    iter_values(1) = p1 - D(1, 0) * value(n-1) - D(1, 1) * value(n);

    return iter_values;
}

arma::vec runge_kutta_5(long long unsigned int n, double x1, double x2, double p1, double p2, double alpha) {
    arma::vec ODE(static_cast<arma::uword>(4), arma::fill::zeros);
    arma::vec ODE1(static_cast<arma::uword>(4), arma::fill::zeros);
    arma::vec ODE2(static_cast<arma::uword>(4), arma::fill::zeros);
    arma::vec value(n + 1, arma::fill::zeros);
    double h = 1.0/static_cast<double>(n);
    double t = 0.0;
    double err = 1e-15;
    bool working = true;

    ODE(0) = x1;
    ODE(1) = x2;
    ODE(2) = p1;
    ODE(3) = p2;

    while (working) {
        if (t + h >= 1.0) {
            h = 1.0 - t;
            working = false;
        }
		ODE1 = ini_k(ODE, alpha, t, h/2.0);
        ODE2 = ODE1 + ini_k(ODE + ODE1, alpha, t, h/2.0);
        ODE1 = ini_k(ODE, alpha, t, h);

        if (arma::max(arma::abs(ODE1 - ODE2)) > err) {
            h = h/2.0;
        } else
        if (arma::max(arma::abs(ODE1 - ODE2)) < err/16) {
            t += h;
            h = h*1.5;
            ODE += ODE2;
        } else {
            t += h;
            ODE += ODE2;
        }
    }

    value(n-1) = ODE(0);
    value(n) = ODE(1);

    return value;
}

arma::vec ini_k(arma::vec Y, double alpha, double t, double h) {
    arma::vec k1(static_cast<arma::uword>(4), arma::fill::zeros);
    arma::vec k2(static_cast<arma::uword>(4), arma::fill::zeros);
    arma::vec k3(static_cast<arma::uword>(4), arma::fill::zeros);
    arma::vec k4(static_cast<arma::uword>(4), arma::fill::zeros);
    arma::vec k5(static_cast<arma::uword>(4), arma::fill::zeros);
    arma::vec k6(static_cast<arma::uword>(4), arma::fill::zeros);

    k1 = h * init_func(Y, alpha, t);
    k2 = h * init_func(Y + (k1/2.0), alpha, t);
    k3 = h * init_func(Y + ((k1 + k2)/4.0), alpha, t);
    k4 = h * init_func(Y - k2 + 2.0 * k3, alpha, t);
    k5 = h * init_func(Y + ((7.0 * k1 + 10.0 * k2 + k4)/27.0), alpha, t);
    k6 = h * init_func(Y + ((28.0 * k1 - 125.0 * k2 + 546.0 * k3 + 54.0 * k4 - 378 * k5)/625.0), alpha, t);
    return k1/24.0 + (5.0 * k4)/48.0 + (27.0 * k5)/56.0 + (125.0 * k6)/336.0;
}


#pragma once

// EIGEN
#include <Eigen/Dense>
#include <Eigen/Geometry>

// CPLEX
#include <ilcplex/ilocplex.h>

// Submodules
#include <rbp_corridor.hpp>
#include <init_traj_planner.hpp>
#include <mission.hpp>
#include <param.hpp>

ILOSTLBEGIN

namespace SwarmPlanning {
    class RBPPlanner {
    public:
        RBPPlanner(Mission _mission,
                   Param _param)
                : mission(std::move(_mission)),
                  param(std::move(_param)) {
            n = param.n; // degree of polynomial
            phi = param.phi; // desired derivatives
            N = mission.qn; // the number of agents
            outdim = 3; // the number of outputs (x,y,z)

            setBatch(0);
        }

        bool update(bool log, SwarmPlanning::PlanResult* _planResult_ptr) {
            planResult_ptr = _planResult_ptr;
            M = planResult_ptr->T.size() - 1; // the number of segments
            offset_quad = M * (n + 1);
            offset_seg = n + 1;

            coef.resize(N);
            for (int qi = 0; qi < N; qi++) {
                coef[qi] = Eigen::MatrixXd::Zero(offset_quad, outdim);
            }

            IloEnv env;
            Timer timer;
            try {
                // Construct constraint matrix
                timer.reset();
                buildConstMtx();
                timer.stop();
                ROS_INFO_STREAM("RBPPlanner: Constraint Matrix runtime=" << timer.elapsedSeconds());

                // Solve QP
                timer.reset();
                solveQP(env, log);
                timer.stop();
                ROS_INFO_STREAM("RBPPlanner: QP runtime=" << timer.elapsedSeconds());
                ROS_INFO_STREAM("RBPPlanner: x size=" << count_x);
                ROS_INFO_STREAM("RBPPlanner: eq const size=" << count_eq);
                ROS_INFO_STREAM("RBPPlanner: ineq const size=" << count_lq);
            }
            catch (IloException &e) {
                ROS_ERROR_STREAM("RBPPlanner: CPLEX Concert exception caught: " << e);
                return false;
            }
            catch (...) {
                ROS_ERROR("RBPPlanner: CPLEX Unknown exception caught");
                return false;
            }
            env.end();

            if(param.time_scale) {
                timer.reset();
                timeScale();
                timer.stop();
                ROS_INFO_STREAM("RBPPlanner: timeScale runtime=" << timer.elapsedSeconds());
            }

            generateROSMsg();
            if(param.log){
                generateCoefCSV();
            }
            return true;
        }

    private:
        Mission mission;
        Param param;

        SwarmPlanning::PlanResult* planResult_ptr;
        std::vector<std::vector<int>> batches;
        int M, n, phi, N, outdim, offset_quad, offset_seg;
        IloNum count_x, count_eq, count_lq;

        // std::shared_ptr<Eigen::MatrixXd> Q_obj, Aeq_obj, Alq_obj, deq_obj, dlq_obj;
        Eigen::MatrixXd Q_base, Aeq_base, Alq, deq, dlq, basis;
        Eigen::MatrixXd dummy;
        std::vector<Eigen::MatrixXd> coef;

        void buildConstMtx() {
            build_Q_base();
            build_Aeq_base();
            build_deq();
            build_dlq();

            if (param.sequential) {
                build_dummy();
            }
        }

        void solveQP(const IloEnv &env, bool log) {
            Timer timer;
            IloNum total_cost = 0;

            IloCplex cplex(env);
//            cplex.setParam(IloCplex::Param::TimeLimit, 0.04);

            // publish Initial trajectory
            if(param.sequential && param.batch_iter == 0){
                // Translate Bernstein basis to Polynomial coefficients
                for (int k = 0; k < outdim; k++) {
                    for (int qi = 0; qi < N; qi++) {
                        for (int m = 0; m < M; m++) {
                            Eigen::MatrixXd c = Eigen::MatrixXd::Zero(1, n + 1);
                            Eigen::MatrixXd tm;
                            timeMatrix(1.0 / (planResult_ptr->T[m+1] - planResult_ptr->T[m]), &tm);
                            tm = basis * tm;

                            for (int i = 0; i < n + 1; i++) {
                                c = c + dummy(qi * offset_quad + m * offset_seg + i, k) * tm.row(i);
                            }
                            coef[qi].block(m * offset_seg, k, n + 1, 1) = c.transpose();
                        }
                    }
                    timer.stop();
                }
                return;
            }

            for (int iter = 0; iter < param.iteration; iter++) {
                total_cost = 0;
                for (int l = 0; l < param.batch_iter; l++) {
                    timer.reset();
                    IloModel model(env);
                    IloNumVarArray var(env);
                    IloRangeArray con(env);

                    populatebyrow(model, var, con, l);
                    cplex.extract(model);
                    if (log) {
                        std::string QPmodel_path = param.package_path + "/log/QPmodel.lp";
                        cplex.exportModel(QPmodel_path.c_str());
                    } else {
                        cplex.setOut(env.getNullStream());
                    }

                    // Optimize the problem and obtain solution.
                    if (!cplex.solve()) {
                        ROS_ERROR("RBPPlanner: Failed to optimize QP");
                        throw (-1);
                    }

                    IloNumArray vals(env);
                    total_cost += cplex.getObjValue();
                    cplex.getValues(vals, var);

                    // Translate Bernstein basis to Polynomial coefficients
                    int offset_dim = batches[l].size() * M * (n + 1);
                    int batch_max_iter = ceil((double)N / (double)param.batch_size);
                    for (int k = 0; k < outdim; k++) {
                        for (int qi = 0; qi < N; qi++) {
                            for (int m = 0; m < M; m++) {
                                Eigen::MatrixXd c = Eigen::MatrixXd::Zero(1, n + 1);
                                Eigen::MatrixXd tm;
                                timeMatrix(1.0 / (planResult_ptr->T[m+1] - planResult_ptr->T[m]), &tm);
                                tm = basis * tm;

                                int bi = isQuadInBatch(qi, l);
                                if (bi >= 0) {
                                    for (int i = 0; i < n + 1; i++) {
                                        c = c + vals[k * offset_dim + bi * offset_quad + m * offset_seg + i] * tm.row(i);
                                        if (param.sequential) {
                                            dummy(qi * offset_quad + m * (n + 1) + i, k) = vals[k * offset_dim + bi * offset_quad + m * offset_seg + i];
                                        }
                                    }
                                    coef[qi].block(m * offset_seg, k, n + 1, 1) = c.transpose();
                                } else if (param.sequential && param.batch_iter < batch_max_iter) {
                                    for (int i = 0; i < n + 1; i++) {
                                        c = c + dummy(qi * offset_quad + m * offset_seg + i, k) * tm.row(i);
                                    }
                                    coef[qi].block(m * offset_seg, k, n + 1, 1) = c.transpose();
                                }
                            }
                        }
                        timer.stop();
                    }
                    if (param.sequential) {
                        ROS_INFO_STREAM("RBPPlanner: QP runtime of batch " << l << "=" << timer.elapsedSeconds());
                        ROS_INFO_STREAM("RBPPlanner: QP cost of batch " << l << "=" << cplex.getObjValue());
                    }
                }
                if (param.iteration > 1)
                    ROS_INFO_STREAM("RBPPlanner: QP iteration " << iter << " total_cost=" << total_cost);
            }
            ROS_INFO_STREAM("RBPPlanner: QP total cost=" << total_cost);
        }

        // For all segment of trajectory, check maximum velocity and accelation, and scale the segment time
        void timeScale() {
            if (phi != 3 || n != 5) {
                return;
            }

            Eigen::MatrixXd coef_der;
            double time_scale, time_scale_tmp, acc_max;
            time_scale = 1;
            for (int qi = 0; qi < N; qi++) {
                for (int k = 0; k < outdim; k++) {
                    for (int m = 0; m < M; m++) {
                        derivative_segment(qi, k, m, &coef_der);

                        time_scale_tmp = scale_to_max_vel(qi, k, m, coef_der);
                        if (time_scale < time_scale_tmp) {
                            time_scale = time_scale_tmp;
                        }

                        time_scale_tmp = scale_to_max_acc(qi, k, m, coef_der);
                        if (time_scale < time_scale_tmp) {
                            time_scale = time_scale_tmp;
                        }
                    }
                }
            }

            ROS_INFO_STREAM("RBPPlanner: Time scale=" << time_scale);
            if (time_scale != 1) {
                for (int qi = 0; qi < N; qi++) {
                    // trajectory
                    for (int k = 0; k < outdim; k++) {
                        for (int m = 0; m < M; m++) {
                            Eigen::MatrixXd tm;
                            timeMatrix(1.0 / time_scale, &tm);

                            coef[qi].block(m * offset_seg, k, n + 1, 1) =
                                    tm * coef[qi].block(m * offset_seg, k, n + 1, 1);
                        }
                    }

                    // SFC
                    for (int bi = 0; bi < planResult_ptr->SFC[qi].size(); bi++){
                        planResult_ptr->SFC[qi][bi].second *= time_scale;
                    }

                    // RSFC
                    for (int qj = qi + 1; qj < N; qj++){
                        for (int ri = 0; ri < planResult_ptr->RSFC[qi][qj].size(); ri++){
                            planResult_ptr->RSFC[qi][qj][ri].second *= time_scale;
                        }
                    }
                }
                // segment time
                for (int m = 0; m < M + 1; m++) {
                    planResult_ptr->T[m] *= time_scale;
                }
            }
        }

        // generate ros message to transfer planning result
        void generateROSMsg() {
            std::vector<double> traj_info;
            traj_info.emplace_back(N);
            traj_info.emplace_back(n);
            traj_info.insert(traj_info.end(), planResult_ptr->T.begin(), planResult_ptr->T.end());
            planResult_ptr->msgs_traj_info.data = traj_info;

            planResult_ptr->msgs_traj_coef.resize(N);
            for (int qi = 0; qi < N; qi++) {
                std_msgs::MultiArrayDimension rows;
                rows.size = M * (n + 1);
                planResult_ptr->msgs_traj_coef[qi].layout.dim.emplace_back(rows);

                std_msgs::MultiArrayDimension cols;
                cols.size = outdim;
                planResult_ptr->msgs_traj_coef[qi].layout.dim.emplace_back(cols);

                std::vector<double> coef_temp(coef[qi].data(), coef[qi].data() + coef[qi].size());
                planResult_ptr->msgs_traj_coef[qi].data.insert(planResult_ptr->msgs_traj_coef[qi].data.end(),
                                                               coef_temp.begin(),
                                                               coef_temp.end());
            }
        }

        // translate coef to crazyswarm trajectory csv file
        // n should be smaller than 7
        void generateCoefCSV(){
            if(n > 7){
                ROS_WARN("RBPPlanner: n>8, do not make CSV file");
                return;
            }
            for(int qi = 0; qi < N; qi++) {
                std::ofstream coefCSV;
                coefCSV.open(param.package_path + "/log/coef" + std::to_string(qi + 1) + ".csv");
                coefCSV << "duration,x^0,x^1,x^2,x^3,x^4,x^5,x^6,x^7,y^0,y^1,y^2,y^3,y^4,y^5,y^6,y^7,z^0,z^1,z^2,z^3,z^4,z^5,z^6,z^7,yaw^0,yaw^1,yaw^2,yaw^3,yaw^4,yaw^5,yaw^6,yaw^7\n";
                for(int m = 0; m < M; m++) {
                    coefCSV << planResult_ptr->T[m + 1] - planResult_ptr->T[m] << ",";
                    // x,y,z
                    for (int k = 0; k < outdim; k++) {
                        for (int i = n; i >= 0; i--) {
                            coefCSV << coef[qi](m * offset_seg + i, k) << ",";
                        }
                        // to match
                        for(int i = 0; i < 7 - n; i++){
                            coefCSV << "0,";
                        }
                    }
                    // yaw
                    for(int i = 0; i < 8; i++){
                        coefCSV << "0,";
                    }
                    coefCSV << "\n";
                }
                coefCSV.close();
            }
        }

        // Cost matrix Q
        void build_Q_base() {
            if (phi == 3 && n == 5) {
                Q_base = Eigen::MatrixXd::Zero(n + 1, n + 1);
                Q_base << 720, -1800, 1200, 0, 0, -120,
                        -1800, 4800, -3600, 0, 600, 0,
                        1200, -3600, 3600, -1200, 0, 0,
                        0, 0, -1200, 3600, -3600, 1200,
                        0, 600, 0, -3600, 4800, -1800,
                        -120, 0, 0, 1200, -1800, 720;

                basis = Eigen::MatrixXd::Zero(n + 1, n + 1);
                basis << -1, 5, -10, 10, -5, 1,
                        5, -20, 30, -20, 5, 0,
                        -10, 30, -30, 10, 0, 0,
                        10, -20, 10, 0, 0, 0,
                        -5, 5, 0, 0, 0, 0,
                        1, 0, 0, 0, 0, 0;
            } else {
                ROS_ERROR("RBPPlanner: n should be 5"); //TODO: debug when n is not 5
            }
        }

        Eigen::MatrixXd build_Q_p(int qi, int m) {
            return Q_base * pow(planResult_ptr->T[m+1] - planResult_ptr->T[m], -2 * phi + 1);
        }

        void build_Aeq_base() {
            Aeq_base = Eigen::MatrixXd::Zero((2 * phi + (M - 1) * phi), M * (n + 1));
            Eigen::MatrixXd A_waypoints = Eigen::MatrixXd::Zero(2 * phi, M * (n + 1));
            Eigen::MatrixXd A_cont = Eigen::MatrixXd::Zero((M - 1) * phi, M * (n + 1));
            Eigen::MatrixXd A_0 = Eigen::MatrixXd::Zero(n + 1, n + 1);
            Eigen::MatrixXd A_T = Eigen::MatrixXd::Zero(n + 1, n + 1);

            // Build A_0, A_T
            if (phi == 3 && n == 5) {
                A_0 << 1, 0, 0, 0, 0, 0,
                        -1, 1, 0, 0, 0, 0,
                        1, -2, 1, 0, 0, 0,
                        -1, 3, -3, 1, 0, 0,
                        1, -4, 6, -4, 1, 0,
                        -1, 5, -10, 10, -5, 1;

                A_T << 0, 0, 0, 0, 0, 1,
                        0, 0, 0, 0, -1, 1,
                        0, 0, 0, 1, -2, 1,
                        0, 0, -1, 3, -3, 1,
                        0, 1, -4, 6, -4, 1,
                        -1, 5, -10, 10, -5, 1;
            } else {
                ROS_ERROR("RBPPlanner: n should be 5"); //TODO: debug when n is not 5
            }

            // Build A_waypoints
            int nn = 1;
            for (int i = 0; i < phi; i++) {
                A_waypoints.block(i, 0, 1, n + 1) =
                        pow(planResult_ptr->T[1] - planResult_ptr->T[0], -i) * nn * A_0.row(i);
                A_waypoints.block(phi + i, (n + 1) * (M - 1), 1, n + 1) =
                        pow(planResult_ptr->T[planResult_ptr->T.size() - 1] - planResult_ptr->T[planResult_ptr->T.size() - 2], -i) * nn * A_T.row(i);
                nn = nn * (n - i);
            }

            // Build A_cont
            for (int m = 1; m < M; m++) {
                nn = 1;
                for (int j = 0; j < phi; j++) {
                    A_cont.block(phi * (m - 1) + j, (n + 1) * (m - 1), 1, n + 1) =
                            pow(planResult_ptr->T[m] - planResult_ptr->T[m-1], -j) * nn * A_T.row(j);
                    A_cont.block(phi * (m - 1) + j, (n + 1) * m, 1, n + 1) =
                            -pow(planResult_ptr->T[m+1] - planResult_ptr->T[m], -j) * nn * A_0.row(j);
                    nn = nn * (n - j);
                }
            }

            // Build Aeq_base
            Aeq_base << A_waypoints,
                    A_cont;

        }

        // Equality constraints condition vector deq
        void build_deq() {
//        deq_obj.reset(new Eigen::MatrixXd(N * (2*n + (M-1)*n), outdim));
//        deq_obj->setZero();
            deq = Eigen::MatrixXd::Zero(N * (2 * phi + (M - 1) * phi), outdim);
            for (int qi = 0; qi < N; qi++) {
                Eigen::MatrixXd d_waypoints = Eigen::MatrixXd::Zero(2 * phi, outdim);
                Eigen::MatrixXd d_cont = Eigen::MatrixXd::Zero((M - 1) * phi, outdim);
                for (int k = 0; k < outdim; k++) {
                    d_waypoints(0, k) = mission.startState[qi][k];
                    d_waypoints(1, k) = mission.startState[qi][k + 3];
                    d_waypoints(2, k) = mission.startState[qi][k + 6];
                    d_waypoints(phi, k) = mission.goalState[qi][k];
                    d_waypoints(phi + 1, k) = mission.goalState[qi][k + 3];
                    d_waypoints(phi + 2, k) = mission.goalState[qi][k + 6];
                }

                // Build deq
                int deq_p_rows = d_waypoints.rows() + d_cont.rows();
                int deq_p_cols = outdim;
//            deq_obj->block(qi * deq_p_rows, 0, deq_p_rows, deq_p_cols) << d_waypoints,
//                                                                          d_cont;
                deq.block(qi * deq_p_rows, 0, deq_p_rows, deq_p_cols) << d_waypoints,
                        d_cont;
            }
        }

        // Inequality constraints condition vector dlq
        void build_dlq() {
//        dlq_obj.reset(new Eigen::MatrixXd(N*2*(n+1)*M + N*(N-1)*(n+1)*M, outdim));
//        dlq_obj->setZero();
            dlq = Eigen::MatrixXd::Zero(N * 2 * (n + 1) * M + N * (N - 1) * (n + 1) * M, outdim);
            Eigen::MatrixXd dlq_rel = Eigen::MatrixXd::Zero(N * (N - 1) * (n + 1) * M, outdim);
            Eigen::MatrixXd dlq_box = Eigen::MatrixXd::Zero(N * 2 * (n + 1) * M, outdim);

            // Build dlq_box
            for (int qi = 0; qi < N; qi++) {
                Eigen::MatrixXd d_upper = Eigen::MatrixXd::Zero((n + 1) * M, outdim);
                Eigen::MatrixXd d_lower = Eigen::MatrixXd::Zero((n + 1) * M, outdim);

                int bi = 0;
                for (int m = 0; m < M; m++) {
                    // find box number
                    while (bi < planResult_ptr->SFC[qi].size() &&
                           planResult_ptr->SFC[qi][bi].second < planResult_ptr->T[m + 1]) {
                        bi++;
                    }

                    d_upper.block((n + 1) * m, 0, n + 1, 1) =
                            Eigen::MatrixXd::Constant(n + 1, 1, planResult_ptr->SFC[qi][bi].first[3]);
                    d_lower.block((n + 1) * m, 0, n + 1, 1) =
                            Eigen::MatrixXd::Constant(n + 1, 1, -planResult_ptr->SFC[qi][bi].first[0]);

                    d_upper.block((n + 1) * m, 1, n + 1, 1) =
                            Eigen::MatrixXd::Constant(n + 1, 1, planResult_ptr->SFC[qi][bi].first[4]);
                    d_lower.block((n + 1) * m, 1, n + 1, 1) =
                            Eigen::MatrixXd::Constant(n + 1, 1, -planResult_ptr->SFC[qi][bi].first[1]);

                    d_upper.block((n + 1) * m, 2, n + 1, 1) =
                            Eigen::MatrixXd::Constant(n + 1, 1, planResult_ptr->SFC[qi][bi].first[5]);
                    d_lower.block((n + 1) * m, 2, n + 1, 1) =
                            Eigen::MatrixXd::Constant(n + 1, 1, -planResult_ptr->SFC[qi][bi].first[2]);
                }

                int dlq_box_rows = d_upper.rows() + d_lower.rows();
                dlq_box.block(qi * dlq_box_rows, 0, dlq_box_rows, outdim) << d_upper,
                        d_lower;
            }

            // Build dlq_rel
            int iter = 0;
            for (int qi = 0; qi < N; qi++) {
                for (int qj = qi + 1; qj < N; qj++) {
                    Eigen::MatrixXd d_upper = Eigen::MatrixXd::Constant((n + 1) * M, outdim, 10000000);
                    Eigen::MatrixXd d_lower = Eigen::MatrixXd::Constant((n + 1) * M, outdim, 10000000);

                    for (int m = 0; m < M; m++) {
                        // Find box number
                        int ri = 0;
                        while (ri < planResult_ptr->RSFC[qi][qj].size() &&
                               planResult_ptr->RSFC[qi][qj][ri].second < planResult_ptr->T[m + 1]) {
                            ri++;
                        }

                        octomap::point3d normal_vector = planResult_ptr->RSFC[qi][qj][ri].first;
                        d_upper.block((n + 1) * m, 0, n + 1, 1) =
                                Eigen::MatrixXd::Constant(n + 1, 1, normal_vector.x());
                        d_upper.block((n + 1) * m, 1, n + 1, 1) =
                                Eigen::MatrixXd::Constant(n + 1, 1, normal_vector.y());
                        d_upper.block((n + 1) * m, 2, n + 1, 1) =
                                Eigen::MatrixXd::Constant(n + 1, 1, normal_vector.z());
                    }
                    int dlq_rel_rows = d_upper.rows() + d_lower.rows();
                    dlq_rel.block(iter * dlq_rel_rows, 0, dlq_rel_rows, outdim) << d_upper,
                            d_lower;
                    iter++;
                }
            }

            // Build dlq
//        dlq_obj->block(0, 0, dlq_box.rows(), dlq_box.cols()) = dlq_box;
//        dlq_obj->block(dlq_box.rows(), 0, dlq_rel.rows(), dlq_rel.cols()) = dlq_rel;
            dlq << dlq_box,
                    dlq_rel;
        }

        void build_dummy() {
            dummy = Eigen::MatrixXd::Zero(N * offset_quad, outdim);

            for (int qi = 0; qi < N; qi++) {
                int m = 0;
                int idx = 0;
                while (m < M) {
                    if (idx >= planResult_ptr->initTraj[qi].size() - 1) {
                        idx = planResult_ptr->initTraj[qi].size() - 1;
                        for (int j = 0; j < n + 1; j++) {
                            dummy(qi * offset_quad + m * (n + 1) + j, 0) = planResult_ptr->initTraj[qi][idx].x();
                            dummy(qi * offset_quad + m * (n + 1) + j, 1) = planResult_ptr->initTraj[qi][idx].y();
                            dummy(qi * offset_quad + m * (n + 1) + j, 2) = planResult_ptr->initTraj[qi][idx].z();
                        }
                        m++;
                    } else {
                        for (int j = 0; j < n + 1; j++) {
                            int a = 1;
                            if (j < (n + 1) / 2) {
                                a = 0;
                            }
                            dummy(qi * offset_quad + m * (n + 1) + j, 0) =
                                    (1 - a) * planResult_ptr->initTraj[qi][idx].x()
                                    + a * planResult_ptr->initTraj[qi][idx + 1].x();
                            dummy(qi * offset_quad + m * (n + 1) + j, 1) =
                                    (1 - a) * planResult_ptr->initTraj[qi][idx].y()
                                    + a * planResult_ptr->initTraj[qi][idx + 1].y();
                            dummy(qi * offset_quad + m * (n + 1) + j, 2) =
                                    (1 - a) * planResult_ptr->initTraj[qi][idx].z()
                                    + a * planResult_ptr->initTraj[qi][idx + 1].z();
                        }
                        m++;
                    }
                    idx++;
                }
            }
        }

        void populatebyrow(IloModel model, IloNumVarArray x, IloRangeArray c, int l) {
            int offset_dim = batches[l].size() * M * (n + 1);
            IloEnv env = model.getEnv();
            for (int k = 0; k < outdim; k++) {
                for (int bi = 0; bi < batches[l].size(); bi++) {
                    for (int m = 0; m < M; m++) {
                        for (int i = 0; i < n + 1; i++) {
                            x.add(IloNumVar(env, -IloInfinity, IloInfinity));

                            int qi = batches[l][bi];
                            int row = k * offset_dim + bi * offset_quad + m * (n + 1) + i;
                            std::string name;
                            if (k == 0) {
                                name = "x_" + std::to_string(qi) + "_" + std::to_string(m) + "_" + std::to_string(i);
                            } else if (k == 1) {
                                name = "y_" + std::to_string(qi) + "_" + std::to_string(m) + "_" + std::to_string(i);
                            } else if (k == 2) {
                                name = "z_" + std::to_string(qi) + "_" + std::to_string(m) + "_" + std::to_string(i);
                            } else {
                                ROS_ERROR("RBPPlanner: Invalid outdim");
                            }

                            x[row].setName(name.c_str());
                        }
                    }
                }
            }
            count_x = x.getSize();

            // Cost function
            IloNumExpr cost(env);
            for (int k = 0; k < outdim; k++) {
                for (int bi = 0; bi < batches[l].size(); bi++) {
                    for (int m = 0; m < M; m++) {
                        int qi = batches[l][bi];
                        Eigen::MatrixXd Q_p = build_Q_p(qi, m);

                        for (int i = 0; i < n + 1; i++) {
                            int row = qi * M * (n + 1) + m * (n + 1) + i;
                            int row_idx = k * offset_dim + bi * offset_quad + m * (n + 1) + i;

                            for (int j = 0; j < n + 1; j++) {
                                int col = qi * M * (n + 1) + m * (n + 1) + j;
                                int col_idx =
                                        k * offset_dim + bi * offset_quad + m * (n + 1) + j;

                                if (Q_p(i, j) != 0) {
                                    cost += Q_p(i, j) * x[row_idx] * x[col_idx];
                                }
                            }
                        }
                    }
                }
            }
            model.add(IloMinimize(env, cost));

            // Equality Constraints
            for (int k = 0; k < outdim; k++) {
                for (int bi = 0; bi < batches[l].size(); bi++) {
                    for (int i = 0; i < 2 * phi + (M - 1) * phi; i++) {
                        IloNumExpr expr(env);
                        for (int j = 0; j < M * (n + 1); j++) {
                            if (Aeq_base(i, j) != 0) {
                                expr += Aeq_base(i, j) * x[k * offset_dim + bi * offset_quad + j];
                            }
                        }
                        int qi = batches[l][bi];
                        c.add(expr == deq(qi * (2 * phi + (M - 1) * phi) + i, k));
                        expr.end();
                    }
                }
            }
            count_eq = c.getSize();

            // Inequality Constraints
            for (int k = 0; k < outdim; k++) {
                for (int bi = 0; bi < batches[l].size(); bi++) {
                    int qi = batches[l][bi];
                    for (int j = 0; j < (n + 1) * M; j++) {
                        int idx = k * offset_dim + bi * offset_quad + j;
                        c.add(x[idx] <= dlq(2 * qi * offset_quad + j, k));
                        c.add(-x[idx] <= dlq((2 * qi + 1) * offset_quad + j, k));
                    }
                }
            }
            int offset_box = 2 * N * offset_quad;
            int iter = 0;
            for (int qi = 0; qi < N; qi++) {
                for (int qj = qi + 1; qj < N; qj++) {
                    int bi = isQuadInBatch(qi, l);
                    int bj = isQuadInBatch(qj, l);

                    if (bi < 0 && bj < 0) {

                    } else if (bi >= 0 && bj < 0) {
                        for (int j = 0; j < M * (n + 1); j++) {
                            int idx = bi * offset_quad + j;
                            c.add(dlq(offset_box + offset_quad * 2 * iter + j, 0) *
                                  (dummy(qj * offset_quad + j, 0) - x[0 * offset_dim + idx]) +
                                  dlq(offset_box + offset_quad * 2 * iter + j, 1) *
                                  (dummy(qj * offset_quad + j, 1) - x[1 * offset_dim + idx]) +
                                  dlq(offset_box + offset_quad * 2 * iter + j, 2) *
                                  (dummy(qj * offset_quad + j, 2) - x[2 * offset_dim + idx])
                                  >= mission.quad_size[qi] + mission.quad_size[qj]);
                        }
                    } else if (bi < 0 && bj >= 0) {
                        for (int j = 0; j < M * (n + 1); j++) {
                            int jdx = bj * offset_quad + j;
                            c.add(dlq(offset_box + offset_quad * 2 * iter + j, 0) *
                                  (x[0 * offset_dim + jdx] - dummy(qi * offset_quad + j, 0)) +
                                  dlq(offset_box + offset_quad * 2 * iter + j, 1) *
                                  (x[1 * offset_dim + jdx] - dummy(qi * offset_quad + j, 1)) +
                                  dlq(offset_box + offset_quad * 2 * iter + j, 2) *
                                  (x[2 * offset_dim + jdx] - dummy(qi * offset_quad + j, 2))
                                  >= mission.quad_size[qi] + mission.quad_size[qj]);
                        }
                    } else {
                        for (int j = 0; j < M * (n + 1); j++) {
                            int idx = bi * offset_quad + j;
                            int jdx = bj * offset_quad + j;

                            c.add(dlq(offset_box + offset_quad * 2 * iter + j, 0) *
                                  (x[0 * offset_dim + jdx] - x[0 * offset_dim + idx]) +
                                  dlq(offset_box + offset_quad * 2 * iter + j, 1) *
                                  (x[1 * offset_dim + jdx] - x[1 * offset_dim + idx]) +
                                  dlq(offset_box + offset_quad * 2 * iter + j, 2) *
                                  (x[2 * offset_dim + jdx] - x[2 * offset_dim + idx])
                                  >= mission.quad_size[qi] + mission.quad_size[qj]);
                        }
                    }

                    iter++;
                }
            }
            model.add(c);

            count_lq = c.getSize() - count_eq;
        }

        // timeMatrix is mapping matrix (n + 1) x (n + 1)
        // e.g. [1   0   0   ...]
        //      [0   t   0   ...]
        //      [0   0   t^2 ...]
        //      [... ... ... ...]
        void timeMatrix(double t, Eigen::MatrixXd* tm_ptr) {
            *tm_ptr = Eigen::MatrixXd::Zero(n + 1, n + 1);
            for (int i = 0; i < n + 1; i++) {
                (*tm_ptr)(i, i) = pow(t, n - i);
            }
        }

        // Get derivative of m^th segment(polynomial) of qi^th agent
        // e.g. if phi = 3, n = 5, polynomial [1 1 1 1 1 1] then
        //      coef_der = [1  1  1  1  1  1]
        //                 [5  4  3  2  1  0]
        //                 [20 12 6  2  0  0]
        //                 [60 24 6  0  0  0]
        void derivative_segment(int qi, int k, int m, Eigen::MatrixXd* coef_der_ptr) {
            *coef_der_ptr = Eigen::MatrixXd::Zero(phi + 1, n + 1);
            for (int i = 0; i < phi + 1; i++) {
                for (int j = 0; j < n + 1; j++) {
                    if (i <= j)
                        (*coef_der_ptr)(i, n - j) = coef_derivative(i, j) * coef[qi](m * offset_seg + n - j, k);
                    else
                        (*coef_der_ptr)(i, n - j) = 0;
                }
            }
        }

        // Get j^th coefficient of i^th derivative of polynomial [1 1 1 1 ... 1]
        int coef_derivative(int i, int j) {
            return (i == 0) ? 1 : coef_derivative(i - 1, j - 1) * j;
        }

        // Get roots of i^th derivative of polynomial with coefficient coef
        // The roots of the polynomial are calculated by computing the eigenvalues of the companion matrix, A
        std::vector<double> roots_derivative(int i, const Eigen::MatrixXd& coef_der) {
            std::vector<double> roots_der;
            int n_der = n - i;
            int iter = 0;
            while(n_der > 0 && coef_der(i, n - i - n_der) == 0 ){
                n_der--;
            }
            if(n_der == 0){
                return roots_der; // return empty vector
            }

            Eigen::MatrixXd A = Eigen::MatrixXd::Zero(n_der, n_der);
            for (int j = 0; j < n_der; j++) {
                if (j < n_der - 1) {
                    A(j + 1, j) = 1;
                }
                A(0, j) = -coef_der(i, n - i - n_der + j + 1) / coef_der(i, n - i - n_der);
            }

            Eigen::EigenSolver<Eigen::MatrixXd> es(A);
            for (int j = 0; j < i; j++) {
                complex<double> lambda = es.eigenvalues()[j];
                if (lambda.imag() == 0) {
                    roots_der.emplace_back(lambda.real());
                }
            }
            return roots_der;
        }

        double scale_to_max_vel(int qi, int k, int m, const Eigen::MatrixXd& coef_der) {
            assert(phi > 1);
            double scale_update_rate = 1.1; //TODO: parameterization?

            // Get maximum velocity
            double vel_max = 0, t_max = 0;
            std::vector<double> ts = roots_derivative(2, coef_der);
            ts.emplace_back(0);
            ts.emplace_back(planResult_ptr->T[m + 1] - planResult_ptr->T[m]);
            for (auto t : ts) {
                if (t < 0 || t > planResult_ptr->T[m + 1] - planResult_ptr->T[m]) {
                    continue;
                }

                double vel = 0;
                for (int i = 0; i <= (n - 1); i++) {
                    vel += coef_der(1, i) * pow(t, n - 1 - i);
                }
                vel = abs(vel);
                if (vel_max < vel) {
                    vel_max = vel;
                    t_max = t;
                }
            }

            // time_scale update
            double time_scale = 1;
            while (vel_max > mission.max_vel[qi][k]) {
                time_scale *= scale_update_rate;

                double vel = 0;
                for (int i = 0; i <= (n - 1); i++) {
                    vel += coef_der(1, i) * pow(1 / time_scale, n - i) * pow(t_max, n - 1 - i);
                }
                vel_max = abs(vel);
            }

            return time_scale;
        }

        double scale_to_max_acc(int qi, int k, int m, const Eigen::MatrixXd &coef_der) {
            assert(phi == 3 && n == 5);
            double scale_update_rate = 1.1; //TODO: parameterization?

            // Get maximum accelaration
            double a, b, c, D, acc_max, t_max = 0;
            a = coef_der(3, 0);
            b = coef_der(3, 1);
            c = coef_der(3, 2);
            D = b * b - 4 * a * c;
            acc_max = 0;

            std::vector<double> ts{0, planResult_ptr->T[m+1] - planResult_ptr->T[m]};
            if (D >= 0 && a != 0) {
                ts.emplace_back((-b + sqrt(D)) / (2 * a));
                ts.emplace_back((-b - sqrt(D)) / (2 * a));
            }
            else if(a == 0 && b != 0){
                ts.emplace_back(-c/b);
            }

            for (auto t : ts) {
                if (t < 0 || t > planResult_ptr->T[m+1] - planResult_ptr->T[m]) {
                    continue;
                }

                double acc = 0;
                for (int i = 0; i < 4; i++) {
                    acc += coef_der(2, i) * pow(t, 3 - i);
                }
                acc = abs(acc);
                if (acc_max < acc) {
                    acc_max = acc;
                    t_max = t;
                }
            }

            // time_scale update
            double time_scale = 1;
            while (acc_max > mission.max_acc[qi][k]) {
                time_scale *= scale_update_rate;

                double acc = 0;
                for (int i = 0; i < 4; i++) {
                    acc += coef_der(2, i) * pow(1 / time_scale, n - i) * pow(t_max, 3 - i);
                }
                acc_max = abs(acc);
            }

            return time_scale;
        }

        void setBatch(int alg){
            int batch_max_iter = ceil((double)N / (double)param.batch_size);
            if (param.sequential) {
                int batch_max_iter = ceil((double)N / (double)param.batch_size);
                if (param.batch_iter < 0 || param.batch_iter > batch_max_iter) {
                    param.batch_iter = batch_max_iter;
                }
            } else {
                param.batch_size = N;
                param.batch_iter = 1;
            }

            batches.resize(batch_max_iter);

            //default groups
            if(alg == 0) {
                for (int qi = 0; qi < N; qi++) {
                    batches[qi / param.batch_size].emplace_back(qi);
                }
            }
            else{
                ROS_ERROR("RBPPlaner: invalid batch algorithm");
            }
        }

        int isQuadInBatch(int qi, int l){
            for(int bi = 0; bi < batches[l].size(); bi++){
                if(qi == batches[l][bi]){
                    return bi;
                }
            }
            return -1;
        }
    };
}