#include "ShortestPath.h"

#include "Exceptions.h"

#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <utility>

using std::pair;

ShortestPath::ShortestPath(Graph* g_) : g(g_) {}

ShortestPath::~ShortestPath() {
    //delete g;
}

Vertex ShortestPath::findVertex(int id_) {
    VertexIterator it, end;
    for(boost::tie(it, end) = boost::vertices(*g); it != end; ++it) {
        if(getId(it) == id_)
            return *it;
    }

    throw MapException("Wrong vertex id!");
}

int ShortestPath::getId(VertexIterator it) {
    return ((*g)[*(it)]).id;
}

int ShortestPath::getId(Vertex v) {
    return ((*g)[v]).id;
}

vector<int> ShortestPath::getPath(int from_, int to_) {
    Vertex from = findVertex(from_);
    Vertex to = findVertex(to_);

    vector<Vertex> predecessors(boost::num_vertices(*g));
    vector<Weight> distances(boost::num_vertices(*g));

    IndexMap indexMap;
    PredecessorMap predecessorMap(&predecessors[0], indexMap);
    DistanceMap distanceMap(&distances[0], indexMap);
    boost::dijkstra_shortest_paths(*g, from, boost::distance_map(distanceMap).predecessor_map(predecessorMap));

    vector<int> path;
    Vertex v = to;

    path.push_back(getId(to));
    for(Vertex u = predecessorMap[v]; u != v; v = u, u = predecessorMap[v]) {
        if(getId(u) != from_) {
            path.push_back(getId(u));
        }
    }

    std::reverse(path.begin(), path.end());
    path.erase(path.begin()); // usuwamy pierwszy element bo ze spawnu pojedzie prosto do niego
    return path;
}


/******************************************************************************
 * This file is for the Prime Numbers plugin.
 * It requires prime.cu that implements genPrimesOnDevice()
 ******************************************************************************/
#include "constants.h"
#include "pluginHeader.h"

#include <boost/algorithm/string.hpp> // starts_with
#include <cstdlib>                    // atoi, strtoull
#include <cstring>                    /* strcpy */
#include <fstream>                    // ifstream, ofstream
#include <iomanip>                    // setw
#include <iostream>

// Prime Number functions
int writePrimesFile(const char* filename, const char* primes, uint64_t size);
uint64_t getPrimesCount(const char* primes, uint64_t size);
void displayPrimes(const char* primes, uint64_t size);

// genPrimesOnDevice is defined in "primes.cu"
extern void genPrimesOnDevice(char* primes, uint64_t limit);
void genPrimesOnHost(char* primes, uint64_t limit);

/* globals */
char params[][256] = {"prime_numbers.txt", "101"};
const int NUM_ARGS = sizeof params / sizeof params[0];
clock_t total_t = 0;
const char* PARAM_INFO = "outputFile,limit";

/******************************************************************************
 * main()
 * - this function is not used by the plugin, but this can be built as
 *   a standalone executable
 ******************************************************************************/
int main(int argc, char** argv)
{
    if (argc == 1)
    {
        std::cout << "Usage: " << argv[0] << " upperLimit outputFile\n";
        return 0;
    }

    if (argc > 1)
    {
        if (strcmp(argv[1], "--help") == 0)
        {
            displayPluginInfo();
            return 0;
        }
        strcpy(params[1], argv[1]);
    }

    if (argc > 2)
        strcpy(params[0], argv[2]);

    return run();
}

////////////////////////////// PRIME FUNCTIONS ////////////////////////////////

/******************************************************************************
 * TODO: implement me
 * genPrimesOnHost
 * - does the computation in serial, using the CPU
 ******************************************************************************/
void genPrimesOnHost(char* primes, uint64_t limit) {}

/******************************************************************************
 * writePrimesFile() - saves the generated prime numbers to the file
 ******************************************************************************/
int writePrimesFile(const char* filename, const char* primes, uint64_t size)
{
    // open the file
    std::ofstream fout(filename);
    if (!fout || !primes || !size)
    {
        return ERROR;
    }

    /*
    // write a comma-separated list of primes
    uint64_t count = getPrimesCount(primes, size);
    for (uint64_t i = 0; i < size; i++)
    {
       if (primes[i] == 0)
       {
          count--;
          fout << i;
          if (((i + 1) != size) && count)
             fout << ", ";
       }
    }
    fout << "\n";
    */

    // write a newline separated list of primes
    for (uint64_t i = 0; i < size; i++)
    {
        if (primes[i] == 0) // a value of '0' means the number is prime,
        {                   // a value of '1' means the number is composite
            fout << i << "\n";
        }
    }

    fout.close();
    return OK;
}

/******************************************************************************
 * getPrimesCount() - counts the number of primes
 ******************************************************************************/
uint64_t getPrimesCount(const char* primes, uint64_t size)
{
    uint64_t count = 0;
    for (uint64_t i = 0; i < size; i++)
        if (primes[i] == 0)
            count++;
    return count;
}

/******************************************************************************
 * displayPrimes() - displays a comma-separated list of prime numbers
 ******************************************************************************/
void displayPrimes(const char* primes, uint64_t size)
{
    uint64_t count = getPrimesCount(primes, size);
    for (uint64_t i = 0; i < size; i++)
    {
        if (primes[i] == 0)
        {
            count--;
            std::cout << i;
            if (((i + 1) != size) && count)
                std::cout << ", ";
        }
    }
    std::cout << std::endl;
}

///////////////////////////// PLUGIN FUNCTIONS ////////////////////////////////

/******************************************************************************
 * run()
 * - computes the matrix multiplication: C = A * B
 * - reads the input files from the parameters: A = params[0], B = params[1]
 * - writes the output result C to the file set in params[2]
 * - if params[3] is set, then display the result to the screen
 ******************************************************************************/
int run()
{
    clock_t start_t;
    clock_t end_t;
    total_t = 0; // reset the clock counter

    uint64_t maxPrime = strtoull(params[1], nullptr, 10); // TODO: check for errors
    char* primes;
    maxPrime++; // allow the given parameter to be part of the search

#ifdef DEBUG
    std::cout << "calling genPrimesOnDevice with maxPrime = " << maxPrime << std::endl;
#endif

    primes = new (std::nothrow) char[maxPrime]();
    if (primes == nullptr)
    {
        std::cout << "Failed to allocate memory for the prime numbers.\n"
                  << "The limit parameter might be too large: " << maxPrime << std::endl;
        return ERROR;
    }

    start_t = clock();
    genPrimesOnDevice(primes, maxPrime);
    end_t = clock();
    total_t = end_t - start_t;

    // now save to the file
    std::cout << "Found " << getPrimesCount(primes, maxPrime) << " primes"
              << " in " << total_t / (double)CLOCKS_PER_SEC << " seconds.\n"
              << "Writing results to: \"" << params[0] << "\"\n";
    int result = writePrimesFile(params[0], primes, maxPrime);
    if (result == ERROR)
    {
        std::cout << "Failed to write Primes to: \"" << params[0] << "\"\n";
        std::cout << "Displaying result instead:\n";
        displayPrimes(primes, maxPrime);
    }

#ifdef DEBUG
    if (result != ERROR)
        displayPrimes(primes, maxPrime);
#endif

    delete[] primes;

    return OK;
}

/******************************************************************************
 * setParams()
 * - input: DELIM separated buffer
 * - splits the buffer and sets the "params" to the new passed in parameters
 * - this function only works if all the parameters are passed in to be set
 ******************************************************************************/
int setParams(const char* buffer)
{
    int bufferSize = strlen(buffer) + 1; // +1 for '\0'

    // if the buffer is empty, then just return
    if (bufferSize == 1 && NUM_ARGS == 0)
        return OK;

    // check if the passed in buffer will fit in our params
    if (bufferSize > NUM_ARGS * BUFFER_SIZE)
        return ERROR;

    // count the number of arguments by counting the number of delimiters
    int commas = 0;
    const char* p;
    for (p = buffer; *p; p++)
        if (*p == DELIM)
            commas++;

    if (commas + 1 == NUM_ARGS)
    {
        // make a copy of the input buffer (so strtok doesnt change the original)
        char buf[bufferSize];
        strcpy(buf, buffer);
        char* arg;
        int i;

        // copy the arguments to the params array
        arg = strtok(buf, DELIM_STR);
        for (i = 0; i < NUM_ARGS && arg; ++i)
        {
            strcpy(params[i], arg);
            arg = strtok(nullptr, DELIM_STR);
        }

        return OK; // OK
    }
    return ERROR; // NOT_OK
}

/******************************************************************************
 * getParams()
 * - builds a DELIM delimited string of the current parameter values
 * - sets the "buffer" input to the newly built cstring
 ******************************************************************************/
int getParams(char* buffer, int bufferSize)
{
    int i;

    // count the size needed
    int size = 0;
    for (i = 0; i < NUM_ARGS; i++)
        size += strlen(params[i]);
    size += NUM_ARGS - 1;

    // the size of the array must be big enough
    if (bufferSize < size || bufferSize < 1)
        return ERROR; // ERROR

    // join the arguments into a DELIM separated list
    buffer[0] = '\0';
    for (i = 0; i < NUM_ARGS; i++)
    {
        strcat(buffer, params[i]);
        strcat(buffer, DELIM_STR); // NOTE: could use DC1 as delimeter
    }
    buffer[size] = '\0'; // delete the last inserted comma
    return OK;           // OK
}

/******************************************************************************
 * returns info / help on how to use this plugin
 ******************************************************************************/
void* displayPluginInfo()
{
    std::cout << "This program runs on the GPU and generates prime numbers.\n"
              << "There's two settings for this plugin. They are as follows:\n"
              << "\t* limit - an integer for the upper number limit to check for primality\n"
              << "\t* outputFile - the file where results will be store\n"
              << "By default, the generated primes won't be displayed to the screen.\n"
              << "However, if an error occurs while writing the results to the file, this program\n"
              << "will display the generated list of prime numbers.\n";
    return nullptr;
}

/******************************************************************************
 * returns a comma-separated list of the parameter names
 ******************************************************************************/
const char* getParamInfo()
{
    return PARAM_INFO;
}

/******************************************************************************
 * returns the number of arguments this plugin contains
 ******************************************************************************/
const int getNumArgs()
{
    return NUM_ARGS;
}

/******************************************************************************
 * returns how many milliseconds it took to run the main program
 ******************************************************************************/
clock_t getRunTime()
{
    return total_t;
}


#include <boost/numeric/odeint/stepper/bulirsch_stoer_dense_out.hpp>


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

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

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

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

 /*!

    @ingroup group-trigonometric
    Function object implementing rem_2pi capabilities

    compute the remainder modulo \f$2\pi\f$.

    the result is in \f$[-\pi, \pi]\f$. If the input
    is near \f$\pi\f$ the output can be \f$\pi\f$ or \f$-\pi\f$
    depending
    on register disponibility if extended arithmetic is used.

    @par Semantic:

    For every parameter of floating type T

    @code
    T r = rem_2pi(x);
    @endcode

    is similar to:

    @code
    T r = remainder(x, Twopi<T>();
    @endcode

    @see rem_pio2, rem_pio2_straight, rem_pio2_cephes,  rem_pio2_medium,

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

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

#endif


#include "PF.h"
#include <string>

#include <Eigen/Core>
#include <Eigen/Eigenvalues>
#include <localization/lza.hpp>
#include <pid/rpath.h>
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <iostream>

#include "Time_management.h"
#include "Formatings.h"
#include "Geometrie.h"

using namespace std;

//With US
//void PFThread(SafeShared<int> &Mode, SafeShared<euclid_position> &Odopos, SafeShared<laser_4m_record> &las1, SafeShared<laser_4m_record> &las2, SafeShared<std::vector<struct polar_coordinate>> &US, bool verbose, bool Logging,SafeShared<euclid_position> &Estimation){
//Without US
void PFThread(SafeShared<int> &Mode, SafeShared<euclid_position> &odom, SafeShared<laser_4m_record> &las1, SafeShared<laser_4m_record> &las2, bool verbose, bool Logging, SafeShared<euclid_position> &Estimation){
/*********INIT VARIABLES************/
int presentMode;
struct timespec StartTime, EndTime, Time, LauncheTime, TimeEstime;
struct timespec Las1Old, Las2Old, Las1Time, Las2Time;
struct timespec Todo, Toldodo;
struct timespec TimeLog;
laser_4m_record relevel1, relevel2;
euclid_position odoposP3D,odoposP3D_Old, Val_estime;

/*
odoposP3D_Old.pos.x = 0;
odoposP3D_Old.pos.y = 0;
odoposP3D_Old.dir.theta = 0;
*/
std::vector<struct polar_coordinate> Telemetries;
std::vector<struct polar_coordinate> OldLas1,OldLas2;
int Running=0;
double lsleep;

std::vector<double> Logs;
std::vector<double> LZA_LAS;
std::vector<struct timespec> LogTime;
std::vector<int> LogMode;

int N = 600; //particle number (need to be an even number)
double portee = 4; //portee of sensors

vector<vector<double>> particles1; //particles in the first part of the map
vector<vector<double>> particles2; //particles in the second part of the map
vector<vector<double>> particles; //our particles (combination of both at first)

vector<double> iNextGeneration(N,0.0);
vector<double> poids(N,1/N); //vector of N element all equal to 1/N
double sumPoids;
// for estimating laser readings of particles
int NR = 16;//number of rays
vector<double> rhoParticles(3,0.0);
vector<double> ximp(NR);
vector<double> yimp(NR);

vector<double> theta=linspace(-M_PI, M_PI, NR+1);
theta.pop_back(); //delete last element as it's the same as the first one
for(int i=0; i!=theta.size(); i++)
{
  // round angles to the closest angle of the 1022 rayes
  theta[i] = (round(theta[i]*(1022/(2*M_PI)))*(2*M_PI/1022));
  theta[i] = wrapAngle(theta[i]);
}
vector<int> indexes;

bool firstIteration = true; //bool if it's first iteration of the simulation
bool flagConvergance = false; // flag for convergance
bool flagRedistribution = false; // flag for redistribution

euclid_position tempParticle; //temporary particle to be used in control part

vector<double> rhoRobot(NR,0); //get robot measurement to be used in likelihood

double wsd [3] = {0,0,0}; //weighted standard deviation for x, y and theta
double meanX = 0.0; // mean of all particles x position
double meanY = 0.0; // mean of all particles y position
double meanTheta = 0.0; // mean of all particles theta position

double poseEstimate[3]={0.0,0.0,0.0}; //estimated position of the robot

double ssl=0.0; //right and left wheel displacement
double ssr=0.0;
/**********Get MAP file***************/
  std::string mapfolder = PID_PATH("PF_Files/obstacles.csv");
  std::cout<<"Map folder : "<<mapfolder<<std::endl;
	string fname = mapfolder;//"obstacles.csv";
	Obstacles Obstacles1;
	vector<vector<string>> obstacles;
	vector<string> row;
	string line, data;

	fstream file (fname, ios::in);
  std::cout << "PF: Lecture Carte---" << '\n';
	if(file.is_open())
	{
		while(getline(file, line))
		{
			row.clear();
			stringstream str(line);
			while(getline(str, data, ','))
			row.push_back(data);
			obstacles.push_back(row);
	}

	}
	else
	cout<<"Could not open the file\n";

  std::cout << "PF: Carte Lue---" << '\n';

	for(int i=0;i<obstacles.size();i++)
	{
		Obstacles1.posVertex[i][0][0] = stod(obstacles[i][0]);
		Obstacles1.posVertex[i][0][1] = stod(obstacles[i][1]);
		Obstacles1.posVertex[i][1][0] = stod(obstacles[i][2]);
		Obstacles1.posVertex[i][1][1] = stod(obstacles[i][3]);
		Obstacles1.centre[i][0] = stod(obstacles[i][4]);
		Obstacles1.centre[i][1] = stod(obstacles[i][5]);
		Obstacles1.distDetect[i] = stod(obstacles[i][6]);
		}

	//transform obstacles
	vector <vector<double>> GrandObstacle;
	vector <double> distDetect;
	for (int i = 0; i<217;i++)
	{
		vector<double> temp;
		temp.push_back(Obstacles1.posVertex[i][0][0]);
		temp.push_back(Obstacles1.posVertex[i][0][1]);
		GrandObstacle.push_back(temp);
		vector<double> temp1;
		temp1.push_back(Obstacles1.posVertex[i][1][0]);
		temp1.push_back(Obstacles1.posVertex[i][1][1]);
		GrandObstacle.push_back(temp1);
		distDetect.push_back(Obstacles1.distDetect[i]);
	}
  //Load and read the map.****************************************************************

  /**********INIT TIMERS*****************/
  clock_gettime(CLOCK_REALTIME, &Time);

  StartTime=Time;
  Las1Old=Time;
  Las2Old=Time;
  Todo=Time;
  Toldodo=Time;
  LauncheTime=Time;
  TimeEstime=Time;

  /***************INIT LOG File*************/
    std::string filename = PID_PATH("+PANORAMA_Log/"+std::to_string(LauncheTime.tv_sec)+"/PF_LOG.txt");
    std::ofstream savefile;
    if(Logging){
      savefile.open(filename,std::ios::out);
    }

    std::cout << "PF Mode : Save file ok \n";
    /***************MAIN LOOP*********************/
    std::cout << "PF Mode : Start Particules \n";
    particles1 = particleGenerator(26.5747,29.02,-0.269984,56,-M_PI,M_PI,N/2,Obstacles1,GrandObstacle);
    particles2 = particleGenerator(-5,26.5747,-0.269984,3,-M_PI,M_PI,N/2,Obstacles1,GrandObstacle);
    particles1.insert( particles1.end(), particles2.begin(), particles2.end());
    particles = particles1;
    std::cout << "PF Mode : Particules_rdy \n";
    odom.Get(odoposP3D_Old);

    /**********Ending INIT Thread*************/
    LauncheTime=Mode.Get(presentMode);
    Mode.Set(PF_IDLEMODE,LauncheTime);
    presentMode=PF_IDLEMODE;
    Las2Old=LauncheTime;
    Las1Old=LauncheTime;

    Running=1;
    while(Running){
        Mode.Get(presentMode);
        clock_gettime(CLOCK_REALTIME, &StartTime);

        if(verbose){
          std::cout << "PF Mode : "<< presentMode << '\n';
        }

        switch (presentMode) {
          case PF_HALTMODE:
            Running=0;
            break;
          case PF_IDLEMODE:
            break;
          case PF_INITMODE:
          case PF_RUNMODE:
            /*******************UPDATE SENSORS Reading (Odo, Las1+Las2)***********************/
            if(verbose){
              std::cout << "PF Mode : Start PF"<< '\n';
            }
            Todo=odom.Get(odoposP3D);
            std::cout << (Todo.tv_nsec+(Todo.tv_sec*1000000000)) << "\n";
            Las2Time=las2.Get(relevel2);
            Las1Time=las1.Get(relevel1);
            if (isafter(Las2Time,Las2Old))
            {
              //OldLas2=format_Telemetre(relevel2.data,relevel2.last_lenght,0.03,0,M_PI);
              OldLas2=format_Telemetre(relevel2.data,relevel2.last_lenght,0.0,0.0,M_PI);
              Las2Old=Las2Time;
            }
            if (isafter(Las1Time,Las1Old))
            {
//              OldLas1=format_Telemetre(relevel1.data,relevel1.last_lenght,0.03,0,0.7031*M_PI/180);
              OldLas1=format_Telemetre(relevel1.data,relevel1.last_lenght,0.0,0.0,0.0);
              Las1Old=Las1Time;
            }

            Telemetries.clear();
//            if(isafter(Las2Old,LauncheTime) && !(enoughttimepassed(StartTime, Las2Old, 1000))){
              Telemetries.insert(Telemetries.end(),OldLas2.begin(),OldLas2.end());
//            }
//            if(isafter(Las1Old,LauncheTime) &&  !(enoughttimepassed(StartTime, Las1Old, 1000))){
              Telemetries.insert(Telemetries.end(),OldLas1.begin(),OldLas1.end());
//            }
            if(verbose){
              std::cout << "PF Mode : Sensors read"<< '\n';
            }

            if (firstIteration)
            {
              //get indexes of Telemetries to use in simulation
              for (int i=0; i<NR; i++)
              {
                double min = 1000;
                for(int j=0; j<Telemetries.size();j++)
                {
                  if (abs(theta[i]-Telemetries[j].dir.theta)<0.02)
                    {
                      indexes.push_back(j);
                      break;
                    }/*
                    else
                    {
                      if (abs(theta[i]-Telemetries[j].dir.theta)<min)
                      min = abs(theta[i]-Telemetries[j].dir.theta);
                    }
                    if (j == Telemetries.size()-1)
                    {
                    std::cout << "PF : Angle non trouvee : "<< theta[i] << '\n';
                    std::cout << "PF : minimum trouvé : "<< min << '\n';
                    for(int k=0; k<Telemetries.size();k++)
                    {
                      if (abs(theta[i]-Telemetries[k].dir.theta)<0.2)
                        {
                        std::cout << "PF : Val proche : "<< Telemetries[k].dir.theta<< " - i = "<< k  << '\n';
                        }
                    }
                  }*/
                }

              }
              firstIteration = false;
            }

            if(verbose){
              std::cout << "PF Mode : 1er itération ok"<< '\n';
            }
            /*******************************************************/
            /******************PF start*****************************/
            /*******************************************************/
            //control of particles:
            Recomput_Weeldist(odoposP3D_Old,odoposP3D,ssl,ssr);
            if(verbose){
              std::cout << "PF Mode : weeldist ok"<< '\n';
//              std::cout << "PF Mode : weeldist ok"<< '\n';
            }
            odoposP3D_Old = odoposP3D;
            for (int j=0;j<N;j++)
            {
              //tempParticle is of type euclid_position
              tempParticle.pos.x = particles[j][0];
              tempParticle.pos.y = particles[j][1];
              tempParticle.dir.theta = particles[j][2];
              SimuOdo(tempParticle,tempParticle,ssl,ssr);
              particles[j][0]= tempParticle.pos.x;
              particles[j][1]= tempParticle.pos.y;
              particles[j][2]= tempParticle.dir.theta;
            }

            if(verbose){
              std::cout << "PF Mode : Particules simulated"<< '\n';
            }
            if(verbose){
              std::cout << "PF Mode : Lecture capteurs faite"<< '\n';
            }

            //start measurement for particles:
            int out=0;
            for (int k=0;k<N;k++)
            {
              if(verbose){
                std::cout << "PF Mode : Mesure simulee particule "<< k << '\n';
              }
              Mesure_act(portee,theta,GrandObstacle,distDetect,particles[k][0],particles[k][1],particles[k][2],rhoParticles,ximp,yimp);

              //calculate likelihood:
              if(isInBoxMax(particles[k][0],particles[k][1],Obstacles1,GrandObstacle))
              {
              poids[k] = likelihood(rhoRobot,rhoParticles);

            }
            else{
              out++;
              poids[k]=0;
            }

//              if(verbose){
//                std::cout << "PF Mode : Poid simulee particule "<< poids[k] << '\n';
//              }
            }

            if(verbose){
              std::cout << "PF Mode : Nombre de particule paume :" << out<< '\n';
            }

            if(verbose){
              std::cout << "PF Mode : Poids fait"<< '\n';
            }
            //calculate the estimated position:
            sumPoids = accumulate(poids.begin(), poids.end(), 0.0);
            poseEstimate[0] = 0.0;
            poseEstimate[1] = 0.0;
            poseEstimate[2] = 0.0;

            for (int i=0;i<N;i++)
            {
                poseEstimate[0] = poseEstimate[0]+particles[i][0]*poids[i];
                poseEstimate[1] = poseEstimate[1]+particles[i][1]*poids[i];
                poseEstimate[2] = poseEstimate[2]+particles[i][2]*poids[i];
            }
            poseEstimate[0] = poseEstimate[0]/sumPoids;
            poseEstimate[1] = poseEstimate[1]/sumPoids;
            poseEstimate[2] = poseEstimate[2]/sumPoids;

            if(verbose){
              std::cout << "PF Mode : Estime fait"<< '\n';
            }

            Val_estime.pos.x = poseEstimate[0];
            Val_estime.pos.y = poseEstimate[1];
            Val_estime.dir.theta = poseEstimate[2];
            clock_gettime(CLOCK_REALTIME, &TimeEstime);

            meanX = 0.0;
            meanY = 0.0;
            meanTheta = 0.0;
            // check for convergance
            for (int ii = 0; ii<N; ii++)
            {
                meanX += particles[ii][0];
                meanY += particles[ii][1];
                meanTheta += particles[ii][2];
            }

            meanX /= N*1.0;
            meanY /= N*1.0;
            meanTheta /= N*1.0;

            if(verbose){
              std::cout << "PF Mode : Means"<< '\n';
            }

            for (int ii = 0; ii<N; ii++)
            {
                wsd[0] += poids[ii]*(particles[ii][0]-meanX)*(particles[ii][0]-meanX)*1.0; // we didn't use pow because particles[ii][0] is a vector of double not a double
                wsd[1] += poids[ii]*(particles[ii][1]-meanY)*(particles[ii][1]-meanY)*1.0;
                wsd[2] += poids[ii]*(particles[ii][2]-meanTheta)*(particles[ii][2]-meanTheta)*1.0;
            }

            wsd[0] = sqrt(wsd[0]/((N-1)/N*sumPoids))*1.0;
            wsd[1] = sqrt(wsd[1]/((N-1)/N*sumPoids))*1.0;
            wsd[2] = sqrt(wsd[2]/((N-1)/N*sumPoids))*1.0;

            if(verbose){
              std::cout << "PF Mode : Ecart type ok"<< '\n';
            }

            if (wsd[0]<2 && wsd[1]<2 && wsd[2]<0.5)
            {
                //particles filter converged we can send info if we want
                flagConvergance = true;
            }
            else
            {
                flagConvergance = false;
            }

            //check for redistribution:
            if (wsd[0]<0.3 && wsd[1]<0.3)
            {
              flagRedistribution = true;
            }
            else
            {
              flagRedistribution = false;
            }

            if(verbose){
              std::cout << "PF Mode : Eclate flag ok"<< '\n';
            }
            flagRedistribution=false;

            iNextGeneration = selection(poids,N);
            if(iNextGeneration.size()==0){//Totalement paume
              flagRedistribution=true;
            }
              if(verbose){
                std::cout << "PF Mode : Selec ok"<< '\n';
              }

            if (flagRedistribution)
            {
              //redistribute particels
              particles1 = particleGenerator(26.5747,29.02,-0.269984,56,-M_PI,M_PI,N/2,Obstacles1,GrandObstacle);
              particles2 = particleGenerator(-5,26.5747,-0.269984,3,-M_PI,M_PI,N/2,Obstacles1,GrandObstacle);
              particles1.insert( particles1.end(), particles2.begin(), particles2.end() );
              particles = particles1;
              for (int k=0;k<N;k++)
              {
                  poids[k] = 1/N;
              }
            }
            else
            {
              //get the new particles
              cout << "nombre de particule a regenerer " << iNextGeneration.size() << endl;
              particles = testInext(iNextGeneration, particles, Obstacles1, GrandObstacle);
              if(verbose){
                std::cout << "PF Mode : test ok"<< '\n';
              }
            }

            if (presentMode == PF_RUNMODE && flagConvergance)
            {
              // send data if run mode and if we converged
              Estimation.Set(Val_estime,TimeEstime);
            }
            /*******************************************************/
            /******************PF end*******************************/
            /*******************************************************/
            break;
        }

        if(verbose){
          std::cout << "PF_Will log \n";

        }
        if(Logging){

          std::cout << "PF logging now \n";
          std::cout << "++++++++++++++++++++++++++++++++++++++++++++++++++ \n";
          std::cout << poseEstimate[0] << "\n";
          std::cout << poseEstimate[1] << "\n";
          std::cout << poseEstimate[2] << "\n";
          std::cout  << odoposP3D.pos.x << "\n";
          std::cout  << odoposP3D.pos.y << "\n";
          std::cout  << odoposP3D.dir.theta << "\n";
          std::cout << wsd[0] << "\n";
          std::cout << wsd[1] << "\n";
          std::cout << wsd[2] << "\n";
          std::cout  << *max_element(poids.begin(), poids.end()) << "\n"; //return the max wheight
          std::cout  << sumPoids << "\n"; //return sum of the wheights
          std::cout << "++++++++++++++++++++++++++++++++++++++++++++++++++ \n";
          clock_gettime(CLOCK_REALTIME, &TimeLog);
          /***************Logging**************/
          savefile << (StartTime.tv_nsec+(StartTime.tv_sec*1000000000)) << "," << presentMode << ",";
          savefile << (TimeLog.tv_nsec+(TimeLog.tv_sec*1000000000)) << ",";
          savefile << (Las1Old.tv_nsec+(Las1Old.tv_sec*1000000000)) << ",";
          savefile << (Las2Old.tv_nsec+(Las2Old.tv_sec*1000000000)) << ",";
          savefile << (Todo.tv_nsec+(Todo.tv_sec*1000000000)) << ",";
          savefile << (TimeEstime.tv_nsec+(TimeEstime.tv_sec*1000000000)) << ",";
          savefile << odoposP3D.pos.x << ",";
          savefile << odoposP3D.pos.y << ",";
          savefile << odoposP3D.dir.theta << ",";
          savefile << poseEstimate[0] << ",";
          savefile << poseEstimate[1] << ",";
          savefile << poseEstimate[2] << ",";
          savefile << wsd[0] << ","; //wheighted standard deviation for x
          savefile << wsd[1] << ","; //wheighted standard deviation for y
          savefile << wsd[2] << ","; //wheighted standard deviation for theta
          savefile << *max_element(poids.begin(), poids.end()) << ","; //return the max wheight
          savefile << sumPoids << ","; //return sum of the wheights
          savefile << N << ","; //return number of particles
          savefile << NR << ","; //return number of rayes
          savefile << flagConvergance << ",";
          savefile << flagRedistribution << ",";

        ///Anything
        savefile<<"\n";
        }

        //Computing for a 10Hz maximum frequency
        lsleep=delayusleep(StartTime, 100);
        if(lsleep<0){
          lsleep=0;
        }
        usleep(lsleep);
    }

    /****************Closing thread***************/
    if(Logging){
      savefile.close();
      std::cout << "PF log file closed at : " << filename << '\n';
    }
}


/*! \file solveeom.cpp
    \brief 単振り子に対して運動方程式を解くクラスの実装

    Copyright ©  2016 @dc1394 All Rights Reserved.
    (but this is originally adapted by Freddie Witherden for doublependulum.cpp from https://freddie.witherden.org/tools/doublependulum/ )
    This software is released under the BSD 2-Clause License.
*/
#include "solveeom.h"
#include <cmath>                                // for std::sin, std::cos
#include <fstream>                              // for std::ofstream
#include <boost/assert.hpp>                     // for BOOST_ASSERT
#include <boost/format.hpp>                     // for boost::format
#include <boost/math/constants/constants.hpp>   // for boost::math::constants::pi

namespace solveeom {
    // #region コンストラクタ

    SolveEOM::SolveEOM(float l, float m, float theta1_0, float theta2_0) :
        Theta1([this] { return static_cast<float>(x_[0]); }, [this](auto theta) { return x_[0] = theta; }),
        Theta2([this] { return static_cast<float>(x_[2]); }, [this](auto theta) { return x_[2] = theta; }),
        V1([this] { return static_cast<float>(x_[1]); }, [this](auto v) { return x_[1] = v; }),
        V2([this] { return static_cast<float>(x_[3]); }, [this](auto v) { return x_[3] = v; }),
        l_(l),
        m_(m),
        stepper_(SolveEOM::EPS, SolveEOM::EPS)
    {

        x_ = { theta1_0, 0.0, theta2_0, 0.0 };
    }

    // #endregion コンストラクタ

    // #region publicメンバ関数

    float SolveEOM::kinetic_energy() const
    {
        return static_cast<float>(
            m_ * l_ * l_ * (sqr(x_[1]) + 0.5 * sqr(x_[3])) +
            m_ * l_ * l_ * x_[1] * x_[3] * std::cos(x_[0] - x_[2]));
    }

    void SolveEOM::operator()(float dt, float * theta1, float * theta2)
    {
        boost::numeric::odeint::integrate_adaptive(
            stepper_,
            getEOM(),
            x_,
            0.0,
            static_cast<double>(dt),
            SolveEOM::DX);

        *theta1 = static_cast<float>(x_[0]);
        *theta2 = static_cast<float>(x_[2]);
    }

    void SolveEOM::operator()(double dt, std::string const & filename, double t)
    {
        std::ofstream result(filename);

        boost::numeric::odeint::integrate_const(
            stepper_,
            getEOM(),
            x_,
            0.0,
            t,
            dt,
            [&result, this](auto const & x, auto const t)
        {
			result << boost::format("%.3f, %.15f, %.15f, %.15f\n") % t % x[0] % x[2] % total_energy();
        });
    }

    float SolveEOM::potential_energy() const
    {
        return static_cast<float>(m_ * SolveEOM::g * l_ * (3.0 - 2.0 * std::cos(x_[0]) - std::cos(x_[2])));
    }

    // #endregion publicメンバ関数

    // #region privateメンバ関数

    std::function<void(SolveEOM::state_type const &, SolveEOM::state_type &, double const)> SolveEOM::getEOM() const
    {
        auto const eom = [this](state_type const & x, state_type & dxdt, double const)
        {
            // Delta is θ2 - θ1
            auto const delta = x[Num_eqns::THETA_2] - x[Num_eqns::THETA_1];

            // `Big-M' is the total mass of the system, m1 + m2;
            auto const M = 2.0 * m_;

            // Denominator expression for ω1
            auto den = M * l_ - m_ * l_ * std::cos(delta) * std::cos(delta);

            // dθ/dt = ω, by definition
            dxdt[Num_eqns::THETA_1] = x[Num_eqns::OMEGA_1];

            // Compute ω1
            dxdt[OMEGA_1] = (m_ * l_ * x[Num_eqns::OMEGA_1] * x[Num_eqns::OMEGA_1] * std::sin(delta) * std::cos(delta)
                + m_ * g * std::sin(x[Num_eqns::THETA_2]) * std::cos(delta)
                + m_ * l_ * x[Num_eqns::OMEGA_2] * x[Num_eqns::OMEGA_2] * std::sin(delta)
                - M * g * std::sin(x[THETA_1])) / den;

            // Again, dθ/dt = ω for θ2 as well
            dxdt[THETA_2] = x[OMEGA_2];

            // Multiply den by the length ratio of the two bobs
            den *= l_ / l_;

            // Compute ω2
            dxdt[Num_eqns::OMEGA_2] = (-m_ * l_ * x[Num_eqns::OMEGA_2] * x[Num_eqns::OMEGA_2] * std::sin(delta) * std::cos(delta)
                + M * g * std::sin(x[Num_eqns::THETA_1]) * std::cos(delta)
                - M * l_ * x[OMEGA_1] * x[Num_eqns::OMEGA_1] * std::sin(delta)
                - M * g * std::sin(x[Num_eqns::THETA_2])) / den;
        };

        return eom;
    }

	double SolveEOM::total_energy() const
	{
		auto const kinetic = m_ * l_ * l_ * (sqr(x_[1]) + 0.5 * sqr(x_[3])) + m_ * l_ * l_ * x_[1] * x_[3] * std::cos(x_[0] - x_[2]);
		auto const potential = m_ * SolveEOM::g * l_ * (3.0 - 2.0 * std::cos(x_[0]) - std::cos(x_[2]));

		return kinetic + potential;
	}

    // #endregion privateメンバ関数
}


#include <stan/math/rev.hpp>
#include <gtest/gtest.h>
#include <boost/numeric/odeint.hpp>
#include <test/unit/math/rev/functor/util_cvodes_adams.hpp>
#include <test/unit/math/prim/functor/harmonic_oscillator.hpp>
#include <test/unit/math/prim/functor/forced_harmonic_oscillator.hpp>
#include <test/unit/math/prim/functor/lorenz.hpp>
#include <iostream>
#include <sstream>
#include <vector>
#include <string>

template <typename F, typename T_y0, typename T_theta>
void sho_value_test(F harm_osc, std::vector<double>& y0, double t0,
                    std::vector<double>& ts, std::vector<double>& theta,
                    std::vector<double>& x, std::vector<int>& x_int) {
  using stan::math::promote_scalar;
  using stan::math::var;

  std::vector<std::vector<var> > ode_res_vd = stan::math::integrate_ode_adams(
      harm_osc, promote_scalar<T_y0>(y0), t0, ts,
      promote_scalar<T_theta>(theta), x, x_int);

  EXPECT_NEAR(0.995029, ode_res_vd[0][0].val(), 1e-5);
  EXPECT_NEAR(-0.0990884, ode_res_vd[0][1].val(), 1e-5);

  EXPECT_NEAR(-0.421907, ode_res_vd[99][0].val(), 1e-5);
  EXPECT_NEAR(0.246407, ode_res_vd[99][1].val(), 1e-5);
}

void sho_finite_diff_test(double t0) {
  using stan::math::var;
  harm_osc_ode_fun harm_osc;

  std::vector<double> theta;
  theta.push_back(0.15);

  std::vector<double> y0;
  y0.push_back(1.0);
  y0.push_back(0.0);

  std::vector<double> ts;
  for (int i = 0; i < 100; i++)
    ts.push_back(t0 + 0.1 * (i + 1));

  std::vector<double> x;
  std::vector<int> x_int;

  test_ode_cvode(harm_osc, t0, ts, y0, theta, x, x_int, 1e-8, 1e-4);

  sho_value_test<harm_osc_ode_fun, double, var>(harm_osc, y0, t0, ts, theta, x,
                                                x_int);
  sho_value_test<harm_osc_ode_fun, var, double>(harm_osc, y0, t0, ts, theta, x,
                                                x_int);
  sho_value_test<harm_osc_ode_fun, var, var>(harm_osc, y0, t0, ts, theta, x,
                                             x_int);
}

void sho_data_finite_diff_test(double t0) {
  using stan::math::var;
  harm_osc_ode_data_fun harm_osc;

  std::vector<double> theta;
  theta.push_back(0.15);

  std::vector<double> y0;
  y0.push_back(1.0);
  y0.push_back(0.0);

  std::vector<double> ts;
  for (int i = 0; i < 100; i++)
    ts.push_back(t0 + 0.1 * (i + 1));

  std::vector<double> x(3, 1);
  std::vector<int> x_int(2, 0);

  test_ode_cvode(harm_osc, t0, ts, y0, theta, x, x_int, 1e-8, 1e-4);

  sho_value_test<harm_osc_ode_data_fun, double, var>(harm_osc, y0, t0, ts,
                                                     theta, x, x_int);
  sho_value_test<harm_osc_ode_data_fun, var, double>(harm_osc, y0, t0, ts,
                                                     theta, x, x_int);
  sho_value_test<harm_osc_ode_data_fun, var, var>(harm_osc, y0, t0, ts, theta,
                                                  x, x_int);
}

template <typename T_y0, typename T_theta, typename F>
void sho_error_test(F harm_osc, std::vector<double>& y0, double t0,
                    std::vector<double>& ts, std::vector<double>& theta,
                    std::vector<double>& x, std::vector<int>& x_int,
                    std::string error_msg) {
  using stan::math::promote_scalar;
  using stan::math::var;

  EXPECT_THROW_MSG(stan::math::integrate_ode_adams(
                       harm_osc, promote_scalar<T_y0>(y0), t0, ts,
                       promote_scalar<T_theta>(theta), x, x_int),
                   std::invalid_argument, error_msg);
}

TEST(StanAgradRevOde_integrate_ode_adams, harmonic_oscillator_finite_diff) {
  sho_finite_diff_test(0);
  sho_finite_diff_test(2.0);
  sho_finite_diff_test(-2.0);

  sho_data_finite_diff_test(0);
  sho_data_finite_diff_test(2.5);
  sho_data_finite_diff_test(-2.5);
}

TEST(StanAgradRevOde_integrate_ode_adams, harmonic_oscillator_error) {
  using stan::math::var;
  harm_osc_ode_wrong_size_1_fun harm_osc;

  std::vector<double> theta;
  theta.push_back(0.15);

  std::vector<double> y0;
  y0.push_back(1.0);
  y0.push_back(0.0);

  double t0 = 0;
  std::vector<double> ts;
  for (int i = 0; i < 100; i++)
    ts.push_back(t0 + 0.1 * (i + 1));

  std::vector<double> x(3, 1);
  std::vector<int> x_int(2, 0);

  // aligned error handling with non-stiff case
  std::string error_msg
      = "cvodes_ode_data: dz_dt (3) and states (2) must match in size";

  sho_error_test<double, var>(harm_osc, y0, t0, ts, theta, x, x_int, error_msg);
  sho_error_test<var, double>(harm_osc, y0, t0, ts, theta, x, x_int, error_msg);
  sho_error_test<var, var>(harm_osc, y0, t0, ts, theta, x, x_int, error_msg);
}

TEST(StanAgradRevOde_integrate_ode_adams, time_steps_as_param) {
  using stan::math::integrate_ode_adams;
  using stan::math::to_var;

  const double t0 = 0.0;
  harm_osc_ode_fun ode;
  std::vector<double> theta{0.15};
  std::vector<double> y0{1.0, 0.0};
  std::vector<stan::math::var> ts;
  for (int i = 0; i < 100; i++)
    ts.push_back(t0 + 0.1 * (i + 1));
  std::vector<double> x;
  std::vector<int> x_int;
  std::vector<stan::math::var> y0v = to_var(y0);
  std::vector<stan::math::var> thetav = to_var(theta);
  stan::math::var t0v = 0.0;

  std::vector<std::vector<stan::math::var> > res;
  auto test_val = [&res]() {
    EXPECT_NEAR(0.995029, res[0][0].val(), 1e-5);
    EXPECT_NEAR(-0.0990884, res[0][1].val(), 1e-5);
    EXPECT_NEAR(-0.421907, res[99][0].val(), 1e-5);
    EXPECT_NEAR(0.246407, res[99][1].val(), 1e-5);
  };
  res = integrate_ode_adams(ode, y0, t0, ts, theta, x, x_int);
  test_val();
  res = integrate_ode_adams(ode, y0v, t0, ts, theta, x, x_int);
  test_val();
  res = integrate_ode_adams(ode, y0, t0, ts, thetav, x, x_int);
  test_val();
  res = integrate_ode_adams(ode, y0v, t0, ts, thetav, x, x_int);
  test_val();
  res = integrate_ode_adams(ode, y0, t0v, ts, theta, x, x_int);
  test_val();
  res = integrate_ode_adams(ode, y0v, t0v, ts, theta, x, x_int);
  test_val();
  res = integrate_ode_adams(ode, y0, t0v, ts, thetav, x, x_int);
  test_val();
  res = integrate_ode_adams(ode, y0v, t0v, ts, thetav, x, x_int);
  test_val();
}

TEST(StanAgradRevOde_integrate_ode_adams, time_steps_as_param_AD) {
  using stan::math::integrate_ode_adams;
  using stan::math::to_var;
  using stan::math::value_of;
  using stan::math::var;
  const double t0 = 0.0;
  const int nt = 100;  // nb. of time steps
  const int ns = 2;    // nb. of states
  std::ostream* msgs = NULL;

  forced_harm_osc_ode_fun ode;

  std::vector<double> theta{0.15, 0.25};
  std::vector<double> y0{1.0, 0.0};
  std::vector<stan::math::var> ts;
  for (int i = 0; i < nt; i++)
    ts.push_back(t0 + 0.1 * (i + 1));

  std::vector<double> x;
  std::vector<int> x_int;
  std::vector<stan::math::var> y0v = to_var(y0);
  std::vector<stan::math::var> thetav = to_var(theta);

  std::vector<std::vector<stan::math::var> > res;
  std::vector<double> g;
  auto test_ad = [&res, &g, &ts, &ode, &nt, &ns, &theta, &x, &x_int, &msgs]() {
    for (auto i = 0; i < nt; ++i) {
      std::vector<double> res_d = value_of(res[i]);
      for (auto j = 0; j < ns; ++j) {
        g.clear();
        res[i][j].grad(ts, g);
        for (auto k = 0; k < nt; ++k) {
          if (k != i) {
            EXPECT_FLOAT_EQ(g[k], 0.0);
          } else {
            std::vector<double> y0(res_d.begin(), res_d.begin() + ns);
            EXPECT_FLOAT_EQ(g[k],
                            ode(ts[i].val(), y0, theta, x, x_int, msgs)[j]);
          }
        }
        stan::math::set_zero_all_adjoints();
      }
    }
  };
  res = integrate_ode_adams(ode, y0, t0, ts, theta, x, x_int);
  test_ad();
  res = integrate_ode_adams(ode, y0v, t0, ts, theta, x, x_int);
  test_ad();
  res = integrate_ode_adams(ode, y0, t0, ts, thetav, x, x_int);
  test_ad();
  res = integrate_ode_adams(ode, y0v, t0, ts, thetav, x, x_int);
  test_ad();
}

TEST(StanAgradRevOde_integrate_ode_adams, t0_as_param_AD) {
  using stan::math::integrate_ode_adams;
  using stan::math::to_var;
  using stan::math::value_of;
  using stan::math::var;
  const double t0 = 0.0;
  const int nt = 100;  // nb. of time steps
  const int ns = 2;    // nb. of states
  std::ostream* msgs = NULL;

  forced_harm_osc_ode_fun ode;

  std::vector<double> theta{0.15, 0.25};
  std::vector<double> y0{1.0, 0.0};
  std::vector<double> ts;
  for (int i = 0; i < nt; i++)
    ts.push_back(t0 + 0.1 * (i + 1));

  std::vector<double> x;
  std::vector<int> x_int;
  std::vector<stan::math::var> y0v = to_var(y0);
  std::vector<stan::math::var> thetav = to_var(theta);
  stan::math::var t0v = 0.0;

  std::vector<std::vector<stan::math::var> > res;
  auto test_ad = [&res, &t0v, &ode, &nt, &ns, &theta, &x, &x_int, &msgs]() {
    for (auto i = 0; i < nt; ++i) {
      std::vector<double> res_d = value_of(res[i]);
      for (auto j = 0; j < ns; ++j) {
        res[i][j].grad();
        for (auto k = 0; k < nt; ++k) {
          EXPECT_FLOAT_EQ(t0v.adj(), 0.0);
        }
        stan::math::set_zero_all_adjoints();
      }
    }
  };
  res = integrate_ode_adams(ode, y0, t0v, ts, theta, x, x_int);
  test_ad();
  res = integrate_ode_adams(ode, y0v, t0v, ts, theta, x, x_int);
  test_ad();
  res = integrate_ode_adams(ode, y0, t0v, ts, thetav, x, x_int);
  test_ad();
  res = integrate_ode_adams(ode, y0v, t0v, ts, thetav, x, x_int);
  test_ad();
}


/*
 * Complex arrays example
 */

#include <blitz/array.h>

BZ_USING_NAMESPACE(blitz)

const double pi = 3.14159265358979323846264338327950288;

#ifndef BZ_HAVE_COMPLEX
int main()
{
    cout << "Complex number support is required to compile this example."
         << endl;
    return 0;
}

#else

int main()
{
    const int N = 16;

    Array<complex<double>,1> A(N);
    Array<double,1> theta(N);

    BZ_USING_NAMESPACE(blitz::tensor);

    // Fill the theta array with angles from 0..2 Pi, evenly spaced
    theta = (2 * pi * i) / N;

    // Set A[i] = cos(theta[i]) + _I * sin(theta[i])
    A = zip(cos(theta), sin(theta), complex<double>());

    cout << A << endl;

#ifdef BZ_HAVE_COMPLEX_MATH1
    // Here's another way of doing it, which eliminates the need for
    // the theta array:
    // Set A[i] = exp(Pi i _I / N)
    A = exp(zip(0.0, (2 * pi * i) / N, complex<double>()));

    cout << A << endl;
#endif

    return 0;
}
#endif // BZ_HAVE_COMPLEX


#include <cassert>
#include <armadillo>

void repack_matrix_to_vector_fast(arma::vec &v, const arma::mat &m)
{

    const size_t d1 = m.n_cols;
    const size_t d2 = m.n_rows;
    const size_t dv = d1 * d2;
    assert(v.n_elem == dv);

    size_t ia;
    for (size_t i = 0; i < d1; i++) {
        for (size_t a = 0; a < d2; a++) {
            ia = i*d2 + a;
            v(ia) = m(a, i);
        }
    }

    return;

}

void repack_matrix_to_vector(arma::vec &v, const arma::mat &m)
{

    const size_t d1 = m.n_rows;
    const size_t d2 = m.n_cols;
    const size_t dv = d1 * d2;
    assert(v.n_elem == dv);

    size_t ia;
    for (size_t i = 0; i < d1; i++) {
        for (size_t a = 0; a < d2; a++) {
            ia = i*d2 + a;
            v(ia) = m(i, a);
        }
    }

    return;

}

void repack_vector_to_matrix(arma::mat &m, const arma::vec &v)
{

    const size_t d1 = m.n_rows;
    const size_t d2 = m.n_cols;
    const size_t dv = d1 * d2;
    assert(v.n_elem == dv);

    size_t ia;
    for (size_t i = 0; i < d1; i++) {
        for (size_t a = 0; a < d2; a++) {
            ia = i*d2 + a;
            m(i, a) = v(ia);
        }
    }

    return;
}

int main() {

    const size_t d1 = 3;
    const size_t d2 = 4;

    arma::mat m(d1, d2);
    arma::vec v(d1 * d2);

    size_t ia;
    for (size_t i = 0; i < d1; i++) {
        for (size_t a = 0; a < d2; a++) {
            ia = i*d2 + a;
            m(i, a) = ia;
        }
    }

    m.print("m");
    repack_matrix_to_vector(v, m);
    v.print("v");

    arma::vec v2(d1 * d2);
    arma::mat m2 = m.t();
    repack_matrix_to_vector_fast(v2, m2);
    m2.print("m2");
    v2.print("v2");

    arma::vec mv = arma::vectorise(m);
    mv.print("vectorise(m)");
    // otherwise we get a rowvec!
    arma::vec mv1 = arma::vectorise(m, 1).t();
    mv1.print("vectorise(m, 1)");

    arma::vec mv2 = arma::vectorise(m2);
    mv2.print("vectorise(m2)");

    return 0;
}


//////////////////////////////////////////////////////////////////////////////////////
// distribution::toolkit::distributions::students_t::description.hpp                 //
//                                                                                //
//  (C) Copyright 2009 Erwann Rogard                                                //
//  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_STATISTICS_DETAIL_DISTRIBUTION_TOOLKIT_STUDENTS_T_DESCRIPTION_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_DISTRIBUTION_TOOLKIT_STUDENTS_T_DESCRIPTION_HPP_ER_2009
#include <string>
#include <boost/format.hpp>
#include <boost/math/distributions/students_t.hpp>

namespace boost{
namespace math{

    template<typename T,typename P>
    std::string
    description(const boost::math::students_t_distribution<T,P>& dist)
    {
        static const char* msg = "students_t(%1%)";
        format f(msg); f%dist.degrees_of_freedom();
        return f.str();
    }

}// math
}// boost

#endif


/*! \file foelement.cpp
    \brief Bogoliubov-de Gennes方程式を解くクラスの実装
    Copyright ©  2016 @dc1394 All Rights Reserved.
    (but this is originally adapted by cometscome for Chev.py from https://github.com/cometscome/ChebyshevPolynomialBdG )
    This software is released under the BSD 2-Clause License.
*/

#include "chev.h"
#include <cmath>                // for std::acos, for std::sin
#include <iomanip>              // for std::setprecision
#include <iostream>             // for std::cout
#include <vector>               // for std::vector
#include <Eigen/EigenValues>    // for Eigen::SelfAdjointEigenSolver

namespace chebyshevpolynomialbdg {
    // #region コンストラクタ

    Chev::Chev()
        :   vec_ai_(Chev::NC),
            vec_delta_(Chev::LN_2, Chev::LN_2)
    {
        std::cout.setf(std::ios::fixed, std::ios::floatfield);
    }

    // #endregion コンストラクタ

    // #region publicメンバ関数

    void Chev::iteration(bool full)
    {
        std::cout << std::setprecision(15);

        init_delta();
        calc_A();
        Eigen::SparseMatrix<double> vec_delta_old(vec_delta_);

        for (auto ite = 0; ite < ITERMAX; ite++) {
            if (full) {
                calc_meanfields<true>();
            }
            else {
                calc_meanfields<false>();
            }

            vec_delta_ = vec_delta_ * Chev::U;

            calc_A2();

            auto eps = 0.0;
            auto nor = 0.0;
            for (auto i = 0; i < Chev::LN_2; i++) {
                eps += sqr(vec_delta_.coeff(i, i) - vec_delta_old.coeff(i, i));
                nor += sqr(vec_delta_old.coeff(i, i));
            }

            eps /= nor;

            std::cout << "ite = " << ite << ", eps = " << eps << '\n';
            if (eps <= Chev::EPSTHRESHOLD) {
                std::cout << "End " << vec_delta_.coeff(Chev::NX / 2, Chev::NY / 2) << std::endl;
                break;
            }

            vec_delta_old = vec_delta_;
        }
    }

    // #region publicメンバ関数

    // #region privateメンバ関数

    void Chev::calc_A()
    {
        A_ = Eigen::SparseMatrix<double>(Chev::LN, Chev::LN);

        for (auto ix = 0; ix < Chev::NX; ix++) {
            for (auto iy = 0; iy < Chev::NY; iy++) {
                // A_.setdiag(-mu)
                auto const ii = xy2i(ix, iy);
                auto jx = ix;
                auto jy = iy;
                auto jj = xy2i(jx, jy);
                A_.coeffRef(ii, jj) = -Chev::MYU;

                // +1 in x direction
                jx = ix + 1;

                if (jx == Chev::NX) {
                    jx = 0;
                }

                jy = iy;
                jj = xy2i(jx, jy);
                A_.coeffRef(ii, jj) = -1.0;

                // -1 in x direction
                jx = ix - 1;

                if (jx == -1) {
                    jx = Chev::NX - 1;
                }

                jy = iy;
                jj = xy2i(jx, jy);

                A_.coeffRef(ii, jj) = -1.0;

                // + 1 in y direction
                jx = ix;
                jy = iy + 1;

                if (jy == Chev::NY) {
                    jy = 0;
                }

                jj = xy2i(jx, jy);
                A_.coeffRef(ii, jj) = -1.0;

                // -1 in y direction
                jx = ix;
                jy = iy - 1;
                if (jy == -1) {
                    jy = Chev::NY - 1;
                }
                jj = xy2i(jx, jy);
                A_.coeffRef(ii, jj) = -1.0;

                for (auto i = 0; i < Chev::LN_2; i++) {
                    for (auto j = 0; j < Chev::LN_2; j++) {
                        A_.coeffRef(i + Chev::LN_2, j + Chev::LN_2) = -A_.coeff(i, j);
                        A_.coeffRef(i, j + Chev::LN_2) = vec_delta_.coeff(i, j);
                        A_.coeffRef(i + Chev::LN_2, j) = vec_delta_.coeff(j, i);
                    }
                }
            }
        }

        A_ /= Chev::AA;
    }

    void Chev::calc_A2()
    {
        A_ *= Chev::AA;

        for (auto i = 0; i < Chev::LN_2; i++) {
            for (auto j = 0; j < Chev::LN_2; j++) {
                A_.coeffRef(i, j + Chev::LN_2) = vec_delta_.coeff(i, j);
                A_.coeffRef(i + Chev::LN_2, j) = vec_delta_.coeff(j, i);
            }
        }

        A_ /= Chev::AA;
    }

    double Chev::calc_meanfield() const
    {
        auto const ba = std::acos(-Chev::BB / Chev::AA);
        auto const omeb = std::acos(-(Chev::OMEGAC + Chev::BB) / Chev::AA);

        auto density = 0.0;
        for (auto j = 0; j < Chev::NC - 1; j++) {
            auto const i = j + 1;
            density += vec_ai_[i] * (std::sin(static_cast<double>(i) * omeb) - std::sin(static_cast<double>(i) * ba)) / static_cast<double>(i);
            density += vec_ai_[0] * (omeb - ba) / 2.0;
        }

        return density * 2.0 / Chev::PI;
    }

    void Chev::calc_polynomials(std::int32_t left_i, std::int32_t right_j)
    {
        Eigen::VectorXd vec_jn(Chev::LN), vec_jnm(Chev::LN), vec_jnmm(Chev::LN);
        vec_jn.fill(0.0);
        vec_jnm.fill(0.0);
        vec_jnmm.fill(0.0);

        vec_jn.coeffRef(right_j) = 1.0;
        vec_ai_.fill(0.0);

        auto A = A_.toDense();
        for (auto n = 0; n < Chev::NC; n++) {
            switch (n) {
            case 0:
                vec_jnm.resize(1);
                vec_jnm.fill(0.0);
                vec_jnmm.resize(1);
                vec_jnmm.fill(0.0);
                vec_jn.coeffRef(right_j) = 1.0;
                break;

            case 1:
                vec_jn = A * vec_jn;
                break;

            default:
                vec_jn = 2.0 * A * vec_jnm - vec_jnmm;
                break;
            }

            vec_ai_[n] = vec_jn.coeff(left_i);
            vec_jnmm = vec_jnm;
            vec_jnm = vec_jn;
        }
    }

    void Chev::init_delta()
    {
        std::vector<Eigen::Triplet<double>> A(Chev::LN_2);
        for (auto i = 0; i < Chev::LN_2; i++) {
            A[i] = Eigen::Triplet<double>(i, i, Chev::DELTA);
        }

        vec_delta_.setFromTriplets(A.begin(), A.end());
    }

    std::int32_t Chev::xy2i(std::int32_t ix, std::int32_t iy) const
    {
        return iy * Chev::NX + ix;
    }

    // #endregion privateメンバ関数
}


// Copyright Michael Drexl 2005, 2006.
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://boost.org/LICENSE_1_0.txt)

#include <boost/config.hpp>

#ifdef BOOST_MSVC
#  pragma warning(disable: 4267)
#endif

#include <boost/graph/adjacency_list.hpp>
//#include <boost/graph/dijkstra_shortest_paths.hpp>

#include <boost/graph/r_c_shortest_paths.hpp>
#include <iostream>
#include <boost/test/minimal.hpp>

using namespace boost;

struct SPPRC_Example_Graph_Vert_Prop
{
  SPPRC_Example_Graph_Vert_Prop( int n = 0, int e = 0, int l = 0 )
  : num( n ), eat( e ), lat( l ) {}
  int num;
  // earliest arrival time
  int eat;
  // latest arrival time
  int lat;
};

struct SPPRC_Example_Graph_Arc_Prop
{
  SPPRC_Example_Graph_Arc_Prop( int n = 0, int c = 0, int t = 0 )
  : num( n ), cost( c ), time( t ) {}
  int num;
  // traversal cost
  int cost;
  // traversal time
  int time;
};

typedef adjacency_list<vecS,
                       vecS,
                       directedS,
                       SPPRC_Example_Graph_Vert_Prop,
                       SPPRC_Example_Graph_Arc_Prop>
  SPPRC_Example_Graph;

// data structures for spp without resource constraints:
// ResourceContainer model
struct spp_no_rc_res_cont
{
  spp_no_rc_res_cont( int c = 0 ) : cost( c ) {};
  spp_no_rc_res_cont& operator=( const spp_no_rc_res_cont& other )
  {
    if( this == &other )
      return *this;
    this->~spp_no_rc_res_cont();
    new( this ) spp_no_rc_res_cont( other );
    return *this;
  }
  int cost;
};

bool operator==( const spp_no_rc_res_cont& res_cont_1,
                 const spp_no_rc_res_cont& res_cont_2 )
{
  return ( res_cont_1.cost == res_cont_2.cost );
}

bool operator<( const spp_no_rc_res_cont& res_cont_1,
                const spp_no_rc_res_cont& res_cont_2 )
{
  return ( res_cont_1.cost < res_cont_2.cost );
}

// ResourceExtensionFunction model
class ref_no_res_cont
{
public:
  inline bool operator()( const SPPRC_Example_Graph& g,
                          spp_no_rc_res_cont& new_cont,
                          const spp_no_rc_res_cont& old_cont,
                          graph_traits
                            <SPPRC_Example_Graph>::edge_descriptor ed ) const
  {
    new_cont.cost = old_cont.cost + g[ed].cost;
    return true;
  }
};

// DominanceFunction model
class dominance_no_res_cont
{
public:
  inline bool operator()( const spp_no_rc_res_cont& res_cont_1,
                          const spp_no_rc_res_cont& res_cont_2 ) const
  {
    // must be "<=" here!!!
    // must NOT be "<"!!!
    return res_cont_1.cost <= res_cont_2.cost;
    // this is not a contradiction to the documentation
    // the documentation says:
    // "A label $l_1$ dominates a label $l_2$ if and only if both are resident
    // at the same vertex, and if, for each resource, the resource consumption
    // of $l_1$ is less than or equal to the resource consumption of $l_2$,
    // and if there is at least one resource where $l_1$ has a lower resource
    // consumption than $l_2$."
    // one can think of a new label with a resource consumption equal to that
    // of an old label as being dominated by that old label, because the new
    // one will have a higher number and is created at a later point in time,
    // so one can implicitly use the number or the creation time as a resource
    // for tie-breaking
  }
};
// end data structures for spp without resource constraints:

// data structures for shortest path problem with time windows (spptw)
// ResourceContainer model
struct spp_spptw_res_cont
{
  spp_spptw_res_cont( int c = 0, int t = 0 ) : cost( c ), time( t ) {}
  spp_spptw_res_cont& operator=( const spp_spptw_res_cont& other )
  {
    if( this == &other )
      return *this;
    this->~spp_spptw_res_cont();
    new( this ) spp_spptw_res_cont( other );
    return *this;
  }
  int cost;
  int time;
};

bool operator==( const spp_spptw_res_cont& res_cont_1,
                 const spp_spptw_res_cont& res_cont_2 )
{
  return ( res_cont_1.cost == res_cont_2.cost
           && res_cont_1.time == res_cont_2.time );
}

bool operator<( const spp_spptw_res_cont& res_cont_1,
                const spp_spptw_res_cont& res_cont_2 )
{
  if( res_cont_1.cost > res_cont_2.cost )
    return false;
  if( res_cont_1.cost == res_cont_2.cost )
    return res_cont_1.time < res_cont_2.time;
  return true;
}

// ResourceExtensionFunction model
class ref_spptw
{
public:
  inline bool operator()( const SPPRC_Example_Graph& g,
                          spp_spptw_res_cont& new_cont,
                          const spp_spptw_res_cont& old_cont,
                          graph_traits
                            <SPPRC_Example_Graph>::edge_descriptor ed ) const
  {
    const SPPRC_Example_Graph_Arc_Prop& arc_prop =
      get( edge_bundle, g )[ed];
    const SPPRC_Example_Graph_Vert_Prop& vert_prop =
      get( vertex_bundle, g )[target( ed, g )];
    new_cont.cost = old_cont.cost + arc_prop.cost;
    int& i_time = new_cont.time;
    i_time = old_cont.time + arc_prop.time;
    i_time < vert_prop.eat ? i_time = vert_prop.eat : 0;
    return i_time <= vert_prop.lat ? true : false;
  }
};

// DominanceFunction model
class dominance_spptw
{
public:
  inline bool operator()( const spp_spptw_res_cont& res_cont_1,
                          const spp_spptw_res_cont& res_cont_2 ) const
  {
    // must be "<=" here!!!
    // must NOT be "<"!!!
    return res_cont_1.cost <= res_cont_2.cost
           && res_cont_1.time <= res_cont_2.time;
    // this is not a contradiction to the documentation
    // the documentation says:
    // "A label $l_1$ dominates a label $l_2$ if and only if both are resident
    // at the same vertex, and if, for each resource, the resource consumption
    // of $l_1$ is less than or equal to the resource consumption of $l_2$,
    // and if there is at least one resource where $l_1$ has a lower resource
    // consumption than $l_2$."
    // one can think of a new label with a resource consumption equal to that
    // of an old label as being dominated by that old label, because the new
    // one will have a higher number and is created at a later point in time,
    // so one can implicitly use the number or the creation time as a resource
    // for tie-breaking
  }
};
// end data structures for shortest path problem with time windows (spptw)

int test_main(int argc, char* argv[])
{
  SPPRC_Example_Graph g;
  add_vertex( SPPRC_Example_Graph_Vert_Prop( 0, 0, 1000000000 ), g );
  add_vertex( SPPRC_Example_Graph_Vert_Prop( 1, 56, 142 ), g );
  add_vertex( SPPRC_Example_Graph_Vert_Prop( 2, 0, 1000000000 ), g );
  add_vertex( SPPRC_Example_Graph_Vert_Prop( 3, 89, 178 ), g );
  add_vertex( SPPRC_Example_Graph_Vert_Prop( 4, 0, 1000000000 ), g );
  add_vertex( SPPRC_Example_Graph_Vert_Prop( 5, 49, 76 ), g );
  add_vertex( SPPRC_Example_Graph_Vert_Prop( 6, 0, 1000000000 ), g );
  add_vertex( SPPRC_Example_Graph_Vert_Prop( 7, 98, 160 ), g );
  add_vertex( SPPRC_Example_Graph_Vert_Prop( 8, 0, 1000000000 ), g );
  add_vertex( SPPRC_Example_Graph_Vert_Prop( 9, 90, 158 ), g );
  add_edge( 0, 7, SPPRC_Example_Graph_Arc_Prop( 6, 33, 2 ), g );
  add_edge( 0, 6, SPPRC_Example_Graph_Arc_Prop( 5, 31, 6 ), g );
  add_edge( 0, 4, SPPRC_Example_Graph_Arc_Prop( 3, 14, 4 ), g );
  add_edge( 0, 1, SPPRC_Example_Graph_Arc_Prop( 0, 43, 8 ), g );
  add_edge( 0, 4, SPPRC_Example_Graph_Arc_Prop( 4, 28, 10 ), g );
  add_edge( 0, 3, SPPRC_Example_Graph_Arc_Prop( 1, 31, 10 ), g );
  add_edge( 0, 3, SPPRC_Example_Graph_Arc_Prop( 2, 1, 7 ), g );
  add_edge( 0, 9, SPPRC_Example_Graph_Arc_Prop( 7, 25, 9 ), g );
  add_edge( 1, 0, SPPRC_Example_Graph_Arc_Prop( 8, 37, 4 ), g );
  add_edge( 1, 6, SPPRC_Example_Graph_Arc_Prop( 9, 7, 3 ), g );
  add_edge( 2, 6, SPPRC_Example_Graph_Arc_Prop( 12, 6, 7 ), g );
  add_edge( 2, 3, SPPRC_Example_Graph_Arc_Prop( 10, 13, 7 ), g );
  add_edge( 2, 3, SPPRC_Example_Graph_Arc_Prop( 11, 49, 9 ), g );
  add_edge( 2, 8, SPPRC_Example_Graph_Arc_Prop( 13, 47, 5 ), g );
  add_edge( 3, 4, SPPRC_Example_Graph_Arc_Prop( 17, 5, 10 ), g );
  add_edge( 3, 1, SPPRC_Example_Graph_Arc_Prop( 15, 47, 1 ), g );
  add_edge( 3, 2, SPPRC_Example_Graph_Arc_Prop( 16, 26, 9 ), g );
  add_edge( 3, 9, SPPRC_Example_Graph_Arc_Prop( 21, 24, 10 ), g );
  add_edge( 3, 7, SPPRC_Example_Graph_Arc_Prop( 20, 50, 10 ), g );
  add_edge( 3, 0, SPPRC_Example_Graph_Arc_Prop( 14, 41, 4 ), g );
  add_edge( 3, 6, SPPRC_Example_Graph_Arc_Prop( 19, 6, 1 ), g );
  add_edge( 3, 4, SPPRC_Example_Graph_Arc_Prop( 18, 8, 1 ), g );
  add_edge( 4, 5, SPPRC_Example_Graph_Arc_Prop( 26, 38, 4 ), g );
  add_edge( 4, 9, SPPRC_Example_Graph_Arc_Prop( 27, 32, 10 ), g );
  add_edge( 4, 3, SPPRC_Example_Graph_Arc_Prop( 24, 40, 3 ), g );
  add_edge( 4, 0, SPPRC_Example_Graph_Arc_Prop( 22, 7, 3 ), g );
  add_edge( 4, 3, SPPRC_Example_Graph_Arc_Prop( 25, 28, 9 ), g );
  add_edge( 4, 2, SPPRC_Example_Graph_Arc_Prop( 23, 39, 6 ), g );
  add_edge( 5, 8, SPPRC_Example_Graph_Arc_Prop( 32, 6, 2 ), g );
  add_edge( 5, 2, SPPRC_Example_Graph_Arc_Prop( 30, 26, 10 ), g );
  add_edge( 5, 0, SPPRC_Example_Graph_Arc_Prop( 28, 38, 9 ), g );
  add_edge( 5, 2, SPPRC_Example_Graph_Arc_Prop( 31, 48, 10 ), g );
  add_edge( 5, 9, SPPRC_Example_Graph_Arc_Prop( 33, 49, 2 ), g );
  add_edge( 5, 1, SPPRC_Example_Graph_Arc_Prop( 29, 22, 7 ), g );
  add_edge( 6, 1, SPPRC_Example_Graph_Arc_Prop( 34, 15, 7 ), g );
  add_edge( 6, 7, SPPRC_Example_Graph_Arc_Prop( 35, 20, 3 ), g );
  add_edge( 7, 9, SPPRC_Example_Graph_Arc_Prop( 40, 1, 3 ), g );
  add_edge( 7, 0, SPPRC_Example_Graph_Arc_Prop( 36, 23, 5 ), g );
  add_edge( 7, 6, SPPRC_Example_Graph_Arc_Prop( 38, 36, 2 ), g );
  add_edge( 7, 6, SPPRC_Example_Graph_Arc_Prop( 39, 18, 10 ), g );
  add_edge( 7, 2, SPPRC_Example_Graph_Arc_Prop( 37, 2, 1 ), g );
  add_edge( 8, 5, SPPRC_Example_Graph_Arc_Prop( 46, 36, 5 ), g );
  add_edge( 8, 1, SPPRC_Example_Graph_Arc_Prop( 42, 13, 10 ), g );
  add_edge( 8, 0, SPPRC_Example_Graph_Arc_Prop( 41, 40, 5 ), g );
  add_edge( 8, 1, SPPRC_Example_Graph_Arc_Prop( 43, 32, 8 ), g );
  add_edge( 8, 6, SPPRC_Example_Graph_Arc_Prop( 47, 25, 1 ), g );
  add_edge( 8, 2, SPPRC_Example_Graph_Arc_Prop( 44, 44, 3 ), g );
  add_edge( 8, 3, SPPRC_Example_Graph_Arc_Prop( 45, 11, 9 ), g );
  add_edge( 9, 0, SPPRC_Example_Graph_Arc_Prop( 48, 41, 5 ), g );
  add_edge( 9, 1, SPPRC_Example_Graph_Arc_Prop( 49, 44, 7 ), g );

  // spp without resource constraints

  std::vector
    <std::vector
      <graph_traits<SPPRC_Example_Graph>::edge_descriptor> >
        opt_solutions;
  std::vector<spp_no_rc_res_cont> pareto_opt_rcs_no_rc;
  std::vector<int> i_vec_opt_solutions_spp_no_rc;
  //std::cout << "r_c_shortest_paths:" << std::endl;
  for( int s = 0; s < 10; ++s )
  {
    for( int t = 0; t < 10; ++t )
    {
      r_c_shortest_paths
      ( g,
        get( &SPPRC_Example_Graph_Vert_Prop::num, g ),
        get( &SPPRC_Example_Graph_Arc_Prop::num, g ),
        s,
        t,
        opt_solutions,
        pareto_opt_rcs_no_rc,
        spp_no_rc_res_cont( 0 ),
        ref_no_res_cont(),
        dominance_no_res_cont(),
        std::allocator
          <r_c_shortest_paths_label
            <SPPRC_Example_Graph, spp_no_rc_res_cont> >(),
        default_r_c_shortest_paths_visitor() );
      i_vec_opt_solutions_spp_no_rc.push_back( pareto_opt_rcs_no_rc[0].cost );
      //std::cout << "From " << s << " to " << t << ": ";
      //std::cout << pareto_opt_rcs_no_rc[0].cost << std::endl;
    }
  }

  //std::vector<graph_traits<SPPRC_Example_Graph>::vertex_descriptor>
  //  p( num_vertices( g ) );
  //std::vector<int> d( num_vertices( g ) );
  //std::vector<int> i_vec_dijkstra_distances;
  //std::cout << "Dijkstra:" << std::endl;
  //for( int s = 0; s < 10; ++s )
  //{
  //  dijkstra_shortest_paths( g,
  //                           s,
  //                           &p[0],
  //                           &d[0],
  //                           get( &SPPRC_Example_Graph_Arc_Prop::cost, g ),
  //                           get( &SPPRC_Example_Graph_Vert_Prop::num, g ),
  //                           std::less<int>(),
  //                           closed_plus<int>(),
  //                           (std::numeric_limits<int>::max)(),
  //                           0,
  //                           default_dijkstra_visitor() );
  //  for( int t = 0; t < 10; ++t )
  //  {
  //    i_vec_dijkstra_distances.push_back( d[t] );
  //    std::cout << "From " << s << " to " << t << ": " << d[t] << std::endl;
  //  }
  //}

  std::vector<int> i_vec_correct_solutions;
  i_vec_correct_solutions.push_back( 0 );
  i_vec_correct_solutions.push_back( 22 );
  i_vec_correct_solutions.push_back( 27 );
  i_vec_correct_solutions.push_back( 1 );
  i_vec_correct_solutions.push_back( 6 );
  i_vec_correct_solutions.push_back( 44 );
  i_vec_correct_solutions.push_back( 7 );
  i_vec_correct_solutions.push_back( 27 );
  i_vec_correct_solutions.push_back( 50 );
  i_vec_correct_solutions.push_back( 25 );
  i_vec_correct_solutions.push_back( 37 );
  i_vec_correct_solutions.push_back( 0 );
  i_vec_correct_solutions.push_back( 29 );
  i_vec_correct_solutions.push_back( 38 );
  i_vec_correct_solutions.push_back( 43 );
  i_vec_correct_solutions.push_back( 81 );
  i_vec_correct_solutions.push_back( 7 );
  i_vec_correct_solutions.push_back( 27 );
  i_vec_correct_solutions.push_back( 76 );
  i_vec_correct_solutions.push_back( 28 );
  i_vec_correct_solutions.push_back( 25 );
  i_vec_correct_solutions.push_back( 21 );
  i_vec_correct_solutions.push_back( 0 );
  i_vec_correct_solutions.push_back( 13 );
  i_vec_correct_solutions.push_back( 18 );
  i_vec_correct_solutions.push_back( 56 );
  i_vec_correct_solutions.push_back( 6 );
  i_vec_correct_solutions.push_back( 26 );
  i_vec_correct_solutions.push_back( 47 );
  i_vec_correct_solutions.push_back( 27 );
  i_vec_correct_solutions.push_back( 12 );
  i_vec_correct_solutions.push_back( 21 );
  i_vec_correct_solutions.push_back( 26 );
  i_vec_correct_solutions.push_back( 0 );
  i_vec_correct_solutions.push_back( 5 );
  i_vec_correct_solutions.push_back( 43 );
  i_vec_correct_solutions.push_back( 6 );
  i_vec_correct_solutions.push_back( 26 );
  i_vec_correct_solutions.push_back( 49 );
  i_vec_correct_solutions.push_back( 24 );
  i_vec_correct_solutions.push_back( 7 );
  i_vec_correct_solutions.push_back( 29 );
  i_vec_correct_solutions.push_back( 34 );
  i_vec_correct_solutions.push_back( 8 );
  i_vec_correct_solutions.push_back( 0 );
  i_vec_correct_solutions.push_back( 38 );
  i_vec_correct_solutions.push_back( 14 );
  i_vec_correct_solutions.push_back( 34 );
  i_vec_correct_solutions.push_back( 44 );
  i_vec_correct_solutions.push_back( 32 );
  i_vec_correct_solutions.push_back( 29 );
  i_vec_correct_solutions.push_back( 19 );
  i_vec_correct_solutions.push_back( 26 );
  i_vec_correct_solutions.push_back( 17 );
  i_vec_correct_solutions.push_back( 22 );
  i_vec_correct_solutions.push_back( 0 );
  i_vec_correct_solutions.push_back( 23 );
  i_vec_correct_solutions.push_back( 43 );
  i_vec_correct_solutions.push_back( 6 );
  i_vec_correct_solutions.push_back( 41 );
  i_vec_correct_solutions.push_back( 43 );
  i_vec_correct_solutions.push_back( 15 );
  i_vec_correct_solutions.push_back( 22 );
  i_vec_correct_solutions.push_back( 35 );
  i_vec_correct_solutions.push_back( 40 );
  i_vec_correct_solutions.push_back( 78 );
  i_vec_correct_solutions.push_back( 0 );
  i_vec_correct_solutions.push_back( 20 );
  i_vec_correct_solutions.push_back( 69 );
  i_vec_correct_solutions.push_back( 21 );
  i_vec_correct_solutions.push_back( 23 );
  i_vec_correct_solutions.push_back( 23 );
  i_vec_correct_solutions.push_back( 2 );
  i_vec_correct_solutions.push_back( 15 );
  i_vec_correct_solutions.push_back( 20 );
  i_vec_correct_solutions.push_back( 58 );
  i_vec_correct_solutions.push_back( 8 );
  i_vec_correct_solutions.push_back( 0 );
  i_vec_correct_solutions.push_back( 49 );
  i_vec_correct_solutions.push_back( 1 );
  i_vec_correct_solutions.push_back( 23 );
  i_vec_correct_solutions.push_back( 13 );
  i_vec_correct_solutions.push_back( 37 );
  i_vec_correct_solutions.push_back( 11 );
  i_vec_correct_solutions.push_back( 16 );
  i_vec_correct_solutions.push_back( 36 );
  i_vec_correct_solutions.push_back( 17 );
  i_vec_correct_solutions.push_back( 37 );
  i_vec_correct_solutions.push_back( 0 );
  i_vec_correct_solutions.push_back( 35 );
  i_vec_correct_solutions.push_back( 41 );
  i_vec_correct_solutions.push_back( 44 );
  i_vec_correct_solutions.push_back( 68 );
  i_vec_correct_solutions.push_back( 42 );
  i_vec_correct_solutions.push_back( 47 );
  i_vec_correct_solutions.push_back( 85 );
  i_vec_correct_solutions.push_back( 48 );
  i_vec_correct_solutions.push_back( 68 );
  i_vec_correct_solutions.push_back( 91 );
  i_vec_correct_solutions.push_back( 0 );
  BOOST_CHECK(i_vec_opt_solutions_spp_no_rc.size() == i_vec_correct_solutions.size() );
  for( int i = 0; i < static_cast<int>( i_vec_correct_solutions.size() ); ++i )
    BOOST_CHECK( i_vec_opt_solutions_spp_no_rc[i] == i_vec_correct_solutions[i] );

  // spptw
  std::vector
    <std::vector
      <graph_traits<SPPRC_Example_Graph>::edge_descriptor> >
        opt_solutions_spptw;
  std::vector<spp_spptw_res_cont> pareto_opt_rcs_spptw;
  std::vector
    <std::vector
      <std::vector
        <std::vector
          <graph_traits<SPPRC_Example_Graph>::edge_descriptor> > > >
            vec_vec_vec_vec_opt_solutions_spptw( 10 );

  for( int s = 0; s < 10; ++s )
  {
    for( int t = 0; t < 10; ++t )
    {
      r_c_shortest_paths
      ( g,
        get( &SPPRC_Example_Graph_Vert_Prop::num, g ),
        get( &SPPRC_Example_Graph_Arc_Prop::num, g ),
        s,
        t,
        opt_solutions_spptw,
        pareto_opt_rcs_spptw,
        // be careful, do not simply take 0 as initial value for time
        spp_spptw_res_cont( 0, g[s].eat ),
        ref_spptw(),
        dominance_spptw(),
        std::allocator
          <r_c_shortest_paths_label
            <SPPRC_Example_Graph, spp_spptw_res_cont> >(),
        default_r_c_shortest_paths_visitor() );
      vec_vec_vec_vec_opt_solutions_spptw[s].push_back( opt_solutions_spptw );
      if( opt_solutions_spptw.size() )
      {
        bool b_is_a_path_at_all = false;
        bool b_feasible = false;
        bool b_correctly_extended = false;
        spp_spptw_res_cont actual_final_resource_levels( 0, 0 );
        graph_traits<SPPRC_Example_Graph>::edge_descriptor ed_last_extended_arc;
        check_r_c_path( g,
                        opt_solutions_spptw[0],
                        spp_spptw_res_cont( 0, g[s].eat ),
                        true,
                        pareto_opt_rcs_spptw[0],
                        actual_final_resource_levels,
                        ref_spptw(),
                        b_is_a_path_at_all,
                        b_feasible,
                        b_correctly_extended,
                        ed_last_extended_arc );
        BOOST_CHECK(b_is_a_path_at_all && b_feasible && b_correctly_extended);
        b_is_a_path_at_all = false;
        b_feasible = false;
        b_correctly_extended = false;
        spp_spptw_res_cont actual_final_resource_levels2( 0, 0 );
        graph_traits<SPPRC_Example_Graph>::edge_descriptor ed_last_extended_arc2;
        check_r_c_path( g,
                        opt_solutions_spptw[0],
                        spp_spptw_res_cont( 0, g[s].eat ),
                        false,
                        pareto_opt_rcs_spptw[0],
                        actual_final_resource_levels2,
                        ref_spptw(),
                        b_is_a_path_at_all,
                        b_feasible,
                        b_correctly_extended,
                        ed_last_extended_arc2 );
        BOOST_CHECK(b_is_a_path_at_all && b_feasible && b_correctly_extended);
      }
    }
  }

  std::vector<int> i_vec_correct_num_solutions_spptw;
  i_vec_correct_num_solutions_spptw.push_back( 1 );
  i_vec_correct_num_solutions_spptw.push_back( 2 );
  i_vec_correct_num_solutions_spptw.push_back( 3 );
  i_vec_correct_num_solutions_spptw.push_back( 1 );
  i_vec_correct_num_solutions_spptw.push_back( 3 );
  i_vec_correct_num_solutions_spptw.push_back( 1 );
  i_vec_correct_num_solutions_spptw.push_back( 2 );
  i_vec_correct_num_solutions_spptw.push_back( 1 );
  i_vec_correct_num_solutions_spptw.push_back( 2 );
  i_vec_correct_num_solutions_spptw.push_back( 1 );
  i_vec_correct_num_solutions_spptw.push_back( 1 );
  i_vec_correct_num_solutions_spptw.push_back( 1 );
  i_vec_correct_num_solutions_spptw.push_back( 4 );
  i_vec_correct_num_solutions_spptw.push_back( 1 );
  i_vec_correct_num_solutions_spptw.push_back( 3 );
  i_vec_correct_num_solutions_spptw.push_back( 1 );
  i_vec_correct_num_solutions_spptw.push_back( 1 );
  i_vec_correct_num_solutions_spptw.push_back( 1 );
  i_vec_correct_num_solutions_spptw.push_back( 2 );
  i_vec_correct_num_solutions_spptw.push_back( 2 );
  i_vec_correct_num_solutions_spptw.push_back( 4 );
  i_vec_correct_num_solutions_spptw.push_back( 1 );
  i_vec_correct_num_solutions_spptw.push_back( 1 );
  i_vec_correct_num_solutions_spptw.push_back( 1 );
  i_vec_correct_num_solutions_spptw.push_back( 4 );
  i_vec_correct_num_solutions_spptw.push_back( 1 );
  i_vec_correct_num_solutions_spptw.push_back( 2 );
  i_vec_correct_num_solutions_spptw.push_back( 1 );
  i_vec_correct_num_solutions_spptw.push_back( 1 );
  i_vec_correct_num_solutions_spptw.push_back( 3 );
  i_vec_correct_num_solutions_spptw.push_back( 2 );
  i_vec_correct_num_solutions_spptw.push_back( 2 );
  i_vec_correct_num_solutions_spptw.push_back( 2 );
  i_vec_correct_num_solutions_spptw.push_back( 1 );
  i_vec_correct_num_solutions_spptw.push_back( 2 );
  i_vec_correct_num_solutions_spptw.push_back( 0 );
  i_vec_correct_num_solutions_spptw.push_back( 1 );
  i_vec_correct_num_solutions_spptw.push_back( 1 );
  i_vec_correct_num_solutions_spptw.push_back( 2 );
  i_vec_correct_num_solutions_spptw.push_back( 1 );
  i_vec_correct_num_solutions_spptw.push_back( 1 );
  i_vec_correct_num_solutions_spptw.push_back( 2 );
  i_vec_correct_num_solutions_spptw.push_back( 2 );
  i_vec_correct_num_solutions_spptw.push_back( 1 );
  i_vec_correct_num_solutions_spptw.push_back( 1 );
  i_vec_correct_num_solutions_spptw.push_back( 1 );
  i_vec_correct_num_solutions_spptw.push_back( 2 );
  i_vec_correct_num_solutions_spptw.push_back( 1 );
  i_vec_correct_num_solutions_spptw.push_back( 2 );
  i_vec_correct_num_solutions_spptw.push_back( 1 );
  i_vec_correct_num_solutions_spptw.push_back( 4 );
  i_vec_correct_num_solutions_spptw.push_back( 2 );
  i_vec_correct_num_solutions_spptw.push_back( 2 );
  i_vec_correct_num_solutions_spptw.push_back( 1 );
  i_vec_correct_num_solutions_spptw.push_back( 4 );
  i_vec_correct_num_solutions_spptw.push_back( 1 );
  i_vec_correct_num_solutions_spptw.push_back( 4 );
  i_vec_correct_num_solutions_spptw.push_back( 1 );
  i_vec_correct_num_solutions_spptw.push_back( 1 );
  i_vec_correct_num_solutions_spptw.push_back( 2 );
  i_vec_correct_num_solutions_spptw.push_back( 2 );
  i_vec_correct_num_solutions_spptw.push_back( 1 );
  i_vec_correct_num_solutions_spptw.push_back( 4 );
  i_vec_correct_num_solutions_spptw.push_back( 2 );
  i_vec_correct_num_solutions_spptw.push_back( 5 );
  i_vec_correct_num_solutions_spptw.push_back( 1 );
  i_vec_correct_num_solutions_spptw.push_back( 1 );
  i_vec_correct_num_solutions_spptw.push_back( 1 );
  i_vec_correct_num_solutions_spptw.push_back( 2 );
  i_vec_correct_num_solutions_spptw.push_back( 2 );
  i_vec_correct_num_solutions_spptw.push_back( 1 );
  i_vec_correct_num_solutions_spptw.push_back( 3 );
  i_vec_correct_num_solutions_spptw.push_back( 1 );
  i_vec_correct_num_solutions_spptw.push_back( 1 );
  i_vec_correct_num_solutions_spptw.push_back( 2 );
  i_vec_correct_num_solutions_spptw.push_back( 0 );
  i_vec_correct_num_solutions_spptw.push_back( 2 );
  i_vec_correct_num_solutions_spptw.push_back( 1 );
  i_vec_correct_num_solutions_spptw.push_back( 1 );
  i_vec_correct_num_solutions_spptw.push_back( 1 );
  i_vec_correct_num_solutions_spptw.push_back( 3 );
  i_vec_correct_num_solutions_spptw.push_back( 1 );
  i_vec_correct_num_solutions_spptw.push_back( 2 );
  i_vec_correct_num_solutions_spptw.push_back( 1 );
  i_vec_correct_num_solutions_spptw.push_back( 3 );
  i_vec_correct_num_solutions_spptw.push_back( 1 );
  i_vec_correct_num_solutions_spptw.push_back( 3 );
  i_vec_correct_num_solutions_spptw.push_back( 1 );
  i_vec_correct_num_solutions_spptw.push_back( 1 );
  i_vec_correct_num_solutions_spptw.push_back( 2 );
  i_vec_correct_num_solutions_spptw.push_back( 1 );
  i_vec_correct_num_solutions_spptw.push_back( 1 );
  i_vec_correct_num_solutions_spptw.push_back( 4 );
  i_vec_correct_num_solutions_spptw.push_back( 1 );
  i_vec_correct_num_solutions_spptw.push_back( 3 );
  i_vec_correct_num_solutions_spptw.push_back( 0 );
  i_vec_correct_num_solutions_spptw.push_back( 2 );
  i_vec_correct_num_solutions_spptw.push_back( 3 );
  i_vec_correct_num_solutions_spptw.push_back( 4 );
  i_vec_correct_num_solutions_spptw.push_back( 1 );
  for( int s = 0; s < 10; ++s )
    for( int t = 0; t < 10; ++t )
      BOOST_CHECK( static_cast<int>
            ( vec_vec_vec_vec_opt_solutions_spptw[s][t].size() ) ==
                   i_vec_correct_num_solutions_spptw[10 * s + t] );

  // one pareto-optimal solution
  SPPRC_Example_Graph g2;
  add_vertex( SPPRC_Example_Graph_Vert_Prop( 0, 0, 1000000000 ), g2 );
  add_vertex( SPPRC_Example_Graph_Vert_Prop( 1, 0, 1000000000 ), g2 );
  add_vertex( SPPRC_Example_Graph_Vert_Prop( 2, 0, 1000000000 ), g2 );
  add_vertex( SPPRC_Example_Graph_Vert_Prop( 3, 0, 1000000000 ), g2 );
  add_edge( 0, 1, SPPRC_Example_Graph_Arc_Prop( 0, 1, 1 ), g2 );
  add_edge( 0, 2, SPPRC_Example_Graph_Arc_Prop( 1, 2, 1 ), g2 );
  add_edge( 1, 3, SPPRC_Example_Graph_Arc_Prop( 2, 3, 1 ), g2 );
  add_edge( 2, 3, SPPRC_Example_Graph_Arc_Prop( 3, 1, 1 ), g2 );
  std::vector<graph_traits<SPPRC_Example_Graph>::edge_descriptor> opt_solution;
  spp_spptw_res_cont pareto_opt_rc;
  r_c_shortest_paths( g2,
                      get( &SPPRC_Example_Graph_Vert_Prop::num, g2 ),
                      get( &SPPRC_Example_Graph_Arc_Prop::num, g2 ),
                      0,
                      3,
                      opt_solution,
                      pareto_opt_rc,
                      spp_spptw_res_cont( 0, 0 ),
                      ref_spptw(),
                      dominance_spptw(),
                      std::allocator
                        <r_c_shortest_paths_label
                          <SPPRC_Example_Graph, spp_spptw_res_cont> >(),
                      default_r_c_shortest_paths_visitor() );

  BOOST_CHECK(pareto_opt_rc.cost == 3);

  return 0;
}


// Boost.Geometry

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

// This file was modified by Oracle on 2014, 2016, 2017.
// Modifications copyright (c) 2014-2017 Oracle and/or its affiliates.

// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

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

#ifndef BOOST_GEOMETRY_FORMULAS_VINCENTY_INVERSE_HPP
#define BOOST_GEOMETRY_FORMULAS_VINCENTY_INVERSE_HPP

#include <boost/math/constants/constants.hpp>

#include <boost/geometry/core/radius.hpp>
#include <boost/geometry/core/srs.hpp>

#include <boost/geometry/util/condition.hpp>
#include <boost/geometry/util/math.hpp>

#include <boost/geometry/formulas/differential_quantities.hpp>
#include <boost/geometry/formulas/flattening.hpp>
#include <boost/geometry/formulas/result_inverse.hpp>

#ifndef BOOST_GEOMETRY_DETAIL_VINCENTY_MAX_STEPS
#define BOOST_GEOMETRY_DETAIL_VINCENTY_MAX_STEPS 1000
#endif

namespace boost { namespace geometry { namespace formula
{

/*!
\brief The solution of the inverse problem of geodesics on latlong coordinates, after Vincenty, 1975
\author See
    - http://www.ngs.noaa.gov/PUBS_LIB/inverse.pdf
    - http://www.icsm.gov.au/gda/gda-v_2.4.pdf
\author Adapted from various implementations to get it close to the original document
    - http://www.movable-type.co.uk/scripts/LatLongVincenty.html
    - http://exogen.case.edu/projects/geopy/source/geopy.distance.html
    - http://futureboy.homeip.net/fsp/colorize.fsp?fileName=navigation.frink

*/
template <
    typename CT,
    bool EnableDistance,
    bool EnableAzimuth,
    bool EnableReverseAzimuth = false,
    bool EnableReducedLength = false,
    bool EnableGeodesicScale = false
>
struct vincenty_inverse
{
    static const bool CalcQuantities = EnableReducedLength || EnableGeodesicScale;
    static const bool CalcAzimuths = EnableAzimuth || EnableReverseAzimuth || CalcQuantities;
    static const bool CalcFwdAzimuth = EnableAzimuth || CalcQuantities;
    static const bool CalcRevAzimuth = EnableReverseAzimuth || CalcQuantities;

public:
    typedef result_inverse<CT> result_type;

    template <typename T1, typename T2, typename Spheroid>
    static inline result_type apply(T1 const& lon1,
                                    T1 const& lat1,
                                    T2 const& lon2,
                                    T2 const& lat2,
                                    Spheroid const& spheroid)
    {
        result_type result;

        if (math::equals(lat1, lat2) && math::equals(lon1, lon2))
        {
            return result;
        }

        CT const c1 = 1;
        CT const c2 = 2;
        CT const c3 = 3;
        CT const c4 = 4;
        CT const c16 = 16;
        CT const c_e_12 = CT(1e-12);

        CT const pi = geometry::math::pi<CT>();
        CT const two_pi = c2 * pi;

        // lambda: difference in longitude on an auxiliary sphere
        CT L = lon2 - lon1;
        CT lambda = L;

        if (L < -pi) L += two_pi;
        if (L > pi) L -= two_pi;

        CT const radius_a = CT(get_radius<0>(spheroid));
        CT const radius_b = CT(get_radius<2>(spheroid));
        CT const f = formula::flattening<CT>(spheroid);

        // U: reduced latitude, defined by tan U = (1-f) tan phi
        CT const one_min_f = c1 - f;
        CT const tan_U1 = one_min_f * tan(lat1); // above (1)
        CT const tan_U2 = one_min_f * tan(lat2); // above (1)

        // calculate sin U and cos U using trigonometric identities
        CT const temp_den_U1 = math::sqrt(c1 + math::sqr(tan_U1));
        CT const temp_den_U2 = math::sqrt(c1 + math::sqr(tan_U2));
        // cos = 1 / sqrt(1 + tan^2)
        CT const cos_U1 = c1 / temp_den_U1;
        CT const cos_U2 = c1 / temp_den_U2;
        // sin = tan / sqrt(1 + tan^2)
        // sin = tan * cos
        CT const sin_U1 = tan_U1 * cos_U1;
        CT const sin_U2 = tan_U2 * cos_U2;

        // calculate sin U and cos U directly
        //CT const U1 = atan(tan_U1);
        //CT const U2 = atan(tan_U2);
        //cos_U1 = cos(U1);
        //cos_U2 = cos(U2);
        //sin_U1 = tan_U1 * cos_U1; // sin(U1);
        //sin_U2 = tan_U2 * cos_U2; // sin(U2);

        CT previous_lambda;
        CT sin_lambda;
        CT cos_lambda;
        CT sin_sigma;
        CT sin_alpha;
        CT cos2_alpha;
        CT cos_2sigma_m;
        CT cos2_2sigma_m;
        CT sigma;

        int counter = 0; // robustness

        do
        {
            previous_lambda = lambda; // (13)
            sin_lambda = sin(lambda);
            cos_lambda = cos(lambda);
            sin_sigma = math::sqrt(math::sqr(cos_U2 * sin_lambda) + math::sqr(cos_U1 * sin_U2 - sin_U1 * cos_U2 * cos_lambda)); // (14)
            CT cos_sigma = sin_U1 * sin_U2 + cos_U1 * cos_U2 * cos_lambda; // (15)
            sin_alpha = cos_U1 * cos_U2 * sin_lambda / sin_sigma; // (17)
            cos2_alpha = c1 - math::sqr(sin_alpha);
            cos_2sigma_m = math::equals(cos2_alpha, 0) ? 0 : cos_sigma - c2 * sin_U1 * sin_U2 / cos2_alpha; // (18)
            cos2_2sigma_m = math::sqr(cos_2sigma_m);

            CT C = f/c16 * cos2_alpha * (c4 + f * (c4 - c3 * cos2_alpha)); // (10)
            sigma = atan2(sin_sigma, cos_sigma); // (16)
            lambda = L + (c1 - C) * f * sin_alpha *
                (sigma + C * sin_sigma * (cos_2sigma_m + C * cos_sigma * (-c1 + c2 * cos2_2sigma_m))); // (11)

            ++counter; // robustness

        } while ( geometry::math::abs(previous_lambda - lambda) > c_e_12
               && geometry::math::abs(lambda) < pi
               && counter < BOOST_GEOMETRY_DETAIL_VINCENTY_MAX_STEPS ); // robustness

        if ( BOOST_GEOMETRY_CONDITION(EnableDistance) )
        {
            // Oops getting hard here
            // (again, problem is that ttmath cannot divide by doubles, which is OK)
            CT const c1 = 1;
            CT const c2 = 2;
            CT const c3 = 3;
            CT const c4 = 4;
            CT const c6 = 6;
            CT const c47 = 47;
            CT const c74 = 74;
            CT const c128 = 128;
            CT const c256 = 256;
            CT const c175 = 175;
            CT const c320 = 320;
            CT const c768 = 768;
            CT const c1024 = 1024;
            CT const c4096 = 4096;
            CT const c16384 = 16384;

            //CT sqr_u = cos2_alpha * (math::sqr(radius_a) - math::sqr(radius_b)) / math::sqr(radius_b); // above (1)
            CT sqr_u = cos2_alpha * ( math::sqr(radius_a / radius_b) - c1 ); // above (1)

            CT A = c1 + sqr_u/c16384 * (c4096 + sqr_u * (-c768 + sqr_u * (c320 - c175 * sqr_u))); // (3)
            CT B = sqr_u/c1024 * (c256 + sqr_u * ( -c128 + sqr_u * (c74 - c47 * sqr_u))); // (4)
            CT const cos_sigma = cos(sigma);
            CT const sin2_sigma = math::sqr(sin_sigma);
            CT delta_sigma = B * sin_sigma * (cos_2sigma_m + (B/c4) * (cos_sigma* (-c1 + c2 * cos2_2sigma_m)
                - (B/c6) * cos_2sigma_m * (-c3 + c4 * sin2_sigma) * (-c3 + c4 * cos2_2sigma_m))); // (6)

            result.distance = radius_b * A * (sigma - delta_sigma); // (19)
        }

        if ( BOOST_GEOMETRY_CONDITION(CalcAzimuths) )
        {
            if (BOOST_GEOMETRY_CONDITION(CalcFwdAzimuth))
            {
                result.azimuth = atan2(cos_U2 * sin_lambda, cos_U1 * sin_U2 - sin_U1 * cos_U2 * cos_lambda); // (20)
            }

            if (BOOST_GEOMETRY_CONDITION(CalcRevAzimuth))
            {
                result.reverse_azimuth = atan2(cos_U1 * sin_lambda, -sin_U1 * cos_U2 + cos_U1 * sin_U2 * cos_lambda); // (21)
            }
        }

        if (BOOST_GEOMETRY_CONDITION(CalcQuantities))
        {
            typedef differential_quantities<CT, EnableReducedLength, EnableGeodesicScale, 2> quantities;
            quantities::apply(lon1, lat1, lon2, lat2,
                              result.azimuth, result.reverse_azimuth,
                              radius_b, f,
                              result.reduced_length, result.geodesic_scale);
        }

        return result;
    }
};

}}} // namespace boost::geometry::formula

#endif // BOOST_GEOMETRY_FORMULAS_VINCENTY_INVERSE_HPP


#include <boost/test/unit_test.hpp>

#include <iostream>
#include <boost/graph/directed_graph.hpp> /** Boost directed graph **/
#include <boost/graph/labeled_graph.hpp> /** Boost directed graph **/
#include <boost/uuid/uuid.hpp> /** uuid class */
#include <boost/uuid/uuid_generators.hpp>

BOOST_AUTO_TEST_CASE(dummy_graph_test)
{

    typedef boost::directed_graph<boost::no_property> Graph;

    // Create a graph object
    Graph g;

    // Add vertices
    Graph::vertex_descriptor v0 = g.add_vertex();
    Graph::vertex_descriptor v1 = g.add_vertex();
    Graph::vertex_descriptor v2 = g.add_vertex();
    std::cout<<"v0: "<<v0<<"\n";
    std::cout<<"v1: "<<v1<<"\n";
    std::cout<<"v2: "<<v2<<"\n";

    std::cout << "There are " << g.num_vertices() << " vertices." << std::endl;
    Graph::vertex_descriptor u0 = boost::vertex(0, g);
    std::cout<<"u0: "<<u0<<"\n";
    g.remove_vertex(v0);
    std::cout << "There are " << g.num_vertices() << " vertices." << std::endl;
    u0 = boost::vertex(0, g);
    std::cout<<"u0: "<<u0<<"\n";
    Graph::vertex_descriptor u1 = boost::vertex(1, g);
    std::cout<<"u1: "<<u1<<"\n";
    Graph::vertex_descriptor u2 = boost::vertex(2, g);
    std::cout<<"u2: "<<u2<<"\n";

    typedef boost::labeled_graph< Graph, boost::uuids::uuid > LabeledGraph;

    LabeledGraph lg;

    // Add vertices
    boost::uuids::uuid id = boost::uuids::random_generator()();
    LabeledGraph::vertex_descriptor lv0 = boost::add_vertex(id, lg);
    LabeledGraph::vertex_descriptor lv1 = boost::add_vertex(boost::uuids::random_generator()(), lg);
    LabeledGraph::vertex_descriptor lv2 = boost::add_vertex(boost::uuids::random_generator()(), lg);
    std::cout<<"\nlv0: "<<lv0<<"\n";
    std::cout<<"lv1: "<<lv1<<"\n";
    std::cout<<"lv2: "<<lv2<<"\n";

    std::cout << "There are " << boost::num_vertices(lg) << " vertices." << std::endl;
    LabeledGraph::vertex_descriptor lu0 = boost::vertex_by_label(id, lg);
    std::cout<<"lu0: "<<lu0<<"\n";

}

//#include <boost/graph/dijkstra_shortest_paths.hpp>
//    std::vector<double> distances(num_vertices(envire_tree.tree));
//    boost::dijkstra_shortest_paths(envire_tree.tree, root,
//      boost::weight_map(boost::get(&envire::core::Edge::idx, envire_tree.tree))
//      .distance_map(boost::make_iterator_property_map(distances.begin(),
//                                               boost::get(boost::vertex_index, envire_tree.tree))));

    //property accessors
//    boost::property_map<envire::core::Tree::Graph, int>::type edgeIdx = boost::get(&envire::core::Edge::idx, envire_tree);

   // printDependencies(std::cout, envire_tree, boost::get(boost::vertex_index, envire_tree));

template < typename _Graph, typename _VertexNameMap > void
printDependencies(std::ostream & out, const _Graph & g,
                   _VertexNameMap name_map)
{
  typename boost::graph_traits < _Graph >::edge_iterator ei, ei_end;
  for (boost::tie(ei, ei_end) = edges(g); ei != ei_end; ++ei)
    out << boost::get(name_map, boost::source(*ei, g)) << " -$>$ "
      << boost::get(name_map, boost::target(*ei, g)) << std::endl;
}