// Copyright 2018-2019 Henry Schreiner and Hans Dembinski
//
// Distributed under the 3-Clause BSD License.  See accompanying
// file LICENSE or https://github.com/scikit-hep/boost-histogram for details.

#include <bh_python/pybind11.hpp>

#include <pybind11/functional.h>
#include <pybind11/operators.h>

#include <bh_python/axis.hpp>
#include <bh_python/make_pickle.hpp>
#include <bh_python/transform.hpp>
#include <boost/histogram/axis/regular.hpp>

template <class T, class... Args>
py::class_<T> register_transform(py::module& mod, Args&&... args) {
    py::class_<T> transform(mod, std::forward<Args>(args)...);

    transform

        .def(py::init<T>())
        .def("forward", [](const T& self, double v) { return self.forward(v); })
        .def("inverse", [](const T& self, double v) { return self.inverse(v); })
        .def(make_pickle<T>())
        .def("__copy__", [](const T& self) { return T(self); })
        .def("__deepcopy__", &deep_copy<T>)

        ;

    return transform;
}

extern "C" {
double _log_fn(double v) { return std::log(v); }
double _exp_fn(double v) { return std::exp(v); }
double _sqrt_fn(double v) { return std::sqrt(v); }
double _sq_fn(double v) { return v * v; }
}

void register_transforms(py::module& mod) {
    mod.def("_log_fn", &_log_fn);
    mod.def("_exp_fn", &_exp_fn);
    mod.def("_sqrt_fn", &_sqrt_fn);
    mod.def("_sq_fn", &_sq_fn);

    register_transform<bh::axis::transform::id>(mod, "id")
        .def(py::init<>())
        .def("__repr__",
             [](py::object self) {
                 return py::str("{}()").format(self.attr("__class__").attr("__name__"));
             })

        ;

    register_transform<bh::axis::transform::sqrt>(mod, "sqrt")
        .def(py::init<>())
        .def("__repr__",
             [](py::object self) {
                 return py::str("{}()").format(self.attr("__class__").attr("__name__"));
             })

        ;

    register_transform<bh::axis::transform::log>(mod, "log")
        .def(py::init<>())
        .def("__repr__",
             [](py::object self) {
                 return py::str("{}()").format(self.attr("__class__").attr("__name__"));
             })

        ;

    register_transform<bh::axis::transform::pow>(mod, "pow")
        .def(py::init<double>(), "power"_a)
        .def_readonly("power", &bh::axis::transform::pow::power)
        .def("__repr__",
             [](py::object self) {
                 double power = py::cast<bh::axis::transform::pow>(self).power;
                 return py::str("{}({:g})")
                     .format(self.attr("__class__").attr("__name__"), power);
             })

        ;

    register_transform<func_transform>(mod, "func_transform")
        .def(py::init<py::object, py::object, py::object, py::str>(),
             "forward"_a,
             "inverse"_a,
             "convert"_a,
             "name"_a)
        .def("__repr__",
             [](py::object self) {
                 auto& s = py::cast<func_transform&>(self);
                 if(s._name.equal(py::str(""))) {
                     return py::str("{}({}, {})")
                         .format(self.attr("__class__").attr("__name__"),
                                 s._forward_ob,
                                 s._inverse_ob);
                 } else
                     return s._name;
             })

        ;
}


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

// Copyright (c) 2012-2014 Barend Gehrels, Amsterdam, the Netherlands.

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

#include <test_buffer.hpp>

#include <boost/geometry/multi/geometries/multi_geometries.hpp>

static std::string const simplex = "MULTILINESTRING((0 0,4 5),(5 4,10 0))";
static std::string const two_bends = "MULTILINESTRING((0 0,4 5,7 4,10 6),(1 5,5 9,8 6))";
static std::string const turn_inside = "MULTILINESTRING((0 0,4 5,7 4,10 6),(1 5,5 9,8 6),(0 4,-2 6))";

template <typename P>
void test_all()
{
    typedef bg::model::linestring<P> linestring;
    typedef bg::model::multi_linestring<linestring> multi_linestring_type;
    typedef bg::model::polygon<P> polygon;

    bg::strategy::buffer::join_miter join_miter;
    bg::strategy::buffer::join_round join_round(100);
    bg::strategy::buffer::join_round_by_divide join_round_by_divide(4);
    bg::strategy::buffer::end_flat end_flat;
    bg::strategy::buffer::end_round end_round(100);

    // Round joins / round ends
    test_one<multi_linestring_type, polygon>("simplex", simplex, join_round, end_round, 49.0217, 1.5, 1.5);
    test_one<multi_linestring_type, polygon>("two_bends", two_bends, join_round, end_round, 74.73, 1.5, 1.5);
    test_one<multi_linestring_type, polygon>("turn_inside", turn_inside, join_round, end_round, 86.3313, 1.5, 1.5);
    test_one<multi_linestring_type, polygon>("two_bends_asym", two_bends, join_round, end_round, 58.3395, 1.5, 0.75);

    // Round joins / flat ends:
    test_one<multi_linestring_type, polygon>("simplex", simplex, join_round, end_flat, 38.2623, 1.5, 1.5);
    test_one<multi_linestring_type, polygon>("two_bends", two_bends, join_round, end_flat, 64.6217, 1.5, 1.5);

    // TODO this should be fixed test_one<multi_linestring_type, polygon>("turn_inside", turn_inside, join_round, end_flat, 99, 1.5, 1.5);
    test_one<multi_linestring_type, polygon>("two_bends_asym", two_bends, join_round, end_flat, 52.3793, 1.5, 0.75);

    // This one is far from done:
    // test_one<multi_linestring_type, polygon>("turn_inside_asym_neg", turn_inside, join_round, end_flat, 99, +1.5, -1.0);

    // Miter / divide joins, various ends
    test_one<multi_linestring_type, polygon>("two_bends", two_bends, join_round_by_divide, end_flat, 64.6217, 1.5, 1.5);
    test_one<multi_linestring_type, polygon>("two_bends", two_bends, join_miter, end_flat, 65.1834, 1.5, 1.5);
    test_one<multi_linestring_type, polygon>("two_bends", two_bends, join_miter, end_round, 75.2917, 1.5, 1.5);
}

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

    return 0;
}


#include <ompl/base/spaces/RealVectorBounds.h>
#include <ompl/geometric/SimpleSetup.h>
#include <ompl/base/PlannerDataStorage.h>
#include <ompl/base/spaces/DubinsStateSpace.h>
#include <ompl/base/spaces/ReedsSheppStateSpace.h>

#include <ompl/geometric/planners/rrt/RRTstar.h>
#include <ompl/util/Exception.h>

#include <sys/stat.h>
#include <sys/types.h>

#include <Eigen/Eigen>
#include <dart/common/common.h>

#include <iostream>
#include <fstream>

#include <ompl/config.h>
#include "config/config2D.h"
#include "ompl/geometric/planners/rrt/RRTstarFND.h"
#include "model.h"

namespace ob = ompl::base;
namespace og = ompl::geometric;

class DubinsCarEnvironment {
public:
    double reconnectTime;

    DubinsCarEnvironment(std::string& obstacleFilepath, std::string& dynamicObstaclesFile)
        : maxWidth_(2440.0)
        , maxHeight_(2160.0)
        , prefix_("")
    {
        model_ = new Model(obstacleFilepath);
        model_->setDynamicObstaclesFile(dynamicObstaclesFile);
        // ob::StateSpacePtr space(new ob::DubinsStateSpace(0.05, true));
        ob::StateSpacePtr space(new ob::DubinsStateSpace(125, false)); // only forward
        //ob::StateSpacePtr space(new ob::ReedsSheppStateSpace(125)); // only forward

        ob::RealVectorBounds bounds(2);
        bounds.setLow(0);
        bounds.high[0] = maxWidth_;
        bounds.high[1] = maxHeight_;

        space->as<ob::SE2StateSpace>()->setBounds(bounds);

        ss_.reset(new og::SimpleSetup(ob::StateSpacePtr(space)));
        // set state validity checking for this space
        ob::SpaceInformationPtr si = ss_->getSpaceInformation();
        model_->setSpaceInformation(si);
        ss_->setStateValidityChecker(
            std::bind(&Model::isStateValid, model_, std::placeholders::_1));
        space->setup();
        ss_->setPlanner(
            ob::PlannerPtr(new og::DRRTstarFN(ss_->getSpaceInformation())));
        ss_->getPlanner()->as<og::DRRTstarFN>()->setRange(35.0);
        ss_->getPlanner()->as<og::DRRTstarFN>()->setMaxNodes(15000);
        ss_->getSpaceInformation()->setStateValidityCheckingResolution(0.01125);
    }

    ~DubinsCarEnvironment()
    {
        delete model_;
    }

    std::vector<ompl::base::State*> pathArray_;

    void prepareDynamic(int from)
    {
        //dart::common::Timer t1("select branch");
        try {
            ob::SpaceInformationPtr si = ss_->getSpaceInformation();
            og::PathGeometric& p = ss_->getSolutionPath();
            pathNodeCount_ = p.getStateCount();
            og::DRRTstarFN* localPlanner=ss_->getPlanner()->as<og::DRRTstarFN>();
            localPlanner->as<og::DRRTstarFN>()->setRange(35.0);

            ompl::base::State* s = si->cloneState(p.getState(from));

            p.interpolate();
            for (std::size_t i = from; i < p.getStates().size(); ++i) {
                ompl::base::State* st = p.getState(i);
                pathArray_.push_back(si->cloneState(st));
            }

            //t1.start();
            localPlanner->selectBranch(s);
            //t1.stop();
            //t1.print();

            localPlanner->setSampleRadius(1000);
            localPlanner->setOrphanedBias(0.65);
            localPlanner->setLocalPlanning(true);
            localPlanner->swapNN();
        }
        catch (ompl::Exception e) {
            dtwarn << "No solution, man\n";
        }
    }

    ob::State* prepareFromScratchRRTstar(const size_t from, Model::Point& final)
    {
        //dart::common::Timer t1("select branch");
        try {
            ob::SpaceInformationPtr si = ss_->getSpaceInformation();
            og::PathGeometric& p = ss_->getSolutionPath();
            pathNodeCount_ = p.getStateCount();

            ompl::base::State* s = si->cloneState(p.getState(static_cast<unsigned int>(from)));

            ss_->setPlanner(
                ob::PlannerPtr(new og::DRRTstarFN(ss_->getSpaceInformation())));
            ss_->getPlanner()->as<og::DRRTstarFN>()->setRange(35.0);
            ss_->getSpaceInformation()->setStateValidityCheckingResolution(0.01125);

            ob::ScopedState<> start(ss_->getStateSpace(), s);
            ob::ScopedState<> goal(ss_->getStateSpace());
            goal[0] = final.x();
            goal[1] = final.y();
            ss_->setStartAndGoalStates(start, goal);

            ss_->setup();

            //ss_->getPlanner()->as<og::DRRTstarFN>()->setTerminateFirstSolution(true); // OBSOLETE

            return s;
        }
        catch (ompl::Exception e) {
            dtwarn << "No solution, man\n";
        }
        return nullptr;
    }

    ob::State* prepareFromScratchRRTstarFN(const size_t from, Model::Point& final)
    {
        //dart::common::Timer t1("select branch");
        try {
            ob::SpaceInformationPtr si = ss_->getSpaceInformation();
            og::PathGeometric& p = ss_->getSolutionPath();
            pathNodeCount_ = p.getStateCount();

            ompl::base::State* s = si->cloneState(p.getState(static_cast<unsigned int>(from)));

            ss_->setPlanner(
                ob::PlannerPtr(new og::DRRTstarFN(ss_->getSpaceInformation())));
            ss_->getPlanner()->as<og::DRRTstarFN>()->setRange(35.0);
            ss_->getPlanner()->as<og::DRRTstarFN>()->setMaxNodes(6000);
            ss_->getSpaceInformation()->setStateValidityCheckingResolution(0.01125);

            ob::ScopedState<> start(ss_->getStateSpace(), s);
            ob::ScopedState<> goal(ss_->getStateSpace());
            goal[0] = final.x();
            goal[1] = final.y();
            ss_->setStartAndGoalStates(start, goal);

            ss_->setup();

            // ss_->getPlanner()->as<og::DRRTstarFN>()->setTerminateFirstSolution(true); // OBSOLETE
            return s;
        }
        catch (ompl::Exception e) {
            dtwarn << "No solution, man\n";
        }
        return nullptr;
    }

    double removeInvalidNodes()
    {
        dart::common::Timer t1("node removal");
        t1.start();
        ss_->getSpaceInformation()->setStateValidityCheckingResolution(0.01);
        int removed = ss_->getPlanner()->as<og::DRRTstarFN>()->removeInvalidNodes();
        ss_->getPlanner()->as<og::DRRTstarFN>()->populateDetachedPath();
        t1.stop();
        //t1.print();
        OMPL_INFORM("removed nodes from the sub tree is %d", removed);

        ss_->getProblemDefinition()->clearSolutionPaths();
        return t1.getLastElapsedTime();
    }

    /*
    void cleanup(size_t from)
    {
        ompl::base::State* s = pathArray_[from];
        ss_->getPlanner()->as<og::DRRTstarFN>()->nodeCleanUp(s);
    }
    */

    double replan(const Model::Point& initial, const Model::Point& final,
        double time, bool clearPlanner = true)
    {

        reconnectTime = 0;
        ss_->getSpaceInformation()->setStateValidityCheckingResolution(0.01125);
        dart::common::Timer t1("reconnect");
        t1.start();
        bool is_reconnected = ss_->getPlanner()->as<og::DRRTstarFN>()->reconnect();
        t1.stop();
        reconnectTime = t1.getLastElapsedTime();

        ss_->getSpaceInformation()->setStateValidityCheckingResolution(0.01125);
        if (!is_reconnected) {
            ss_->solve(time);
        }
        ss_->getProblemDefinition()->clearSolutionPaths();
        ss_->getPlanner()->as<og::DRRTstarFN>()->evaluateSolutionPath();
        return reconnectTime + ss_->getLastPlanComputationTime();
    }

    bool plan(const Model::Point& initial, const Model::Point& final, double time,
        bool clearPlanner = true)
    {
        if (!ss_)
            return false;
        ob::ScopedState<> start(ss_->getStateSpace());
        start[0] = initial.x();
        start[1] = initial.y();
        ob::ScopedState<> goal(ss_->getStateSpace());
        goal[0] = final.x();
        goal[1] = final.y();
        ss_->setStartAndGoalStates(start, goal);
        // generate a few solutions; all will be added to the goal;

        ss_->getPlanner()->as<og::DRRTstarFN>()->setGoalBias(0.0015);

        if (ss_->getPlanner())
            if (clearPlanner)
                ss_->getPlanner()->clear();

        ss_->solve(time);

        const std::size_t ns = ss_->getProblemDefinition()->getSolutionCount();
        OMPL_INFORM("Found %d solutions", (int)ns);
        if (ss_->haveSolutionPath()) {
            return true;
        }
        else
            return false;
    }

    bool plan(const ob::State* initial, const Model::Point& final, double time,
        bool clearPlanner = true)
    {
        if (!ss_)
            return false;
        ob::ScopedState<> start(ss_->getStateSpace(), initial);
        ob::ScopedState<> goal(ss_->getStateSpace());
        goal[0] = final.x();
        goal[1] = final.y();
        ss_->setStartAndGoalStates(start, goal);
        // generate a few solutions; all will be added to the goal;

        ss_->getPlanner()->as<og::DRRTstarFN>()->setGoalBias(0.0015);

        if (ss_->getPlanner())
            if (clearPlanner)
                ss_->getPlanner()->clear();

        ss_->solve(time);

        const std::size_t ns = ss_->getProblemDefinition()->getSolutionCount();
        OMPL_INFORM("Found %d solutions", (int)ns);
        if (ss_->haveSolutionPath()) {
            return true;
        }
        else
            return false;
    }

    void recordSolution() { recordSolution(-1); }

    void recordSolution(int num)
    {
        if (!ss_ || !ss_->haveSolutionPath())
            return;

        std::string fileName;
        std::string filenameInterp;
        std::string filenameSolution;

        if (num != -1) {
            fileName = "dubins-results" + std::to_string(num) + ".txt";
            filenameInterp = "dubins-results-interp" + std::to_string(num) + ".txt";
            filenameSolution = "dubins-results-is_solution" + std::to_string(num) + ".txt";
        }
        else {
            fileName = "dubins-results.txt";
            filenameInterp = "dubins-results-interp.txt";
            filenameSolution = "dubins-results-is_solution.txt";
        }

        std::ofstream fout(prefix_ + fileName);
        std::ofstream foutInterp(prefix_ + filenameInterp);
        std::ofstream foutSolution(prefix_ + filenameSolution);

        foutSolution << ss_->haveExactSolutionPath() << std::endl;

        og::PathGeometric& p = ss_->getSolutionPath();
        p.printAsMatrix(fout);
        p.interpolate();
        p.printAsMatrix(foutInterp);
    }

    //==============================================================================
    void printEdge(std::ostream& os, const ob::StateSpacePtr& space,
        const ob::PlannerDataVertex& vertex)
    {
        std::vector<double> reals;
        if (vertex != ob::PlannerData::NO_VERTEX) {
            space->copyToReals(reals, vertex.getState());
            for (size_t j(0); j < reals.size(); ++j)
                os << " " << reals[j];
            os << " " << vertex.getTag();
        }
    }

    void recordTreeState() { recordTreeState(-1); }

    void recordTreeState(int num)
    {
        if (!ss_) {
            return;
        }
        // Get the planner data to visualize the vertices and the edges
        ob::PlannerData pdat(ss_->getSpaceInformation());
        ss_->getPlannerData(pdat);

        // Print the vertices to file
        std::string fileName;
        if (num == -1)
            fileName = "dubins-vertices.dat";
        else
            fileName = "dubins-vertices" + std::to_string(num) + ".dat";

        std::ofstream ofs_v(prefix_ + fileName);
        for (unsigned int i(0); i < pdat.numVertices(); ++i) {
            printEdge(ofs_v, ss_->getStateSpace(), pdat.getVertex(i));
            ofs_v << std::endl;
        }
        ofs_v.flush();

        ob::DubinsStateSpace* space;
        space = ss_->getStateSpace()->as<ob::DubinsStateSpace>();

        // Print the edges to file
        if (num == -1)
            fileName = "dubins-edges.dat";
        else
            fileName = "dubins-edges" + std::to_string(num) + ".dat";
        std::ofstream ofs_e(prefix_ + fileName);
        std::vector<unsigned int> edge_list;
        std::vector<double> reals;
        std::vector<double> realsOld;
        bool isMajorTree = false;
        ob::State* s3 = space->allocState();
        for (unsigned int i(0); i < pdat.numVertices(); ++i) {
            unsigned int n_edge = pdat.getEdges(i, edge_list);
            const ob::State* s1 = pdat.getVertex(i).getState();
            isMajorTree = pdat.getVertex(i).getTag();
            for (unsigned int i2(0); i2 < n_edge; ++i2) {
                const ob::State* s2 = pdat.getVertex(edge_list[i2]).getState();
                double step = 0.05;
                if (space->distance(s1, s2) < 0.03) {
                    step = 0.2;
                }
                space->copyToReals(realsOld, s1);
                for (double t = step; t <= 1.01; t += step) {
                    space->interpolate(s1, s2, t, s3);
                    space->copyToReals(reals, s3);
                    for (const auto& r : realsOld)
                        ofs_e << r << " ";
                    realsOld = reals;
                    for (const auto& r : reals)
                        ofs_e << r << " ";
                    //
                    ofs_e << "0x" << std::hex << (isMajorTree ? 0x4488AA : 0xDD6060)
                          << std::endl;
                }
            }
        }
        ofs_e.flush();
    }

    Model* getModel() { return model_; }

    // proxy method
    void updateObstacles() { model_->updateObstacles(); }

    //==============================================================================
    void store(const char* filename)
    {
        // Get the planner data to visualize the vertices and the edges
        ob::PlannerData pdat(ss_->getSpaceInformation());
        ss_->getPlannerData(pdat);

        ob::PlannerDataStorage pdstorage;

        pdstorage.store(pdat, filename);
    }

    //==============================================================================
    void load(const char* filename, Model::Point& i, Model::Point& f)
    {
        if (ss_->getPlanner()) {
            ss_->getPlanner()->clear();
        }

        ss_->setup();

        Model::Point initial(i);
        Model::Point final(f);

        ob::ScopedState<> start(ss_->getStateSpace());
        start[0] = initial.x();
        start[1] = initial.y();
        ob::ScopedState<> goal(ss_->getStateSpace());
        goal[0] = final.x();
        goal[1] = final.y();

        ss_->setStartAndGoalStates(start, goal);

        ss_->getPlanner()->as<og::DRRTstarFN>()->restoreTree(filename);
    }

    size_t getPathNodesCount() const
    {
        return pathNodeCount_;
    }

    std::string statistics()
    {
        std::string output;
        output += std::to_string(ss_->getLastPlanComputationTime());

        output += ", " + std::to_string(reconnectTime);

        try {
            og::PathGeometric& p = ss_->getSolutionPath();
            output += ", " + std::to_string(p.length());
        }
        catch (ompl::Exception e) {
            ;
        }

        output += ", " + std::to_string(ss_->haveExactSolutionPath());
        return output;
    }

    void setRecordDirectoryPrefix(std::string path)
    {
        struct stat buff;
        if (stat(path.c_str(), &buff) != 0)
            mkdir(path.c_str(), 0777);

        prefix_ = path + "/";
    }

private:
    og::SimpleSetupPtr ss_;
    const double maxWidth_;
    const double maxHeight_;
    size_t pathNodeCount_;

    std::string prefix_;

    Model* model_;
};

std::istream& operator>>(std::istream& is, Model::Point& p)
{
    double x, y;

    is >> x;
    is >> y;
    p = Model::Point(x, y);
    return is;
}

int main(int argc, char** argv)
{
    std::string setupFilename = "config/setup.txt";
    if (argc > 1) {
        for (int i = 0; i < argc; ++i) {
            if (std::string(argv[i]) == std::string("-s") && i + 1 < argc) {
                setupFilename = argv[++i];
            }
        }
    }

    std::ifstream fin(setupFilename);

    assert(!fin.fail() && "Cannot open file");

    Model::Point start;
    Model::Point goal;
    fin >> start;
    fin >> goal;

    std::ofstream goalStartfout("goal_start.txt");
    goalStartfout << "set object 1001 circle at " << start.x() << "," << start.y() << " size 50 "
                  << "fc rgb \"#ff00ff\" front\n";
    goalStartfout << "set object 1002 circle at " << goal.x() << "," << goal.y() << " size 50 "
                  << "fc rgb \"#ff00ff\" front\n";

    goalStartfout.flush();
    double time;
    size_t iter;
    fin >> time >> iter;

    double dt = time / ((double)iter);
    size_t ITERATIONS = iter;

    std::string fileDump;
    fin >> fileDump;

    bool plan;
    fin >> plan;

    std::string obstaclesFile;
    fin >> obstaclesFile;

    std::string dynamicObstaclesFile;
    fin >> dynamicObstaclesFile;

    DubinsCarEnvironment problem(obstaclesFile, dynamicObstaclesFile);

#define PLOTTING
#ifdef PLOTTING
    if (plan) {
        for (size_t i = 0; i < ITERATIONS; i++) {
            bool clearPlanner = (i == 0);
            if (problem.plan(start, goal, dt, clearPlanner)) {
                problem.recordSolution(i);
                problem.recordTreeState(i);
                std::cout << i << " done\n";
            }
        }
        problem.store(fileDump.c_str());
    }
#endif

    std::cout << time << "\n" << dt << std::endl;
    if (system("date") < 0) {
        std::cout << "cannot run system()\n";
    }

#ifdef SOLVING
    if (plan) {
        if (problem.plan(start, goal, time)) {
            problem.recordSolution();
            problem.recordTreeState();
            std::cout << "done\n";
        }
        problem.store(fileDump.c_str());
    }
    else {
        problem.load(fileDump.c_str());
    }
#endif

    problem.updateObstacles();
    std::cout << "obstacle has moved\n";

    std::ofstream fout("benchmark_results.txt");

    fout << obstaclesFile << "\n";

    fout << "from time cost reached\n";

    const size_t benchmark_path_nodes = 6;
    //for (size_t from = 0; from < problem.getPathNodesCount(); ++from) {

    for (size_t from = 2; from < 3; ++from) {
        problem.setRecordDirectoryPrefix(std::string("path_node_") + std::to_string(from));
        problem.load(fileDump.c_str(), start, goal);

        dart::common::Timer t1("benchmark");
        t1.start();

        problem.prepareDynamic(static_cast<int>(from));
        std::cout << "prepared tree for removal\n";

        //==============================================================================
        problem.recordSolution(800);
        problem.recordTreeState(800);
        std::cout << "recorded 800\n";

        double removalTime = problem.removeInvalidNodes();

        std::cout << "invalid branch removal: done\n";
        problem.recordTreeState(801);

        //==============================================================================

        const int DYNAMIC_ITERATIONS = 600;
        std::cout << std::endl;
        for (size_t i = ITERATIONS + 1; i < DYNAMIC_ITERATIONS + ITERATIONS + 1;
             i++) {
            if (problem.replan(start, goal, 1.0, false)) {
                //problem.setRecordDirectoryPrefix(std::string("path_node_") + std::to_string(from));
                problem.recordSolution(i);
                problem.recordTreeState(i);
                std::cout << " " << i << " done\n";
            }
        }
        t1.stop();
        fout << from << ", " << t1.getLastElapsedTime() << ", " << problem.statistics() << removalTime << std::endl;
    }
    return 0;
    std::cout << ">>>>>>>>>>>>>>>>>>> RRT star initiated\n";
    fout << "rrt star\n";
    for (size_t from = 3; from < 4; ++from) {
        problem.setRecordDirectoryPrefix(std::string("rrts_path_node_") + std::to_string(from));
        problem.load(fileDump.c_str(), start, goal);

        dart::common::Timer t1("benchmark");
        t1.start();

        ob::State* s = problem.prepareFromScratchRRTstar(from, goal);

        std::cout << "prepared tree for removal\n";

        //==============================================================================
        problem.recordSolution(800);
        problem.recordTreeState(800);
        std::cout << "recorded 800\n";
        problem.recordTreeState(801);

        //==============================================================================
        const int DYNAMIC_ITERATIONS = 600;
        std::cout << std::endl;

        for (size_t i = ITERATIONS + 1; i < DYNAMIC_ITERATIONS + ITERATIONS + 1;
             i++) {
            if (problem.plan(s, goal, 1.0, false)) {
                problem.recordSolution(i);
                problem.recordTreeState(i);
                std::cout << i << " done\n";
            }
        }
        t1.stop();
        fout << from << ", " << t1.getLastElapsedTime() << ", " << problem.statistics() << std::endl;
    }
    return 0;
    std::cout << ">>>>>>>>>>>>>>>>>>> RRT star FN initiated\n";
    fout << "rrt star fn\n";
    for (size_t from = 0; from < benchmark_path_nodes; ++from) {
        problem.setRecordDirectoryPrefix(std::string("rrtsfn_path_node_") + std::to_string(from));
        problem.load(fileDump.c_str(), start, goal);

        dart::common::Timer t1("benchmark");
        t1.start();

        ob::State* s = problem.prepareFromScratchRRTstarFN(from, goal);
        std::cout << "prepared tree for removal\n";

        //==============================================================================
        problem.recordSolution(800);
        problem.recordTreeState(800);
        std::cout << "recorded 800\n";
        problem.recordTreeState(801);

        //==============================================================================
        const int DYNAMIC_ITERATIONS = 1;
        std::cout << std::endl;
        for (size_t i = ITERATIONS + 1; i < DYNAMIC_ITERATIONS + ITERATIONS + 1;
             i++) {
            if (problem.plan(s, goal, 600.00, false)) {
                problem.recordSolution(i);
                problem.recordTreeState(i);
                std::cout << i << " done\n";
            }
        }
        t1.stop();
        fout << from << ", " << t1.getLastElapsedTime() << ", " << problem.statistics() << std::endl;
    }
    return 0;
}


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

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

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

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

 /*!
    @ingroup group-operator
    Function object implementing minus capabilities

    Perform the difference of two parameters of the same type.

    @par Semantic

    For any value @c a and @c b of type @c T,

    @code
    T x = minus(a,b);
    @endcode

    returns the difference of @c a and @c b

  **/
  const boost::dispatch::functor<tag::minus_> minus = {};
} }
#endif

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

#endif


#pragma once

#include <vector>

#include <Eigen/Core>

#include <ccd.hpp>

#include <CollisionObject/CollisionObject.h>
#include <Mesh.hpp>

#ifdef IPC_WITH_TIGHT_INCLUSION
#define TIGHT_INCLUSION_MAX_ITER 1e6
#define TIGHT_INCLUSION_CCD_TYPE 1
#define TIGHT_INCLUSION_DIST_P 0.2
#define TIGHT_INCLUSION_MIN_DIST tolerance
#endif

namespace IPC {

#ifdef IPC_WITH_TIGHT_INCLUSION
extern std::array<double, 3> tight_inclusion_vf_err, tight_inclusion_ee_err;
#endif

extern Eigen::Vector3d invShift;

#ifdef IPC_WITH_TIGHT_INCLUSION
template <int dim>
void computeTightInclusionError(
    Mesh<dim>& mesh,
    std::vector<std::shared_ptr<CollisionObject<dim>>>& meshCollisionObjects);

template <int dim>
void computeTightInclusionError(
    Mesh<dim>& mesh,
    std::vector<std::shared_ptr<CollisionObject<dim>>>& meshCollisionObjects,
    const Eigen::Vector3d& world_min,
    const Eigen::Vector3d& world_max);
#endif

#ifdef IPC_WITH_FPRP_CCD
template <int dim>
Eigen::Vector3d shiftVertices(
    Mesh<dim>& mesh,
    std::vector<std::shared_ptr<CollisionObject<dim>>>& meshCollisionObjects);

template <int dim>
Eigen::Vector3d shiftVertices(
    Mesh<dim>& mesh,
    std::vector<std::shared_ptr<CollisionObject<dim>>>& meshCollisionObjects,
    const Eigen::Vector3d& world_min,
    const Eigen::Vector3d& world_max);
#endif

bool vertexFaceToIBisection(
    const Eigen::Vector3d& v_t0,
    const Eigen::Vector3d& f0_t0,
    const Eigen::Vector3d& f1_t0,
    const Eigen::Vector3d& f2_t0,
    const Eigen::Vector3d& v_t1,
    const Eigen::Vector3d& f0_t1,
    const Eigen::Vector3d& f1_t1,
    const Eigen::Vector3d& f2_t1,
    const ccd::CCDMethod ccdMetod,
    double& toi,
    double eps_t = 1e-3);

bool edgeEdgeToIBisection(
    const Eigen::Vector3d& ea0_t0,
    const Eigen::Vector3d& ea1_t0,
    const Eigen::Vector3d& eb0_t0,
    const Eigen::Vector3d& eb1_t0,
    const Eigen::Vector3d& ea0_t1,
    const Eigen::Vector3d& ea1_t1,
    const Eigen::Vector3d& eb0_t1,
    const Eigen::Vector3d& eb1_t1,
    const ccd::CCDMethod ccdMetod,
    double& toi,
    double eps_t = 1e-3);

} // namespace IPC


#include <dirent.h>
#include <algorithm>
#include <ctime>
#include <fstream>
#include <iostream>
#include <sstream>
#include <string>

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

#include <cv_bridge/cv_bridge.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/PoseArray.h>
#include <geometry_msgs/PoseStamped.h>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>
#include <ros/package.h>
#include <ros/ros.h>
#include <sensor_msgs/Image.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>

#include "Thirdparty/g2o/g2o/core/block_solver.h"
#include "Thirdparty/g2o/g2o/core/optimization_algorithm_levenberg.h"
#include "Thirdparty/g2o/g2o/solvers/linear_solver_dense.h"
#include "Thirdparty/g2o/g2o/solvers/linear_solver_eigen.h"
#include "Thirdparty/g2o/g2o/types/types_six_dof_expmap.h"

#include <quadric_slam/Frame.h>
#include <quadric_slam/Object_landmark.h>
#include <quadric_slam/g2o_Object.h>

using namespace g2o;

void getFiles(string path, vector<string>& files) {
  DIR* dir;
  struct dirent* ptr;
  dir = opendir((char*)path.c_str());
  while ((ptr = readdir(dir)) != NULL) {
    files.push_back(path + ptr->d_name);
  }
  return;
}

int main() {
  std::string base_folder =
      ros::package::getPath("object_slam") + "/data/quadic_test_data/";

  vector<string> files;
  getFiles(base_folder, files);

  Matrix3d calib;
  calib << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1;

  Quadric quadric = Quadric();
  std::cout << "read file\n" << std::endl << std::endl;
  Eigen::MatrixXd rawQuadric(4, 19);
  read_all_number_txt(base_folder + "quadric_parameters.txt", rawQuadric);
  std::cout << "read file\n" << std::endl << std::endl;
  Eigen::MatrixXd rawCamPose(35, 16);
  read_all_number_txt(base_folder + "camera_rt35.txt", rawCamPose);

  std::cout << "rawQuadric \n" << rawQuadric << std::endl << std::endl;
  std::cout << "rawCamPose \n" << rawCamPose << std::endl;

  std::cout << "read image" << std::endl;
  for (int j = 0; j < 35; j++) {
    cv::Mat src = imread(
        "/home/jerryai/Documents/slam/quadricslam_ws/src/quadric_slam/"
        "object_slam/data/quadic_test_data/1311873050.488248.png");
    for (int i = 0; i < 4; i++) {
      // set quadric
      quadric.scale = rawQuadric.block(i, 0, 1, 3).transpose();
      Eigen::MatrixXd Rq(3, 3);
      Rq.block(0, 0, 3, 1) = rawQuadric.block(i, 3, 1, 3).transpose();
      Rq.block(0, 1, 3, 1) = rawQuadric.block(i, 7, 1, 3).transpose();
      Rq.block(0, 2, 3, 1) = rawQuadric.block(i, 11, 1, 3).transpose();

      std::cout << "Rq" << Rq << std::endl;
      std::cout << "Rq.det" << Rq.determinant() << std::endl;
      Vector3d Tq = rawQuadric.block(i, 15, 1, 3).transpose();
      std::cout << "Tq" << Tq << std::endl;
      quadric.pose = SE3Quat(Rq, Tq);
      std::cout << "rotation matrix"
                << quadric.pose.rotation().toRotationMatrix() << std::endl;
      std::cout << "E"
                << Rq * quadric.pose.rotation()
                            .toRotationMatrix()
                            .block(0, 0, 3, 3)
                            .inverse()
                << std::endl;
      // set cam pose
      Eigen::MatrixXd Rc(3, 3);
      Rc.block(0, 0, 3, 1) = rawCamPose.block(j, 0, 1, 3).transpose();
      Rc.block(0, 1, 3, 1) = rawCamPose.block(j, 4, 1, 3).transpose();
      Rc.block(0, 2, 3, 1) = rawCamPose.block(j, 8, 1, 3).transpose();
      Vector3d Tc = rawCamPose.block(j, 12, 1, 3).transpose();
      std::cout << "Rc" << Rc << std::endl;
      std::cout << "Tc" << Tc << std::endl;
      SE3Quat camPose = SE3Quat(Rc, Tc);
      Vector4d bbox = quadric.projectOntoImageRect(camPose.inverse(), calib);

      cv::rectangle(src, Point(bbox(0), bbox(1)), Point(bbox(2), bbox(3)),
                    Scalar(0, i * 40, 150), 1, LINE_8, 0);
    }
    std::cout << "show image" << std::endl;
    cv::imshow("src", src);
    cv::waitKey(0);
  }
  return 0;
}


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

  @copyright 2015 NumScale SAS
  @copyright 2015 J.T.Lapreste

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

#include <boost/simd/constant/maxexponent.hpp>
#include <boost/simd/constant/nbmantissabits.hpp>
#include <boost/simd/function/scalar/bitwise_cast.hpp>
#include <boost/simd/function/scalar/shift_left.hpp>
#include <boost/simd/function/scalar/toint.hpp>
#include <boost/dispatch/meta/as_integer.hpp>

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

    template < class A0 >
    inline A0 scale(A0 const & y, const A0& k)
    {
      using i_t = bd::as_integer_t<A0>;
      i_t ik =  toint(k);
      ik +=  Maxexponent<A0>();
      ik = shift_left(ik, Nbmantissabits<A0>());
      return y*bitwise_cast<A0>(ik);;
    }
  }
} }
#endif


#include "brbc.h"

#include "graph.h"
#include "salt/base/mst.h"

#include <boost/graph/dijkstra_shortest_paths.hpp>

namespace salt {

void BrbcBuilder::ExportPath(const Net& net, Tree& path) {
    Tree mst;
    MstBuilder mstB;
    mstB.Run(net, mst);
    path.net = &net;
    path.source = make_shared<TreeNode>(net.source()->loc, net.source(), net.source()->id);
    auto cur = path.source;
    mst.PreOrder([&](const shared_ptr<TreeNode>& tn) {
        if (!tn->parent) return;
        auto p = tn->pin;
        auto pre = cur;
        cur = make_shared<TreeNode>(p->loc, p, p->id);
        TreeNode::SetParent(cur, pre);
    });
}

void BrbcBuilder::Run(const Net& net, Tree& tree, double eps) {
    // init graph by MST
    Tree mst;
    MstBuilder mstB;
    mstB.Run(net, mst);
    Graph g = Graph(net.pins.size());
    boost::property_map<Graph, boost::edge_weight_t>::type weightmap = get(boost::edge_weight, g);
    mst.PreOrder([&](const shared_ptr<TreeNode>& tn) {
        if (!tn->parent) return;
        Edge e;
        bool inserted;
        tie(e, inserted) = add_edge(tn->pin->id, tn->parent->pin->id, g);  // Vertex is type int?
        weightmap[e] = Dist(tn->loc, tn->parent->loc);
    });

    // depth-first tour (n-1 vertices)
    DTYPE curDist = 0, shortestDist;
    Point preLoc = net.source()->loc;
    mst.PreOrder([&](const shared_ptr<TreeNode>& tn) {
        if (!tn->parent) return;
        auto p = tn->pin;
        curDist += Dist(p->loc, preLoc);
        shortestDist = Dist(p->loc, net.source()->loc);
        if (this->Comp(curDist, shortestDist, eps)) {
            this->Update(curDist, shortestDist);
            auto res = add_edge(p->id, net.source()->id, g);
            weightmap[res.first] = shortestDist;
        }
        preLoc = p->loc;
    });

    // Dijkstra
    vector<Vertex> p(num_vertices(g));
    vector<DTYPE> d(num_vertices(g));
    Vertex s = vertex(net.source()->id, g);
    dijkstra_shortest_paths(
        g,
        s,
        predecessor_map(boost::make_iterator_property_map(p.begin(), get(boost::vertex_index, g)))
            .distance_map(boost::make_iterator_property_map(d.begin(), get(boost::vertex_index, g))));

    // transform to saltTree
    vector<shared_ptr<TreeNode>> nodes(net.pins.size());
    for (int i = 0; i < net.pins.size(); ++i) {
        auto p = net.pins[i];
        nodes[i] = make_shared<TreeNode>(p->loc, p, p->id);
    }
    boost::graph_traits<Graph>::vertex_iterator vi, vend;
    for (boost::tie(vi, vend) = vertices(g); vi != vend; ++vi) {
        int down = *vi, up = p[*vi];
        if (up == down) continue;
        TreeNode::SetParent(nodes[down], nodes[up]);
    }

    tree.source = nodes[net.source()->id];
    tree.net = &net;
}

}  // namespace salt


#include "PCE.h"
#include "GeomCommonFunctions.h"
#include <Gui/Application.h>
#include <BRepBuilderAPI_GTransform.hxx>
#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <tchar.h>
#include <iostream>

using namespace PartDesignGui;

void PCEEnd::PCECUT::Translate(const Vector2d& t)
{
	Vector3d t_3d(t[0], t[1], 0.0);
	origin_s += t;
	origin_e += t;
	origin_c += t;
	offset_s += t;
	offset_e += t;
	offset_c += t;
	u_cutting_line_s += t_3d;
	u_cutting_line_e += t_3d;
	l_cutting_line_s += t_3d;
	l_cutting_line_e += t_3d;
}

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
PCEEnd::PCEEnd(
	const double& kerf, const int& index_,
	const Vector3d& u_s_, const Vector3d& u_e_,
	const Vector3d& d_s_, const  Vector3d& d_e_,
	const Vector3d2& con_surfs_,
	const Vector3d1& con_surf_normals_,
	const std::vector<int> &con_surf_indexes_) :
	index(index_),
	u_s(u_s_), u_e(u_e_),
	l_s(d_s_), l_e(d_e_),
	con_surfs(con_surfs_),
	con_surf_normals(con_surf_normals_),
	con_surf_indexes(con_surf_indexes_)
{
	style = -1;
	GetEdge(kerf);
}

void PCEEnd::GetEdge(const double& kerf)
{
	double angle = Math::Functs::GetAngleBetween(u_e - u_s, l_e - l_s);
	if (angle > Math::Math_PI / 2.0)angle = Math::Math_PI - angle;
	if (!Math::Functs::IsAlmostZero(angle))
	{
		std::cerr << "double angle = Math::Functions::GetAngleBetween(u_e - u_s, l_e - l_s)" << std::endl;
		system("pause");
	}

	auto x = Vector2d((u_e - u_s)[0], (u_e - u_s)[1]);
	auto o = Vector2d(((u_s + u_e) / 2.0)[0], ((u_s + u_e) / 2.0)[1]);
	auto y = Math::Functs::RotationAxis2d(x, -Math::Math_PI / 2.0, Vector2d(0.0, 0.0));
	if (Math::Functs::GetAngleBetween(y, Vector2d(con_surf_normals[0][0], con_surf_normals[0][1])) > Math::Math_PI / 2.0)
		x = -x;

	LocalCS cs(o, x);

	Vector2d l_s_2d = cs.GetLocal(Vector2d(l_s[0], l_s[1]));
	Vector2d l_e_2d = cs.GetLocal(Vector2d(l_e[0], l_e[1]));

	if (Math::Functs::IsAlmostZero(l_s_2d[1]) && Math::Functs::IsAlmostZero(l_e_2d[1]))
		style = VerticalEnd;
	else
		style = TiltingEnd;

	Vector2d2 surfs_2d;
	for (auto& surf : con_surfs)
	{
		Vector2d1 surf_2d;
		for (auto& p : surf)
			surf_2d.emplace_back(cs.GetLocal(Vector2d(p.x, p.y)));
		surfs_2d.emplace_back(surf_2d);
	}

	Vector2d minimal_corner, maximal_corner;
	Math::Functs::GetBoundingBox(surfs_2d, minimal_corner, maximal_corner);
	cut.origin_s = cs.GetGlobalPos(Vector2d(minimal_corner[0], maximal_corner[1]));
	cut.origin_e = cs.GetGlobalPos(Vector2d(maximal_corner[0], maximal_corner[1]));

	double delta_z = u_s[2] - l_s[2];

	if (style == VerticalEnd)
	{
		cut.offset_s = cs.GetGlobalPos(Vector2d(minimal_corner[0], maximal_corner[1] + kerf / 2.0));
		cut.offset_e = cs.GetGlobalPos(Vector2d(maximal_corner[0], maximal_corner[1] + kerf / 2.0));

		cut.cutting_surface_normal = con_surf_normals[0];
		cut.l_cutting_line_s = Vector3d(cut.offset_s[0], cut.offset_s[1], 0.0);
		cut.l_cutting_line_e = Vector3d(cut.offset_e[0], cut.offset_e[1], 0.0);
		cut.u_cutting_line_s = Vector3d(cut.offset_s[0], cut.offset_s[1], delta_z);
		cut.u_cutting_line_e = Vector3d(cut.offset_e[0], cut.offset_e[1], delta_z);
		cut.l_u_vector = Vector3d(0.0, 0.0, delta_z);
	}
	else
	{
		auto tilting_angle = Math::Functs::GetAngleBetween(con_surf_normals[0], Vector3d(0.0, 0.0, 1.0));
		double kerf_distance = 0.5 * kerf / cos(abs(tilting_angle - Math::Math_PI / 2.0));
		cut.offset_s = cs.GetGlobalPos(Vector2d(minimal_corner[0], maximal_corner[1] + kerf_distance));
		cut.offset_e = cs.GetGlobalPos(Vector2d(maximal_corner[0], maximal_corner[1] + kerf_distance));
		cut.cutting_surface_normal = con_surf_normals[0];
		if (tilting_angle < Math::Math_PI / 2.0)
		{
			cut.l_cutting_line_s = Vector3d(cut.offset_s[0], cut.offset_s[1], 0.0);
			cut.l_cutting_line_e = Vector3d(cut.offset_e[0], cut.offset_e[1], 0.0);
			Vector3d cutting_direction = Math::Functs::GetCrossproduct(cut.l_cutting_line_e - cut.l_cutting_line_s, cut.cutting_surface_normal);
			if (Math::Functs::GetAngleBetween(cutting_direction, Vector3d(0.0, 0.0, 1.0)) > Math::Math_PI / 2.0)
				cutting_direction = -cutting_direction;
			Math::Functs::SetVectorLength(cutting_direction, delta_z / cos(Math::Math_PI / 2.0 - tilting_angle));
			cut.u_cutting_line_s = cut.l_cutting_line_s + cutting_direction;
			cut.u_cutting_line_e = cut.l_cutting_line_e + cutting_direction;
			cut.l_u_vector = cutting_direction;
		}
		else
		{
			cut.u_cutting_line_s = Vector3d(cut.offset_s[0], cut.offset_s[1], delta_z);
			cut.u_cutting_line_e = Vector3d(cut.offset_e[0], cut.offset_e[1], delta_z);
			Vector3d cutting_direction = Math::Functs::GetCrossproduct(cut.u_cutting_line_e - cut.u_cutting_line_s, cut.cutting_surface_normal);
			if (Math::Functs::GetAngleBetween(cutting_direction, Vector3d(0.0, 0.0, 1.0)) < Math::Math_PI / 2.0)
				cutting_direction = -cutting_direction;
			Math::Functs::SetVectorLength(cutting_direction, delta_z / cos(tilting_angle - Math::Math_PI / 2.0));
			cut.l_cutting_line_s = cut.u_cutting_line_s + cutting_direction;
			cut.l_cutting_line_e = cut.u_cutting_line_e + cutting_direction;
			cut.l_u_vector = -cutting_direction;
		}
	}
}

void PCEEnd::Clear()
{
	Vector3d2().swap(con_surfs);
}


#ifndef STAN_MATH_PMETRICS_PKMODEL_INTEGRATOR_HPP
#define STAN_MATH_PMETRICS_PKMODEL_INTEGRATOR_HPP

#include <Eigen/Dense>
#include <stan/math/prim/arr/functor/integrate_ode_rk45.hpp>
#include <stan/math/rev/mat/functor/integrate_ode_bdf.hpp>
#include <iostream>
#include <string>
#include <vector>

namespace torsten {

/**
 *  Construct functors that run the ODE integrator. Specify integrator
 *  type, the base ODE system, and the tuning parameters (relative tolerance,
 *  absolute tolerance, and maximum number of steps).
 */
struct integrator_structure {
private:
  double rel_tol, abs_tol;
  long int max_num_steps;  // NOLINT(runtime/int)
  std::ostream* msgs;
  std::string solver_type;

public:
  integrator_structure() {
    rel_tol = 1e-10;
    abs_tol = 1e-10;
    max_num_steps = 1e8;
    msgs = 0;
    solver_type = "rk45";
  }

  integrator_structure(double p_rel_tol,
                       double p_abs_tol,
                       long int p_max_num_steps,  // NOLINT
                       std::ostream* p_msgs,
                       std::string p_solver_type) {
      rel_tol = p_rel_tol;
      abs_tol = p_abs_tol;
      max_num_steps = p_max_num_steps;
      msgs = p_msgs;
      solver_type = p_solver_type;
  }

  // CONSTRUCTOR FOR OPERATOR
  template<typename F, typename T1, typename T2>
  std::vector<std::vector<typename stan::return_type<T1, T2>::type> >
  operator() (const F& f,
              const std::vector<T1> y0,
              const double t0,
              const std::vector<double>& ts,
              const std::vector<T2>& theta,
              const std::vector<double>& x,
              const std::vector<int>& x_int) const {
    if (solver_type == "bdf")
      return stan::math::integrate_ode_bdf(f, y0, t0, ts, theta, x, x_int,
                                           msgs,
                                           rel_tol, abs_tol, max_num_steps);
    else  // if(solver_type == "rk45")
      return stan::math::integrate_ode_rk45(f, y0, t0, ts, theta, x, x_int,
                                            msgs,
                                            rel_tol, abs_tol, max_num_steps);
  }
};

}

#endif


/*
 * BatchEstimator.hpp
 *
 *  Created on: 03.09.2018
 *      Author: tomlucas
 */

#ifndef ESTIMATORS_BATCHESTIMATOR_HPP_
#define ESTIMATORS_BATCHESTIMATOR_HPP_

#include "../Plugins/imu_plugin.hpp"
#include "../OSG_VIZ/osg_viz.hpp"
#include <OSG_Utils.hpp>
#include "AlignType.hpp"

#include <ZaviConfig.hpp>
#include <Eigen/Core>
#include <ceres/ceres.h>

#include <ceres/autodiff_local_parameterization.h>
#include <ceres/local_parameterization.h>
#include <ceres/loss_function.h>
#include <vector>
#include <limits>
#include <Callbacker.hpp>
#include "Models/State.hpp"
#include "StateBoxes/HomogeneousCoordinatesBox.hpp"
namespace zavi
::estimator {
#define CARTESIAN_DIMENSIONS 3
	typedef model::State<state_boxes::HomoCoordBox> ALIGNMENT_PARAMETERIZATION;

	/**
	 * This class implements the basic functionality of a batch estimator
	 *
	 * constraints (measurements) have to be added in derivates of this class via addConstraint
	 */
	template<typename model_type>
	class BatchEstimator: public Estimator {
	protected:

		/**
		 * Create the plot to show the estimated trajectory this is basic functionality since the batch estimator shouldnt be called at every iteration for performance reasons
		 * instead it shows the past trajectory as a state plot
		 */
		BatchEstimator(std::shared_ptr<plugin::SensorPlugin> sensor, bool output_replay = false, double skip_time=0) :
		box_model(sensor), output_replay(output_replay), current_estimation(NULL), current_replay_index(-1), current_size(
				0), states(NULL), alignment(NULL), align_parameter(sensor) , skip_time(skip_time),num_states(0) {
			//plot=zavi::plot::createPlot();
			state_vector = state_vector.Zero();
			lower_bound = lower_bound.Ones() * (0 - std::numeric_limits<double>::infinity());
			upper_bound = upper_bound.Ones() * std::numeric_limits<double>::infinity();
			if (output_replay)
			Timer::activateReplay(true);
		}
	public:
		// List of callback indices
		enum CallbackIndices {CI_ESTIMATE_SUBSEQUENCE_READY,     //!< called after every subsequence (first 15k states, first 25k states...)
			CI_EVERY_SOLVER_ITERATION_CALLBACK,//!< called at every ceres iteration callback
			CI_REPLAY_COUNTER_INCREASE,//!< called whenever the counter for replay of states is increased
			CI_FAST_REPLAY_ITERATION,//!< called in every iteration of fast replay
			CI_ESTIMATE_READY};     //!< called when estimate is completed

		typedef model_type MODEL_TYPE;//model type
		template<typename T>
		using STATE_TYPE_T= typename MODEL_TYPE::template OUTER_T<T>;
		template<typename T>
		using SIGMA_TYPE_T= typename MODEL_TYPE::template INNER_T<T>;
		template<typename T>
		using INPUT_TYPE_T= typename MODEL_TYPE::template INPUT_T<T>;
		virtual ~BatchEstimator() {
			if (current_estimation != NULL)
			delete current_estimation;
			if (alignment != NULL)
			delete alignment;

		}

		/**
		 * Callback for imu
		 * @param plug the imu plugin which has new data
		 * @param estimator the estimator which gets the new data
		 * @param time the current time stamp
		 */
		static void batchCallback(plugin::SensorPlugin * plug, void* estimator, double time) {
			BatchEstimator * esti = static_cast<BatchEstimator *>(estimator);
			esti->estimate();
		}

		/**
		 *
		 * @param start start point for transition
		 * @param next  after the transition
		 * @param time_diff time_diff between points
		 * @param next_index the index to take readings of input
		 */
		virtual void advanceState(double * start, double * next, double time_diff, double next_index)=0;

		virtual void setInitialState(double * data, unsigned int index)=0;
		/**
		 * Set the pose in data to the middle of the constrained area
		 * @param data the pointer to the data
		 */
		virtual void setNeutralPose(double * data) {
			for (int i = 0; i < 3; i++)
			data[i] = lower_bound(i, 0) + (upper_bound(i, 0) - lower_bound(i, 0)) * 0.5;
		}
		/**
		 * Add the calibration matrix to the problem
		 * @param problem the problem to add the alignment to
		 * @param align  the alignment
		 * @param constant if constant is true, the estimator does not alter the alignment
		 */
		void addAlignment(ceres::Problem & problem,const double * align=NULL, bool constant = true) {
			ALIGNMENT_TYPE<double> temp = temp.Identity();
			alignment = (double *) malloc(ALIGNMENT_SIZE * sizeof(double));
			if(align !=NULL) {
				LOG(INFO) << "loading alignment";
				memcpy(alignment,align,ALIGNMENT_SIZE * sizeof(double));
				Eigen::Map<Eigen::Matrix4d> map(alignment);
				map.block<3,3>(0,0)=eigen_util::normaliseRotation(map.block<3,3>(0,0));
			}
			else {
				memcpy(alignment, temp.data(), sizeof(double) * ALIGNMENT_SIZE);
			}
			ceres::LocalParameterization * local_parameterization = new ceres::AutoDiffLocalParameterization<
			ALIGNMENT_PARAMETERIZATION, ALIGNMENT_PARAMETERIZATION::outer_size,
			ALIGNMENT_PARAMETERIZATION::inner_size>(new ALIGNMENT_PARAMETERIZATION(align_parameter));
			problem.AddParameterBlock(alignment, ALIGNMENT_PARAMETERIZATION::outer_size, local_parameterization);
			if (constant)
			problem.SetParameterBlockConstant(alignment);
		}

		/**
		 * Fullfills a batch estimate
		 */
		virtual void estimate() {
			if (inputs.size() == 0)
			return;     // nothing to do if no no constraints can be put
			double * temp = NULL;

			num_states = inputs.size();

			if (states == NULL)
			states = (double *) malloc(num_states * MODEL_TYPE::outer_size * sizeof(double));

			options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
			options.minimizer_progress_to_stdout = true;
			options.num_threads = 10;
			options.gradient_tolerance = 1.;
			options.max_num_iterations = 12;
			options.update_state_every_iteration = true;

			IterationCounter counter(states, num_states, this);
			options.callbacks.push_back(&counter);
			ceres::Solver::Summary summary;

			osg::ref_ptr<osg::Geode> geo(new osg::Geode());
			zavi::osg_viz::root->addChild(geo);
			std::printf("Creating %zu states\n", num_states);
			for (unsigned int i = 0; i < num_states; i++) {
				setInitialState(&states[MODEL_TYPE::outer_size * i], i);
			}
			bool final_iteration = false;
			unsigned int last_state_size = 0;

			for (unsigned int state_size = 15000; !final_iteration; state_size += 2500) {

				ceres::LocalParameterization * local_parameterization = new ceres::AutoDiffLocalParameterization<MODEL_TYPE,
				MODEL_TYPE::outer_size, MODEL_TYPE::inner_size>(new MODEL_TYPE(box_model));
				if (state_size >= num_states) {
					state_size = num_states;
					final_iteration = true;
					options.gradient_tolerance = 1e-10;
					options.max_num_iterations = 50;
				}

				ceres::Problem problem;
				addAlignment(problem);
				for (unsigned int i = 0; i < state_size; i++) {
					if (last_state_size > 0 and i >= last_state_size)
					advanceState(&states[(i - 1) * MODEL_TYPE::outer_size], &states[i * MODEL_TYPE::outer_size],
							time_diffs[i - 1], i);

					//setNeutralPose(&states[i*MODEL_TYPE::outer_size]);

					problem.AddParameterBlock(&states[i * MODEL_TYPE::outer_size], MODEL_TYPE::outer_size,
							local_parameterization);
					for (int k = 0; k < MODEL_TYPE::outer_size; k++) {
						problem.SetParameterLowerBound(&states[i * MODEL_TYPE::outer_size], k, lower_bound(k, 0));
						problem.SetParameterUpperBound(&states[i * MODEL_TYPE::outer_size], k, upper_bound(k, 0));
					}
				}
				addConstraints(problem, states, state_size, this);

				printf("Starting solver\n");
				ceres::Solve(options, &problem, &summary);
				std::cout << summary.FullReport() << "\n";
				printf("Estimate complete\n");
				//zavi::plot::stateDraw<0>(states,"Batch estimated states");
				if (output_replay) {
					temp = (double *) current_estimation;     // safe pointer to delete old data
					current_estimation = (double *) malloc(state_size * MODEL_TYPE::outer_size * sizeof(double));
					current_size = state_size;
					memcpy((void *) current_estimation, states, state_size * MODEL_TYPE::outer_size * sizeof(double));
					if (temp != NULL and temp != states)
					delete temp;
					std::vector<STATE_TYPE_T<double>> all_states;
					std::vector<double> norms;
					//auto input_measurement=typename MODEL_TYPE::input_measurement();
					double max_norm = 0.;
					for (unsigned int i = 0; i < state_size; i++) {
						all_states.push_back(STATE_TYPE_T<double>(&states[i * MODEL_TYPE::outer_size]));
						//norms.push_back((input_measurement.template operator()<double>(all_states.back(),Eigen::Map<Eigen::Matrix<double,4,4> >(alignment),NULL)-inputs[i]).norm());
						if (norms.back() > max_norm)
						max_norm = norms.back();
					}

					for (unsigned int i = 0; i < state_size; i++) {
						norms[i] = norms[i] / max_norm;
					}
					zavi::printf("First state is :");
					zavi::printf(all_states[0]);
					if (geo->getNumChildren() > 0) {
						geo->removeChild(0, 1);
					}
					geo->addChild(
							zavi::osg_viz::pathShower((double *) current_estimation, &norms[0], current_size,
									MODEL_TYPE::outer_size));
					//zavi::plot::stateDraw<0>(all_states,"Estimated States");
					//zavi::plot::stateDraw<1>(inputs,"INputs");

				} else {
					state_vector = STATE_TYPE_T<double>(&states[(state_size - 1) * MODEL_TYPE::outer_size]);
					delete states;
					delete alignment;
				}
				last_state_size = state_size;
				notifyCallbacks(CI_ESTIMATE_SUBSEQUENCE_READY);
			}
			notifyCallbacks(CI_ESTIMATE_READY);
		}
		/**
		 * Does a fast replay and calls
		 * callbacks under CI_FAST_REPLAY_ITERATION
		 *
		 * Inhibits normal replay and restarts it afterwards
		 *
		 * @param state_size the amount of states to replay
		 */
		void fastReplay(int state_size) {
			int temp_cur_size = current_size;
			current_size = 0;     // set 0 to not be intereferd by normal replay
			double time = skip_time;
			while (current_replay_index < state_size - 1) {
				current_replay_index++;
				time += time_diffs[current_replay_index];
				if (!current_replay_index)     // fix that first time diff is artificially 0.00001 or something like that
				time = skip_time;
				Timer::setReplayTime(time);
				notifyCallbacks(CI_FAST_REPLAY_ITERATION);
			}
			//restart normal replay
			current_size = temp_cur_size;
			current_replay_index = -1;
		}

		/**
		 * calls estimate with freq
		 * @param freq loop frequency
		 * @param running function to determine whether this thread should be running
		 */
		void run(double freq, bool (*running)()) {
			Timer loop(freq);
			double time = skip_time;
			while (running()) {
				if (output_replay) {
					if (current_replay_index < current_size - 1) {
						current_replay_index++;
						time += time_diffs[current_replay_index];
						if (!current_replay_index)     // fix that first time diff is artificially 0.00001 or something like that
						time = skip_time;
						Timer::setReplayTime(time);
						notifyCallbacks(CI_REPLAY_COUNTER_INCREASE);
					}
					loop.wait();
				} else {
					loop.wait();     // dont start estimating directly since nothing is there to estimate
					estimate();
					printf("Estimated values\n");
				}
			}

		}

		virtual inline Eigen::Vector3d getEstimatedPosition() {
			if (current_replay_index >= 0) {
				return Eigen::Map<Eigen::Vector3d>(
						(double *) &current_estimation[current_replay_index * MODEL_TYPE::outer_size]);
			} else
			return (state_vector.block(0, 0, 3, 1));
		}

		virtual inline Eigen::Quaterniond getEstimatedOrientation() {
			if (current_replay_index >= 0) {
				return Eigen::Quaterniond(
						MODEL_TYPE::template getOrientation<double>(
								Eigen::Map<STATE_TYPE_T<double> >(
										(double *) &current_estimation[current_replay_index * MODEL_TYPE::outer_size])));
			} else
			return Eigen::Quaterniond(Eigen::AngleAxisd(0, Eigen::Vector3d(1, 0, 0)));     //< static orientation
		}

		virtual inline Eigen::Matrix3d getEstimatedPositionError() {
			throw " Not implemented getEstimatedPositionError for BatchEstimator";
		}

		inline MODEL_TYPE getBoxModel() {
			return box_model;
		}
	protected:

		/**
		 * Callback to get the iteration count
		 */
		class IterationCounter: public ceres::IterationCallback {
		public:
			static constexpr int num_iterations_till_full = 20;
			static inline volatile int counter = 1;
			static double ratio() {
				return (double) counter / num_iterations_till_full;
			}
			double* states;
			int num_states;
			osg::ref_ptr<osg::Geode> geo;
			BatchEstimator * estimator;
			IterationCounter(double * states, int num_states, BatchEstimator * estimator) :
			states(states), num_states(num_states), geo(new osg::Geode()), estimator(estimator) {
				zavi::osg_viz::root->addChild(geo);

			}
			virtual ~IterationCounter() {
				zavi::printf("Deleting geometry of callback");
				zavi::osg_viz::root->removeChild(geo);
			}

			virtual ceres::CallbackReturnType operator()(const ceres::IterationSummary& summary) {
				counter = summary.iteration + 2;     // +2 because it starts at 0 but we assume 1 to be the first element and we get the number after and not before the iteration
				if (geo->getNumChildren() > 0) {
					geo->removeChild(0, 1);
				}
				geo->addChild(zavi::osg_viz::pathShower((double *) states, num_states, MODEL_TYPE::outer_size));

				for (unsigned int i = 0; i < num_states; i++) {
					Eigen::Map<STATE_TYPE_T<double> > state(&states[i * MODEL_TYPE::outer_size]);
					state=estimator->box_model.normalise(state);
				}

				estimator->notifyCallbacks(CI_EVERY_SOLVER_ITERATION_CALLBACK);
				return ceres::SOLVER_CONTINUE;
			}
		}
		;

		/**
		 * A cost function comparing the difference of 2 states with the corresponding measurement
		 *
		 * uses boxMinus of the Model
		 */
		struct TransitionCostFunction {
			double time_diff;     //< the timediff at this transition
			BatchEstimator *estimator;
			double ratio_index;//number of this constrain
			TransitionCostFunction(double time_diff, BatchEstimator *estimator, int index, int state_num) :
			time_diff(time_diff), estimator(estimator), ratio_index((double) index / state_num) {

			}

			double costModifier() const {
				return 1. + 1. / pow(7.5 * (ratio_index + IterationCounter::ratio()), 2);
			}

			template<typename T>
			bool operator()(const T * const a, const T* const b, T* result) const {
				(Eigen::Map<typename MODEL_TYPE::template COST_T<T> >(result)) =
				(this->estimator->getBoxModel().template transitionCost < T
						> (Eigen::Map<const STATE_TYPE_T<T> >(a), time_diff, Eigen::Map<const STATE_TYPE_T<T> >(
										b)));
				return true;
			}
		};
		/**
		 * applies the transition constraint
		 * @param problem  ceres problem
		 * @param states list of all states
		 * @param inputs list of all inputs
		 * @param time_diffs list of all time diffs
		 */
		static void transitionConstrain(ceres::Problem &problem, double *states, double * alignment, int state_size,
				void * data, std::vector<double> &time_diffs, BatchEstimator *estimator) {
			for (int i = 0; i < state_size - 1; i++) {
				ceres::CostFunction* cost_function = new ceres::AutoDiffCostFunction<TransitionCostFunction,
				MODEL_TYPE::cost_size, MODEL_TYPE::outer_size, MODEL_TYPE::outer_size>(
						new TransitionCostFunction(time_diffs[i+1], estimator, i, state_size - 1));
				problem.AddResidualBlock(cost_function, NULL, &states[i * MODEL_TYPE::outer_size],
						&states[(i + 1) * MODEL_TYPE::outer_size]);
			}
		}
		/**
		 * A cost function comparing the 1 state with the required circle trajectory
		 */
		template<template<typename T> class STATE_TYPE_T, typename MEASUREMENT_FUNCTION, int measure_dim>
		struct StateDiffFunction {
			template<typename T>
			using MEASUREMENT_TYPE_T= Eigen::Matrix<T,measure_dim,1>;
			MEASUREMENT_TYPE_T<double> & measurement;
			BatchEstimator *estimator;
			static inline MEASUREMENT_FUNCTION measurement_function = MEASUREMENT_FUNCTION();
			StateDiffFunction(MEASUREMENT_TYPE_T<double> & measurement, BatchEstimator *estimator) :
			measurement(measurement), estimator(estimator) {
				for (int i = 0; i < measure_dim; i++) {
					assert(measurement(i,0)!=NAN);
				}
				assert(measurement_function.stiffnes.determinant() > 0.);
			}
			template<typename T>
			bool operator()(const T * const a, const T * const alignment, T* result) const {
				(Eigen::Map<Eigen::Matrix<T, MEASUREMENT_FUNCTION::output_size, 1> >(result)) =
				measurement_function.stiffnes
				* measurement_function.template boxminus<T>(
						measurement_function.template operator()<T>(
								Eigen::Map<const STATE_TYPE_T<T> >(a),
								Eigen::Map<const ALIGNMENT_TYPE<T> >(alignment), this->estimator),
						measurement);
				return true;
			}
		};

		/**
		 * A cost function comparing the 1 state with the required circle trajectory
		 */
		template<template<typename T> class STATE_TYPE_T, typename MEASUREMENT_FUNCTION, int measure_dim>
		struct InputDiffFunction {
			template<typename T>
			using MEASUREMENT_TYPE_T= Eigen::Matrix<T,measure_dim,1>;
			MEASUREMENT_TYPE_T<double> & measurement;
			BatchEstimator *estimator;
			static inline MEASUREMENT_FUNCTION measurement_function = MEASUREMENT_FUNCTION();
			InputDiffFunction(MEASUREMENT_TYPE_T<double> & measurement, BatchEstimator *estimator) :
			measurement(measurement), estimator(estimator) {
				for (int i = 0; i < measure_dim; i++) {
					assert(measurement(i,0)!=NAN);
				}
				assert(measurement_function.stiffnes.determinant() > 0.);
			}
			template<typename T>
			bool operator()(const T * const a, const T * const alignment, T* result) const {
				(Eigen::Map<Eigen::Matrix<T, MEASUREMENT_FUNCTION::output_size, 1> >(result)) =
				measurement_function.stiffnes
				* measurement_function.template boxminus<T>(
						measurement_function.template operator()<T>(
								Eigen::Map<const STATE_TYPE_T<T> >(a),
								Eigen::Map<const ALIGNMENT_TYPE<T> >(alignment), this->estimator),
						measurement);
				return true;
			}
		};

		/**
		 * A cost function comparing the 1 state with the required circle trajectory
		 */
		template<template<typename T> class STATE_TYPE_T, typename MEASUREMENT_FUNCTION, int measure_dim>
		struct LowInputFunction {
			template<typename T>
			using MEASUREMENT_TYPE_T= Eigen::Matrix<T,measure_dim,1>;
			BatchEstimator *estimator;
			static inline MEASUREMENT_FUNCTION measurement_function = MEASUREMENT_FUNCTION();
			LowInputFunction( BatchEstimator *estimator) :
			estimator(estimator) {
			}
			template<typename T>
			bool operator()(const T * const a, const T * const alignment, T* result) const {
				static Eigen::Matrix<double,6,6> stiffness=stiffness.Identity()*cfg->lookup("dummy_acc_stiffness");
				static bool firstcall=true;
				if(firstcall) {//really bad implementation
					stiffness.block<3,3>(3,3)=Eigen::Matrix3d::Identity()*cfg->lookup("dummy_ar_stiffness");
					firstcall=false;
				}
				(Eigen::Map<Eigen::Matrix<T, MEASUREMENT_FUNCTION::output_size, 1> >(result)) =
				stiffness*measurement_function.template operator()<T>(
						Eigen::Map<const STATE_TYPE_T<T> >(a),
						Eigen::Map<const ALIGNMENT_TYPE<T> >(alignment), this->estimator);
				return true;
			}
		};

		/**
		 * Applies the input constraint to a ceres problem
		 * @param problem  ceres problem
		 * @param states list of all states
		 * @param inputs list of all inputs
		 * @param time_diffs list of all time diffs
		 */
		template<typename input_type, typename measurement, int measure_dim>
		static void stateConstrain(ceres::Problem &problem, double *states, double * alignment, int state_size,
				void * data, std::vector<double> &time_diffs, BatchEstimator *estimator) {
			std::vector<input_type>* input_vector = static_cast<std::vector<input_type>*>(data);
			typedef StateDiffFunction<STATE_TYPE_T, measurement, measure_dim> FUNCTION_TYPE;
			for (int i = 0; i < state_size; i++) {
				ceres::CostFunction* cost_function = new ceres::AutoDiffCostFunction<FUNCTION_TYPE,
				measurement::output_size, MODEL_TYPE::outer_size, ALIGNMENT_SIZE>(
						new FUNCTION_TYPE((*input_vector)[i], estimator));
				problem.AddResidualBlock(cost_function, NULL, &states[i * MODEL_TYPE::outer_size], alignment);
			}
		}

		/**
		 * Applies the input constraint to a ceres problem
		 * @param problem  ceres problem
		 * @param states list of all states
		 * @param inputs list of all inputs
		 * @param time_diffs list of all time diffs
		 */
		template<typename input_type, typename measurement, int measure_dim, int stateNumber>
		static void singleStateConstrain(ceres::Problem &problem, double *states, double * alignment, int state_size,
				void * data, std::vector<double> &time_diffs, BatchEstimator *estimator) {
			std::vector<input_type>* input_vector = static_cast<std::vector<input_type>*>(data);
			typedef StateDiffFunction<STATE_TYPE_T, measurement, measure_dim> FUNCTION_TYPE;

			ceres::CostFunction* cost_function = new ceres::AutoDiffCostFunction<FUNCTION_TYPE,
			measurement::output_size, MODEL_TYPE::outer_size, ALIGNMENT_SIZE>(
					new FUNCTION_TYPE((*input_vector)[stateNumber], estimator));
			problem.AddResidualBlock(cost_function, NULL, &states[stateNumber * MODEL_TYPE::outer_size], alignment);

		}

		/**
		 * Applies the input constraint to a ceres problem
		 * @param problem  ceres problem
		 * @param states list of all states
		 * @param inputs list of all inputs
		 * @param time_diffs list of all time diffs
		 */
		template<typename measurement, int measure_dim, int stateNumber,
		Eigen::Matrix<double, measure_dim, 1> &measure_target>
		static void singleStateConstrain(ceres::Problem &problem, double *states, double * alignment, int state_size,
				void * data, std::vector<double> &time_diffs, BatchEstimator *estimator) {
			typedef StateDiffFunction<STATE_TYPE_T, measurement, measure_dim> FUNCTION_TYPE;
			ceres::CostFunction* cost_function = new ceres::AutoDiffCostFunction<FUNCTION_TYPE,
			measurement::output_size, MODEL_TYPE::outer_size, ALIGNMENT_SIZE>(
					new FUNCTION_TYPE(measure_target, estimator));
			problem.AddResidualBlock(cost_function, NULL, &states[stateNumber * MODEL_TYPE::outer_size], alignment);

		}

		/**
		 * Applies the input constraint to a ceres problem
		 * @param problem  ceres problem
		 * @param states list of all states
		 * @param inputs list of all inputs
		 * @param time_diffs list of all time diffs
		 */
		static void inputConstrain(ceres::Problem &problem, double *states, double * alignment, int state_size,
				void * data, std::vector<double> &time_diffs, BatchEstimator *estimator) {
			std::vector<INPUT_TYPE_T<double>>* inputs = static_cast<std::vector<INPUT_TYPE_T<double> >*>(data);
			typedef InputDiffFunction<STATE_TYPE_T, typename MODEL_TYPE::input_measurement, MODEL_TYPE::input_size> FUNCTION_TYPE;
			typedef LowInputFunction<STATE_TYPE_T, typename MODEL_TYPE::input_measurement, MODEL_TYPE::input_size> LOW_FUNCTION_TYPE;
			ceres::LossFunction* loss_function= new ceres::HuberLoss(cfg->lookup("input_huber"));
			ceres::CostFunction* no_input_cost = new ceres::AutoDiffCostFunction<LOW_FUNCTION_TYPE,
			MODEL_TYPE::input_size, MODEL_TYPE::outer_size, ALIGNMENT_SIZE>(
					new LOW_FUNCTION_TYPE(estimator));
			for (int i = 0; i < state_size; i++) {
				if(!estimator->input_mask[i]) {
					problem.AddResidualBlock(no_input_cost, NULL, &states[i * MODEL_TYPE::outer_size], alignment);
				}
				else {
					ceres::CostFunction* cost_function = new ceres::AutoDiffCostFunction<FUNCTION_TYPE,
					MODEL_TYPE::input_size, MODEL_TYPE::outer_size, ALIGNMENT_SIZE>(
							new FUNCTION_TYPE((*inputs)[i], estimator));
					problem.AddResidualBlock(cost_function, loss_function, &states[i * MODEL_TYPE::outer_size], alignment);
				}
			}
		}

	public:

		/**
		 * Type Erasure for batch constraints
		 */
		struct BatchConstrain {
			virtual void operator()(ceres::Problem &problem, double *states, double * alignment, int state_size,
					void *inputs, std::vector<double> &time_diffs, BatchEstimator *estimator)=0;
		};
		struct BatchConstrainFunctionWrapper: public BatchConstrain {
			void (*function)(ceres::Problem &problem, double *states, double * alignment, int state_size, void *inputs,
					std::vector<double> &time_diffs, BatchEstimator *estimator);
			BatchConstrainFunctionWrapper(
					void (*function)(ceres::Problem &problem, double *states, double * alignment, int state_size,
							void *inputs, std::vector<double> &time_diffs, BatchEstimator *estimator)) :
			function(function) {

			}
			virtual void operator()(ceres::Problem &problem, double *states, double * alignment, int state_size,
					void *inputs, std::vector<double> &time_diffs, BatchEstimator *estimator) {
				function(problem, states, alignment, state_size, inputs, time_diffs, estimator);
			}

		};
		/**
		 * add a constraint function
		 * @param constraint a function which will add a constraint to the ceres problem
		 * it gets access to all states and inputs to add constraints in any  manner
		 */
		void addConstraint(
				void (*constraint)(ceres::Problem &problem, double *states, double * alignment, int state_size, void *,
						std::vector<double> &time_diffs, BatchEstimator *estimator), void * data) {
			constraints.push_back(
					std::make_tuple(
							std::shared_ptr<BatchConstrainFunctionWrapper>(
									new BatchConstrainFunctionWrapper(constraint)), data));
		}
		/**
		 * add a constraint function
		 * @param constraint a function which will add a constraint to the ceres problem
		 * it gets access to all states and inputs to add constraints in any  manner
		 */
		void addConstraint(std::shared_ptr<BatchConstrain> constraint, void * data) {
			constraints.push_back(std::make_tuple(constraint, data));
		}

		/**
		 * Pointer to first state vector
		 * @return states
		 */
		double * getStates() {return states;}

		/**
		 * Number of states
		 * @return num_states
		 */
		size_t getNumStates() {return num_states;}

		/**
		 * Gives the options struct of the solver for editing and reading
		 * @return ceres solver options
		 */
		ceres::Solver::Options * getOptions() {
			return &options;
		}

	protected:
		/**
		 * Add constraints to the minimization problem
		 * @param problem the problem  to constraint
		 * @param states the states on which the constraints can be applied
		 */
		void addConstraints(ceres::Problem &problem, double * states, int state_size, BatchEstimator *estimator) {
			std::cout << "Adding " << constraints.size() << " constraints" << std::endl;
			for (auto constraint : constraints) {
				(*std::get<0>(constraint))(problem, states, alignment, state_size, std::get<1>(constraint), time_diffs,
						estimator);
			}
		}

		//std::shared_ptr<Gnuplot> plot;     //< plot
	public:
		//public so static function can write to them
		std::vector<INPUT_TYPE_T<double> > inputs;//< vector to store all inputs up till now
		std::vector<double> time_diffs;//<vector with time_diffs between measurements
		std::vector<char> input_mask;//!< saves whether the input was valid
	protected:
		std::vector<std::tuple<std::shared_ptr<BatchConstrain>, void *> > constraints;//< vector over constraint functions
		STATE_TYPE_T<double> state_vector;//< the current state vector (from last estimate)
		STATE_TYPE_T<double> lower_bound, upper_bound;// lower and upper bound
		MODEL_TYPE box_model;// model of the estimator
		const bool output_replay;// whether this batch estimator outputs its estimation sequently after it has finished
		volatile double * current_estimation;// the pointer to the data of the last estimation
		volatile int current_replay_index;// the index of the replay
		volatile int current_size;// the size of the current_estimation
		double * states;// pointer to the states
		double * alignment;// the alignment storage
		ALIGNMENT_PARAMETERIZATION align_parameter;
		double skip_time;// the time skipped for this batch
		size_t num_states;//!< number of states in states
		ceres::Solver::Options options;//!< options for the solver
	}
	;

}

#endif /* ESTIMATORS_BATCHESTIMATOR_HPP_ */


#include "graph_map/graph_map_navigator.h"
#include <boost/serialization/export.hpp>
BOOST_CLASS_EXPORT(perception_oru::libgraphMap::GraphMapNavigator)
namespace perception_oru{
  namespace libgraphMap{
  GraphMapNavigator::GraphMapNavigator(const Eigen::Affine3d &nodepose, const MapParamPtr &mapparam, const GraphParamPtr graphparam): GraphMap(nodepose, mapparam ,graphparam){}

  bool GraphMapNavigator::SwitchToClosestMapNode(Affine3d &Tnow, const Matrix6d &cov, Affine3d & T_world_to_local_map,const double radius){
    bool node_found=false;
    MapNodePtr closest_map_node=NULL;
    double closest_distance=-1.0;
    cout<<"currently at pose="<<Tnow.translation()<<endl;
    for(std::vector<MapNodePtr>::iterator itr_node = map_nodes_.begin(); itr_node != map_nodes_.end(); ++itr_node) { //loop thorugh all existing nodes
      if(radius==0.0 || (*itr_node)->WithinRadius(Tnow,radius) ){ //if a node is within radius of pose and the node is of type "map type"
        double found_distance= Eigen::Vector3d(Tnow.translation()-(*itr_node)->GetPose().translation()).norm();//Euclidian distance between robot pose and map node pose;
        cout<<"Node is within range of previously created map, distance="<<found_distance<<endl;
        if(closest_distance==-1.0|| found_distance<closest_distance){
          node_found=true;
          closest_map_node=*itr_node;
          closest_distance=found_distance;
          cout<<"closest node found at pose=\n"<<closest_map_node->GetPose().translation()<<endl;
        }
      }
    }
    if(node_found ){//if any node at all was found, switch to the closest and update Tnow & T_world_to_local_map (transformation to current node)
      prevNode_=currentNode_;
      currentNode_=closest_map_node; //switch to that node
      //cout<<"multiple factor disabled"<<endl;
      //AddFactor(prevNode_,currentNode_,prevNode_->GetPose().inverse()*currentNode_->GetPose(),cov);
    }
    return node_found;
  }
  bool GraphMapNavigator::SwitchToClosestMapNode(Affine3d &Tnow, bool &new_current){
    bool node_found=false;
    MapNodePtr closest_map_node=NULL;
    double closest_distance=0.0;
    for(std::vector<MapNodePtr>::iterator itr_node = map_nodes_.begin(); itr_node != map_nodes_.end(); ++itr_node) { //loop thorugh all existing nodes
      double found_distance= Eigen::Vector3d(Tnow.translation()-(*itr_node)->GetPose().translation()).norm();//Euclidian distance between robot pose and map node pose;
      if(closest_distance==0.0|| found_distance<closest_distance){
        node_found=true;
        closest_map_node=*itr_node;
        closest_distance=found_distance;
      }
    }
    if(node_found ){//if any node at all was found, switch to the closest and update Tnow & T_world_to_local_map (transformation to current node)
      prevNode_=currentNode_;
      currentNode_=closest_map_node; //switch to that node
      if(currentNode_!=prevNode_)
        new_current=true;
      else
        new_current=false;
    }
    return node_found;
  }

  bool GraphMapNavigator::SwitchToClosestMapNode(Affine3d &Tnow){

    double min_dist=DBL_MAX;
    MapNodePtr closest_map_node=NULL;
    for(std::vector<MapNodePtr>::iterator itr_node = map_nodes_.begin(); itr_node != map_nodes_.end(); ++itr_node) { //loop thorugh all existing nodes
      double distance= Eigen::Vector3d(Tnow.translation()-(*itr_node)->GetPose().translation()).norm();
      if(distance<min_dist){
        closest_map_node=*itr_node;
        min_dist=distance;
      }
    }
    if(closest_map_node!=NULL){
      if(closest_map_node!=currentNode_){
        prevNode_=currentNode_;
        currentNode_=closest_map_node;
        cout<<"Switched to node: "<<currentNode_->GetPose().translation().transpose()<<endl;
        return true;
      }
      else
        cout<<"No map transition"<<endl;
    }
    else return false;
  }

  bool GraphMapNavigator::AutomaticMapInterchange(Affine3d &Tnow, const Matrix6d &cov_incr, Affine3d & T_world_to_local_map,bool &changed_map_node,bool &created_map_node){

    created_map_node=false;
    changed_map_node=false;
    static Matrix6d pose_covar=unit_covar;
    if(use_submap_!=false){
      Tnow=T_world_to_local_map.inverse()*Tnow;//map Tnow to world frame
      if(! currentNode_->WithinRadius(Tnow,interchange_radius_)){ //No longer within radius of node
        cout<<"Left boundries of previous map, will  search through "<<map_nodes_.size()<<" node(s) to find a map node within range of"<<interchange_radius_<<"m"<<endl;
        if( changed_map_node=SwitchToClosestMapNode(Tnow,cov_incr,T_world_to_local_map,interchange_radius_)){
          cout<<"switched to node="<<currentNode_->GetPose().translation()<<endl;
          //Reset covariance
        }
        else{
          cout<<"No node was found, will create a new map pose."<<endl;
          prevNode_=currentNode_;
          //NDT2DMapPtr prev_ndt_map = boost::dynamic_pointer_cast< NDTMapType >(prevNode_->GetMap());
          AddMapNode(mapparam_,T_world_to_local_map*Tnow,cov_incr); //if no node already exists, create a new node
          prevNode_->GetMap()->CompoundMapsByRadius(currentNode_->GetMap(),prevNode_->GetPose(),currentNode_->GetPose(),compound_radius_);
          created_map_node=true;
        }
      }
      T_world_to_local_map=currentNode_->GetPose().inverse();
      Tnow=T_world_to_local_map*Tnow;//map Tnow from global to new local frame
    }
    return created_map_node || changed_map_node;
  }
  string GraphMapNavigator::ToString(){
    std::stringstream ss;
    ss <<"GraphMapNavigator:\n"<<GraphMap::ToString();
    return ss.str();
  }
  void LoadGraphMap(const std::string &file_name,  GraphMapNavigatorPtr ptr){

    std::ifstream ifs(file_name);
    boost::archive::text_iarchive ia(ifs);
    ia & ptr;
  }

  }
}


//          Copyright Oliver Kowalke 2014.
// 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 <cstdlib>
#include <iostream>
#include <memory>

#include <boost/context/all.hpp>

namespace ctx = boost::context;

int main() {
    int n=35;
    ctx::execution_context< int > source(
        [n](ctx::execution_context< int > && sink, int) mutable {
            int a=0;
            int b=1;
            while(n-->0){
                auto result=sink(a);
                sink=std::move(std::get<0>(result));
                auto next=a+b;
                a=b;
                b=next;
            }
            return std::move( sink);
        });
    for(int i=0;i<10;++i){
        auto result=source(i);
        source=std::move(std::get<0>(result));
        std::cout<<std::get<1>(result)<<" ";
    }
    std::cout<<std::endl;

    std::cout << "main: done" << std::endl;
}


// Copyright 2021 Anthony Paul Astolfi
//
#pragma once
#ifndef BATTERIES_RADIX_QUEUE_HPP
#define BATTERIES_RADIX_QUEUE_HPP

#include <batteries/assert.hpp>
#include <batteries/int_types.hpp>

#include <boost/functional/hash.hpp>

#include <array>
#include <limits>
#include <type_traits>

namespace batt {

template <usize kCapacityInBits>
class RadixQueue;

template <usize N_>
std::ostream& operator<<(std::ostream& out, const RadixQueue<N_>& t);

// A fixed-capacity FIFO queue of integers with variable radix per integer.  This is used to store sequences
// of events.
//
template <usize kCapacityInBits>
class RadixQueue
{
   public:
    static constexpr usize kQueueSize = kCapacityInBits / 64;

    //==#==========+==+=+=++=+++++++++++-+-+--+----- --- -- -  -  -   -

    using index_type = std::conditional_t<
        (kCapacityInBits <= std::numeric_limits<u16>::max()),
        std::conditional_t<(kCapacityInBits <= std::numeric_limits<u8>::max()), u8, u16>,
        std::conditional_t<(kCapacityInBits <= std::numeric_limits<u32>::max()), u32, u64>>;

    // The queue is stored in segments of 64 bits each.
    //
    struct Segment {
        u64 radix = 1;
        u64 value = 0;

        friend inline std::ostream& operator<<(std::ostream& out, const Segment& t)
        {
            return out << "{.value=" << t.value << ", .radix=" << t.radix << "}";
        }
    };

    // Default hash function.
    //
    struct Hash {
        using value_type = usize;

        usize operator()(const RadixQueue& r) const
        {
            usize seed = r.queue_size();
            for (usize i = 0; i < r.queue_size(); ++i) {
                usize j = (r.front_ + i) % kQueueSize;
                boost::hash_combine(seed, r.queue[j].radix);
                boost::hash_combine(seed, r.queue[j].value);
            }
            return seed;
        }
    };

    //==#==========+==+=+=++=+++++++++++-+-+--+----- --- -- -  -  -   -

    RadixQueue() = default;

    // Returns true when there are no items in the queue.
    //
    bool empty() const
    {
        return this->queue_size() == 1 && this->front().radix == 1;
    }

    // Returns true when the queue has reached its maximum capacity.
    //
    bool full() const
    {
        return this->front_ == (this->back_ + 1) % kQueueSize;
    }

    // Discards the contents of the queue, resetting it to default state.
    //
    void clear()
    {
        this->front_ = 0;
        this->back_ = 0;
        this->queue_[0] = Segment{};
    }

    // Insert the given value with the given radix at the back of the queue.
    //
    void push(u64 radix, u64 value)
    {
        BATT_CHECK_GT(radix, value) << "value must not exceed the supplied radix";

        const bool would_overflow = std::numeric_limits<u64>::max() / this->back().radix < radix;

        if (would_overflow) {
            this->push_back();
        }

        Segment& s = this->back();
        s.value += value * s.radix;
        s.radix *= radix;
    }

    // Extract the next value out of the queue.  The passed radix must match the radix used when inserting
    // the item originally; otherwise behavior is undefined.
    //
    u64 pop(u64 radix)
    {
        Segment& s = this->front();
        BATT_CHECK_LE(radix, s.radix) << "the supplied radix is too large";

        const u64 value = s.value % radix;
        s.radix /= radix;
        s.value /= radix;

        BATT_CHECK_LT(s.value, s.radix);

        if (s.radix == 1 && this->queue_size() > 1) {
            this->pop_front();
        }

        return value;
    }

    template <usize N_>
    friend std::ostream& operator<<(std::ostream& out, const RadixQueue<N_>& t);

   private:
    static void advance_index(index_type* i)
    {
        *i = (*i + 1) % kQueueSize;
    }

    usize queue_size() const
    {
        const usize upper_bound = [&]() -> usize {
            if (this->front_ <= this->back_) {
                return this->back_ + 1;
            }
            return kQueueSize + this->back_ + 1;
        }();
        BATT_CHECK_LT(this->front_, upper_bound);

        return upper_bound - this->front_;
    }

    Segment& front()
    {
        return this->queue_[this->front_];
    }
    const Segment& front() const
    {
        return this->queue_[this->front_];
    }

    Segment& back()
    {
        return this->queue_[this->back_];
    }
    const Segment& back() const
    {
        return this->queue_[this->back_];
    }

    void pop_front()
    {
        BATT_CHECK_NE(this->front_, this->back_) << "pull failed; the RadixQueue is empty";
        advance_index(&this->front_);
    }

    void push_back()
    {
        BATT_CHECK(!this->full()) << "push failed; the RadixQueue is full";
        advance_index(&this->back_);
        this->queue_[this->back_] = Segment{};
    }

    //==#==========+==+=+=++=+++++++++++-+-+--+----- --- -- -  -  -   -

    index_type front_ = 0;
    index_type back_ = 0;
    std::array<Segment, kQueueSize> queue_;
};

template <usize N_>
inline std::ostream& operator<<(std::ostream& out, const RadixQueue<N_>& t)
{
    usize end = (t.front_ <= t.back_) ? (t.back_ + 1) : (t.back_ + 1 + t.queue_.size());
    out << "{";
    for (usize i = t.front_; i < end; ++i) {
        const auto& s = t.queue_[i % t.queue_.size()];
        out << s.value << "/" << s.radix << ",";
    }
    return out << "}";
}

}  // namespace batt

#endif  // BATTERIES_RADIX_QUEUE_HPP