// Boost.Graph library isomorphism test

// Copyright (C) 2001-20044 Douglas Gregor (dgregor at cs dot indiana dot edu)
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

// For more information, see http://www.boost.org
//
// Revision History:
//
// 29 Nov 2001    Jeremy Siek
//      Changed to use Boost.Random.
// 29 Nov 2001    Doug Gregor
//      Initial checkin.

#include <iostream>
#include <fstream>
#include <map>
#include <algorithm>
#include <cstdlib>
#include <time.h> // clock used without std:: qualifier?
#include <boost/test/minimal.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/isomorphism.hpp>
#include <boost/property_map/property_map.hpp>
#include <boost/random/variate_generator.hpp>
#include <boost/random/uniform_real.hpp>
#include <boost/random/uniform_int.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/lexical_cast.hpp>

using namespace boost;

template <typename Generator>
struct random_functor {
  random_functor(Generator& g) : g(g) { }
  std::size_t operator()(std::size_t n) {
    boost::uniform_int<std::size_t> distrib(0, n-1);
    boost::variate_generator<boost::mt19937&, boost::uniform_int<std::size_t> >
      x(g, distrib);
    return x();
  }
  Generator& g;
};

template<typename Graph1, typename Graph2>
void randomly_permute_graph(const Graph1& g1, Graph2& g2)
{
  // Need a clean graph to start with
  BOOST_REQUIRE(num_vertices(g2) == 0);
  BOOST_REQUIRE(num_edges(g2) == 0);

  typedef typename graph_traits<Graph1>::vertex_descriptor vertex1;
  typedef typename graph_traits<Graph2>::vertex_descriptor vertex2;
  typedef typename graph_traits<Graph1>::edge_iterator edge_iterator;

  boost::mt19937 gen;
  random_functor<boost::mt19937> rand_fun(gen);

  // Decide new order
  std::vector<vertex1> orig_vertices;
  std::copy(vertices(g1).first, vertices(g1).second, std::back_inserter(orig_vertices));
  std::random_shuffle(orig_vertices.begin(), orig_vertices.end(), rand_fun);
  std::map<vertex1, vertex2> vertex_map;

  for (std::size_t i = 0; i < num_vertices(g1); ++i) {
    vertex_map[orig_vertices[i]] = add_vertex(g2);
  }

  for (edge_iterator e = edges(g1).first; e != edges(g1).second; ++e) {
    add_edge(vertex_map[source(*e, g1)], vertex_map[target(*e, g1)], g2);
  }
}

template<typename Graph>
void generate_random_digraph(Graph& g, double edge_probability)
{
  typedef typename graph_traits<Graph>::vertex_iterator vertex_iterator;
  boost::mt19937 random_gen;
  boost::uniform_real<double> distrib(0.0, 1.0);
  boost::variate_generator<boost::mt19937&, boost::uniform_real<double> >
    random_dist(random_gen, distrib);

  for (vertex_iterator u = vertices(g).first; u != vertices(g).second; ++u) {
    vertex_iterator v = u;
    ++v;
    for (; v != vertices(g).second; ++v) {
      if (random_dist() <= edge_probability)
        add_edge(*u, *v, g);
    }
  }
}

void test_isomorphism2()
{
  using namespace boost::graph::keywords;
  typedef adjacency_list<vecS, vecS, bidirectionalS> graph1;
  typedef adjacency_list<listS, listS, bidirectionalS,
                         property<vertex_index_t, int> > graph2;

  graph1 g1(2);
  add_edge(vertex(0, g1), vertex(1, g1), g1);
  add_edge(vertex(1, g1), vertex(1, g1), g1);
  graph2 g2;
  randomly_permute_graph(g1, g2);

  int v_idx = 0;
  for (graph2::vertex_iterator v = vertices(g2).first;
       v != vertices(g2).second; ++v) {
    put(vertex_index_t(), g2, *v, v_idx++);
  }

  std::map<graph1::vertex_descriptor, graph2::vertex_descriptor> mapping;

  bool isomorphism_correct;
  clock_t start = clock();
  BOOST_CHECK(isomorphism_correct = boost::graph::isomorphism
               (g1, g2, _isomorphism_map = make_assoc_property_map(mapping)));
  clock_t end = clock();

  std::cout << "Elapsed time (clock cycles): " << (end - start) << std::endl;

  bool verify_correct;
  BOOST_CHECK(verify_correct =
             verify_isomorphism(g1, g2, make_assoc_property_map(mapping)));

  if (!isomorphism_correct || !verify_correct) {
    // Output graph 1
    {
      std::ofstream out("isomorphism_failure.bg1");
      out << num_vertices(g1) << std::endl;
      for (graph1::edge_iterator e = edges(g1).first;
           e != edges(g1).second; ++e) {
        out << get(vertex_index_t(), g1, source(*e, g1)) << ' '
            << get(vertex_index_t(), g1, target(*e, g1)) << std::endl;
      }
    }

    // Output graph 2
    {
      std::ofstream out("isomorphism_failure.bg2");
      out << num_vertices(g2) << std::endl;
      for (graph2::edge_iterator e = edges(g2).first;
           e != edges(g2).second; ++e) {
        out << get(vertex_index_t(), g2, source(*e, g2)) << ' '
            << get(vertex_index_t(), g2, target(*e, g2)) << std::endl;
      }
    }
  }
}

void test_isomorphism(int n, double edge_probability)
{
  using namespace boost::graph::keywords;
  typedef adjacency_list<vecS, vecS, bidirectionalS> graph1;
  typedef adjacency_list<listS, listS, bidirectionalS,
                         property<vertex_index_t, int> > graph2;

  graph1 g1(n);
  generate_random_digraph(g1, edge_probability);
  graph2 g2;
  randomly_permute_graph(g1, g2);

  int v_idx = 0;
  for (graph2::vertex_iterator v = vertices(g2).first;
       v != vertices(g2).second; ++v) {
    put(vertex_index_t(), g2, *v, v_idx++);
  }

  std::map<graph1::vertex_descriptor, graph2::vertex_descriptor> mapping;

  bool isomorphism_correct;
  clock_t start = clock();
  BOOST_CHECK(isomorphism_correct = boost::graph::isomorphism
               (g1, g2, _isomorphism_map = make_assoc_property_map(mapping)));
  clock_t end = clock();

  std::cout << "Elapsed time (clock cycles): " << (end - start) << std::endl;

  bool verify_correct;
  BOOST_CHECK(verify_correct =
             verify_isomorphism(g1, g2, make_assoc_property_map(mapping)));

  if (!isomorphism_correct || !verify_correct) {
    // Output graph 1
    {
      std::ofstream out("isomorphism_failure.bg1");
      out << num_vertices(g1) << std::endl;
      for (graph1::edge_iterator e = edges(g1).first;
           e != edges(g1).second; ++e) {
        out << get(vertex_index_t(), g1, source(*e, g1)) << ' '
            << get(vertex_index_t(), g1, target(*e, g1)) << std::endl;
      }
    }

    // Output graph 2
    {
      std::ofstream out("isomorphism_failure.bg2");
      out << num_vertices(g2) << std::endl;
      for (graph2::edge_iterator e = edges(g2).first;
           e != edges(g2).second; ++e) {
        out << get(vertex_index_t(), g2, source(*e, g2)) << ' '
            << get(vertex_index_t(), g2, target(*e, g2)) << std::endl;
      }
    }
  }
}

int test_main(int argc, char* argv[])
{
  if (argc < 3) {
    test_isomorphism(30, 0.45);
    return 0;
  }

  int n = boost::lexical_cast<int>(argv[1]);
  double edge_prob = boost::lexical_cast<double>(argv[2]);
  test_isomorphism(n, edge_prob);

  return 0;
}


//////////////////////////////////////////////////////////////////////////////
///   Copyright 2003 and onward LASMEA UMR 6602 CNRS/U.B.P Clermont-Ferrand
///   Copyright 2009 and onward LRI    UMR 8623 CNRS/Univ Paris Sud XI
///
///          Distributed under the Boost Software License, Version 1.0
///                 See accompanying file LICENSE.txt or copy at
///                     http://www.boost.org/LICENSE_1_0.txt
//////////////////////////////////////////////////////////////////////////////
#ifndef NT2_TOOLBOX_LIBC_FUNCTION_SCALAR_FREXP_HPP_INCLUDED
#define NT2_TOOLBOX_LIBC_FUNCTION_SCALAR_FREXP_HPP_INCLUDED
#include <boost/fusion/include/vector.hpp>
#include <boost/fusion/include/at.hpp>
#include <nt2/sdk/meta/adapted_traits.hpp>

/////////////////////////////////////////////////////////////////////////////
// Implementation when type  is double_
/////////////////////////////////////////////////////////////////////////////
NT2_REGISTER_DISPATCH(libc::tag::frexp_, tag::cpu_,
                       (A0),
                       (double_<A0>)
                      )

namespace nt2 { namespace ext
{
  template<class Dummy>
  struct call<libc::tag::frexp_(tag::double_),
              tag::cpu_, Dummy> : callable
  {
    template<class Sig> struct result;
    template<class This,class A0>struct result<This(A0)>
    {
      typedef typename std::tr1::result_of<meta::floating(A0)>::type mantissa;
      typedef typename meta::as_integer<A0,signed>::type          exponent;
      typedef boost::fusion::vector<mantissa,exponent>             type;
    };

    NT2_FUNCTOR_CALL(1)
    {
      typename NT2_RETURN_TYPE(1)::type res;
      int r1t;
      boost::fusion::at_c<0>(res) = ::frexp(a0, &r1t);
      boost::fusion::at_c<1>(res) = r1t;
      return res;
    }

  };
} }
/////////////////////////////////////////////////////////////////////////////
// Implementation when type  is float_
/////////////////////////////////////////////////////////////////////////////
NT2_REGISTER_DISPATCH(libc::tag::frexp_, tag::cpu_,
                       (A0),
                       (float_<A0>)
                      )

namespace nt2 { namespace ext
{
  template<class Dummy>
  struct call<libc::tag::frexp_(tag::float_),
              tag::cpu_, Dummy> : callable
  {
    template<class Sig> struct result;
    template<class This,class A0>struct result<This(A0)>
    {
      typedef typename std::tr1::result_of<meta::floating(A0)>::type mantissa;
      typedef typename meta::as_integer<A0,signed>::type          exponent;
      typedef boost::fusion::vector<mantissa,exponent>             type;
    };

    NT2_FUNCTOR_CALL(1)
    {
      typename NT2_RETURN_TYPE(1)::type res;
      int r1t;
      boost::fusion::at_c<0>(res) = ::frexpf(a0, &r1t);
      boost::fusion::at_c<1>(res) = r1t;
      return res;
    }

  };
} }
/////////////////////////////////////////////////////////////////////////////
// Implementation when type  is long_double_
/////////////////////////////////////////////////////////////////////////////
NT2_REGISTER_DISPATCH(libc::tag::frexp_, tag::cpu_,
                       (A0),
                       (long_double_<A0>)
                      )

namespace nt2 { namespace ext
{
  template<class Dummy>
  struct call<libc::tag::frexp_(tag::long_double_),
              tag::cpu_, Dummy> : callable
  {
    template<class Sig> struct result;
    template<class This,class A0>struct result<This(A0)>
    {
      typedef typename std::tr1::result_of<meta::floating(A0)>::type mantissa;
      typedef typename meta::as_integer<A0,signed>::type          exponent;
      typedef boost::fusion::vector<mantissa,exponent>             type;
    };

    NT2_FUNCTOR_CALL(1)
    {
      typename NT2_RETURN_TYPE(1)::type res;
      int r1t;
      boost::fusion::at_c<0>(res) = ::frexpl(a0, &r1t);
      boost::fusion::at_c<1>(res) = r1t;
      return res;
    }

  };
} }

#endif
// modified by jt the 29/12/2010


// https://www.boost.org/doc/libs/1_67_0/doc/html/mpi/tutorial.html

#include <boost/mpi.hpp>
#include <boost/mpi/collectives.hpp>
#include <iostream>
#include <cstdlib>
#include <vector>

namespace mpi = boost::mpi;

int main(int argc, char *argv[])
{
    mpi::environment env(argc, argv);
    mpi::communicator world;

    std::srand(time(0) + world.rank());
    std::vector<int> all;
    int mine = -1;
    if (world.rank() == 0)
    {
        all.resize(world.size());
        std::generate(all.begin(), all.end(), std::rand);
    }
    mpi::scatter(world, all, mine, 0);
    for (int r = 0; r < world.size(); ++r)
    {
        world.barrier();
        if (r == world.rank())
        {
            std::cout << "Rank " << r << " got " << mine << '\n';
        }
    }
    return 0;
}


#include "../Block_Info.hxx"

#include <boost/filesystem/fstream.hpp>

namespace
{
  void
  read_vector_with_index(std::ifstream &input_stream,
                         const std::vector<size_t> &indices,
                         const size_t &index_scale, std::vector<size_t> &v)
  {
    std::vector<size_t> file_v;
    read_vector(input_stream, file_v);
    for(size_t index = 0; index < file_v.size(); ++index)
      {
        const size_t mapped_index(index_scale * indices[index / index_scale]
                                  + index % index_scale);
        if(mapped_index >= v.size())
          {
            v.resize(mapped_index + 1);
          }
        v[mapped_index] = file_v[index];
      }
  }
}

void Block_Info::read_block_info(const boost::filesystem::path &sdp_directory)
{
  size_t file_rank(0);
  do
    {
      const boost::filesystem::path block_path(
        sdp_directory / ("blocks." + std::to_string(file_rank)));
      boost::filesystem::ifstream block_stream(block_path);
      if(!block_stream.good())
        {
          throw std::runtime_error("Could not open '" + block_path.string()
                                   + "'");
        }
      block_stream >> file_num_procs;
      if(!block_stream.good())
        {
          throw std::runtime_error("Corrupted file: " + block_path.string());
        }
      file_block_indices.emplace_back();
      auto &file_block_index(file_block_indices.back());
      read_vector(block_stream, file_block_index);

      read_vector_with_index(block_stream, file_block_index, 1, dimensions);
      read_vector_with_index(block_stream, file_block_index, 1, degrees);
      read_vector_with_index(block_stream, file_block_index, 1,
                             schur_block_sizes);
      read_vector_with_index(block_stream, file_block_index, 2,
                             psd_matrix_block_sizes);
      read_vector_with_index(block_stream, file_block_index, 2,
                             bilinear_pairing_block_sizes);
      ++file_rank;
    }
  while(file_rank < file_num_procs);
}


/*
   For more information, please see: http://software.sci.utah.edu

   The MIT License

   Copyright (c) 2020 Scientific Computing and Imaging Institute,
   University of Utah.

   Permission is hereby granted, free of charge, to any person obtaining a
   copy of this software and associated documentation files (the "Software"),
   to deal in the Software without restriction, including without limitation
   the rights to use, copy, modify, merge, publish, distribute, sublicense,
   and/or sell copies of the Software, and to permit persons to whom the
   Software is furnished to do so, subject to the following conditions:

   The above copyright notice and this permission notice shall be included
   in all copies or substantial portions of the Software.

   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
   OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
   THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
   LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
   FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
   DEALINGS IN THE SOFTWARE.
*/

#include <Modules/Math/ConvertRealToComplexMatrix.h>
#include <Core/Datatypes/Legacy/Field/Field.h>
#include <Core/Datatypes/DenseMatrix.h>
#include <Core/Datatypes/DenseColumnMatrix.h>
#include <Core/Datatypes/SparseRowMatrix.h>
#include <Testing/ModuleTestBase/ModuleTestBase.h>
#include <Eigen/Dense>
#include <Core/Datatypes/MatrixTypeConversions.h>

using namespace SCIRun;
using namespace SCIRun::Testing;
using namespace SCIRun::Core::Datatypes;
using namespace SCIRun::Core::Algorithms;
using namespace SCIRun::Modules::Math;
using namespace SCIRun::Dataflow::Networks;
using ::testing::_;
using ::testing::NiceMock;
using ::testing::DefaultValue;
using ::testing::Return;

class ConvertRealToComplexMatrixModuleTests : public ModuleTest
{

};

TEST_F(ConvertRealToComplexMatrixModuleTests, ThrowsForNullFieldInput)
{
  auto csdf = makeModule("ConvertRealToComplexMatrix");
  FieldHandle nullField;
  stubPortNWithThisData(csdf, 0, nullField);
  stubPortNWithThisData(csdf, 1, nullField);
  EXPECT_THROW(csdf->execute(), NullHandleOnPortException);
}

TEST_F(ConvertRealToComplexMatrixModuleTests, ThrowsForNullMatrixInput)
{
  auto csdf = makeModule("ConvertRealToComplexMatrix");
  MatrixHandle nullMatrix;
  stubPortNWithThisData(csdf, 0, nullMatrix);
  stubPortNWithThisData(csdf, 1, nullMatrix);
  EXPECT_THROW(csdf->execute(), NullHandleOnPortException);
}

TEST_F(ConvertRealToComplexMatrixModuleTests, SparseMatrixOnInputPort)
{
  SparseRowMatrixHandle m(makeShared<SparseRowMatrix>(3, 3));
  m->insert(0, 0) = 1;
  m->insert(1, 1) = 2;
  m->insert(2, 2) = 3;
  m->makeCompressed();
  auto csdf = makeModule("ConvertRealToComplexMatrix");
  stubPortNWithThisData(csdf, 0, m);
  stubPortNWithThisData(csdf, 1, m);
  EXPECT_NO_THROW(csdf->execute());
}

TEST_F(ConvertRealToComplexMatrixModuleTests, DenseMatrixOnInputPort)
{
  DenseColumnMatrixHandle m(makeShared<DenseColumnMatrix>(3));
  (*m)(0) = 1;
  (*m)(1) = 2;
  (*m)(2) = 3;
  auto csdf = makeModule("ConvertRealToComplexMatrix");
  stubPortNWithThisData(csdf, 0, m);
  stubPortNWithThisData(csdf, 1, m);
  EXPECT_NO_THROW(csdf->execute());
}

TEST_F(ConvertRealToComplexMatrixModuleTests, ColumnMatrixOnInputPort)
{
  DenseMatrixHandle m(makeShared<DenseMatrix>(3, 1));
  (*m)(0, 0) = 1;
  (*m)(1, 0) = 2;
  (*m)(2, 0) = 3;
  auto csdf = makeModule("ConvertRealToComplexMatrix");
  stubPortNWithThisData(csdf, 0, m);
  stubPortNWithThisData(csdf, 1, m);
  EXPECT_NO_THROW(csdf->execute());
}

TEST_F(ConvertRealToComplexMatrixModuleTests, ThisShouldWork)
{
  DenseMatrixHandle m(makeShared<DenseMatrix>(2, 2));
  (*m)(0, 0) = 1;
  (*m)(0, 1) = 2;
  (*m)(1, 0) = 3;
  (*m)(1, 1) = 4;
  DenseMatrixHandle n(makeShared<DenseMatrix>(2, 2));
  (*n)(0, 0) = -1;
  (*n)(0, 1) = -2;
  (*n)(1, 0) = -3;
  (*n)(1, 1) = -4;
  auto csdf = makeModule("ConvertRealToComplexMatrix");
  stubPortNWithThisData(csdf, 0, m);
  stubPortNWithThisData(csdf, 1, n);
  EXPECT_NO_THROW(csdf->execute());
  auto output = getDataOnThisOutputPort(csdf, 0);
  ASSERT_TRUE(output != nullptr);
  auto result = std::dynamic_pointer_cast<ComplexDenseMatrix>(output);
  for (int x = 0; x < 2; x++)
    for (int y = 0; y < 2; y++)
    {
      auto  input_real = (*m)(x, y), input_imag = (*n)(x, y);
      auto output_real = (*result)(x, y).real(), output_imag = (*result)(x, y).imag();
      ASSERT_TRUE(input_real == output_real);
      ASSERT_TRUE(input_imag == output_imag);
    }
}

TEST_F(ConvertRealToComplexMatrixModuleTests, ThisShouldWork_sparse)
{
  auto m(makeShared<SparseRowMatrix>(3, 3));
  m->insert(0, 0) = 1;
  m->insert(0, 2) = 1;
  m->insert(1, 0) = 3;
  m->insert(1, 1) = 5;
  m->insert(2, 1) = 1;
  m->insert(2, 2) = 3;
  m->makeCompressed();
  auto n(makeShared<SparseRowMatrix>(3, 3));
  n->insert(0, 1) = 1;
  n->insert(0, 2) = 2;
  n->insert(1, 0) = 8;
  n->insert(1, 2) = -3;
  n->insert(2, 0) = 9;
  n->insert(2, 2) = 8;
  n->makeCompressed();
  auto csdf = makeModule("ConvertRealToComplexMatrix");
  stubPortNWithThisData(csdf, 0, m);
  stubPortNWithThisData(csdf, 1, n);
  EXPECT_NO_THROW(csdf->execute());
  auto output = getDataOnThisOutputPort(csdf, 0);
  ASSERT_TRUE(output != nullptr);
  auto out = std::dynamic_pointer_cast<ComplexSparseRowMatrix>(output);
  for (int k = 0; k < out->outerSize(); ++k)
  {
    for (ComplexSparseRowMatrix::InnerIterator it(*out, k); it; ++it)
    {
      auto tmp = it.value();
      ASSERT_TRUE(m->coeffRef(it.row(), it.col()) == tmp.real());
      ASSERT_TRUE(n->coeffRef(it.row(), it.col()) == tmp.imag());
    }
  }
}


/*
	ALL THE DISTANCE UNITS HERE ARE IN METERS!
	Speed would be meters/second
	For x axis:
		positive = forwards
		negative = backwards
	For y axis:
		postive = left
		negative = right
*/

#include <vector>
#include "Drone.h"
#include <math.h>
#include <boost/asio/io_service.hpp>
#include <Fullnavdata.h>
#include <gnuplot_iostream.h>
#include <deque>
#include <fstream>
#include <iostream>
#include <sstream>
#include "navi.h"

using namespace std;

//If the distances on the perpendicular axis of the current direction the drone is going is greater than 4 meters,
// we assume that we are at an intersection
const double Navi::INTERSECTION_THRESHOLD = 4.0;
//If the LIDAR scan shows we are .8 meters away from the direction we are going, we assume we are at a wall
const double Navi::WALL_THRESHOLD = .8;
//arbitrary value... probably something better could be used; indicates there is an intersection
double Navi::INTERSECTION_INDICATOR[2] = { 100000000000, -100000000000 };

Navi::Navi(){
	getting_info_loop = true;
	//node info array
	nia = NULL;
	current_direction = 'n';
	initial_direction = 'n';

	currently_reversing = false;
	did_reverse = false;
}

/* mainLoop: This loop is used as the full navigation module;
              it will tell the user which direction to go,
              determine intersections, and walls.

    Parameters:
        file_name: the filename where the LIDAR data is scanned to.
                    The format is based on the scan.cpp module
*/
void Navi::mainLoop( string file_name ){
	while(getting_info_loop)
	{
        //Want to wait til the LIDAR scan is in the file and ready to go
        ReadyToReadScan();

		//nia[0] = node coordinates
		//nia[1] = max backward distance vector
		//nia[2] = max forward distance vector
		//nia[3] = max left distance vector
		//nia[4] = max right distance vector
		nia = ReadScanFile(file_name);

		//the X Y coordinates on the plane. Z wasn't used here, so isn't included
		nia_position = nia[0];
		//max back distance
        mbd = nia[1][1];
        //max forward distance
		mfd = nia[2][1];
        //max left distance
		mld = nia[3][1];
        //max right distance
		mrd = nia[4][1];

        //We are seeing which way to start the drone; the farthest distance is the way it goes.
		if( initial_direction == 'n' ) //n indicates that there is no direcetion
		{
			drone_dd_vals = FindMaxDistance(mbd, mfd, mld, mrd);
			initial_direction = drone_dd_vals.direction;
			current_direction = initial_direction;
			cout << "Go: " << initial_direction << endl;
			continue;
		}

		intersection = CheckForIntersections(mbd, mfd, mld, mrd, current_direction);

		// n indicates no value was set, thus it isn't an intersection
		if( intersection.direction != 'n')
		{
			handleIntersection();
		}
		//need to insert the position after checking the intersection, since
		// the intersection check looks for it inside. Not a huge deal,
		// but will be easier since we don't need it til now anyways...
		// That was a lot of comments for no real good reason.
		dpos.push_back(nia_position);

		current_distance = GetDistance(current_direction);

        //If we are in range of a wall, we start to reverse the drone

		if( current_distance <= Navi::WALL_THRESHOLD )
		{
			cout << "WALLLLLLL!" << endl;
			current_direction = ReverseDirection(current_direction);
			currently_reversing = true;
			dpos.pop_back();
		}

        // NOTE: This is very rudimentary. It just says to go the opposite direction
        // the number of times you went forward. It doesn't actually scan each time!
        // Just waits for drone/user to get back to the point in the interesection.
		if( currently_reversing == true )
		{
			cout << "Begin reversing" << endl;
			vector <double*> :: reverse_iterator dprit;
			for (dprit = dpos.rbegin(); *dprit != Navi::INTERSECTION_INDICATOR; ++dprit)
			{
				ReadyToReadScan();
				cout << "Go: " << current_direction << endl;
				//dpos.pop_back();
			}

			cout << "End reversing" << endl;
			currently_reversing = false;
			did_reverse = true;
		}

		//just don't want to print out what direction to go twice
		if(did_reverse != true)
		{
			cout << "Go: " << current_direction << endl;
		}
		else //we did just reverse
		{
			did_reverse = false;
		}

	}

}

/*  handleIntersection: We know an intersection happened, and we are handeling the process of the interesection.

    Post-Conditions:
        dpos is updated with INTERSECTION_INDICATOR
        ddirec updated with intersection information
*/
void Navi::handleIntersection(){
	cout << "INTERSECTION!!!!!" << endl;

	bool prev_intersection = false;

	char prev_direction = current_direction;

	current_direction = intersection.direction;

	//looking to see if we are at a new intersection!
    // Doing this by seeing what positions we have been at; i.e. the X Y coordinates
    // if this X Y coordinate is not in the dpos, then we say it is a new intersection
    //  NOTE: This will probably cause potential problems. Will need some love I am sure.
	if ( !IsValueInDpos(nia_position) )
	{
        //indicator of an intersection
		ddirec.push_back('x');
		//If we reach an intersection, don't want to go down the way we came
		// when we come back to it
		ddirec.push_back(ReverseDirection(prev_direction));
	}
	else //we are at a previous intersection
	{
		prev_intersection = true;
	}

	//This means we need to go back through the other paths we did not take yet
	if( prev_intersection == true )
	{
		//making temp ints for intersection maxes
        //imbd = intersection max back distance; mbd = max back distance
		double imbd = mbd;
        //imfd = intersection max forward distance; mfd = max forward distance
		double imfd = mfd;
        //imld = intersection max left distance; mld = max left distance
		double imld = mld;
        //imrd = intersection max right distance; mrd = max right distance
		double imrd = mrd;

        //used to send into the ChangeDistance function
		double idist[4] = {imbd, imfd, imld, imrd};

		//the idea behind this is that we go through all the previous routes we
		// went to at this intersection and make their max 0 (temporarily)
		// when we do this, we know which new direction to go
		vector <char> :: reverse_iterator ddrit;
		for (ddrit = ddirec.rbegin(); ddrit != ddirec.rend() && (*ddrit) != 'x'; ddrit++)
		{
            //idist values can/should change after this call!
			ChangeDistance(*ddrit, 0, idist);
		}

        //update to the new max distance values
		imbd = idist[0];
		imfd = idist[1];
		imld = idist[2];
		imrd = idist[3];

        //Checking l and f here, since it will basically check all directions at the
		// intersection. This is mostly arbitrary.
		current_direction = 'l';
		intersection = CheckForIntersections(imbd, imfd, imld, imrd, current_direction);
        //If there wasn't an interesection indcated, then check the perpendicular axis (i.e. check for forward)
		if(intersection.direction == 'n')
		{
			current_direction = 'f';
			intersection = CheckForIntersections(imbd, imfd, imld, imrd, current_direction);
		}

		current_direction = intersection.direction;

        //If both left and front did not indicate an interesection, that means there was no path to go down, and is thus
        // no other paths to take. This will make the drone end at this interesection though.
		if (current_direction == 'n')
		{
			cout << "THE DRONE HAS NOWHERE ELSE TO GO! NAVIGATION COMPLETE!" << endl;
			exit(0);
		}

	}

    //inditaces there is an intersection at this position after the last X Y coordinate
	dpos.push_back(Navi::INTERSECTION_INDICATOR);

	//don't want to go back down this way when we travel back to the
	// intersection from another direction, so make sure to put on ddirec
	ddirec.push_back(current_direction);
}

/* ReadyToReadScan: This just waits until a command is given ('y') to indicate that
                     the drone/user is ready for the navigation to read the new scan file.
*/
void Navi::ReadyToReadScan()
{
	char scan_ready;
	cout << "Ready for next scan (y for yes; e for exit)" << endl;
	bool scan_ready_loop = true;
	while(scan_ready_loop)
	{
		cin >> scan_ready;
		switch(scan_ready)
		{
			case 'y':
				scan_ready_loop = false;
				break;

			case 'e':
				exit(0);
				break;

			default:
				cout << "Please enter either 'y' or 'e'." << endl;
		}
	}
}

/* ReadScanFile: Reads the LIDAR Data.
                 Format of the lidar data should be:
                 NODE <X-coordinate> <Y-coordinate> <Z-coordinate> 0 0 0
                 <angle_in_degrees> <distance_in_meters>
                 ...

                 Note: The scan file should NOT append; it should be a new scan each time,
                        using the same file name

    Returns:
        array[0] = node coordinates; [X-coordinate][Y-coordinate]
		array[1] = max backward distance vector; [angle][distance]
		array[2] = max forward distance vector; [angle][distance]
		array[3] = max left distance vector; [angle][distance]
		array[4] = max right distance vector; [angle][distance]
*/
double ** Navi::ReadScanFile(string fn)
{
	ifstream fin;
	double scan_angle;
	double scan_dist;

	double *node_info = new double[2];
	double *max_forward_x = NULL;
	double *max_backward_x = NULL;
	double *max_left_y = NULL;
	double *max_right_y = NULL;

	string info_line;

	string junk;
	double x_node_info;
	double y_node_info;

	fin.open(fn);

	//the first line is the "NODE" line, which should be discarded here
	getline(fin, info_line);

	istringstream iss(info_line);
    //junk is the "NODE" string
	iss >> junk;
	iss >> x_node_info;
	iss >> y_node_info;
    //don't need the z-coordinate or the 0 0 0; the 0 0 0 is for point clouds, which we didn't use
	node_info[0] = x_node_info;
	node_info[1] = y_node_info;

	//getting each x and y pair of data
	while( getline(fin, info_line) )
	{
		istringstream iss(info_line);

		iss >> scan_angle;
		iss >> scan_dist;

        //checking to see how this scan does when compared to all directions;
        // need to do this since it might be closer to one of angles than another
        // i.e. if the angle is 1 degree, it will be very close to the x forward axis, which is 0/360.
        //      if there isn't anything closer, it will be the max distance for the forward.
        //      But, 1 degree is not indictive to backwards, which is 180 degrees.
		max_backward_x = CheckMaxCoord(scan_angle, scan_dist, 'b', max_backward_x);
		max_forward_x = CheckMaxCoord(scan_angle, scan_dist, 'f', max_forward_x);
		max_left_y = CheckMaxCoord(scan_angle, scan_dist, 'l', max_left_y);
		max_right_y = CheckMaxCoord(scan_angle, scan_dist, 'r', max_right_y);
	}
	fin.close();

    //This is just so we can return everything at once
	double ** max_arrays = new double*[5];
	max_arrays[0] = node_info;
	max_arrays[1] = max_backward_x;
	max_arrays[2] = max_forward_x;
	max_arrays[3] = max_left_y;
	max_arrays[4] = max_right_y;

	return max_arrays;
}

/*  CheckMaxCoord: Returns the max distance, based on the angles closest to the x and y axis;
                    i.e. 0/360, 90, 180, and 270 degrees

    Parameters:
        angle: the angle of the current scan information
        dist: the distance of the current scan information
        check_axis: indicates if we are checking 'f'orward (0/360), 'b'ackward(180), 'l'eft(90), or 'r'ight(270).

    Return: a double array for the current angle closest to the respective check_axis, with its respective distance.
*/
double * Navi::CheckMaxCoord(double angle, double dist, char check_axis, double current_min[])
{
    //keeps track of the min array
	double * new_min_array = new double[2];

	//if it is the first loop, need to set the min to current point as initialization
	if( current_min == NULL )
	{
		new_min_array[0] = angle;
		new_min_array[1] = dist;
		return new_min_array;
	}

    //check_angle is used to compare against the current angle
    double check_angle = current_min[0];
    //check_dist is used to compare against the current distance
	double check_dist = current_min[1];

    //need large values since we are checking for min
    double angle_calc = 100000;
    double calc_check_angle = 100000;

	new_min_array[0] = current_min[0];
	new_min_array[1] = current_min[1];

    switch(check_axis)
    {
        //backwards
        case 'b':
            //check to see how close angle is to 180 degrees
            angle_calc = abs(angle - 180);
            calc_check_angle = abs(check_angle - 180);
            //if this angle is closer to 180, then change the min array
            if (angle_calc < calc_check_angle)
            {
                new_min_array[0] = angle;
                new_min_array[1] = dist;
            }
            break;

        //forward
        case 'f':
            if (angle >= 180) //check for numbers closer to 360
            {
                if( check_angle >= 180 ) //closer to 360
                {
                    calc_check_angle = abs(check_angle - 360);
                }
                else //is closer to 0
                {
                    calc_check_angle = abs(check_angle - 0);
                }

                angle_calc = abs(angle - 360);

                //if this angle is closer to 360, then change the min array
                if (angle_calc < calc_check_angle)
                {
                    new_min_array[0] = angle;
                    new_min_array[1] = dist;
                }
            }
            else //check for numbers closer to 0
            {
                if( check_angle >= 180 ) //closer to 360
                {
                    calc_check_angle = abs(check_angle - 360);
                }
                else //is closer to 0
                {
                    calc_check_angle = abs(check_angle - 0);
                }
                angle_calc = abs(angle - 0);

                //if this angle is closer to 0, then change the min array
                if (angle_calc < calc_check_angle)
                {
                    new_min_array[0] = angle;
                    new_min_array[1] = dist;
                }
            }
            break;

        //left
        case 'l':
            //check to see how close angle is to 90
            angle_calc = abs(angle - 90);
            calc_check_angle = abs(check_angle - 90);

            //if this angle is closer to 90, then change the min array
            if (angle_calc < calc_check_angle)
            {
                new_min_array[0] = angle;
                new_min_array[1] = dist;
            }
            break;

        //right
        case 'r':
            //check to see how close angle is to 270
            angle_calc = abs(angle - 270);
            calc_check_angle = abs(check_angle - 270);
            //if this angle is closer to 270, then change the min array
            if (angle_calc < calc_check_angle)
            {
                new_min_array[0] = angle;
                new_min_array[1] = dist;
            }
            break;

        default:
            cout << "CheckMaxCoor check_axis is not valid; or you are at the end of the navigation(?)." << endl;
            break;

    }
    delete[] current_min;

	return new_min_array;
}

/* FindMaxDistance: Goes through the distances of the 4 directions and finds the max distance among them.
    Parameters:
        bd: back distance
        fd: forward distance
        ld: left distance
        rd: right distance

    Returns:
        The distance and direction of the direction that had the maximum distance
*/
DistAndDirec Navi::FindMaxDistance(double bd, double fd, double ld, double rd)
{
    //I am putting fd first because this would be the direction to move
    //if there is a tie between the distances.
	double distance_array[4] = {fd, bd, ld, rd};

    //need to initialize to 0 since looking for max
	double cur_max = 0;
	int best_index = -1;

	char direction = 'n';

    //goes through each distance to find the max
	for(int i = 0; i < 4; i++)
	{
		if (abs(distance_array[i]) > cur_max)
		{
			cur_max = abs(distance_array[i]);
			best_index = i;
		}
	}

    // index indicated by above loop to find max distance
	switch(best_index)
	{
		case 0:
			direction = 'f';
			break;
		case 1:
			direction = 'b';
			break;
		case 2:
			direction = 'l';
			break;
		case 3:
			direction = 'r';
			break;
		default:
			cout << "This should be an intersection." << endl;
			break;

	}

	drone_dd_vals.distance = cur_max;
	drone_dd_vals.direction = direction;

	return drone_dd_vals;
}

/*  CheckForIntersections: Checks to see if there is an interesection

    Parameters:
        bd = backwards distance
        fd = forward distance
        ld = left distance
        rf = right distance
        cd = current direction

    Returns:
        The direction and distance for which way to go down the interesection, based on max distance.
*/
DistAndDirec Navi::CheckForIntersections(double bd, double fd, double ld, double rd, char cd)
{
	intersection.distance = 0;
    //n is used for error checking/knowing there is nothing left to check at the intersection
	intersection.direction = 'n';

	//We check for f and b here, because we don't want to go in the opposite direction
	// and loop between the two largest values.
	if( cd == 'f' || cd == 'b' )
	{
		//0, 0 is because we only want to look for mld and mrd
		drone_dd_vals = FindMaxDistance(0, 0, ld, rd);
		if (drone_dd_vals.distance >= Navi::INTERSECTION_THRESHOLD)
		{
			intersection.distance = drone_dd_vals.distance;
			intersection.direction = drone_dd_vals.direction;
		}
	}

    //We check for l and r here, because we don't want to go in the opposite direction
	// and loop between the two largest values.
	if( cd == 'l' || cd == 'r' )
	{
		//0, 0 is because we only want to look for mld and mrd
		drone_dd_vals = FindMaxDistance(bd, fd, 0, 0);
		if (drone_dd_vals.distance >= Navi::INTERSECTION_THRESHOLD)
		{
			intersection.distance = drone_dd_vals.distance;
			intersection.direction = drone_dd_vals.direction;
		}
	}

	return intersection;
}

/*  GetDistance: Gets the distance of the current direction

    Parameters:
        cd = current direction

    Returns:
        Distance of the current direction
*/
double Navi::GetDistance(char cd)
{
	double current_distance;

	switch(cd)
	{
		case 'b':
			current_distance = abs(mbd);
			break;
		case 'f':
			current_distance = abs(mfd);
			break;
		case 'l':
			current_distance = abs(mld);
			break;
		case 'r':
			current_distance = abs(mrd);
			break;
		default:
			cout << "Error in GetDistance fuction; invalid direcetion input." << endl;
			break;
	}

	return current_distance;
}

/*  ReverseDirection: Gives the direction opposite of the current direction moving.

    Parameters:
        cd = current_direction

    Returns:
        The direction opposite of the current moving direction.
        i.e. if going forward, return 'b'ackwards, and etc.
*/
char Navi::ReverseDirection(char cd)
{
	//new direction
	char nd;

	switch(cd)
	{
		case 'f':
			nd = 'b';
			break;
		case 'b':
			nd = 'f';
			break;
		case 'l':
			nd = 'r';
			break;
		case 'r':
			nd = 'l';
			break;
		default:
			cout << "STOP MESSING THINGS UP!" << endl;
			break;
	}

	return nd;
}

/*  ChangeDistance: Changes the value of one value in the dist array, based on the cd.

    Parameters:
        cd = current_direction; this indicates which value to change in the dist array
        dist = array of [backwards dist, foward dist, left dist, right dist]
        cv = The value to change to

    Post-Conditions:
        The value in the dist array will change to the cv value, based on the cd index.
*/
void Navi::ChangeDistance(char cd, double cv, double * dist)
{
	//new value
	double nv;

	switch(cd)
	{
		case 'f': //forward
			dist[1] = cv;
			break;
		case 'b': //backward
			dist[0] = cv;
			break;
		case 'l': //left
			dist[2] = cv;
			break;
		case 'r': //right
			dist[3] = cv;
			break;
		default:
			cout << "Invalid input of direction in ChangeDistance function." << endl;
			break;
	}
}

/* IsValueInDpos: Checks to see if a X Y node coordinate is in dpos

    Parameters:
        value: the X Y coordinate pair to check for in dpos

    Returns:
        True if value is in the dpos array; false otherwise.
*/
bool Navi::IsValueInDpos(double *value)
{
	vector <double*> :: iterator dpit;
    //goes through the dpos array
	for( dpit = dpos.begin(); dpit != dpos.end(); dpit++)
	{
		//if X and Y are same values, then is in dpos vector
        if( (*dpit)[0] == value[0] && (*dpit)[1] == value[1] )
		{
			return true;
		}
	}
	return false;
}


#ifndef N_BODY_RANDOM_HPP
#define N_BODY_RANDOM_HPP

#include <boost/mpi/collectives.hpp>
#include <boost/mpi/communicator.hpp>
#include <cstdint>
#include <memory>
#include <random>

namespace n_body::random {

template <typename UIntType, UIntType m> class Engine;

using MinimunStandardEngine = Engine<std::uint_fast32_t, 2147483647>;

// random engine for parallel random number generating
// the engine must be construct at the same time on all mpi process
template <typename UIntType, UIntType m> class Engine {
public:
  using result_type = UIntType;

  Engine(const boost::mpi::communicator &comm, int root, UIntType seed) {
    const UIntType origin_a = 48271;
    const UIntType origin_c = 0;

    const int size = comm.size();

    if (comm.rank() == root) {
      std::vector<UIntType> xs;
      xs.reserve(size);
      xs.push_back(seed);
      this->a = 1;
      this->c = 0;

      for (int i = 0; i < size; ++i) {
        if (i != 0)
          xs.push_back(run(origin_a, origin_c, xs[i - 1]));
        this->c += this->a;
        this->c %= m;
        this->a *= origin_a;
        this->a %= m;
      }
      this->c *= origin_c;

      boost::mpi::scatter(comm, xs, this->current, root);
    } else {
      boost::mpi::scatter(comm, this->current, root);
    }
    boost::mpi::broadcast(comm, this->a, root);
    boost::mpi::broadcast(comm, this->c, root);
  }

  Engine(const boost::mpi::communicator &comm, int root)
      : Engine(comm, root, new_seed_in_root(comm, root)) {}

  static constexpr result_type min() { return 1; }

  static constexpr result_type max() { return m - 1; }

  result_type operator()() {
    this->current = run(this->a, this->c, this->current);
    return this->current;
  }

private:
  static UIntType new_seed_in_root(const boost::mpi::communicator &comm,
                                   int root) {
    if (comm.rank() == root) {
      std::random_device rd;
      std::uniform_int_distribution<UIntType> dist(min(), max());
      return dist(rd);
    } else {
      return 0;
    }
  }

  UIntType run(UIntType pa, UIntType pc, UIntType x) {
    return (pa * x + pc) % m;
  }

  UIntType a;
  UIntType c;

  UIntType current;
};

} // namespace n_body::random

#endif


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

#include <bfl/filter/bootstrapfilter.h>
#include <bfl/model/systemmodel.h>
#include <bfl/model/measurementmodel.h>
#include <bfl/pdf/conditionalpdf.h>
#include <bfl/pdf/gaussian.h>
#include <iostream>

#include <ros/ros.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/PoseArray.h>
#include <geometry_msgs/TransformStamped.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <std_msgs/Header.h>
#include <tf/tf.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2/utils.h>

const float WHEEL_RAD = 0.27;
tf2::Transform lastCenterPose;
/**
 * @brief 姿勢を表示
 * @param arg 姿勢データ
 */
void printTrans(tf2::Transform arg) {
  double roll, pitch, yaw;
  tf2::getEulerYPR(arg.getRotation(), yaw, pitch, roll);
  ROS_INFO_STREAM("x:" << arg.getOrigin().getX() << " y:" << arg.getOrigin().getY()
                       << " z:" << arg.getOrigin().getZ() << " yaw:" << yaw * 180.0 / 3.14
                       << " pitch:" << pitch * 180.0 / 3.14 << " roll:" << roll * 180.0 / 3.14);
}

//////////////////////
//////////////////////
// nonlinearSystemPdf/
//////////////////////
#include <math.h>
#include <wrappers/rng/rng.h>  // Wrapper around several rng libraries

#define SYSMODEL_NUMCONDARGUMENTS_MOBILE 2
#define SYSMODEL_DIMENSION_MOBILE 3

namespace BFL {
using namespace MatrixWrapper;
class NonlinearSystemPdf
    : public ConditionalPdf<MatrixWrapper::ColumnVector, MatrixWrapper::ColumnVector> {
 public:
  // @param additiveNoise Pdf representing the additive Gaussian uncertainty
  NonlinearSystemPdf(const Gaussian& additiveNoise)
      : ConditionalPdf<ColumnVector, ColumnVector>(SYSMODEL_DIMENSION_MOBILE,
                                                   SYSMODEL_NUMCONDARGUMENTS_MOBILE) {
    _additiveNoise = additiveNoise;
  }
  virtual ~NonlinearSystemPdf(){};
  /**
 * @brief パーティクルの移動
 * @param one_sample 状態遷移の完了したパーティクル
 * @detail 要はノイズの乗った次状態予測値をone_sampleで返せればおｋ
 */  // implement this virtual function for system model of a particle filter
  virtual bool SampleFrom(Sample<MatrixWrapper::ColumnVector>& one_sample, int method = DEFAULT,
                          void* args = NULL) const {
    //! state: [x, y, rad, 左車輪定常ノイズ, 右車輪定常ノイズ, z,  x, y, z, w],
    //パーティクルの状態変数
    ColumnVector state = ConditionalArgumentGet(0);
    //! trans: [移動距離, 角度変化], オドメトリの生データ
    ColumnVector input = ConditionalArgumentGet(1);

    // 現在座標の型変換
    tf2::Transform currentTrans;
    currentTrans.getOrigin().setX(state(1));
    currentTrans.getOrigin().setY(state(2));
    currentTrans.getOrigin().setZ(state(6));
    tf2::Quaternion quat(state(7), state(8), state(9), state(10));
    currentTrans.setRotation(quat.normalize());

    const char ODOM_DIM = 2;
    const char TRANS_DIM = 7;
    // ROS_INFO_STREAM("rows:"<<input.rows());
    if (ODOM_DIM == input.rows()) {
      // sample from additive noise
      ///! noise: [左車輪白色ノイズ, 右車輪白色ノイズ],
      ///平均値0標準偏差10のガウス分布に従う乱数から値を獲得
      Sample<ColumnVector> noise;
      _additiveNoise.SampleFrom(noise, method, args);

      // 左右車輪の滑りの白色ノイズ
      const float err_l = noise.ValueGet()(1);
      const float err_r = noise.ValueGet()(2);
      // 左右車輪の直径スケールのノイズ
      ColumnVector mu(2);
      mu(1) = 0;
      mu(2) = 0;
      SymmetricMatrix cov(2);
      cov(1, 1) = pow(0.001, 2);
      cov(1, 2) = 0.0;
      cov(2, 1) = 0.0;
      cov(2, 2) = pow(0.001, 2);
      Gaussian scale_gaus(mu, cov);
      //! @todo:車輪半径の差は考慮するほどずれていないため無視
      const float scale_err_l = 0.0;
      const float scale_err_r = 0.0;
      /*Sample<ColumnVector> scale_noise;
      scale_gaus.SampleFrom(scale_noise, method);
      const float scale_err_l = state(4) + scale_noise.ValueGet()(1);
      const float scale_err_r = state(5) + scale_noise.ValueGet()(2);
      if (0.5<fabs(scale_err_l)||0.5<fabs(scale_err_r)){
        ROS_WARN_STREAM("scale_err_l:"<<scale_err_l<<"   scale_err_r:"<<scale_err_r);
      }*/
      // ノイズの乗った左右車輪速度
      const float vel_odom = input(1);
      const float omega_odom = input(2);
      const float Vl = (vel_odom - omega_odom * WHEEL_RAD) * (1.0 + err_l + scale_err_l);
      const float Vr = (vel_odom + omega_odom * WHEEL_RAD) * (1.0 + err_r + scale_err_r);
      // 移動距離，変化角度
      const float vel = (Vl + Vr) / 2.0;
      const float omega = (Vr - Vl) / (2.0 * WHEEL_RAD);

      ///! パーティクルの状態変数を更新
      tf2::Transform diffTrans;
      // オドメトリによる変移の型変換
      diffTrans.getOrigin().setX(vel);
      quat.setRPY(0, 0, omega);
      diffTrans.setRotation(quat.normalize());
      // 遷移後の姿勢を計算
      currentTrans = currentTrans * diffTrans;
      // state(4) = scale_err_l;
      // state(5) = scale_err_r;
    } else if (TRANS_DIM == input.rows()) {
      // 重心からみた該当パーティクルの姿勢
      tf2::Transform centParticleTrans = lastCenterPose.inverse() * currentTrans;
      // 重心の姿勢変化
      tf2::Transform centDiffTrans;
      centDiffTrans.getOrigin().setX(input(1));
      centDiffTrans.getOrigin().setY(input(2));
      centDiffTrans.getOrigin().setZ(input(3));
      tf2::Quaternion quat(input(4), input(5), input(6), input(7));
      centDiffTrans.setRotation(quat.normalize());
      // 重心との相対姿勢を保ったままパーティクルを移動
      ROS_INFO("before:");
      printTrans(currentTrans);
      ROS_INFO("lastCenterPose:");
      printTrans(lastCenterPose);
      ROS_INFO("centParticleTrans:");
      printTrans(centParticleTrans);
      ROS_INFO("centDiffTrans:");
      printTrans(centDiffTrans);
      currentTrans = (lastCenterPose * centDiffTrans) * centParticleTrans;
      ROS_INFO("after:");
      printTrans(currentTrans);
      ROS_INFO("a");
    }
    currentTrans.setRotation(currentTrans.getRotation().normalize());
    geometry_msgs::Transform currentTransMsg = tf2::toMsg(currentTrans);

    // 状態変数に保存
    state(1) = currentTransMsg.translation.x;
    state(2) = currentTransMsg.translation.y;
    state(6) = currentTransMsg.translation.z;
    state(7) = currentTransMsg.rotation.x;
    state(8) = currentTransMsg.rotation.y;
    state(9) = currentTransMsg.rotation.z;
    state(10) = currentTransMsg.rotation.w;

    state(3) = tf2::getYaw(currentTrans.getRotation());

    // パーティクルの新しい状態を返す
    one_sample.ValueSet(state);
    return true;
  }

 private:
  Gaussian _additiveNoise;
};
}  // End namespace BFL

///////////////////////////
///////////////////////////
// NonlinearMeasurementPdf/
///////////////////////////
#define MEASMODEL_NUMCONDARGUMENTS_MOBILE 1
#define MEASMODEL_DIMENSION_MOBILE 3
namespace BFL {
using namespace MatrixWrapper;
/// Non Linear Conditional Gaussian
class NonlinearMeasurementPdf
    : public ConditionalPdf<MatrixWrapper::ColumnVector, MatrixWrapper::ColumnVector> {
 public:
  //  @param additiveNoise Pdf representing the additive Gaussian uncertainty
  NonlinearMeasurementPdf(const Gaussian& measNoise)
      : ConditionalPdf<ColumnVector, ColumnVector>(MEASMODEL_DIMENSION_MOBILE,
                                                   MEASMODEL_NUMCONDARGUMENTS_MOBILE) {
    _measNoise = measNoise;
  }
  virtual ~NonlinearMeasurementPdf() {}

  /**
 * @brief パーティクルの確率取得
 * @param measurement センサで観測されたグローバル座標
 * @detail 要は確率をProbability型で返せればおｋ
 *          Probability p(0.3)とかでも作れる
 */  // implement this virtual function for measurement model of a particle filter
  virtual Probability ProbabilityGet(const MatrixWrapper::ColumnVector& measurement) const {
    //! state: [x, y, rad, 左車輪定常ノイズ, 右車輪定常ノイズ], パーティクルの状態変数
    ColumnVector state = ConditionalArgumentGet(0);
    ColumnVector expected_measurement(2);
    expected_measurement(1) = state(1);
    expected_measurement(2) = state(2);
    Probability prb = _measNoise.ProbabilityGet(measurement - expected_measurement);
    return prb;
  }

 private:
  Gaussian _measNoise;
};
}  // End namespace BFL

////////////////////////
////////////////////////
// CustomParticleFilter/
////////////////////////
using namespace BFL;

class CustomParticleFilter
    : public BootstrapFilter<MatrixWrapper::ColumnVector, MatrixWrapper::ColumnVector> {
 public:
  CustomParticleFilter(MCPdf<MatrixWrapper::ColumnVector>* prior, int resampleperiod = 0,
                       double resamplethreshold = 0, int resamplescheme = DEFAULT_RS);
  vector<WeightedSample<MatrixWrapper::ColumnVector> > getNewSamples() { return _new_samples; }
};

CustomParticleFilter::CustomParticleFilter(MCPdf<ColumnVector>* prior, int resampleperiod,
                                           double resamplethreshold, int resamplescheme)
    : BootstrapFilter<ColumnVector, ColumnVector>(prior, resampleperiod, resamplethreshold,
                                                  resamplescheme) {}

/////////
/////////
// main//
/////////
using namespace MatrixWrapper;
// using namespace BFL;
using namespace std;

const int NUM_SAMPLES = 200;
// const int STATE_SIZE = 3;
// const int STATE_SIZE = 5;  //! [x, y, θ, 左車輪定常ノイズ, 右車輪定常ノイズ]
const int STATE_SIZE = 10;  //! [x, y, θ, 左車輪定常ノイズ, 右車輪定常ノイズ, z  x, y, z, w]
const int INPUT_SIZE = 2;
const int MEAS_SIZE = 2;

// SystemModel<ColumnVector>* g_sys_model;
// MeasurementModel<ColumnVector, ColumnVector>* g_meas_model;
CustomParticleFilter* g_filter;

ros::Publisher pub_filtered;
ros::Publisher particle_pub;
bool is_got_initial_pose = false;
std_msgs::Header g_latest_header;
tf2_ros::Buffer tfBuffer_;
tf2_ros::TransformListener* tfListener_;
std::vector<geometry_msgs::PointStamped> observeBuffer;
std::vector<nav_msgs::Odometry> odomBuffer;

/**
 * @brief PFで推定した姿勢を発行
 */
void PublishPose() {
  if (!is_got_initial_pose) {
    return;
  }
  Pdf<ColumnVector>* posterior = g_filter->PostGet();                         // 座標計算用
  vector<WeightedSample<ColumnVector> > samples = g_filter->getNewSamples();  // 角度計用
  ColumnVector pose = posterior->ExpectedValueGet();
  SymmetricMatrix pose_cov = posterior->CovarianceGet();

  // ROS_INFO_STREAM("Err_l:" << pose(4) << " Err_r:" << pose(5));

  nav_msgs::Odometry pose_msg;
  pose_msg.header.stamp = g_latest_header.stamp;
  pose_msg.header.frame_id = "map";

  pose_msg.pose.pose.position.x = pose(1);
  pose_msg.pose.pose.position.y = pose(2);
  pose_msg.pose.pose.position.z = pose(6);

  // 四元数の加重平均を取得
  // 参考:http://stackoverflow.com/questions/12374087/average-of-multiple-quaternions
  vector<WeightedSample<ColumnVector> >::iterator sample_it;
  Eigen::Matrix<double, 4, NUM_SAMPLES> quatMat;
  int i = 0;
  for (sample_it = samples.begin(); sample_it < samples.end(); sample_it++) {
    ColumnVector sample = (*sample_it).ValueGet();
    double w = (*sample_it).WeightGet();
    quatMat(0, i) = w * sample(7);
    quatMat(1, i) = w * sample(8);
    quatMat(2, i) = w * sample(9);
    quatMat(3, i) = w * sample(10);
    i++;
  }
  Eigen::SelfAdjointEigenSolver<Eigen::Matrix<double, 4, 4> > es(quatMat * quatMat.transpose());
  // cout << "The eigenvalues of A are:" << endl << es.eigenvalues() << endl;
  // cout << "The matrix of eigenvectors, V, is:" << endl << es.eigenvectors() << endl << endl;
  // 最後尾の最大固有ベクトルを利用
  Eigen::Vector4d quatAve = es.eigenvectors().col(3);

  pose_msg.pose.pose.orientation.x = quatAve(0);
  pose_msg.pose.pose.orientation.y = quatAve(1);
  pose_msg.pose.pose.orientation.z = quatAve(2);
  pose_msg.pose.pose.orientation.w = quatAve(3);

  pub_filtered.publish(pose_msg);
  tf2::fromMsg(pose_msg.pose.pose, lastCenterPose);
}

/**
 * @brief パーティクル自体を発行
 */
void PublishParticles() {
  if (!is_got_initial_pose) {
    return;
  }
  geometry_msgs::PoseArray particles_msg;
  particles_msg.header.stamp = g_latest_header.stamp;
  particles_msg.header.frame_id = "map";

  vector<WeightedSample<ColumnVector> >::iterator sample_it;
  vector<WeightedSample<ColumnVector> > samples;

  samples = g_filter->getNewSamples();
  for (sample_it = samples.begin(); sample_it < samples.end(); sample_it++) {
    geometry_msgs::Pose pose;
    ColumnVector sample = (*sample_it).ValueGet();
    pose.position.x = sample(1);
    pose.position.y = sample(2);
    pose.position.z = sample(6);
    pose.orientation.x = sample(7);
    pose.orientation.y = sample(8);
    pose.orientation.z = sample(9);
    pose.orientation.w = sample(10);
    // pose.orientation = tf::createQuaternionMsgFromYaw(sample(3));
    particles_msg.poses.insert(particles_msg.poses.begin(), pose);
  }
  particle_pub.publish(particles_msg);
}

/**
 * @brief パーティクルフィルタのパーティクル群を生成
 * @detail 引数は座標，角度，座標の分散，角度の分散
 */
void GenInitialParticles(double x, double y, double rad, double cov_trans, double cov_rad) {
  //パーティクルの初期配置
  ColumnVector prior_Mu(
      STATE_SIZE);  // [x, y, θ, 左車輪定常ノイズ, 右車輪定常ノイズ, z  x, y, z, w]
  prior_Mu(1) = x;
  prior_Mu(2) = y;
  prior_Mu(3) = rad;  //角度
  prior_Mu(4) = 0.0;  //左車輪定常ノイズ
  prior_Mu(5) = 0.0;  //右車輪定常ノイズ
  prior_Mu(6) = 0.0;  // z
  tf2::Quaternion quat;
  quat.setRPY(0, 0, rad);
  quat = quat.normalize();
  geometry_msgs::Quaternion quatMsg = tf2::toMsg(quat);
  prior_Mu(7) = quatMsg.x;   // quat x
  prior_Mu(8) = quatMsg.y;   // quat y
  prior_Mu(9) = quatMsg.z;   // quat z
  prior_Mu(10) = quatMsg.w;  // quat w
  SymmetricMatrix prior_Cov(STATE_SIZE);
  prior_Cov = 0.0;
  prior_Cov(1, 1) = cov_trans;
  prior_Cov(2, 2) = cov_trans;
  prior_Cov(3, 3) = cov_rad;
  // 初期配置で利用する分布
  Gaussian prior_cont(prior_Mu, prior_Cov);

  // Discrete prior for Particle filter (using the continuous Gaussian prior)
  // 初期分布が離散化されたパーティクルを取得
  vector<Sample<ColumnVector> > prior_samples(NUM_SAMPLES);
  prior_cont.SampleFrom(prior_samples, NUM_SAMPLES, CHOLESKY, NULL);
  MCPdf<ColumnVector> prior_discr(NUM_SAMPLES, STATE_SIZE);
  prior_discr.ListOfSamplesSet(prior_samples);
  if (is_got_initial_pose) {
    // 古いパーティクル分布を消す
    delete g_filter;
  }
  // パーティクルフィルタを生成
  // g_filter = new CustomParticleFilter(&prior_discr, 0, NUM_SAMPLES / 4.0, MULTINOMIAL_RS);
  g_filter = new CustomParticleFilter(&prior_discr, 0.0, NUM_SAMPLES * 9.0 / 10.0, MULTINOMIAL_RS);
  is_got_initial_pose = true;

  ///! 推定姿勢発行
  PublishPose();
  PublishParticles();
}

ros::Time odomLast;
ros::Time observeLast;

/**
 * @brief オドメトリでパーティクルの予測（移動）
 * @param arg オドメトリ
 */
void pf_predict(const nav_msgs::Odometry::ConstPtr& arg) {
  /// LRFから初期位置が観測されるまで待つ
  if (!is_got_initial_pose) {
    return;
  }

  ///! オドメトリの周期を計算
  static ros::Time prevNavDataTime;
  if (prevNavDataTime.isZero()) {
    prevNavDataTime = arg->header.stamp;
    return;
  }
  double dt = (arg->header.stamp - prevNavDataTime).toSec();
  prevNavDataTime = arg->header.stamp;
  g_latest_header = arg->header;
  odomLast = arg->header.stamp;

  ///! 移動距離，変化角度計算
  ColumnVector input(2);  // [v, omega]
  input(1) = dt * arg->twist.twist.linear.x;
  input(2) = dt * arg->twist.twist.angular.z;

  //  /****************************
  //  * NonLinear system model      *
  //  ***************************/
  ///! オドメトリには標準偏差が速度の10%となるガウスノイズがのる
  ColumnVector mu(INPUT_SIZE);
  mu(1) = 0.0;
  mu(2) = 0.0;
  SymmetricMatrix cov(INPUT_SIZE);
  cov(1, 1) = pow(0.1, 2);
  cov(1, 2) = 0.0;
  cov(2, 1) = 0.0;
  cov(2, 2) = pow(0.1, 2);
  Gaussian gaus_noise(mu, cov);
  NonlinearSystemPdf sys_pdf(gaus_noise);
  SystemModel<ColumnVector> sys_model(&sys_pdf);
  // g_sys_model->SystemPdfSet(&sys_pdf);

  ///! パーティクルフィルタを更新
  // ROS_INFO("odom updating");
  g_filter->Update(&sys_model, input);

  ///! 推定姿勢発行
  PublishPose();
  PublishParticles();
}

/**
 * @brief 観測座標を持ちいてPFの更新（重み付け）
 * @param arg 車輪の座標
 */
void pf_update(const geometry_msgs::PointStamped::ConstPtr& arg) {
  g_latest_header = arg->header;
  observeLast = arg->header.stamp;
  geometry_msgs::PointStamped mapPoint;
  try {
    geometry_msgs::TransformStamped mapSframeTransMsg;
    mapSframeTransMsg = tfBuffer_.lookupTransform(
        "map", arg->header.frame_id, /*ros::Time(0)*/ arg->header.stamp /*ros::Time()*/,
        ros::Duration(3.0));
    tf2::doTransform(*arg, mapPoint, mapSframeTransMsg);
  } catch (tf2::TransformException& ex) {
    ROS_WARN("Could NOT transform : %s", ex.what());
    return;
  }

  if (!is_got_initial_pose) {
    GenInitialParticles(mapPoint.point.x, mapPoint.point.y, 0.0, pow(0.1, 2),
                        pow(10000000.0 * M_PI / 180.0, 2));

    return;
  }

  /// タイムスタンプ確認
  if (ros::Duration(4.0) < odomLast - observeLast) {
    ROS_INFO_STREAM("time diff:" << odomLast - observeLast);
    ROS_WARN("too large time gap. skip to update PF");
    return;
  }
  ColumnVector measurement(2);  // [x, y]
  measurement(1) = mapPoint.point.x;
  measurement(2) = mapPoint.point.y;

  // /*********************************
  //  * NonLinear Measurement model   *
  //  ********************************/
  ColumnVector mu(MEAS_SIZE);
  mu(1) = 0.0;  // E[x]
  mu(2) = 0.0;  // E[y]
  SymmetricMatrix cov(MEAS_SIZE);
  const double cov_min = 0.01;
  cov(1, 1) = 1.0;  // Var[x,x]
  cov(1, 2) = 0.0;
  cov(2, 1) = 0.0;
  cov(2, 2) = 1.0;  // Var[y,y]
  Gaussian gaus_noise(mu, cov);
  NonlinearMeasurementPdf meas_pdf(gaus_noise);
  MeasurementModel<ColumnVector, ColumnVector> meas_model(&meas_pdf);

  g_filter->Update(&meas_model, measurement);
  PublishPose();
  PublishParticles();
}

/**
 * @brief パーティクル配置の手動生成
 */
void initialPoseReceived(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& arg) {
  ROS_INFO("got initila pose");
  // geometry_msgs::PointStamped mapPoint;
  geometry_msgs::PoseStamped mapPose;
  try {
    geometry_msgs::TransformStamped mapSframeTransMsg;
    mapSframeTransMsg = tfBuffer_.lookupTransform(
        "map", arg->header.frame_id, /*ros::Time(0)*/ arg->header.stamp /*ros::Time()*/,
        ros::Duration(3.0));
    geometry_msgs::PoseStamped tmp;
    tmp.pose = arg->pose.pose;
    tmp.header = arg->header;
    tf2::doTransform(tmp, mapPose, mapSframeTransMsg);
  } catch (tf2::TransformException& ex) {
    ROS_WARN("Could NOT transform : %s", ex.what());
    return;
  }
  tf2::Transform tmp;
  tf2::fromMsg(mapPose.pose, tmp);
  double roll, pitch, yaw;
  tf2::getEulerYPR(tmp.getRotation(), yaw, pitch, roll);
  if (10.0<fabs(roll*180.0/M_PI) || 10.0<fabs(pitch*180.0/M_PI)){
    ROS_WARN("Vicon has noise.");
    return;
  }
  double rad = yaw;
  // double rad = tf::getYaw(mapPose.pose.orientation);
  GenInitialParticles(mapPose.pose.position.x, mapPose.pose.position.y, rad,
                      arg->pose.covariance.at(0), arg->pose.covariance.at(35));
}

void transCallback(const geometry_msgs::TransformStamped::ConstPtr& arg) {
  /// LRFから初期位置が観測されるまで待つ
  if (!is_got_initial_pose) {
    return;
  }
  /// 屋外が遠ければNDTを無視
  geometry_msgs::TransformStamped mapBasefoot;
  // geometry_msgs::PoseStamped mapBasefoot;
  try {
    // geometry_msgs::TransformStamped mapBasefoot;
    mapBasefoot = tfBuffer_.lookupTransform(
        "map", "base_footprint", /*ros::Time(0)*/ arg->header.stamp /*ros::Time()*/,
        ros::Duration(3.0));
  } catch (tf2::TransformException& ex) {
    ROS_WARN("Could NOT transform : %s", ex.what());
    return;
  }
  ROS_INFO_STREAM("mapBasefoot Pose: "<<mapBasefoot);
  if(-10<mapBasefoot.transform.translation.x &&
      mapBasefoot.transform.translation.x < 10 &&
    mapBasefoot.transform.translation.y < -10){
    // mapBasefoot.transform.translation.y < -6){
    ROS_INFO_STREAM("In indoor and ignore NDT transform");
    return;
  }

  const char TRANS_SIZE = 7;
  ///! 移動距離，変化角度計算
  ColumnVector input(TRANS_SIZE);  // [x, y, z,  x, y, z, w]
  input(1) = arg->transform.translation.x;
  input(2) = arg->transform.translation.y;
  input(3) = arg->transform.translation.z;
  input(4) = arg->transform.rotation.x;
  input(5) = arg->transform.rotation.y;
  input(6) = arg->transform.rotation.z;
  input(7) = arg->transform.rotation.w;
  ColumnVector mu(TRANS_SIZE);
  mu = 0.0;
  SymmetricMatrix cov(TRANS_SIZE);
  cov = 0.00000000000000001;
  Gaussian gaus_noise(mu, cov);
  NonlinearSystemPdf sys_pdf(gaus_noise);
  SystemModel<ColumnVector> sys_model(&sys_pdf);

  ///! パーティクルフィルタを更新
  g_filter->Update(&sys_model, input);

  ///! 推定姿勢発行
  PublishPose();
  PublishParticles();
}

// void PushOdom(const nav_msgs::Odometry::ConstPtr& arg){
//   odomBuffer.pushback(*arg);
// }

// void PushObserve(const geometry_msgs::PointStamped::ConstPtr& arg){
//   observeBuffer.pushback(*arg);
// }

// /**
//  * @brief call PF as stamped time
//  * @param e TimerEvent status
//  */
// void pf_spin(const ros::TimerEvent& e){
//   ROS_DEBUG("Spin function at time %f", ros::Time::now().toSec());
//   ros::Time filter_stamp_ = ros::Time::now()-ros::Duration(2.0);
//   filter_stamp_ = min(filter_stamp_, odom_stamp_);
//   if (imu_active_)   filter_stamp_ = min(filter_stamp_, imu_stamp_);
// }

/**
 * @brief main
 */
int main(int argc, char** argv) {
  cout << "Hello World" << endl;

  // Gaussian useless_sys_noise(INPUT_SIZE);
  // Gaussian useless_sys_noise(STATE_SIZE);

  Gaussian useless_meas_noise(MEAS_SIZE);
  NonlinearMeasurementPdf meas_pdf(useless_meas_noise);
  MeasurementModel<ColumnVector, ColumnVector> meas_model(&meas_pdf);

  /******************************
   * Construction of the Filter *
   ******************************/
  // MULTINOMIAL_RSでしか動作しない．
  // CustomParticleFilter filter(&prior_discr, 0, NUM_SAMPLES / 4.0, MULTINOMIAL_RS);
  // g_filter = &filter;
  // NonlinearSystemPdf sys_pdf(useless_sys_noise);
  // SystemModel<ColumnVector> sys_model(&sys_pdf);
  // g_sys_model = &sys_model;
  // g_meas_model = &meas_model;

  ROS_INFO("Hello World!");
  ros::init(argc, argv, "bfl_pf");
  // tf
  tf2_ros::TransformListener tmp_listener(tfBuffer_);
  tfListener_ = &tmp_listener;

  ros::NodeHandle nh;
  // ros::NodeHandle nh_private("~");
  pub_filtered = nh.advertise<nav_msgs::Odometry>("pf_filtered", 5);
  particle_pub = nh.advertise<geometry_msgs::PoseArray>("pf_particles", 5);
  ros::Subscriber pf_predict_sub = nh.subscribe<nav_msgs::Odometry>("odom", 10, pf_predict);
  ros::Subscriber pf_update_sub =
      nh.subscribe<geometry_msgs::PointStamped>("observation_point", 10, pf_update);
  ros::Subscriber initialpose_sub = nh.subscribe<geometry_msgs::PoseWithCovarianceStamped>(
      "initialpose", 10, initialPoseReceived);
  ros::Subscriber trans_sub =
      nh.subscribe<geometry_msgs::TransformStamped>("trans", 10, transCallback);
  // ros::Subscriber pf_predict_sub = nh.subscribe<nav_msgs::Odometry>("odom", 10, PushOdom);
  // ros::Subscriber pf_update_sub =
  //     nh.subscribe<geometry_msgs::PointStamped>("lrf_pose", 10, PushObserve);

  // 原点位置に初期値パーティクルを生成
  GenInitialParticles(0.0, 0.0, 0.0, pow(0.1, 2),
                      pow(30 * M_PI / 180.0, 2));
  ros::spin();
  delete g_filter;
}


#define BOOST_TEST_MODULE Simple
#include <boost/test/included/unit_test.hpp>
#include "../core/models/Alloymaker.h"
#include "../core/models/Standard.h"
#include "../core/optimization/Variable.h"
#include "../core/optimization/Solution.h"

using namespace std;

BOOST_AUTO_TEST_CASE(solve_test)
{
    Alloymaker a1;
    a1.setPrice(15633);
    a1.setLossPercentage(5);
    a1.getCompositions().get("C").setPercentage(0.2);
    a1.getCompositions().get("Si").setPercentage(0.6);
    Alloymaker a2;
    a2.setPrice(23134);
    a2.setLossPercentage(12);
    a2.getCompositions().get("C").setPercentage(90);
    Alloymaker a3;
    a3.setPrice(5665);
    a3.setLossPercentage(2);
    a3.getCompositions().get("Si").setPercentage(1.6);
    auto std1 = Standard();
    std1.setSymbols(vector<string>{"C"});
    std1.setMax(0.6);
    std1.setMin(0.4);
    auto std2 = Standard();
    std2.setSymbols(vector<string>{"Si"});
    std2.setMax(1.2);
    std2.setMin(1);
    auto v1 = Variable(a1);
    auto v2 = Variable(a2);
    auto v3 = Variable(a3);
    auto s = Solution(5000);
    auto vs = vector<Variable>{v1, v2, v3};
    auto stds = vector<Standard>{std1};
    s.setStandards(stds);
    s.autoSolve(vs, 500);
    ostringstream st;
    st << vs.at(0).getAnswer();
    BOOST_CHECK_EQUAL(st.str(), "0");
    st.str("");
    st << vs.at(1).getAnswer();
    BOOST_CHECK_EQUAL(st.str(), "2.52525");
    st.str("");
    st << vs.at(2).getAnswer();
    BOOST_CHECK_EQUAL(st.str(), "507.937");
}


#include <iostream>
#include <utility>
#include <vector>
#include <map>
#include <fstream>
#include <iterator>
#include <string>
#include <algorithm>
#include <boost/algorithm/string.hpp>

#define pb push_back
#define fi first
#define se second
using namespace std;

struct info {
    string Sex;
    string Age;
    string Female;
    info (string _Sex, string _Age, string _Female):Sex(_Sex), Age(_Age), Female(_Female){}
    info() = default;
    friend bool operator<(info &a, info &b);
    friend bool operator>(info &a, info &b);
    friend bool operator==(info &a, info &b);
    friend bool operator!=(info &a, info &b);
};

bool operator<(info &a, info &b) {
    return a.Age < b.Age;
}

bool operator>(info &a, info &b) {
    return a.Age < b.Age;
}

bool operator==(info &a, info &b) {
    return a.Age == b.Age;
}

bool operator!=(info &a, info &b) {
    return a.Age != b.Age;
}

struct Node {
    string data;
    info man;
    Node *left;
    Node *right;
    Node *parent;
};

class CSVReader
{
    std::string fileName;
    std::string delimeter;

public:
    CSVReader(std::string filename, std::string delm = ",") :
    fileName(filename), delimeter(delm)
    { }

    // Function to fetch data from a CSV File
    std::vector<std::vector<std::string> > getData();
};

/*
 * Parses through csv file line by line and returns the data
 * in vector of vector of strings.
 */
std::vector<std::vector<std::string> > CSVReader::getData()
{
    std::ifstream file(fileName);

    std::vector<std::vector<std::string> > dataList;

    std::string line = "";
    // Iterate through each line and split the content using delimeter
    while (getline(file, line))
    {
        std::vector<std::string> vec;
        boost::algorithm::split(vec, line, boost::is_any_of(delimeter));
        dataList.push_back(vec);
    }
    // Close the File
    file.close();

    return dataList;
}

class Hashing {
public:
    vector <Node*> a;
    long long m;
    Hashing(long long _m);
    long long Hash_func(string s);
    void insert(string x, info people);
    vector <info> find(string x);
    void deletes(string x);
};

Hashing::Hashing(long long _m) {
    m = _m;
    a.resize(m + 1);
}

long long Hashing::Hash_func(string s) {
    int p = 31;
    long long p_pow = 1, ans = 0;
    for (int i = 0; i < s.size(); i++) {
        ans += ((int)s[i])*p_pow;
        p_pow *= p;
        p_pow = p_pow % m;
        if (ans >= m) {
            ans -= m;
        }
    }
    return ans % m;
}

void Hashing::insert(string x, info people) {
    long long h = Hash_func(x);
    Node *root = a[h];
    if (root == nullptr) {
        Node *temp = new Node();
        temp -> data = x;
        temp -> man = people;
        temp -> right = nullptr;
        temp -> left = nullptr;
        temp -> parent = nullptr;
        a[h] = temp;
    }
    else {
        while (root != nullptr) {
            if (root -> data <= x) {
                if (root -> right == nullptr) {
                    Node *temp = new Node();;
                    temp -> data = x;
                    temp -> man = people;
                    temp -> right = nullptr;
                    temp -> left = nullptr;
                    temp -> parent = root;
                    root -> right = temp;
                    break;
                }
                else {
                    root = root -> right;
                }
            }
            else {
                if (root -> left == nullptr) {
                    Node *temp = new Node();;
                    temp -> data = x;
                    temp -> man = people;
                    temp -> right = nullptr;
                    temp -> left = nullptr;
                    temp -> parent = root;
                    root -> left = temp;
                    break;
                }
                else {
                    root = root -> left;
                }
            }
        }
    }
}

vector <info> Hashing::find (string x) {
    long long h = Hash_func(x);
    Node *root = a[h];
    vector <info> ans;
    while (root != nullptr) {
        if (root -> data < x) {
            root = root -> right;
        }
        else if (root -> data > x) {
            root = root -> left;
        }
        else {
            ans.pb(root -> man);
            root = root -> right;
        }
    }
    return ans;
}

void Hashing::deletes (string x) {
    long long h = Hash_func(x);
    Node *root = a[h];
    while (root != nullptr) {
        if (root -> data < x) {
            root = root -> right;
        }
        else if (root -> data > x) {
            root = root -> left;
        }
        else {
            if (root -> right == nullptr) {
                if (root -> parent -> left-> data == x) {
                    root -> parent -> left = root -> left;
                }
                else {
                    root -> parent -> right = root -> left;
                }
            }
            else if (root -> right != nullptr && root -> right -> left == nullptr) {
                if (root -> parent -> left -> data == x) {
                    root -> parent -> left = root -> right;
                }
                else {
                    root -> parent -> right = root -> right;
                }
            }
            else {
                if (root -> parent -> left-> data == x) {
                    root -> parent -> left = root -> right -> left;
                }
                else {
                    root -> parent -> right = root -> right -> left;
                }
            }
            delete root;
        }
    }
}
int main() {
    CSVReader reader("/Users/vladislavdevlikamov/Downloads/_ea07570741a3ec966e284208f588e50e_titanic (1).csv");
    std::vector<std::vector<std::string> > dataList = reader.getData();
    Hashing t(1024);
    for (std::vector <std::string> vec : dataList) {
        string name;
        for (int i = 0; i < vec[3].size(); i++) {
            if (vec[3][i] >= 'a' && vec[3][i] <= 'z') name += vec[3][i];
            else if (vec[3][i] >= 'A' && vec[3][i] <= 'Z') name += vec[3][i];
        }
        string female;
        for (int i = 0; i < vec[4].size(); i++) {
            if (vec[4][i] >= 'a' && vec[4][i] <= 'z') female += vec[4][i];
            else if (vec[4][i] >= 'A' && vec[4][i] <= 'Z') female += vec[4][i];
            else if (vec[4][i] == '.') female += vec[4][i];
        }
        info mans(vec[5], vec[6], female);
        t.insert(name, mans);
    }
    cout << "Write name: " << "\n";
    string name;
    cin >> name;
    vector <info> temp = t.find(name);
    if (temp.size() == 0) {
        cout << "Not found" << "\n";
    }
    for (int i = 0; i < temp.size(); i++) {
          cout << name << " " << temp[i].Female << " " << temp[i].Sex << " " << temp[i].Age << "\n";
    }
    t.deletes(name);
    vector <info> temp1 = t.find(name);
    if (temp1.size() == 0) {
        cout << "Not found" << "\n";
    }
    for (int i = 0; i < temp1.size(); i++) {
          cout << name << " " << temp1[i].Female << " " << temp1[i].Sex << " " << temp1[i].Age << "\n";
    }
    return 0;
}


#include <iostream>
#include <cmath>
#include <algorithm>
#include <vector>
#include <boost/multiprecision/cpp_int.hpp>
using namespace boost::multiprecision;
using namespace std;
int main() {
    cpp_int n;
    long long int k; cin >> n >> k;
    for (int i = 0; i < k; i++) {
        if (n % 200 == 0) n /= 200;
        else n *= 1000, n += 200;
    }
    cout << n << endl;
}


/*
    @file groundplanfit.cpp
    @brief ROS Node for ground plane fitting

    This is a ROS node to perform ground plan fitting.
    Implementation accoriding to <Fast Segmentation of 3D Point Clouds: A Paradigm>

    In this case, it's assumed that the x,y axis points at sea-level,
    and z-axis points up. The sort of height is based on the Z-axis value.

    @author Vincent Cheung(VincentCheungm)
    @bug Sometimes the plane is not fit.
*/

#include <iostream>
// For disable PCL complile lib, to use PointXYZIR
#define PCL_NO_PRECOMPILE

#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>

#include <pcl_ros/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/filters/filter.h>
#include <pcl/point_types.h>
#include <pcl/common/centroid.h>

#include <velodyne_pointcloud/point_types.h>

//Customed Point Struct for holding clustered points
namespace scan_line_run
{
  /** Euclidean Velodyne coordinate, including intensity and ring number, and label. */
  struct PointXYZIRL
  {
    PCL_ADD_POINT4D;                    // quad-word XYZ
    float    intensity;                 ///< laser intensity reading
    uint16_t ring;                      ///< laser ring number
    uint16_t label;                     ///< point label
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW     // ensure proper alignment
  } EIGEN_ALIGN16;

}; // namespace scan_line_run

#define SLRPointXYZIRL scan_line_run::PointXYZIRL
#define VPoint velodyne_pointcloud::PointXYZIR
#define RUN pcl::PointCloud<SLRPointXYZIRL>
// Register custom point struct according to PCL
POINT_CLOUD_REGISTER_POINT_STRUCT(scan_line_run::PointXYZIRL,
                                  (float, x, x)
                                  (float, y, y)
                                  (float, z, z)
                                  (float, intensity, intensity)
                                  (uint16_t, ring, ring)
                                  (uint16_t, label, label))

// using eigen lib
#include <Eigen/Dense>
using Eigen::MatrixXf;
using Eigen::JacobiSVD;
using Eigen::VectorXf;

pcl::PointCloud<VPoint>::Ptr g_seeds_pc(new pcl::PointCloud<VPoint>());
pcl::PointCloud<VPoint>::Ptr g_ground_pc(new pcl::PointCloud<VPoint>());
pcl::PointCloud<VPoint>::Ptr g_not_ground_pc(new pcl::PointCloud<VPoint>());
pcl::PointCloud<SLRPointXYZIRL>::Ptr g_all_pc(new pcl::PointCloud<SLRPointXYZIRL>());

/*
    @brief Compare function to sort points. Here use z axis.
    @return z-axis accent
*/
bool point_cmp(VPoint a, VPoint b){
    return a.z<b.z;
}

/*
    @brief Ground Plane fitting ROS Node.
    @param Velodyne Pointcloud topic.
    @param Sensor Model.
    @param Sensor height for filtering error mirror points.
    @param Num of segment, iteration, LPR
    @param Threshold of seeds distance, and ground plane distance

    @subscirbe:/velodyne_points
    @publish:/points_no_ground, /points_ground
*/
class GroundPlaneFit{
public:
    GroundPlaneFit();
private:
    ros::NodeHandle node_handle_;
    ros::Subscriber points_node_sub_;
    ros::Publisher ground_points_pub_;
    ros::Publisher groundless_points_pub_;
    ros::Publisher all_points_pub_;

    std::string point_topic_;

    int sensor_model_;
    double sensor_height_;
    int num_seg_;
    int num_iter_;
    int num_lpr_;
    double th_seeds_;
    double th_dist_;

    void velodyne_callback_(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg);
    void estimate_plane_(void);
    void extract_initial_seeds_(const pcl::PointCloud<VPoint>& p_sorted);

    // Model parameter for ground plane fitting
    // The ground plane model is: ax+by+cz+d=0
    // Here normal:=[a,b,c], d=d
    // th_dist_d_ = threshold_dist - d
    float d_;
    MatrixXf normal_;
    float th_dist_d_;
};

/*
    @brief Constructor of GPF Node.
    @return void
*/
GroundPlaneFit::GroundPlaneFit():node_handle_("~"){
    // Init ROS related
    ROS_INFO("Inititalizing Ground Plane Fitter...");
    node_handle_.param<std::string>("point_topic", point_topic_, "/velodyne_points"); //"/merged_velodyne" "/kitti/velo/pointcloud"
    ROS_INFO("Input Point Cloud: %s", point_topic_.c_str());

    node_handle_.param("sensor_model", sensor_model_, 64);
    ROS_INFO("Sensor Model: %d", sensor_model_);

    node_handle_.param("sensor_height", sensor_height_, 2.5);
    ROS_INFO("Sensor Height: %f", sensor_height_);

    node_handle_.param("num_seg", num_seg_, 1);
    ROS_INFO("Num of Segments: %d", num_seg_);

    node_handle_.param("num_iter", num_iter_, 3);
    ROS_INFO("Num of Iteration: %d", num_iter_);

    node_handle_.param("num_lpr", num_lpr_, 20);
    ROS_INFO("Num of LPR: %d", num_lpr_);

    node_handle_.param("th_seeds", th_seeds_, 1.2);
    ROS_INFO("Seeds Threshold: %f", th_seeds_);

    node_handle_.param("th_dist", th_dist_, 0.3);
    ROS_INFO("Distance Threshold: %f", th_dist_);

    // Listen to velodyne topic
    points_node_sub_ = node_handle_.subscribe(point_topic_, 2, &GroundPlaneFit::velodyne_callback_, this);

    // Publish Init
    std::string no_ground_topic, ground_topic;
    node_handle_.param<std::string>("no_ground_point_topic", no_ground_topic, "/points_no_ground");
    ROS_INFO("No Ground Output Point Cloud: %s", no_ground_topic.c_str());
    node_handle_.param<std::string>("ground_point_topic", ground_topic, "/points_ground");
    ROS_INFO("Only Ground Output Point Cloud: %s", ground_topic.c_str());
    groundless_points_pub_ = node_handle_.advertise<sensor_msgs::PointCloud2>(no_ground_topic, 2);
    ground_points_pub_ = node_handle_.advertise<sensor_msgs::PointCloud2>(ground_topic, 2);
    all_points_pub_ =  node_handle_.advertise<sensor_msgs::PointCloud2>("/all_points", 2);
}

/*
    @brief The function to estimate plane model. The
    model parameter `normal_` and `d_`, and `th_dist_d_`
    is set here.
    The main step is performed SVD(UAV) on covariance matrix.
    Taking the sigular vector in U matrix according to the smallest
    sigular value in A, as the `normal_`. `d_` is then calculated
    according to mean ground points.

    @param g_ground_pc:global ground pointcloud ptr.

*/
void GroundPlaneFit::estimate_plane_(void){
    // Create covarian matrix in single pass.
    // TODO: compare the efficiency.
    Eigen::Matrix3f cov;
    Eigen::Vector4f pc_mean;
    pcl::computeMeanAndCovarianceMatrix(*g_ground_pc, cov, pc_mean);
    // Singular Value Decomposition: SVD
    JacobiSVD<MatrixXf> svd(cov,Eigen::DecompositionOptions::ComputeFullU);
    // use the least singular vector as normal
    normal_ = (svd.matrixU().col(2));
    // mean ground seeds value
    Eigen::Vector3f seeds_mean = pc_mean.head<3>();

    // according to normal.T*[x,y,z] = -d
    d_ = -(normal_.transpose()*seeds_mean)(0,0);
    // set distance threhold to `th_dist - d`
    th_dist_d_ = th_dist_ - d_;

    // return the equation parameters
}

/*
    @brief Extract initial seeds of the given pointcloud sorted segment.
    This function filter ground seeds points accoring to heigt.
    This function will set the `g_ground_pc` to `g_seed_pc`.
    @param p_sorted: sorted pointcloud

    @param ::num_lpr_: num of LPR points
    @param ::th_seeds_: threshold distance of seeds
    @param ::

*/
void GroundPlaneFit::extract_initial_seeds_(const pcl::PointCloud<VPoint>& p_sorted){
    // LPR is the mean of low point representative
    double sum = 0;
    int cnt = 0;
    // Calculate the mean height value.
    for(int i=0;i<p_sorted.points.size() && cnt<num_lpr_;i++){
        sum += p_sorted.points[i].z;
        cnt++;
    }
    double lpr_height = cnt!=0?sum/cnt:0;// in case divide by 0
    g_seeds_pc->clear();
    // iterate pointcloud, filter those height is less than lpr.height+th_seeds_
    for(int i=0;i<p_sorted.points.size();i++){
        if(p_sorted.points[i].z < lpr_height + th_seeds_){
            g_seeds_pc->points.push_back(p_sorted.points[i]);
        }
    }
    // return seeds points
}

/*
    @brief Velodyne pointcloud callback function. The main GPF pipeline is here.
    PointCloud SensorMsg -> Pointcloud -> z-value sorted Pointcloud
    ->error points removal -> extract ground seeds -> ground plane fit mainloop
*/
void GroundPlaneFit::velodyne_callback_(const sensor_msgs::PointCloud2ConstPtr& in_cloud_msg){
    // 1.Msg to pointcloud
    pcl::PointCloud<VPoint> laserCloudIn;
    pcl::fromROSMsg(*in_cloud_msg, laserCloudIn);
    pcl::PointCloud<VPoint> laserCloudIn_org;
    pcl::fromROSMsg(*in_cloud_msg, laserCloudIn_org);
    // For mark ground points and hold all points
    SLRPointXYZIRL point;
    for(size_t i=0;i<laserCloudIn.points.size();i++){
        point.x = laserCloudIn.points[i].x;
        point.y = laserCloudIn.points[i].y;
        point.z = laserCloudIn.points[i].z;
        point.intensity = laserCloudIn.points[i].intensity;
        point.ring = laserCloudIn.points[i].ring;
        point.label = 0u;// 0 means uncluster
        g_all_pc->points.push_back(point);
    }
    //std::vector<int> indices;
    //pcl::removeNaNFromPointCloud(laserCloudIn, laserCloudIn,indices);
    // 2.Sort on Z-axis value.
    sort(laserCloudIn.points.begin(),laserCloudIn.points.end(),point_cmp);
    // 3.Error point removal
    // As there are some error mirror reflection under the ground,
    // here regardless point under 2* sensor_height
    // Sort point according to height, here uses z-axis in default
    pcl::PointCloud<VPoint>::iterator it = laserCloudIn.points.begin();
    for(int i=0;i<laserCloudIn.points.size();i++){
        if(laserCloudIn.points[i].z < -1.5*sensor_height_){
            it++;
        }else{
            break;
        }
    }
    laserCloudIn.points.erase(laserCloudIn.points.begin(),it);
    // 4. Extract init ground seeds.
    extract_initial_seeds_(laserCloudIn);
    g_ground_pc = g_seeds_pc;

    // 5. Ground plane fitter mainloop
    for(int i=0;i<num_iter_;i++){
        estimate_plane_();
        g_ground_pc->clear();
        g_not_ground_pc->clear();

        //pointcloud to matrix
        MatrixXf points(laserCloudIn_org.points.size(),3);
        int j =0;
        for(auto p:laserCloudIn_org.points){
            points.row(j++)<<p.x,p.y,p.z;
        }
        // ground plane model
        VectorXf result = points*normal_;
        // threshold filter
        for(int r=0;r<result.rows();r++){
            if(result[r]<th_dist_d_){
                g_all_pc->points[r].label = 1u;// means ground
                g_ground_pc->points.push_back(laserCloudIn_org[r]);
            }else{
                g_all_pc->points[r].label = 0u;// means not ground and non clusterred
                g_not_ground_pc->points.push_back(laserCloudIn_org[r]);
            }
        }
    }

    // publish ground points
    sensor_msgs::PointCloud2 ground_msg;
    pcl::toROSMsg(*g_ground_pc, ground_msg);
    ground_msg.header.stamp = in_cloud_msg->header.stamp;
    ground_msg.header.frame_id = in_cloud_msg->header.frame_id;
    ground_points_pub_.publish(ground_msg);
    // publish not ground points
    sensor_msgs::PointCloud2 groundless_msg;
    pcl::toROSMsg(*g_not_ground_pc, groundless_msg);
    groundless_msg.header.stamp = in_cloud_msg->header.stamp;
    groundless_msg.header.frame_id = in_cloud_msg->header.frame_id;
    groundless_points_pub_.publish(groundless_msg);
    // publish all points
    sensor_msgs::PointCloud2 all_points_msg;
    pcl::toROSMsg(*g_all_pc, all_points_msg);
    all_points_msg.header.stamp = in_cloud_msg->header.stamp;
    all_points_msg.header.frame_id = in_cloud_msg->header.frame_id;
    all_points_pub_.publish(all_points_msg);
    g_all_pc->clear();
}

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

    ros::init(argc, argv, "GroundPlaneFit");
    GroundPlaneFit node;
    ros::spin();

    return 0;

}


#include <fstream>
#include <iostream>
#include <omp.h>
#include <cmath>
#include <cstdio>
#include <chrono>
#include <string>
#include <armadillo>
#include <vector>
#include "wigner/gaunt.hpp"

using namespace std::chrono;
using namespace arma;

static const double kB = 1.3806504e-23;         // J/K
static const double NA = 6.02214179e23;         // 1/mol
static const double EHARTREE = 4.35974434e-18;  // J/Hartree
static const double AMU = 1.660538921e-27;      // kg/amu
static const double HBAR = 1.054571726e-34;     // J.s
static const double HBAR1 = HBAR / EHARTREE;    // Hartree.s
static const double HBAR2 = HBAR * 1e20 / AMU;  // amu.Å^2/s
static const double SCH4 = 186.25; // J/mol.K

std::string getDirectory(std::string sysname="METH-CHA") {
    std::string dirname = "/Users/lancebettinson/Thesis/umrr/code/hamiltonian-cpp/data";
    if (sysname == "") {
        std::string sysname;
        std::cout << "Enter system name:" << std::endl;
        std::cin >> sysname;
    }
    return dirname+'/'+sysname;
}

Col<double> getCoefficients() {
    std::string dirname = getDirectory();
    std::string filename = dirname+'/'+"vdat.txt";
    std::ifstream is(filename);
    if (is.fail())
    {
        std::cout << "cannot open file " << filename;
    }
    double theta, phi, v;
    std::vector<double> my_vec;
    while (is) {
        if (!(is >> theta >> phi >> v)) {
            break;
        }
        //std::cout << theta << '\t' << phi << '\t' << v << std::endl;
        my_vec.push_back(v);
    }
    Col<double> cvec = conv_to<vec>::from(my_vec);
    //cvec.print();
    is.close();
    return cvec;
}

std::vector<double> getMomentOfInertia() {
    std::string dirname = getDirectory();
    std::string filename = dirname+'/'+"I.txt";
    std::ifstream is(filename);
    double val;
    std::vector<double> Ivec;
    while (is) {
        if (!(is >> val)) {
            break;
        }
        Ivec.push_back(val);
    }
    return Ivec;
}

double getReducedI(std::vector<double> Ivec) {
    double result = 0;
    for (double i : Ivec) {
        result += pow(i, -1);
    }
    return pow(result, -1);
}

double getPartitionFunction(double T, Mat<double>& H) {
    /* Solve the Eigenvalues
     * Inputs:  T;  the temperature [=] K
     *          H;  the Hamiltonian matrix [=] Hartree
     */
    Col<double> eigval = eig_sym(H);
    double Q = 0;
    double b = pow(kB * T, -1) * EHARTREE;
    //b = 1;
    int count = 0;
    for (double e : eigval) {
        Q += exp(-b * e);
    }
    for (double e : eigval) {
        if (count == 5) {
            //std::cout << std::endl;
        } //std::cout << exp(-b*e)/Q << '\t';
        count++;
    }
    std::cout << std::endl << "Q predicted by eig_sym: " << Q << std::endl;
    Mat<double> bH = -b*H;
    Mat<double> expbH = expmat_sym(bH);
    double tr = trace(expbH);
    std::cout << pow(b,-1) << std::endl;
    return tr;
    //return Q;
}

Mat<double> getHamiltonian(int lmax, double I, Col<double>& ahat) {
    /* Construct the Hamiltonian Matrix
     * Inputs:  lmax;   the maximum quantum number for the spherical basis
     *             I;   the reduced gas-phase moment of inertia [=] amu*Å^2
     *          ahat;   the number of fitting coefficients to the potential [=] Hartree
     * Outputs:    H;   the Hamiltonian matrix (Hermitian) [=] Hartree
     */
    int Lmax = sqrt(ahat.size());
    Mat<double> H = zeros<mat>(lmax*lmax, lmax*lmax);
    // Define rotational constants:
    double A = HBAR1*HBAR2/(2.0*I);
    //int count = 0;
    #pragma omp parallel
    {
    double g;
    double a;
    #pragma omp for
    for (int l = 0; l < lmax; l++) {
        for (int m = -l; m < l+1; m++) {
            int j = l*l + l + m;
            for (int ll = 0; ll < l+1; ll++) {
                for (int mm = -ll; mm < ll+1; mm++) {
                    int i = ll*ll + ll + mm;
                    if (j < i) {
                        continue;
                    }
                    for (int K = -ll; K < ll+1; K++) {
                    for (int L = 0; L < Lmax; L++) {
                        for (int M = -L; M < L+1; M++) {
                            int k = L*L + L + M;
                            //int hello = omp_get_thread_num();
                            g = wigner::gaunt<double>(l,ll,L,m,mm,M);
                            a = ahat(k);
                            //H(i,j) += a*g;
                            //H(j,i) += a*g;
                        }
                    }
                    if (ll == l && mm == m) {
                        H(i,j) /= 2.0;
                        // Account for projection of l onto body-fixed axis
                        H(i,j) += HBAR1*HBAR2 * l*(l+1.0) /(2.0*I);
                    }// else if (ll == l) {
                     //   A(i,j) += HBAR1*HBAR2 * l*(l+1.0) /(2.0*I);
                    //}
                    //#pragma omp atomic
                    //count++;
                    }
                }
            }
        }
    }
    } // end parallel
    //std::cout << "Iterated over " << count << " entries." << std::endl;
    //A.print("A = ");
    return H;
}

int main() {
    auto start = high_resolution_clock::now();
    std::vector<double> Ivec = getMomentOfInertia();
    double I = getReducedI(Ivec);
    std::cout << "Printing I:" << std::endl;
    for (double i : Ivec) {
        std::cout << i << '\t';
    }
    // Let I = 9
    //
    //I = 10;
    I = 3.2;
    std::cout << std::endl << "Reduced I: " << I << std::endl;
    std::cout << "Rotational Constant: = " << HBAR1*HBAR2/2.0/I << std::endl;
    std::cout << "kT = " << kB*300 / EHARTREE << std::endl;
    std::cout << "Ratio = " << HBAR1*HBAR2/2.0/I *EHARTREE/(kB*300) << std::endl;
    std::cout << "Qapprox = " << kB*300/EHARTREE / (HBAR1*HBAR2/2.0/I) << std::endl;

    Col<double> ahat = getCoefficients();
    //Col<double> ahat = ones<vec>(24*24);
    //double I = 1.0;
    std::string dirname = getDirectory();
    std::cout << "Directory is: " << dirname << std::endl;

    Mat<double> H = getHamiltonian(20, I, ahat);
    //H.print();
    //Mat<double> H = getHamiltonian(10, I, ahat);
    double Q = getPartitionFunction(300, H);

    std::cout << std::endl;
    std::cout << "Q = " << Q << std::endl;
    std::cout << std::endl;
    auto stop = high_resolution_clock::now();
    auto duration = duration_cast<microseconds>(stop - start);
    std::cout << std::endl << duration.count()/1e6 << " secs" << std::endl;
    return 0;
}


// Copyright Louis Dionne 2013-2017
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)

#include "minimal.hpp"

#include <boost/hana/and.hpp>
#include <boost/hana/equal.hpp>
#include <boost/hana/eval_if.hpp>
#include <boost/hana/if.hpp>
#include <boost/hana/not.hpp>
#include <boost/hana/not_equal.hpp>
#include <boost/hana/or.hpp>
#include <boost/hana/tuple.hpp>
#include <boost/hana/while.hpp>

#include <laws/base.hpp>
#include <laws/logical.hpp>
namespace hana = boost::hana;

struct invalid { };

int main() {
    constexpr auto bools = hana::make_tuple(
        minimal_constant<bool, false>{},
        minimal_constant<bool, true>{}
    );

    hana::test::TestLogical<minimal_constant_tag<bool>>{bools};

    constexpr auto true_ = minimal_constant<bool, true>{};
    constexpr auto false_ = minimal_constant<bool, false>{};

    // not_
    {
        BOOST_HANA_CONSTANT_CHECK(hana::equal(hana::not_(true_), false_));
        BOOST_HANA_CONSTANT_CHECK(hana::equal(hana::not_(false_), true_));
    }

    // and_
    {
        BOOST_HANA_CONSTANT_CHECK(hana::equal(
            hana::and_(true_),
            true_
        ));
        BOOST_HANA_CONSTANT_CHECK(hana::equal(
            hana::and_(false_),
            false_
        ));

        BOOST_HANA_CONSTANT_CHECK(hana::equal(
            hana::and_(true_, true_),
            true_
        ));
        BOOST_HANA_CONSTANT_CHECK(hana::equal(
            hana::and_(true_, false_),
            false_
        ));
        BOOST_HANA_CONSTANT_CHECK(hana::equal(
            hana::and_(false_, invalid{}),
            false_
        ));

        BOOST_HANA_CONSTANT_CHECK(hana::equal(
            hana::and_(true_, true_, true_),
            true_
        ));
        BOOST_HANA_CONSTANT_CHECK(hana::equal(
            hana::and_(true_, true_, false_),
            false_
        ));
        BOOST_HANA_CONSTANT_CHECK(hana::equal(
            hana::and_(true_, false_, invalid{}),
            false_
        ));
        BOOST_HANA_CONSTANT_CHECK(hana::equal(
            hana::and_(false_, invalid{}, invalid{}),
            false_
        ));
    }

    // or_
    {
        BOOST_HANA_CONSTANT_CHECK(hana::equal(
            hana::or_(true_),
            true_
        ));
        BOOST_HANA_CONSTANT_CHECK(hana::equal(
            hana::or_(false_),
            false_
        ));

        BOOST_HANA_CONSTANT_CHECK(hana::equal(
            hana::or_(false_, false_),
            false_
        ));
        BOOST_HANA_CONSTANT_CHECK(hana::equal(
            hana::or_(false_, true_),
            true_
        ));
        BOOST_HANA_CONSTANT_CHECK(hana::equal(
            hana::or_(true_, invalid{}),
            true_
        ));

        BOOST_HANA_CONSTANT_CHECK(hana::equal(
            hana::or_(false_, false_, false_),
            false_
        ));
        BOOST_HANA_CONSTANT_CHECK(hana::equal(
            hana::or_(false_, false_, true_),
            true_
        ));
        BOOST_HANA_CONSTANT_CHECK(hana::equal(
            hana::or_(false_, true_, invalid{}),
            true_
        ));
        BOOST_HANA_CONSTANT_CHECK(hana::equal(
            hana::or_(true_, invalid{}, invalid{}),
            true_
        ));
    }

    // if_
    {
        BOOST_HANA_CONSTANT_CHECK(hana::equal(
            hana::if_(true_, hana::test::ct_eq<3>{}, hana::test::ct_eq<4>{}),
            hana::test::ct_eq<3>{}
        ));

        BOOST_HANA_CONSTANT_CHECK(hana::equal(
            hana::if_(false_, hana::test::ct_eq<3>{}, hana::test::ct_eq<4>{}),
            hana::test::ct_eq<4>{}
        ));
    }

    // eval_if
    {
        auto t = [](auto) { return hana::test::ct_eq<2>{}; };
        auto e = [](auto) { return hana::test::ct_eq<3>{}; };

        BOOST_HANA_CONSTANT_CHECK(hana::equal(
            hana::eval_if(true_, t, invalid{}),
            hana::test::ct_eq<2>{}
        ));

        BOOST_HANA_CONSTANT_CHECK(hana::equal(
            hana::eval_if(false_, invalid{}, e),
            hana::test::ct_eq<3>{}
        ));
    }

    // while_
    {
        hana::test::_injection<0> f{};

        BOOST_HANA_CONSTANT_CHECK(hana::equal(
            hana::while_(hana::not_equal.to(hana::test::ct_eq<0>{}), hana::test::ct_eq<0>{}, invalid{}),
            hana::test::ct_eq<0>{}
        ));

        BOOST_HANA_CONSTANT_CHECK(hana::equal(
            hana::while_(hana::not_equal.to(f(hana::test::ct_eq<0>{})), hana::test::ct_eq<0>{}, f),
            f(hana::test::ct_eq<0>{})
        ));

        BOOST_HANA_CONSTANT_CHECK(hana::equal(
            hana::while_(hana::not_equal.to(f(f(hana::test::ct_eq<0>{}))), hana::test::ct_eq<0>{}, f),
            f(f(hana::test::ct_eq<0>{}))
        ));

        BOOST_HANA_CONSTANT_CHECK(hana::equal(
            hana::while_(hana::not_equal.to(f(f(f(hana::test::ct_eq<0>{})))), hana::test::ct_eq<0>{}, f),
            f(f(f(hana::test::ct_eq<0>{})))
        ));

        BOOST_HANA_CONSTANT_CHECK(hana::equal(
            hana::while_(hana::not_equal.to(f(f(f(f(hana::test::ct_eq<0>{}))))), hana::test::ct_eq<0>{}, f),
            f(f(f(f(hana::test::ct_eq<0>{}))))
        ));

        // Make sure it can be called with an lvalue state:
        auto state = hana::test::ct_eq<0>{};
        BOOST_HANA_CONSTANT_CHECK(hana::equal(
            hana::while_(hana::not_equal.to(f(f(f(f(hana::test::ct_eq<0>{}))))), state, f),
            f(f(f(f(hana::test::ct_eq<0>{}))))
        ));
    }
}