#include <Eigen/Sparse>

#include "gtest/gtest.h"

#include "tbb/blocked_range.h"
#include "tbb/parallel_for.h"
#include "tbb/parallel_reduce.h"

#include "ScalarGrid.h"
#include "Transform.h"
#include "Utilities.h"
#include "VectorGrid.h"

using namespace FluidSim3D;

class AnalyticalViscositySolver
{
    static constexpr int UNASSIGNED = -1;

public:
    AnalyticalViscositySolver(const Transform& xform, const Vec3i& size)
        : myXform(xform)
        , myVelocityIndex(myXform, size, Vec3i::Constant(UNASSIGNED), VectorGridSettings::SampleType::STAGGERED)
    {}

    // Returns the infinity-norm error of the numerical solution
    template<typename Initial, typename Solution, typename Viscosity>
    double solve(const Initial& initial, const Solution& solution, const Viscosity& viscosity, const double dt);

    Vec3d cellIndexToWorld(const Vec3d& index) const { return myXform.indexToWorld(index + Vec3d::Constant(.5)); }
    Vec3d edgeIndexToWorld(const Vec3d& index, int edgeAxis) const
    {
        Vec3d offset;
        if (edgeAxis == 0)
            offset = Vec3d(.5, 0., 0.);
        else if (edgeAxis == 1)
            offset = Vec3d(0., .5, 0.);
        else
            offset = Vec3d(0., 0., .5);

        return myXform.indexToWorld(index + offset);
    }

private:
    Transform myXform;
    int setVelocityIndices();
    VectorGrid<int> myVelocityIndex;
};

template<typename Initial, typename Solution, typename Viscosity>
double AnalyticalViscositySolver::solve(const Initial& initialFunction, const Solution& solutionFunction, const Viscosity& viscosityFunction, const double dt)
{
    int velocityDOFCount = setVelocityIndices();

    // Build reduced system.
    // (Note we don't need control volumes since the cells are the same size and there is no free surface).
    // (I - dt * mu * D^T K D) u(n+1) = u(n)

    std::vector<Eigen::Triplet<double>> sparseMatrixElements;

    VectorXd rhsVector = VectorXd::Zero(velocityDOFCount);

    double dx = myVelocityIndex.dx();
    double baseCoeff = dt / std::pow(dx, 2);

    tbb::enumerable_thread_specific<std::vector<Eigen::Triplet<double>>> parallelSparseElements;

    for (int faceAxis : {0, 1, 2})
    {
        tbb::parallel_for(tbb::blocked_range<int>(0, myVelocityIndex.grid(faceAxis).voxelCount()), [&](const tbb::blocked_range<int>& range)
        {
            auto& localSparseElements = parallelSparseElements.local();

            for (int faceIndex = range.begin(); faceIndex != range.end(); ++faceIndex)
            {
                Vec3i face = myVelocityIndex.grid(faceAxis).unflatten(faceIndex);

                int velocityIndex = myVelocityIndex(face, faceAxis);

                if (velocityIndex >= 0)
                {
                    Vec3d facePosition = myVelocityIndex.indexToWorld(face.cast<double>(), faceAxis);

                    rhsVector(velocityIndex) += initialFunction(facePosition, faceAxis);

                    localSparseElements.emplace_back(velocityIndex, velocityIndex, 1);

                    // Build cell-centered stresses.
                    for (int divergenceDirection : {0, 1})
                    {
                        Vec3i cell = faceToCell(face, faceAxis, divergenceDirection);

                        Vec3d cellPosition = cellIndexToWorld(cell.cast<double>());
                        double cellCoeff = 2. * viscosityFunction(cellPosition) * baseCoeff;

                        double divSign = (divergenceDirection == 0) ? -1 : 1;

                        for (int gradientDirection : {0, 1})
                        {
                            Vec3i adjacentFace = cellToFace(cell, faceAxis, gradientDirection);

                            double gradSign = (gradientDirection == 0) ? -1 : 1;

                            int adjacentFaceIndex = myVelocityIndex(adjacentFace, faceAxis);
                            if (adjacentFaceIndex >= 0)
                                localSparseElements.emplace_back(velocityIndex, adjacentFaceIndex, -divSign * gradSign * cellCoeff);
                            else
                            {
                                Vec3d adjacentFacePosition = myVelocityIndex.indexToWorld(adjacentFace.cast<double>(), faceAxis);
                                rhsVector(velocityIndex) += divSign * gradSign * cellCoeff * solutionFunction(adjacentFacePosition, faceAxis);
                            }
                        }
                    }

                    for (int edgeAxis : {0, 1, 2})
                    {
                        if (edgeAxis == faceAxis) continue;

                        for (int divergenceDirection : {0, 1})
                        {
                            Vec3i edge = faceToEdge(face, faceAxis, edgeAxis, divergenceDirection);

                            Vec3d edgePosition = edgeIndexToWorld(edge.cast<double>(), edgeAxis);
                            double edgeCoeff = viscosityFunction(edgePosition) * baseCoeff;

                            double divSign = (divergenceDirection == 0) ? -1 : 1;

                            for (int gradientAxis : {0, 1, 2})
                            {
                                if (gradientAxis == edgeAxis) continue;

                                int gradientFaceAxis = 3 - gradientAxis - edgeAxis;

                                for (int gradientDirection : {0, 1})
                                {
                                    double gradSign = (gradientDirection == 0) ? -1 : 1;

                                    Vec3i localGradientFace = edgeToFace(edge, edgeAxis, gradientFaceAxis, gradientDirection);

                                    // Check for out of bounds
                                    if (gradientDirection == 0 && localGradientFace[gradientAxis] < 0 || gradientDirection == 1 && localGradientFace[gradientAxis] >= myVelocityIndex.size(gradientFaceAxis)[gradientAxis])
                                    {
                                        Vec3d gradientFacePosition = myVelocityIndex.indexToWorld(localGradientFace.cast<double>(), gradientFaceAxis);
                                        rhsVector(velocityIndex) += divSign * gradSign * edgeCoeff * solutionFunction(gradientFacePosition, gradientFaceAxis);
                                    }
                                    else
                                    {
                                        int gradientFaceIndex = myVelocityIndex(localGradientFace, gradientFaceAxis);

                                        if (gradientFaceIndex >= 0)
                                            localSparseElements.emplace_back(velocityIndex, gradientFaceIndex, -divSign * gradSign * edgeCoeff);
                                        else
                                        {
                                            Vec3d gradientFacePosition = myVelocityIndex.indexToWorld(localGradientFace.cast<double>(), gradientFaceAxis);
                                            rhsVector(velocityIndex) += divSign * gradSign * edgeCoeff * solutionFunction(gradientFacePosition, gradientFaceAxis);
                                        }
                                    }
                                }
                            }
                        }
                    }
                }
            }
        });

        mergeLocalThreadVectors(sparseMatrixElements, parallelSparseElements);
    }

    Eigen::SparseMatrix<double> sparseMatrix(velocityDOFCount, velocityDOFCount);
    sparseMatrix.setFromTriplets(sparseMatrixElements.begin(), sparseMatrixElements.end());

    Eigen::ConjugateGradient<Eigen::SparseMatrix<double>, Eigen::Upper | Eigen::Lower> solver;
    solver.compute(sparseMatrix);

    if (solver.info() != Eigen::Success)
    {
        return -1;
    }

    VectorXd solutionVector = solver.solve(rhsVector);

    if (solver.info() != Eigen::Success)
    {
        return -1;
    }

    double error = 0;

    for (int axis : {0, 1})
    {
        double localError = tbb::parallel_reduce(tbb::blocked_range<int>(0, myVelocityIndex.grid(axis).voxelCount()), double(0),
            [&](const tbb::blocked_range<int>& range, double error) -> double {
                for (int faceIndex = range.begin(); faceIndex != range.end(); ++faceIndex)
                {
                    Vec3i face = myVelocityIndex.grid(axis).unflatten(faceIndex);

                    int velocityIndex = myVelocityIndex(face, axis);

                    if (velocityIndex >= 0)
                    {
                        Vec3d facePosition = myVelocityIndex.indexToWorld(face.cast<double>(), axis);
                        double localError = fabs(solutionVector(velocityIndex) - solutionFunction(facePosition, axis));

                        error = std::max(error, localError);
                    }
                }

                return error;
            },
            [](double a, double b) -> double {
                return std::max(a, b);
            });

        error = std::max(error, localError);
    }

    return error;
}

int AnalyticalViscositySolver::setVelocityIndices()
{
    // Loop over each face. If it's not along the boundary
    // then include it into the system.

    int index = 0;

    for (int axis : {0, 1, 2})
    {
        Vec3i size = myVelocityIndex.size(axis);

        forEachVoxelRange(Vec3i::Zero(), size, [&](const Vec3i& face)
        {
            // Faces along the boundary are removed from the simulation
            if (face[axis] > 0 && face[axis] < size[axis] - 1)
                myVelocityIndex(face, axis) = index++;
        });
    }
    // Returning index gives the number of velocity positions required for a linear system
    return index;
}

TEST(ANALYTICAL_VISCOSITY_TEST, CONVERGENCE_TEST)
{
    double dt = 1.;

    auto solution = [](const Vec3d& pos, int)
    {
        return std::sin(pos[0]) * std::sin(pos[1]) * std::sin(pos[2]);
    };
    auto viscosity = [](const Vec3d& pos) { return 1.; };

    auto initial = [&](const Vec3d& pos, int axis)
    {
        double x = pos[0];
        double y = pos[1];
        double z = pos[2];

        if (axis == 0)
            return (3. * std::sin(x) * std::sin(y) * std::sin(z)) / 2. - (std::cos(x) * std::cos(z) * std::sin(y)) / 2. - (std::cos(x) * std::cos(y) * std::sin(z)) / 2. + (3. * std::sin(x) * std::sin(y) * std::sin(z)) / 2.;
        else if (axis == 1)
            return (3. * std::sin(x) * std::sin(y) * std::sin(z)) / 2. - (std::cos(y) * std::cos(z) * std::sin(x)) / 2. - (std::cos(x) * std::cos(y) * std::sin(z)) / 2. + (3. * std::sin(x) * std::sin(y) * std::sin(z)) / 2.;
        else
            return (3. * std::sin(x) * std::sin(y) * std::sin(z)) / 2. - (std::cos(y) * std::cos(z) * std::sin(x)) / 2. - (std::cos(x) * std::cos(z) * std::sin(y)) / 2. + (3. * std::sin(x) * std::sin(y) * std::sin(z)) / 2.;
    };

    const int startGrid = 8;
    const int endGrid = startGrid * int(pow(2, 4));

    std::vector<double> errors;
    for (int gridSize = startGrid; gridSize < endGrid; gridSize *= 2)
    {
        const double dx = 1. / double(gridSize);
        Vec3d origin = Vec3d::Zero();
        Vec3i size = Vec3i::Constant(startGrid);
        Transform xform(dx, origin);

        AnalyticalViscositySolver solver(xform, size);
        double error = solver.solve(initial, solution, viscosity, dt);

        errors.push_back(error);
        EXPECT_GT(error, 0.);
    }

    for (int errorIndex = 1; errorIndex < errors.size(); ++errorIndex)
    {
        double errorRatio = errors[errorIndex - 1] / errors[errorIndex];
        EXPECT_GT(errorRatio, 4.);
    }
}


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

#define BOOST_MATH_POLY_METHOD 0
#define BOOST_MATH_RATIONAL_METHOD 0

#include <boost/random.hpp>
#include <boost/math/tools/rational.hpp>
#include <iostream>
#include <fstream>
#include "mp_t.hpp"

int main()
{
   using namespace boost::math;
   using namespace boost::math::tools;

   static const unsigned max_order = 20;
   std::cout << std::scientific << std::setprecision(40);

   boost::mt19937 rnd;
   boost::variate_generator<
      boost::mt19937,
      boost::uniform_int<> > gen(rnd, boost::uniform_int<>(1, 12));

   for(unsigned i = 1; i < max_order; ++i)
   {
      std::vector<int> coef;
      for(unsigned j = 0; j < i; ++j)
      {
         coef.push_back(gen());
      }
      std::cout << std::scientific;
      std::cout <<
"   //\n"
"   // Polynomials of order " << i-1 << "\n"
"   //\n"
"   static const U n" << i << "c[" << i << "] = { ";
      for(unsigned j = 0; j < i; ++j)
      {
         if(j)
            std::cout  << ", ";
         std::cout << coef[j];
      }
      std::cout << " };\n";
      std::cout <<
         "   static const boost::array<U, " << i << "> n" << i << "a = { ";
      for(unsigned j = 0; j < i; ++j)
      {
         if(j)
            std::cout  << ", ";
         std::cout << coef[j];
      }
      std::cout << " };\n";

      mp_t r1 = boost::math::tools::evaluate_polynomial(&coef[0], mp_t(0.125), i);
      mp_t r2 = boost::math::tools::evaluate_polynomial(&coef[0], mp_t(0.25), i);
      mp_t r3 = boost::math::tools::evaluate_polynomial(&coef[0], mp_t(0.75), i);
      mp_t r4 = boost::math::tools::evaluate_polynomial(&coef[0], mp_t(1) - mp_t(1) / 64, i);
      mp_t r5 = boost::math::tools::evaluate_polynomial(&coef[0], mp_t(6.5), i);
      mp_t r6 = boost::math::tools::evaluate_polynomial(&coef[0], mp_t(10247.25), i);

      std::cout <<
         "   BOOST_CHECK_CLOSE(\n"
         "      boost::math::tools::evaluate_polynomial(n" << i << "c, static_cast<T>(0.125), " << i << "),\n"
         "      static_cast<T>(" << r1 << "L),\n"
         "      tolerance);\n";
      std::cout <<
         "   BOOST_CHECK_CLOSE(\n"
         "      boost::math::tools::evaluate_polynomial(n" << i << "c, static_cast<T>(0.25), " << i << "),\n"
         "      static_cast<T>(" << r2 << "L),\n"
         "      tolerance);\n";
      std::cout <<
         "   BOOST_CHECK_CLOSE(\n"
         "      boost::math::tools::evaluate_polynomial(n" << i << "c, static_cast<T>(0.75), " << i << "),\n"
         "      static_cast<T>(" << r3 << "L),\n"
         "      tolerance);\n";
      std::cout <<
         "   BOOST_CHECK_CLOSE(\n"
         "      boost::math::tools::evaluate_polynomial(n" << i << "c, static_cast<T>(1.0f - 1.0f/64.0f), " << i << "),\n"
         "      static_cast<T>(" << r4 << "L),\n"
         "      tolerance);\n";
      if(fabs(r5) < tools::max_value<float>())
         std::cout <<
            "   BOOST_CHECK_CLOSE(\n"
            "      boost::math::tools::evaluate_polynomial(n" << i << "c, static_cast<T>(6.5), " << i << "),\n"
            "      static_cast<T>(" << r5 << "L),\n"
            "      tolerance);\n";
      if(fabs(r6) < tools::max_value<float>())
         std::cout <<
            "   BOOST_CHECK_CLOSE(\n"
            "      boost::math::tools::evaluate_polynomial(n" << i << "c, static_cast<T>(10247.25), " << i << "),\n"
            "      static_cast<T>(" << r6 << "L),\n"
            "      tolerance);\n\n";

      std::cout <<
         "   BOOST_CHECK_CLOSE(\n"
         "      boost::math::tools::evaluate_polynomial(n" << i << "c, static_cast<T>(0.125)),\n"
         "      static_cast<T>(" << r1 << "L),\n"
         "      tolerance);\n";
      std::cout <<
         "   BOOST_CHECK_CLOSE(\n"
         "      boost::math::tools::evaluate_polynomial(n" << i << "c, static_cast<T>(0.25)),\n"
         "      static_cast<T>(" << r2 << "L),\n"
         "      tolerance);\n";
      std::cout <<
         "   BOOST_CHECK_CLOSE(\n"
         "      boost::math::tools::evaluate_polynomial(n" << i << "c, static_cast<T>(0.75)),\n"
         "      static_cast<T>(" << r3 << "L),\n"
         "      tolerance);\n";
      std::cout <<
         "   BOOST_CHECK_CLOSE(\n"
         "      boost::math::tools::evaluate_polynomial(n" << i << "c, static_cast<T>(1.0f - 1.0f/64.0f)),\n"
         "      static_cast<T>(" << r4 << "L),\n"
         "      tolerance);\n";
      if(fabs(r5) < tools::max_value<float>())
         std::cout <<
            "   BOOST_CHECK_CLOSE(\n"
            "      boost::math::tools::evaluate_polynomial(n" << i << "c, static_cast<T>(6.5)),\n"
            "      static_cast<T>(" << r5 << "L),\n"
            "      tolerance);\n";
      if(fabs(r6) < tools::max_value<float>())
         std::cout <<
            "   BOOST_CHECK_CLOSE(\n"
            "      boost::math::tools::evaluate_polynomial(n" << i << "c, static_cast<T>(10247.25)),\n"
            "      static_cast<T>(" << r6 << "L),\n"
            "      tolerance);\n\n";

      std::cout <<
         "   BOOST_CHECK_CLOSE(\n"
         "      boost::math::tools::evaluate_polynomial(n" << i << "a, static_cast<T>(0.125)),\n"
         "      static_cast<T>(" << r1 << "L),\n"
         "      tolerance);\n";
      std::cout <<
         "   BOOST_CHECK_CLOSE(\n"
         "      boost::math::tools::evaluate_polynomial(n" << i << "a, static_cast<T>(0.25)),\n"
         "      static_cast<T>(" << r2 << "L),\n"
         "      tolerance);\n";
      std::cout <<
         "   BOOST_CHECK_CLOSE(\n"
         "      boost::math::tools::evaluate_polynomial(n" << i << "a, static_cast<T>(0.75)),\n"
         "      static_cast<T>(" << r3 << "L),\n"
         "      tolerance);\n";
      std::cout <<
         "   BOOST_CHECK_CLOSE(\n"
         "      boost::math::tools::evaluate_polynomial(n" << i << "a, static_cast<T>(1.0f - 1.0f/64.0f)),\n"
         "      static_cast<T>(" << r4 << "L),\n"
         "      tolerance);\n";
      if(fabs(r5) < tools::max_value<float>())
         std::cout <<
            "   BOOST_CHECK_CLOSE(\n"
            "      boost::math::tools::evaluate_polynomial(n" << i << "a, static_cast<T>(6.5)),\n"
            "      static_cast<T>(" << r5 << "L),\n"
            "      tolerance);\n";
      if(fabs(r6) < tools::max_value<float>())
         std::cout <<
            "   BOOST_CHECK_CLOSE(\n"
            "      boost::math::tools::evaluate_polynomial(n" << i << "a, static_cast<T>(10247.25)),\n"
            "      static_cast<T>(" << r6 << "L),\n"
            "      tolerance);\n\n";

      r1 = boost::math::tools::evaluate_even_polynomial(&coef[0], mp_t(0.125), i);
      r2 = boost::math::tools::evaluate_even_polynomial(&coef[0], mp_t(0.25), i);
      r3 = boost::math::tools::evaluate_even_polynomial(&coef[0], mp_t(0.75), i);
      r4 = boost::math::tools::evaluate_even_polynomial(&coef[0], mp_t(1) - mp_t(1) / 64, i);
      r5 = boost::math::tools::evaluate_even_polynomial(&coef[0], mp_t(6.5), i);
      r6 = boost::math::tools::evaluate_even_polynomial(&coef[0], mp_t(10247.25), i);

      std::cout <<
         "   BOOST_CHECK_CLOSE(\n"
         "      boost::math::tools::evaluate_even_polynomial(n" << i << "c, static_cast<T>(0.125), " << i << "),\n"
         "      static_cast<T>(" << r1 << "L),\n"
         "      tolerance);\n";
      std::cout <<
         "   BOOST_CHECK_CLOSE(\n"
         "      boost::math::tools::evaluate_even_polynomial(n" << i << "c, static_cast<T>(0.25), " << i << "),\n"
         "      static_cast<T>(" << r2 << "L),\n"
         "      tolerance);\n";
      std::cout <<
         "   BOOST_CHECK_CLOSE(\n"
         "      boost::math::tools::evaluate_even_polynomial(n" << i << "c, static_cast<T>(0.75), " << i << "),\n"
         "      static_cast<T>(" << r3 << "L),\n"
         "      tolerance);\n";
      std::cout <<
         "   BOOST_CHECK_CLOSE(\n"
         "      boost::math::tools::evaluate_even_polynomial(n" << i << "c, static_cast<T>(1.0f - 1.0f/64.0f), " << i << "),\n"
         "      static_cast<T>(" << r4 << "L),\n"
         "      tolerance);\n";
      if(fabs(r5) < tools::max_value<float>())
         std::cout <<
            "   BOOST_CHECK_CLOSE(\n"
            "      boost::math::tools::evaluate_even_polynomial(n" << i << "c, static_cast<T>(6.5f), " << i << "),\n"
            "      static_cast<T>(" << r5 << "L),\n"
            "      tolerance);\n";
      if(fabs(r6) < tools::max_value<float>())
         std::cout <<
            "   BOOST_CHECK_CLOSE(\n"
            "      boost::math::tools::evaluate_even_polynomial(n" << i << "c, static_cast<T>(10247.25f), " << i << "),\n"
            "      static_cast<T>(" << r6 << "L),\n"
            "      tolerance);\n\n";

      std::cout <<
         "   BOOST_CHECK_CLOSE(\n"
         "      boost::math::tools::evaluate_even_polynomial(n" << i << "c, static_cast<T>(0.125)),\n"
         "      static_cast<T>(" << r1 << "L),\n"
         "      tolerance);\n";
      std::cout <<
         "   BOOST_CHECK_CLOSE(\n"
         "      boost::math::tools::evaluate_even_polynomial(n" << i << "c, static_cast<T>(0.25)),\n"
         "      static_cast<T>(" << r2 << "L),\n"
         "      tolerance);\n";
      std::cout <<
         "   BOOST_CHECK_CLOSE(\n"
         "      boost::math::tools::evaluate_even_polynomial(n" << i << "c, static_cast<T>(0.75)),\n"
         "      static_cast<T>(" << r3 << "L),\n"
         "      tolerance);\n";
      std::cout <<
         "   BOOST_CHECK_CLOSE(\n"
         "      boost::math::tools::evaluate_even_polynomial(n" << i << "c, static_cast<T>(1.0f - 1.0f/64.0f)),\n"
         "      static_cast<T>(" << r4 << "L),\n"
         "      tolerance);\n";
      if(fabs(r5) < tools::max_value<float>())
         std::cout <<
            "   BOOST_CHECK_CLOSE(\n"
            "      boost::math::tools::evaluate_even_polynomial(n" << i << "c, static_cast<T>(6.5f)),\n"
            "      static_cast<T>(" << r5 << "L),\n"
            "      tolerance);\n";
      if(fabs(r6) < tools::max_value<float>())
         std::cout <<
            "   BOOST_CHECK_CLOSE(\n"
            "      boost::math::tools::evaluate_even_polynomial(n" << i << "c, static_cast<T>(10247.25f)),\n"
            "      static_cast<T>(" << r6 << "L),\n"
            "      tolerance);\n\n";

      std::cout <<
         "   BOOST_CHECK_CLOSE(\n"
         "      boost::math::tools::evaluate_even_polynomial(n" << i << "a, static_cast<T>(0.125)),\n"
         "      static_cast<T>(" << r1 << "L),\n"
         "      tolerance);\n";
      std::cout <<
         "   BOOST_CHECK_CLOSE(\n"
         "      boost::math::tools::evaluate_even_polynomial(n" << i << "a, static_cast<T>(0.25)),\n"
         "      static_cast<T>(" << r2 << "L),\n"
         "      tolerance);\n";
      std::cout <<
         "   BOOST_CHECK_CLOSE(\n"
         "      boost::math::tools::evaluate_even_polynomial(n" << i << "a, static_cast<T>(0.75)),\n"
         "      static_cast<T>(" << r3 << "L),\n"
         "      tolerance);\n";
      std::cout <<
         "   BOOST_CHECK_CLOSE(\n"
         "      boost::math::tools::evaluate_even_polynomial(n" << i << "a, static_cast<T>(1.0f - 1.0f/64.0f)),\n"
         "      static_cast<T>(" << r4 << "L),\n"
         "      tolerance);\n";
      if(fabs(r5) < tools::max_value<float>())
         std::cout <<
            "   BOOST_CHECK_CLOSE(\n"
            "      boost::math::tools::evaluate_even_polynomial(n" << i << "a, static_cast<T>(6.5f)),\n"
            "      static_cast<T>(" << r5 << "L),\n"
            "      tolerance);\n";
      if(fabs(r6) < tools::max_value<float>())
         std::cout <<
            "   BOOST_CHECK_CLOSE(\n"
            "      boost::math::tools::evaluate_even_polynomial(n" << i << "a, static_cast<T>(10247.25f)),\n"
            "      static_cast<T>(" << r6 << "L),\n"
            "      tolerance);\n\n";

      if(i > 1)
      {
         r1 = boost::math::tools::evaluate_odd_polynomial(&coef[0], mp_t(0.125), i);
         r2 = boost::math::tools::evaluate_odd_polynomial(&coef[0], mp_t(0.25), i);
         r3 = boost::math::tools::evaluate_odd_polynomial(&coef[0], mp_t(0.75), i);
         r4 = boost::math::tools::evaluate_odd_polynomial(&coef[0], mp_t(1) - mp_t(1) / 64, i);
         r5 = boost::math::tools::evaluate_odd_polynomial(&coef[0], mp_t(6.5), i);
         r6 = boost::math::tools::evaluate_odd_polynomial(&coef[0], mp_t(10247.25), i);

         std::cout <<
            "   BOOST_CHECK_CLOSE(\n"
            "      boost::math::tools::evaluate_odd_polynomial(n" << i << "c, static_cast<T>(0.125), " << i << "),\n"
            "      static_cast<T>(" << r1 << "L),\n"
            "      tolerance);\n";
         std::cout <<
            "   BOOST_CHECK_CLOSE(\n"
            "      boost::math::tools::evaluate_odd_polynomial(n" << i << "c, static_cast<T>(0.25), " << i << "),\n"
            "      static_cast<T>(" << r2 << "L),\n"
            "      tolerance);\n";
         std::cout <<
            "   BOOST_CHECK_CLOSE(\n"
            "      boost::math::tools::evaluate_odd_polynomial(n" << i << "c, static_cast<T>(0.75), " << i << "),\n"
            "      static_cast<T>(" << r3 << "L),\n"
            "      tolerance);\n";
         std::cout <<
            "   BOOST_CHECK_CLOSE(\n"
            "      boost::math::tools::evaluate_odd_polynomial(n" << i << "c, static_cast<T>(1.0f - 1.0f/64.0f), " << i << "),\n"
            "      static_cast<T>(" << r4 << "L),\n"
            "      tolerance);\n";
         if(fabs(r5) < tools::max_value<float>())
            std::cout <<
               "   BOOST_CHECK_CLOSE(\n"
               "      boost::math::tools::evaluate_odd_polynomial(n" << i << "c, static_cast<T>(6.5f), " << i << "),\n"
               "      static_cast<T>(" << r5 << "L),\n"
               "      tolerance);\n";
         if(fabs(r6) < tools::max_value<float>())
            std::cout <<
               "   BOOST_CHECK_CLOSE(\n"
               "      boost::math::tools::evaluate_odd_polynomial(n" << i << "c, static_cast<T>(10247.25f), " << i << "),\n"
               "      static_cast<T>(" << r6 << "L),\n"
               "      tolerance);\n\n";

         std::cout <<
            "   BOOST_CHECK_CLOSE(\n"
            "      boost::math::tools::evaluate_odd_polynomial(n" << i << "c, static_cast<T>(0.125)),\n"
            "      static_cast<T>(" << r1 << "L),\n"
            "      tolerance);\n";
         std::cout <<
            "   BOOST_CHECK_CLOSE(\n"
            "      boost::math::tools::evaluate_odd_polynomial(n" << i << "c, static_cast<T>(0.25)),\n"
            "      static_cast<T>(" << r2 << "L),\n"
            "      tolerance);\n";
         std::cout <<
            "   BOOST_CHECK_CLOSE(\n"
            "      boost::math::tools::evaluate_odd_polynomial(n" << i << "c, static_cast<T>(0.75)),\n"
            "      static_cast<T>(" << r3 << "L),\n"
            "      tolerance);\n";
         std::cout <<
            "   BOOST_CHECK_CLOSE(\n"
            "      boost::math::tools::evaluate_odd_polynomial(n" << i << "c, static_cast<T>(1.0f - 1.0f/64.0f)),\n"
            "      static_cast<T>(" << r4 << "L),\n"
            "      tolerance);\n";
         if(fabs(r5) < tools::max_value<float>())
            std::cout <<
               "   BOOST_CHECK_CLOSE(\n"
               "      boost::math::tools::evaluate_odd_polynomial(n" << i << "c, static_cast<T>(6.5f)),\n"
               "      static_cast<T>(" << r5 << "L),\n"
               "      tolerance);\n";
         if(fabs(r6) < tools::max_value<float>())
            std::cout <<
               "   BOOST_CHECK_CLOSE(\n"
               "      boost::math::tools::evaluate_odd_polynomial(n" << i << "c, static_cast<T>(10247.25f)),\n"
               "      static_cast<T>(" << r6 << "L),\n"
               "      tolerance);\n\n";

         std::cout <<
            "   BOOST_CHECK_CLOSE(\n"
            "      boost::math::tools::evaluate_odd_polynomial(n" << i << "a, static_cast<T>(0.125)),\n"
            "      static_cast<T>(" << r1 << "L),\n"
            "      tolerance);\n";
         std::cout <<
            "   BOOST_CHECK_CLOSE(\n"
            "      boost::math::tools::evaluate_odd_polynomial(n" << i << "a, static_cast<T>(0.25)),\n"
            "      static_cast<T>(" << r2 << "L),\n"
            "      tolerance);\n";
         std::cout <<
            "   BOOST_CHECK_CLOSE(\n"
            "      boost::math::tools::evaluate_odd_polynomial(n" << i << "a, static_cast<T>(0.75)),\n"
            "      static_cast<T>(" << r3 << "L),\n"
            "      tolerance);\n";
         std::cout <<
            "   BOOST_CHECK_CLOSE(\n"
            "      boost::math::tools::evaluate_odd_polynomial(n" << i << "a, static_cast<T>(1.0f - 1.0f/64.0f)),\n"
            "      static_cast<T>(" << r4 << "L),\n"
            "      tolerance);\n";
         if(fabs(r5) < tools::max_value<float>())
            std::cout <<
               "   BOOST_CHECK_CLOSE(\n"
               "      boost::math::tools::evaluate_odd_polynomial(n" << i << "a, static_cast<T>(6.5f)),\n"
               "      static_cast<T>(" << r5 << "L),\n"
               "      tolerance);\n";
         if(fabs(r6) < tools::max_value<float>())
            std::cout <<
               "   BOOST_CHECK_CLOSE(\n"
               "      boost::math::tools::evaluate_odd_polynomial(n" << i << "a, static_cast<T>(10247.25f)),\n"
               "      static_cast<T>(" << r6 << "L),\n"
               "      tolerance);\n\n";
      }

      r1 = boost::math::tools::evaluate_polynomial(&coef[0], mp_t(0.125), i);
      r2 = boost::math::tools::evaluate_polynomial(&coef[0], mp_t(0.25), i);
      r3 = boost::math::tools::evaluate_polynomial(&coef[0], mp_t(0.75), i);
      r4 = boost::math::tools::evaluate_polynomial(&coef[0], mp_t(1) - mp_t(1) / 64, i);
      r5 = boost::math::tools::evaluate_polynomial(&coef[0], mp_t(6.5), i);
      r6 = boost::math::tools::evaluate_polynomial(&coef[0], mp_t(10247.25), i);

      coef.clear();
      for(unsigned j = 0; j < i; ++j)
      {
         coef.push_back(gen());
      }
      std::cout <<
"   //\n"
"   // Rational functions of order " << i-1 << "\n"
"   //\n"
"   static const U d" << i << "c[" << i << "] = { ";
      for(unsigned j = 0; j < i; ++j)
      {
         if(j)
            std::cout  << ", ";
         std::cout << coef[j];
      }
      std::cout << " };\n";
      std::cout <<
         "   static const boost::array<U, " << i << "> d" << i << "a = { ";
      for(unsigned j = 0; j < i; ++j)
      {
         if(j)
            std::cout  << ", ";
         std::cout << coef[j];
      }
      std::cout << " };\n";

      mp_t r1d = boost::math::tools::evaluate_polynomial(&coef[0], mp_t(0.125), i);
      mp_t r2d = boost::math::tools::evaluate_polynomial(&coef[0], mp_t(0.25), i);
      mp_t r3d = boost::math::tools::evaluate_polynomial(&coef[0], mp_t(0.75), i);
      mp_t r4d = boost::math::tools::evaluate_polynomial(&coef[0], mp_t(1) - mp_t(1) / 64, i);
      mp_t r5d = boost::math::tools::evaluate_polynomial(&coef[0], mp_t(6.5), i);
      mp_t r6d = boost::math::tools::evaluate_polynomial(&coef[0], mp_t(10247.25), i);

      std::cout <<
         "   BOOST_CHECK_CLOSE(\n"
         "      boost::math::tools::evaluate_rational(n" << i << "c, d" << i << "c, static_cast<T>(0.125), " << i << "),\n"
         "      static_cast<T>(" << r1/r1d << "L),\n"
         "      tolerance);\n";
      std::cout <<
         "   BOOST_CHECK_CLOSE(\n"
         "      boost::math::tools::evaluate_rational(n" << i << "c, d" << i << "c, static_cast<T>(0.25), " << i << "),\n"
         "      static_cast<T>(" << r2/r2d << "L),\n"
         "      tolerance);\n";
      std::cout <<
         "   BOOST_CHECK_CLOSE(\n"
         "      boost::math::tools::evaluate_rational(n" << i << "c, d" << i << "c, static_cast<T>(0.75), " << i << "),\n"
         "      static_cast<T>(" << r3/r3d << "L),\n"
         "      tolerance);\n";
      std::cout <<
         "   BOOST_CHECK_CLOSE(\n"
         "      boost::math::tools::evaluate_rational(n" << i << "c, d" << i << "c, static_cast<T>(1.0f - 1.0f/64.0f), " << i << "),\n"
         "      static_cast<T>(" << r4/r4d << "L),\n"
         "      tolerance);\n";
      if(fabs(r5/r5d) < tools::max_value<float>())
         std::cout <<
            "   BOOST_CHECK_CLOSE(\n"
            "      boost::math::tools::evaluate_rational(n" << i << "c, d" << i << "c, static_cast<T>(6.5f), " << i << "),\n"
            "      static_cast<T>(" << r5/r5d << "L),\n"
            "      tolerance);\n";
      if(fabs(r6/r6d) < tools::max_value<float>())
         std::cout <<
            "   BOOST_CHECK_CLOSE(\n"
            "      boost::math::tools::evaluate_rational(n" << i << "c, d" << i << "c, static_cast<T>(10247.25f), " << i << "),\n"
            "      static_cast<T>(" << r6/r6d << "L),\n"
            "      tolerance);\n\n";

      std::cout <<
         "   BOOST_CHECK_CLOSE(\n"
         "      boost::math::tools::evaluate_rational(n" << i << "c, d" << i << "c, static_cast<T>(0.125)),\n"
         "      static_cast<T>(" << r1/r1d << "L),\n"
         "      tolerance);\n";
      std::cout <<
         "   BOOST_CHECK_CLOSE(\n"
         "      boost::math::tools::evaluate_rational(n" << i << "c, d" << i << "c, static_cast<T>(0.25)),\n"
         "      static_cast<T>(" << r2/r2d << "L),\n"
         "      tolerance);\n";
      std::cout <<
         "   BOOST_CHECK_CLOSE(\n"
         "      boost::math::tools::evaluate_rational(n" << i << "c, d" << i << "c, static_cast<T>(0.75)),\n"
         "      static_cast<T>(" << r3/r3d << "L),\n"
         "      tolerance);\n";
      std::cout <<
         "   BOOST_CHECK_CLOSE(\n"
         "      boost::math::tools::evaluate_rational(n" << i << "c, d" << i << "c, static_cast<T>(1.0f - 1.0f/64.0f)),\n"
         "      static_cast<T>(" << r4/r4d << "L),\n"
         "      tolerance);\n";
      if(fabs(r5/r5d) < tools::max_value<float>())
         std::cout <<
            "   BOOST_CHECK_CLOSE(\n"
            "      boost::math::tools::evaluate_rational(n" << i << "c, d" << i << "c, static_cast<T>(6.5f)),\n"
            "      static_cast<T>(" << r5/r5d << "L),\n"
            "      tolerance);\n";
      if(fabs(r6/r6d) < tools::max_value<float>())
         std::cout <<
            "   BOOST_CHECK_CLOSE(\n"
            "      boost::math::tools::evaluate_rational(n" << i << "c, d" << i << "c, static_cast<T>(10247.25f)),\n"
            "      static_cast<T>(" << r6/r6d << "L),\n"
            "      tolerance);\n\n";

      std::cout <<
         "   BOOST_CHECK_CLOSE(\n"
         "      boost::math::tools::evaluate_rational(n" << i << "a, d" << i << "a, static_cast<T>(0.125)),\n"
         "      static_cast<T>(" << r1/r1d << "L),\n"
         "      tolerance);\n";
      std::cout <<
         "   BOOST_CHECK_CLOSE(\n"
         "      boost::math::tools::evaluate_rational(n" << i << "a, d" << i << "a, static_cast<T>(0.25)),\n"
         "      static_cast<T>(" << r2/r2d << "L),\n"
         "      tolerance);\n";
      std::cout <<
         "   BOOST_CHECK_CLOSE(\n"
         "      boost::math::tools::evaluate_rational(n" << i << "a, d" << i << "a, static_cast<T>(0.75)),\n"
         "      static_cast<T>(" << r3/r3d << "L),\n"
         "      tolerance);\n";
      std::cout <<
         "   BOOST_CHECK_CLOSE(\n"
         "      boost::math::tools::evaluate_rational(n" << i << "a, d" << i << "a, static_cast<T>(1.0f - 1.0f/64.0f)),\n"
         "      static_cast<T>(" << r4/r4d << "L),\n"
         "      tolerance);\n";
      if(fabs(r5/r5d) < tools::max_value<float>())
         std::cout <<
            "   BOOST_CHECK_CLOSE(\n"
            "      boost::math::tools::evaluate_rational(n" << i << "a, d" << i << "a, static_cast<T>(6.5f)),\n"
            "      static_cast<T>(" << r5/r5d << "L),\n"
            "      tolerance);\n";
      if(fabs(r6/r6d) < tools::max_value<float>())
         std::cout <<
            "   BOOST_CHECK_CLOSE(\n"
            "      boost::math::tools::evaluate_rational(n" << i << "a, d" << i << "a, static_cast<T>(10247.25f)),\n"
            "      static_cast<T>(" << r6/r6d << "L),\n"
            "      tolerance);\n\n";
   }

   return 0;
}


// smooth_feedback: Control theory on Lie groups
// https://github.com/pettni/smooth_feedback
//
// Licensed under the MIT License <http://opensource.org/licenses/MIT>.
//
// Copyright (c) 2021 Petter Nilsson
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.

#ifndef SMOOTH__FEEDBACK__ASIF_FUNC_HPP_
#define SMOOTH__FEEDBACK__ASIF_FUNC_HPP_

/**
 * @file
 * @brief Functions for active Set Invariance (ASI) filtering on Lie groups.
 */

#include <Eigen/Core>
#include <boost/numeric/odeint.hpp>
#include <smooth/compat/odeint.hpp>
#include <smooth/diff.hpp>
#include <smooth/lie_group.hpp>

#include "common.hpp"
#include "qp.hpp"

namespace smooth::feedback {

/**
 * @brief Active set invariance problem definition.
 *
 * The active set invariance problem is
 * \f[
 * \begin{cases}
 *  \min_u        & (u \ominus u_{des})' W_u (u \ominus u_{des})  \\
 *  \text{s.t.}   & x(0) = x_0 \\
 *                & u \in ulim \\
 *                & h(x(t)) \geq 0, \quad t \in [0, T]    \\
 * \end{cases}
 * \f]
 * for a system \f$ \mathrm{d}^r x_t = f(x(t), u(t)) \f$.
 */
template<LieGroup G, Manifold U>
  requires(Dof<G> > 0 && Dof<U> > 0)
struct ASIFProblem
{
  /// time horizon
  double T{1};
  /// initial state
  G x0{Default<G>()};
  /// desired input
  U u_des{Default<U>()};
  /// weights on desired input
  Eigen::Matrix<double, Dof<U>, 1> W_u{Eigen::Matrix<double, Dof<U>, 1>::Ones()};
  /// input bounds
  ManifoldBounds<U> ulim{};
};

/**
 * @brief Parameters for asif_to_qp
 */
struct ASIFtoQPParams
{
  /// number of constraint instances (equally spaced over the time horizon)
  std::size_t K{10};
  /// barrier function time constant \f$ \alpha \f$ s.t. \f$ \dot h - \alpha h \geq 0 \f$.
  double alpha{1};
  /// maximal integration time step
  double dt{0.1};
  /// relaxation cost
  double relax_cost{100};
};

/**
 * @brief Allocate QP matrices (part 1 of asif_to_qp())
 *
 * @param[in] K number of constraint instances
 * @param[in] nu_ineq number in inequalities in input constraint
 * @param[in] nh number of barrier constraints
 * @param[out] qp allocated QP with zero matrices
 */
template<LieGroup G, Manifold U>
  requires(Dof<G> > 0 && Dof<U> > 0)
void asif_to_qp_allocate(
  std::size_t K, std::size_t nu_ineq, std::size_t nh, QuadraticProgram<-1, -1, double> & qp)
{
  static constexpr int nx = Dof<G>;
  static constexpr int nu = Dof<U>;

  static_assert(nx > 0, "State space dimension must be static");
  static_assert(nu > 0, "Input space dimension must be static");

  const int M = K * nh + nu_ineq + 1;
  const int N = nu + 1;

  qp.A.setZero(M, N);
  qp.l.setZero(M);
  qp.u.setZero(M);

  qp.P.setZero(N, N);
  qp.q.setZero(N);
}

/**
 * @brief Fill QP matrices (part 2 of asif_to_qp())
 *
 * Note that the (dense) QP matrices must be pre-allocated and filled with zeros.
 */
template<
  LieGroup G,
  Manifold U,
  typename Dyn,
  typename SafeSet,
  typename BackupU,
  diff::Type DT = diff::Type::Default>
  requires(Dof<G> > 0 && Dof<U> > 0)
void asif_to_qp_fill(
  const ASIFProblem<G, U> & pbm,
  const ASIFtoQPParams & prm,
  Dyn && f,
  SafeSet && h,
  BackupU && bu,
  QuadraticProgram<-1, -1, double> & qp)
{
  using boost::numeric::odeint::euler, boost::numeric::odeint::vector_space_algebra;
  using std::placeholders::_1;

  static constexpr int nx = Dof<G>;
  static constexpr int nu = Dof<U>;
  static constexpr int nh = std::invoke_result_t<SafeSet, double, G>::SizeAtCompileTime;

  euler<G, double, Tangent<G>, double, vector_space_algebra> state_stepper{};
  euler<TangentMap<G>, double, TangentMap<G>, double, vector_space_algebra> sensi_stepper{};

  const int nu_ineq = pbm.ulim.A.rows();

  [[maybe_unused]] const int M = prm.K * nh + nu_ineq + 1;
  [[maybe_unused]] const int N = nu + 1;

  assert(qp.A.rows() == M);
  assert(qp.A.cols() == N);
  assert(qp.l.rows() == M);
  assert(qp.u.rows() == M);

  assert(qp.P.rows() == N);
  assert(qp.P.cols() == N);
  assert(qp.q.rows() == N);

  // iteration variables
  const double tau     = pbm.T / static_cast<double>(prm.K);
  const double dt      = std::min<double>(prm.dt, tau);
  double t             = 0;
  G x                  = pbm.x0;
  TangentMap<G> dx_dx0 = TangentMap<G>::Identity();

  // define ODEs for closed-loop dynamics and its sensitivity
  const auto x_ode = [&f, &bu](const G & xx, Tangent<G> & dd, double tt) {
    dd = f(tt, xx, bu(tt, xx));
  };

  const auto dx_dx0_ode = [&f, &bu, &x](const auto & S_v, auto & dS_dt_v, double tt) {
    auto f_cl = [&]<typename T>(const CastT<T, G> & vx) { return f(T(tt), vx, bu(T(tt), vx)); };
    const auto [fcl, dr_fcl_dx] = diff::dr<1, DT>(std::move(f_cl), wrt(x));
    dS_dt_v                     = (-ad<G>(fcl) + dr_fcl_dx) * S_v;
  };

  // value of dynamics at call time
  const auto [f0, d_f0_du] = diff::dr<1, DT>(
    [&]<typename T>(const CastT<T, U> & vu) { return f(T(t), cast<T>(x), vu); }, wrt(pbm.u_des));

  // loop over constraint number
  for (auto k = 0u; k != prm.K; ++k) {
    // differentiate barrier function w.r.t. x
    const auto [hval, dh_dtx] = diff::dr<1, DT>(
      [&h]<typename T>(const T & vt, const CastT<T, G> & vx) { return h(vt, vx); }, wrt(t, x));

    const Eigen::Matrix<double, nh, 1> dh_dt  = dh_dtx.template leftCols<1>();
    const Eigen::Matrix<double, nh, nx> dh_dx = dh_dtx.template rightCols<nx>();

    // insert barrier constraint
    const Eigen::Matrix<double, nh, nx> dh_dx0 = dh_dx * dx_dx0;
    qp.A.template block<nh, nu>(k * nh, 0)     = dh_dx0 * d_f0_du;
    qp.l.template segment<nh>(k * nh)          = -dh_dt - prm.alpha * hval - dh_dx0 * f0;
    qp.u.template segment<nh>(k * nh).setConstant(std::numeric_limits<double>::infinity());

    // integrate system and sensitivity forward until next constraint
    double dt_act = std::min(dt, tau * (k + 1) - t);
    while (t < tau * (k + 1)) {
      state_stepper.do_step(x_ode, x, t, dt_act);
      sensi_stepper.do_step(dx_dx0_ode, dx_dx0, t, dt_act);
      t += dt_act;
    }
  }

  // relaxation of barrier constraints
  qp.A.block(0, nu, prm.K * nh, 1).setConstant(1);

  // input bounds
  qp.A.block(prm.K * nh, 0, nu_ineq, nu) = pbm.ulim.A;
  qp.l.segment(prm.K * nh, nu_ineq)      = pbm.ulim.l - pbm.ulim.A * rminus(pbm.u_des, pbm.ulim.c);
  qp.u.segment(prm.K * nh, nu_ineq)      = pbm.ulim.u - pbm.ulim.A * rminus(pbm.u_des, pbm.ulim.c);

  // upper and lower bounds on delta
  qp.A(prm.K * nh + nu_ineq, nu) = 1;
  qp.l(prm.K * nh + nu_ineq)     = 0;
  qp.u(prm.K * nh + nu_ineq)     = std::numeric_limits<double>::infinity();

  qp.P.template block<nu, nu>(0, 0) = pbm.W_u.asDiagonal();

  qp.P(nu, nu) = prm.relax_cost;
  qp.q(nu)     = 0;
}

/**
 * @brief Convert an ASIFProblem to a QuadraticProgram.
 *
 * The objective is to impose constraints on the current input \f$ u \f$ of a system \f$
 * \mathrm{d}^r x_t = f(x, u) \f$ s.t.
 * \f[
 *    \frac{\mathrm{d}}{\mathrm{d}t} h(\phi(t; x_0, bu(\cdot)))
 *    \geq \alpha h(\phi(t; x_0, bu(\cdot)))
 * \f]
 * which enforces forward invariance of the set \f$ \{ x
 * : h(t, x) \geq 0 \} \f$ along the **backup trajectory** \f$ bu \f$. The constraint is enforced at
 * \f$K\f$ look-ahead time steps \f$ t_k = k \tau\f$ for \f$k = 0, \ldots, K \f$.
 *
 * This function encodes the problem as a QuadraticProgram that solves
 * \f[
 *  \begin{cases}
 *   \min_{\mu}  & \left\| u - u_{des} \right\|^2 \\
 *   \text{s.t.} & \text{constraint above holds for } u = u_{des} + \mu
 *   \end{cases}
 * \f]
 * A solution \f$ \mu^* \f$ to the QuadraticProgram corresponds to an input \f$ u_{des} \oplus \mu^*
 * \f$ applied to the system.
 *
 * @tparam G state LieGroup type \f$\mathbb{G}\f$
 * @tparam U input Manifold type \f$\mathbb{G}\f$
 *
 * @param pbm problem definition
 * @param prm algorithm parameters
 * @param f system model \f$f : \mathbb{R} \times \mathbb{G} \times \mathbb{U} \rightarrow
 * \mathbb{R}^{\dim \mathfrak g}\f$ s.t. \f$ \mathrm{d}^r x_t = f(t, x, u) \f$
 * @param h safe set \f$h : \mathbb{R} \times \mathbb{G} \rightarrow \mathbb{R}^{n_h}\f$ s.t. \f$
 * S(t) = \{ h(t, x) \geq 0 \} \f$ denotes the safe set at time \f$ t \f$
 * @param bu backup controller \f$ub : \mathbb{R} \times \mathbb{G} \rightarrow \mathbb{U} \f$
 *
 * \note The algorithm relies on automatic differentiation. The following supplied functions must be
 * differentiable (i.e. be templated on the scalar type if an automatic differentiation method is
 * selected):
 *   * \p f differentiable w.r.t. x and u
 *   * h differentiable w.r.t. t and x
 *   * bu differentiable w.r.t. x
 *
 * @return QuadraticProgram modeling the ASIF filtering problem
 */
template<
  LieGroup G,
  Manifold U,
  typename Dyn,
  typename SS,
  typename BackupU,
  diff::Type DT = diff::Type::Default>
QuadraticProgram<-1, -1, double> asif_to_qp(
  const ASIFProblem<G, U> & pbm, const ASIFtoQPParams & prm, Dyn && f, SS && h, BackupU && bu)
{
  static constexpr int nh = std::invoke_result_t<SS, double, G>::SizeAtCompileTime;

  static_assert(Dof<G> > 0, "State space dimension must be static");
  static_assert(Dof<U> > 0, "Input space dimension must be static");
  static_assert(nh > 0, "Safe set dimension must be static");

  const int nu_ineq = pbm.ulim.A.rows();
  QuadraticProgram<-1, -1, double> qp;
  asif_to_qp_allocate<G, U>(prm.K, nu_ineq, nh, qp);
  asif_to_qp_fill(pbm, prm, f, h, bu, qp);
  return qp;
}

}  // namespace smooth::feedback

#endif  // SMOOTH__FEEDBACK__INTERNAL__ASIF_FUNC_HPP_


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

#include <Eigen/StdVector>
#include <stdio.h>
#include <math.h>
#include <algorithm>
#include <vector>
#include <stdlib.h>

#include "mader.hpp"
#include "timer.hpp"
#include "termcolor.hpp"

#if USE_GUROBI_FLAG
#else
#include "nlopt_utils.hpp"
#endif

using namespace termcolor;

// Uncomment the type of timer you want:
// typedef ROSTimer MyTimer;getNextGoal
// typedef ROSWallTimer MyTimer;
typedef MADER_timers::Timer MyTimer;

Mader::Mader(mt::parameters par) : par_(par)
{
  drone_status_ == DroneStatus::YAWING;
  G_.pos << 0, 0, 0;
  G_term_.pos << 0, 0, 0;

  mtx_initial_cond.lock();
  stateA_.setZero();
  mtx_initial_cond.unlock();

#if USE_GUROBI_FLAG
#else
  // Check that the gradients are right
  if (nlopt_utils::checkGradientsNlopt(par_.basis) == false)
  {
    std::cout << "==============================================" << std::endl;
    std::cout << bold << "Gradient check was " << red << "NOT OK " << reset << std::endl;
    std::cout << "==============================================" << std::endl;

    abort();
  }
  else
  {
    std::cout << "==============================================" << std::endl;
    std::cout << bold << "Gradient check was " << green << " OK " << reset << std::endl;
    std::cout << "==============================================" << std::endl;
  }
#endif

  changeDroneStatus(DroneStatus::GOAL_REACHED);
  resetInitialization();

  ms::par_solver par_for_solver;

  par_for_solver.x_min = par_.x_min;
  par_for_solver.x_max = par_.x_max;

  par_for_solver.y_min = par_.y_min;
  par_for_solver.y_max = par_.y_max;

  par_for_solver.z_min = par_.z_min;
  par_for_solver.z_max = par_.z_max;

  par_for_solver.Ra = par_.Ra;
  par_for_solver.v_max = par_.v_max;
  par_for_solver.a_max = par_.a_max;
  par_for_solver.j_max = par_.j_max;
  par_for_solver.dc = par_.dc;
  par_for_solver.dist_to_use_straight_guess = par_.goal_radius;
  par_for_solver.a_star_samp_x = par_.a_star_samp_x;
  par_for_solver.a_star_samp_y = par_.a_star_samp_y;
  par_for_solver.a_star_samp_z = par_.a_star_samp_z;
  par_for_solver.a_star_fraction_voxel_size = par_.a_star_fraction_voxel_size;
  par_for_solver.num_pol = par_.num_pol;
  par_for_solver.deg_pol = par_.deg_pol;
  par_for_solver.weight = par_.weight;
  par_for_solver.epsilon_tol_constraints = par_.epsilon_tol_constraints;
  par_for_solver.xtol_rel = par_.xtol_rel;
  par_for_solver.ftol_rel = par_.ftol_rel;
  par_for_solver.solver = par_.solver;
  par_for_solver.basis = par_.basis;
  par_for_solver.a_star_bias = par_.a_star_bias;
  par_for_solver.allow_infeasible_guess = par_.allow_infeasible_guess;
  par_for_solver.alpha_shrink = par_.alpha_shrink;

  mt::basisConverter basis_converter;

  if (par.basis == "MINVO")
  {
    A_rest_pos_basis_ = basis_converter.getArestMinvo();
  }
  else if (par.basis == "BEZIER")
  {
    A_rest_pos_basis_ = basis_converter.getArestBezier();
  }
  else if (par.basis == "B_SPLINE")
  {
    A_rest_pos_basis_ = basis_converter.getArestBSpline();
  }
  else
  {
    std::cout << red << "Basis " << par.basis << " not implemented yet" << reset << std::endl;
    std::cout << red << "============================================" << reset << std::endl;
    abort();
  }

  A_rest_pos_basis_inverse_ = A_rest_pos_basis_.inverse();

#if USE_GUROBI_FLAG
  solver_ = std::unique_ptr<SolverGurobi>(new SolverGurobi(par_for_solver));
#else
  solver_ = std::unique_ptr<SolverNlopt>(new SolverNlopt(par_for_solver));
#endif

  // solver_ = new SolverNlopt(par_for_solver);
  // solver_ = new SolverGurobi(par_for_solver);

  separator_solver_ = new separator::Separator();
}

void Mader::dynTraj2dynTrajCompiled(const mt::dynTraj& traj, mt::dynTrajCompiled& traj_compiled)
{
  mtx_t_.lock();
  for (auto function_i : traj.function)
  {
    typedef exprtk::symbol_table<double> symbol_table_t;
    typedef exprtk::expression<double> expression_t;
    typedef exprtk::parser<double> parser_t;

    symbol_table_t symbol_table;
    symbol_table.add_variable("t", t_);
    symbol_table.add_constants();
    expression_t expression;
    expression.register_symbol_table(symbol_table);

    parser_t parser;
    parser.compile(function_i, expression);

    traj_compiled.function.push_back(expression);
  }

  mtx_t_.unlock();

  traj_compiled.bbox = traj.bbox;
  traj_compiled.id = traj.id;
  traj_compiled.time_received = traj.time_received;  // ros::Time::now().toSec();

  traj_compiled.is_static =
      ((traj.is_agent == false) &&                           // is an obstacle and
       (traj.function[0].find("t") == std::string::npos) &&  // there is no dependence on t in the coordinate x
       (traj.function[1].find("t") == std::string::npos) &&  // there is no dependence on t in the coordinate y
       (traj.function[2].find("t") == std::string::npos))    // there is no dependence on t in the coordinate z
      ||                                                     // OR
      (traj.is_agent == true && fabs(traj.pwp.times.back() - traj.pwp.times.front()) < 1e-7);

  traj_compiled.pwp = traj.pwp;
}
// Note that we need to compile the trajectories inside mader.cpp because t_ is in mader.hpp
void Mader::updateTrajObstacles(mt::dynTraj traj)
{
  MyTimer tmp_t(true);

  if (started_check_ == true && traj.is_agent == true)
  {
    have_received_trajectories_while_checking_ = true;
  }

  mtx_trajs_.lock();

  std::vector<mt::dynTrajCompiled>::iterator obs_ptr =
      std::find_if(trajs_.begin(), trajs_.end(),
                   [=](const mt::dynTrajCompiled& traj_compiled) { return traj_compiled.id == traj.id; });

  bool exists_in_local_map = (obs_ptr != std::end(trajs_));

  mt::dynTrajCompiled traj_compiled;
  dynTraj2dynTrajCompiled(traj, traj_compiled);

  if (exists_in_local_map)
  {  // if that object already exists, substitute its trajectory
    *obs_ptr = traj_compiled;
  }
  else
  {  // if it doesn't exist, add it to the local map
    trajs_.push_back(traj_compiled);
    // ROS_WARN_STREAM("Adding " << traj_compiled.id);
  }

  // and now let's delete those trajectories of the obs/agents whose current positions are outside the local map
  // Note that these positions are obtained with the trajectory stored in the past in the local map
  std::vector<int> ids_to_remove;

  for (int index_traj = 0; index_traj < trajs_.size(); index_traj++)
  {
    bool traj_affects_me = false;

    mtx_t_.lock();
    t_ = ros::Time::now().toSec();

    Eigen::Vector3d center_obs;
    center_obs << trajs_[index_traj].function[0].value(),  ////////////////////
        trajs_[index_traj].function[1].value(),            ////////////////
        trajs_[index_traj].function[2].value();            /////////////////

    mtx_t_.unlock();

    // mtx_t_.unlock();
    if (((traj_compiled.is_static == true) && (center_obs - state_.pos).norm() > 2 * par_.Ra) ||  ////
        ((traj_compiled.is_static == false) && (center_obs - state_.pos).norm() > 4 * par_.Ra))
    // #### Static Obstacle: 2*Ra because: traj_{k-1} is inside a sphere of Ra.
    // Then, in iteration k the point A (which I don't
    // know yet)  is taken along that trajectory, and
    // another trajectory of radius Ra will be obtained.
    // Therefore, I need to take 2*Ra to make sure the
    // extreme case (A taken at the end of traj_{k-1} is
    // covered).

    // #### Dynamic Agent: 4*Ra. Same reasoning as above, but with two agets
    // #### Dynamic Obstacle: 4*Ra, it's a heuristics.

    // ######REMEMBER######
    // Note that removeTrajsThatWillNotAffectMe will later
    // on take care of deleting the ones I don't need once
    // I know A
    {
      ids_to_remove.push_back(trajs_[index_traj].id);
    }
  }

  for (auto id : ids_to_remove)
  {
    // ROS_WARN_STREAM("Removing " << id);
    trajs_.erase(
        std::remove_if(trajs_.begin(), trajs_.end(), [&](mt::dynTrajCompiled const& traj) { return traj.id == id; }),
        trajs_.end());
  }

  mtx_trajs_.unlock();

  have_received_trajectories_while_checking_ = false;
  // std::cout << bold << blue << "updateTrajObstacles took " << tmp_t << reset << std::endl;
}

std::vector<Eigen::Vector3d> Mader::vertexesOfInterval(mt::PieceWisePol& pwp, double t_start, double t_end,
                                                       const Eigen::Vector3d& delta)
{
  std::vector<Eigen::Vector3d> points;

  std::vector<double>::iterator low = std::lower_bound(pwp.times.begin(), pwp.times.end(), t_start);
  std::vector<double>::iterator up = std::upper_bound(pwp.times.begin(), pwp.times.end(), t_end);

  // Example: times=[1 2 3 4 5 6 7]
  // t_start=1.5;
  // t_end=5.5
  // then low points to "2" (low - pwp.times.begin() is 1)
  // and up points to "6" (up - pwp.times.begin() is 5)

  int index_first_interval = low - pwp.times.begin() - 1;  // index of the interval [1,2]
  int index_last_interval = up - pwp.times.begin() - 1;    // index of the interval [5,6]

  mu::saturate(index_first_interval, 0, (int)(pwp.coeff_x.size() - 1));
  mu::saturate(index_last_interval, 0, (int)(pwp.coeff_x.size() - 1));

  Eigen::Matrix<double, 3, 4> P;
  Eigen::Matrix<double, 3, 4> V;

  // push all the complete intervals
  for (int i = index_first_interval; i <= index_last_interval; i++)
  {
    P.row(0) = pwp.coeff_x[i];
    P.row(1) = pwp.coeff_y[i];
    P.row(2) = pwp.coeff_z[i];

    V = P * A_rest_pos_basis_inverse_;

    for (int j = 0; j < V.cols(); j++)
    {
      double x = V(0, j);
      double y = V(1, j);
      double z = V(2, j);  //[x,y,z] is the point

      if (delta.norm() < 1e-6)
      {  // no inflation
        points.push_back(Eigen::Vector3d(x, y, z));
      }
      else
      {
        // points.push_back(Eigen::Vector3d(V(1, j), V(2, j), V(3, j)));  // x,y,z
        points.push_back(Eigen::Vector3d(x + delta.x(), y + delta.y(), z + delta.z()));
        points.push_back(Eigen::Vector3d(x + delta.x(), y - delta.y(), z - delta.z()));
        points.push_back(Eigen::Vector3d(x + delta.x(), y + delta.y(), z - delta.z()));
        points.push_back(Eigen::Vector3d(x + delta.x(), y - delta.y(), z + delta.z()));
        points.push_back(Eigen::Vector3d(x - delta.x(), y - delta.y(), z - delta.z()));
        points.push_back(Eigen::Vector3d(x - delta.x(), y + delta.y(), z + delta.z()));
        points.push_back(Eigen::Vector3d(x - delta.x(), y + delta.y(), z - delta.z()));
        points.push_back(Eigen::Vector3d(x - delta.x(), y - delta.y(), z + delta.z()));
      }
    }
  }

  return points;
}

// return a vector that contains all the vertexes of the polyhedral approx of an interval.
std::vector<Eigen::Vector3d> Mader::vertexesOfInterval(mt::dynTrajCompiled& traj, double t_start, double t_end)
{
  Eigen::Vector3d delta = Eigen::Vector3d::Zero();
  if (traj.is_agent == false)
  {
    std::vector<Eigen::Vector3d> points;
    delta = traj.bbox / 2.0 + (par_.drone_radius + par_.beta + par_.alpha) *
                                  Eigen::Vector3d::Ones();  // every side of the box will be increased by 2*delta
                                                            //(+delta on one end, -delta on the other)
    // Will always have a sample at the beginning of the interval, and another at the end.
    for (double t = t_start;                           /////////////
         (t < t_end) ||                                /////////////
         ((t > t_end) && ((t - t_end) < par_.gamma));  /////// This is to ensure we have a sample a the end
         t = t + par_.gamma)
    {
      mtx_t_.lock();
      t_ = std::min(t, t_end);  // this min only has effect on the last sample

      double x = traj.function[0].value();
      double y = traj.function[1].value();
      double z = traj.function[2].value();
      mtx_t_.unlock();

      //"Minkowski sum along the trajectory: box centered on the trajectory"
      points.push_back(Eigen::Vector3d(x + delta.x(), y + delta.y(), z + delta.z()));
      points.push_back(Eigen::Vector3d(x + delta.x(), y - delta.y(), z - delta.z()));
      points.push_back(Eigen::Vector3d(x + delta.x(), y + delta.y(), z - delta.z()));
      points.push_back(Eigen::Vector3d(x + delta.x(), y - delta.y(), z + delta.z()));
      points.push_back(Eigen::Vector3d(x - delta.x(), y - delta.y(), z - delta.z()));
      points.push_back(Eigen::Vector3d(x - delta.x(), y + delta.y(), z + delta.z()));
      points.push_back(Eigen::Vector3d(x - delta.x(), y + delta.y(), z - delta.z()));
      points.push_back(Eigen::Vector3d(x - delta.x(), y - delta.y(), z + delta.z()));
    }

    return points;
  }
  else
  {  // is an agent --> use the pwp field

    delta = traj.bbox / 2.0 + (par_.drone_radius) * Eigen::Vector3d::Ones();
    // std::cout << "****traj.bbox = " << traj.bbox << std::endl;
    // std::cout << "****par_.drone_radius = " << par_.drone_radius << std::endl;
    // std::cout << "****Inflation by delta= " << delta.transpose() << std::endl;

    return vertexesOfInterval(traj.pwp, t_start, t_end, delta);
  }
}

// See https://doc.cgal.org/Manual/3.7/examples/Convex_hull_3/quickhull_3.cpp
CGAL_Polyhedron_3 Mader::convexHullOfInterval(mt::dynTrajCompiled& traj, double t_start, double t_end)
{
  std::vector<Eigen::Vector3d> points = vertexesOfInterval(traj, t_start, t_end);

  std::vector<Point_3> points_cgal;
  for (auto point_i : points)
  {
    points_cgal.push_back(Point_3(point_i.x(), point_i.y(), point_i.z()));
  }

  return cu::convexHullOfPoints(points_cgal);
}

// trajs_ is already locked when calling this function
void Mader::removeTrajsThatWillNotAffectMe(const mt::state& A, double t_start, double t_end)
{
  std::vector<int> ids_to_remove;

  for (auto traj : trajs_)
  {
    bool traj_affects_me = false;

    // STATIC OBSTACLES/AGENTS
    if (traj.is_static == true)
    {
      mtx_t_.lock();
      t_ = t_start;  // which is constant along the trajectory

      Eigen::Vector3d center_obs;
      if (traj.is_agent == false)
      {
        center_obs << traj.function[0].value(), traj.function[1].value(), traj.function[2].value();
      }
      else
      {
        center_obs = traj.pwp.eval(t_);
      }

      mtx_t_.unlock();
      // mtx_t_.unlock();
      Eigen::Vector3d positive_half_diagonal;
      positive_half_diagonal << traj.bbox[0] / 2.0, traj.bbox[1] / 2.0, traj.bbox[2] / 2.0;

      Eigen::Vector3d c1 = center_obs - positive_half_diagonal;
      Eigen::Vector3d c2 = center_obs + positive_half_diagonal;
      traj_affects_me = mu::boxIntersectsSphere(A.pos, par_.Ra, c1, c2);
    }
    else
    {                                                            // DYNAMIC OBSTACLES/AGENTS
      double deltaT = (t_end - t_start) / (1.0 * par_.num_pol);  // num_pol is the number of intervals
      for (int i = 0; i < par_.num_pol; i++)                     // for each interval
      {
        std::vector<Eigen::Vector3d> points =
            vertexesOfInterval(traj, t_start + i * deltaT, t_start + (i + 1) * deltaT);

        for (auto point_i : points)  // for every vertex of each interval
        {
          if ((point_i - A.pos).norm() <= par_.Ra)
          {
            traj_affects_me = true;
            goto exit;
          }
        }
      }
    }

  exit:
    if (traj_affects_me == false)
    {
      // std::cout << red << bold << "Going to  delete traj " << trajs_[index_traj].id << reset << std::endl;
      ids_to_remove.push_back(traj.id);
    }
  }

  for (auto id : ids_to_remove)
  {
    // ROS_INFO_STREAM("traj " << id << " doesn't affect me");
    trajs_.erase(
        std::remove_if(trajs_.begin(), trajs_.end(), [&](mt::dynTrajCompiled const& traj) { return traj.id == id; }),
        trajs_.end());
  }

  /*  std::cout << "After deleting the trajectory, we have these ids= " << std::endl;

    for (auto traj : trajs_)
    {
      std::cout << traj.id << std::endl;
    }*/
}

bool Mader::IsTranslating()
{
  return (drone_status_ == DroneStatus::GOAL_SEEN || drone_status_ == DroneStatus::TRAVELING);
}

ConvexHullsOfCurve Mader::convexHullsOfCurve(mt::dynTrajCompiled& traj, double t_start, double t_end)
{
  ConvexHullsOfCurve convexHulls;
  double deltaT = (t_end - t_start) / (1.0 * par_.num_pol);  // num_pol is the number of intervals

  for (int i = 0; i < par_.num_pol; i++)
  {
    convexHulls.push_back(convexHullOfInterval(traj, t_start + i * deltaT, t_start + (i + 1) * deltaT));
  }

  return convexHulls;
}

ConvexHullsOfCurves Mader::convexHullsOfCurves(double t_start, double t_end)
{
  ConvexHullsOfCurves result;

  for (auto traj : trajs_)
  {
    result.push_back(convexHullsOfCurve(traj, t_start, t_end));
  }

  return result;
}

void Mader::setTerminalGoal(mt::state& term_goal)
{
  mtx_G_term.lock();
  mtx_G.lock();
  mtx_state.lock();
  mtx_planner_status_.lock();

  G_term_.pos = term_goal.pos;
  Eigen::Vector3d temp = state_.pos;
  G_.pos = G_term_.pos;
  if (drone_status_ == DroneStatus::GOAL_REACHED)
  {
    changeDroneStatus(DroneStatus::YAWING);
  }
  if (drone_status_ == DroneStatus::GOAL_SEEN)
  {
    changeDroneStatus(DroneStatus::TRAVELING);
  }
  terminal_goal_initialized_ = true;

  // std::cout << bold << red << "[FA] Received Term Goal=" << G_term_.pos.transpose() << reset << std::endl;
  // std::cout << bold << red << "[FA] Received Proj Goal=" << G_.pos.transpose() << reset << std::endl;

  mtx_state.unlock();
  mtx_G.unlock();
  mtx_G_term.unlock();
  mtx_planner_status_.unlock();
}

void Mader::getG(mt::state& G)
{
  G = G_;
}

void Mader::getState(mt::state& data)
{
  mtx_state.lock();
  data = state_;
  mtx_state.unlock();
}

void Mader::updateState(mt::state data)
{
  state_ = data;

  if (state_initialized_ == false)
  {
    mt::state tmp;
    tmp.pos = data.pos;
    tmp.yaw = data.yaw;
    plan_.push_back(tmp);
    previous_yaw_ = tmp.yaw;
  }

  state_initialized_ = true;
}

bool Mader::initializedAllExceptPlanner()
{
  if (!state_initialized_ || !terminal_goal_initialized_)
  {
    /*    std::cout << "state_initialized_= " << state_initialized_ << std::endl;
        std::cout << "terminal_goal_initialized_= " << terminal_goal_initialized_ << std::endl;*/
    return false;
  }
  return true;
}

bool Mader::initializedStateAndTermGoal()
{
  if (!state_initialized_ || !terminal_goal_initialized_)
  {
    return false;
  }
  return true;
}

bool Mader::initialized()
{
  if (!state_initialized_ || !terminal_goal_initialized_ || !planner_initialized_)
  {
    /*    std::cout << "state_initialized_= " << state_initialized_ << std::endl;
        std::cout << "terminal_goal_initialized_= " << terminal_goal_initialized_ << std::endl;
        std::cout << "planner_initialized_= " << planner_initialized_ << std::endl;*/
    return false;
  }
  return true;
}

// check wheter a mt::dynTrajCompiled and a pwp_optimized are in collision in the interval [t_start, t_end]
bool Mader::trajsAndPwpAreInCollision(mt::dynTrajCompiled traj, mt::PieceWisePol pwp_optimized, double t_start,
                                      double t_end)
{
  Eigen::Vector3d n_i;
  double d_i;

  double deltaT = (t_end - t_start) / (1.0 * par_.num_pol);  // num_pol is the number of intervals
  for (int i = 0; i < par_.num_pol; i++)                     // for each interval
  {
    // This is my trajectory (no inflation)
    std::vector<Eigen::Vector3d> pointsA =
        vertexesOfInterval(pwp_optimized, t_start + i * deltaT, t_start + (i + 1) * deltaT, Eigen::Vector3d::Zero());

    // This is the trajectory of the other agent/obstacle
    std::vector<Eigen::Vector3d> pointsB = vertexesOfInterval(traj, t_start + i * deltaT, t_start + (i + 1) * deltaT);

    // std::cout << "Going to solve model with pointsA.size()= " << pointsA.size() << std::endl;
    // for (auto point_i : pointsA)
    // {
    //   std::cout << point_i.transpose() << std::endl;
    // }

    // std::cout << "Going to solve model with pointsB.size()= " << pointsB.size() << std::endl;
    // for (auto point_i : pointsB)
    // {
    //   std::cout << point_i.transpose() << std::endl;
    // }

    if (separator_solver_->solveModel(n_i, d_i, pointsA, pointsB) == false)
    {
      return true;  // There is not a solution --> they collide
    }
  }

  // if reached this point, they don't collide
  return false;
}
// Checks that I have not received new trajectories that affect me while doing the optimization
bool Mader::safetyCheckAfterOpt(mt::PieceWisePol pwp_optimized)
{
  started_check_ = true;

  bool result = true;
  for (auto traj : trajs_)
  {
    if (traj.time_received > time_init_opt_ && traj.is_agent == true)
    {
      if (trajsAndPwpAreInCollision(traj, pwp_optimized, pwp_optimized.times.front(), pwp_optimized.times.back()))
      {
        ROS_ERROR_STREAM("Traj collides with " << traj.id);
        result = false;  // will have to redo the optimization
        break;
      }
    }
  }

  // and now do another check in case I've received anything while I was checking. Note that mtx_trajs_ is locked!
  if (have_received_trajectories_while_checking_ == true)
  {
    ROS_ERROR_STREAM("Recvd traj while checking ");
    result = false;
  }
  started_check_ = false;

  return result;
}

bool Mader::isReplanningNeeded()
{
  if (initializedStateAndTermGoal() == false)
  {
    return false;
  }

  //////////////////////////////////////////////////////////////////////////
  mtx_G_term.lock();

  mt::state G_term = G_term_;  // Local copy of the terminal terminal goal

  mtx_G_term.unlock();

  // Check if we have reached the goal
  double dist_to_goal = (G_term.pos - plan_.front().pos).norm();
  // std::cout << "dist_to_goal= " << dist_to_goal << std::endl;
  if (dist_to_goal < par_.goal_radius)
  {
    changeDroneStatus(DroneStatus::GOAL_REACHED);
    exists_previous_pwp_ = false;
  }

  // Check if we have seen the goal in the last replan
  mtx_plan_.lock();
  double dist_last_plan_to_goal = (G_term.pos - plan_.back().pos).norm();
  // std::cout << "dist_last_plan_to_goal= " << dist_last_plan_to_goal << std::endl;
  mtx_plan_.unlock();
  if (dist_last_plan_to_goal < par_.goal_radius && drone_status_ == DroneStatus::TRAVELING)
  {
    changeDroneStatus(DroneStatus::GOAL_SEEN);
    std::cout << "Status changed to GOAL_SEEN!" << std::endl;
    exists_previous_pwp_ = false;
  }

  // Don't plan if drone is not traveling
  if (drone_status_ == DroneStatus::GOAL_REACHED || (drone_status_ == DroneStatus::YAWING) ||
      (drone_status_ == DroneStatus::GOAL_SEEN))
  {
    // std::cout << "No replanning needed because" << std::endl;
    // printDroneStatus();
    return false;
  }
  return true;
}

bool Mader::replan(mt::Edges& edges_obstacles_out, std::vector<mt::state>& X_safe_out,
                   std::vector<Hyperplane3D>& planes, int& num_of_LPs_run, int& num_of_QCQPs_run,
                   mt::PieceWisePol& pwp_out)
{
  if (isReplanningNeeded() == false)
  {
    return false;
  }

  MyTimer replanCB_t(true);

  std::cout << bold << on_white << "**********************IN REPLAN CB*******************" << reset << std::endl;

  //////////////////////////////////////////////////////////////////////////
  ///////////////////////// Select mt::state A /////////////////////////////////
  //////////////////////////////////////////////////////////////////////////

  mtx_G_term.lock();
  mt::state G_term = G_term_;  // Local copy of the terminal terminal goal
  mtx_G_term.unlock();

  mt::state A;
  int k_index_end, k_index;

  // If k_index_end=0, then A = plan_.back() = plan_[plan_.size() - 1]

  mtx_plan_.lock();

  mu::saturate(deltaT_, par_.lower_bound_runtime_snlopt / par_.dc, par_.upper_bound_runtime_snlopt / par_.dc);

  k_index_end = std::max((int)(plan_.size() - deltaT_), 0);

  if (plan_.size() < 5)
  {
    k_index_end = 0;
  }

  k_index = plan_.size() - 1 - k_index_end;
  A = plan_.get(k_index);

  mtx_plan_.unlock();

  // std::cout << blue << "k_index:" << k_index << reset << std::endl;
  // std::cout << blue << "k_index_end:" << k_index_end << reset << std::endl;
  // std::cout << blue << "plan_.size():" << plan_.size() << reset << std::endl;

  double runtime_snlopt;

  if (k_index_end != 0)
  {
    runtime_snlopt = k_index * par_.dc;  // std::min(, par_.upper_bound_runtime_snlopt);
  }
  else
  {
    runtime_snlopt = par_.upper_bound_runtime_snlopt;  // I'm stopped at the end of the trajectory --> take my
                                                       // time to replan
  }
  mu::saturate(runtime_snlopt, par_.lower_bound_runtime_snlopt, par_.upper_bound_runtime_snlopt);

  // std::cout << green << "Runtime snlopt= " << runtime_snlopt << reset << std::endl;

  //////////////////////////////////////////////////////////////////////////
  ///////////////////////// Get point G ////////////////////////////////////
  //////////////////////////////////////////////////////////////////////////

  double distA2TermGoal = (G_term.pos - A.pos).norm();
  double ra = std::min((distA2TermGoal - 0.001), par_.Ra);  // radius of the sphere S
  mt::state G;
  G.pos = A.pos + ra * (G_term.pos - A.pos).normalized();

  mt::state initial = A;
  mt::state final = G;

  mtx_G.lock();
  G_ = G;
  mtx_G.unlock();

  //////////////////////////////////////////////////////////////////////////
  ///////////////////////// Solve optimization! ////////////////////////////
  //////////////////////////////////////////////////////////////////////////

  solver_->setMaxRuntimeKappaAndMu(runtime_snlopt, par_.kappa, par_.mu);

  //////////////////////
  double time_now = ros::Time::now().toSec();  // TODO this ros dependency shouldn't be here

  double t_start = k_index * par_.dc + time_now;

  double factor_alloc = (distA2TermGoal > par_.dist_factor_alloc_close) ? par_.factor_alloc : par_.factor_alloc_close;

  double time_allocated = factor_alloc * mu::getMinTimeDoubleIntegrator3D(initial.pos, initial.vel, final.pos,
                                                                          final.vel, par_.v_max, par_.a_max);

  std::cout << "time_allocated= " << time_allocated << std::endl;

  double t_final = t_start + time_allocated;

  bool correctInitialCond =
      solver_->setInitStateFinalStateInitTFinalT(initial, final, t_start,
                                                 t_final);  // note that here t_final may have been updated

  if (correctInitialCond == false)
  {
    std::cout << bold << red << "The solver cannot guarantee feasibility for v1" << reset << std::endl;
    return false;
  }

  ////////////////

  mtx_trajs_.lock();

  time_init_opt_ = ros::Time::now().toSec();
  removeTrajsThatWillNotAffectMe(A, t_start, t_final);
  ConvexHullsOfCurves hulls = convexHullsOfCurves(t_start, t_final);
  mtx_trajs_.unlock();

  mt::ConvexHullsOfCurves_Std hulls_std = cu::vectorGCALPol2vectorStdEigen(hulls);
  // poly_safe_out = cu::vectorGCALPol2vectorJPSPol(hulls);
  edges_obstacles_out = cu::vectorGCALPol2edges(hulls);

  solver_->setHulls(hulls_std);

  //////////////////////
  std::cout << on_cyan << bold << "Solved so far" << solutions_found_ << "/" << total_replannings_ << reset
            << std::endl;

  std::cout << "[FA] Calling NL" << std::endl;

  bool result = solver_->optimize();

  num_of_LPs_run = solver_->getNumOfLPsRun();
  num_of_QCQPs_run = solver_->getNumOfQCQPsRun();

  total_replannings_++;
  if (result == false)
  {
    int states_last_replan = ceil(replanCB_t.ElapsedMs() / (par_.dc * 1000));  // Number of states that
                                                                               // would have been needed for
                                                                               // the last replan
    deltaT_ = std::max(par_.factor_alpha * states_last_replan, 1.0);
    deltaT_ = std::min(1.0 * deltaT_, 2.0 / par_.dc);
    return false;
  }

  solver_->getPlanes(planes);

  solutions_found_++;

  // av_improvement_nlopt_ = ((solutions_found_ - 1) * av_improvement_nlopt_ + solver_->improvement_) /
  // solutions_found_;

  // std::cout << blue << "Average improvement so far" << std::setprecision(5) << av_improvement_nlopt_ << reset
  //          << std::endl;

  mt::PieceWisePol pwp_now;
  solver_->getSolution(pwp_now);

  MyTimer check_t(true);
  mtx_trajs_.lock();
  bool is_safe_after_opt = safetyCheckAfterOpt(pwp_now);
  mtx_trajs_.unlock();
  // std::cout << bold << "Check Timer=" << check_t << std::endl;

  if (is_safe_after_opt == false)
  {
    ROS_ERROR_STREAM("safetyCheckAfterOpt is not satisfied, returning");
    return false;
  }

  M_ = G_term;

  //////////////////////////////////////////////////////////////////////////
  ///////////////////////// Append to plan /////////////////////////////////
  //////////////////////////////////////////////////////////////////////////
  mtx_plan_.lock();

  int plan_size = plan_.size();

  if ((plan_size - 1 - k_index_end) < 0)
  {
    std::cout << bold << red << "Already published the point A" << reset << std::endl;
    // std::cout << "plan_size= " << plan_size << std::endl;
    // std::cout << "k_index_end= " << k_index_end << std::endl;
    mtx_plan_.unlock();
    return false;
  }
  else
  {
    // std::cout << "Appending" << std::endl;
    // std::cout << "before, plan_size=" << plan_.size() << std::endl;
    plan_.erase(plan_.end() - k_index_end - 1, plan_.end());  // this deletes also the initial condition...
    // std::cout << "middle, plan_size=" << plan_.size() << " sol.size()=" << (solver_->traj_solution_).size()
    // << std::endl;
    for (int i = 0; i < (solver_->traj_solution_).size(); i++)  //... which is included in traj_solution_[0]
    {
      plan_.push_back(solver_->traj_solution_[i]);
    }
    // std::cout << "after, plan_size=" << plan_.size() << std::endl;
  }

  mtx_plan_.unlock();

  ////////////////////
  ////////////////////

  if (exists_previous_pwp_ == true)
  {
    pwp_out = mu::composePieceWisePol(time_now, par_.dc, pwp_prev_, pwp_now);
    pwp_prev_ = pwp_out;
  }
  else
  {  //
    pwp_out = pwp_now;
    pwp_prev_ = pwp_now;
    exists_previous_pwp_ = true;
  }

  X_safe_out = plan_.toStdVector();

  ///////////////////////////////////////////////////////////
  ///////////////       OTHER STUFF    //////////////////////
  //////////////////////////////////////////////////////////

  // Check if we have planned until G_term
  mt::state F = plan_.back();  // Final point of the safe path (\equiv final point of the comitted path)
  double dist = (G_term_.pos - F.pos).norm();

  if (dist < par_.goal_radius)
  {
    changeDroneStatus(DroneStatus::GOAL_SEEN);
  }

  mtx_offsets.lock();

  int states_last_replan = ceil(replanCB_t.ElapsedMs() / (par_.dc * 1000));  // Number of states that
                                                                             // would have been needed for
                                                                             // the last replan
  deltaT_ = std::max(par_.factor_alpha * states_last_replan, 1.0);
  mtx_offsets.unlock();

  planner_initialized_ = true;

  return true;
}

void Mader::resetInitialization()
{
  planner_initialized_ = false;
  state_initialized_ = false;

  terminal_goal_initialized_ = false;
}

void Mader::yaw(double diff, mt::state& next_goal)
{
  mu::saturate(diff, -par_.dc * par_.w_max, par_.dc * par_.w_max);
  double dyaw_not_filtered;

  dyaw_not_filtered = copysign(1, diff) * par_.w_max;

  dyaw_filtered_ = (1 - par_.alpha_filter_dyaw) * dyaw_not_filtered + par_.alpha_filter_dyaw * dyaw_filtered_;
  next_goal.dyaw = dyaw_filtered_;
  next_goal.yaw = previous_yaw_ + dyaw_filtered_ * par_.dc;
}

void Mader::getDesiredYaw(mt::state& next_goal)
{
  double diff = 0.0;
  double desired_yaw = 0.0;

  switch (drone_status_)
  {
    case DroneStatus::YAWING:
      desired_yaw = atan2(G_term_.pos[1] - next_goal.pos[1], G_term_.pos[0] - next_goal.pos[0]);
      diff = desired_yaw - state_.yaw;
      break;
    case DroneStatus::TRAVELING:
    case DroneStatus::GOAL_SEEN:
      desired_yaw = atan2(M_.pos[1] - next_goal.pos[1], M_.pos[0] - next_goal.pos[0]);
      diff = desired_yaw - state_.yaw;
      break;
    case DroneStatus::GOAL_REACHED:
      next_goal.dyaw = 0.0;
      next_goal.yaw = previous_yaw_;
      return;
  }

  mu::angle_wrap(diff);
  if (fabs(diff) < 0.04 && drone_status_ == DroneStatus::YAWING)
  {
    changeDroneStatus(DroneStatus::TRAVELING);
  }
  yaw(diff, next_goal);
}

bool Mader::getNextGoal(mt::state& next_goal)
{
  if (initializedStateAndTermGoal() == false)  // || (drone_status_ == DroneStatus::GOAL_REACHED && plan_.size() == 1))
  {
    // std::cout << "Not publishing new goal!!" << std::endl;
    return false;
  }

  mtx_goals.lock();
  mtx_plan_.lock();

  next_goal.setZero();
  next_goal = plan_.front();

  if (plan_.size() > 1)
  {
    plan_.pop_front();
  }
  getDesiredYaw(next_goal);

  previous_yaw_ = next_goal.yaw;

  mtx_goals.unlock();
  mtx_plan_.unlock();
  return true;
}

// Debugging functions
void Mader::changeDroneStatus(int new_status)
{
  if (new_status == drone_status_)
  {
    return;
  }

  std::cout << "Changing DroneStatus from ";
  switch (drone_status_)
  {
    case DroneStatus::YAWING:
      std::cout << bold << "YAWING" << reset;
      break;
    case DroneStatus::TRAVELING:
      std::cout << bold << "TRAVELING" << reset;
      break;
    case DroneStatus::GOAL_SEEN:
      std::cout << bold << "GOAL_SEEN" << reset;
      break;
    case DroneStatus::GOAL_REACHED:
      std::cout << bold << "GOAL_REACHED" << reset;
      break;
  }
  std::cout << " to ";

  switch (new_status)
  {
    case DroneStatus::YAWING:
      std::cout << bold << "YAWING" << reset;
      break;
    case DroneStatus::TRAVELING:
      std::cout << bold << "TRAVELING" << reset;
      break;
    case DroneStatus::GOAL_SEEN:
      std::cout << bold << "GOAL_SEEN" << reset;
      break;
    case DroneStatus::GOAL_REACHED:
      std::cout << bold << "GOAL_REACHED" << reset;
      break;
  }

  std::cout << std::endl;

  drone_status_ = new_status;
}

void Mader::printDroneStatus()
{
  switch (drone_status_)
  {
    case DroneStatus::YAWING:
      std::cout << bold << "status_=YAWING" << reset << std::endl;
      break;
    case DroneStatus::TRAVELING:
      std::cout << bold << "status_=TRAVELING" << reset << std::endl;
      break;
    case DroneStatus::GOAL_SEEN:
      std::cout << bold << "status_=GOAL_SEEN" << reset << std::endl;
      break;
    case DroneStatus::GOAL_REACHED:
      std::cout << bold << "status_=GOAL_REACHED" << reset << std::endl;
      break;
  }
}


#pragma once

#include <Eigen/Core>
#include "vgl/math/constants.hpp"

namespace vgl {
    struct Attenuation {
        float constant = 1.0;
        float linear = 0.0;
        float quadratic = 0.0;
        float pad1 = 0.0;
    };

    struct Light {
        Eigen::Vector4f pos = Eigen::Vector4f::Zero();
        Eigen::Vector4f color = Eigen::Vector4f::Zero();
        Eigen::Vector4f dir = Eigen::Vector4f(0.0, -1.0, 0.0, 0.0);
        Attenuation attenuation;
        float outer_cutoff{ math::pi<float> / 4.0f };
        float inner_cutoff{ math::pi<float> / 4.5f };
        int type = 1;
        int pad1;
    };
}


//=======================================================================
// Copyright 2015 - 2020 Jeff Linahan
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================
#include "theorem4.h"
#include "strutil.h"
#include "graphutil.h"
#include "lemmas.h"
#include "BFSVisitor.h"
#include <boost/graph/copy.hpp>
#include <boost/graph/graph_concepts.hpp>
#include <boost/graph/is_straight_line_drawing.hpp>
#include <boost/graph/make_maximal_planar.hpp>
#include <boost/graph/copy.hpp>
#include <boost/bimap.hpp>
#include <boost/config.hpp>
#include <iostream>
using namespace std;
using namespace boost;

// may be able to inline this and hoist it out of the if blocks of theorem4_disconnected if we're able to get rid of bigger_than_two_thirds
uint lowest_i(uint n, uint num_components, vector<uint> const& num_verts_per_component)
{
        uint total_cost = 0;
        uint i = 0;
        for( ; i < num_components; ++i ){
                total_cost += num_verts_per_component[i];
                if ( total_cost*3 > n ) return i;
        }

        return -1;
}

// L[l] = # of vertices on level l
Partition theorem4_connected(GraphCR g, vector<uint> const& L, uint l[3], uint r, Graph* g_shrunk, BFSVisitorData const* bfs2)
{
        uint n = num_vertices(g);
        cout << "g:\n";
        //print_graph(g);
        vertex_t v;

        // l1 = the level such that the sum of costs in levels 0 thru l1-1 < 1/2, but the sum of costs in levels 0 thru l1 is >= 1/2
        uint total = 0;
        uint level = 0;
        while( total < n/2 ){
                total += L[level++];
        }

        uint l1 = level;

        BOOST_ASSERT(1 == L[0]);
        // Partition the vertices into levels according to their distance from some vertex v.
        //uint r;
        /*If r is the maximum distance of any vertex from v, define additional levels -1 and r+1 containing no vertices*/
        //uint l[3];
        //(If no such l1 exists, the total cost of all vertices < 1/2, and B = C = {} and return true) */
        uint k = 0;// = # of vertices on levels 0 thru l1.
        for( uint i = 0; i <= l1; ++i ) k += L[i];

        /*Find a level l0 such that l0 <= l1 and |L[l0]| + 2(l1-l0) <= 2sqrt(k)
        Find a level l2 such that l1+1 <= l2 and |L[l2] + 2(l2-l1-1) <= 2sqrt(n-k) */
        uint sqrtk = 2*sqrt(k);
        uint sqrtnk = 2*sqrt(n-k);

        uint l0 = l1;
        while( L[l0] + 2*(l1-l0) > sqrtk ) --l0;

        uint l2 = l1+1;
        while( L[l2] + 2*(l2-l1-1) > sqrtnk) ++l2;

        BOOST_ASSERT(l0 <= l1);
        BOOST_ASSERT(l1+1 <= l2);

        BFSVisitorData bfs(&g, *vertices(g).first);
        breadth_first_search(g, bfs.root, boost::visitor(BFSVisitor(bfs)));
        cout << "bfsroot: " << bfs.root << '\n';

        vector<vertex_t> cycle;
        l1 -= 2;
        --l2;
        Partition p = lemma3(g, L, l1, l2, r, bfs, *bfs2, cycle, g_shrunk);

        uint climit = sqrtk - sqrtnk;
        BOOST_ASSERT(p.verify_edges(g));
        //BOOST_ASSERT(p.verify_sizes_lemma3(L, l1, l2));

        /*If 2 such levels exist, then by Lemma 3 the vertices of G can be partitioned into three sets A, B, C such that no edge joins a vertex in A with a vertex in B,
        neither A or C has cost > 2/3, and C contains no more than 2(sqrt(k) + sqrt(n-k)) vertices.
        But 2(sqrt(k) + sqrt(n-k) <= 2(sqrt(n/2) + sqrt(n/2)) = 2sqrt(2)sqrt(n)
        Thus the theorem holds if suitable levels l0 and l2 exist
                Suppose a suitable level l0 does not exist.  Then, for i <= l1, L[i] >= 2sqrt(k) - 2(l1-i)
                Since L[0] = 1, this means 1 >= 2sqrt(k) - 2l1 and l1 + 1/2 >= sqrt(k).  Thus l1 = floor(l1 + 1/2) >
                Contradiction*/

        return p;
}

Partition theorem4_ccbigger23(GraphCR g_all, Partition const& biggest_comp_p)
{
        uint alln = num_vertices(g_all);
        cout << "alln: " << alln << '\n';

        Graph g_comp(g_all); // connected component

        auto g_all_prop_map = get(vertex_index, g_all);
        auto g_comp_prop_map = get(vertex_index, g_comp);

        auto vertid_to_vert_t = [&](uint id, decltype(get(vertex_index, g_all)) prop_map)
        {
                VertIter vit, vjt;
                tie(vit, vjt) = vertices(g_all);
                for( ; vit != vjt; ++vit ){
                        if( prop_map[*vit] == id ) return *vit;
                }
                return Graph::null_vertex();
        };

        // for all vertices v in g_comp,
        vector<vertex_t> toremove;
        VertIter vit, vjt;
        tie(vit, vjt) = vertices(g_comp);
        for( ; vit != vjt; ++vit ){
                vertex_t v = *vit;
                cout << "currently examining " << v << " comp_propmap" << g_comp_prop_map[v] << '\n';
                // cv = lookup the corresponding vertex in g_all
                vertex_t cv = vertid_to_vert_t(g_all_prop_map[v], g_all_prop_map);
                cout << "       with cv " << cv << " all_propmap" << g_all_prop_map[v] << '\n';
                // if cv is not in biggest_comp_p,
                if( !biggest_comp_p.a.contains(cv) &&
                    !biggest_comp_p.b.contains(cv) &&
                    !biggest_comp_p.c.contains(cv) ){
                        // delete v from g_comp
                            toremove.push_back(v);
                    }

        }

        cout << "gcomp:\n";
        //print_graph(g_comp);
        //print_graph_addresses(g_comp);

        uint nremove = toremove.size();
        cout << "nremove: " << nremove << '\n';
        for(auto& v : toremove ){
                cout << "   nr: " << v << '\n';
                kill_vertex(v, g_comp);
        }

        reset_vertex_indices(g_comp);

        cout << "gcomp:\n";
        //print_graph(g_comp);
        //print_graph_addresses(g_comp);
        BFSVisitorData vd(&g_comp, *vertices(g_comp).first);
        breadth_first_search(g_comp, vd.root, boost::visitor(BFSVisitor(vd)));

        // disabled because they don't support multiple connected components
        //BOOST_ASSERT(assert_verts(g_orig, vis_data_orig));
        //BOOST_ASSERT(assert_verts(g_copy, vis_data_copy));

        uint n = num_vertices(g_comp);

        vector<uint> L(vd.num_levels + 1, 0);
	cout << "L levels: " << L.size() << '\n';
        for( auto& d : vd.verts ){
                cout << "level: " << d.second.level << '\n';
	       	++L[d.second.level];
	}

        uint k = L[0];
        uint l[3];
        l[1] = 0;
        while( k <= n/2 ){
                uint indx = ++l[1];
                uint lsize = L.size();
                if( indx >= lsize ) break;
	       	k += L.at(indx);
	}

        float sq  = 2 * sqrt(k);
        float snk = 2 * sqrt(num_vertices(g_comp) - k);
        cout << "sq:     " << sq << '\n';
        cout << "snk:    " << snk << '\n';
        cout << "L size: " << L.size() << '\n';

        l[0] = l[1];
        cout << "l[0]:   " << l[0] << '\n';
        while( l[0] < L.size() ){
                float val = L.at(l[0]) + 2*(l[1] - l[0]);
                if( val <= sq ) break;
                --l[0];
        }
        cout << "l0: " << l[0] << "     highest level <= l1\n";

        l[2] = l[1] + 1;
        cout << "l[2]" << l[2] << '\n';
        while( l[2] < L.size() ){
                float val = L.at(l[2]) + 2*(l[2] - l[1] - 1);
                if( val <= snk ) break;
                ++l[2];
        }
        cout << "l2: " << l[2] << "     lowest  level >= l1 + 1\n";

        uint r = vd.num_levels - 1;

        Partition star_p = theorem4_connected(g_comp, L, l, r, nullptr, nullptr);
        // ????

        Partition p;
        p.c = star_p.c;
        p.b.insert(STLALL(toremove));
        if( star_p.a.size() > star_p.b.size() ){
                p.a = star_p.a;
                p.b.insert(STLALL(star_p.b));
        } else {
                p.a = star_p.b;
                p.b.insert(STLALL(star_p.a));
        }
        return p;
}

Partition theorem4_disconnected(GraphCR g, uint n, uint num_components, associative_property_map<vertex_map> const& vertid_to_component, vector<uint> const& num_verts_per_component, Partition const& biggest_comp_p)
{
        vector<vector<VertIter>> vertex_sets; // set of vertex ids, indexed by component number (second vector should be set but compiler did not like call to .insert())

        vertex_sets.resize(num_components);

        // populate vertex sets
        VertIter vit, vjt;
        tie(vit, vjt) = vertices(g);
        for( uint i = 0; vit != vjt; ++vit, ++i ){
                uint component = vertid_to_component[*vit];
                vector<VertIter>& vset = vertex_sets[component];
                vset.push_back(vit);
        }

        // Let G1, G2, ... , Gk be the connected components of G, with vertex sets V1, V2, ... , Vk respectively.
        bool bigger_than_one_third = false;
        bool bigger_than_two_thirds = false;
        for( uint i = 0; i < num_components; ++i ){
                if( num_verts_per_component[i] > n    /3.0 ) bigger_than_one_third  = true;
                if( num_verts_per_component[i] > n*2.0/3.0 ){ bigger_than_two_thirds = true; break;}
        }

        if( !bigger_than_one_third ){

                // If no connected component has total vertex cost > 1/3, let i be the minimum index such that the total cost of V1 U V2 U ... U Vi > 1/3
                int i = lowest_i(n, num_components, num_verts_per_component); // -1 indicates no such index found

                Partition p;

                // populate partition A = V1 U V2 U ... U Vi
                for( int j = 0; j <= i; ++j ){
                        vector<VertIter>& vec = vertex_sets[j];
                        for( VertIter& v : vec ) p.a.insert(*v);
                }

                // populate partition B = Vi+1 U Vi+2 U ... U Vk
                for( int j = i+1; j < (int)num_components; ++j ){
                        vector<VertIter>& vec = vertex_sets[j];
                        for( VertIter& v : vec ) p.b.insert(*v);
                }

                // Since i is minimum and the cost of Vi <= 1/3, the cost of A <= 2/3. return true;
                cout << "not bigger than one third\n";
                return p;
        } else if( !bigger_than_two_thirds ){
                // If some connected component (say Gi) has total vertex cost between 1/3 and 2/3,
                Partition p;

                int i = lowest_i(n, num_components, num_verts_per_component);
                BOOST_ASSERT(i >= 0 && num_verts_per_component[i] >= n/3 && num_verts_per_component[i] <= 2*n/3);

                // populate partition A
                cout << "!! populating partition A\n";
                for( uint j = 0; j <= i; ++j ){
                        for( uint v = 0; v < vertex_sets[j].size(); ++v ){
                                p.a.insert(*vertex_sets[j][v]);
                        }

                }
                cout << '\n';

                // populate partition B, should be everything except what's in partition A
                cout << "!! populating partition B\n";
                for( uint j = i+1; j < num_components; ++j ){

                        vector<VertIter>& vset = vertex_sets[j];
                        for( VertIter& v : vset ){
                                p.b.insert(*v);
                                cout << "v: " << *v << endl;
                        }
                }

                // partition C should be empty
                BOOST_ASSERT(p.c.empty());

                p.print(&g);

                cout << "bigger than one third but less than two thirds\n";
                return p;
        }

        return theorem4_ccbigger23(g, biggest_comp_p);
}

/* Theorem 4: Let G be any (possibly disconnected) n-vertex planar graph having nonnegative vertex costs summing to no more than one.
Then the vertices of G can be partitioned into three sets A, B, C such that no edge joins a vertex
in A with a vertex in B, neither A nor B has total cost exceeding 2/3, and C contains no more than
2sqrt(2)sqrt(n) vertices :*/
Partition theorem4(GraphCR g, associative_property_map<vertex_map> const& vertid_to_component, vector<uint> const& num_verts_per_component, Partition const& biggest_comp_p)
{
	uint n = num_vertices(g);
	uint num_components = num_verts_per_component.size();
	bool is_graph_connected = (num_components == 1);

	if( is_graph_connected ){
		cout << "graph is connected\n";
                vector<uint> L;
                uint l[3];
                uint r;
                BOOST_ASSERT(0);
                return theorem4_connected(g, L, l, r, nullptr, nullptr);
	} else {
                cout << "graph is disconnected with " << num_components << " components\n";
                return theorem4_disconnected(g, n, num_components, vertid_to_component, num_verts_per_component, biggest_comp_p);
	}
}


// Copyright (C) 2016 Gideon May (gideon@borges.xyz)
//
// Permission to copy, use, sell and distribute this software is granted
// provided this copyright notice appears in all copies.
// Permission to modify the code and to distribute modified code is granted
// provided this copyright notice appears in all copies, and a notice
// that the code was modified is included with the copyright notice.
//
// This software is provided "as is" without express or implied warranty,
// and with no claim as to its suitability for any purpose.

#include <boost/python.hpp>

#include <osg/Matrix>
#include <osg/Quat>
#include <osg/Vec3>

#include "Math.hpp"

using namespace boost::python;
using namespace osg;

namespace {
#ifdef WIN32
void preMult(osg::Matrix * self, const Matrix& other)
{
    self->preMult(other);
}

void postMult(osg::Matrix * self, const Matrix& other)
{
    self->postMult(other);
}
#endif
}

namespace PyOSG {
void init_matrix2(class_<Matrix> &matrix)
{
    matrix
        .def("mult", &osg::Matrix::mult)
#ifdef WIN32 // Generates ICE C1001
        .def("preMult", &preMult)
        .def("postMult", &postMult)
#else
        .def("preMult",
            (void (osg::Matrix::*)(const osg::Matrix&)) &osg::Matrix::preMult)
        .def("postMult",
            (void (osg::Matrix::*)(const osg::Matrix&)) &osg::Matrix::postMult)
#endif
        .def("identity",  &Matrix::identity)

        .def("scale", (Matrix (*)(const Vec3&)) &Matrix::scale)
        .def("scale", (Matrix (*)(Matrix::value_type, Matrix::value_type, Matrix::value_type)) &Matrix::scale)
        .def("translate", (Matrix (*)(const Vec3&)) &Matrix::translate)
        .def("translate", (Matrix (*)(Matrix::value_type, Matrix::value_type, Matrix::value_type)) &Matrix::translate)
        .def("rotate", (Matrix (*)(Matrix::value_type, Matrix::value_type, Matrix::value_type, Matrix::value_type)) &Matrix::rotate)
        .def("rotate", (Matrix (*)(Matrix::value_type, const Vec3&)) &Matrix::rotate)
        .def("rotate", (Matrix (*)(const Vec3&, const Vec3&)) &Matrix::rotate)
        .def("rotate", (Matrix (*)(Matrix::value_type, const Vec3&, Matrix::value_type, const Vec3&, Matrix::value_type, const Vec3&)) &Matrix::rotate)
        .def("rotate", (Matrix (*)(const Quat&)) &Matrix::rotate)
        .def("inverse",  &Matrix::inverse)
        .def("ortho",  &Matrix::ortho)
        .def("ortho2D",  &Matrix::ortho2D)
        .def("frustum",  &Matrix::frustum)
        .def("perspective",  &Matrix::perspective)
        .def("lookAt",  (Matrix(*)(const Vec3f&, const Vec3f&, const Vec3f&)) &Matrix::lookAt)
        .def("lookAt",  (Matrix(*)(const Vec3d&, const Vec3d&, const Vec3d&)) &Matrix::lookAt)

        .staticmethod("identity")
        .staticmethod("scale")
        .staticmethod("translate")
        .staticmethod("rotate")
        .staticmethod("inverse")
        .staticmethod("ortho")
        .staticmethod("ortho2D")
        .staticmethod("frustum")
        .staticmethod("perspective")
        .staticmethod("lookAt")
    ;
}
}