#ifndef SURROUNDINGBASE_HPP
#define SURROUNDINGBASE_HPP

#include <array>
#include <vector>

#include <boost/serialization/array.hpp>
#include <boost/serialization/vector.hpp>

#include <seqan/modifier.h>
#include <seqan/sequence.h>

#include "utilities.hpp"

namespace reseq{
	template<uintSurBlockId N,uintSurPos R, int16_t S, typename I> class SurroundingBase{
	protected:
		static const uintSurBlockId kNumBlocks = N; // Number of surrounding blocks
		static const uintSurPos kRange = R; // Length of a single surrounding block
		static constexpr size_t Size(){
			return 1 << 2*static_cast<size_t>(kRange);
		}

	public:
		typedef I intType;
		static const int16_t kStartPos = S; // Position where the surrounding should start relative to the first base in the fragment (so 10 is 10 bases before the fragment) [0 <= kStartPos < kNumBlocks*kRange]
		static constexpr uintSurPos Length(){
			return kNumBlocks * kRange;
		}

		static uintSeqLen StartPos(uintSeqLen pos, uintSeqLen seq_len){
			return pos+seq_len - kStartPos;
		}
		static uintSeqLen EndPos(uintSeqLen pos, uintSeqLen seq_len){
			return StartPos(pos, seq_len) + Length();
		}

		static uintSeqLen StartPosReverse(uintSeqLen pos, uintSeqLen seq_len){
			return pos+seq_len + kStartPos;
		}
		static uintSeqLen EndPosReverse(uintSeqLen pos, uintSeqLen seq_len){
			return StartPosReverse(pos, seq_len) - Length();
		}

	protected:
		std::array<intType, kNumBlocks> sur_;

		uintSeqLen BlockStartPos(uintSeqLen pos, uintSeqLen seq_len, uintSurBlockId block) const{
			return StartPos(pos, seq_len) + block*kRange;
		}

		uintSeqLen BlockLastPos(uintSeqLen pos, uintSeqLen seq_len, uintSurBlockId block) const{
			return StartPos(pos, seq_len) + (block+1)*kRange - 1;
		}

		uintSeqLen BlockStartPosReverse(uintSeqLen pos, uintSeqLen seq_len, uintSurBlockId block) const{
			return StartPosReverse(pos, seq_len) - block*kRange;
		}

		uintSeqLen BlockLastPosReverse(uintSeqLen pos, uintSeqLen seq_len, uintSurBlockId block) const{
			return StartPosReverse(pos, seq_len) - (block+1)*kRange + 1;
		}

		template<typename T> intType GetBlock(const T &sequence, uintSeqLen start_pos) const{
			intType sur(0);

			for(auto pos = start_pos; pos < start_pos+kRange; ++pos){
				sur <<= 2;
				sur += static_cast<uintBaseCall>(utilities::at(sequence, pos%length(sequence)));
			}

			return sur;
		}

		template<typename T> void Set(const T &sequence, uintSeqLen pos){
			for(auto block=0; block < kNumBlocks; ++block){
				// Add sequence length to avoid negative positions if block reaches over lower sequence ends so it wraps around and start at the end
				// The higher sequence end is covered by the called function
				sur_.at(block) = GetBlock(sequence, BlockStartPos(pos, length(sequence), block));
			}
		}

		bool BlockHasN(intType &invalid_sur, const seqan::Dna5String &sequence, uintSeqLen start_pos){
			for( auto cur_pos = start_pos+kRange; cur_pos-- > start_pos; ){
				if(utilities::IsN(sequence, cur_pos%length(sequence))){
					invalid_sur = static_cast<intType>(start_pos) - cur_pos - 1; // Negative counts of base increments needed to not have an N anymore
					return true; // Last N is only interesting one
				}
			}

			return false;
		}

		void UpdateOnRightSide(seqan::Dna new_base){
			for(auto block=0; block < kNumBlocks-1; ++block){
				sur_.at(block) <<= 2;
				sur_.at(block) %= Size();
				sur_.at(block) += sur_.at(block+1) >> 2*(kRange-1);
			}

			sur_.at(kNumBlocks-1) <<= 2;
			sur_.at(kNumBlocks-1) %= Size();
			sur_.at(kNumBlocks-1) += static_cast<uintBaseCall>(new_base);
		}

		void UpdateOnLeftSide(seqan::Dna new_base){
			for(auto block=kNumBlocks; block-- > 1; ){
				sur_.at(block) >>= 2;
				sur_.at(block) += sur_.at(block-1)%4 << 2*(kRange-1);
			}

			sur_.at(0) >>= 2;
			sur_.at(0) += static_cast<uintBaseCall>(new_base) << 2*(kRange-1);
		}

		void UpdateWithN(const seqan::Dna5String &sequence, uintSeqLen new_pos, bool forward){
			uintBaseCall new_base;
			for(auto block=kNumBlocks; block--; ){
				if(forward){
					new_base = utilities::at( sequence, BlockLastPos(new_pos, length(sequence), block)%length(sequence) );
				}
				else{
					new_base = utilities::Complement::Dna5( utilities::at( sequence, BlockStartPosReverse(new_pos, length(sequence), block)%length(sequence) ) );
				}

				if(utilities::IsN(new_base)){
					sur_.at(block) = -kRange; // Wait kRange positions until calculating surrounding again
				}
				else if(0 > sur_.at(block)){
					if(-1 == sur_.at(block)){
						// Calculate current surrounding
						if(forward){
							sur_.at(block) = GetBlock(sequence, BlockStartPos(new_pos, length(sequence), block));
						}
						else{
							sur_.at(block) = GetBlock(utilities::ConstDna5StringReverseComplement(sequence), BlockStartPos(length(sequence)-new_pos-1, length(sequence), block));
						}
					}
					else{
						++(sur_.at(block));
					}
				}
				else{
					// No N: Update surrounding normally
					if(forward){
						sur_.at(block) = (sur_.at(block) << 2)%Size() + new_base;
					}
					else{
						sur_.at(block) = (sur_.at(block) + new_base*Size()) >> 2;
					}
				}
			}
		}

	public:
		SurroundingBase(){
			sur_.fill(-1);
		}

		SurroundingBase(const SurroundingBase &origin){
			sur_ = origin.sur_;
		}

#ifndef SWIG // To avoid the warning that the operator= will be ignored
		SurroundingBase& operator=(const SurroundingBase& x){
			for(auto block=kNumBlocks; block--; ){
				sur_.at(block) = x.sur_.at(block);
			}
			return *this;
		}

		bool operator==(const SurroundingBase& right) const{
			for(auto block=kNumBlocks; block--; ){
				if( sur_.at(block) != right.sur_.at(block) ){
					return false;
				}
			}
			return true;
		}
#endif //SWIG

		inline uintBaseCall BaseAt(uintSurPos pos) const{
			return (sur_.at(pos/kRange) >> 2*(kRange - pos%kRange - 1)) % 4;
		}

		bool Valid() const{
			for(auto block=kNumBlocks; block--; ){
				if(0 > sur_.at(block)){
					return false;
				}
			}

			return true;
		}

		void Forward(const seqan::Dna5String &sequence, uintSeqLen pos){
			Set(sequence, pos);
		}

		void Reverse(const seqan::Dna5String &sequence, uintSeqLen pos){
			Set(utilities::ConstDna5StringReverseComplement(sequence), length(sequence)-pos-1);
		}

		void ForwardWithN(const seqan::Dna5String &sequence, uintSeqLen pos){
			for(auto block=0; block < kNumBlocks; ++block){
				if( !BlockHasN(sur_.at(block), sequence, BlockStartPos(pos, length(sequence), block)) ){
					sur_.at(block) = GetBlock(sequence, BlockStartPos(pos, length(sequence), block));
				}
			}
		}

		void ReverseWithN(const seqan::Dna5String &sequence, uintSeqLen pos){
			for(auto block=0; block < kNumBlocks; ++block){
				if( !BlockHasN(sur_.at(block), sequence, BlockLastPosReverse(pos, length(sequence), block)) ){
					sur_.at(block) = GetBlock(utilities::ConstDna5StringReverseComplement(sequence), BlockStartPos(length(sequence)-pos-1, length(sequence), block));
				}
			}
		}

		inline void UpdateForward(const seqan::Dna5String &sequence, uintSeqLen new_pos){
			UpdateOnRightSide( utilities::at(sequence, (EndPos(new_pos, length(sequence))-1)%length(sequence)) );
		}

		inline void UpdateReverse(const seqan::Dna5String &sequence, uintSeqLen new_pos){
			UpdateOnLeftSide( utilities::Complement::Dna5( utilities::at(sequence, (StartPosReverse(new_pos, length(sequence)))%length(sequence)) ) );
		}

		inline void RollBackReverse(const seqan::Dna5String &sequence, uintSeqLen new_pos){
			UpdateOnRightSide( utilities::Complement::Dna5( utilities::at(sequence, (EndPosReverse(new_pos, length(sequence))+1)%length(sequence)) ) );
		}

		inline void UpdateForwardWithN(const seqan::Dna5String &sequence, uintSeqLen new_pos){
			UpdateWithN(sequence, new_pos, true);
		}

		inline void UpdateReverseWithN(const seqan::Dna5String &sequence, uintSeqLen new_pos){
			UpdateWithN(sequence, new_pos, false);
		}
	};
}

#endif // SURROUNDINGBASE_HPP


/*    Copyright (c) 2010-2019, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 */

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MAIN

#include <limits>

#include <boost/make_shared.hpp>
#include <memory>
#include <boost/lambda/lambda.hpp>
#include <boost/test/tools/floating_point_comparison.hpp>
#include <boost/test/unit_test.hpp>

#include <Eigen/Core>
#include <Eigen/Geometry>

#include "tudat/basics/testMacros.h"
#include "tudat/astro/aerodynamics/customAerodynamicCoefficientInterface.h"
#include "tudat/astro/aerodynamics/aerodynamicAcceleration.h"
#include "tudat/astro/reference_frames/aerodynamicAngleCalculator.h"
#include "tudat/simulation/propagation_setup/dynamicsSimulator.h"
#include "tudat/interface/spice/spiceEphemeris.h"
#include "tudat/interface/spice/spiceRotationalEphemeris.h"
#include "tudat/io/basicInputOutput.h"
#include "tudat/simulation/environment_setup/body.h"
#include "tudat/simulation/estimation_setup/createNumericalSimulator.h"
#include "tudat/simulation/environment_setup/defaultBodies.h"

namespace tudat
{

namespace unit_tests
{

BOOST_AUTO_TEST_SUITE( test_wind_models )

//! Function to compute a (non-physical) wind vector
Eigen::Vector3d getCustomWindVector(
        const double altitude, const double longitude, const double latitude, const double time )
{
    return ( Eigen::Vector3d( ) << 200.0, -120.0, 75.0 ).finished( ) *
            ( longitude / ( 2.0 * mathematical_constants::PI ) ) *
            ( latitude / mathematical_constants::PI ) * ( altitude - 250.0E3 ) / 10E3 * ( time - 500.0 ) / 500.0;
}

BOOST_AUTO_TEST_CASE( testWindModelInPropagation )
{
    using namespace tudat;
    using namespace aerodynamics;
    using namespace simulation_setup;
    using namespace numerical_integrators;
    using namespace simulation_setup;
    using namespace basic_astrodynamics;
    using namespace propagators;
    using namespace basic_mathematics;
    using namespace basic_astrodynamics;

    //Load spice kernels.
    spice_interface::loadStandardSpiceKernels( );

    for( int test = 0; test < 2; test++ )
    {
        // Create Earth object
        BodyListSettings defaultBodySettings =
                getDefaultBodySettings( { "Earth" }, -1.0E6, 1.0E6 );
        defaultBodySettings.at( "Earth" )->ephemerisSettings = std::make_shared< ConstantEphemerisSettings >(
                    Eigen::Vector6d::Zero( ) );
        if( test == 0 )
        {
            defaultBodySettings.at( "Earth" )->atmosphereSettings->setWindSettings(
                        std::make_shared< CustomWindModelSettings >(
                            std::bind( &getCustomWindVector,
                                       std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4 ),
                            reference_frames::corotating_frame ) );
        }
        else
        {
            defaultBodySettings.at( "Earth" )->atmosphereSettings->setWindSettings(
                        std::make_shared< CustomWindModelSettings >(
                            std::bind( &getCustomWindVector,
                                       std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4 ),
                            reference_frames::vertical_frame ) );
        }
        SystemOfBodies bodies = createSystemOfBodies( defaultBodySettings );

        // Create vehicle object.
        double vehicleMass = 5.0E3;
        bodies.createEmptyBody( "Vehicle" );
        bodies.at( "Vehicle" )->setConstantBodyMass( vehicleMass );

        // Set aerodynamic coefficients.
        std::shared_ptr< AerodynamicCoefficientSettings > aerodynamicCoefficientSettings =
                std::make_shared< ConstantAerodynamicCoefficientSettings >(
                    2.0, 4.0, 1.5, Eigen::Vector3d::Zero( ), Eigen::Vector3d::UnitX( ), Eigen::Vector3d::Zero( ), 1, 1 );
        bodies.at( "Vehicle" )->setAerodynamicCoefficientInterface(
                    createAerodynamicCoefficientInterface( aerodynamicCoefficientSettings, "Vehicle" ) );

        // Define acceleration model settings.
        SelectedAccelerationMap accelerationMap;
        std::vector< std::string > bodiesToPropagate;
        std::vector< std::string > centralBodies;
        std::map< std::string, std::vector< std::shared_ptr< AccelerationSettings > > > accelerationsOfVehicle;
        accelerationsOfVehicle[ "Earth" ].push_back( std::make_shared< AccelerationSettings >( point_mass_gravity ) );
        accelerationsOfVehicle[ "Earth" ].push_back( std::make_shared< AccelerationSettings >( aerodynamic ) );
        accelerationMap[ "Vehicle" ] = accelerationsOfVehicle;
        bodiesToPropagate.push_back( "Vehicle" );
        centralBodies.push_back( "Earth" );

        // Set initial state
        Eigen::Vector6d systemInitialState = Eigen::Vector6d::Zero( );
        systemInitialState( 0 ) = 6.8E6;
        systemInitialState( 4 ) = 7.5E3;

        // Create acceleration models and propagation settings.
        basic_astrodynamics::AccelerationMap accelerationModelMap = createAccelerationModelsMap(
                    bodies, accelerationMap, bodiesToPropagate, centralBodies );

        // Set variables to save
        std::shared_ptr< DependentVariableSaveSettings > dependentVariableSaveSettings;
        std::vector< std::shared_ptr< SingleDependentVariableSaveSettings > > dependentVariables;
        dependentVariables.push_back(
                    std::make_shared< SingleDependentVariableSaveSettings >(
                        altitude_dependent_variable, "Vehicle", "Earth" ) );
        dependentVariables.push_back(
                    std::make_shared< BodyAerodynamicAngleVariableSaveSettings >(
                        "Vehicle", reference_frames::longitude_angle ) );
        dependentVariables.push_back(
                    std::make_shared< BodyAerodynamicAngleVariableSaveSettings >(
                        "Vehicle", reference_frames::latitude_angle ) );
        dependentVariables.push_back(
                    std::make_shared< SingleDependentVariableSaveSettings >(
                        local_density_dependent_variable, "Vehicle", "Earth" ) );
        dependentVariables.push_back(
                    std::make_shared< SingleDependentVariableSaveSettings >(
                        airspeed_dependent_variable, "Vehicle", "Earth" ) );
        dependentVariables.push_back(
                    std::make_shared< SingleDependentVariableSaveSettings >(
                        body_fixed_airspeed_based_velocity_variable, "Vehicle" ) );
        dependentVariables.push_back(
                    std::make_shared< SingleDependentVariableSaveSettings >(
                        body_fixed_groundspeed_based_velocity_variable, "Vehicle" ) );
        dependentVariables.push_back(
                    std::make_shared< SingleAccelerationDependentVariableSaveSettings >(
                        aerodynamic, "Vehicle", "Earth", 0 ) );
        dependentVariableSaveSettings = std::make_shared< DependentVariableSaveSettings >( dependentVariables );

        // Set propagation/integration settings
        std::shared_ptr< PropagationTimeTerminationSettings > terminationSettings =
                std::make_shared< propagators::PropagationTimeTerminationSettings >( 1000.0 );
        std::shared_ptr< TranslationalStatePropagatorSettings< double > > translationalPropagatorSettings =
                std::make_shared< TranslationalStatePropagatorSettings< double > >
                ( centralBodies, accelerationModelMap, bodiesToPropagate, systemInitialState, terminationSettings,
                  cowell, dependentVariableSaveSettings );
        std::shared_ptr< IntegratorSettings< > > integratorSettings =
                std::make_shared< IntegratorSettings< > >
                ( rungeKutta4, 0.0, 5.0 );

        // Create simulation object and propagate dynamics.
        SingleArcDynamicsSimulator< > dynamicsSimulator(
                    bodies, integratorSettings, translationalPropagatorSettings, true, false, false );

        // Retrieve numerical solutions for state and dependent variables
        std::map< double, Eigen::VectorXd > numericalSolution =
                dynamicsSimulator.getEquationsOfMotionNumericalSolution( );
        std::map< double, Eigen::VectorXd > dependentVariableOutput =
                dynamicsSimulator.getDependentVariableHistory( );

        // Get function to transform aerodynamic coefficients to inertial frame
        std::function< Eigen::Vector3d( const Eigen::Vector3d& ) > toPropagationFrameTransformation;
        toPropagationFrameTransformation =
                reference_frames::getAerodynamicForceTransformationFunction(
                    bodies.at( "Vehicle" )->getFlightConditions( )->getAerodynamicAngleCalculator( ),
                    reference_frames::aerodynamic_frame,
                    std::bind( &Body::getCurrentRotationToGlobalFrame, bodies.at( "Earth" ) ),
                    reference_frames::inertial_frame );

        // Iterate over all time steps and compute test quantities
        for( std::map< double, Eigen::VectorXd >::const_iterator dataIterator = dependentVariableOutput.begin( );
             dataIterator != dependentVariableOutput.end( ); dataIterator++ )
        {
            // Update environment
            bodies.at( "Vehicle" )->setState( numericalSolution.at( dataIterator->first ) );
            bodies.at( "Earth" )->setCurrentRotationalStateToLocalFrameFromEphemeris( dataIterator->first );
            bodies.at( "Vehicle" )->getFlightConditions( )->updateConditions( dataIterator->first );

            // Retrieve dependent variables
            double altitude = dataIterator->second( 0 );
            double longitude = dataIterator->second( 1 );
            double latitude = dataIterator->second( 2 );
            double localDensity = dataIterator->second( 3 );
            double airspeed = dataIterator->second( 4 );
            Eigen::Vector3d airspeedBasedVelocity = dataIterator->second.segment( 5, 3 );
            Eigen::Vector3d groundspeedBasedVelocity = dataIterator->second.segment( 8, 3 );
            Eigen::Vector3d aerodynamicAcceleration = dataIterator->second.segment( 11, 3 );
            Eigen::Quaterniond rotationToVerticalFrame =
                    reference_frames::getLocalVerticalToRotatingPlanetocentricFrameTransformationQuaternion(
                        longitude, latitude ).inverse( );

            // Compute wind vector from dependent variables and directly from function.
            Eigen::Vector3d expectedWindVelocity = getCustomWindVector(
                        altitude, longitude, latitude, dataIterator->first );
            Eigen::Vector3d computedWindVector = groundspeedBasedVelocity - airspeedBasedVelocity;
            if( test == 1 )
            {
                computedWindVector = rotationToVerticalFrame * computedWindVector;
            }

            // Manually compute aerodynamic acceleration vector
            Eigen::Vector3d airSpeedVelocityUnitVectorInInertialFrame =
                    reference_frames::transformVectorFunctionFromVectorFunctions(
                        [ & ]( ){ return Eigen::Vector3d::UnitX( ); }, toPropagationFrameTransformation );
            Eigen::Vector3d expectedAerodynamicAcceleration = -0.5 * localDensity * airspeed * airspeed *
                    airSpeedVelocityUnitVectorInInertialFrame * 4.0 / vehicleMass;

            for( unsigned int i = 0; i < 3; i++ )
            {
                // Test wind velocity vector
                BOOST_CHECK_SMALL( std::fabs( expectedWindVelocity( i ) - computedWindVector( i ) ),
                                   systemInitialState( 4 ) * 5.0 * std::numeric_limits< double >::epsilon( ) );

                // Test aerodynamic acceleration unit vector
                BOOST_CHECK_SMALL( std::fabs( airSpeedVelocityUnitVectorInInertialFrame.normalized( )( i ) +
                                              aerodynamicAcceleration.normalized( )( i ) ),
                                   5.0 * std::numeric_limits< double >::epsilon( ) );

                // Test aerodynamic acceleration
                BOOST_CHECK_SMALL( std::fabs( aerodynamicAcceleration( i ) - expectedAerodynamicAcceleration( i ) ),
                                   aerodynamicAcceleration.norm( ) * 5.0 * std::numeric_limits< double >::epsilon( ) );
            }

            // Test airspeed
            BOOST_CHECK_SMALL( std::fabs( airspeedBasedVelocity.norm( ) - airspeed ),
                               systemInitialState( 4 ) * 5.0 * std::numeric_limits< double >::epsilon( ) );

        }
    }
}

BOOST_AUTO_TEST_SUITE_END( )

}

}


// Copyright 2014 Robert Horvath, Johannes Vogel
#include <stdio.h>
#include <stdlib.h>
#include <signal.h>
#include <boost/program_options.hpp>
#include <string>
#include <ctime>
#include <iostream>

#include "./instance.h"
#include "./solution.h"
#include "./greedy.h"
#include "./randomGreedy.h"
#include "./complexMoveHeuristic.h"

static Solution* best_solution;
static int best_solution_time = 0;
static time_t last_interrupt = 0;
static float alpha = 0.4;
static int timeout = 10;
static bool randomized = false;
static bool best_improvement = false;
static time_t global_start_time;

void interrupt_handler(int signum) {
    fprintf(stderr, "Caught signal %d\n", signum);
    bool stop = (time(0) - last_interrupt) < 1;
    if (best_solution) {
        best_solution->print(stop ? stdout : stderr);
    }
    if (stop) exit(0);
    last_interrupt = time(0);
}

void set_best_solution(Solution* solution) {
    if (best_solution && best_solution->length <= solution->length) {
        return;
    }
    best_solution_time = time(0);
    if (best_solution && best_solution != solution) {
        delete best_solution;
    }
    best_solution = solution->clone();
}

Solution* vnd(Solution* solution) {
    // Perform Variable Neighborhood Descent
    int current_length = solution->length;
    fprintf(stderr, "Current solution length: %i\n", current_length);
    time_t start_time = time(0);
    time_t time_to_stop = start_time + timeout;

    while (complexMove(solution, time_to_stop, best_improvement)) {
        solution->trim();
        current_length = solution->length;
        fprintf(stderr, "Current solution length: %i (%lus / %lus)\n",
            current_length, time(0)-start_time, time(0)-global_start_time);
        time_to_stop = time(0) + timeout;

        set_best_solution(solution);
    }
    return solution;
}

Solution* grasp(Instance* instance) {
    for (int i = 0; i < 30; i++) {
        Solution* solution = randomGreedy(instance, alpha);

        fprintf(stderr, "Greedy Solution Nr. %i:\n", i);
        solution->print(stderr);

        solution = vnd(solution);
        solution->print(stderr);
    }
    fprintf(stderr, "Best solution found in %lus, total runtime %lus:\n",
        best_solution_time-global_start_time, time(0)-global_start_time);
    best_solution->print();
    return best_solution;
}

/// Normal old solving, no GRASP
void solve(Instance* instance) {
    Solution* solution;

    if (randomized) {
        solution = randomGreedy(instance, alpha);
    } else {
        solution = greedy(instance);
    }

    fprintf(stderr, "Greedy Solution:\n");
    solution->print(stderr);
    set_best_solution(solution);
    signal(SIGINT, interrupt_handler);

    solution = vnd(solution);
    fprintf(stderr, "Best solution found in %lus, total runtime %lus:\n",
        best_solution_time-global_start_time, time(0)-global_start_time);
    solution->print();
}

int main(int argc, char** argv) {
    namespace po = boost::program_options;
    po::options_description description
        ("Time-Constrained Bepartite Vehicle Routing Problem Solver");

    description.add_options()
        ("timeout,t", po::value<int>()->implicit_value(10),
            "Timeout after new solution has been found")
        ("randomized,r", "Use a randomized construction heuristic")
        ("alpha,a", po::value<float>(), "Alpha value for random construction heuristic [0-1]")
        ("grasp,g", "Use GRASP")
        ("best-improvement,b", "Use best-improvement instead of first-improvement")
        ("help,h", "Display this help message")
        ("instance,i", po::value<std::string>(), "Instance file")
        ("version,v", "Display the version number");

    po::positional_options_description p;
    p.add("instance", 1);
    po::variables_map vm;
    try {
        po::store(po::command_line_parser(argc, argv).options(description).positional(p).run(), vm);
        po::notify(vm);
    } catch(boost::program_options::error& e) {
        std::cout << description;
        exit(1);
    }

    if (vm.count("version")) {
        printf("1.0\n");
        exit(0);
    } else if (vm.count("help") || !vm.count("instance")) {
        std::cout << description;
        exit(0);
    }

    std::string file = vm["instance"].as<std::string>();
    Instance* instance = ReadInstanceFile(file.c_str());
    instance->print_summary();

    if (vm.count("timeout")) {
        timeout = vm["timeout"].as<int>();
    } else if (vm.count("grasp")) {
        timeout = instance->num_nodes / 2;
    } else {
        timeout = 3 * instance->num_nodes;
    }
    fprintf(stderr, "Timeout: %is\n", timeout);

    if (vm.count("randomized")) {
        randomized = true;
    }

    if (vm.count("alpha")) {
        alpha = vm["alpha"].as<float>();
        if (alpha < 0) alpha = 0;
        else if (alpha > 1) alpha = 1;
    }

    if (vm.count("best-improvement")) {
        best_improvement = true;
    }

    global_start_time = time(0);
    if (vm.count("grasp")) {
        grasp(instance);
    } else {
        solve(instance);
    }

    return 0;
}


/*
@copyright Louis Dionne 2015
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#ifndef BOOST_HANA_EXAMPLE_CPPCON_2014_MATRIX_GROUP_HPP
#define BOOST_HANA_EXAMPLE_CPPCON_2014_MATRIX_GROUP_HPP

#include "matrix.hpp"

#include <boost/hana/concept/group.hpp>

#include <utility>

namespace boost { namespace hana {
    template <unsigned R, unsigned C>
    struct minus_impl<cppcon::Matrix<R, C>, cppcon::Matrix<R, C>> {
        template <typename M1, typename M2>
        static constexpr decltype(auto) apply(M1&& m1, M2&& m2) {
            return element_wise(minus)(
                std::forward<M1>(m1),
                std::forward<M2>(m2)
            );
        }
    };
}}

#endif // !BOOST_HANA_EXAMPLE_CPPCON_2014_MATRIX_GROUP_HPP


// Last update: April 5, 2013
// This is main.
// This is MD simulation of an electrolyte confined within planar walls
// Problem : Compute density profiles of ions trapped within planar walls
/* Useful studies :
		     1. Role of dielectric contrast
		     2. Role of valency of ions
		     3. Role of varying salt concentration
// To implement:
		     1. Charged walls

*/
/* lessons learned :
		     1. do not use exit(1) to exit the program. this leads to memory leaks.

*/

#include <boost/program_options.hpp>
#include <time.h>
#include "utility.h"
#include "interface.h"
#include "particle.h"
#include "vertex.h"
#include "BIN.h"
#include "control.h"
#include "functions.h"
#include "precalculations.h"
#include "thermostat.h"
#include "fmd.h"

//MPI boundary parameters
unsigned int lowerBoundIons;
unsigned int upperBoundIons;
unsigned int sizFVecIons;
unsigned int lowerBoundMesh;
unsigned int upperBoundMesh;
unsigned int sizFVecMesh;

mpi::environment env;
mpi::communicator world;

void
cpmd(vector<PARTICLE> &, INTERFACE &, vector<THERMOSTAT> &, vector<THERMOSTAT> &, vector<BIN> &, CONTROL &, CONTROL &);
// removed particle colloid, fake thermostat and fmdremote

int main(int argc, char *argv[]) {

    // Electrostatic system variables
    double bx, by, bz;        // lengths of the box
    double ein;            // permittivity of inside medium
    double e_left;                // permittivity of leftside medium
    double e_right;               // permittivity of rightside medium
    int pz_in;            // positive valency of ions inside
    int nz_in;            // negative valency of ions inside
    double salt_conc_in;        // salt concentration outside	(enter in M)
    double saltion_diameter_in;    // inside salt ion diameter	(positive and negative ions assumed to have same diameter at this point)
    double T, fake_T;            // temperature at which the system of ions is

    // Simulation related variables
    double fraction_diameter;        // fraction that multiplies the diameter to generate the discretization width for the interface
    double Q, fake_Q;                // thermostat mass required to generate canonical ensemble
    unsigned int chain_length_real, chain_length_fake;
    double bin_width;            // width of the bins used to compute density profiles
    CONTROL fmdremote, cpmdremote;            // remote control for cpmd

    // Different parts of the system
    vector<PARTICLE> saltion_in;        // salt ions inside
    vector<PARTICLE> ion;        // all ions in the system
    INTERFACE box;            // interface, z planes hard walls; rest periodic boundaries

    // Analysis
    vector<BIN> bin;            // bins

    // Fake system
    double fakemass;

    // Get input values from the user
    options_description desc("Usage:\nrandom_mesh <options>");
    desc.add_options()
            ("help,h", "print usage message")
            ("bx,X", value<double>(&bx)->default_value(22.848),
             "box length in x direction in nanometers")                    // enter in nanometers
            ("by,Y", value<double>(&by)->default_value(22.848),
             "box length in y direction in nanometers")                    // enter in nanometers
            ("bz,Z", value<double>(&bz)->default_value(3),
             "box length in z direction in nanometers")                    // enter in nanometers
            ("epsilon_in,e", value<double>(&ein)->default_value(80.1), "dielectric const inside")
            ("epsilon_left,l", value<double>(&e_left)->default_value(20), "dielectric const leftside")
            ("epsilon_right,r", value<double>(&e_right)->default_value(80.1), "dielectric const rightside")
            ("pz_in,p", value<int>(&pz_in)->default_value(1), "positive valency inside")
            ("nz_in,n", value<int>(&nz_in)->default_value(-1), "negative valency inside")
            ("salt_conc_in,c", value<double>(&salt_conc_in)->default_value(0.1), "salt concentration inside")
            ("saltion_diameter_in,d", value<double>(&saltion_diameter_in)->default_value(0.714),
             "salt ion diameter inside")        // enter in nanometers
            ("fraction_diameter,g", value<double>(&fraction_diameter)->default_value(1.4),
             "for interface discretization width")
            ("thermostat_mass,Q", value<double>(&Q)->default_value(1.0), "thermostat mass")
            ("chain_length_real,L", value<unsigned int>(&chain_length_real)->default_value(5),
             "chain length for real system: enter L+1 if you want L thermostats")
            ("fake_temperature,k", value<double>(&fake_T)->default_value(0.002), "fake temperature")
            ("fake_thermostat_mass,q", value<double>(&fake_Q)->default_value(1.0), "fake thermostat mass")
            ("chain_length_fake,D", value<unsigned int>(&chain_length_fake)->default_value(5),
             "chain length for fake system: enter L+1 if you want L thermostats")
            ("fakemass,f", value<double>(&fakemass)->default_value(100.0), "fake mass")
            ("cpmd_fake_mass,M", value<double>(&cpmdremote.fakemass)->default_value(1.0), "cpmd fake mass")
            ("bin_width,B", value<double>(&bin_width)->default_value(0.1), "bin width")
            ("cpmd_timestep,T", value<double>(&cpmdremote.timestep)->default_value(0.0005), "time step used in cpmd")
            ("fmd_steps,s", value<int>(&fmdremote.steps)->default_value(200), "steps used in fmd")
            ("cpmd_steps,S", value<int>(&cpmdremote.steps)->default_value(1000), "steps used in cpmd")
            ("cpmd_eqm,P", value<int>(&cpmdremote.hiteqm)->default_value(10), "production begin (cpmd)")
            ("cpmd_freq,F", value<int>(&cpmdremote.freq)->default_value(10), "sample frequency (cpmd)")
            ("cpmd_verify,y", value<int>(&cpmdremote.verify)->default_value(1000), "verify (cpmd)")
            ("cpmd_extra_compute,x", value<int>(&cpmdremote.extra_compute)->default_value(10),
             "compute additional (cpmd)")
            ("cpmd_writedensity,w", value<int>(&cpmdremote.writedensity)->default_value(100), "write density files")
            ("cpmd_movie_freq,m", value<int>(&cpmdremote.moviefreq)->default_value(100), "compute additional (cpmd)");

    variables_map vm;
    store(parse_command_line(argc, argv, desc), vm);
    notify(vm);
    if (vm.count("help")) {
        std::cout << desc << "\n";
        return 0;
    }

    // Set up the system
    T = 1;        // set temperature
    box.ein = ein;
    box.e_left = e_left;
    box.e_right = e_right;
    box.set_up(salt_conc_in, 0, 0, pz_in, 0, 0, bx / unitlength, by / unitlength, bz / unitlength);
    box.put_saltions_inside(saltion_in, pz_in, nz_in, salt_conc_in, saltion_diameter_in, ion);
    // box.put_one_saltion_in_center(saltion_in, pz_in, nz_in, saltion_diameter_in, ion);						//put one ion in center

    make_bins(bin, box, bin_width);    // set up bins to be used for computing density profiles
    vector<double> initial_density;
    bin_ions(ion, box, initial_density, bin);    // bin the ions to get initial density profile

//   box.number_of_vertices = total_gridpoints;
    box.discretize(saltion_diameter_in / unitlength, fraction_diameter);

    fmdremote.fakemass = fakemass;

    // output to screen the parameters of the problem
    if (world.rank() == 0) {
        cout << "\nProgram starts\n";
        cout << "Dielectric constant of water is " << epsilon_water << endl;
        cout << "Reduced units: scalefactor entering in Coloumb interaction is " << scalefactor << endl;
        cout << "Box dimensions x | y | z " << setw(15) << box.lx << setw(15) << box.ly << setw(15) << box.lz << endl;
        cout << "Permittivity inside " << box.ein << endl;
        cout << "Permittivity leftside " << box.e_left << endl;
        cout << "Permittivity rightside " << box.e_right << endl;
        cout << "Contrast strength left " << 2 * (box.e_left - box.ein) / (box.e_left + box.ein) << endl;
        cout << "Contrast strength right " << 2 * (box.e_right - box.ein) / (box.e_right + box.ein) << endl;
        cout << "Positive ion valency inside " << pz_in << endl;
        cout << "Negative ion valency inside " << nz_in << endl;
        cout << "Salt ion diameter inside " << saltion_diameter_in / unitlength << endl;
        cout << "Salt concentration inside " << salt_conc_in << endl;
        cout << "Debye length inside " << box.inv_kappa_in << endl;
        cout << "Mean separation inside " << box.mean_sep_in << endl;
        cout << "Number of salt ions inside " << saltion_in.size() << endl;
        cout << "Temperature " << T << endl;
        cout << "Binning width (uniform) " << bin[0].width << endl;
        cout << "Number of bins " << bin.size() << endl;
        cout << "Number of points discretizing the left and right z planes " << box.leftplane.size() << setw(10)
             << box.rightplane.size() << endl;
    }

    int numOfNodes = world.size();
    if (world.rank() == 0) {
#pragma omp parallel default(shared)
        {
            if (omp_get_thread_num() == 0) {
                printf("The app comes with MPI and OpenMP (Hybrid) parallelization)\n");
                printf("Number of MPI processes used %d\n", numOfNodes);
                printf("Number of OpenMP threads per MPI process %d\n", omp_get_num_threads());
                printf("Make sure that number of grid points / ions is greater than %d\n",
                       omp_get_num_threads() * numOfNodes);
            }
        }
    }

    // write to files

    // initial configuration
    if (world.rank() == 0) {
        ofstream initial_configuration("outfiles/initialconfig.dat");
        for (unsigned int i = 0; i < ion.size(); i++)
            initial_configuration << "ion" << setw(5) << ion[i].id << setw(15) << "charge" << setw(5) << ion[i].q
                                  << setw(15) << "position" << setw(15) << ion[i].posvec << endl;
        initial_configuration.close();

        // initial density
        ofstream density_profile("outfiles/initial_density_profile.dat", ios::out);
        for (unsigned int b = 0; b < initial_density.size(); b++)
            density_profile << bin[b].lower << setw(15) << initial_density.at(b) << endl;
        density_profile.close();
    }
    // check point
    double totalions = 0;
    for (unsigned int b = 0; b < initial_density.size(); b++)
        totalions += initial_density.at(b) * bin[b].volume;
    if (world.rank() == 0)
        cout << "total ions " << totalions << "  should be " << ion.size() << endl;
    int totalpions = 0, totalnions = 0;
    for (unsigned int i = 0; i < ion.size(); i++) {
        if (ion[i].valency > 0)
            totalpions += 1;
        else if (ion[i].valency < 0)
            totalnions += 1;
    }

    // some calculations before simulation begins
    double charge = box.total_charge_inside(ion);

    if (world.rank() == 0) {
        cout << "Total positive ions " << totalpions << endl;
        cout << "Total negative ions " << totalnions << endl;
        cout << "Total charge inside" << charge << endl;
    }
    // prepare for cpmd : make real baths
    vector<THERMOSTAT> real_bath;
    if (chain_length_real == 1)
        real_bath.push_back(THERMOSTAT(0, T, 3 * ion.size(), 0.0, 0, 0));
    else {
        real_bath.push_back(THERMOSTAT(Q, T, 3 * ion.size(), 0, 0, 0));
        while (real_bath.size() != chain_length_real - 1)
            real_bath.push_back(THERMOSTAT(Q / (3 * ion.size()), T, 1, 0, 0, 0));
        real_bath.push_back(THERMOSTAT(0, T, 3 * ion.size(), 0.0, 0, 0));
        // final bath is dummy bath (dummy bath always has zero mass)
    }

    vector<THERMOSTAT> fake_bath;
    if (chain_length_fake == 1)
        fake_bath.push_back(THERMOSTAT(0, fake_T, box.leftplane.size(), 0.0, 0, 0));
    else {
        fake_bath.push_back(THERMOSTAT(fake_Q, fake_T, box.leftplane.size(), 0, 0, 0));
        while (fake_bath.size() != chain_length_fake - 1)
            fake_bath.push_back(THERMOSTAT(fake_Q / box.leftplane.size(), fake_T, 1, 0, 0, 0));
        fake_bath.push_back(THERMOSTAT(0, fake_T, box.leftplane.size(), 0.0, 0,
                                       0));            // finally, the coding trick: dummy bath (dummy bath always has zero mass)
    }

    // Induced charge system
    // fake positions initialized
    for (unsigned int i = 0; i < box.leftplane.size(); i++)
        box.leftplane[i].w = 0.0;
    double total_ind_charge_left = 0;
    for (unsigned int i = 0; i < box.leftplane.size(); i++)
        total_ind_charge_left += box.leftplane[i].w * box.leftplane[i].a;

    for (unsigned int i = 0; i < box.rightplane.size(); i++)
        box.rightplane[i].w = 0.0;
    double total_ind_charge_right = 0;
    for (unsigned int i = 0; i < box.rightplane.size(); i++)
        total_ind_charge_right += box.rightplane[i].w * box.rightplane[i].a;

    if (world.rank() == 0) {
        cout << "total induced left interface" << total_ind_charge_left << endl;
        cout << "total induced right interface" << total_ind_charge_right << endl;
    }

    clock_t t;
    t = clock();
    precalculate(box); // calculate the Greens function within interface or between interface
    t = clock() - t;
    //exact incuded charge density calculation
    //vector<VERTEX> exact_induced_density_left;
    //vector<VERTEX> exact_induced_density_right;

    //exact_induced_density_left = box.compute_exact_induced_density(ion,'l');
    //exact_induced_density_right = box.compute_exact_induced_density(ion,'r');
    //double total_energy = box.exact_total_energy(exact_induced_density_left, exact_induced_density_right, ion);
    //cout << "total energy should be " << total_energy << endl;
    if (world.rank() == 0)
        cout << "pre_calculation finished,  time taken: " << ((double) t) / CLOCKS_PER_SEC << " seconds" << endl;

    // Fictitious Molecular Dynamics

    t = clock();
    fmdremote.timestep = 0.0005;
    fmdremote.verify = 0;
    fmdremote.anneal = 'y';
    fmd(ion, box, fmdremote, cpmdremote);
    t = clock() - t;
    if (world.rank() == 0)
        cout << "fmd calculation finished,  time taken: " << ((double) t) / CLOCKS_PER_SEC << " seconds" << endl;
/*
  ofstream induced_density_left ("outfiles_windows/induced_density_left.dat");
  for (unsigned int k = 0; k < box.leftplane.size(); k++)
    induced_density_left << k+1 <<setw(15) << box.leftplane[k].posvec << setw(15) <<
    sqrt(box.leftplane[k].posvec.x * box.leftplane[k].posvec.x + box.leftplane[k].posvec.y * box.leftplane[k].posvec.y) << setw(15) <<
    box.leftplane[k].w << setw(15) << box.leftplane[k].wmean << setw(15)<< exact_induced_density_left[k].w << endl;		// NOTE plotting also the cartesian coordinates
  induced_density_left.close();
  ofstream induced_density_right("outfiles_windows/induced_density_right.dat");
  for (unsigned int k = 0; k < box.rightplane.size(); k++)
    induced_density_right << k+1 <<setw(15) << box.rightplane[k].posvec << setw(15) <<
    sqrt(box.rightplane[k].posvec.x * box.rightplane[k].posvec.x + box.rightplane[k].posvec.y * box.rightplane[k].posvec.y) << setw(15) <<
    box.rightplane[k].w << setw(15) << box.rightplane[k].wmean << setw(15)<< exact_induced_density_right[k].w << endl;		// NOTE plotting also the cartesian coordinates
  induced_density_right.close();
 */

    //MPI Boundary calculations for ions
    unsigned int rangeIons = (ion.size() + world.size() - 1) / (1.0 * world.size());
    lowerBoundIons = world.rank() * rangeIons;
    upperBoundIons = (world.rank() + 1) * rangeIons - 1;
    sizFVecIons = rangeIons;
    if (world.rank() == world.size() - 1) {
        upperBoundIons = ion.size() - 1;
    }

    //MPI Boundary calculations for meshPoints
    unsigned int rangeMesh = (box.leftplane.size() + world.size() - 1) / (1.0 * world.size());
    lowerBoundMesh = world.rank() * rangeMesh;
    upperBoundMesh = (world.rank() + 1) * rangeMesh - 1;
    sizFVecMesh = rangeMesh;
    if (world.rank() == world.size() - 1) {
        upperBoundMesh = box.leftplane.size() - 1;
    }

    // Car-Parrinello Molecular Dynamics
    cpmd(ion, box, real_bath, fake_bath, bin, fmdremote, cpmdremote);

//   // Post simulation analysis (useful for short runs, but performed otherwise too)
    //   auto_correlation_function();
    if (world.rank() == 0) {
        cout << "MD trust factor R (should be < 0.05) is " << compute_MD_trust_factor_R(cpmdremote.hiteqm) << endl;
        cout << "Program ends \n\n";
    }

    return 0;
}
// End of main

// useful codes

/* random number generation code from gsl
int N = 10;  // number of random numbers to generate

// random nmbr declarations

const gsl_rng_type * T;
gsl_rng * r;

T = gsl_rng_default;
r = gsl_rng_alloc (T);

// prepare for seeding of gsl random nmbr generator:

//   srand((time(0)));                // srand & time are built-in
//   unsigned long int s = random();  // gsl_rng_uniform will eventually
unsigned long int s = 23897897;  // gsl_rng_uniform will eventually
                                 // want a non-negative "long" integer

gsl_rng_env_setup();
gsl_rng_set(r,s); // seed the random number generator;

// generate N random numbers:

for(int i = 0; i < N; ++i) {
  cout << " random number = " << gsl_rng_uniform (r) << endl;;
  }

gsl_rng_free (r);

return 0;
*/


#include <iostream>
#include <stdio.h>
#include <string>
#include <zlib.h>

#include "kseq.h"

#ifdef _OPENMP
#include <omp.h>
#endif

#include <boost/dynamic_bitset.hpp>

struct SparseDist
{
    std::vector<int> rows;
    std::vector<int> cols;
    std::vector<double> distances;
    std::vector<std::string> seq_names;
};

template <typename T>
std::vector<T> combine_vectors(const std::vector<std::vector<T> > &vec,
                               const size_t len)
{
    std::vector<T> all(len);
    auto all_it = all.begin();
    for (size_t i = 0; i < vec.size(); ++i)
    {
        std::copy(vec[i].cbegin(), vec[i].cend(), all_it);
        all_it += vec[i].size();
    }
    return all;
}

KSEQ_INIT(gzFile, gzread)

SparseDist pairsnp(std::string fasta, int n_threads, int dist, int knn)
{
    // open filename and initialise kseq
    int l;
    gzFile fp = gzopen(fasta.c_str(), "r");
    kseq_t *seq = kseq_init(fp);

    size_t n_seqs = 0;
    size_t seq_length = 0;

    // initialise bitmaps
    std::vector<std::string> seq_names;
    std::vector<boost::dynamic_bitset<> > A_snps;
    std::vector<boost::dynamic_bitset<> > C_snps;
    std::vector<boost::dynamic_bitset<> > G_snps;
    std::vector<boost::dynamic_bitset<> > T_snps;

    while (true)
    {
        l = kseq_read(seq);

        if (l == -1) // end of file
            break;
        if (l == -2)
        {
            throw std::runtime_error("Error reading FASTA!");
        }
        if (l == -3)
        {
            throw std::runtime_error("Error reading FASTA!");
        }

        // check sequence length
        if ((n_seqs > 0) && (seq->seq.l != seq_length))
        {
            throw std::runtime_error(
                "Error reading FASTA, variable sequence lengths!");
        }
        seq_length = seq->seq.l;

        seq_names.push_back(seq->name.s);
        boost::dynamic_bitset<> As(seq_length);
        boost::dynamic_bitset<> Cs(seq_length);
        boost::dynamic_bitset<> Gs(seq_length);
        boost::dynamic_bitset<> Ts(seq_length);

        for (size_t j = 0; j < seq_length; j++)
        {

            seq->seq.s[j] = std::toupper(seq->seq.s[j]);

            switch (seq->seq.s[j])
            {
            case 'A':
                As[j] = 1;
                break;
            case 'C':
                Cs[j] = 1;
                break;
            case 'G':
                Gs[j] = 1;
                break;
            case 'T':
                Ts[j] = 1;
                break;

            // M = A or C
            case 'M':
                As[j] = 1;
                Cs[j] = 1;
                break;

            // R = A or G
            case 'R':
                As[j] = 1;
                Gs[j] = 1;
                break;

            // W = A or T
            case 'W':
                As[j] = 1;
                Ts[j] = 1;
                break;

            // S = C or G
            case 'S':
                Cs[j] = 1;
                Gs[j] = 1;
                break;

            // Y = C or T
            case 'Y':
                Cs[j] = 1;
                Ts[j] = 1;
                break;

            // K = G or T
            case 'K':
                Gs[j] = 1;
                Ts[j] = 1;
                break;

            // V = A,C or G
            case 'V':
                As[j] = 1;
                Cs[j] = 1;
                Gs[j] = 1;
                break;

            // H = A,C or T
            case 'H':
                As[j] = 1;
                Cs[j] = 1;
                Ts[j] = 1;
                break;

            // D = A,G or T
            case 'D':
                As[j] = 1;
                Gs[j] = 1;
                Ts[j] = 1;
                break;

            // B = C,G or T
            case 'B':
                Cs[j] = 1;
                Gs[j] = 1;
                Ts[j] = 1;
                break;

            // N = A,C,G or T
            default:
                As[j] = 1;
                Cs[j] = 1;
                Gs[j] = 1;
                Ts[j] = 1;
                break;
            }
        }
        A_snps.push_back(As);
        C_snps.push_back(Cs);
        G_snps.push_back(Gs);
        T_snps.push_back(Ts);

        n_seqs++;
    }
    kseq_destroy(seq);
    gzclose(fp);

    std::vector<std::vector<int> > rows(n_seqs);
    std::vector<std::vector<int> > cols(n_seqs);
    std::vector<std::vector<double> > distances(n_seqs);
    uint64_t len = 0;

#pragma omp parallel for schedule(dynamic, 5) reduction(+ \
                                                        : len) num_threads(n_threads)
    for (uint64_t i = 0; i < n_seqs; i++)
    {

        std::vector<int> comp_snps(n_seqs);
        boost::dynamic_bitset<> res(seq_length);

        for (uint64_t j = 0; j < n_seqs; j++)
        {

            res = A_snps[i] & A_snps[j];
            res |= C_snps[i] & C_snps[j];
            res |= G_snps[i] & G_snps[j];
            res |= T_snps[i] & T_snps[j];

            comp_snps[j] = seq_length - res.count();
        }

        // if using knn find the distance needed
        if (knn >= 0)
        {
            std::vector<int> s_comp = comp_snps;
            std::sort(s_comp.begin(), s_comp.end());
            dist = s_comp[knn + 1];
        }

        // output distances
        for (size_t j = 0; j < n_seqs; j++)
        {
            if ((dist == -1) || (comp_snps[j] <= dist))
            {
                rows[i].push_back(i);
                cols[i].push_back(j);
                distances[i].push_back(comp_snps[j]);
            }
        }
        len += distances[i].size();

        if (i % 10 == 0)
        {
            fprintf(stdout, "Calculating distances: %.1lf%%\n", (double)i / n_seqs * 100);
            fflush(stdout);
        }
    }

    // Combine the lists from each thread
    SparseDist results;
    results.rows = std::vector<int>(combine_vectors(rows, len));
    results.cols = std::vector<int>(combine_vectors(cols, len));
    results.distances = std::vector<double>(combine_vectors(distances, len));
    results.seq_names = seq_names;

    return results;
}


/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2011, 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 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, James D. Marble, Ryan Luna, Henning Kayser */

#include "ompl/geometric/planners/prm/PRM.h"
#include "ompl/geometric/planners/prm/ConnectionStrategy.h"
#include "ompl/base/goals/GoalSampleableRegion.h"
#include "ompl/base/objectives/PathLengthOptimizationObjective.h"
#include "ompl/datastructures/PDF.h"
#include "ompl/tools/config/SelfConfig.h"
#include "ompl/tools/config/MagicConstants.h"
#include <boost/graph/astar_search.hpp>
#include <boost/graph/incremental_components.hpp>
#include <boost/property_map/vector_property_map.hpp>
#include <boost/foreach.hpp>
#include <thread>
#include <typeinfo>

#include "GoalVisitor.hpp"

#define foreach BOOST_FOREACH

namespace ompl
{
    namespace magic
    {
        /** \brief The number of steps to take for a random bounce
            motion generated as part of the expansion step of PRM. */
        static const unsigned int MAX_RANDOM_BOUNCE_STEPS = 5;

        /** \brief The time in seconds for a single roadmap building operation (dt)*/
        static const double ROADMAP_BUILD_TIME = 0.2;

        /** \brief The number of nearest neighbors to consider by
            default in the construction of the PRM roadmap */
        static const unsigned int DEFAULT_NEAREST_NEIGHBORS = 10;
    }  // namespace magic
}  // namespace ompl

ompl::geometric::PRM::PRM(const base::SpaceInformationPtr &si, bool starStrategy)
  : base::Planner(si, "PRM")
  , starStrategy_(starStrategy)
  , stateProperty_(boost::get(vertex_state_t(), g_))
  , totalConnectionAttemptsProperty_(boost::get(vertex_total_connection_attempts_t(), g_))
  , successfulConnectionAttemptsProperty_(boost::get(vertex_successful_connection_attempts_t(), g_))
  , weightProperty_(boost::get(boost::edge_weight, g_))
  , disjointSets_(boost::get(boost::vertex_rank, g_), boost::get(boost::vertex_predecessor, g_))
{
    specs_.recognizedGoal = base::GOAL_SAMPLEABLE_REGION;
    specs_.approximateSolutions = true;
    specs_.optimizingPaths = true;
    specs_.multithreaded = true;

    if (!starStrategy_)
        Planner::declareParam<unsigned int>("max_nearest_neighbors", this, &PRM::setMaxNearestNeighbors,
                                            &PRM::getMaxNearestNeighbors, std::string("8:1000"));

    addPlannerProgressProperty("iterations INTEGER", [this] { return getIterationCount(); });
    addPlannerProgressProperty("best cost REAL", [this] { return getBestCost(); });
    addPlannerProgressProperty("milestone count INTEGER", [this] { return getMilestoneCountString(); });
    addPlannerProgressProperty("edge count INTEGER", [this] { return getEdgeCountString(); });
}

ompl::geometric::PRM::PRM(const base::PlannerData &data, bool starStrategy)
  : PRM(data.getSpaceInformation(), starStrategy)
{
    if (data.numVertices() > 0)
    {
        // mapping between vertex id from PlannerData and Vertex in Boost.Graph
        std::map<unsigned int, Vertex> vertices;
        // helper function to create vertices as needed and update the vertices mapping
        const auto &getOrCreateVertex = [&](unsigned int vertex_index) {
            if (!vertices.count(vertex_index))
            {
                const auto &data_vertex = data.getVertex(vertex_index);
                Vertex graph_vertex = boost::add_vertex(g_);
                stateProperty_[graph_vertex] = si_->cloneState(data_vertex.getState());
                totalConnectionAttemptsProperty_[graph_vertex] = 1;
                successfulConnectionAttemptsProperty_[graph_vertex] = 0;
                vertices[vertex_index] = graph_vertex;
            }
            return vertices.at(vertex_index);
        };

        specs_.multithreaded = false;  // temporarily set to false since nn_ is used only in single thread
        nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Vertex>(this));
        specs_.multithreaded = true;
        nn_->setDistanceFunction([this](const Vertex a, const Vertex b) { return distanceFunction(a, b); });

        for (size_t vertex_index = 0; vertex_index < data.numVertices(); ++vertex_index)
        {
            Vertex m = getOrCreateVertex(vertex_index);
            std::vector<unsigned int> neighbor_indices;
            data.getEdges(vertex_index, neighbor_indices);
            if (neighbor_indices.empty())
            {
                disjointSets_.make_set(m);
            }
            else
            {
                for (const unsigned int neighbor_index : neighbor_indices)
                {
                    Vertex n = getOrCreateVertex(neighbor_index);
                    totalConnectionAttemptsProperty_[n]++;
                    successfulConnectionAttemptsProperty_[n]++;
                    base::Cost weight;
                    data.getEdgeWeight(vertex_index, neighbor_index, &weight);
                    const Graph::edge_property_type properties(weight);
                    boost::add_edge(m, n, properties, g_);
                    uniteComponents(m, n);
                }
            }
            nn_->add(m);
        }
    }
}

ompl::geometric::PRM::~PRM()
{
    freeMemory();
}

void ompl::geometric::PRM::setup()
{
    Planner::setup();
    if (!nn_)
    {
        specs_.multithreaded = false;  // temporarily set to false since nn_ is used only in single thread
        nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Vertex>(this));
        specs_.multithreaded = true;
        nn_->setDistanceFunction([this](const Vertex a, const Vertex b) { return distanceFunction(a, b); });
    }
    if (!connectionStrategy_)
        setDefaultConnectionStrategy();
    if (!connectionFilter_)
        connectionFilter_ = [](const Vertex &, const Vertex &) { return true; };

    // Setup optimization objective
    //
    // If no optimization objective was specified, then default to
    // optimizing path length as computed by the distance() function
    // in the state space.
    if (pdef_)
    {
        if (pdef_->hasOptimizationObjective())
            opt_ = pdef_->getOptimizationObjective();
        else
        {
            opt_ = std::make_shared<base::PathLengthOptimizationObjective>(si_);
            if (!starStrategy_)
                opt_->setCostThreshold(opt_->infiniteCost());
        }
    }
    else
    {
        OMPL_INFORM("%s: problem definition is not set, deferring setup completion...", getName().c_str());
        setup_ = false;
    }
}

void ompl::geometric::PRM::setMaxNearestNeighbors(unsigned int k)
{
    if (starStrategy_)
        throw Exception("Cannot set the maximum nearest neighbors for " + getName());
    if (!nn_)
    {
        specs_.multithreaded = false;  // temporarily set to false since nn_ is used only in single thread
        nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Vertex>(this));
        specs_.multithreaded = true;
        nn_->setDistanceFunction([this](const Vertex a, const Vertex b) { return distanceFunction(a, b); });
    }
    if (!userSetConnectionStrategy_)
        connectionStrategy_ = KStrategy<Vertex>(k, nn_);
    if (isSetup())
        setup();
}

unsigned int ompl::geometric::PRM::getMaxNearestNeighbors() const
{
    const auto strategy = connectionStrategy_.target<KStrategy<Vertex>>();
    return strategy ? strategy->getNumNeighbors() : 0u;
}

void ompl::geometric::PRM::setDefaultConnectionStrategy()
{
    if (starStrategy_)
        connectionStrategy_ = KStarStrategy<Vertex>([this] { return milestoneCount(); }, nn_, si_->getStateDimension());
    else
        connectionStrategy_ = KStrategy<Vertex>(magic::DEFAULT_NEAREST_NEIGHBORS, nn_);
}

void ompl::geometric::PRM::setProblemDefinition(const base::ProblemDefinitionPtr &pdef)
{
    Planner::setProblemDefinition(pdef);
    clearQuery();
}

void ompl::geometric::PRM::clearQuery()
{
    startM_.clear();
    goalM_.clear();
    pis_.restart();
}

void ompl::geometric::PRM::clear()
{
    Planner::clear();
    sampler_.reset();
    simpleSampler_.reset();
    freeMemory();
    if (nn_)
        nn_->clear();
    clearQuery();

    iterations_ = 0;
    bestCost_ = base::Cost(std::numeric_limits<double>::quiet_NaN());
}

void ompl::geometric::PRM::freeMemory()
{
    foreach (Vertex v, boost::vertices(g_))
        si_->freeState(stateProperty_[v]);
    g_.clear();
}

void ompl::geometric::PRM::expandRoadmap(double expandTime)
{
    expandRoadmap(base::timedPlannerTerminationCondition(expandTime));
}

void ompl::geometric::PRM::expandRoadmap(const base::PlannerTerminationCondition &ptc)
{
    if (!simpleSampler_)
        simpleSampler_ = si_->allocStateSampler();

    std::vector<base::State *> states(magic::MAX_RANDOM_BOUNCE_STEPS);
    si_->allocStates(states);
    expandRoadmap(ptc, states);
    si_->freeStates(states);
}

void ompl::geometric::PRM::expandRoadmap(const base::PlannerTerminationCondition &ptc,
                                         std::vector<base::State *> &workStates)
{
    // construct a probability distribution over the vertices in the roadmap
    // as indicated in
    //  "Probabilistic Roadmaps for Path Planning in High-Dimensional Configuration Spaces"
    //        Lydia E. Kavraki, Petr Svestka, Jean-Claude Latombe, and Mark H. Overmars

    PDF<Vertex> pdf;
    foreach (Vertex v, boost::vertices(g_))
    {
        const unsigned long int t = totalConnectionAttemptsProperty_[v];
        pdf.add(v, (double)(t - successfulConnectionAttemptsProperty_[v]) / (double)t);
    }

    if (pdf.empty())
        return;

    while (!ptc)
    {
        iterations_++;
        Vertex v = pdf.sample(rng_.uniform01());
        unsigned int s =
            si_->randomBounceMotion(simpleSampler_, stateProperty_[v], workStates.size(), workStates, false);
        if (s > 0)
        {
            s--;
            Vertex last = addMilestone(si_->cloneState(workStates[s]));

            graphMutex_.lock();
            for (unsigned int i = 0; i < s; ++i)
            {
                // add the vertex along the bouncing motion
                Vertex m = boost::add_vertex(g_);
                stateProperty_[m] = si_->cloneState(workStates[i]);
                totalConnectionAttemptsProperty_[m] = 1;
                successfulConnectionAttemptsProperty_[m] = 0;
                disjointSets_.make_set(m);

                // add the edge to the parent vertex
                const base::Cost weight = opt_->motionCost(stateProperty_[v], stateProperty_[m]);
                const Graph::edge_property_type properties(weight);
                boost::add_edge(v, m, properties, g_);
                uniteComponents(v, m);

                // add the vertex to the nearest neighbors data structure
                nn_->add(m);
                v = m;
            }

            // if there are intermediary states or the milestone has not been connected to the initially sampled vertex,
            // we add an edge
            if (s > 0 || !sameComponent(v, last))
            {
                // add the edge to the parent vertex
                const base::Cost weight = opt_->motionCost(stateProperty_[v], stateProperty_[last]);
                const Graph::edge_property_type properties(weight);
                boost::add_edge(v, last, properties, g_);
                uniteComponents(v, last);
            }
            graphMutex_.unlock();
        }
    }
}

void ompl::geometric::PRM::growRoadmap(double growTime)
{
    growRoadmap(base::timedPlannerTerminationCondition(growTime));
}

void ompl::geometric::PRM::growRoadmap(const base::PlannerTerminationCondition &ptc)
{
    if (!isSetup())
        setup();
    if (!sampler_)
        sampler_ = si_->allocValidStateSampler();

    base::State *workState = si_->allocState();
    growRoadmap(ptc, workState);
    si_->freeState(workState);
}

void ompl::geometric::PRM::growRoadmap(const base::PlannerTerminationCondition &ptc, base::State *workState)
{
    /* grow roadmap in the regular fashion -- sample valid states, add them to the roadmap, add valid connections */
    while (!ptc)
    {
        iterations_++;
        // search for a valid state
        bool found = false;
        while (!found && !ptc)
        {
            unsigned int attempts = 0;
            do
            {
                found = sampler_->sample(workState);
                attempts++;
            } while (attempts < magic::FIND_VALID_STATE_ATTEMPTS_WITHOUT_TERMINATION_CHECK && !found);
        }
        // add it as a milestone
        if (found)
            addMilestone(si_->cloneState(workState));
    }
}

void ompl::geometric::PRM::checkForSolution(const base::PlannerTerminationCondition &ptc, base::PathPtr &solution)
{
    auto *goal = static_cast<base::GoalSampleableRegion *>(pdef_->getGoal().get());
    while (!ptc && !addedNewSolution_)
    {
        // Check for any new goal states
        if (goal->maxSampleCount() > goalM_.size())
        {
            const base::State *st = pis_.nextGoal();
            if (st != nullptr)
                goalM_.push_back(addMilestone(si_->cloneState(st)));
        }

        // Check for a solution
        addedNewSolution_ = maybeConstructSolution(startM_, goalM_, solution);
        // Sleep for 1ms
        if (!addedNewSolution_)
            std::this_thread::sleep_for(std::chrono::milliseconds(1));
    }
}

bool ompl::geometric::PRM::maybeConstructSolution(const std::vector<Vertex> &starts, const std::vector<Vertex> &goals,
                                                  base::PathPtr &solution)
{
    base::Goal *g = pdef_->getGoal().get();
    base::Cost sol_cost(opt_->infiniteCost());
    foreach (Vertex start, starts)
    {
        foreach (Vertex goal, goals)
        {
            // we lock because the connected components algorithm is incremental and may change disjointSets_
            graphMutex_.lock();
            bool same_component = sameComponent(start, goal);
            graphMutex_.unlock();

            if (same_component && g->isStartGoalPairValid(stateProperty_[goal], stateProperty_[start]))
            {
                base::PathPtr p = constructSolution(start, goal);
                if (p)
                {
                    base::Cost pathCost = p->cost(opt_);
                    if (opt_->isCostBetterThan(pathCost, bestCost_))
                        bestCost_ = pathCost;
                    // Check if optimization objective is satisfied
                    if (opt_->isSatisfied(pathCost))
                    {
                        solution = p;
                        return true;
                    }
                    if (opt_->isCostBetterThan(pathCost, sol_cost))
                    {
                        solution = p;
                        sol_cost = pathCost;
                    }
                }
            }
        }
    }

    return false;
}

bool ompl::geometric::PRM::addedNewSolution() const
{
    return addedNewSolution_;
}

ompl::base::PlannerStatus ompl::geometric::PRM::solve(const base::PlannerTerminationCondition &ptc)
{
    checkValidity();
    auto *goal = dynamic_cast<base::GoalSampleableRegion *>(pdef_->getGoal().get());

    if (goal == nullptr)
    {
        OMPL_ERROR("%s: Unknown type of goal", getName().c_str());
        return base::PlannerStatus::UNRECOGNIZED_GOAL_TYPE;
    }

    // Add the valid start states as milestones
    while (const base::State *st = pis_.nextStart())
        startM_.push_back(addMilestone(si_->cloneState(st)));

    if (startM_.empty())
    {
        OMPL_ERROR("%s: There are no valid initial states!", getName().c_str());
        return base::PlannerStatus::INVALID_START;
    }

    if (!goal->couldSample())
    {
        OMPL_ERROR("%s: Insufficient states in sampleable goal region", getName().c_str());
        return base::PlannerStatus::INVALID_GOAL;
    }

    // Ensure there is at least one valid goal state
    if (goal->maxSampleCount() > goalM_.size() || goalM_.empty())
    {
        const base::State *st = goalM_.empty() ? pis_.nextGoal(ptc) : pis_.nextGoal();
        if (st != nullptr)
            goalM_.push_back(addMilestone(si_->cloneState(st)));

        if (goalM_.empty())
        {
            OMPL_ERROR("%s: Unable to find any valid goal states", getName().c_str());
            return base::PlannerStatus::INVALID_GOAL;
        }
    }

    unsigned long int nrStartStates = boost::num_vertices(g_);
    OMPL_INFORM("%s: Starting planning with %lu states already in datastructure", getName().c_str(), nrStartStates);

    // Reset addedNewSolution_ member and create solution checking thread
    addedNewSolution_ = false;
    base::PathPtr sol;
    std::thread slnThread([this, &ptc, &sol] { checkForSolution(ptc, sol); });

    // construct new planner termination condition that fires when the given ptc is true, or a solution is found
    base::PlannerTerminationCondition ptcOrSolutionFound([this, &ptc] { return ptc || addedNewSolution(); });

    constructRoadmap(ptcOrSolutionFound);

    // Ensure slnThread is ceased before exiting solve
    slnThread.join();

    OMPL_INFORM("%s: Created %u states", getName().c_str(), boost::num_vertices(g_) - nrStartStates);

    if (sol)
    {
        base::PlannerSolution psol(sol);
        psol.setPlannerName(getName());
        // if the solution was optimized, we mark it as such
        psol.setOptimized(opt_, bestCost_, addedNewSolution());
        pdef_->addSolutionPath(psol);
    }
    else
    {
        // Return an approximate solution.
        ompl::base::Cost diff = constructApproximateSolution(startM_, goalM_, sol);
        if (!opt_->isFinite(diff))
        {
            OMPL_INFORM("Closest path is still start and goal");
            return base::PlannerStatus::TIMEOUT;
        }
        OMPL_INFORM("Using approximate solution, heuristic cost-to-go is %f", diff.value());
        pdef_->addSolutionPath(sol, true, diff.value(), getName());
        return base::PlannerStatus::APPROXIMATE_SOLUTION;
    }

    return sol ? base::PlannerStatus::EXACT_SOLUTION : base::PlannerStatus::TIMEOUT;
}

void ompl::geometric::PRM::constructRoadmap(const base::PlannerTerminationCondition &ptc)
{
    if (!isSetup())
        setup();
    if (!sampler_)
        sampler_ = si_->allocValidStateSampler();
    if (!simpleSampler_)
        simpleSampler_ = si_->allocStateSampler();

    std::vector<base::State *> xstates(magic::MAX_RANDOM_BOUNCE_STEPS);
    si_->allocStates(xstates);
    bool grow = true;

    bestCost_ = opt_->infiniteCost();
    while (!ptc())
    {
        // maintain a 2:1 ratio for growing/expansion of roadmap
        // call growRoadmap() twice as long for every call of expandRoadmap()
        if (grow)
            growRoadmap(base::plannerOrTerminationCondition(
                            ptc, base::timedPlannerTerminationCondition(2.0 * magic::ROADMAP_BUILD_TIME)),
                        xstates[0]);
        else
            expandRoadmap(base::plannerOrTerminationCondition(
                              ptc, base::timedPlannerTerminationCondition(magic::ROADMAP_BUILD_TIME)),
                          xstates);
        grow = !grow;
    }

    si_->freeStates(xstates);
}

ompl::geometric::PRM::Vertex ompl::geometric::PRM::addMilestone(base::State *state)
{
    std::lock_guard<std::mutex> _(graphMutex_);

    Vertex m = boost::add_vertex(g_);
    stateProperty_[m] = state;
    totalConnectionAttemptsProperty_[m] = 1;
    successfulConnectionAttemptsProperty_[m] = 0;

    // Initialize to its own (dis)connected component.
    disjointSets_.make_set(m);

    // Which milestones will we attempt to connect to?
    const std::vector<Vertex> &neighbors = connectionStrategy_(m);

    foreach (Vertex n, neighbors)
        if (connectionFilter_(n, m))
        {
            totalConnectionAttemptsProperty_[m]++;
            totalConnectionAttemptsProperty_[n]++;
            if (si_->checkMotion(stateProperty_[n], stateProperty_[m]))
            {
                successfulConnectionAttemptsProperty_[m]++;
                successfulConnectionAttemptsProperty_[n]++;
                const base::Cost weight = opt_->motionCost(stateProperty_[n], stateProperty_[m]);
                const Graph::edge_property_type properties(weight);
                boost::add_edge(n, m, properties, g_);
                uniteComponents(n, m);
            }
        }

    nn_->add(m);

    return m;
}

void ompl::geometric::PRM::uniteComponents(Vertex m1, Vertex m2)
{
    disjointSets_.union_set(m1, m2);
}

bool ompl::geometric::PRM::sameComponent(Vertex m1, Vertex m2)
{
    return boost::same_component(m1, m2, disjointSets_);
}

ompl::base::Cost ompl::geometric::PRM::constructApproximateSolution(const std::vector<Vertex> &starts,
                                                                    const std::vector<Vertex> &goals,
                                                                    base::PathPtr &solution)
{
    std::lock_guard<std::mutex> _(graphMutex_);
    base::Goal *g = pdef_->getGoal().get();
    base::Cost closestVal(opt_->infiniteCost());
    bool approxPathJustStart = true;

    foreach (Vertex start, starts)
    {
        foreach (Vertex goal, goals)
        {
            base::Cost heuristicCost(costHeuristic(start, goal));
            if (opt_->isCostBetterThan(heuristicCost, closestVal))
            {
                closestVal = heuristicCost;
                approxPathJustStart = true;
            }
            if (!g->isStartGoalPairValid(stateProperty_[goal], stateProperty_[start]))
            {
                continue;
            }
            base::PathPtr p;
            boost::vector_property_map<Vertex> prev(boost::num_vertices(g_));
            boost::vector_property_map<base::Cost> dist(boost::num_vertices(g_));
            boost::vector_property_map<base::Cost> rank(boost::num_vertices(g_));

            try
            {
                // Consider using a persistent distance_map if it's slow
                boost::astar_search(
                    g_, start, [this, goal](Vertex v) { return costHeuristic(v, goal); },
                    boost::predecessor_map(prev)
                        .distance_map(dist)
                        .rank_map(rank)
                        .distance_compare(
                            [this](base::Cost c1, base::Cost c2) { return opt_->isCostBetterThan(c1, c2); })
                        .distance_combine([this](base::Cost c1, base::Cost c2) { return opt_->combineCosts(c1, c2); })
                        .distance_inf(opt_->infiniteCost())
                        .distance_zero(opt_->identityCost())
                        .visitor(AStarGoalVisitor<Vertex>(goal)));
            }
            catch (AStarFoundGoal &)
            {
            }

            Vertex closeToGoal = start;
            for (auto vp = vertices(g_); vp.first != vp.second; vp.first++)
            {
                // We want to get the distance of each vertex to the goal.
                // Boost lets us get cost-to-come, cost-to-come+dist-to-goal,
                // but not just dist-to-goal.
                ompl::base::Cost dist_to_goal(costHeuristic(*vp.first, goal));
                if (opt_->isFinite(rank[*vp.first]) && opt_->isCostBetterThan(dist_to_goal, closestVal))
                {
                    closeToGoal = *vp.first;
                    closestVal = dist_to_goal;
                    approxPathJustStart = false;
                }
            }
            if (closeToGoal != start)
            {
                auto p(std::make_shared<PathGeometric>(si_));
                for (Vertex pos = closeToGoal; prev[pos] != pos; pos = prev[pos])
                    p->append(stateProperty_[pos]);
                p->append(stateProperty_[start]);
                p->reverse();

                solution = p;
            }
        }
    }
    if (approxPathJustStart)
    {
        return opt_->infiniteCost();
    }
    return closestVal;
}

ompl::base::PathPtr ompl::geometric::PRM::constructSolution(const Vertex &start, const Vertex &goal)
{
    std::lock_guard<std::mutex> _(graphMutex_);
    boost::vector_property_map<Vertex> prev(boost::num_vertices(g_));

    try
    {
        // Consider using a persistent distance_map if it's slow
        boost::astar_search(
            g_, start, [this, goal](Vertex v) { return costHeuristic(v, goal); },
            boost::predecessor_map(prev)
                .distance_compare([this](base::Cost c1, base::Cost c2) { return opt_->isCostBetterThan(c1, c2); })
                .distance_combine([this](base::Cost c1, base::Cost c2) { return opt_->combineCosts(c1, c2); })
                .distance_inf(opt_->infiniteCost())
                .distance_zero(opt_->identityCost())
                .visitor(AStarGoalVisitor<Vertex>(goal)));
    }
    catch (AStarFoundGoal &)
    {
    }

    if (prev[goal] == goal)
        throw Exception(name_, "Could not find solution path");

    auto p(std::make_shared<PathGeometric>(si_));
    for (Vertex pos = goal; prev[pos] != pos; pos = prev[pos])
        p->append(stateProperty_[pos]);
    p->append(stateProperty_[start]);
    p->reverse();

    return p;
}

void ompl::geometric::PRM::getPlannerData(base::PlannerData &data) const
{
    Planner::getPlannerData(data);

    // Explicitly add start and goal states:
    for (unsigned long i : startM_)
        data.addStartVertex(
            base::PlannerDataVertex(stateProperty_[i], const_cast<PRM *>(this)->disjointSets_.find_set(i)));

    for (unsigned long i : goalM_)
        data.addGoalVertex(
            base::PlannerDataVertex(stateProperty_[i], const_cast<PRM *>(this)->disjointSets_.find_set(i)));

    // Adding edges and all other vertices simultaneously
    foreach (const Edge e, boost::edges(g_))
    {
        const Vertex v1 = boost::source(e, g_);
        const Vertex v2 = boost::target(e, g_);
        data.addEdge(base::PlannerDataVertex(stateProperty_[v1]), base::PlannerDataVertex(stateProperty_[v2]));

        // Add the reverse edge, since we're constructing an undirected roadmap
        data.addEdge(base::PlannerDataVertex(stateProperty_[v2]), base::PlannerDataVertex(stateProperty_[v1]));

        // Add tags for the newly added vertices
        data.tagState(stateProperty_[v1], const_cast<PRM *>(this)->disjointSets_.find_set(v1));
        data.tagState(stateProperty_[v2], const_cast<PRM *>(this)->disjointSets_.find_set(v2));
    }
}

ompl::base::Cost ompl::geometric::PRM::costHeuristic(Vertex u, Vertex v) const
{
    return opt_->motionCostHeuristic(stateProperty_[u], stateProperty_[v]);
}


#pragma once
#include "serialization/traits.hpp"
#include <chrono>
#include <boost/serialization/split_free.hpp>

// boostに勝手にtypedefするのは不味そうなのでマクロで対処
//! 基準となる差分形式
#define Duration_t	std::chrono::nanoseconds
#define Rep_t		Duration_t::rep
namespace boost {
	namespace serialization {
		template <class Archive, class C, class D>
		void serialize(Archive& ar, std::chrono::time_point<C,D>& tp, const unsigned int ver) {
			split_free(ar, tp, ver);
		}
		template <class Archive, class C, class D>
		void load(Archive& ar, std::chrono::time_point<C,D>& tp, const unsigned int) {
			Rep_t rep;
			std::time_t epoch;
			ar & BOOST_SERIALIZATION_NVP(rep) & BOOST_SERIALIZATION_NVP(epoch);

			Duration_t dur(rep);
			// 現在使用中ClockのEpochタイムとの差分を考慮 (別PCでの読み込みなど)
			auto tp2 = std::chrono::time_point_cast<Duration_t>(C::from_time_t(epoch));
			tp = std::chrono::time_point<C,D>(dur - tp2.time_since_epoch());
		}
		template <class Archive, class C, class D>
		void save(Archive& ar, const std::chrono::time_point<C,D>& tp, const unsigned int) {
			// Epochをtime_tに変換したものと、Epochからの差分を保存
			// time_tはPOSIX timeのEpochなので機種間で共通
			auto rep = std::chrono::duration_cast<Duration_t>(tp.time_since_epoch()).count();
			std::time_t epoch = C::to_time_t(std::chrono::time_point<C,D>(Duration_t(0)));
			ar & BOOST_SERIALIZATION_NVP(rep) & BOOST_SERIALIZATION_NVP(epoch);
		}

		template <class Archive, class R, class P>
		void serialize(Archive& ar, std::chrono::duration<R,P>& dur, const unsigned int ver) {
			split_free(ar, dur, ver);
		}
		template <class Archive, class R, class P>
		void load(Archive& ar, std::chrono::duration<R,P>& dur, const unsigned int) {
			using Src = std::chrono::duration<R,P>;
			Rep_t rep;
			ar & BOOST_SERIALIZATION_NVP(rep);
			dur = std::chrono::duration_cast<Src>(Duration_t(rep));
		}
		template <class Archive, class R, class P>
		void save(Archive& ar, const std::chrono::duration<R,P>& dur, const unsigned int) {
			// 全てnanosecondsに変換して保存
			auto dur2 = std::chrono::duration_cast<Duration_t>(dur);
			auto rep = dur2.count();
			ar & BOOST_SERIALIZATION_NVP(rep);
		}
	}
}
#undef Duration_t
#undef Rep_t
BOOST_CLASS_IMPLEMENTATION_TEMPLATE((class)(class), std::chrono::time_point, object_serializable)
BOOST_CLASS_TRACKING_TEMPLATE((class)(class), std::chrono::time_point, track_never)


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

#ifndef BOOST_MATH_DISTRIBUTIONS_DETAIL_HG_QUANTILE_HPP
#define BOOST_MATH_DISTRIBUTIONS_DETAIL_HG_QUANTILE_HPP

#include <boost/math/policies/error_handling.hpp>
#include <boost/math/distributions/detail/hypergeometric_pdf.hpp>

namespace boost{ namespace math{ namespace detail{

template <class T>
inline unsigned round_x_from_p(unsigned x, T p, T cum, T fudge_factor, unsigned lbound, unsigned /*ubound*/, const policies::discrete_quantile<policies::integer_round_down>&)
{
   if((p < cum * fudge_factor) && (x != lbound))
   {
      BOOST_MATH_INSTRUMENT_VARIABLE(x-1);
      return --x;
   }
   return x;
}

template <class T>
inline unsigned round_x_from_p(unsigned x, T p, T cum, T fudge_factor, unsigned /*lbound*/, unsigned ubound, const policies::discrete_quantile<policies::integer_round_up>&)
{
   if((cum < p * fudge_factor) && (x != ubound))
   {
      BOOST_MATH_INSTRUMENT_VARIABLE(x+1);
      return ++x;
   }
   return x;
}

template <class T>
inline unsigned round_x_from_p(unsigned x, T p, T cum, T fudge_factor, unsigned lbound, unsigned ubound, const policies::discrete_quantile<policies::integer_round_inwards>&)
{
   if(p >= 0.5)
      return round_x_from_p(x, p, cum, fudge_factor, lbound, ubound, policies::discrete_quantile<policies::integer_round_down>());
   return round_x_from_p(x, p, cum, fudge_factor, lbound, ubound, policies::discrete_quantile<policies::integer_round_up>());
}

template <class T>
inline unsigned round_x_from_p(unsigned x, T p, T cum, T fudge_factor, unsigned lbound, unsigned ubound, const policies::discrete_quantile<policies::integer_round_outwards>&)
{
   if(p >= 0.5)
      return round_x_from_p(x, p, cum, fudge_factor, lbound, ubound, policies::discrete_quantile<policies::integer_round_up>());
   return round_x_from_p(x, p, cum, fudge_factor, lbound, ubound, policies::discrete_quantile<policies::integer_round_down>());
}

template <class T>
inline unsigned round_x_from_p(unsigned x, T /*p*/, T /*cum*/, T /*fudge_factor*/, unsigned /*lbound*/, unsigned /*ubound*/, const policies::discrete_quantile<policies::integer_round_nearest>&)
{
   return x;
}

template <class T>
inline unsigned round_x_from_q(unsigned x, T q, T cum, T fudge_factor, unsigned lbound, unsigned /*ubound*/, const policies::discrete_quantile<policies::integer_round_down>&)
{
   if((q * fudge_factor > cum) && (x != lbound))
   {
      BOOST_MATH_INSTRUMENT_VARIABLE(x-1);
      return --x;
   }
   return x;
}

template <class T>
inline unsigned round_x_from_q(unsigned x, T q, T cum, T fudge_factor, unsigned /*lbound*/, unsigned ubound, const policies::discrete_quantile<policies::integer_round_up>&)
{
   if((q < cum * fudge_factor) && (x != ubound))
   {
      BOOST_MATH_INSTRUMENT_VARIABLE(x+1);
      return ++x;
   }
   return x;
}

template <class T>
inline unsigned round_x_from_q(unsigned x, T q, T cum, T fudge_factor, unsigned lbound, unsigned ubound, const policies::discrete_quantile<policies::integer_round_inwards>&)
{
   if(q < 0.5)
      return round_x_from_q(x, q, cum, fudge_factor, lbound, ubound, policies::discrete_quantile<policies::integer_round_down>());
   return round_x_from_q(x, q, cum, fudge_factor, lbound, ubound, policies::discrete_quantile<policies::integer_round_up>());
}

template <class T>
inline unsigned round_x_from_q(unsigned x, T q, T cum, T fudge_factor, unsigned lbound, unsigned ubound, const policies::discrete_quantile<policies::integer_round_outwards>&)
{
   if(q >= 0.5)
      return round_x_from_q(x, q, cum, fudge_factor, lbound, ubound, policies::discrete_quantile<policies::integer_round_down>());
   return round_x_from_q(x, q, cum, fudge_factor, lbound, ubound, policies::discrete_quantile<policies::integer_round_up>());
}

template <class T>
inline unsigned round_x_from_q(unsigned x, T /*q*/, T /*cum*/, T /*fudge_factor*/, unsigned /*lbound*/, unsigned /*ubound*/, const policies::discrete_quantile<policies::integer_round_nearest>&)
{
   return x;
}

template <class T, class Policy>
unsigned hypergeometric_quantile_imp(T p, T q, unsigned r, unsigned n, unsigned N, const Policy& pol)
{
#ifdef BOOST_MSVC
#  pragma warning(push)
#  pragma warning(disable:4267)
#endif
   typedef typename Policy::discrete_quantile_type discrete_quantile_type;
   BOOST_MATH_STD_USING
   BOOST_FPU_EXCEPTION_GUARD
   T result;
   T fudge_factor = 1 + tools::epsilon<T>() * ((N <= boost::math::prime(boost::math::max_prime - 1)) ? 50 : 2 * N);
   unsigned base = static_cast<unsigned>((std::max)(0, (int)(n + r) - (int)(N)));
   unsigned lim = (std::min)(r, n);

   BOOST_MATH_INSTRUMENT_VARIABLE(p);
   BOOST_MATH_INSTRUMENT_VARIABLE(q);
   BOOST_MATH_INSTRUMENT_VARIABLE(r);
   BOOST_MATH_INSTRUMENT_VARIABLE(n);
   BOOST_MATH_INSTRUMENT_VARIABLE(N);
   BOOST_MATH_INSTRUMENT_VARIABLE(fudge_factor);
   BOOST_MATH_INSTRUMENT_VARIABLE(base);
   BOOST_MATH_INSTRUMENT_VARIABLE(lim);

   if(p <= 0.5)
   {
      unsigned x = base;
      result = hypergeometric_pdf<T>(x, r, n, N, pol);
      T diff = result;
      while(result < p)
      {
         diff = (diff > tools::min_value<T>() * 8)
            ? T(n - x) * T(r - x) * diff / (T(x + 1) * T(N + x + 1 - n - r))
            : hypergeometric_pdf<T>(x + 1, r, n, N, pol);
         if(result + diff / 2 > p)
            break;
         ++x;
         result += diff;
#ifdef BOOST_MATH_INSTRUMENT
         if(diff != 0)
         {
            BOOST_MATH_INSTRUMENT_VARIABLE(x);
            BOOST_MATH_INSTRUMENT_VARIABLE(diff);
            BOOST_MATH_INSTRUMENT_VARIABLE(result);
         }
#endif
      }
      return round_x_from_p(x, p, result, fudge_factor, base, lim, discrete_quantile_type());
   }
   else
   {
      unsigned x = lim;
      result = 0;
      T diff = hypergeometric_pdf<T>(x, r, n, N, pol);
      while(result + diff / 2 < q)
      {
         result += diff;
         diff = (diff > tools::min_value<T>() * 8)
            ? x * T(N + x - n - r) * diff / (T(1 + n - x) * T(1 + r - x))
            : hypergeometric_pdf<T>(x - 1, r, n, N, pol);
         --x;
#ifdef BOOST_MATH_INSTRUMENT
         if(diff != 0)
         {
            BOOST_MATH_INSTRUMENT_VARIABLE(x);
            BOOST_MATH_INSTRUMENT_VARIABLE(diff);
            BOOST_MATH_INSTRUMENT_VARIABLE(result);
         }
#endif
      }
      return round_x_from_q(x, q, result, fudge_factor, base, lim, discrete_quantile_type());
   }
#ifdef BOOST_MSVC
#  pragma warning(pop)
#endif
}

template <class T, class Policy>
inline unsigned hypergeometric_quantile(T p, T q, unsigned r, unsigned n, unsigned N, const Policy&)
{
   BOOST_FPU_EXCEPTION_GUARD
   typedef typename tools::promote_args<T>::type result_type;
   typedef typename policies::evaluation<result_type, Policy>::type value_type;
   typedef typename policies::normalise<
      Policy,
      policies::promote_float<false>,
      policies::promote_double<false>,
      policies::assert_undefined<> >::type forwarding_policy;

   return detail::hypergeometric_quantile_imp<value_type>(p, q, r, n, N, forwarding_policy());
}

}}} // namespaces

#endif


#include <iostream>
#include <boost/regex.hpp>
#include <cstdlib>
#include <string>
using namespace std;

void printMenu(const string *, int);
//checks whether entered data is in required range
bool checkEntry(string, const string *, int);

string dataEntry(string prompt, const string *terms, int size) {
   if (size == 0) { //we return an empty string when we call the function with an empty list
      return "";
   }

   string entry;
   do {
      printMenu(terms, size);
      cout << prompt;

      cin >> entry;
   }
   while( !checkEntry(entry, terms, size) );

   int number = atoi(entry.c_str());
   return terms[number - 1];
}

void printMenu(const string *terms, int num) {
   for (int i = 1 ; i < num + 1 ; i++) {
      cout << i << ')' << terms[ i - 1 ] << '\n';
   }
}

bool checkEntry(string myEntry, const string *terms, int num) {
   boost::regex e("^\\d+$");
   if (!boost::regex_match(myEntry, e))
      return false;
   int number = atoi(myEntry.c_str());
   if (number < 1 || number > num)
      return false;
   return true;
}

int main( ) {
   const string terms[ ] = { "fee fie" , "huff and puff" , "mirror mirror" , "tick tock" };
   int size = sizeof terms / sizeof *terms;
   cout << "You chose: " << dataEntry("Which is from the three pigs: ", terms, size);
   return 0;
}


// Copyright 2019 The Beam Team
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//    http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include "tx_history_to_csv.h"
#include "core/block_crypt.h"
#include "wallet/core/common.h"
#include <boost/serialization/nvp.hpp>
#include <boost/multiprecision/cpp_dec_float.hpp>

namespace
{
using namespace beam;
using namespace beam::wallet;
using boost::multiprecision::cpp_dec_float_50;

std::string getIdentity(const TxParameters& txParams, bool isSender)
{
    auto v = isSender ? txParams.GetParameter<PeerID>(TxParameterID::MyWalletIdentity)
                      : txParams.GetParameter<PeerID>(TxParameterID::PeerWalletIdentity);

    return v ? std::to_string(*v) : "";
}

std::string getToken(const TxParameters& txParams)
{
    auto token = txParams.GetParameter<std::string>(TxParameterID::OriginalToken);
    return token ? *token : "";
}

std::string convertAmount(const Amount& amount, const Amount& rate, uint32_t precision)
{
    if (!rate) return "";
    cpp_dec_float_50 dec_first(amount);
    dec_first /= Rules::Coin;
    cpp_dec_float_50 dec_second(rate);
    dec_second /= Rules::Coin;
    cpp_dec_float_50 product = dec_first * dec_second;

    std::ostringstream oss;
    oss.precision(precision);
    oss << std::fixed << product;
    return oss.str();
}
} // namespace

namespace beam::wallet
{
std::string ExportTxHistoryToCsv(const IWalletDB& db)
{
    // TODO:SWAP add to history if necessary https://github.com/BeamMW/beam/issues/1735
    std::stringstream ss;
    ss << "Type" << ","
       << "Date | Time" << ","
       << "\"Amount\"" << ","
       << "\"Unit name\"" << ","
       << "\"Amount, USD\"" << ","
       << "\"Amount, BTC\"" << ","
       << "\"Transaction fee, ARC\"" << ","
       << "Status" << ","
       << "Comment" << ","
       << "Transaction ID" << ","
       << "Kernel ID" << ","
       << "Sending address" << ","
       << "Sending wallet's signature" << ","
       << "Receiving address" << ","
       << "Receiving wallet's signature" << ","
       << "Token" << ","
       << "Payment proof" << std::endl;

    auto transactions = db.getTxHistory(TxType::Simple);
    auto maxPrivacyTx = db.getTxHistory(TxType::PushTransaction);
    transactions.reserve(transactions.size() + maxPrivacyTx.size());
    copy(maxPrivacyTx.begin(), maxPrivacyTx.end(), back_inserter(transactions));
    sort(transactions.begin(), transactions.end(),
        [](const TxDescription& a, const TxDescription& b)
        {
            return a.m_createTime > b.m_createTime;
        }
    );

    for (const auto& tx : transactions)
    {
        std::string strProof;
        if (tx.m_status == TxStatus::Completed &&
            tx.m_sender &&
            !tx.m_selfTx)
        {
            auto proof = storage::ExportPaymentProof(db, tx.m_txId);
            strProof.resize(proof.size() * 2);
            beam::to_hex(strProof.data(), proof.data(), proof.size());
        }

        auto assetIdOptional = tx.GetParameter<Asset::ID>(TxParameterID::AssetID);
        Asset::ID assetId  = assetIdOptional ? *assetIdOptional : 0;
        std::string unitName = "ARC";
        const auto info = db.findAsset(assetId);
        if (info.is_initialized())
        {
            const WalletAssetMeta &meta = info.is_initialized() ? WalletAssetMeta(*info) : WalletAssetMeta(Asset::Full());
            unitName = meta.GetUnitName();
        }

        std::string amountInUsd = convertAmount(tx.m_amount, tx.getExchangeRate(Currency::USD()), 2);
        std::string amountInBtc = convertAmount(tx.m_amount, tx.getExchangeRate(Currency::BTC()), 8);

        auto statusInterpreter = db.getStatusInterpreter(tx);
        ss << (tx.m_sender ? "Send" : "Receive") << ","                                     // Type
            << format_timestamp(kTimeStampFormatCsv, tx.m_createTime * 1000, false) << ","   // Date | Time
            << "\"" << PrintableAmount(tx.m_amount, true) << "\"" << ","                     // Amount
            << "\"" << unitName << "\"" << ","                                               // Unit name
            << "\"" << amountInUsd << "\"" << ","                                            // Amount, USD
            << "\"" << amountInBtc << "\"" << ","                                            // Amount, BTC
            << "\"" << PrintableAmount(tx.m_fee, true) << "\"" << ","                        // Transaction fee, BEAM
            << statusInterpreter->getStatus() << ","                                         // Status
            << std::string { tx.m_message.begin(), tx.m_message.end() } << ","               // Comment
            << to_hex(tx.m_txId.data(), tx.m_txId.size()) << ","                             // Transaction ID
            << std::to_string(tx.m_kernelID) << ","                                          // Kernel ID
            << std::to_string(tx.m_sender ? tx.m_myId : tx.m_peerId) << ","                  // Sending address
            << getIdentity(tx, tx.m_sender) << ","                                           // Sending wallet's signature
            << std::to_string(!tx.m_sender ? tx.m_myId : tx.m_peerId) << ","                 // Receiving address
            << getIdentity(tx, !tx.m_sender) << ","                                          // Receiving wallet's signature
            << getToken(tx) << ","                                                           // Token
            << strProof << std::endl;                                                        // Payment proof
    }
    return ss.str();
}
} // namespace beam::wallet


#ifndef SHARED_GRIDMAP_HPP_9FWOQEMW
#define SHARED_GRIDMAP_HPP_9FWOQEMW

#include <cmath>

#include <ros/ros.h>
#include <ros/console.h>

#include <boost/interprocess/managed_shared_memory.hpp>
#include <boost/interprocess/sync/named_semaphore.hpp>

#include <scrollgrid/scrollgrid3.hpp>
#include <scrollgrid/dense_array3.hpp>

#include <gridmapping2/proxy_gridmap.hpp>

namespace ca
{

// TODO make this configurable

struct MapParameters {
  MapParameters() {
    shm_name = "shared_grid_map";
    dimension = ca::Vec3Ix(200,200,100);
    resolution = 0.1;
  }
  MapParameters(std::string name, ca::Vec3Ix dim, float res) {
    shm_name = name;
    dimension = dim;
    resolution = res;
  }
  std::string shm_name;
  ca::Vec3Ix dimension;
  float resolution;
};

class SharedGridMap {
public:
  SharedGridMap() : parameters_() { }
  SharedGridMap(std::string name, ca::Vec3Ix dim, float res):
    parameters_(name,dim,res) { }

  bool init() {
    namespace bipc = boost::interprocess;
    init_grid_semaphore_name_=parameters_.shm_name +"_init_grid_semaphore";
    init_semaphore_= new bipc::named_semaphore(bipc::open_or_create,init_grid_semaphore_name_.c_str(),0);
    size_t to_allocate = 0;
    to_allocate += sizeof(ScrollGrid3f);  // pointer
    to_allocate += sizeof(DenseArray3<uint8_t>);  // pointer
    to_allocate += sizeof(uint8_t)*(parameters_.dimension.prod()); // main storage part occupancy value
    to_allocate += sizeof(Eigen::Vector3f)*(parameters_.dimension.prod()); // by me, also RBG
    to_allocate += sizeof(Vector_Xxf)*(parameters_.dimension.prod()); // by me, label
    to_allocate += sizeof(int)*(parameters_.dimension.prod()); // by me, label
    to_allocate += 65535; // arbitrary buffer to account for my stupidity
    // nearest power of 2 -- desirable? necessary?
    //size_t to_allocate = (pow(2, ceil(log2(to_allocate))));
    to_allocate += (4096 - (to_allocate % 4096));
    ROS_INFO_STREAM("to_allocate = " << to_allocate);

    segment_ = new bipc::managed_shared_memory(bipc::create_only, parameters_.shm_name.c_str(), to_allocate);
    grid3_ = segment_->construct<ScrollGrid3f>("scrollgrid3f")(Eigen::Vector3f(0, 0, 0),
                                                             parameters_.dimension,
                                                             parameters_.resolution);  // pointer
    grid_data_ = segment_->construct<uint8_t>("grid_data")[grid3_->num_cells()]();  // main data occ value   uint8_t is the template type for dense_array3
    grid_data_wrapper_.reset(grid3_->dimension(), grid_data_); // get pointer

    rgb_data_ = segment_->construct<Eigen::Vector3f>("RGB_data")[grid3_->num_cells()]();  // by me, RBG data
    rgb_data_wrapper_.reset(grid3_->dimension(), rgb_data_); // main data by me, RBG data

    label_data_ = segment_->construct<Vector_Xxf>("Label_data")[grid3_->num_cells()]();  // by me, label data data
    label_data_wrapper_.reset(grid3_->dimension(), label_data_); // main data by me, label data

    grid_counts_ = segment_->construct<int>("Grid_count")[grid3_->num_cells()]();  // by me, label data data
    grid_counts_wrapper_.reset(grid3_->dimension(), grid_counts_); // main data by me, label data

    ROS_INFO("grid is created");
    init_semaphore_->post();
    return true;
  }

  virtual ~SharedGridMap() {
    namespace bipc = boost::interprocess;
    //segment.destroy<ScrollGrid3f>("scrollgrid3f");
    delete segment_;
    bipc::shared_memory_object::remove(parameters_.shm_name.c_str());
    init_semaphore_->remove(init_grid_semaphore_name_.c_str());
  }

private:
  MapParameters parameters_;
  boost::interprocess::managed_shared_memory* segment_;
  boost::interprocess::named_semaphore* init_semaphore_;
  std::string init_grid_semaphore_name_;
  ca::ScrollGrid3f* grid3_;
  ca::DenseArray3<uint8_t> grid_data_wrapper_;
  ca::DenseArray3<Eigen::Vector3f> rgb_data_wrapper_;
  ca::DenseArray3<Vector_Xxf> label_data_wrapper_;   //change accordingly in proxy_map
  ca::DenseArray3<int> grid_counts_wrapper_;   //change accordingly in proxy_map

  uint8_t* grid_data_;
  Eigen::Vector3f* rgb_data_;  // rgb
  Vector_Xxf* label_data_;  // float prob
  int* grid_counts_;

private:
  SharedGridMap(const SharedGridMap& other);
  SharedGridMap& operator=(const SharedGridMap& other);
};

} /* ca */

#endif /* end of include guard: SHARED_GRIDMAP_HPP_9FWOQEMW */