// This file is part of MLDB. Copyright 2015 mldb.ai inc. All rights reserved.

/* perceptron_test.cc
   Jeremy Barnes, 14 May 2010
   Copyright (c) 2010 Jeremy Barnes.  All rights reserved.

   Test of the GLZ classifier class.
*/

#define BOOST_TEST_MAIN
#define BOOST_TEST_DYN_LINK
#define MLDB_TESTING_PERCEPTRON

#include <boost/test/unit_test.hpp>
#include <thread>
#include "mldb/utils/thread_barrier.h"
#include <vector>
#include <stdint.h>
#include <iostream>

#include "mldb/plugins/jml/neural/perceptron_generator.h"
#include "mldb/plugins/jml/jml/training_data.h"
#include "mldb/plugins/jml/jml/dense_features.h"
#include "mldb/plugins/jml/jml/feature_info.h"
#include "mldb/plugins/jml/jml/training_index.h"
#include "mldb/utils/smart_ptr_utils.h"
#include "mldb/utils/vector_utils.h"
#include "mldb/arch/exception_handler.h"

using namespace ML;
using namespace std;

using boost::unit_test::test_suite;

static const char * config_options = "\
    type=perceptron\n\
    arch=1\n\
    # 5 hidden units\n                          \
    verbosity=3\n\
    max_iter=100\n\
    learning_rate=1\n\
    batch_size=10\n\
    verbosity=3\n\
    decorrelate=0\n\
    normalize=0\n\
";

int nfv = 1000;

BOOST_AUTO_TEST_CASE( test_perceptron_classification )
{
    /* Create the dataset */

    Dense_Feature_Space fs;
    fs.add_feature("LABEL", Feature_Info(BOOLEAN, false, true));
    fs.add_feature("feature1", REAL);
    fs.add_feature("feature2", REAL);

    std::shared_ptr<Dense_Feature_Space> fsp(make_unowned_sp(fs));

    Training_Data data(fsp);

    //float NaN = std::numeric_limits<float>::quiet_NaN();

    for (unsigned i = 0;  i < nfv;  ++i) {
        distribution<float> features;

        features.push_back(i % 3  == 0);
        features.push_back(i % 3  == 0);
        features.push_back(i % 5  == 0);

        std::shared_ptr<Feature_Set> fset
            = fs.encode(features);

        data.add_example(fset);
    }

    /* Create the decision tree generator */
    Configuration config;
    config.parse_string(config_options, "inbuilt config file");

    Perceptron_Generator generator;
    generator.configure(config);
    generator.init(fsp, fs.features()[0]);

    distribution<float> training_weights(nfv, 1);

    vector<Feature> features = fs.features();
    features.erase(features.begin(), features.begin() + 1);

    Thread_Context context;

    std::shared_ptr<Classifier_Impl> classifier
        = generator.generate(context, data, training_weights, features);

    float accuracy MLDB_UNUSED = classifier->accuracy(data).first;

    cerr << "accuracy = " << accuracy << endl;

    BOOST_CHECK_EQUAL(accuracy, 1);
}

BOOST_AUTO_TEST_CASE( test_perceptron_regression )
{
    /* Create the dataset */

    Dense_Feature_Space fs;
    fs.add_feature("LABEL", Feature_Info(REAL, false, true));
    fs.add_feature("feature1", REAL);
    fs.add_feature("feature2", REAL);

    std::shared_ptr<Dense_Feature_Space> fsp(make_unowned_sp(fs));

    Training_Data data(fsp);

    //float NaN = std::numeric_limits<float>::quiet_NaN();

    for (unsigned i = 0;  i < nfv;  ++i) {
        distribution<float> features;

        features.push_back(i % 3 + i % 5);
        features.push_back(i % 3  != 0);
        features.push_back(i % 5  != 0);

        std::shared_ptr<Feature_Set> fset
            = fs.encode(features);

        data.add_example(fset);
    }

    /* Create the decision tree generator */
    Configuration config;
    config.parse_string(config_options, "inbuilt config file");

    Perceptron_Generator generator;
    generator.configure(config);
    generator.init(fsp, fs.features()[0]);
    generator.output_activation = TF_IDENTITY;
    generator.activation = TF_IDENTITY;

    distribution<float> training_weights(nfv, 1);

    vector<Feature> features = fs.features();
    features.erase(features.begin(), features.begin() + 1);

    Thread_Context context;

    std::shared_ptr<Classifier_Impl> classifier
        = generator.generate(context, data, training_weights, features);

    float accuracy MLDB_UNUSED = classifier->accuracy(data).first;

    cerr << "accuracy = " << accuracy << endl;

    BOOST_CHECK_EQUAL(accuracy, 1);
}

#if 0

BOOST_AUTO_TEST_CASE( test_perceptron_missing )
{
    cerr << endl << endl << endl << "missing1" << endl;

    /* In this test, feature1a and feature1b contain the information about
       the label between them.  Exactly one of them is always present. */

    /* Create the dataset */

    Dense_Feature_Space fs;
    fs.add_feature("LABEL", Feature_Info(BOOLEAN, false, true));
    fs.add_feature("feature1a", REAL);
    fs.add_feature("feature1b", REAL);
    fs.add_feature("feature2",  REAL);

    std::shared_ptr<Dense_Feature_Space> fsp(make_unowned_sp(fs));

    Training_Data data(fsp);

    float NaN = std::numeric_limits<float>::quiet_NaN();

    for (unsigned i = 0;  i < nfv;  ++i) {
        distribution<float> features;

        features.push_back(i % 3  == 0);
        if (i % 2 == 0) {
            features.push_back(i % 3  == 0);
            features.push_back(NaN);
        }
        else {
            features.push_back(NaN);
            features.push_back(i % 3  == 0);
        }
        features.push_back(i % 5  == 0);

        std::shared_ptr<Feature_Set> fset
            = fs.encode(features);

        data.add_example(fset);
    }

    /* Create the decision tree generator */
    Configuration config;
    config.parse_string(config_options, "inbuilt config file");

    Perceptron_Generator generator;
    generator.configure(config);
    generator.init(fsp, fs.features()[0]);

    distribution<float> training_weights(nfv, 1);

    vector<Feature> features = fs.features();
    features.erase(features.begin(), features.begin() + 1);

    Thread_Context context;

    std::shared_ptr<Classifier_Impl> classifier
        = generator.generate(context, data, training_weights, features);

    float accuracy MLDB_UNUSED = classifier->accuracy(data).first;

    cerr << "accuracy = " << accuracy << endl;

    BOOST_CHECK_EQUAL(accuracy, 1);

    BOOST_CHECK_EQUAL(classifier->all_features(), features);

    // Check that we can optimize
    Optimization_Info info = classifier->optimize(fs.features());

    BOOST_CHECK(info);
}

BOOST_AUTO_TEST_CASE( test_perceptron_missing2 )
{
    cerr << endl << endl << endl << "missing2" << endl;

    /* In this test, feature1 contains the information about
       the label: if it's missing, the label is 1. */

    /* Create the dataset */

    Dense_Feature_Space fs;
    fs.add_feature("LABEL", Feature_Info(BOOLEAN, false, true));
    fs.add_feature("feature1", REAL);
    fs.add_feature("feature2",  REAL);

    std::shared_ptr<Dense_Feature_Space> fsp(make_unowned_sp(fs));

    Training_Data data(fsp);

    float NaN = std::numeric_limits<float>::quiet_NaN();

    for (unsigned i = 0;  i < nfv;  ++i) {
        distribution<float> features;

        features.push_back(i % 3  == 0);

        if (i % 3 == 0)
            features.push_back(0.0);
        else features.push_back(NaN);
        features.push_back(i % 5  == 0);

        std::shared_ptr<Feature_Set> fset
            = fs.encode(features);

        data.add_example(fset);
    }

    BOOST_CHECK(data.index().constant(Feature(1)));

    /* Create the decision tree generator */
    Configuration config;
    config.parse_string(config_options, "inbuilt config file");

    Perceptron_Generator generator;
    generator.configure(config);
    generator.init(fsp, fs.features()[0]);

    distribution<float> training_weights(nfv, 1);

    vector<Feature> features = fs.features();
    features.erase(features.begin(), features.begin() + 1);

    Thread_Context context;

    std::shared_ptr<Classifier_Impl> classifier
        = generator.generate(context, data, training_weights, features);

    float accuracy MLDB_UNUSED = classifier->accuracy(data).first;

    cerr << "accuracy = " << accuracy << endl;

    BOOST_CHECK_EQUAL(accuracy, 1);

    // Check that we can optimize
    Optimization_Info info = classifier->optimize(fs.features());

    BOOST_CHECK(info);
}

#endif

#if 0

#define do_decode(val, type)                           \
    classifier.decode_value(val, \
                            Perceptron::Feature_Spec(Feature(1),    \
                                                         Perceptron::Feature_Spec::type))

BOOST_AUTO_TEST_CASE( test_decode_value )
{
    Dense_Feature_Space fs;
    fs.add_feature("LABEL", Feature_Info(BOOLEAN, false, true));
    fs.add_feature("feature1", REAL);
    fs.add_feature("feature2",  REAL);

    Perceptron classifier(make_unowned_sp(fs), Feature(0));

    float NaN = std::numeric_limits<float>::quiet_NaN();
    float Inf = INFINITY;

    BOOST_CHECK_EQUAL(do_decode(0.0, VALUE), 0.0);
    BOOST_CHECK_EQUAL(do_decode(1.0, VALUE), 1.0);

    BOOST_CHECK_EQUAL(do_decode(0.0, VALUE_IF_PRESENT), 0.0);
    BOOST_CHECK_EQUAL(do_decode(1.0, VALUE_IF_PRESENT), 1.0);
    BOOST_CHECK_EQUAL(do_decode(NaN, VALUE_IF_PRESENT), 0.0);

    BOOST_CHECK_EQUAL(do_decode(0.0, PRESENCE), 1.0);
    BOOST_CHECK_EQUAL(do_decode(1.0, PRESENCE), 1.0);
    BOOST_CHECK_EQUAL(do_decode(Inf, PRESENCE), 1.0);
    BOOST_CHECK_EQUAL(do_decode(NaN, PRESENCE), 0.0);

    {
        MLDB_TRACE_EXCEPTIONS(false);
        BOOST_CHECK_THROW(do_decode(Inf, VALUE), Exception);
        BOOST_CHECK_THROW(do_decode(Inf, VALUE_IF_PRESENT), Exception);
        BOOST_CHECK_THROW(do_decode(NaN, VALUE), Exception);
    }
}

#endif


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

#ifndef cartesian_controller_hpp___
#define cartesian_controller_hpp___

#include <controller_interface/controller.h>
#include <hardware_interface/joint_command_interface.h>
#include <geometry_msgs/TwistStamped.h>

#include <boost/scoped_ptr.hpp>
#include <kdl/chain.hpp>
#include <kdl/tree.hpp>
#include <kdl/chainjnttojacsolver.hpp>
#include <kdl/chainfksolverpos_recursive.hpp>
#include <kdl/frames.hpp>
#include <kdl/jacobian.hpp>
#include <kdl/jntarray.hpp>
#include <kdl/chainiksolver.hpp>
#include <kdl/chainiksolvervel_pinv.hpp>
#include <kdl/chainfksolver.hpp>
#include <kdl/chainfksolverpos_recursive.hpp>

namespace velocity_controllers
{

class CartesianController : public controller_interface::Controller<hardware_interface::VelocityJointInterface>
{
public:
    bool init(hardware_interface::VelocityJointInterface* hw, ros::NodeHandle &nh);

    void update(const ros::Time& time, const ros::Duration& period);

    void starting(const ros::Time& time);
    void stopping(const ros::Time& time);

private:
    hardware_interface::JointHandle joint_;
    ros::Subscriber vel_cmd_sub_;

    KDL::Tree kdl_tree_;
    KDL::Chain kdl_chain_;
    std::vector<hardware_interface::JointHandle> joint_handles_;

    boost::scoped_ptr<KDL::ChainIkSolverVel> chain_ik_solver_vel_;
    boost::scoped_ptr<KDL::ChainFkSolverPos> chain_fk_solver_;

    KDL::Twist cmd_linear_twist_;
    KDL::Twist cmd_angular_twist_;

    std::string root_name_, tip_name_;

    bool got_msg_;

    ros::Time last_msg_;
    ros::Duration dead_man_timeout_;

    double joint_vel_limit_;

    void velCmdCB(const geometry_msgs::TwistStampedConstPtr& msg);
};

}

#endif // cartesian_controller_hpp___


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

/* Author: Ioan Sucan */

#define BOOST_TEST_MODULE "GeometricPlanningIK"
#include <boost/test/unit_test.hpp>
#include "ompl/util/DisableCompilerWarning.h"
OMPL_PUSH_DISABLE_CLANG_WARNING(-Wunused-function)
OMPL_PUSH_DISABLE_GCC_WARNING(-Wunused-function)
#include "2DmapSetup.h"
OMPL_POP_CLANG
#include <iostream>

#include "ompl/geometric/GeneticSearch.h"
#include "ompl/util/Time.h"

using namespace ompl;

BOOST_AUTO_TEST_CASE(SimpleIK)
{
    msg::setLogLevel(msg::LOG_ERROR);

    /* load environment */
    Environment2D env;
    boost::filesystem::path path(TEST_RESOURCES_DIR);
    path = path / "env1.txt";
    loadEnvironment(path.string().c_str(), env);

    if (env.width * env.height == 0)
    {
        BOOST_FAIL( "The environment has a 0 dimension. Cannot continue" );
    }

    /* instantiate space information */
    base::SpaceInformationPtr si = geometric::spaceInformation2DMap(env);

    /* set the goal state; the memory for this is automatically cleaned by SpaceInformation */
    base::GoalState goal(si);
    base::ScopedState<base::RealVectorStateSpace> gstate(si);
    gstate->values[0] = env.goal.first;
    gstate->values[1] = env.goal.second;
    goal.setState(gstate);
    goal.setThreshold(0.1);

    geometric::GeneticSearch gaik(si);
    gaik.setRange(5.0);
    base::ScopedState<base::RealVectorStateSpace> found(si);
    double time = 0.0;

    const int N = 100;
    for (int i = 0 ; i < N ; ++i)
    {
        ompl::time::point startTime = ompl::time::now();
        bool solved = gaik.solve(1.0, goal, found.get());
        ompl::time::duration elapsed = ompl::time::now() - startTime;
        time += ompl::time::seconds(elapsed);
        BOOST_CHECK(solved);
        BOOST_CHECK(si->distance(found.get(), gstate.get()) < goal.getThreshold());
    }
    time = time / (double)N;
    BOOST_CHECK(time < 0.01);
}


#include <algorithm>
#include <cfloat>
#include <chrono>
#include <climits>
#include <csignal>
#include <cstdlib>
#include <fstream>
#include <iomanip>
#include <iostream>
#include <sstream>
#include <string>
#include <utility>

#include <Math/Vector3D.h>
#include <TTree.h>

#include <Eigen/Eigen>

#include "core/config/ConfigReader.hpp"
#include "core/config/Configuration.hpp"
#include "core/config/exceptions.h"
#include "core/utils/log.h"
#include "core/utils/unit.h"
#include "tools/ROOT.h"
#include "tools/field_parser.h"
#include "tools/units.h"

#include "DFISEParser.hpp"
#include "MeshElement.hpp"
#include "ThreadPool.hpp"
#include "combinations/combinations.h"
#include "octree/Octree.hpp"

using namespace mesh_converter;
using namespace ROOT::Math;

void interrupt_handler(int);

/**
 * @brief Handle termination request (CTRL+C)
 */
void interrupt_handler(int) {
    LOG(STATUS) << "Interrupted! Aborting conversion...";
    allpix::Log::finish();
    std::exit(0);
}

int main(int argc, char** argv) {
    using XYZVectorInt = DisplacementVector3D<Cartesian3D<int>>;
    using XYVectorInt = DisplacementVector2D<Cartesian2D<int>>;
    using FileType = allpix::FileType;
    using FieldQuantity = allpix::FieldQuantity;

    // Register the default set of units with this executable:
    allpix::register_units();

    // If no arguments are provided, print the help:
    bool print_help = false;
    int return_code = 0;
    if(argc == 1) {
        print_help = true;
        return_code = 1;
    }

    // Add stream and set default logging level
    allpix::Log::addStream(std::cout);

    // Install abort handler (CTRL+\) and interrupt handler (CTRL+C)
    std::signal(SIGQUIT, interrupt_handler);
    std::signal(SIGINT, interrupt_handler);

    std::string file_prefix;
    std::string init_file_prefix;
    std::string log_file_name;

    std::string conf_file_name;
    allpix::LogLevel log_level = allpix::LogLevel::INFO;

    for(int i = 1; i < argc; i++) {
        if(strcmp(argv[i], "-h") == 0) {
            print_help = true;
        } else if(strcmp(argv[i], "-v") == 0 && (i + 1 < argc)) {
            try {
                log_level = allpix::Log::getLevelFromString(std::string(argv[++i]));
            } catch(std::invalid_argument& e) {
                LOG(ERROR) << "Invalid verbosity level \"" << std::string(argv[i]) << "\", ignoring overwrite";
                return_code = 1;
            }
        } else if(strcmp(argv[i], "-f") == 0 && (i + 1 < argc)) {
            file_prefix = std::string(argv[++i]);

            // Pre-fill config file name if not set yet:
            if(conf_file_name.empty()) {
                conf_file_name = file_prefix + ".conf";
            }
        } else if(strcmp(argv[i], "-c") == 0 && (i + 1 < argc)) {
            conf_file_name = std::string(argv[++i]);
        } else if(strcmp(argv[i], "-o") == 0 && (i + 1 < argc)) {
            init_file_prefix = std::string(argv[++i]);
        } else if(strcmp(argv[i], "-l") == 0 && (i + 1 < argc)) {
            log_file_name = std::string(argv[++i]);
        } else {
            LOG(ERROR) << "Unrecognized command line argument or missing value \"" << argv[i] << "\"";
            print_help = true;
            return_code = 1;
        }
    }

    // Set log level:
    allpix::Log::setReportingLevel(log_level);

    if(file_prefix.empty()) {
        print_help = true;
        return_code = 1;
    }

    if(init_file_prefix.empty()) {
        init_file_prefix = file_prefix;
        auto sep_idx = init_file_prefix.find_last_of('/');
        if(sep_idx != std::string::npos) {
            init_file_prefix = init_file_prefix.substr(sep_idx + 1);
        }
    }

    // Print help if requested or no arguments given
    if(print_help) {
        std::cerr << "Usage: mesh_converter -f <file_name> [<options>]" << std::endl;
        std::cout << "Required parameters:" << std::endl;
        std::cout << "\t -f <file_prefix>  common prefix of DF-ISE grid (.grd) and data (.dat) files" << std::endl;
        std::cout << "Optional parameters:" << std::endl;
        std::cout << "\t -c <config_file>  configuration file name" << std::endl;
        std::cout << "\t -h                display this help text" << std::endl;
        std::cout << "\t -l <file>         file to log to besides standard output (disabled by default)" << std::endl;
        std::cout
            << "\t -o <file_prefix>  output file prefix without .init extension (defaults to file name of <file_prefix>)"
            << std::endl;
        std::cout << "\t -v <level>        verbosity level (default reporiting level is INFO)" << std::endl;

        allpix::Log::finish();
        return return_code;
    }

    LOG(STATUS) << "Welcome to the Mesh Converter Tool of Allpix^2 " << ALLPIX_PROJECT_VERSION;
    LOG(STATUS) << "Using " << conf_file_name << " configuration file";
    std::ifstream file(conf_file_name);
    allpix::ConfigReader reader(file, conf_file_name);
    allpix::Configuration config = reader.getHeaderConfiguration();

    std::string format = config.get<std::string>("model", "apf");
    std::transform(format.begin(), format.end(), format.begin(), ::tolower);
    FileType file_type = (format == "init" ? FileType::INIT : format == "apf" ? FileType::APF : FileType::UNKNOWN);

    auto regions = config.getArray<std::string>("region", {"bulk"});
    auto observable = config.get<std::string>("observable", "ElectricField");

    const auto radius_step = config.get<double>("radius_step", 0.5);
    const auto max_radius = config.get<double>("max_radius", 10);

    const auto volume_cut = config.get<double>("volume_cut", 10e-9);

    XYZVectorInt divisions;
    const auto dimension = config.get<size_t>("dimension", 3);
    if(dimension == 2) {
        auto divisions_yz = config.get<XYVectorInt>("divisions", XYVectorInt(100, 100));
        divisions = XYZVectorInt(1, divisions_yz.x(), divisions_yz.y());
    } else {
        divisions = config.get<XYZVectorInt>("divisions", XYZVectorInt(100, 100, 100));
    }

    std::vector<std::string> rot = {"x", "y", "z"};
    if(config.has("xyz")) {
        rot = config.getArray<std::string>("xyz");
    }
    if(rot.size() != 3) {
        LOG(FATAL) << "Configuration keyword xyz must have 3 entries.";
        allpix::Log::finish();
        return 1;
    }

    const auto mesh_tree = config.get<bool>("mesh_tree", false);

    // NOTE: this stream should be available for the duration of the logging
    std::ofstream log_file;
    if(!log_file_name.empty()) {
        log_file.open(log_file_name, std::ios_base::out | std::ios_base::trunc);
        if(!log_file.good()) {
            LOG(FATAL) << "Cannot write to provided log file! Check if permissions are sufficient.";
            allpix::Log::finish();
            return 1;
        }
        allpix::Log::addStream(log_file);
    }

    auto start = std::chrono::system_clock::now();

    std::string grid_file = file_prefix + ".grd";
    LOG(STATUS) << "Reading mesh grid from grid file \"" << grid_file << "\"";

    std::vector<Point> points;
    try {
        auto region_grid = read_grid(grid_file, mesh_tree);
        LOG(INFO) << "Grid sizes for all regions:";
        for(auto& reg : region_grid) {
            LOG(INFO) << "\t" << std::left << std::setw(25) << reg.first << " " << reg.second.size();
        }

        // Append all grid regions to the mesh:
        for(const auto& region : regions) {
            if(region_grid.find(region) != region_grid.end()) {
                points.insert(points.end(), region_grid[region].begin(), region_grid[region].end());
            } else {
                LOG(ERROR) << "Region \"" << region << "\" not found in TCAD mesh";
            }
        }

        if(points.empty()) {
            throw std::runtime_error("Empty grid");
        }
        LOG(DEBUG) << "Grid with " << points.size() << " points";
    } catch(std::runtime_error& e) {
        LOG(FATAL) << "Failed to parse grid file \"" << grid_file << "\":\n" << e.what();
        allpix::Log::finish();
        return 1;
    }

    std::string data_file = file_prefix + ".dat";
    LOG(STATUS) << "Reading electric field from data file \"" << data_file << "\"";

    std::vector<Point> field;
    try {
        auto region_fields = read_electric_field(data_file);
        LOG(INFO) << "Field sizes for all regions and observables:";
        for(auto& reg : region_fields) {
            LOG(INFO) << " " << reg.first << ":";
            for(auto& fld : reg.second) {
                LOG(INFO) << "\t" << std::left << std::setw(25) << fld.first << " " << fld.second.size();
            }
        }

        // Append all field regions to the field vector:
        for(const auto& region : regions) {
            if(region_fields.find(region) != region_fields.end() &&
               region_fields[region].find(observable) != region_fields[region].end()) {
                field.insert(
                    field.end(), region_fields[region][observable].begin(), region_fields[region][observable].end());
            } else {
                LOG(ERROR) << "Region \"" << region << "\" with observable \"" << observable << "\" not found in TCAD field";
            }
        }

        if(field.empty()) {
            throw std::runtime_error("Empty observable data");
        }
        LOG(DEBUG) << "Field with " << field.size() << " points";
    } catch(std::runtime_error& e) {
        LOG(FATAL) << "Failed to parse data file \"" << data_file << "\":\n" << e.what();
        allpix::Log::finish();
        return 1;
    }

    if(points.size() != field.size()) {
        LOG(FATAL) << "Field and grid file do not match, found " << points.size() << " and " << field.size()
                   << " data points, respectively.";
        allpix::Log::finish();
        return 1;
    }

    auto points_temp = points;
    auto field_temp = field;
    if(rot.at(0) == "-y" || rot.at(0) == "y") {
        for(size_t i = 0; i < points.size(); ++i) {
            points_temp[i].x = points[i].y;
            field_temp[i].x = field[i].y;
        }
    }
    if(rot.at(0) == "-z" || rot.at(0) == "z") {
        for(size_t i = 0; i < points.size(); ++i) {
            points_temp[i].x = points[i].z;
            field_temp[i].x = field[i].z;
        }
    }
    if(rot.at(1) == "-x" || rot.at(1) == "x") {
        for(size_t i = 0; i < points.size(); ++i) {
            points_temp[i].y = points[i].x;
            field_temp[i].y = field[i].x;
        }
    }
    if(rot.at(1) == "-z" || rot.at(1) == "z") {
        for(size_t i = 0; i < points.size(); ++i) {
            points_temp[i].y = points[i].z;
            field_temp[i].y = field[i].z;
        }
    }
    if(rot.at(2) == "-x" || rot.at(2) == "x") {
        for(size_t i = 0; i < points.size(); ++i) {
            points_temp[i].z = points[i].x;
            field_temp[i].z = field[i].x;
        }
    }
    if(rot.at(2) == "-y" || rot.at(2) == "y") {
        for(size_t i = 0; i < points.size(); ++i) {
            points_temp[i].z = points[i].y;
            field_temp[i].z = field[i].y;
        }
    }
    points = points_temp;
    field = field_temp;

    // Find minimum and maximum from mesh coordinates
    double minx = DBL_MAX, miny = DBL_MAX, minz = DBL_MAX;
    double maxx = DBL_MIN, maxy = DBL_MIN, maxz = DBL_MIN;
    for(auto& point : points) {
        if(dimension == 2) {
            maxx = 1;
            minx = 0;
            maxy = std::max(maxy, point.y);
            maxz = std::max(maxz, point.z);
            miny = std::min(miny, point.y);
            minz = std::min(minz, point.z);
        }

        if(dimension == 3) {
            maxx = std::max(maxx, point.x);
            maxy = std::max(maxy, point.y);
            maxz = std::max(maxz, point.z);
            minx = std::min(minx, point.x);
            miny = std::min(miny, point.y);
            minz = std::min(minz, point.z);
        }
    }

    // Creating a new mesh points cloud with a regular pitch
    const double xstep = (maxx - minx) / static_cast<double>(divisions.x());
    const double ystep = (maxy - miny) / static_cast<double>(divisions.y());
    const double zstep = (maxz - minz) / static_cast<double>(divisions.z());
    const double cell_volume = xstep * ystep * zstep;

    // Using the minimal cell dimension as initial search radius for the point cloud:
    const auto initial_radius = config.get<double>("initial_radius", std::min({xstep, ystep, zstep}));
    LOG(INFO) << "Using initial neighbor search radius of " << initial_radius;

    if(rot.at(0) != "x" || rot.at(1) != "y" || rot.at(2) != "z") {
        LOG(STATUS) << "TCAD mesh (x,y,z) coords. transformation into: (" << rot.at(0) << "," << rot.at(1) << ","
                    << rot.at(2) << ")";
    }
    LOG(STATUS) << "Mesh dimensions: " << maxx - minx << " x " << maxy - miny << " x " << maxz - minz << std::endl
                << "New mesh element dimension: " << xstep << " x " << ystep << " x " << zstep
                << " ==>  Volume = " << cell_volume;

    if(rot.at(0).find('-') != std::string::npos) {
        LOG(WARNING) << "Inverting coordinate X. This might change the right-handness of the coordinate system!";
        for(size_t i = 0; i < points.size(); ++i) {
            points[i].x = maxx - (points[i].x - minx);
            field[i].x = -field[i].x;
        }
    }
    if(rot.at(1).find('-') != std::string::npos) {
        LOG(WARNING) << "Inverting coordinate Y. This might change the right-handness of the coordinate system!";
        for(size_t i = 0; i < points.size(); ++i) {
            points[i].y = maxy - (points[i].y - miny);
            field[i].y = -field[i].y;
        }
    }
    if(rot.at(2).find('-') != std::string::npos) {
        LOG(WARNING) << "Inverting coordinate Z. This might change the right-handness of the coordinate system!";
        for(size_t i = 0; i < points.size(); ++i) {
            points[i].z = maxz - (points[i].z - minz);
            field[i].z = -field[i].z;
        }
    }

    rot.at(0).erase(std::remove(rot.at(0).begin(), rot.at(0).end(), '-'), rot.at(0).end());
    rot.at(1).erase(std::remove(rot.at(1).begin(), rot.at(1).end(), '-'), rot.at(1).end());
    rot.at(2).erase(std::remove(rot.at(2).begin(), rot.at(2).end(), '-'), rot.at(2).end());

    auto end = std::chrono::system_clock::now();
    auto elapsed_seconds = std::chrono::duration_cast<std::chrono::seconds>(end - start).count();
    LOG(INFO) << "Reading the files took " << elapsed_seconds << " seconds.";

    // Initializing the Octree with points from mesh cloud.
    unibn::Octree<Point> octree;
    octree.initialize(points);

    auto mesh_section = [&](double x, double y) {
        allpix::Log::setReportingLevel(log_level);

        // New mesh slice
        std::vector<Point> new_mesh;

        double z = minz + zstep / 2.0;
        for(int k = 0; k < divisions.z(); ++k) {
            // New mesh vertex and field
            Point q(dimension == 2 ? -1 : x, y, z), e;
            bool valid = false;

            size_t prev_neighbours = 0;
            double radius = initial_radius;

            while(radius < max_radius) {
                LOG(DEBUG) << "Search radius: " << radius;
                // Calling octree neighbours search and sorting the results list with the closest neighbours first
                std::vector<unsigned int> results;
                octree.radiusNeighbors<unibn::L2Distance<Point>>(q, radius, results);
                LOG(DEBUG) << "Number of vertices found: " << results.size();

                // If after a radius step no new neighbours are found, go to the next radius step
                if(results.size() <= prev_neighbours || results.empty()) {
                    prev_neighbours = results.size();
                    LOG(DEBUG) << "No (new) neighbour found with radius " << radius << ". Increasing search radius.";
                    radius = radius + radius_step;
                    continue;
                }

                // If we have less than N close neighbors, no full mesh element can be formed. Increase radius.
                if(results.size() < (dimension == 3 ? 4 : 3)) {
                    LOG(DEBUG) << "Incomplete mesh element found for radius " << radius << ", increasing radius";
                    radius = radius + radius_step;
                    continue;
                }

                // Sort by lowest distance first, this drastically reduces the number of permutations required to find a
                // valid mesh element and also ensures that this is the one with the smallest volume.
                std::sort(results.begin(), results.end(), [&](unsigned int a, unsigned int b) {
                    return unibn::L2Distance<Point>::compute(points[a], q) < unibn::L2Distance<Point>::compute(points[b], q);
                });

                // Finding tetrahedrons by checking all combinations of N elements, starting with closest to reference point
                auto res = for_each_combination(results.begin(),
                                                results.begin() + (dimension == 3 ? 4 : 3),
                                                results.end(),
                                                Combination(&points, &field, q, volume_cut));
                valid = res.valid();
                if(valid) {
                    e = res.result();
                    break;
                }

                radius = radius + radius_step;
                LOG(DEBUG) << "All combinations tried. Increasing search radius to " << radius;
            }

            if(!valid) {
                throw std::runtime_error("Couldn't interpolate new mesh point, the grid might be too irregular");
            }

            new_mesh.push_back(e);
            z += zstep;
        }

        return new_mesh;
    };

    // Start the interpolation on many threads:
    auto num_threads = config.get<unsigned int>("workers", std::max(std::thread::hardware_concurrency(), 1u));
    LOG(STATUS) << "Starting regular grid interpolation with " << num_threads << " threads.";
    std::vector<Point> e_field_new_mesh;

    try {
        // clang-format off
        auto init_function = [log_level = allpix::Log::getReportingLevel(), log_format = allpix::Log::getFormat()]() {
            // clang-format on
            // Initialize the threads to the same log level and format as the master setting
            allpix::Log::setReportingLevel(log_level);
            allpix::Log::setFormat(log_format);
        };

        ThreadPool pool(num_threads, init_function);
        std::vector<std::future<std::vector<Point>>> mesh_futures;
        // Set starting point
        double x = minx + xstep / 2.0;
        // Loop over x coordinate, add tasks for each coorinate to the queue
        for(int i = 0; i < divisions.x(); ++i) {
            double y = miny + ystep / 2.0;
            for(int j = 0; j < divisions.y(); ++j) {
                mesh_futures.push_back(pool.submit(mesh_section, x, y));
                y += ystep;
            }
            x += xstep;
        }

        // Merge the result vectors:
        unsigned int mesh_slices_done = 0;
        for(auto& mesh_future : mesh_futures) {
            auto mesh_slice = mesh_future.get();
            e_field_new_mesh.insert(e_field_new_mesh.end(), mesh_slice.begin(), mesh_slice.end());
            LOG_PROGRESS(INFO, "m") << "Interpolating new mesh: " << mesh_slices_done << " of " << mesh_futures.size()
                                    << ", " << (100 * mesh_slices_done / mesh_futures.size()) << "%";
            mesh_slices_done++;
        }
        pool.destroy();
    } catch(std::runtime_error& e) {
        LOG(FATAL) << "Failed to interpolate new mesh:\n" << e.what();
        allpix::Log::finish();
        return 1;
    }

    end = std::chrono::system_clock::now();
    elapsed_seconds = std::chrono::duration_cast<std::chrono::seconds>(end - start).count();
    LOG(INFO) << "New mesh created in " << elapsed_seconds << " seconds.";

    // Prepare header and auxiliary information:
    std::string header =
        "Allpix Squared " + std::string(ALLPIX_PROJECT_VERSION) + " TCAD Mesh Converter, observable: " + observable;
    std::array<double, 3> size{{allpix::Units::get(maxx - minx, "um"),
                                allpix::Units::get(maxy - miny, "um"),
                                allpix::Units::get(maxz - minz, "um")}};
    std::array<size_t, 3> gridsize{
        {static_cast<size_t>(divisions.x()), static_cast<size_t>(divisions.y()), static_cast<size_t>(divisions.z())}};

    // FIXME this should be done in a more elegant way
    FieldQuantity quantity = (observable == "ElectricField" ? FieldQuantity::VECTOR : FieldQuantity::SCALAR);
    std::string units = (observable == "ElectricField" ? "V/cm" : "");

    // Prepare data:
    auto data = std::make_shared<std::vector<double>>();
    for(int i = 0; i < divisions.x(); ++i) {
        for(int j = 0; j < divisions.y(); ++j) {
            for(int k = 0; k < divisions.z(); ++k) {
                auto& point =
                    e_field_new_mesh[static_cast<unsigned int>(i * divisions.y() * divisions.z() + j * divisions.z() + k)];
                // We need to convert to framework-internal units:
                data->push_back(allpix::Units::get(point.x, units));
                // For a vector field, we push three values:
                if(quantity == FieldQuantity::VECTOR) {
                    data->push_back(allpix::Units::get(point.y, units));
                    data->push_back(allpix::Units::get(point.z, units));
                }
            }
        }
    }

    allpix::FieldData<double> field_data(header, gridsize, size, data);
    std::string init_file_name = init_file_prefix + "_" + observable + (file_type == FileType::INIT ? ".init" : ".apf");

    allpix::FieldWriter<double> field_writer(quantity);
    field_writer.write_file(field_data, init_file_name, file_type, (file_type == FileType::INIT ? units : ""));
    LOG(STATUS) << "New mesh written to file \"" << init_file_name << "\"";

    end = std::chrono::system_clock::now();
    elapsed_seconds = std::chrono::duration_cast<std::chrono::seconds>(end - start).count();
    LOG(STATUS) << "Interpolation and conversion completed in " << elapsed_seconds << " seconds.";

    allpix::Log::finish();
    return 1;
}


/**
 * @file
 * @copyright This code is licensed under the 3-clause BSD license.\n
 *            Copyright ETH Zurich, Laboratory for Physical Chemistry, Reiher Group.\n
 *            See LICENSE.txt for details.
 */

#include "Ediis.h"
#include "EdiisCoefficientOptimizer.h"
#include <Eigen/QR>
#include <iostream>

namespace Scine {
namespace Utils {

Ediis::Ediis() {
  setSubspaceSize(6);
}

void Ediis::setUnrestricted(bool b) {
  unrestricted_ = b;
}

void Ediis::setSubspaceSize(int n) {
  bool resizeNeeded = n != subspaceSize_;

  subspaceSize_ = n;

  if (resizeNeeded)
    resizeMembers();
}

void Ediis::setNAOs(int n) {
  bool resizeNeeded = n != nAOs_;

  nAOs_ = n;

  if (resizeNeeded)
    resizeMembers();
}

void Ediis::resizeMembers() {
  fockMatrices.resize(subspaceSize_);
  densityMatrices.resize(subspaceSize_);
  energies.resize(subspaceSize_);

  B = Eigen::MatrixXd::Zero(subspaceSize_, subspaceSize_);

  restart();
}

void Ediis::restart() {
  iterationNo_ = 0;
  index_ = 0;
}

void Ediis::addMatrices(double energy, const SpinAdaptedMatrix& F, const DensityMatrix& P) {
  iterationNo_++;
  lastAdded_ = index_;

  fockMatrices[index_] = F;
  densityMatrices[index_] = P;
  energies[index_] = energy;

  updateBMatrix();

  index_ = (index_ + 1) % subspaceSize_;
}

void Ediis::updateBMatrix() {
  int activeSize = iterationNo_ > subspaceSize_ ? subspaceSize_ : iterationNo_;

  // Bii element
  B(lastAdded_, lastAdded_) = 0;

  // Bij elements
  for (int i = 0; i < activeSize; i++) {
    if (i == lastAdded_)
      continue;
    double v = getBMatrixElement(lastAdded_, i);
    B(lastAdded_, i) = v;
    B(i, lastAdded_) = B(lastAdded_, i);
  }
}

double Ediis::getBMatrixElement(int i, int j) const {
  if (unrestricted_) {
    double va = ((fockMatrices[i].alphaMatrix() - fockMatrices[j].alphaMatrix()).selfadjointView<Eigen::Lower>() *
                 (densityMatrices[i].alphaMatrix() - densityMatrices[j].alphaMatrix()))
                    .trace();
    double vb = ((fockMatrices[i].betaMatrix() - fockMatrices[j].betaMatrix()).selfadjointView<Eigen::Lower>() *
                 (densityMatrices[i].betaMatrix() - densityMatrices[j].betaMatrix()))
                    .trace();
    return (va + vb) / 2;
  }
  else {
    double v = ((fockMatrices[i].restrictedMatrix() - fockMatrices[j].restrictedMatrix()).selfadjointView<Eigen::Lower>() *
                (densityMatrices[i].restrictedMatrix() - densityMatrices[j].restrictedMatrix()))
                   .trace();
    // Divide v by two because density matrix is RHF
    return v / 2;
  }
}

SpinAdaptedMatrix Ediis::getMixedFockMatrix() {
  if (iterationNo_ > subspaceSize_)
    iterationNo_ = subspaceSize_;

  // If we have only one Fock matrix
  if (iterationNo_ < 2) {
    return fockMatrices[0];
  }
  else {
    EdiisCoefficientOptimizer opt(energies, B.block(0, 0, iterationNo_, iterationNo_));
    auto coefs = opt.getCoefficients();

    return calculateLinearCombination(coefs);
  }
}

SpinAdaptedMatrix Ediis::calculateLinearCombination(const Eigen::VectorXd& coefs) {
  if (unrestricted_) {
    Eigen::MatrixXd FAlpha = Eigen::MatrixXd::Zero(nAOs_, nAOs_);
    Eigen::MatrixXd FBeta = Eigen::MatrixXd::Zero(nAOs_, nAOs_);
    for (int i = 0; i < iterationNo_; i++) {
      FAlpha += coefs[i] * fockMatrices[i].alphaMatrix();
      FBeta += coefs[i] * fockMatrices[i].betaMatrix();
    }
    return SpinAdaptedMatrix::createUnrestricted(std::move(FAlpha), std::move(FBeta));
  }
  else {
    Eigen::MatrixXd Fsol = Eigen::MatrixXd::Zero(nAOs_, nAOs_);
    for (int i = 0; i < iterationNo_; i++)
      Fsol += coefs[i] * fockMatrices[i].restrictedMatrix();
    return SpinAdaptedMatrix::createRestricted(std::move(Fsol));
  }
}
} // namespace Utils
} // namespace Scine


/*!@file
 * @copyright This code is licensed under the 3-clause BSD license.
 *   Copyright ETH Zurich, Laboratory of Physical Chemistry, Reiher Group.
 *   See LICENSE.txt for details.
 */

#include <boost/test/unit_test.hpp>

#include "Molassembler/GraphAlgorithms.h"
#include "Molassembler/IO/SmilesParser.h"
#include "Molassembler/IO.h"
#include "Molassembler/Molecule.h"
#include "Molassembler/Graph.h"
#include "Molassembler/Temple/Functional.h"
#include "Molassembler/Graph/GraphAlgorithms.h"
#include "Molassembler/Graph/EditDistance.h"

#include <iostream>
#include "Molassembler/Temple/Stringify.h"

using namespace Scine;
using namespace Molassembler;

BOOST_AUTO_TEST_CASE(PathsGeneration, *boost::unit_test::label("Molassembler")) {
  const std::string smiles = "C1CCCC1";
  const auto cyclopentane = IO::Experimental::parseSmilesSingleMolecule(smiles);

  BOOST_REQUIRE(
    Temple::all_of(
      std::vector<AtomIndex> {{0, 1, 2, 3, 4}},
      [&](const AtomIndex i) -> bool {
        return cyclopentane.graph().elementType(i) == Utils::ElementType::C;
      }
    )
  );

  const auto predecessors = shortestPaths(0, cyclopentane.graph());

  const auto toAdjacent = predecessors.path(1);
  BOOST_CHECK_EQUAL(toAdjacent.size(), 2);
  BOOST_CHECK_EQUAL(toAdjacent.front(), 0);
  BOOST_CHECK_EQUAL(toAdjacent.back(), 1);

  const auto toExtended = predecessors.path(2);
  BOOST_CHECK_EQUAL(toExtended.size(), 3);
  BOOST_CHECK_EQUAL(toExtended.at(0), 0);
  BOOST_CHECK_EQUAL(toExtended.at(1), 1);
  BOOST_CHECK_EQUAL(toExtended.at(2), 2);
}

BOOST_AUTO_TEST_CASE(UniqueDescendants, *boost::unit_test::label("Molassembler")) {
  const auto parse = &IO::Experimental::parseSmilesSingleMolecule;
  const auto cyclopentane = parse("C1CCCC1");
  const auto cyclohexane = parse("C1CCCCC1");

  const auto getCarbonNeighbors = [](const AtomIndex i, const Molecule& m) {
    std::vector<AtomIndex> vs;
    for(AtomIndex adjacent : m.graph().adjacents(i)) {
      if(m.graph().elementType(adjacent) == Utils::ElementType::C) {
        vs.push_back(adjacent);
      }
    }
    return vs;
  };

  const auto cyclopentaneNeighbors = getCarbonNeighbors(0, cyclopentane);
  const auto pentaneDescendants = GraphAlgorithms::bfsUniqueDescendants(
    0,
    cyclopentaneNeighbors,
    cyclopentane.graph().inner()
  );

  const auto count = [](const AtomIndex v, const std::vector<AtomIndex>& vs) {
    return std::count(std::begin(vs), std::end(vs), v);
  };

  BOOST_CHECK_EQUAL(count(0, pentaneDescendants), 3);
  BOOST_CHECK_EQUAL(count(cyclopentaneNeighbors.front(), pentaneDescendants), 6);
  BOOST_CHECK_EQUAL(count(cyclopentaneNeighbors.back(), pentaneDescendants), 6);

  const auto cyclohexaneNeighbors = getCarbonNeighbors(0, cyclohexane);
  const auto hexaneDescendants = GraphAlgorithms::bfsUniqueDescendants(
    0,
    cyclohexaneNeighbors,
    cyclohexane.graph().inner()
  );

  constexpr auto split = std::numeric_limits<AtomIndex>::max();
  BOOST_CHECK_EQUAL(count(0, hexaneDescendants), 3);
  BOOST_CHECK_EQUAL(count(split, hexaneDescendants), 3);
  BOOST_CHECK_EQUAL(count(cyclohexaneNeighbors.front(), hexaneDescendants), 6);
  BOOST_CHECK_EQUAL(count(cyclohexaneNeighbors.back(), hexaneDescendants), 6);
}

BOOST_AUTO_TEST_CASE(EditDistance, *boost::unit_test::label("Molassembler")) {
  auto distance = [&](const Molecule& a, const Molecule& b) -> unsigned {
    return minimalEdits(a.graph(), b.graph()).distance;
  };

  const auto parse = &IO::Experimental::parseSmilesSingleMolecule;
  auto methane = parse("C");
  auto methyl = parse("[CH3]");
  auto silane = parse("[SiH4]");
  BOOST_CHECK_EQUAL(distance(methane, methyl), 2);
  BOOST_CHECK_EQUAL(distance(silane, methane), 1);
  BOOST_CHECK_EQUAL(distance(silane, methyl), 3);

  auto propane = parse("CCC");
  auto propionic_acid = parse("CCC(=O)O");
  BOOST_CHECK_EQUAL(distance(propane, propionic_acid), 5);
  BOOST_CHECK_EQUAL(distance(propionic_acid, propane), 5);
  BOOST_CHECK_EQUAL(distance(propane, propane), 0);
  BOOST_CHECK_EQUAL(distance(propionic_acid, propionic_acid), 0);
}


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

// Copyright (c) 2007-2013 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2013 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2013 Mateusz Loskot, London, UK.
// Copyright (c) 2013 Adam Wulkiewicz, Lodz, Poland.
// Copyright (c) 2017 Artem Pavlenko, UK

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

#ifndef BOOST_GEOMETRY_STRATEGY_CARTESIAN_CLOSEST_POINT_HPP
#define BOOST_GEOMETRY_STRATEGY_CARTESIAN_CLOSEST_POINT_HPP

#include <boost/type_traits/remove_const.hpp>

#include <boost/geometry/core/access.hpp>
#include <boost/geometry/core/point_type.hpp>

#include <boost/geometry/algorithms/convert.hpp>
#include <boost/geometry/arithmetic/arithmetic.hpp>
#include <boost/geometry/arithmetic/dot_product.hpp>

#include <boost/geometry/strategies/tags.hpp>
#include <boost/geometry/strategies/distance.hpp>
#include <boost/geometry/strategies/default_distance_result.hpp>
#include <boost/geometry/strategies/cartesian/distance_pythagoras.hpp>

#include <boost/geometry/util/select_coordinate_type.hpp>

#include <boost/geometry/algorithms/detail/overlay/segment_identifier.hpp>

namespace boost { namespace geometry
{

template <typename Point>
struct closest_point_result
{
    typedef Point point_type;
    typedef typename default_distance_result<Point>::type distance_type;
    distance_type distance;
    Point closest_point;

    inline closest_point_result()
        : distance(distance_type())
    {}
};

namespace strategy { namespace distance
{

template
<
    typename CalculationType = void,
    typename Strategy = pythagoras<CalculationType>
>
struct calculate_closest_point
{
public :
    // The three typedefs below are necessary to calculate distances
    // from segments defined in integer coordinates.

    // Integer coordinates can still result in FP distances.
    // There is a division, which must be represented in FP.
    // So promote.
    template <typename Point, typename PointOfSegment>
    struct calculation_type
        : promote_floating_point
          <
              typename strategy::distance::services::return_type
                  <
                      Strategy,
                      Point,
                      PointOfSegment
                  >::type
          >
    {};

public :

    // Helper function
    template <typename Point1, typename Point2>
    inline typename calculation_type<Point1, Point2>::type
    apply_point_point(Point1 const& p1, Point2 const& p2) const
    {
        Strategy point_point_strategy;
        boost::ignore_unused_variable_warning(point_point_strategy);
        return point_point_strategy.apply(p1, p2);
    }

    template <typename Point, typename PointOfSegment, typename Result>
    inline void apply(Point const& p,
                    PointOfSegment const& p1, PointOfSegment const& p2,
                    Result& result) const
    {
        assert_dimension_equal<Point, PointOfSegment>();

        typedef typename calculation_type<Point, PointOfSegment>::type calculation_type;

        //// A projected point of points in Integer coordinates must be able to be
        //// represented in FP.
        typedef model::point
            <
                calculation_type,
                dimension<PointOfSegment>::value,
                typename coordinate_system<PointOfSegment>::type
            > fp_point_type;

        // For convenience
        typedef fp_point_type fp_vector_type;
        fp_vector_type v, w;

        geometry::convert(p2, v);
        geometry::convert(p, w);
        subtract_point(v, p1);
        subtract_point(w, p1);

        calculation_type const zero = calculation_type();

        calculation_type const c1 = dot_product(w, v);
        calculation_type const c2 = dot_product(v, v);

        if (c1 < zero)
        {
            result.distance = apply_point_point(p, p1);
            result.closest_point = p1;
        }
        else if(c1 > c2)
        {
            result.distance = apply_point_point(p, p2);
            result.closest_point = p2;
        }
        else
        {
            Strategy point_point_strategy;
            boost::ignore_unused_variable_warning(point_point_strategy);

            if (geometry::math::equals(c2, zero))
            {
                geometry::convert(p1, result.closest_point);
                result.distance = apply_point_point(p, p1);
                return;
            }

            calculation_type const b = c1 / c2;
            geometry::convert(p1, result.closest_point);
            multiply_value(v, b);
            add_point(result.closest_point, v);
            result.distance = apply_point_point(p, result.closest_point);
        }
    }
};

}} // namespace strategy::distance

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGY_CARTESIAN_CLOSEST_POINT_HPP


#include "nanocv/nanocv.h"
#include "nanocv/tester.h"
#include "nanocv/sampler.h"
#include "nanocv/measure.hpp"
#include <boost/program_options.hpp>
#include <algorithm>

int main(int argc, char *argv[])
{
        ncv::init();

        using namespace ncv;

        // prepare object string-based selection
        const strings_t task_ids = ncv::get_tasks().ids();
        const strings_t loss_ids = ncv::get_losses().ids();
        const strings_t model_ids = ncv::get_models().ids();

        // parse the command line
        boost::program_options::options_description po_desc("", 160);
        po_desc.add_options()("help,h", "help message");
        po_desc.add_options()("task",
                boost::program_options::value<string_t>(),
                text::concatenate(task_ids, ", ").c_str());
        po_desc.add_options()("task-dir",
                boost::program_options::value<string_t>(),
                "directory to load task data from");
        po_desc.add_options()("task-params",
                boost::program_options::value<string_t>()->default_value(""),
                "task parameters (if any)");
        po_desc.add_options()("loss",
                boost::program_options::value<string_t>(),
                text::concatenate(loss_ids, ", ").c_str());
        po_desc.add_options()("model",
                boost::program_options::value<string_t>(),
                text::concatenate(model_ids, ", ").c_str());
        po_desc.add_options()("model-file",
                boost::program_options::value<string_t>(),
                "filepath to load the model from");
        po_desc.add_options()("save-dir",
                boost::program_options::value<string_t>(),
                "directory to save classification results to");
        po_desc.add_options()("save-group-rows",
                boost::program_options::value<size_t>()->default_value(32),
                "number of samples to group in a row");
        po_desc.add_options()("save-group-cols",
                boost::program_options::value<size_t>()->default_value(32),
                "number of samples to group in a column");

        boost::program_options::variables_map po_vm;
        boost::program_options::store(
                boost::program_options::command_line_parser(argc, argv).options(po_desc).run(),
                po_vm);
        boost::program_options::notify(po_vm);

        // check arguments and options
        if (	po_vm.empty() ||
                !po_vm.count("task") ||
                !po_vm.count("task-dir") ||
                !po_vm.count("loss") ||
                !po_vm.count("model") ||
                !po_vm.count("model-file") ||
                po_vm.count("help"))
        {
                std::cout << po_desc;
                return EXIT_FAILURE;
        }

        const string_t cmd_task = po_vm["task"].as<string_t>();
        const string_t cmd_task_dir = po_vm["task-dir"].as<string_t>();
        const string_t cmd_task_params = po_vm["task-params"].as<string_t>();
        const string_t cmd_loss = po_vm["loss"].as<string_t>();
        const string_t cmd_model = po_vm["model"].as<string_t>();
        const string_t cmd_input = po_vm["model-file"].as<string_t>();
        const string_t cmd_save_dir = po_vm.count("save-dir") ? po_vm["save-dir"].as<string_t>() : "";
        const size_t cmd_save_group_rows = math::clamp(po_vm["save-group-rows"].as<size_t>(), 1, 128);
        const size_t cmd_save_group_cols = math::clamp(po_vm["save-group-cols"].as<size_t>(), 1, 128);

        // create task
        const rtask_t rtask = ncv::get_tasks().get(cmd_task, cmd_task_params);

        // load task data
        ncv::measure_critical_and_log(
                [&] () { return rtask->load(cmd_task_dir); },
                "loaded task",
                "failed to load task <" + cmd_task + "> from directory <" + cmd_task_dir + ">");

        // describe task
        rtask->describe();

        // create loss
        const rloss_t rloss = ncv::get_losses().get(cmd_loss);

        // create model
        const rmodel_t rmodel = ncv::get_models().get(cmd_model);

        // load model
        ncv::measure_critical_and_log(
                [&] () { return rmodel->load(cmd_input); },
                "loaded model",
                "failed to load model from <" + cmd_input + ">");

        // test model
        stats_t<scalar_t> lstats, estats;
        for (size_t f = 0; f < rtask->fsize(); f ++)
        {
                const fold_t test_fold = std::make_pair(f, protocol::test);

		// error rate
                const ncv::timer_t timer;
                scalar_t lvalue, lerror;
                ncv::test(*rtask, test_fold, *rloss, *rmodel, lvalue, lerror);
                log_info() << "<<< test error: [" << lvalue << "/" << lerror << "] in " << timer.elapsed() << ".";

                lstats(lvalue);
                estats(lerror);

		// per-label error rates
		sampler_t sampler(*rtask);
                sampler.setup(test_fold);
                sampler.setup(sampler_t::atype::annotated);
                sampler.setup(sampler_t::stype::batch);

                const samples_t samples = sampler.get();

                // split test samples into correctly classified & miss-classified
                samples_t ok_samples;
                samples_t nk_samples;

                for (size_t s = 0; s < samples.size(); s ++)
                {
                        const sample_t& sample = samples[s];
                        const image_t& image = rtask->image(sample.m_index);

                        const vector_t target = sample.m_target;
                        const vector_t output = rmodel->output(image, sample.m_region).vector();

                        const indices_t tclasses = rloss->labels(target);
                        const indices_t oclasses = rloss->labels(output);

                        const bool ok = tclasses.size() == oclasses.size() &&
                                        std::mismatch(tclasses.begin(), tclasses.end(),
                                                      oclasses.begin()).first == tclasses.end();

                        (ok ? ok_samples : nk_samples).push_back(sample);
                }

                log_info() << "miss-classified " << nk_samples.size() << "/" << (samples.size())
                           << " = " << ((0.0 + nk_samples.size()) / (0.0 + samples.size())) << ".";

                // save classification results
                if (!cmd_save_dir.empty())
                {
                        const string_t basepath = cmd_save_dir + "/" + cmd_task + "_test_fold" + text::to_string(f + 1);

                        const size_t grows = cmd_save_group_rows;
                        const size_t gcols = cmd_save_group_cols;

                        const rgba_t ok_bkcolor = color::make_rgba(0, 225, 0);
                        const rgba_t nk_bkcolor = color::make_rgba(225, 0, 0);

                        // further split them by label
                        const strings_t labels = rtask->labels();
                        for (const string_t& label : labels)
                        {
                                const string_t lbasepath = basepath + "_" + label;

                                sampler_t ok_sampler(ok_samples); ok_sampler.setup(label);
                                sampler_t nk_sampler(nk_samples); nk_sampler.setup(label);
                                sampler_t ll_sampler(samples); ll_sampler.setup(label);

                                const samples_t ok_samples = ok_sampler.get();
                                const samples_t nk_samples = nk_sampler.get();
                                const samples_t ll_samples = ll_sampler.get();

                                log_info() << "miss-classified " << nk_samples.size()
                                           << "/" << ll_samples.size()
                                           << " = " << ((0.0 + nk_samples.size()) / (0.0 + ll_samples.size()))
                                           << " [" << label << "] samples.";

                                rtask->save_as_images(ok_samples, lbasepath + "_ok", grows, gcols, 8, ok_bkcolor);
                                rtask->save_as_images(nk_samples, lbasepath + "_nk", grows, gcols, 8, nk_bkcolor);
                        }
                }
        }

        // performance statistics
        log_info() << ">>> performance: loss value = " << lstats.avg() << " +/- " << lstats.stdev()
                   << " in [" << lstats.min() << ", " << lstats.max() << "].";
        log_info() << ">>> performance: loss error = " << estats.avg() << " +/- " << estats.stdev()
                   << " in [" << estats.min() << ", " << estats.max() << "].";

        // OK
        log_info() << done;
        return EXIT_SUCCESS;
}


// g++ main.cpp multibody.cpp -larmadillo -o multibody
#include <regex>
#include <iostream>
#include <armadillo>
#include <sstream>
#include <vector>
#include "multibody.h"

int main(int argc, char** argv)
{
    double eps = 1.0;
    double dim = 3.0;
    double a = 1.0;
    std::regex re("^\\w*\\n*$"); //find an empty line with a new line at end
    /*
    read in file format: N dim eps a {an N x N adjacency matrix as list of numbers, no brackets}
    */
    for (std::string line; std::getline(std::cin, line); ) { //process lines
        if(std::regex_match(line, re)) { //on blank line, exit
            return 0;
        }
        arma::mat adjacency = readMatrix(line, &dim, &eps, &a);
        int N = adjacency.n_cols;
        arma::mat delta = eps*arma::eye(N, N); //This anchors all monomers
        if(eps == 0.0){	//Anchor only 1 monomer.  This has the effect of no confining potential
        	delta = 0*delta;
        	delta(0, 0) = 1.0; //This only anchors one monomer
        }
        arma::mat lap = calcLaplacian(adjacency) + delta;
        GaussSystem chain = GaussSystem(lap, dim, a);
        double res = chain.alpham1();
        double smallparam = chain.isitz();
        std::cout << chain.rg20 << " " << res << " " << smallparam << " " << std::endl; //chain.thirdcorrection() << std::endl;
        std::cerr << chain.rg20 << " " << res << " " << smallparam << " " << std::endl;
        //std::cerr << "Third Virial " << std::pow(chain.secondcorrection(), 2) - 2.0*chain.thirdcorrection() << std::endl;
        return 0; //Only run once
    }
    return 0;
}


#pragma once

#include <Eigen/Dense>

#include <init_traj_planner.hpp>
#include <mission.hpp>
#include <param.hpp>
#include <timer.hpp>

namespace SwarmPlanning {
    class Corridor {
    public:
        Corridor(std::shared_ptr<DynamicEDTOctomap> _distmap_obj,
                 Mission _mission,
                 Param _param)
                : distmap_obj(std::move(_distmap_obj)),
                  mission(std::move(_mission)),
                  param(std::move(_param)) {
        }

        bool update(bool _log, SwarmPlanning::PlanResult* _planResult_ptr) {
            log = _log;
            planResult_ptr = _planResult_ptr;
            makespan = planResult_ptr->T.back();
            return updateObsBox() && updateRelBox();
        }

        bool update_flat_box(bool _log, SwarmPlanning::PlanResult* _planResult_ptr) {
            log = _log;
            planResult_ptr = _planResult_ptr;
            makespan = planResult_ptr->T.size() - 1;
            return updateFlatObsBox() && updateFlatRelBox() && updateTs();
        }

    private:
        std::shared_ptr<DynamicEDTOctomap> distmap_obj;
        Mission mission;
        Param param;

        bool log;
        SwarmPlanning::PlanResult* planResult_ptr;
        double makespan;

        bool isObstacleInBox(const std::vector<double> &box, double margin) {
            double x, y, z;
            int count1 = 0;
            for (double i = box[0]; i < box[3] + SP_EPSILON_FLOAT; i += param.box_xy_res) {
                int count2 = 0;
                for (double j = box[1]; j < box[4] + SP_EPSILON_FLOAT; j += param.box_xy_res) {
                    int count3 = 0;
                    for (double k = box[2]; k < box[5] + SP_EPSILON_FLOAT; k += param.box_z_res) {
                        x = i + SP_EPSILON_FLOAT;
                        if (count1 == 0 && box[0] > param.world_x_min + SP_EPSILON_FLOAT) {
                            x = box[0] - SP_EPSILON_FLOAT;
                        }
                        y = j + SP_EPSILON_FLOAT;
                        if (count2 == 0 && box[1] > param.world_y_min + SP_EPSILON_FLOAT) {
                            y = box[1] - SP_EPSILON_FLOAT;
                        }
                        z = k + SP_EPSILON_FLOAT;
                        if (count3 == 0 && box[2] > param.world_z_min + SP_EPSILON_FLOAT) {
                            z = box[2] - SP_EPSILON_FLOAT;
                        }

                        octomap::point3d cur_point(x, y, z);
                        float dist = distmap_obj.get()->getDistance(cur_point);
                        if (dist < margin - SP_EPSILON_FLOAT) {
                            return true;
                        }
                        count3++;
                    }
                    count2++;
                }
                count1++;
            }

            return false;
        }

        bool isBoxInBoundary(const std::vector<double> &box) {
            return box[0] > param.world_x_min - SP_EPSILON &&
                   box[1] > param.world_y_min - SP_EPSILON &&
                   box[2] > param.world_z_min - SP_EPSILON &&
                   box[3] < param.world_x_max + SP_EPSILON &&
                   box[4] < param.world_y_max + SP_EPSILON &&
                   box[5] < param.world_z_max + SP_EPSILON;
        }

        bool isPointInBox(const octomap::point3d &point,
                          const std::vector<double> &box) {
            return point.x() > box[0] - SP_EPSILON &&
                   point.y() > box[1] - SP_EPSILON &&
                   point.z() > box[2] - SP_EPSILON &&
                   point.x() < box[3] + SP_EPSILON &&
                   point.y() < box[4] + SP_EPSILON &&
                   point.z() < box[5] + SP_EPSILON;
        }

        void expand_box(std::vector<double> &box, double margin) {
            std::vector<double> box_cand, box_update;
            std::vector<int> axis_cand{0, 1, 2, 3, 4, 5};

            int i = -1;
            int axis;
            while (!axis_cand.empty()) {
                box_cand = box;
                box_update = box;

                //check update_box only! update_box + current_box = cand_box
                while (!isObstacleInBox(box_update, margin) && isBoxInBoundary(box_update)) {
                    i++;
                    if (i >= axis_cand.size()) {
                        i = 0;
                    }
                    axis = axis_cand[i];

                    //update current box
                    box = box_cand;
                    box_update = box_cand;

                    //expand cand_box and get updated part of box(update_box)
                    if (axis < 3) {
                        box_update[axis + 3] = box_cand[axis];
                        if (axis == 2) {
                            box_cand[axis] = box_cand[axis] - param.box_z_res;
                        } else {
                            box_cand[axis] = box_cand[axis] - param.box_xy_res;
                        }
                        box_update[axis] = box_cand[axis];
                    } else {
                        box_update[axis - 3] = box_cand[axis];
                        if (axis == 5) {
                            box_cand[axis] = box_cand[axis] + param.box_z_res;
                        } else {
                            box_cand[axis] = box_cand[axis] + param.box_xy_res;
                        }
                        box_update[axis] = box_cand[axis];
                    }
                }
                axis_cand.erase(axis_cand.begin() + i);
                if (i > 0) {
                    i--;
                } else {
                    i = axis_cand.size() - 1;
                }
            }
        }

        bool updateObsBox() {
            double x_next, y_next, z_next, dx, dy, dz;
            Timer timer;

            planResult_ptr->SFC.resize(mission.qn);
            for (size_t qi = 0; qi < mission.qn; ++qi) {
                std::vector<double> box_prev{0, 0, 0, 0, 0, 0};

                for (int i = 0; i < planResult_ptr->initTraj[qi].size() - 1; i++) {
                    auto state = planResult_ptr->initTraj[qi][i];
                    double x = state.x();
                    double y = state.y();
                    double z = state.z();

                    std::vector<double> box;
                    auto state_next = planResult_ptr->initTraj[qi][i + 1];
                    x_next = state_next.x();
                    y_next = state_next.y();
                    z_next = state_next.z();

                    if (isPointInBox(octomap::point3d(x_next, y_next, z_next), box_prev)) {
                        continue;
                    }

                    // Initialize box
                    box.emplace_back(round(std::min(x, x_next) / param.box_xy_res) * param.box_xy_res);
                    box.emplace_back(round(std::min(y, y_next) / param.box_xy_res) * param.box_xy_res);
                    box.emplace_back(round(std::min(z, z_next) / param.box_z_res) * param.box_z_res);
                    box.emplace_back(round(std::max(x, x_next) / param.box_xy_res) * param.box_xy_res);
                    box.emplace_back(round(std::max(y, y_next) / param.box_xy_res) * param.box_xy_res);
                    box.emplace_back(round(std::max(z, z_next) / param.box_z_res) * param.box_z_res);

                    if (isObstacleInBox(box, mission.quad_size[qi])) {
                        ROS_ERROR("Corridor: Invalid initial trajectory. Obstacle invades initial trajectory.");
                        ROS_ERROR_STREAM("Corridor: x " << x << ", y " << y << ", z " << z);

                        bool debug =isObstacleInBox(box, mission.quad_size[qi]);
                        return false;
                    }
                    expand_box(box, mission.quad_size[qi]);

                    planResult_ptr->SFC[qi].emplace_back(std::make_pair(box, -1));

                    box_prev = box;
                }

                // Generate box time segment
                int box_max = planResult_ptr->SFC[qi].size();
                int path_max = planResult_ptr->initTraj[qi].size();
                Eigen::MatrixXd box_log = Eigen::MatrixXd::Zero(box_max, path_max);

                for (int i = 0; i < box_max; i++) {
                    for (int j = 0; j < path_max; j++) {
                        if (isPointInBox(planResult_ptr->initTraj[qi][j], planResult_ptr->SFC[qi][i].first)) {
                            if (j == 0) {
                                box_log(i, j) = 1;
                            } else {
                                box_log(i, j) = box_log(i, j - 1) + 1;
                            }
                        }
                    }
                }

                int box_iter = 0;
                for (int path_iter = 0; path_iter < path_max; path_iter++) {
                    if (box_iter == box_max - 1) {
                        if (box_log(box_iter, path_iter) > 0) {
                            continue;
                        } else {
                            box_iter--;
                        }
                    }
                    if (box_log(box_iter, path_iter) > 0 && box_log(box_iter + 1, path_iter) > 0) {
                        int count = 1;
                        while (path_iter + count < path_max && box_log(box_iter, path_iter + count) > 0
                               && box_log(box_iter + 1, path_iter + count) > 0) {
                            count++;
                        }
                        int obs_index = path_iter + count / 2;
                        planResult_ptr->SFC[qi][box_iter].second = planResult_ptr->T[obs_index];

                        path_iter = path_iter + count / 2;
                        box_iter++;
                    } else if (box_log(box_iter, path_iter) == 0) {
                        box_iter--;
                        path_iter--;
                    }
                }
                planResult_ptr->SFC[qi][box_max - 1].second = makespan;
            }

            timer.stop();
            ROS_INFO_STREAM("Corridor: SFC runtime=" << timer.elapsedSeconds());
            return true;
        }

        bool updateObsBox_seperate() {
            double x_next, y_next, z_next, dx, dy, dz;
            Timer timer;

            planResult_ptr->SFC.resize(mission.qn);
            for (size_t qi = 0; qi < mission.qn; ++qi) {
                std::vector<double> box_prev{0, 0, 0, 0, 0, 0};

                for (int i = 0; i < planResult_ptr->initTraj[qi].size() - 1; i++) {
                    auto state = planResult_ptr->initTraj[qi][i];
                    double x = state.x();
                    double y = state.y();
                    double z = state.z();

                    std::vector<double> box;
                    auto state_next = planResult_ptr->initTraj[qi][i + 1];
                    x_next = state_next.x();
                    y_next = state_next.y();
                    z_next = state_next.z();

                    if (isPointInBox(octomap::point3d(x_next, y_next, z_next), box_prev)) {
                        continue;
                    }

                    // Initialize box
                    box.emplace_back(round(std::min(x, x_next) / param.box_xy_res) * param.box_xy_res);
                    box.emplace_back(round(std::min(y, y_next) / param.box_xy_res) * param.box_xy_res);
                    box.emplace_back(round(std::min(z, z_next) / param.box_z_res) * param.box_z_res);
                    box.emplace_back(round(std::max(x, x_next) / param.box_xy_res) * param.box_xy_res);
                    box.emplace_back(round(std::max(y, y_next) / param.box_xy_res) * param.box_xy_res);
                    box.emplace_back(round(std::max(z, z_next) / param.box_z_res) * param.box_z_res);

                    if (isObstacleInBox(box, mission.quad_size[qi])) {
                        ROS_ERROR("Corridor: Invalid initial trajectory. Obstacle invades initial trajectory.");
                        return false;
                    }
                    expand_box(box, mission.quad_size[qi]);

                    planResult_ptr->SFC[qi].emplace_back(std::make_pair(box, -1));

                    box_prev = box;
                }

                // Generate box time segment
                int box_max = planResult_ptr->SFC[qi].size();
                int path_max = planResult_ptr->initTraj[qi].size();
                Eigen::MatrixXd box_log = Eigen::MatrixXd::Zero(box_max, path_max);

                for (int i = 0; i < box_max; i++) {
                    for (int j = 0; j < path_max; j++) {
                        if (isPointInBox(planResult_ptr->initTraj[qi][j], planResult_ptr->SFC[qi][i].first)) {
                            if (j == 0) {
                                box_log(i, j) = 1;
                            } else {
                                box_log(i, j) = box_log(i, j - 1) + 1;
                            }
                        }
                    }
                }

                int box_iter = 0;
                for (int path_iter = 0; path_iter < path_max; path_iter++) {
                    if (box_iter == box_max - 1) {
                        if (box_log(box_iter, path_iter) > 0) {
                            continue;
                        } else {
                            box_iter--;
                        }
                    }
                    if (box_log(box_iter, path_iter) > 0 && box_log(box_iter + 1, path_iter) > 0) {
                        int count = 1;
                        while (path_iter + count < path_max && box_log(box_iter, path_iter + count) > 0
                               && box_log(box_iter + 1, path_iter + count) > 0) {
                            count++;
                        }
                        int obs_index = path_iter + count / 2;
                        planResult_ptr->SFC[qi][box_iter].second = planResult_ptr->T[obs_index];

                        path_iter = path_iter + count / 2;
                        box_iter++;
                    } else if (box_log(box_iter, path_iter) == 0) {
                        box_iter--;
                        path_iter--;
                    }
                }
                planResult_ptr->SFC[qi][box_max - 1].second = makespan;
            }

            timer.stop();
            ROS_INFO_STREAM("Corridor: SFC runtime=" << timer.elapsedSeconds());
            return true;
        }

        bool updateRelBox() {
            Timer timer;

            planResult_ptr->RSFC.resize(mission.qn);
            for (int qi = 0; qi < mission.qn; qi++) {
                planResult_ptr->RSFC[qi].resize(mission.qn);
                for (int qj = qi + 1; qj < mission.qn; qj++) {
                    int path_size = planResult_ptr->initTraj[qi].size();
                    if (planResult_ptr->initTraj[qi].size() != planResult_ptr->initTraj[qj].size()) {
                        ROS_ERROR("Corridor: size of initial trajectories must be equal");
                        return false;
                    }

                    octomap::point3d a, b, c, n, m;
                    double dist, dist_min;
                    for (int iter = 1; iter < planResult_ptr->T.size(); iter++) {
                        a = planResult_ptr->initTraj[qj][iter - 1] - planResult_ptr->initTraj[qi][iter - 1];
                        b = planResult_ptr->initTraj[qj][iter] - planResult_ptr->initTraj[qi][iter];

                        // Coordinate transformation
                        a.z() = a.z() / param.downwash;
                        b.z() = b.z() / param.downwash;

                        // get closest point of L from origin
                        if (a == b) {
                            m = a;
                        } else {
                            m = a;
                            dist_min = a.norm();

                            dist = b.norm();
                            if (dist_min > dist) {
                                m = b;
                                dist_min = dist;
                            }

                            n = b - a;
                            n.normalize();
                            c = a - n * a.dot(n);
                            dist = c.norm();
                            if ((c - a).dot(c - b) < 0 && dist_min > dist) {
                                m = c;
                            }
                        }
                        m.normalize();

                        m.z() = m.z() / param.downwash;
                        if (m.norm() == 0) {
                            ROS_ERROR("Corridor: initial trajectories are collided with each other");
                            return false;
                        }

                        planResult_ptr->RSFC[qi][qj].emplace_back(std::make_pair(m, planResult_ptr->T[iter]));
                    }
                }
            }

            timer.stop();
            ROS_INFO_STREAM("Corridor: RSFC runtime=" << timer.elapsedSeconds());
            return true;
        }

        bool updateFlatObsBox() {
            double x_next, y_next, z_next, dx, dy, dz;

            Timer timer;

            planResult_ptr->SFC.resize(mission.qn);
            for (size_t qi = 0; qi < mission.qn; ++qi) {
                std::vector<double> box_prev;
                for (int i = 0; i < 6; i++) box_prev.emplace_back(0);

                for (int i = 0; i < planResult_ptr->initTraj[qi].size() - 1; i++) {
                    auto state = planResult_ptr->initTraj[qi][i];
                    double x = state.x();
                    double y = state.y();
                    double z = state.z();

                    std::vector<double> box;
                    auto state_next = planResult_ptr->initTraj[qi][i + 1];
                    x_next = state_next.x();
                    y_next = state_next.y();
                    z_next = state_next.z();

                    if (isPointInBox(octomap::point3d(x_next, y_next, z_next), box_prev)) {
                        continue;
                    }

                    // Initialize box
                    box.emplace_back(std::min(x, x_next) - param.box_xy_res / 2.0);
                    box.emplace_back(std::min(y, y_next) - param.box_xy_res / 2.0);
                    box.emplace_back(std::min(z, z_next) - param.box_z_res / 2.0);
                    box.emplace_back(std::max(x, x_next) + param.box_xy_res / 2.0);
                    box.emplace_back(std::max(y, y_next) + param.box_xy_res / 2.0);
                    box.emplace_back(std::max(z, z_next) + param.box_z_res / 2.0);

                    if (isObstacleInBox(box, mission.quad_size[qi])) {
                        ROS_ERROR("Corridor: Invalid initial trajectory, obstacle invades initial trajectory.");
                        return false;
                    }
                    expand_box(box, mission.quad_size[qi]);

                    planResult_ptr->SFC[qi].emplace_back(std::make_pair(box, -1));

                    box_prev = box;
                }

                // Generate box time segment
                int box_max = planResult_ptr->SFC[qi].size();
                int path_max = planResult_ptr->initTraj[qi].size();
                Eigen::MatrixXd box_log = Eigen::MatrixXd::Zero(box_max, path_max);
                std::vector<int> ts;

                for (int i = 0; i < box_max; i++) {
                    for (int j = 0; j < path_max; j++) {
                        if (isPointInBox(planResult_ptr->initTraj[qi][j], planResult_ptr->SFC[qi][i].first)) {
                            if (j == 0) {
                                box_log(i, j) = 1;
                            } else {
                                box_log(i, j) = box_log(i, j - 1) + 1;
                            }
                        }
                    }
                }

                if (log) {
                    std::cout << qi << std::endl;
                    std::cout << box_log << std::endl;
                }

                int box_iter = 0;
                for (int path_iter = 0; path_iter < path_max; path_iter++) {
                    if (box_iter >= box_max - 1) {
                        break;
                    }
                    if (box_log(box_iter, path_iter) > 0 && box_log(box_iter + 1, path_iter) > 0) {
                        int count = 1;
                        while (path_iter + count < path_max && box_log(box_iter, path_iter + count) > 0
                               && box_log(box_iter + 1, path_iter + count) > 0) {
                            count++;
                        }
                        double obs_index = path_iter + count / 2;
                        planResult_ptr->SFC[qi][box_iter].second = obs_index * param.time_step;
                        planResult_ptr->T.emplace_back(obs_index);

                        path_iter = path_iter + count / 2;
                        box_iter++;
                    }
                }
                planResult_ptr->SFC[qi][box_max - 1].second = makespan * param.time_step;
            }

            timer.stop();
            ROS_INFO_STREAM("Corridor: SFC runtime=" << timer.elapsedSeconds());
            return true;
        }

        bool updateFlatRelBox() {
            int sector_range[6] = {-3, -2, -1, 1, 2, 3};

            Timer timer;

            planResult_ptr->RSFC.resize(mission.qn);
            for (int qi = 0; qi < mission.qn; qi++) {
                planResult_ptr->RSFC[qi].resize(mission.qn);
                for (int qj = qi + 1; qj < mission.qn; qj++) {
                    int path_max = std::max<int>(planResult_ptr->initTraj[qi].size(),
                                                 planResult_ptr->initTraj[qj].size());
                    int path_min = std::min<int>(planResult_ptr->initTraj[qi].size(),
                                                 planResult_ptr->initTraj[qj].size());
                    Eigen::MatrixXd sector_log = Eigen::MatrixXd::Zero(6, path_max);

                    for (int iter = 0; iter < path_max; iter++) {
                        // Get rel_pose
                        int rel_pose[4];
                        double dx, dy, dz;

                        if (iter < path_min) {
                            dx = round((planResult_ptr->initTraj[qj][iter].x()
                                    - planResult_ptr->initTraj[qi][iter].x()) / param.grid_xy_res);
                            dy = round((planResult_ptr->initTraj[qj][iter].y()
                                    - planResult_ptr->initTraj[qi][iter].y()) / param.grid_xy_res);
                            dz = round((planResult_ptr->initTraj[qj][iter].z()
                                    - planResult_ptr->initTraj[qi][iter].z()) / param.grid_z_res);
                        } else if (planResult_ptr->initTraj[qi].size() == path_min) {
                            dx = round((planResult_ptr->initTraj[qj][iter].x()
                                    - planResult_ptr->initTraj[qi][path_min - 1].x()) / param.grid_xy_res);
                            dy = round((planResult_ptr->initTraj[qj][iter].y()
                                    - planResult_ptr->initTraj[qi][path_min - 1].y()) / param.grid_xy_res);
                            dz = round((planResult_ptr->initTraj[qj][iter].z()
                                    - planResult_ptr->initTraj[qi][path_min - 1].z()) / param.grid_z_res);
                        } else {
                            dx = round((planResult_ptr->initTraj[qj][path_min - 1].x()
                                    - planResult_ptr->initTraj[qi][iter].x()) / param.grid_xy_res);
                            dy = round((planResult_ptr->initTraj[qj][path_min - 1].y()
                                    - planResult_ptr->initTraj[qi][iter].y()) / param.grid_xy_res);
                            dz = round((planResult_ptr->initTraj[qj][path_min - 1].z()
                                    - planResult_ptr->initTraj[qi][iter].z()) / param.grid_z_res);
                        }
                        // Caution: (q1_size+q2_size)/grid_size should be small enough!
                        rel_pose[1] = (dx > SP_EPSILON_FLOAT) - (dx < -SP_EPSILON_FLOAT);
                        rel_pose[2] = (dy > SP_EPSILON_FLOAT) - (dy < -SP_EPSILON_FLOAT);
                        rel_pose[3] = (dz > SP_EPSILON_FLOAT) - (dz < -SP_EPSILON_FLOAT);

                        // Save sector information
                        for (int i = 0; i < 6; i++) {
                            int sector = sector_range[i];
                            int sgn = (i > 2) - (i < 3);
                            if (rel_pose[abs(sector)] * sgn > 0) {
                                if (iter == 0) {
                                    sector_log(i, iter) = 1;
                                } else {
                                    sector_log(i, iter) = sector_log(i, iter - 1) + 1;
                                }
                            }
                        }
                    }

                    //find minimum jump sector path (heuristic greedy search)
                    int iter = path_max - 1;
                    int sector_next = -1;
                    int count_next = sector_log.col(iter).maxCoeff(&sector_next);

                    planResult_ptr->RSFC[qi][qj].emplace_back(
                            std::make_pair(sec2normVec(sector_range[sector_next]), makespan * param.time_step));
                    iter = iter - count_next + 1;

                    while (iter > 1) {
                        int sector_curr;
                        int count;

                        // if there is no intersection then allow to jump sector
                        // except jumping through quadrotor (i.e. +x -> -x jumping is not allowed)
                        if (sector_log.col(iter).maxCoeff(&sector_curr) <= 1) {
                            iter = iter - 1;
                            int sector_opp = 6 - 1 - sector_next;

                            if (sector_log.col(iter).maxCoeff(&sector_curr) <= 0) {
                                ROS_ERROR("Corridor: Invalid initial trajectory, there is missing link");
                                std::cout << sector_log << std::endl;
                                return false;
                            } else if (sector_curr == sector_opp) {
                                bool flag = false;
                                for (int i = 0; i < 6; i++) {
                                    if (i != sector_opp &&
                                        sector_log(i, iter) == sector_log.col(iter).maxCoeff(&sector_curr)) {
                                        flag = true;
                                        break;
                                    }
                                }
                                if (!flag) {
                                    ROS_ERROR("Corridor: Invalid Path, jumping through quadrotor");
                                    std::cout << sector_log << std::endl;
                                    return false;
                                }
                            }
                            count = 0;
                        } else {
                            count = 1;
                            while (sector_log(sector_curr, iter + count) > 0) {
                                count++;
                            }
                        }

                        double rel_index;
                        if (count == 0) {
                            rel_index = iter + 0.5;
                        } else {
                            rel_index = floor(iter + count / 2.0);
                        }

                        planResult_ptr->RSFC[qi][qj].insert(planResult_ptr->RSFC[qi][qj].begin(),
                                                            std::make_pair(sec2normVec(sector_range[sector_curr]),
                                                                           rel_index * param.time_step));
                        planResult_ptr->T.emplace_back(rel_index);

                        sector_next = sector_curr;
                        iter = iter - sector_log.col(iter).maxCoeff() + 1;
                    }
                }
            }

            timer.stop();
            ROS_INFO_STREAM("Corridor: RSFC runtime=" << timer.elapsedSeconds());
        }

        octomap::point3d sec2normVec(int sector) {
            octomap::point3d n;
            n.x() = 0;
            n.y() = 0;
            n.z() = 0;
            int sgn = (sector > 0) - (sector < 0);

            switch (abs(sector)) {
                case 1:
                    n.x() = sgn;
                    break;
                case 2:
                    n.y() = sgn;
                    break;
                case 3:
                    n.z() = sgn / param.downwash;
                    break;
                default:
                    ROS_ERROR("Corridor: Invalid sector number.");
                    break;
            }

            return n;
        }

        bool updateTs() {
            Timer timer;

            planResult_ptr->T.emplace_back(0);
            planResult_ptr->T.emplace_back(makespan);

//        // Delete redundant time delay
//        for(int qi = 0; qi < mission.qn; qi++){
//            for(int qj = qi + 1; qj < mission.qn; qj++){
//                for(int ri=0; ri<RSFC[qi][qj].size(); ri++){
//                    double t = RSFC[qi][qj][ri].second;
//                    if(t-floor(t) > 0.001){
//                        std::vector<double> obsbox_i;
//                        int bi = 0;
//                        while(bi < SFC[qi].size() && SFC[qi][bi].second < floor(t)){
//                            bi++;
//                        }
//                        if(SFC[qi][bi].second == floor(t)){
//                            //bi bi+1
//                            for(int i=0; i<3; i++) {
//                                obsbox_i.emplace_back(std::max(SFC[qi][bi].first[i],
//                                                               SFC[qi][bi+1].first[i]));
//                            }
//                            for(int i=3; i<6; i++) {
//                                obsbox_i.emplace_back(std::min(SFC[qi][bi].first[i],
//                                                               SFC[qi][bi+1].first[i]));
//                            }
//                        }
//                        else{
//                            //bi
//                            obsbox_i = SFC[qi][bi].first;
//                        }
//
//                        std::vector<double> obsbox_j;
//                        int bj = 0;
//                        while(bj < SFC[qj].size() && SFC[qj][bj].second < floor(t)){
//                            bj++;
//                        }
//                        if(SFC[qj][bj].second == floor(t)){
//                            // bj bj+1
//                            for(int j=0; j<3; j++) {
//                                obsbox_j.emplace_back(std::max(SFC[qj][bj].first[j],
//                                                               SFC[qj][bj+1].first[j]));
//                            }
//                            for(int j=3; j<6; j++) {
//                                obsbox_j.emplace_back(std::min(SFC[qj][bj].first[j],
//                                                               SFC[qj][bj+1].first[j]));
//                            }
//                        }
//                        else{
//                            obsbox_j = SFC[qj][bj].first;
//                        }
//
//                        bool flag1 = true;
//                        bool flag2 = true;
//                        int sector1 = RSFC[qi][qj][ri].first;
//                        int sector2 = RSFC[qi][qj][ri+1].first;
//
//                        if(sector1 > 0 && obsbox_j[sector1+3-1] < obsbox_i[sector1-1]+mission.quad_size[qi]+mission.quad_size[qj]){ //z axis coeff ////////////////////
//                            flag1 = false;
//                        }
//                        else if(sector1 < 0 && obsbox_j[abs(sector1)-1] > obsbox_i[abs(sector1)+3-1]-mission.quad_size[qi]-mission.quad_size[qj]){
//                            flag1 = false;
//                        }
//
//                        if(sector2 > 0 && obsbox_j[sector2+3-1] < obsbox_i[sector2-1]+mission.quad_size[qi]+mission.quad_size[qj]){ //z axis coeff ////////////////////
//                            flag2 = false;
//                        }
//                        else if(sector2 < 0 && obsbox_j[abs(sector2)-1] > obsbox_i[abs(sector2)+3-1]-mission.quad_size[qi]-mission.quad_size[qj]){
//                            flag2 = false;
//                        }
//
//                        if(flag1 && flag2){
//                            T.erase(std::find(T.begin(), T.end(), t));
//                            T.emplace_back(floor(t));
//                            RSFC[qi][qj][ri].second = floor(t);
//                        }
//                    }
//                }
//            }
//        }

            // update ts_total
            std::sort(planResult_ptr->T.begin(), planResult_ptr->T.end());
            planResult_ptr->T.erase(std::unique(planResult_ptr->T.begin(), planResult_ptr->T.end()),
                                    planResult_ptr->T.end());

//        // check isolate box and update ts_each
//        for(int qi = 0; qi < mission.qn; qi++){
//            ts_each[qi].emplace_back(0);
//            ts_each[qi].emplace_back(makespan);
//
//            for(int qj = qi + 1; qj < qn; qj++){
//                for(int ti = 1; ti < ts_total.size(); ti++){
//                    int bi = 0;
//                    while(bi < obstacle_boxes[qi].size() && obstacle_boxes[qi][bi].second < ts_total[ti]){
//                        bi++;
//                    }
//                    int bj = 0;
//                    while(bj < obstacle_boxes[qj].size() && obstacle_boxes[qj][bj].second < ts_total[ti]){
//                        bj++;
//                    }
//
//                    if(isBoxInBox(obstacle_boxes[qi][bi].first, obstacle_boxes[qj][bj].first)){
//                        ts_each[qi].emplace_back(ts_total[ti-1]);
//                        ts_each[qj].emplace_back(ts_total[ti]);
//                    }
//                }
//            }
//
//            std::sort(ts_each[qi].begin(), ts_each[qi].end());
//            ts_each[qi].erase(std::unique(ts_each[qi].begin(), ts_each[qi].end()), ts_each[qi].end());
//        }

            // scaling
            for (int i = 0; i < planResult_ptr->T.size(); i++) {
                planResult_ptr->T[i] = planResult_ptr->T[i] * param.time_step;
            }

            timer.stop();
            ROS_INFO_STREAM("Corridor: segment time runtime: " << timer.elapsedSeconds());
            return true;
        }
    };
}