#include "dfise_converter.h"

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

#include "TCanvas.h"
#include "TFile.h"
#include "TGraph2D.h"
#include "TH1.h"
#include "TTree.h"

#include <Eigen/Eigen>

#include "Octree.hpp"
#include "config/ConfigReader.hpp"
#include "config/Configuration.hpp"
#include "config/exceptions.h"
#include "read_dfise.h"
#include "utils/log.h"

using namespace mesh_converter;

void interrupt_handler(int) {
    LOG(STATUS) << "Interrupted! Aborting conversion...";
    allpix::Log::finish();
    std::exit(0);
}

void MeshElement::setVertices(std::vector<Point>& new_vertices) {
    if(vertices.size() != new_vertices.size()) {
        LOG(ERROR) << "Invalid vertices vector";
        return;
    }
    for(size_t index = 0; index < new_vertices.size(); index++) {
        vertices[index] = new_vertices[index];
    }
}

void MeshElement::setVertex(size_t index, Point& new_vertice) {
    vertices[index] = new_vertice;
}

Point MeshElement::getVertex(size_t index) {
    return vertices[index];
}

void MeshElement::setVerticesField(std::vector<Point>& new_observable) {
    if(vertices.size() != new_observable.size()) {
        LOG(ERROR) << "Invalid field vector";
        return;
    }
    for(size_t index = 0; index < 4; index++) {
        e_field[index] = new_observable[index];
    }
}

void MeshElement::setVertexField(size_t index, Point& new_observable) {
    e_field[index] = new_observable;
}

Point MeshElement::getVertexProperty(size_t index) {
    return e_field[index];
}

void MeshElement::setDimension(int dimension) {
    _dimension = dimension;
}

int MeshElement::getDimension() {
    return _dimension;
}

double MeshElement::getVolume() {
    double volume = 0;
    if(this->getDimension() == 3) {
        Eigen::Matrix4d element_matrix;
        element_matrix << 1, 1, 1, 1, vertices[0].x, vertices[1].x, vertices[2].x, vertices[3].x, vertices[0].y,
            vertices[1].y, vertices[2].y, vertices[3].y, vertices[0].z, vertices[1].z, vertices[2].z, vertices[3].z;
        volume = (element_matrix.determinant()) / 6;
    }
    if(this->getDimension() == 2) {
        Eigen::Matrix3d element_matrix;
        element_matrix << 1, 1, 1, vertices[0].y, vertices[1].y, vertices[2].y, vertices[0].z, vertices[1].z, vertices[2].z;
        volume = (element_matrix.determinant()) / 2;
    }
    return volume;
}

double MeshElement::getDistance(size_t index, Point& qp) {
    return unibn::L2Distance<Point>::compute(vertices[index], qp);
}

bool MeshElement::validElement(double volume_cut, Point& qp) {
    if(this->getVolume() == 0) {
        LOG(TRACE) << "Invalid tetrahedron with coplanar(3D)/colinear(2D) vertices.";
        return false;
    }
    if(std::abs(this->getVolume()) <= volume_cut) {
        LOG(TRACE) << "Tetrahedron volume smaller than volume cut.";
        return false;
    }

    Eigen::Matrix4d sub_tetra_matrix;
    for(size_t i = 0; i < static_cast<size_t>(this->getDimension()) + 1; i++) {
        std::vector<Point> sub_vertices = vertices;
        sub_vertices[i] = qp;
        MeshElement sub_tetrahedron(sub_vertices);
        sub_tetrahedron.setDimension(this->getDimension());
        double tetra_volume = sub_tetrahedron.getVolume();
        if(this->getVolume() * tetra_volume >= 0) {
            continue;
        }
        if(this->getVolume() * tetra_volume < 0) {
            LOG(TRACE) << "New mesh Point outside found element.";
            return false;
        }
    }
    return true;
}

Point MeshElement::getObservable(Point& qp) {
    Point new_observable;
    Eigen::Matrix4d sub_tetra_matrix;
    for(size_t index = 0; index < static_cast<size_t>(this->getDimension()) + 1; index++) {
        auto sub_vertices = vertices;
        sub_vertices[index] = qp;
        MeshElement sub_tetrahedron(sub_vertices);
        sub_tetrahedron.setDimension(this->getDimension());
        double sub_volume = sub_tetrahedron.getVolume();
        LOG(DEBUG) << "Sub volume " << index << ": " << sub_volume;
        new_observable.x = new_observable.x + (sub_volume * e_field[index].x) / this->getVolume();
        new_observable.y = new_observable.y + (sub_volume * e_field[index].y) / this->getVolume();
        new_observable.z = new_observable.z + (sub_volume * e_field[index].z) / this->getVolume();
    }
    LOG(DEBUG) << "Interpolated electric field: (" << new_observable.x << "," << new_observable.y << "," << new_observable.z
               << ")" << std::endl;
    return new_observable;
}

void MeshElement::printElement(Point& qp) {
    for(size_t index = 0; index < static_cast<size_t>(this->getDimension()) + 1; index++) {
        LOG(DEBUG) << "Tetrahedron vertex " << index_vec[index] << " (" << vertices[index].x << ", " << vertices[index].y
                   << ", " << vertices[index].z << ") - "
                   << " Distance: " << this->getDistance(index, qp) << " - Electric field: (" << e_field[index].x << ", "
                   << e_field[index].y << ", " << e_field[index].z << ")";
    }
    LOG(DEBUG) << "Volume: " << this->getVolume();
}

void mesh_converter::mesh_plotter(const std::string& grid_file,
                                  double ss_radius,
                                  double radius,
                                  double x,
                                  double y,
                                  double z,
                                  std::vector<Point> points,
                                  std::vector<unsigned int> results) {
    auto tg = new TGraph2D();
    tg->SetMarkerStyle(20);
    tg->SetMarkerSize(0.5);
    tg->SetMarkerColor(kBlack);

    for(auto& point : points) {
        if(ss_radius != -1) {
            if((abs(point.x - x) < radius * ss_radius) && (abs(point.y - y) < radius * ss_radius) &&
               (abs(point.z - z) < radius * ss_radius)) {
                tg->SetPoint(tg->GetN(), point.x, point.y, point.z);
            }
        } else {
            tg->SetPoint(tg->GetN(), point.x, point.y, point.z);
        }
    }

    auto tg1 = new TGraph2D();
    auto tg2 = new TGraph2D();
    tg1->SetMarkerStyle(20);
    tg1->SetMarkerSize(1);
    tg1->SetMarkerColor(kBlue);
    tg2->SetMarkerStyle(34);
    tg2->SetMarkerSize(1);
    tg2->SetMarkerColor(kRed);
    tg2->SetPoint(0, x, y, z);
    for(auto& elem : results) {
        tg1->SetPoint(tg1->GetN(), points[elem].x, points[elem].y, points[elem].z);
    }

    std::string root_file_name_out = grid_file + "_INTERPOLATION_POINT_SCREEN_SHOT.root";
    auto root_output = new TFile(root_file_name_out.c_str(), "RECREATE");
    auto c = new TCanvas();
    tg->Draw("p");
    tg1->Draw("p same");
    tg2->Draw("p same");
    c->Write("canvas");
    root_output->Close();

    LOG(STATUS) << "Mesh screen-shot created. Closing the program.";
}

int main(int argc, char** argv) {
    // 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);
    allpix::Log::setReportingLevel(allpix::LogLevel::INFO);

    // 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;

    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 {
                allpix::LogLevel log_level = allpix::Log::getLevelFromString(std::string(argv[++i]));
                allpix::Log::setReportingLevel(log_level);
            } 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]);
            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;
        }
    }

    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: ./tcad_dfise_reader -f <file_name> [<options>]" << std::endl;
        std::cout << "\t -f <file_prefix>       common prefix of DF-ISE grid (.grd) and data (.dat) files" << std::endl;
        std::cout << "\t -c <config_file>       configuration file name" << std::endl;
        std::cout << "\t -o <init_file_prefix>  output file prefix without .init (defaults to file name of <file_prefix>)"
                  << std::endl;
        std::cout << "\t -l <file>              file to log to besides standard output (disabled by default)" << std::endl;
        std::cout << "\t -v <level>             verbosity level (default reporiting level is INFO)" << std::endl;

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

    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 region = config.get<std::string>("region", "bulk");
    std::string observable = config.get<std::string>("observable", "ElectricField");

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

    bool threshold_flag = false;
    auto radius_threshold = config.get<double>("radius_threshold", -1);
    if(radius_threshold != -1) {
        threshold_flag = true;
    }

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

    bool index_cut_flag = false;
    auto index_cut = config.get<size_t>("index_cut", 999999);
    if(index_cut != 999999) {
        index_cut_flag = true;
    }

    auto xdiv = config.get<int>("xdiv", 100);
    auto ydiv = config.get<int>("ydiv", 100);
    auto zdiv = config.get<int>("zdiv", 100);
    auto dimension = config.get<int>("dimension", 3);
    if(dimension == 2) {
        xdiv = 1;
    }

    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;
    }

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

    bool ss_flag = false;
    auto ss_radius = config.get<double>("ss_radius", -1);
    std::vector<int> ss_point = {-1, -1, -1};
    if(config.has("screen_shot")) {
        ss_point = config.getArray<int>("screen_shot");
    }
    if(ss_point[0] != -1 && ss_point[1] != -1 && ss_point[2] != -1 && dimension == 3) {
        ss_flag = true;
    }

    // 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();

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

    std::vector<Point> points;
    try {
        auto region_grid = read_grid(grid_file, mesh_tree);
        points = region_grid[region];
        LOG(DEBUG) << "Grid with " << points.size() << " points";
    } catch(std::runtime_error& e) {
        LOG(FATAL) << "Failed to parse grid file " << grid_file;
        LOG(FATAL) << " " << e.what();
        allpix::Log::finish();
        return 1;
    }

    LOG(STATUS) << "Reading electric field from data file";
    std::string data_file = file_prefix + ".dat";
    std::vector<Point> field;
    try {
        auto region_fields = read_electric_field(data_file);
        field = region_fields[region][observable];
        LOG(DEBUG) << "Field with " << field.size() << " points";
    } catch(std::runtime_error& e) {
        LOG(FATAL) << "Failed to parse data file " << data_file;
        LOG(FATAL) << " " << e.what();
        allpix::Log::finish();
        return 1;
    }

    if(points.size() != field.size()) {
        LOG(FATAL) << "Field and grid file do not match";
        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
    double xstep = (maxx - minx) / static_cast<double>(xdiv);
    double ystep = (maxy - miny) / static_cast<double>(ydiv);
    double zstep = (maxz - minz) / static_cast<double>(zdiv);
    double cell_volume = xstep * ystep * zstep;

    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.";

    LOG(STATUS) << "Starting regular grid interpolation";
    // Initializing the Octree with points from mesh cloud.
    unibn::Octree<Point> octree;
    octree.initialize(points);
    std::vector<Point> e_field_new_mesh;

    double x = minx + xstep / 2.0;
    for(int i = 0; i < xdiv; ++i) {
        double y = miny + ystep / 2.0;
        for(int j = 0; j < ydiv; ++j) {
            double z = minz + zstep / 2.0;
            for(int k = 0; k < zdiv; ++k) {
                Point q, e;
                if(ss_flag) {
                    std::map<std::string, int> map;
                    map.emplace("x", ss_point[0]);
                    map.emplace("y", ss_point[1]);
                    map.emplace("z", ss_point[2]);
                    x = map.find(rot.at(0))->second;
                    y = map.find(rot.at(1))->second;
                    z = map.find(rot.at(2))->second;
                }
                if(dimension == 2) {
                    q.x = -1;
                    q.y = y;
                    q.z = z; // New mesh vertex
                    e.x = -1;
                    e.y = y;
                    e.z = z;
                } else {
                    q.x = x;
                    q.y = y;
                    q.z = z; // New mesh vertex
                    e.x = x;
                    e.y = y;
                    e.z = z; // Corresponding, to be interpolated, electric field
                }
                bool valid = false;

                LOG_PROGRESS(INFO, "POINT") << "Interpolating point X=" << i + 1 << " Y=" << j + 1 << " Z=" << k + 1 << " ("
                                            << q.x << "," << q.y << "," << q.z << ")";

                size_t prev_neighbours = 0;
                double radius = initial_radius;
                size_t index_cut_up;
                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;
                    std::vector<unsigned int> results_high;
                    octree.radiusNeighbors<unibn::L2Distance<Point>>(q, radius, results_high);
                    std::sort(results_high.begin(), results_high.end(), [&](unsigned int a, unsigned int b) {
                        return unibn::L2Distance<Point>::compute(points[a], q) <
                               unibn::L2Distance<Point>::compute(points[b], q);
                    });

                    if(threshold_flag) {
                        size_t results_size = results_high.size();
                        int count = 0;
                        for(size_t index = 0; index < results_size; index++) {
                            if(unibn::L2Distance<Point>::compute(points[results_high[index]], q) < radius_threshold) {
                                count++;
                                continue;
                            }
                            results.push_back(results_high[index]);
                        }
                        LOG(DEBUG) << "Applying radius threshold of " << radius_threshold << std::endl
                                   << "Removing " << count << " of " << results_size;
                    } else {
                        results = results_high;
                    }

                    // 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(WARNING) << "No (new) neighbour found with radius " << radius << ". Increasing search radius."
                                     << std::endl;
                        radius = radius + radius_step;
                        continue;
                    }

                    if(results.size() < 4) {
                        LOG(WARNING) << "Incomplete mesh element found for radius " << radius << std::endl
                                     << "Increasing the readius (setting a higher initial radius may help)";
                        radius = radius + radius_step;
                        continue;
                    }

                    LOG(DEBUG) << "Number of vertices found: " << results.size();

                    if(ss_flag) {
                        mesh_plotter(grid_file, ss_radius, radius, x, y, z, points, results);
                        return 1;
                    }

                    // Finding tetrahedrons
                    Eigen::Matrix4d matrix;
                    size_t num_nodes_element = 0;
                    if(dimension == 3) {
                        num_nodes_element = 4;
                    }
                    if(dimension == 2) {
                        num_nodes_element = 3;
                    }
                    std::vector<Point> element_vertices;
                    std::vector<Point> element_vertices_field;

                    std::vector<int> bitmask(num_nodes_element, 1);
                    bitmask.resize(results.size(), 0);
                    std::vector<size_t> index;

                    if(!index_cut_flag) {
                        index_cut = results.size();
                    }
                    index_cut_up = index_cut;
                    while(index_cut_up <= results.size()) {
                        do {
                            valid = false;
                            index.clear();
                            element_vertices.clear();
                            element_vertices_field.clear();
                            // print integers and permute bitmask
                            for(size_t idk = 0; idk < results.size(); ++idk) {
                                if(bitmask[idk] != 0) {
                                    index.push_back(idk);
                                    element_vertices.push_back(points[results[idk]]);
                                    element_vertices_field.push_back(field[results[idk]]);
                                }
                                if(index.size() == num_nodes_element) {
                                    break;
                                }
                            }

                            bool index_flag = false;
                            for(size_t ttt = 0; ttt < num_nodes_element; ttt++) {
                                if(index[ttt] > index_cut_up) {
                                    index_flag = true;
                                    break;
                                }
                            }
                            if(index_flag) {
                                continue;
                            }

                            if(dimension == 3) {
                                LOG(TRACE) << "Parsing neighbors [index]: " << index[0] << ", " << index[1] << ", "
                                           << index[2] << ", " << index[3];
                            }
                            if(dimension == 2) {
                                LOG(TRACE)
                                    << "Parsing neighbors [index]: " << index[0] << ", " << index[1] << ", " << index[2];
                            }

                            MeshElement element(dimension, index, element_vertices, element_vertices_field);
                            valid = element.validElement(volume_cut, q);
                            if(!valid) {
                                continue;
                            }
                            element.printElement(q);
                            e = element.getObservable(q);
                            break;
                        } while(std::prev_permutation(bitmask.begin(), bitmask.end()));

                        if(valid) {
                            break;
                        }

                        LOG(DEBUG) << "All combinations tried up to index " << index_cut_up
                                   << " done. Increasing the index cut.";
                        index_cut_up = index_cut_up + index_cut;
                    }

                    if(valid) {
                        break;
                    }

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

                if(!valid) {
                    LOG(FATAL) << "Couldn't interpolate new mesh point, probably the grid is too irregular";
                    return 1;
                }

                e_field_new_mesh.push_back(e);
                z += zstep;
            }
            y += ystep;
        }
        x += xstep;
    }

    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.";

    LOG(STATUS) << "Writing INIT file";

    std::ofstream init_file;
    std::stringstream init_file_name;
    init_file_name << init_file_prefix << "_" << observable << ".init";
    init_file.open(init_file_name.str());

    // Write INIT file h"eader
    init_file << "tcad_dfise_converter, ";                                             // NAME
    init_file << "observable: " << observable << std::endl;                            // OBSERVABLE INTERPOLATED
    init_file << "##SEED## ##EVENTS##" << std::endl;                                   // UNUSED
    init_file << "##TURN## ##TILT## 1.0" << std::endl;                                 // UNUSED
    init_file << "0.0 0.0 0.0" << std::endl;                                           // MAGNETIC FIELD (UNUSED)
    init_file << (maxz - minz) << " " << (maxx - minx) << " " << (maxy - miny) << " "; // PIXEL DIMENSIONS
    init_file << "0.0 0.0 0.0 0.0 ";                                                   // UNUSED
    init_file << xdiv << " " << ydiv << " " << zdiv << " ";                            // GRID SIZE
    init_file << "0.0" << std::endl;                                                   // UNUSED

    // Write INIT file data
    for(int i = 0; i < xdiv; ++i) {
        for(int j = 0; j < ydiv; ++j) {
            for(int k = 0; k < zdiv; ++k) {
                auto& point = e_field_new_mesh[static_cast<unsigned int>(i * ydiv * zdiv + j * zdiv + k)];
                init_file << i + 1 << " " << j + 1 << " " << k + 1 << " " << point.x << " " << point.y << " " << point.z
                          << std::endl;
            }
        }
    }
    init_file.close();

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

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


#define BOOST_TEST_MODULE "test_bond_angle_interaction"

#ifdef BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#else
#include <boost/test/included/unit_test.hpp>
#endif

#include <test/util/utility.hpp>

#include <mjolnir/core/BoundaryCondition.hpp>
#include <mjolnir/core/SimulatorTraits.hpp>
#include <mjolnir/forcefield/local/BondAngleInteraction.hpp>
#include <mjolnir/forcefield/local/HarmonicPotential.hpp>
#include <mjolnir/math/constants.hpp>
#include <mjolnir/util/make_unique.hpp>

#include <random>

BOOST_AUTO_TEST_CASE(BondAngleInteraction_numerical_diff)
{
    namespace test = mjolnir::test;

    using traits_type     = mjolnir::SimulatorTraits<double, mjolnir::UnlimitedBoundary>;
    using real_type       = traits_type::real_type;
    using coord_type      = traits_type::coordinate_type;
    using boundary_type   = traits_type::boundary_type;
    using system_type     = mjolnir::System<traits_type>;
    using harmonic_type   = mjolnir::HarmonicPotential<real_type>;
    using bond_angle_type = mjolnir::BondAngleInteraction<traits_type, harmonic_type>;

    const real_type k(10.0);
    const real_type native(mjolnir::math::constants<real_type>::pi() / 3.0);

    std::mt19937 mt(123456789);

    harmonic_type potential{k, native};
    bond_angle_type interaction("none", {{ {{0,1,2}}, potential}});

    for(int i = 0; i < 1000; ++i)
    {
        system_type sys(3, boundary_type{});
        test::clear_everything(sys);

        sys.position(0) = coord_type(1.0, 0.0, 0.0);
        sys.position(1) = coord_type(0.0, 0.0, 1.0);
        sys.position(2) = coord_type(1.0, 1.0, 1.0);

        test::apply_random_rotation(sys, mt);
        test::apply_random_perturbation(sys, mt, 0.01);

        constexpr real_type tol = 1e-4;
        constexpr real_type dr  = 1e-5;

        test::check_force(sys, interaction, tol, dr);
        test::check_virial(sys, interaction, tol);
        test::check_force_and_virial(sys, interaction, tol);
        test::check_force_and_energy(sys, interaction, tol);
        test::check_force_energy_virial(sys, interaction, tol);
    }
}

BOOST_AUTO_TEST_CASE(BondAngleInteraction_force)
{
    using traits_type     = mjolnir::SimulatorTraits<double, mjolnir::UnlimitedBoundary>;
    using real_type       = traits_type::real_type;
    using coord_type      = traits_type::coordinate_type;
    using boundary_type   = traits_type::boundary_type;
    using system_type     = mjolnir::System<traits_type>;
    using harmonic_type   = mjolnir::HarmonicPotential<real_type>;
    using bond_angle_type = mjolnir::BondAngleInteraction<traits_type, harmonic_type>;

    constexpr real_type tol = 1e-7;

    auto normalize = [](const coord_type& v){return v / mjolnir::math::length(v);};

    const real_type k(1e0);
    const real_type native(mjolnir::math::constants<real_type>::pi() * 2.0 / 3.0); // 120 degree

    harmonic_type potential{k, native};
    bond_angle_type interaction("none", {{ {{0,1,2}}, potential}});

    const coord_type pos1(1., 0., 0.);
    const coord_type pos2(0., 0., 0.);
    system_type sys(3, boundary_type{});

    sys.mass(0) = 1.0;
    sys.mass(1) = 1.0;
    sys.mass(2) = 1.0;
    sys.rmass(0) = 1.0;
    sys.rmass(1) = 1.0;
    sys.rmass(2) = 1.0;

    sys.position(0) = pos1;
    sys.position(1) = pos2;
    sys.position(2) = coord_type(0,0,0);
    sys.velocity(0) = coord_type(0,0,0);
    sys.velocity(1) = coord_type(0,0,0);
    sys.velocity(2) = coord_type(0,0,0);
    sys.force(0)    = coord_type(0,0,0);
    sys.force(1)    = coord_type(0,0,0);
    sys.force(2)    = coord_type(0,0,0);

    sys.name(0)  = "X";
    sys.name(1)  = "X";
    sys.name(2)  = "X";
    sys.group(0) = "NONE";
    sys.group(1) = "NONE";
    sys.group(2) = "NONE";

    const int N = 1800;
    const real_type dtheta = mjolnir::math::constants<real_type>::pi()  / N;
    for(int i = 1; i < N; ++i)
    {
        BOOST_TEST(mjolnir::math::length(sys.position(0) - pos1) == 0.0, boost::test_tools::tolerance(tol));
        BOOST_TEST(mjolnir::math::length(sys.position(1) - pos2) == 0.0, boost::test_tools::tolerance(tol));
        BOOST_TEST(mjolnir::math::length(sys.velocity(0))        == 0.0, boost::test_tools::tolerance(tol));
        BOOST_TEST(mjolnir::math::length(sys.velocity(1))        == 0.0, boost::test_tools::tolerance(tol));
        sys.force(0) = coord_type(0,0,0);
        sys.force(1) = coord_type(0,0,0);
        sys.force(2) = coord_type(0,0,0);

        const real_type theta = i * dtheta;
        const coord_type pos3(std::cos(theta), std::sin(theta), 0e0);
        sys.position(2) = pos3;

        const real_type deriv = potential.derivative(theta);
        const real_type coef = std::abs(deriv);

        interaction.calc_force(sys);

        // magnitude
        // if radius == 1e0, then force strength is equal to dV/dtheta.
        BOOST_TEST(mjolnir::math::length(sys.position(0) - sys.position(1)) == 1.0, boost::test_tools::tolerance(tol));
        BOOST_TEST(mjolnir::math::length(sys.position(2) - sys.position(1)) == 1.0, boost::test_tools::tolerance(tol));

        const real_type force_strength1 = mjolnir::math::length(sys.force(0));
        const real_type force_strength3 = mjolnir::math::length(sys.force(2));
        if(i == 1200) // most stable point
        {
            BOOST_TEST(coef == 0.0, boost::test_tools::tolerance(tol));
            BOOST_TEST(coef == 0.0, boost::test_tools::tolerance(tol));
        }
        else
        {
            BOOST_TEST(coef == force_strength1, boost::test_tools::tolerance(tol));
            BOOST_TEST(coef == force_strength3, boost::test_tools::tolerance(tol));
        }

        // direction
        if(i == 1200) // most stable point
        {
            BOOST_TEST(std::abs(force_strength1) == 0.0, boost::test_tools::tolerance(tol));
            BOOST_TEST(std::abs(force_strength3) == 0.0, boost::test_tools::tolerance(tol));
        }
        else if(i < 1200) // narrow
        {
            // perpendicular to radius vector
            const real_type dot1 = mjolnir::math::dot_product(sys.force(0), sys.position(0) - sys.position(1));
            const real_type dot2 = mjolnir::math::dot_product(sys.force(2), sys.position(2) - sys.position(1));
            BOOST_TEST(dot1 == 0.0, boost::test_tools::tolerance(tol));
            BOOST_TEST(dot2 == 0.0, boost::test_tools::tolerance(tol));

            const coord_type f1(0., -1., 0.);
            BOOST_TEST(mjolnir::math::length(normalize(sys.force(0)) - f1) == 0.0, boost::test_tools::tolerance(tol));

            const coord_type f3(
                    cos(theta + mjolnir::math::constants<real_type>::pi() / 2.),
                    sin(theta + mjolnir::math::constants<real_type>::pi() / 2.),
                    0.);
            BOOST_TEST(mjolnir::math::length(normalize(sys.force(2)) - f3) == 0.0, boost::test_tools::tolerance(tol));
        }
        else if(i > 1200) // extensive
        {
            const real_type dot1 = mjolnir::math::dot_product(sys.force(0), sys.position(0) - sys.position(1));
            const real_type dot2 = mjolnir::math::dot_product(sys.force(2), sys.position(2) - sys.position(1));
            BOOST_TEST(dot1 == 0.0, boost::test_tools::tolerance(tol));
            BOOST_TEST(dot2 == 0.0, boost::test_tools::tolerance(tol));

            const coord_type f1(0., 1., 0.);
            BOOST_TEST(mjolnir::math::length(normalize(sys.force(0)) - f1) == 0.0, boost::test_tools::tolerance(tol));

            const coord_type f3(
                    cos(theta - mjolnir::math::constants<real_type>::pi() / 2.),
                    sin(theta - mjolnir::math::constants<real_type>::pi() / 2.),
                    0.);
            BOOST_TEST(mjolnir::math::length(normalize(sys.force(2)) - f3) == 0.0, boost::test_tools::tolerance(tol));
        }

        // perpendicular to z axis
        BOOST_TEST(mjolnir::math::Z(sys.force(0)) == 0.0, boost::test_tools::tolerance(tol));
        BOOST_TEST(mjolnir::math::Z(sys.force(1)) == 0.0, boost::test_tools::tolerance(tol));
        BOOST_TEST(mjolnir::math::Z(sys.force(2)) == 0.0, boost::test_tools::tolerance(tol));
    }
}


#ifndef __PROPULSION_HH__
#define __PROPULSION_HH__
/********************************* TRICK HEADER *******************************
PURPOSE:
      (Describe the propulsion Module Variables and Algorithm)
LIBRARY DEPENDENCY:
      ((../src/Propulsion.cpp))
PROGRAMMERS:
      (((Chun-Hsu Lai) () () () ))
*******************************************************************************/
#include <armadillo>
#include <functional>
#include "aux.hh"
#include "cadac_constants.hh"
#include "datadeck.hh"
#include "vehicle.hh"

class Propulsion : public FH_module
{
    TRICK_INTERFACE(Propulsion);

public:
    Propulsion();

    virtual void init(LaunchVehicle *VehicleIn);
    virtual void algorithm(LaunchVehicle *VehicleIn);

    void load_proptable(const char *filename);

    // XXX: get_thrust_state
    std::function<int()> grab_beco_flag;

private:
    /* Rocket Engine state variables */

    Datadeck proptable;

    /* Internal Propagator / Calculators */
    void propagate_delta_v(LaunchVehicle *VehicleIn);

    /* Internal Calculators */
    // double calculate_thrust(double press);
    double calculate_fmassr(LaunchVehicle *VehicleIn);

    arma::vec3 calculate_XCG(LaunchVehicle *VehicleIn);
    arma::mat33 calculate_IBBB(LaunchVehicle *VehicleIn);
};

#endif  // __PROPULSION_HH__


#include <cassert>
#include <fstream>
#include <iostream>
#include <iterator>
#include <limits>
#include <mutex>
#include <sstream>
#include <string>
#include <thread>
#include <vector>

#include <boost/heap/fibonacci_heap.hpp>

struct Edge {
    const int target;
    const int cost;
    Edge(int target, int cost) : target(target), cost(cost) {}
};

struct Vertex {
    std::mutex mutex;
    std::vector<Edge> edges;
    void add_edge(int target, int cost) {
        std::lock_guard<std::mutex> lock(mutex);
        edges.emplace_back(target, cost);
    }
};

struct DistInfo {
    int v;
    int cost = std::numeric_limits<int>::max();
    int parent = -1;
    bool operator<(const DistInfo& that) const {
        return this->cost > that.cost;
    }
};

struct FilePartition {
    const int threads;
    const ssize_t start, stop, diff, div, mod;
    FilePartition(int threads, ssize_t start, ssize_t stop)
        : threads(threads),
          start(start),
          stop(stop),
          diff(stop - start),
          div(diff / threads),
          mod(diff % threads) {}
    ssize_t operator[](ssize_t tid) const {
        return start + div * tid + std::min(mod, tid);
    }
};

struct IntFile {
    std::ifstream ifs;
    int c = ' ';
    ssize_t g = 0;
    IntFile(const char* filename) : ifs(filename) {}
    operator bool() { return (bool)ifs; }
    IntFile& operator>>(int& other) {
        other = 0;
        while (c == ' ' or c == '\n' or c == '\t') {
            c = ifs.get();
            ++g;
        }
        while ('0' <= c and c <= '9') {
            other *= 10;
            other += c - '0';
            c = ifs.get();
            ++g;
        }
        return *this;
    }
    void seekg(ssize_t pos) {
        ifs.seekg(pos);
        g = pos;
    }
    ssize_t tellg() { return g; }
    void ignore(ssize_t, int chr) {
        while (c != '\n' and c != EOF) {
            ++g;
            c = ifs.get();
        }
    }
};

char* infile;
std::vector<Vertex> vertices;

void read_file(ssize_t from, ssize_t to) {
    // printf("%ld %ld %ld %s\n", from, to, vertices.size(), infile);
    IntFile fin(infile);
    assert(fin);
    fin.seekg(from);
    fin.ignore(std::numeric_limits<std::streamsize>::max(), '\n');
    while (fin.tellg() < to) {
        int u, v, cost;
        fin >> u >> v >> cost;
        fin.ignore(std::numeric_limits<std::streamsize>::max(), '\n');
        --u;
        --v;
        // printf("%d %d %d\n", u, v, cost);
        vertices.at(u).add_edge(v, cost);
        vertices.at(v).add_edge(u, cost);
    }
}

void sincelast(const char* message = 0) {
    static auto last = std::chrono::high_resolution_clock::now();
    auto now = std::chrono::high_resolution_clock::now();
    std::cout << message
              << ((std::chrono::duration<double>)(now - last)).count()
              << std::endl;
    last = now;
}

int main(int argc, char** argv) {
    std::ios_base::sync_with_stdio(false);
    assert(argc == 5);
    auto num_threads = std::stoi(argv[1]);
    assert(num_threads > 0);
    infile = argv[2];
    std::ifstream fin(argv[2]);
    std::ofstream fout(argv[3]);
    auto origin = std::stoi(argv[4]) - 1;
    if (not fin) {
        throw std::runtime_error("bad input file");
    }
    if (not fout) {
        throw std::runtime_error("bad output file");
    }
    int num_vertices, num_edges;
    fin >> num_vertices >> num_edges;
    auto file_start = fin.tellg();
    fin.seekg(0, std::ios_base::end);
    auto file_end = fin.tellg();
    FilePartition fp(num_threads, file_start, file_end);
    vertices = std::vector<Vertex>(num_vertices);
    std::vector<std::thread> rfthreads;
    sincelast("init ");
    for (int i = 0; i < num_threads; ++i) {
        rfthreads.emplace_back(read_file, fp[i] - 1, fp[i + 1]);
    }
    for (auto& thread : rfthreads) {
        thread.join();
    }
    sincelast("input ");
    std::vector<DistInfo> distinfo(num_vertices);
    distinfo[origin].cost = 0;
    distinfo[origin].parent = origin;
    using heap_t = boost::heap::fibonacci_heap<DistInfo>;
    heap_t heap;
    std::vector<heap_t::handle_type> handles;
    handles.reserve(num_vertices);
    for (int v = 0; v < num_vertices; ++v) {
        distinfo[v].v = v;
        handles.emplace_back(heap.push(distinfo[v]));
    }
    while (not heap.empty()) {
        int v = heap.top().v;
        heap.pop();
        const auto& vertex = vertices[v];
        const auto& di = distinfo[v];
        for (const auto& edge : vertex.edges) {
            auto& dst = distinfo[edge.target];
            auto off = di.cost + edge.cost;
            if (dst.cost > off) {
                dst.cost = off;
                dst.parent = v;
                heap.increase(handles[edge.target], dst);
            }
        }
    }
    sincelast("compute ");
    for (int i = 0; i < num_vertices; ++i) {
        std::vector<int> path;
        int cur = i;
        path.push_back(cur + 1);
        do {
            cur = distinfo[cur].parent;
            path.push_back(cur + 1);
        } while (cur != origin);
        std::copy(path.crbegin(), path.crend(),
                  std::ostream_iterator<int>(fout, " "));
        fout << '\n';
    }
    sincelast("output ");
}


/*    Copyright (c) 2010-2015, Delft 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 the Delft 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 HOLDER 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.
 *
 *    Changelog
 *      YYMMDD    Author            Comment
 *      102511    D. Dirkx          First version of file.
 *      110810    J. Leloux         Corrected doxygen documentation.
 *      120328    D. Dirkx          Updated code to use shared_ptrs instead of raw pointers.
 *      120605    J. Vandamme       Boostified unit test.
 *      120825    A. Ronse          Corrected capsule test to check case with angle of attack = 0.
 *                                  Extended spherical test to check for correct settings.
 *      121108    A. Ronse          Updated unit test to new generator architecture, corrected
 *                                  Apollo expected values and tolerances.
 *
 *    References
 *      Gentry, A., Smyth, D., and Oliver, W. . The Mark IV Supersonic-Hypersonic Arbitrary Body
 *        Program, Volume II - Program Formulation, Douglas Aircraft Aircraft Company, 1973.
 *
 *    Notes
 *
 */

#define BOOST_TEST_MAIN

#include <boost/array.hpp>
#include <boost/make_shared.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/unit_test.hpp>

#include <Eigen/Core>

#include "Tudat/Mathematics/BasicMathematics/mathematicalConstants.h"
#include "Tudat/Astrodynamics/Aerodynamics/hypersonicLocalInclinationAnalysis.h"
#include "Tudat/Astrodynamics/Aerodynamics/customAerodynamicCoefficientInterface.h"
#include "Tudat/Mathematics/BasicMathematics/linearAlgebraTypes.h"
#include "Tudat/Mathematics/GeometricShapes/capsule.h"
#include "Tudat/Mathematics/GeometricShapes/sphereSegment.h"

#include "Tudat/Astrodynamics/Aerodynamics/UnitTests/testApolloCapsuleCoefficients.h"

namespace tudat
{
namespace unit_tests
{

using basic_mathematics::Vector6d;
using mathematical_constants::PI;
using std::vector;

using namespace aerodynamics;

BOOST_AUTO_TEST_SUITE( test_aerodynamic_coefficient_generator )

//! Test coefficient generator.
BOOST_AUTO_TEST_CASE( testAerodynamicCoefficientGenerator )
{

    // Set units of coefficients
    const double expectedValueOfForceCoefficient = 1.0;

    // Tolerance in absolute units.
    const double toleranceForceCoefficient = 1.0e-2;
    const double toleranceAerodynamicCoefficients3 = 1.0e-4;
    const double toleranceAerodynamicCoefficients4 = 1.0e-2;
    const double toleranceAerodynamicCoefficients5 = 1.0e-4;

    // Create test sphere.
    boost::shared_ptr< geometric_shapes::SphereSegment > sphere
            = boost::make_shared< geometric_shapes::SphereSegment >( 1.0 );

    // Set vehicle in analysis with 10,000 panels.
    vector< int > numberOfLines( 1, 31 );
    vector< int > numberOfPoints( 1, 31 );
    vector< bool > invertOrder( 1, false );

    // Create analysis object.
    std::vector< std::vector< double > > independentVariableDataPoints;
    independentVariableDataPoints.resize( 3 );

    independentVariableDataPoints[ 0 ] =
            getDefaultHypersonicLocalInclinationMachPoints( "Full" );
    independentVariableDataPoints[ 1 ] =
            getDefaultHypersonicLocalInclinationAngleOfAttackPoints( );
    independentVariableDataPoints[ 2 ] =
            getDefaultHypersonicLocalInclinationAngleOfSideslipPoints( );

    // Set methods to use for aerodynamic analysis.
    std::vector< std::vector< int > > analysisMethod( 2, std::vector< int >( 1, 0 ));
    analysisMethod[ 1 ][ 0 ] = 1;

    // Generate sphere database of aerodynamic coefficients.
    boost::shared_ptr< HypersonicLocalInclinationAnalysis > coefficientInterface =
            boost::make_shared< HypersonicLocalInclinationAnalysis >(
                independentVariableDataPoints, sphere,
                numberOfLines, numberOfPoints,
                invertOrder, analysisMethod, PI, 1.0,
                Eigen::Vector3d::Zero( ) );

    // Test basic properties of coefficient generator
    BOOST_CHECK_EQUAL(
                coefficientInterface->getIndependentVariableNames( ).size( ), 3 );
    BOOST_CHECK_EQUAL( coefficientInterface->getIndependentVariableName( 0 ),
                       mach_number_dependent );
    BOOST_CHECK_EQUAL( coefficientInterface->getIndependentVariableName( 1 ),
                       angle_of_attack_dependent );
    BOOST_CHECK_EQUAL( coefficientInterface->getIndependentVariableName( 2 ),
                       angle_of_sideslip_dependent );

    bool isVariableIndexTooHigh = 0;
    try
    {
        coefficientInterface->getIndependentVariableName( 3 );
    }
    catch ( std::runtime_error )
    {
        isVariableIndexTooHigh = 1;
    }
    BOOST_CHECK_EQUAL( isVariableIndexTooHigh, 1 );

    // Allocate memory for independent variables to pass to analysis for retrieval.
    boost::array< int, 3 > independentVariables;
    independentVariables[ 0 ] = 0;
    independentVariables[ 1 ] = 0;
    independentVariables[ 2 ] = 0;
    std::vector< double > independentVariablesVector( 3 );
    std::vector< double > interpolatingIndependentVariablesVector( 3 );

    // Declare local test variables.
    Vector6d aerodynamicCoefficients_ = Vector6d::Zero( );
    double forceCoefficient_;

    // Iterate over all angles of attack to verify sphere coefficients. Total force coefficient
    // should be one; all moment coefficients should be zero.
    // The functionality is tested directly from the generator, as well as from the
    // coefficient interface, both interpolated at the nodes, and halfway between the nodes.
    for ( int i = 0; i < coefficientInterface->getNumberOfValuesOfIndependentVariable( 0 ); i++ )
    {
        independentVariables[ 0 ] = i;
        independentVariablesVector[ 0 ] = coefficientInterface->getIndependentVariablePoint( 0, i );
        if( i < coefficientInterface->getNumberOfValuesOfIndependentVariable( 0 ) - 1 )
        {
            interpolatingIndependentVariablesVector[ 0 ] =
                    coefficientInterface->getIndependentVariablePoint( 0, i ) + 0.5 * (
                        coefficientInterface->getIndependentVariablePoint( 0, i + 1 ) -
                        coefficientInterface->getIndependentVariablePoint( 0, i ) );
        }

        for ( int j = 0; j <
              coefficientInterface->getNumberOfValuesOfIndependentVariable( 1 ); j++ )
        {
            independentVariables[ 1 ] = j;
            independentVariablesVector[ 1 ] =
                    coefficientInterface->getIndependentVariablePoint( 1, j );
            if( j < coefficientInterface->getNumberOfValuesOfIndependentVariable( 1 ) - 1 )
            {
                interpolatingIndependentVariablesVector[ 1 ] =
                        coefficientInterface->getIndependentVariablePoint( 1, j ) + 0.5 * (
                            coefficientInterface->getIndependentVariablePoint( 1, j + 1 ) -
                            coefficientInterface->getIndependentVariablePoint( 1, j ) );
            }

            for ( int k = 0; k <
                  coefficientInterface->getNumberOfValuesOfIndependentVariable( 2 ); k++ )
            {
                independentVariables[ 2 ] = k;
                independentVariablesVector[ 2 ] =
                        coefficientInterface->getIndependentVariablePoint( 2, k );
                if( k < coefficientInterface->getNumberOfValuesOfIndependentVariable( 2 ) - 1 )
                {
                    interpolatingIndependentVariablesVector[ 2 ] =
                            coefficientInterface->getIndependentVariablePoint( 2, k ) + 0.5 * (
                                coefficientInterface->getIndependentVariablePoint( 2, k + 1 ) -
                                coefficientInterface->getIndependentVariablePoint( 2, k ) );
                }

                // Retrieve aerodynamic coefficients.
                aerodynamicCoefficients_ =
                        coefficientInterface->getAerodynamicCoefficientsDataPoint(
                            independentVariables );
                forceCoefficient_ = ( aerodynamicCoefficients_.head( 3 ) ).norm( );

                // Test if the computed force coefficient corresponds to the expected value
                // within the specified tolerance.
                BOOST_CHECK_CLOSE_FRACTION( forceCoefficient_,
                                            expectedValueOfForceCoefficient,
                                            toleranceForceCoefficient );

                // Test if the computed moment coefficients correspond to the expected value (0.0)
                // within the specified tolerance.
                BOOST_CHECK_SMALL( aerodynamicCoefficients_( 3 ),
                                   toleranceAerodynamicCoefficients3 );

                BOOST_CHECK_SMALL( aerodynamicCoefficients_( 4 ),
                                   toleranceAerodynamicCoefficients4 );

                BOOST_CHECK_SMALL( aerodynamicCoefficients_( 5 ),
                                   toleranceAerodynamicCoefficients5 );

                // Retrieve aerodynamic coefficients from coefficient interface.
                coefficientInterface->updateCurrentCoefficients( independentVariablesVector );

                aerodynamicCoefficients_ =
                        coefficientInterface->getCurrentAerodynamicCoefficients( );
                forceCoefficient_ = ( aerodynamicCoefficients_.head( 3 ) ).norm( );

                // Test if the computed force coefficient corresponds to the expected value
                // within the specified tolerance.
                BOOST_CHECK_CLOSE_FRACTION( forceCoefficient_,
                                            expectedValueOfForceCoefficient,
                                            toleranceForceCoefficient );

                // Test if the computed moment coefficients correspond to the expected value (0.0)
                // within the specified tolerance.
                BOOST_CHECK_SMALL( aerodynamicCoefficients_( 3 ),
                                   toleranceAerodynamicCoefficients3 );

                BOOST_CHECK_SMALL( aerodynamicCoefficients_( 4 ),
                                   toleranceAerodynamicCoefficients4 );

                BOOST_CHECK_SMALL( aerodynamicCoefficients_( 5 ),
                                   toleranceAerodynamicCoefficients5 );

                // Retrieve aerodynamic coefficients from coefficient interface.
                coefficientInterface->updateCurrentCoefficients(
                            interpolatingIndependentVariablesVector );

                aerodynamicCoefficients_ =
                        coefficientInterface->getCurrentAerodynamicCoefficients( );
                forceCoefficient_ = ( aerodynamicCoefficients_.head( 3 ) ).norm( );

                // Test if the computed force coefficient corresponds to the expected value
                // within the specified tolerance.
                BOOST_CHECK_CLOSE_FRACTION( forceCoefficient_,
                                            expectedValueOfForceCoefficient,
                                            toleranceForceCoefficient );

                // Test if the computed moment coefficients correspond to the expected value (0.0)
                // within the specified tolerance.
                BOOST_CHECK_SMALL( aerodynamicCoefficients_( 3 ),
                                   toleranceAerodynamicCoefficients3 );

                BOOST_CHECK_SMALL( aerodynamicCoefficients_( 4 ),
                                   toleranceAerodynamicCoefficients4 );

                BOOST_CHECK_SMALL( aerodynamicCoefficients_( 5 ),
                                   toleranceAerodynamicCoefficients5 );
            }
        }
    }
}

boost::shared_ptr< HypersonicLocalInclinationAnalysis > getApolloCoefficientInterface( )
{

    // Create test capsule.
    boost::shared_ptr< geometric_shapes::Capsule > capsule
            = boost::make_shared< geometric_shapes::Capsule >(
                4.694, 1.956, 2.662, -1.0 * 33.0 * PI / 180.0, 0.196 );

    vector< int > numberOfLines( 4 );
    vector< int > numberOfPoints( 4 );
    vector< bool > invertOrders( 4 );

    // Set number of analysis points.
    numberOfLines[ 0 ] = 31;
    numberOfPoints[ 0 ] = 31;
    numberOfLines[ 1 ] = 31;
    numberOfPoints[ 1 ] = 31;
    numberOfLines[ 2 ] = 31;
    numberOfPoints[ 2 ] = 10;
    numberOfLines[ 3 ] = 11;
    numberOfPoints[ 3 ] = 11;
    invertOrders[ 0 ] = 0;
    invertOrders[ 1 ] = 0;
    invertOrders[ 2 ] = 0;
    invertOrders[ 3 ] = 0;

    Eigen::Vector3d momentReference;
    momentReference( 0 ) = -0.6624;
    momentReference( 1 ) = 0.0;
    momentReference( 2 ) = -0.1369;

    std::vector< std::vector< double > > independentVariableDataPoints( 3 );
    independentVariableDataPoints[ 0 ] = getDefaultHypersonicLocalInclinationMachPoints( "Full" );
    std::vector< double > angleOfAttackPoints( 7 );

    for ( int i = 0; i < 7; i++ )
    {
        angleOfAttackPoints[ i ] = static_cast< double >( i - 6 ) * 5.0 * PI / 180.0;
    }

    independentVariableDataPoints[ 1 ] = angleOfAttackPoints;
    independentVariableDataPoints[ 2 ] =
            getDefaultHypersonicLocalInclinationAngleOfSideslipPoints( );
    std::vector< std::vector< int > > selectedMethods( 2, std::vector< int >( 4 ));

    selectedMethods[ 0 ][ 0 ] = 1;
    selectedMethods[ 0 ][ 1 ] = 5;
    selectedMethods[ 0 ][ 2 ] = 5;
    selectedMethods[ 0 ][ 3 ] = 1;
    selectedMethods[ 1 ][ 0 ] = 6;
    selectedMethods[ 1 ][ 1 ] = 3;
    selectedMethods[ 1 ][ 2 ] = 3;
    selectedMethods[ 1 ][ 3 ] = 3;

    // Create analysis object and capsule database.
    return boost::make_shared< HypersonicLocalInclinationAnalysis >(
                independentVariableDataPoints, capsule, numberOfLines, numberOfPoints,
                invertOrders, selectedMethods, PI * pow( capsule->getMiddleRadius( ), 2.0 ),
                3.9116, momentReference );
}
//! Apollo capsule test case.
BOOST_AUTO_TEST_CASE( testApolloCapsule )
{
    // Set units of coefficients.
    const double expectedValueOfAerodynamicCoefficients0 = -1.51;
    const double expectedValueOfAerodynamicCoefficients4 = -0.052;

    // Tolerance in absolute units.
    const double toleranceAerodynamicCoefficients0 = 0.1;
    const double toleranceAerodynamicCoefficients1 = std::numeric_limits< double >::epsilon( );
    const double toleranceAerodynamicCoefficients2 = std::numeric_limits< double >::epsilon( );
    const double toleranceAerodynamicCoefficients3 = std::numeric_limits< double >::epsilon( );
    const double toleranceAerodynamicCoefficients4 = 0.05;
    const double toleranceAerodynamicCoefficients5 = std::numeric_limits< double >::epsilon( );

    // Create aerodynamic coefficients.
    boost::shared_ptr< HypersonicLocalInclinationAnalysis > coefficientInterface = getApolloCoefficientInterface( );

    // Retrieve coefficients at zero angle of attack for comparison.
    boost::array< int, 3 > independentVariables;

    independentVariables[ 0 ] =
            coefficientInterface->getNumberOfValuesOfIndependentVariable( 0 ) - 1;
    independentVariables[ 1 ] = 6;
    independentVariables[ 2 ] = 0;

    // Declare local test variables.
    Eigen::VectorXd aerodynamicCoefficients_;
    aerodynamicCoefficients_ = coefficientInterface->getAerodynamicCoefficientsDataPoint(
                independentVariables );

    // Compare values to database values.
    BOOST_CHECK_SMALL(
                aerodynamicCoefficients_( 0 ) - expectedValueOfAerodynamicCoefficients0,
                toleranceAerodynamicCoefficients0 );

    BOOST_CHECK_SMALL( aerodynamicCoefficients_( 1 ),
                       toleranceAerodynamicCoefficients1 );

    BOOST_CHECK_SMALL( aerodynamicCoefficients_( 2 ),
                       toleranceAerodynamicCoefficients2 );

    BOOST_CHECK_SMALL( aerodynamicCoefficients_( 3 ),
                       toleranceAerodynamicCoefficients3 );

    BOOST_CHECK_SMALL(
                aerodynamicCoefficients_( 4 ) - expectedValueOfAerodynamicCoefficients4,
                toleranceAerodynamicCoefficients4 );

    BOOST_CHECK_SMALL( aerodynamicCoefficients_( 5 ),
                       toleranceAerodynamicCoefficients5 );

    std::vector< double > independentVariablesVector( 3 );
    independentVariablesVector[ 0 ] = coefficientInterface->getIndependentVariablePoint(
                0, coefficientInterface->getNumberOfValuesOfIndependentVariable( 0 ) - 1 );
    independentVariablesVector[ 1 ] = coefficientInterface->getIndependentVariablePoint(
                1, 6 );
    independentVariablesVector[ 2 ] = coefficientInterface->getIndependentVariablePoint(
                2, 0 );

    coefficientInterface->updateCurrentCoefficients( independentVariablesVector );
    aerodynamicCoefficients_ = coefficientInterface->getCurrentAerodynamicCoefficients( );

    // Compare values to database values.
    BOOST_CHECK_SMALL(
                aerodynamicCoefficients_( 0 ) - expectedValueOfAerodynamicCoefficients0,
                toleranceAerodynamicCoefficients0 );

    BOOST_CHECK_SMALL( aerodynamicCoefficients_( 1 ),
                       toleranceAerodynamicCoefficients1 );

    BOOST_CHECK_SMALL( aerodynamicCoefficients_( 2 ),
                       toleranceAerodynamicCoefficients2 );

    BOOST_CHECK_SMALL( aerodynamicCoefficients_( 3 ),
                       toleranceAerodynamicCoefficients3 );

    BOOST_CHECK_SMALL(
                aerodynamicCoefficients_( 4 ) - expectedValueOfAerodynamicCoefficients4,
                toleranceAerodynamicCoefficients4 );

    BOOST_CHECK_SMALL( aerodynamicCoefficients_( 5 ),
                       toleranceAerodynamicCoefficients5 );
}

BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests
} // namespace tudat


/*************************************************************************
 * SceneML, Copyright (C) 2007, 2008  J.D. Yamokoski
 * All rights reserved.
 * Email: yamokosk at gmail dot com
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public License as
 * published by the Free Software Foundation; either version 2.1 of the License,
 * or (at your option) any later version. The text of the GNU Lesser General
 * Public License is included with this library in the file LICENSE.TXT.
 *
 * This library is distributed in the hope that it will be useful, but WITHOUT
 * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
 * or FITNESS FOR A PARTICULAR PURPOSE. See the file LICENSE.TXT for
 * more details.
 *
 *************************************************************************/
/*
 * Sphere.cpp
 *
 *  Created on: Nov 21, 2008
 *      Author: yamokosk
 */

#include "Sphere.h"
#include <plugin_framework/Plugin.h>
#include <boost/foreach.hpp>

const std::string Sphere::Type("ODESphere");

void* Sphere::create(PF_ObjectParams* params)
{
	Sphere* ptr = new Sphere();
	ptr->services = params->platformServices;
	return ptr;
}

int Sphere::destroy(void *p)
{
	if (!p) return -1;

	delete static_cast<Sphere*>(p);

	return 0;
}

Sphere::Sphere() :
	Geometry()
{

}

Sphere::~Sphere()
{

}

void Sphere::getInfo( ObjectInfo& info ) const
{
	info.type = Type;
	info.addProperty( getProperty("radius") );

	// Have parent class deal with generic ODE properties
	Geometry::getInfo(info);
}

void Sphere::notifyMoved( const Real* translation, const Real* rotation )
{
#ifdef BUG_dGeomSetPosition
	std::cout << "In Sphere::notifyMoved()" << std::endl;
#endif
	dGeomSetPosition(odeobj, (dReal)translation[0], (dReal)translation[1], (dReal)translation[2]);

	dQuaternion q = {0};
	for(unsigned int n=0; n < 4; ++n) q[n] = (dReal)rotation[n];
	dGeomSetQuaternion(odeobj, q);
}

void Sphere::initImpl(const ObjectInfo& info)
{
	// Set object name
	name = info.name;

	odeobj = dCreateSphere (NULL, 1.0);
	dGeomSetData (odeobj, this);
	// If there are any applicable parameters, set them now
	BOOST_FOREACH(Property p, info.parameters)
	{
		setProperty(p);
	}
}

Property Sphere::getPropertyImpl(const std::string& name) const
{
	if ( name == "radius" ) {
		return Property(name, getRadius());
	}

	// Have parent class deal with generic ODE properties
	return Geometry::getPropertyImpl(name);
}

void Sphere::setPropertyImpl(const Property& p)
{
	if ( p.name_str() == "radius" ) {
		setRadius( boost::any_cast<Real>(p.const_value()) );
		return;
	}

	// Have parent class deal with generic ODE properties
	Geometry::setPropertyImpl(p);
}

void Sphere::setRadius(Real r)
{
	dGeomSphereSetRadius(odeobj, (dReal)r);
}

Real Sphere::getRadius() const
{
	return (Real)dGeomSphereGetRadius(odeobj);
}


// Boost.Polygon library voronoi_benchmark.cpp file

//          Copyright Andrii Sydorchuk 2010-2012.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

// See http://www.boost.org for updates, documentation, and revision history.

#include <algorithm>
#include <iomanip>
#include <iostream>
#include <fstream>
#include <list>
#include <numeric>
#include <vector>

#define BOOST_TEST_MODULE benchmark_test
#include <boost/mpl/list.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/test/test_case_template.hpp>
#include <boost/timer.hpp>

#include <boost/polygon/polygon.hpp>
#include <boost/polygon/voronoi.hpp>
using boost::polygon::point_data;
using boost::polygon::segment_data;
using boost::polygon::voronoi_diagram;

typedef boost::mpl::list<int> test_types;
const char *BENCHMARK_FILE = "voronoi_benchmark.txt";
const char *POINT_INPUT_FILE = "input_data/voronoi_point.txt";
const char *SEGMENT_INPUT_FILE = "input_data/voronoi_segment.txt";
const int POINT_RUNS = 10;
const int SEGMENT_RUNS = 10;

boost::mt19937 gen(static_cast<unsigned int>(time(NULL)));
boost::timer timer;

BOOST_AUTO_TEST_CASE_TEMPLATE(benchmark_test_random, T, test_types) {
    typedef T coordinate_type;
    typedef point_data<coordinate_type> point_type;
    voronoi_diagram<double> test_output;

    std::ofstream bench_file(BENCHMARK_FILE, std::ios_base::out | std::ios_base::app);
    bench_file << "Voronoi Benchmark Test (time in seconds):" << std::endl;
    bench_file << "| Number of points | Number of tests | Time per one test |" << std::endl;
    bench_file << std::setiosflags(std::ios::right | std::ios::fixed) << std::setprecision(6);
    int max_points = 100000;
    std::vector<point_type> points;
    coordinate_type x, y;
    for (int num_points = 10; num_points <= max_points; num_points *= 10) {
        points.resize(num_points);
        timer.restart();
        int num_tests = max_points / num_points;
        for (int cur = 0; cur < num_tests; cur++) {
            test_output.clear();
            for (int cur_point = 0; cur_point < num_points; cur_point++) {
                x = static_cast<coordinate_type>(gen());
                y = static_cast<coordinate_type>(gen());
                points[cur_point] = point_type(x, y);
            }
            construct_voronoi(points.begin(), points.end(), &test_output);
        }
        double elapsed_time = timer.elapsed();
        double time_per_test = elapsed_time / num_tests;

        bench_file << "| " << std::setw(16) << num_points << " ";
        bench_file << "| " << std::setw(15) << num_tests << " ";
        bench_file << "| " << std::setw(17) << time_per_test << " ";
        bench_file << "| " << std::endl;
    }
    bench_file.close();
}

BOOST_AUTO_TEST_CASE_TEMPLATE(benchmark_test_points, T, test_types) {
    typedef T coordinate_type;
    typedef point_data<coordinate_type> point_type;

    std::vector<point_type> points;
    {
        std::ifstream input_file(POINT_INPUT_FILE);
        int num_points;
        coordinate_type x, y;
        input_file >> num_points;
        points.reserve(num_points);
        for (int i = 0; i < num_points; ++i) {
            input_file >> x >> y;
            points.push_back(point_type(x, y));
        }
        input_file.close();
    }

    std::vector<double> periods;
    {
        for (int i = 0; i < POINT_RUNS; ++i) {
            voronoi_diagram<double> test_output;
            timer.restart();
            construct_voronoi(points.begin(), points.end(), &test_output);
            periods.push_back(timer.elapsed());
        }
    }
    std::sort(periods.begin(), periods.end());
    // Using olympic system to evaluate average time.
    double elapsed_time =
        std::accumulate(periods.begin() + 2, periods.end() - 2, 0.0);
    elapsed_time /= (periods.size() - 4);

    std::ofstream bench_file(
        BENCHMARK_FILE, std::ios_base::out | std::ios_base::app);
    bench_file << std::setiosflags(std::ios::right | std::ios::fixed) << std::setprecision(6);
    bench_file << "Static test of " << points.size() << " points: " << elapsed_time << std::endl;
    bench_file.close();
}

BOOST_AUTO_TEST_CASE_TEMPLATE(benchmark_test_segments, T, test_types) {
    typedef T coordinate_type;
    typedef point_data<coordinate_type> point_type;
    typedef segment_data<coordinate_type> segment_type;

    std::vector<segment_type> segments;
    {
        std::ifstream input_file(SEGMENT_INPUT_FILE);
        int num_segments;
        coordinate_type x0, y0, x1, y1;
        input_file >> num_segments;
        segments.reserve(num_segments);
        for (int i = 0; i < num_segments; ++i) {
            input_file >> x0 >> y0 >> x1 >> y1;
            point_type p0(x0, y0), p1(x1, y1);
            segments.push_back(segment_type(p0, p1));
        }
        input_file.close();
    }

    std::vector<double> periods;
    {
        for (int i = 0; i < SEGMENT_RUNS; ++i) {
            voronoi_diagram<double> test_output;
            timer.restart();
            construct_voronoi(segments.begin(), segments.end(), &test_output);
            periods.push_back(timer.elapsed());
        }
    }
    std::sort(periods.begin(), periods.end());
    // Using olympic system to evaluate average time.
    double elapsed_time =
        std::accumulate(periods.begin() + 2, periods.end() - 2, 0.0);
    elapsed_time /= (periods.size() - 4);

    std::ofstream bench_file(
        BENCHMARK_FILE, std::ios_base::out | std::ios_base::app);
    bench_file << std::setiosflags(std::ios::right | std::ios::fixed) << std::setprecision(6);
    bench_file << "Static test of " << segments.size() << " segments: " << elapsed_time << std::endl;
    bench_file.close();
}


//
// Created by Alex Beccaro on 18/01/18.
//

#define BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#include "../../src/problems/1-50/44/problem44.hpp"

BOOST_AUTO_TEST_SUITE( Problem44 )

    BOOST_AUTO_TEST_CASE( Solution ) {
        auto res = problems::problem44::solve();
        BOOST_CHECK_EQUAL(res, 5482660);
    }

BOOST_AUTO_TEST_SUITE_END()


/****************************************************************************
 * Copyright (c) 2017-2021 by the ArborX authors                            *
 * All rights reserved.                                                     *
 *                                                                          *
 * This file is part of the ArborX library. ArborX is                       *
 * distributed under a BSD 3-clause license. For the licensing terms see    *
 * the LICENSE file in the top-level directory.                             *
 *                                                                          *
 * SPDX-License-Identifier: BSD-3-Clause                                    *
 ****************************************************************************/

#include <ArborX_DetailsAlgorithms.hpp>
#include <ArborX_KDOP.hpp>

#include "BoostTest_CUDA_clang_workarounds.hpp"
#include <boost/test/unit_test.hpp>

#include <tuple>
#include <type_traits>

using ArborX::Box;
using ArborX::Point;
using ArborX::Experimental::KDOP;

using KDOP_types = std::tuple<KDOP<6>, KDOP<14>, KDOP<18>, KDOP<26>>;

BOOST_AUTO_TEST_SUITE(DiscreteOrientedPolytopes)

BOOST_AUTO_TEST_CASE_TEMPLATE(conversion_to_box, KDOP_t, KDOP_types)
{
  using ArborX::Details::equals;
  KDOP_t x;
  BOOST_TEST(equals((Box)x, Box{}));
  x += Point{0, 1, 0};
  x += Point{1, 0, 1};
  BOOST_TEST(equals((Box)x, Box{{0, 0, 0}, {1, 1, 1}}));
}

BOOST_AUTO_TEST_CASE_TEMPLATE(intersects_KDOP, KDOP_t, KDOP_types)
{
  KDOP_t x;
  BOOST_TEST(!intersects(x, x));
  x += Point{1, 0, 0};
  x += Point{0, 1, 0};
  x += Point{0, 0, 1};
  BOOST_TEST(intersects(x, x));
  BOOST_TEST(!intersects(x, KDOP_t{}));
}

BOOST_AUTO_TEST_CASE_TEMPLATE(intersects_box, KDOP_t, KDOP_types)
{
  KDOP_t x;
  BOOST_TEST(!intersects(x, Box{}));
  BOOST_TEST(!intersects(x, Box{{0, 0, 0}, {1, 1, 1}}));
  x += Point{1, 0, 0};
  x += Point{0, 1, 0};
  x += Point{0, 0, 1};
  BOOST_TEST(!intersects(x, Box{}));
  BOOST_TEST(intersects(x, Box{{0, 0, 0}, {1, 1, 1}}));
}

BOOST_AUTO_TEST_CASE_TEMPLATE(intersects_point, KDOP_t, KDOP_types)
{
  {
    KDOP_t x;
    BOOST_TEST(!intersects(Point{1, 1, 1}, x));
  }
  {
    KDOP_t x; // unit cube with (1,1,0)--(1,1,1) edge chopped away
    // bottom face
    x += Point(0, 0, 0);
    x += Point(1, 0, 0);
    x += Point(1, 0.75, 0);
    x += Point(0.75, 1, 0);
    x += Point(0, 1, 0);
    // top
    x += Point(0, 0, 1);
    x += Point(1, 0, 1);
    x += Point(1, 0.75, 1);
    x += Point(0.75, 1, 1);
    x += Point(0, 1, 1);
    // test intersection with point on the missing edge
    BOOST_TEST(intersects(Point{1, 1, 0.5}, x) ==
               (std::is_same<KDOP_t, KDOP<6>>::value ||
                std::is_same<KDOP_t, KDOP<14>>::value));
    BOOST_TEST(intersects(Point{1, 1, 0.625}, x) ==
               (std::is_same<KDOP_t, KDOP<6>>::value ||
                std::is_same<KDOP_t, KDOP<14>>::value));
    BOOST_TEST(intersects(Point{1, 1, 0.375}, x) ==
               (std::is_same<KDOP_t, KDOP<6>>::value ||
                std::is_same<KDOP_t, KDOP<14>>::value));
    BOOST_TEST(intersects(Point{1, 1, 0.875}, x) ==
               (std::is_same<KDOP_t, KDOP<6>>::value));
    BOOST_TEST(intersects(Point{1, 1, 0.125}, x) ==
               (std::is_same<KDOP_t, KDOP<6>>::value));
    // with both ends of the edge
    BOOST_TEST(intersects(Point{1, 1, 0}, x) ==
               (std::is_same<KDOP_t, KDOP<6>>::value));
    BOOST_TEST(intersects(Point{1, 1, 1}, x) ==
               (std::is_same<KDOP_t, KDOP<6>>::value));
    // with centroid of unit cube
    BOOST_TEST(intersects(Point{0.5, 0.5, 0.5}, x));
    // with some point outside the unit cube
    BOOST_TEST(!intersects(Point{1, 2, 3}, x));
  }
  {
    KDOP_t x; // unit cube with (1,1,1) corner cut off
    // bottom face
    x += Point(0, 0, 0);
    x += Point(1, 0, 0);
    x += Point(1, 1, 0);
    x += Point(0, 1, 0);
    // top
    x += Point(0, 0, 1);
    x += Point(1, 0, 1);
    x += Point(1, 0.75, 1);
    x += Point(0.75, 1, 1);
    x += Point(0, 1, 1);
    // test intersection with center of the missing corner
    BOOST_TEST(intersects(Point{1, 1, 1}, x) ==
               (std::is_same<KDOP_t, KDOP<6>>::value ||
                std::is_same<KDOP_t, KDOP<18>>::value));
    // test with points on the edges out of that corner
    BOOST_TEST(intersects(Point{0.5, 1, 1}, x));
    BOOST_TEST(intersects(Point{0.875, 1, 1}, x) ==
               (std::is_same<KDOP_t, KDOP<6>>::value ||
                std::is_same<KDOP_t, KDOP<18>>::value));
    BOOST_TEST(intersects(Point{1, 0.5, 1}, x));
    BOOST_TEST(intersects(Point{1, 0.875, 1}, x) ==
               (std::is_same<KDOP_t, KDOP<6>>::value ||
                std::is_same<KDOP_t, KDOP<18>>::value));
    BOOST_TEST(intersects(Point{1, 1, 0.5}, x));
    BOOST_TEST(intersects(Point{1, 1, 0.875}, x) ==
               (std::is_same<KDOP_t, KDOP<6>>::value ||
                std::is_same<KDOP_t, KDOP<18>>::value));
    // with centroid of unit cube
    BOOST_TEST(intersects(Point{0.5, 0.5, 0.5}, x));
    // with some point outside the unit cube
    BOOST_TEST(!intersects(Point{1, 2, 3}, x));
  }
}

BOOST_AUTO_TEST_SUITE_END()


// Copyright (c) 2018 by University Paris-Est Marne-la-Vallee
// Constants.hpp
// This file is part of the Garamon for project_namespace.
// Authors: Stephane Breuils and Vincent Nozick
// Contact: vincent.nozick@u-pem.fr
//
// Licence MIT
// A a copy of the MIT License is given along with this program

/// \file Constants.hpp
/// \author Stephane Breuils, Vincent Nozick
/// \brief Constant values and data related to the specified geometric algebra (project_namespace)

// Doxygen
/// \version 0.1
/// \mainpage
/// \tableofcontents
/// \section instroduction_sec What for?
/// Garamon is a C++ library to represent an manipulate the geometric algebra objects.
/// \section install_bigsec How to install
/// \subsection dependencies_sec Dependecies
/// \li cmake (at least version 3.10)
/// \li Eigen (at least version 3.3.4)
/// \li Doxygen (if you want the documentation)
/// \subsection install_sec Install with cmake (Linux / MacOs)
/// \li go to garamon dir
/// \li mkdir build
/// \li cd build
/// \li cmake ..
/// \li make
/// \li (optional) sudo make install
/// \li (optional for documentation) make html
/// \li The documentation is located in [path to build]/doc/doc-doxygen/html/index.html

#ifndef project_inclusion_guard
#define project_inclusion_guard
#pragma once

#include <array>
#include <vector>
#include <utility>
#include <string>
#include <Eigen/Sparse>

#include "project_namespace/Utility.hpp"
#include "project_namespace/BasisTransformations.hpp"
#include "project_namespace/DualCoefficients.hpp"

/*!
 * @namespace project_namespace
 */
namespace project_namespace{

    constexpr unsigned int algebraDimension = project_algebra_dimension; /*!< dimension of the algebra (number of  basis vectors of grade 1) */

    constexpr unsigned int project_per_grade_starting_index;  /*!< array specifying the index of each first element of grade k in the full multivector */

    constexpr unsigned int project_array_binomial_coefficient;  /*!< array of the (dimension + 1) first binomial coefficients */

project_array_xorIndexConversion

project_dual_arrays_permutations_and_coefficients
project_dual_arrays_recursive_coefficients

project_pseudo_scalar_inverse
    const int project_sign_reverse; /*!< array of signs to avoid the computation of (-1)^k*(k-1)/2 during the reverse operation */

    const std::vector<std::string> basisVectors = project_basis_vectors_string; /*!< name of the basis vectors (of grade 1) */

    const std::string metric =
project_metric; /*!< metric / quadratic form of the algebra (inner product between basis vectors) */

    template<class T>
    const T zero = 0;

    const unsigned int scalar = 0;
project_basis_vector_index    /*!< defines the constants for the cga */

project_load_transformation_matrices

    project_diagonal_Metric

}  // namespace

#endif // project_inclusion_guard


///////////////////////////////////////////////////////////////////////////////
// kernel::functional::nw_visitor_unary.hpp                                  //
//                                                                           //
//  Copyright 2009 Erwann Rogard. Distributed under the Boost                //
//  Software License, Version 1.0. (See accompanying file                    //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_KERNEL_ESTIMATION_DETAIL_NW_VISITOR_UNARY_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_KERNEL_ESTIMATION_DETAIL_NW_VISITOR_UNARY_HPP_ER_2009
#include <boost/type_traits.hpp>
#include <boost/mpl/assert.hpp>
#include <boost/call_traits.hpp>
#include <boost/statistics/detail/kernel/estimation/detail/mean_accumulator.hpp>
#include <boost/statistics/detail/kernel/estimation/nw_visitor.hpp>
#include <boost/concept_check.hpp>
#include <boost/concept/assert.hpp>

namespace boost{
namespace statistics{
namespace detail{
namespace kernel{
namespace detail{

// This visitor, g, when called as g(t), forwards to f(f1(t),f2(t)), where f
// is an instance of nw_visitor, and f1(t) and f2(t) are the x and y components.
//
// K, X, A: See rp_visitor
template<
    typename K,
    typename X,
    typename F1,    // x extractor
    typename F2,    // y extractor
    typename A = typename
        statistics::detail::kernel::detail::mean_accumulator<
            typename K::result_type
        >::type
>
class nw_visitor_unary : public nw_visitor<K,X,A>{
    typedef nw_visitor<K,X,A> super_;

    public:
    typedef F1 input_extractor_type;
    typedef F2 output_extractor_type;

    typedef super_ nw_visitor_type;
    typedef typename super_::result_type result_type;

    // Construction
    nw_visitor_unary(){}
    nw_visitor_unary(
        K k,
        typename call_traits<X>::param_type x
    ):super_(k,x){
        BOOST_CONCEPT_ASSERT((
            boost::DefaultConstructible<F1>
        ));
        BOOST_CONCEPT_ASSERT((
            boost::DefaultConstructible<F2>
        ));
    }
    nw_visitor_unary(
        K k,
        typename call_traits<X>::param_type x,
        const A& a
    ):super_(k,x,a)
    {
        BOOST_CONCEPT_ASSERT((
            boost::DefaultConstructible<F1>
        ));
        BOOST_CONCEPT_ASSERT((
            boost::DefaultConstructible<F2>
        ));
    }

    nw_visitor_unary(const nw_visitor_unary& that)
    :super_(that),f1_(that.f1_),f2_(that.f2_){}

    nw_visitor_unary& operator=(const nw_visitor_unary& that)
    {
        if(&that!=this){
            super_::operator=(that);
            this->f1_ = that.f1_;
            this->f2_ = that.f2_;
        }
        return *this;
    }

    // Update
    template<typename T>
    result_type operator()(const T& t)
    {
        const F1& crf1 = this->f1_;
        const F2& crf2 = this->f2_;
        return static_cast<super_&>(*this)(
            crf1(t),
            crf2(t)
        );
    }

    private:
    F1 f1_;
    F2 f2_;

};

}// detail
}// kernel
}// detail
}// statistics
}// boost

#endif


//  Boost Sort library test_pdqsort.cpp file  ----------------------------//

//  Copyright Orson Peters 2017. 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)

//  See http://www.boost.org/libs/sort for library home page.

#include <vector>
#include <string>
#include <random>

#include <boost/sort/pdqsort/pdqsort.hpp>
// Include unit test framework
#include <boost/test/included/test_exec_monitor.hpp>
#include <boost/test/test_tools.hpp>

// Gives a vector containing strings with the same order.
std::string u32_to_str(uint32_t n) {
    std::string r;
    for (int i = 0; i < 32; ++i) {
        r = char('0' + (n & 1)) + r;
        n >>= 1;
    }
    return r;
}

std::vector<std::string> vec_u32_to_str(const std::vector<uint32_t>& v) {
    std::vector<std::string> r; r.reserve(v.size());
    for (size_t i = 0; i < v.size(); ++i) {
        r.push_back(u32_to_str(v[i]));
    }
    return r;
}

// Distributions.
std::vector<uint32_t> shuffled(size_t size, std::mt19937_64& rng) {
    std::vector<uint32_t> v; v.reserve(size);
    for (uint32_t i = 0; i < size; ++i) v.push_back(i);
    std::shuffle(v.begin(), v.end(), rng);
    return v;
}

std::vector<uint32_t> shuffled_16_values(size_t size, std::mt19937_64& rng) {
    std::vector<uint32_t> v; v.reserve(size);
    for (uint32_t i = 0; i < size; ++i) v.push_back(i % 16);
    std::shuffle(v.begin(), v.end(), rng);
    return v;
}

std::vector<uint32_t> all_equal(size_t size, std::mt19937_64& rng) {
    std::vector<uint32_t> v; v.reserve(size);
    for (uint32_t i = 0; i < size; ++i) v.push_back(0);
    return v;
}

std::vector<uint32_t> ascending(size_t size, std::mt19937_64& rng) {
    std::vector<uint32_t> v; v.reserve(size);
    for (uint32_t i = 0; i < size; ++i) v.push_back(i);
    return v;
}

std::vector<uint32_t> descending(size_t size, std::mt19937_64& rng) {
    std::vector<uint32_t> v; v.reserve(size);
    for (uint32_t i = size - 1; ; --i) {
        v.push_back(i);
        if (i == 0) break;
    }
    return v;
}

std::vector<uint32_t> pipe_organ(size_t size, std::mt19937_64& rng) {
    std::vector<uint32_t> v; v.reserve(size);
    for (uint32_t i = 0; i < size/2; ++i) v.push_back(i);
    for (uint32_t i = size/2; i < size; ++i) v.push_back(size - i);
    return v;
}

std::vector<uint32_t> push_front(size_t size, std::mt19937_64& rng) {
    std::vector<uint32_t> v; v.reserve(size);
    for (uint32_t i = 1; i < size; ++i) v.push_back(i);
    v.push_back(0);
    return v;
}

std::vector<uint32_t> push_middle(size_t size, std::mt19937_64& rng) {
    std::vector<uint32_t> v; v.reserve(size);
    for (uint32_t i = 0; i < size; ++i) {
        if (i != size/2) v.push_back(i);
    }
    v.push_back(size/2);
    return v;
}

// Tests.
typedef std::vector<uint32_t> (*DistrF)(size_t, std::mt19937_64&);
void execute_test(DistrF distr, std::string distr_name, int repeats) {
    // In C++14 we'd just use std::less<>().
    std::less<uint32_t> u32less;
    std::greater<uint32_t> u32greater;
    std::less<std::string> sless;
    std::greater<std::string> sgreater;

    for (size_t sz = 1; sz <= 1000; sz *= 10) {
        // Consistent but pseudorandom tests.
        std::mt19937_64 rng; rng.seed(0);

        for (int i = 0; i < repeats; ++i) {
            std::vector<uint32_t> u32l = distr(sz, rng);
            std::vector<uint32_t> u32g = u32l;
            std::vector<std::string> sl = vec_u32_to_str(u32l);
            std::vector<std::string> sg = sl;

            boost::sort::pdqsort(u32l.begin(), u32l.end(), u32less);
            boost::sort::pdqsort(u32g.begin(), u32g.end(), u32greater);
            boost::sort::pdqsort(sl.begin(), sl.end(), sless);
            boost::sort::pdqsort(sg.begin(), sg.end(), sgreater);

            BOOST_CHECK_MESSAGE(
                std::is_sorted(u32l.begin(), u32l.end(), u32less),
                "pdqsort<uint32_t, std::less> " + distr_name + " failed with size " + std::to_string(sz)
            );

            BOOST_CHECK_MESSAGE(
                std::is_sorted(u32g.begin(), u32g.end(), u32greater),
                "pdqsort<uint32_t, std::greater> " + distr_name + " failed with size " + std::to_string(sz)
            );

            BOOST_CHECK_MESSAGE(
                std::is_sorted(sl.begin(), sl.end(), sless),
                "pdqsort<std::string, std::less> " + distr_name + " failed with size " + std::to_string(sz)
            );

            BOOST_CHECK_MESSAGE(
                std::is_sorted(sg.begin(), sg.end(), sgreater),
                "pdqsort<std::string, std::greater> " + distr_name + " failed with size " + std::to_string(sz)
            );
        }
    }
}

// test main
int test_main(int argc, char** argv) {
    // No unused warning.
    (void) argc; (void) argv;

    execute_test(shuffled, "shuffled", 100);
    execute_test(shuffled_16_values, "shuffled_16_values", 100);
    execute_test(all_equal, "all_equal", 1);
    execute_test(ascending, "ascending", 1);
    execute_test(descending, "descending", 1);
    execute_test(pipe_organ, "pipe_organ", 1);
    execute_test(push_front, "push_front", 1);
    execute_test(push_middle, "push_middle", 1);

    return 0;
}