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

#ifndef NQS_ABSTRACTHILBERT_HPP
#define NQS_ABSTRACTHILBERT_HPP

#include <Eigen/Core>
#include <complex>
#include <limits>
#include <nonstd/optional.hpp>
#include <vector>

#include "Graph/abstract_graph.hpp"
#include "Hilbert/hilbert_index.hpp"
#include "Utils/random_utils.hpp"

namespace nqs {

/**
  Abstract class for Hilbert spaces.
  This class prototypes the methods needed
  by a class satisfying the Hilbert concept.
*/
class AbstractHilbert {
 public:
  /**
  Member function returning true if the hilbert space has discrete quantum
  numbers.
  @return true if the local hilbert space is discrete
  */
  virtual bool IsDiscrete() const = 0;

  /**
   * We call a Hilbert space indexable if and only if the total Hilbert space
   * dimension can be represented by an index of type int.
   * @return true, iff the Hilbert space is indexable
   */
  bool IsIndexable() const {
    static constexpr int max_states = std::numeric_limits<int>::max() - 1;
    static const double log_max = std::log(max_states);

    return Size() * std::log(LocalSize()) <= log_max;
  }

  const HilbertIndex &GetIndex() const {
    if (!index_.has_value()) {
      if (!IsIndexable()) {
        throw std::runtime_error("Hilbert space is too large to be indexed");
      }
      index_.emplace(LocalStates(), LocalSize(), Size());
    }
    return *index_;
  }

  /**
  Member function returning the size of the local hilbert space.
  @return Size of the discrete local hilbert space. For continous spaces an
  error message is returned.
  */
  virtual int LocalSize() const = 0;

  /**
  Member function returning the number of visible units needed to describe the
  system.
  @return Number of visible units needed to described the system.
  */
  virtual int Size() const = 0;

  /**
  Member function returning the local states.
  @return Vector containing the value of the discrete local quantum numbers. If
  the local quantum numbers are continous, the vector contains lower and higher
  bounds for the local quantum numbers.
  */
  virtual std::vector<double> LocalStates() const = 0;

  /**
  Member function generating uniformely distributed local random states
  @param state a reference to a visible configuration, in output this contains
  the random state.
  @param rgen the random number generator to be used
  */
  virtual void RandomVals(Eigen::Ref<Eigen::VectorXd> state,
                          nqs::default_random_engine &rgen) const = 0;

  /**
  Member function updating a visible configuration using the information on
  where the local changes have been done.
  @param v is the vector visible units to be modified.
  @param tochange contains a list of which quantum numbers are to be modified.
  @param newconf contains the value that those quantum numbers should take
  */
  virtual void UpdateConf(Eigen::Ref<Eigen::VectorXd> v,
                          nonstd::span<const int> tochange,
                          nonstd::span<const double> newconf) const = 0;

  virtual const AbstractGraph &GetGraph() const noexcept = 0;

  // Allow range-based for over all states in the Hilbert space, iff it is
  // indexable
  StateGenerator begin() const { return StateGenerator(GetIndex()); };
  StateGenerator end() const { return StateGenerator(GetIndex()); };

  virtual ~AbstractHilbert() = default;

 private:
  mutable nonstd::optional<HilbertIndex> index_;
};

}  // namespace nqs

#endif


#include "testsuite.h"
#include <blitz/array.h>

BZ_USING_NAMESPACE(blitz)

int main( void)
{
  Array<float,3> A(1,10,10);
  Array<float,2> B(10,10);

  A = 0.0f;
  B = 0.0f;

  beginCheckAssert();
  A(1, Range::all(), Range::all()) = B;  // first index should have been 0!
  endCheckAssert();

  beginCheckAssert();
  A(1, Range::all(), Range::all()) = B;  // not okay: shape error
  endCheckAssert();

  beginCheckAssert();
  A(Range::all(), 10, Range::all()) = 0;  // not okay: second index out of range
  endCheckAssert();

  A(0, Range::all(), Range::all()) = B;  // okay

  exit(0);
}


#include <boost/icl/split_interval_set.hpp>


#include "ScanImageTo3D.h"
#include "RegardRGBDMainWindow.h"

#include <iostream>
#include <sstream>
#include <limits>

#include <Eigen/LU>
#include <Eigen/Geometry>

#include <open3d/pipelines/registration/GlobalOptimization.h>
#include <open3d/pipelines/color_map/ColorMapOptimization.h>

ScanImageTo3D::ScanImageTo3D()
{
}

ScanImageTo3D::~ScanImageTo3D()
{

}

void ScanImageTo3D::setup()
{
}

void ScanImageTo3D::addNewImage(const open3d::geometry::Image& colorImg, const open3d::geometry::Image& depthImg)
{
	//open3d::pipelines::integration::ScalableTSDFVolume volume(4.0 / 512, 0.04, open3d::pipelines::integration::TSDFVolumeColorType::RGB8);

	open3d::camera::PinholeCameraIntrinsic intrinsic = open3d::camera::PinholeCameraIntrinsic(
		open3d::camera::PinholeCameraIntrinsicParameters::PrimeSenseDefault);
	//intrinsic.SetIntrinsics(640, 480, 524.0, 524.0, 316.7, 238.5);	// from https://www.researchgate.net/figure/ntrinsic-parameters-of-Kinect-RGB-camera_tbl2_305108995
	//intrinsic.SetIntrinsics(640, 480, 517.3, 516.5, 318.6, 255.3);		// from Freiburg test data set
	//intrinsic.SetIntrinsics(640, 480, 537.408, 537.40877, 321.897, 236.29);		// from Calibration of my own camera
	//intrinsic.SetIntrinsics(640, 480, 533.82, 533.82, 320.55, 232.35);		// from Calibration of my own camera
	intrinsic.SetIntrinsics(640, 480, 542.7693, 544.396, 318.79, 239.99);		// from Calibration of my own camera

	//open3d::io::WriteImageToPNG("color.png", colorImg_);
	//open3d::io::WriteImageToPNG("depth.png", depthImg_);

	Eigen::Matrix4d identity = Eigen::Matrix4d::Identity();
	auto depthFlt = depthImg.ConvertDepthToFloatImage(depthScale_, std::numeric_limits<float>::max());

	open3d::geometry::RGBDImage source(colorImg, *depthFlt);

	//volume.Integrate(source, intrinsic, identity);
	{
		std::unique_lock<std::mutex> lock(mutex_);
		//triangleMesh_ = volume.ExtractTriangleMesh();
		pointCloud_ = open3d::geometry::PointCloud::CreateFromRGBDImage(source, intrinsic);
	}

	if(pRegardRGBDMainWindow_ != nullptr)
		pRegardRGBDMainWindow_->update3DScanMesh();
}

void ScanImageTo3D::saveVolume()
{
}

void ScanImageTo3D::reset()
{
	std::unique_lock<std::mutex> lock(mutex_);
	setup();
}

/**
 * Makes a copy of the triangle mesh.
 */
const std::shared_ptr<open3d::geometry::TriangleMesh> ScanImageTo3D::getTriangleMesh()
{
	std::unique_lock<std::mutex> lock(mutex_);
	return std::make_shared<open3d::geometry::TriangleMesh>(*triangleMesh_);
}

/**
 * Makes a copy of the point cloud.
 */
const std::shared_ptr<open3d::geometry::PointCloud> ScanImageTo3D::getPointCloud()
{
	std::unique_lock<std::mutex> lock(mutex_);
	return std::make_shared<open3d::geometry::PointCloud>(*pointCloud_);
}


#include <fstream>
#include <fmt/format.h>
#include <string>
#include <boost/algorithm/string.hpp>
#include <utility>
#include <map>
#include <limits>
namespace algo = boost::algorithm;

struct AllString
{
private:
  std::vector<std::string> vs_;
  std::map<std::string, std::string, std::greater<>> kv_;
  int mincount_ = std::numeric_limits<int>::max();
  int steps_ = 0;

public:
  void push(std::pair<std::string, std::string> p)
  {
    kv_.insert(std::move(p));
  }
  void count(const std::string &source) noexcept
  {
    for (auto &&kv : kv_) {

      typedef std::pair<std::string::const_iterator, std::string::const_iterator> Pair;

      std::vector<Pair> vs;
      boost::algorithm::find_all(vs, source, kv.first);
      if (vs.size()) {
        ++steps_;
      }
      if (steps_ < mincount_) {
        for (int i = 0; i < static_cast<int>(vs.size()); ++i) {
          std::string sc = algo::replace_nth_copy(source, kv.first, i, kv.second);

          if (sc == "e") {
            mincount_ = std::min(steps_, mincount_);
            break;
          } else {
            count(sc);
          }
        }
      }

      if (vs.size()) {
        --steps_;
      }
    }
  }
  constexpr int minsteps() const noexcept
  {
    return mincount_;
  }
};

int main(int argc, char **argv)
{
  if (argc > 1) {
    std::string ignore;
    std::ifstream ifs(argv[1]);
    std::pair<std::string, std::string> p;

    AllString as;
    while (ifs >> p.second >> ignore >> p.first) {
      as.push(std::move(p));
    }
    std::string s_ = "CRnSiRnCaPTiMgYCaPTiRnFArSiThFArCaSiThSiThPBCaCaSiRnSiRnTiTiMgArPBCaPMgYPTiRnFArFArCaSiRnBPMgArPRnCaPTiRnFArCaSiThCaCaFArPBCaCaPTiTiRnFArCaSiRnSiAlYSiThRnFArArCaSiRnBFArCaCaSiRnSiThCaCaCaFYCaPTiBCaSiThCaSiThPMgArSiRnCaPBFYCaCaFArCaCaCaCaSiThCaSiRnPRnFArPBSiThPRnFArSiRnMgArCaFYFArCaSiRnSiAlArTiTiTiTiTiTiTiRnPMgArPTiTiTiBSiRnSiAlArTiTiRnPMgArCaFYBPBPTiRnSiRnMgArSiThCaFArCaSiThFArPRnFArCaSiRnTiBSiThSiRnSiAlYCaFArPRnFArSiThCaFArCaCaSiThCaCaCaSiRnPRnCaFArFYPMgArCaPBCaPBSiRnFYPBCaFArCaSiAl";
    as.count(s_);
    fmt::print("{}\n", as.minsteps());
  }
}


//=======================================================================
// Copyright (c) 2013 Piotr Smulewicz
//
// 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)
//=======================================================================
/**
 * @file shortest_superstring.hpp
 * @brief
 * @author Piotr Smulewicz
 * @version 1.0
 * @date 2013-08-29
 */
#ifndef PAAL_SHORTEST_SUPERSTRING_HPP
#define PAAL_SHORTEST_SUPERSTRING_HPP

#include "paal/utils/algorithms/suffix_array/lcp.hpp"
#include "paal/utils/algorithms/suffix_array/suffix_array.hpp"
#include "paal/utils/type_functions.hpp"
#include "paal/greedy/shortest_superstring/prefix_tree.hpp"
#include "paal/data_structures/bimap.hpp"
#include "paal/utils/irange.hpp"

#include <boost/range/adaptors.hpp>

#include <vector>
#include <algorithm>
#include <utility>
#include <type_traits>

namespace paal {
namespace greedy {
namespace detail {

/**
 * @class shortest_superstring
 * @brief class to solve shortest superstring 3.5 aproximation,
 * using greedy algorithm:
 * contract pair of words with largest overlap until one word stays
    \snippet shortest_superstring_example.cpp Shortest Superstring Example
 *
 * example file is shortest_superstring_example.cpp
 *
 * @tparam Words
 */
template <typename Words> class shortest_superstring {
  public:
    typedef range_to_elem_t<Words> Word;
    typedef range_to_elem_t<Word> Letter;

    shortest_superstring(const Words &words)
        : m_length(count_sum_lenght(words)),
          m_prefix_tree(m_length, m_suffix_array, m_sum_words, m_lcp,
                        m_length_suffix_word) {

        initialize(words);

        suffix_array<Letter>(m_sum_words, m_suffix_array);

        data_structures::rank(m_suffix_array, m_rank);

        lcp(m_suffix_array, m_rank, m_lcp, m_sum_words);

        m_prefix_tree.build_prefix_tree();

        m_prefix_tree.fill_suffix_to_tree();

        join_all_words();
    }

    /**
     * @brief return word contains all words as subwords,
     * of lenght at most 3.5 larger than shortest superstring.
     *
        \snippet shortest_superstring_example.cpp Shortest Superstring Example
     *
     * example file is shortest_superstring_example.cpp
     *
     */
    Word get_solution() {
        Word answer;
        for (auto posInSumWords : irange(1, m_length)) {
            if ((!m_is_joined_sufiix[m_pos_to_word[posInSumWords]]) &&
                (m_sum_words[posInSumWords - 1] == m_prefix_tree.DELIMITER)) {
                for (int nextLetter = posInSumWords;
                     m_sum_words[nextLetter] != m_prefix_tree.DELIMITER;) {
                    answer.push_back(m_sum_words[nextLetter]);
                    if (m_res[nextLetter] == NO_OVERLAP_STARTS_HERE) {
                        ++nextLetter;
                    } else {
                        nextLetter = m_res[nextLetter];
                    }
                }
            }
        }
        return answer;
    }

  private:
    int count_sum_lenght(const Words &words) {
        int length = 1;
        for (auto const &word : words) {
            length += word.size() + 1;
        }
        return length;
    }

    void initialize(const Words &words) {
        m_nu_words = words.size();
        m_first_word_in_block_to_last_word_in_block.resize(m_length);
        m_last_word_in_block_to_first_word_in_block.resize(m_length);
        m_pos_to_word.resize(m_length);
        m_length_suffix_word.resize(m_length);

        m_suffix_array.resize(m_length);
        m_lcp.resize(m_length);
        m_rank.resize(m_length);
        m_res.resize(m_length);
        m_sum_words.resize(m_length);
        m_is_joined_prefix.resize(m_nu_words);
        m_is_joined_sufiix.resize(m_nu_words);
        m_length_to_pos.resize(m_length);

        m_length = 1;
        int wordsId = 0;
        for (auto const &word : words) {
            auto wordSize = boost::distance(word);
            m_length_words.push_back(wordSize);
            m_length_to_pos[wordSize].push_back(m_length);
            int noLetterInWord = 0;
            for (auto letter : word) {
                assert(letter != 0);
                auto globalLetterId = m_length + noLetterInWord;
                m_sum_words[globalLetterId] = letter;
                m_pos_to_word[globalLetterId] = wordsId;
                m_length_suffix_word[globalLetterId] =
                    wordSize - noLetterInWord;
                ++noLetterInWord;
            }
            m_first_word_in_block_to_last_word_in_block[m_length] = m_length;
            m_last_word_in_block_to_first_word_in_block[m_length] = m_length;
            m_length += wordSize + 1;
            ++wordsId;
        }
    }

    void erase_word_form_prefix_tree(int word) {
        m_is_joined_sufiix[m_pos_to_word[word]] = JOINED;
        m_prefix_tree.erase_word_form_prefix_tree(word);
    }

    void join_all_words() {
        auto ovelapSizeRange = irange(m_length) | boost::adaptors::reversed;
        for (auto overlapSize : ovelapSizeRange) {
            for (auto word : m_length_to_pos[overlapSize]) {
                if (m_lcp[m_rank[word]] >= overlapSize) { // check if word is
                                                          // substring
                    erase_word_form_prefix_tree(word);
                }
            }
        }

        // in each iteration we join all pair of words who have overlap size
        // equal overlapSize
        for (auto overlapSize : ovelapSizeRange) {
            for (auto word : m_long_words) {
                join_word(word, overlapSize);
            }
            for (auto word : m_length_to_pos[overlapSize]) {
                if (m_lcp[m_rank[word]] < overlapSize) { // check if word is not
                                                         // substring
                    m_long_words.push_back(word);
                }
            }
        }
    }

    void join_word(int ps, int overlap) {
        if (m_is_joined_prefix[m_pos_to_word[ps]] == JOINED) {
            return;
        };

        int suffix = m_rank[ps + m_length_words[m_pos_to_word[ps]] - overlap];

        int prefix = m_prefix_tree.get_prefixequal_to_suffix(
            suffix, m_last_word_in_block_to_first_word_in_block[ps]);

        if (prefix == NOT_PREFIX) {
            return;
        }
        m_res[ps + m_length_words[m_pos_to_word[ps]] - overlap - 1] = prefix;
        m_is_joined_prefix[m_pos_to_word[ps]] = JOINED;

        m_last_word_in_block_to_first_word_in_block[
            m_first_word_in_block_to_last_word_in_block[prefix]] =
            m_last_word_in_block_to_first_word_in_block[ps];
        m_first_word_in_block_to_last_word_in_block[
            m_last_word_in_block_to_first_word_in_block[prefix]] = prefix;
        erase_word_form_prefix_tree(prefix);
    }

    int m_length, m_nu_words;
    std::vector<Letter> m_sum_words;
    std::vector<int> m_first_word_in_block_to_last_word_in_block,
        m_last_word_in_block_to_first_word_in_block, m_pos_to_word,
        m_length_words, m_length_suffix_word, m_suffix_array, m_lcp, m_rank,
        m_res, m_long_words;
    std::vector<bool> m_is_joined_prefix, m_is_joined_sufiix;
    std::vector<std::vector<int>> m_length_to_pos;

    prefix_tree<Letter> m_prefix_tree;

    const static bool JOINED = true;

    const static int NO_OVERLAP_STARTS_HERE = 0;
    const static int NOT_PREFIX = -1;
};
} //!detail

/**
 * @param words
 * @brief return word contains all words as subwords,
 * of lenght at most 3.5 larger than shortest superstring.
 * words canot contains letter 0
    \snippet shortest_superstring_example.cpp Shortest Superstring Example
 *
 * example file is shortest_superstring_example.cpp
 * @tparam Words
 */
template <typename Words>
auto shortestSuperstring(const Words &words)->decltype(
    std::declval<detail::shortest_superstring<Words>>().get_solution()) {
    detail::shortest_superstring<Words> solver(words);
    return solver.get_solution();
}
;

}      //!greedy
}      //!paal
#endif // PAAL_SHORTEST_SUPERSTRING_HPP


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

#ifndef IROHA_AMOUNT_UTILS_HPP
#define IROHA_AMOUNT_UTILS_HPP

#include <boost/multiprecision/cpp_int.hpp>

#include "common/result.hpp"

namespace shared_model {
  namespace interface {
    class Amount;
  }  // namespace interface

  namespace detail {
    boost::multiprecision::uint256_t increaseValuePrecision(
        boost::multiprecision::uint256_t value, int degree);

    /**
     * Sums up two amounts.
     * Result is returned
     * @param a left term
     * @param b right term
     */
    iroha::expected::PolymorphicResult<shared_model::interface::Amount,
                                       std::string>
    operator+(const shared_model::interface::Amount &a,
              const shared_model::interface::Amount &b);

    /**
     * Subtracts two amounts.
     * Result is returned
     * @param a left term
     * @param b right term
     */
    iroha::expected::PolymorphicResult<shared_model::interface::Amount,
                                       std::string>
    operator-(const shared_model::interface::Amount &a,
              const shared_model::interface::Amount &b);

    /**
     * Make amount with bigger precision
     * Result is returned
     * @param a amount
     * @param b right term
     */
    iroha::expected::PolymorphicResult<shared_model::interface::Amount,
                                       std::string>
    makeAmountWithPrecision(const shared_model::interface::Amount &amount,
                            const int new_precision);

    int compareAmount(const shared_model::interface::Amount &a,
                      const shared_model::interface::Amount &b);
  }  // namespace detail
}  // namespace shared_model
#endif  // IROHA_AMOUNT_UTILS_HPP


/*!
 * @file
 * Contains unit tests for the @ref Bitwise typeclass.
 */

#include <boost/mpl11/bitwise.hpp>

#include <boost/mpl11/core.hpp>
#include <boost/mpl11/integer.hpp>

#include "test_method_dispatch.hpp"

using namespace boost::mpl11;

///////////////////////////
// Test method dispatching
///////////////////////////
namespace boost { namespace mpl11 {
    template <>
    struct Bitwise<Archetype<0>> : instantiate<Bitwise>::with<Archetype<0>> {
        template <typename, typename>
        using bitand_impl  = method_tag<bitand_>;

        template <typename, typename>
        using bitor_impl  = method_tag<bitor_>;

        template <typename, typename>
        using bitxor_impl  = method_tag<bitxor>;

        template <typename, typename>
        using shift_left_impl  = method_tag<shift_left>;

        template <typename, typename>
        using shift_right_impl  = method_tag<shift_right>;

        template <typename>
        using compl_impl  = method_tag<compl_>;
    };
}}

struct test_dispatching
    : method<bitand_, archetype<0>, archetype<0>>
    // , method<bitand_, archetype<0>, archetype<1>>
    // , method<bitand_, archetype<1>, archetype<0>>

    , method<bitor_, archetype<0>, archetype<0>>
    // , method<bitor_, archetype<0>, archetype<1>>
    // , method<bitor_, archetype<1>, archetype<0>>

    , method<bitxor, archetype<0>, archetype<0>>
    // , method<bitxor, archetype<0>, archetype<1>>
    // , method<bitxor, archetype<1>, archetype<0>>

    , method<compl_, archetype<0>>
    , method<shift_right, archetype<0>, int_<0>>
    , method<shift_left, archetype<0>, int_<0>>
{ };

int main() { }


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

/// @file
/// Test whether we can correctly compute the phase space volume
/// of allowed three-phonon processes.

#include <iostream>
#include <functional>
#include <boost/filesystem.hpp>
#include <boost/mpi.hpp>
#include "gtest/gtest.h"
#include <vasp_io.hpp>
#include <qpoint_grid.hpp>
#include <processes.hpp>

TEST(p3_case, p3_test) {
    boost::mpi::environment env;

    auto basedir = boost::filesystem::path(TEST_RESOURCE_DIR);
    auto si_dir = basedir / boost::filesystem::path("Si");
    auto poscar_path = si_dir / boost::filesystem::path("POSCAR");
    auto ifc_path = si_dir / boost::filesystem::path("FORCE_CONSTANTS");

    auto poscar = alma::load_POSCAR(poscar_path.string().c_str());
    auto syms = alma::Symmetry_operations(*poscar);
    auto force_constants =
        alma::load_FORCE_CONSTANTS(ifc_path.string().c_str(), *poscar, 5, 5, 5);

    auto grid = std::make_unique<alma::Gamma_grid>(
        *poscar, syms, *force_constants, 12, 12, 12);

    grid->enforce_asr();
    auto nptk = grid->na * grid->nb * grid->nc;
    auto nmodes = grid->get_spectrum_at_q(0).omega.size();

    boost::mpi::communicator world;
    auto my_id = world.rank();
    auto processes = alma::find_allowed_threeph(*grid, world, 0.1);

    double my_P3_plus = 0.;
    double my_P3_minus = 0.;

    for (std::size_t i = 0; i < processes.size(); ++i) {
        auto g = processes[i].compute_gaussian();
        auto weights = grid->get_cardinal(processes[i].c);
        g *= weights;

        if (processes[i].type == alma::threeph_type::emission)
            my_P3_minus += g;
        else
            my_P3_plus += g;
    }

    double P3_minus = 0.;
    double P3_plus = 0.;
    boost::mpi::reduce(world, my_P3_minus, P3_minus, std::plus<double>(), 0);
    boost::mpi::reduce(world, my_P3_plus, P3_plus, std::plus<double>(), 0);
    double norm = nmodes * nmodes * nmodes * nptk * nptk;
    P3_minus /= norm;
    P3_plus /= norm;
    auto P3_total = 2. * (P3_plus + 0.5 * P3_minus) / 3.;

    if (my_id == 0) {
        EXPECT_TRUE(alma::almost_equal(P3_total, 0.002898838171, 1e-8, 1e-2));
        EXPECT_TRUE(alma::almost_equal(P3_plus, 0.002900801885, 1e-8, 1e-2));
        EXPECT_TRUE(alma::almost_equal(P3_minus, 0.002894910742, 1e-8, 1e-2));
    }
}


#include <unistd.h>
#include <ctime>
#include <iomanip>
#include <map>
#include <vector>
#include <algorithm>
#include "sireen/file_utility.hpp"
#include "sireen/image_feature_extract.hpp"
#include <Eigen/Dense>
using namespace std;
using namespace Eigen;
float* eigen_mat_test()
{
    vector<float> v;
    v.push_back(1.0);
    v.push_back(2.0);
    return &v[0];
}
void print_vector(float *x)
{
    for(size_t i = 0; i < 2; ++i)
    {
        cout << x[i] << endl;
    }
}
int main(int argc, char * argv[])
{
    string images[] = {"test1.jpg","test2.jpg","test3.jpg","test4.jpg","test5.jpg","test6.jpg"};
    string prefix = "/home/bingqingqu/TAOCP/Datasets/test/";
    string codebookPath =  "/home/bingqingqu/TAOCP/Datasets/test/cb1008001019.txt";
    ImageCoder ic;
    float *codebook = new float[128 * 500];

    // clock_t s;
    // s = clock();
    char delim[2] = ",";
    // load codebook
    futil::file_to_pointer(codebookPath.c_str(), codebook,delim);
    // cout << "codebook:" << float(clock() -s) << endl;

    // s = clock();
    // string result_str = futil::file2str(codebookPath.c_str());
    // cout << "file2str:" << float(clock() -s) << endl;

    // const std::string words= "Hello,world,I,love,u,a ,ha,g,a,ha,s,csdsasd,d@@,s!";
    // // const char words1[]= "Hello,world,I,love,u,a ,ha,g,a,ha,s,csdsasd,d@@,s!";
    // std::vector<std::string> result;
    // s = clock();
    // futil::spliter(result_str.c_str(),',',result);
    // cout << "split1:" << float(clock() -s) << endl;
    // cout << result[5]<<endl;

    // std::vector<std::string> result2;
    // s = clock();
    // futil::split2(result_str,',',result2);
    // cout << "split2:" << float(clock() -s) << endl;
    // cout << result2[5]<<endl;
    clock_t start;
    FILE * f = fopen("/home/bingqingqu/TAOCP/Datasets/test/result_new.txt","w+");
    fclose(f);
    for(int i=0;i<6;i++)
    {
        Mat src_new = imread(prefix+images[i],0);

        // test
        string llc_test;
        start= clock();
        try{
            llc_test = ic.llc_sift(src_new, codebook, 500, 5);

            // cout << llc_test<<endl;
            futil::string_to_file("/home/bingqingqu/TAOCP/Datasets/test/result_new.txt",
                            llc_test, "a+");
        }
        catch(...){
            cout << "fail to llc" <<endl;
        }
        cout << "time test:" << float(clock() -start)/CLOCKS_PER_SEC << endl;
        // cout << llc_test<<endl;
    }
    delete [] codebook;
    string directory = "/home/bingqingqu/TAOCP/Datasets/test/";
    vector<string> files_in_dir;
    futil::get_files_in_dir(files_in_dir,directory);
    cout << files_in_dir[0]<<","<<files_in_dir[1] << endl;

    // vector<float> test_descr;
    // float * descr_buf = new float[128];
    // for(int i=0;i<128;++i)
    // {
    //     descr_buf[i] = 1.5;
    // }
    // std::copy(descr_buf, descr_buf+128, back_inserter(test_descr));
    // std::copy(descr_buf, descr_buf+128, back_inserter(test_descr));
    // delete [] descr_buf;
    // for(vector<float>::iterator it(test_descr.begin()); it!=test_descr.end();++it)
    //     cout << " " << *it;
    // cout << endl;
    // float *descr_ptr = &test_descr[0];
    // for(int i=0;i<256;++i)
    //     cout << " " << descr_ptr[i] ;
    // cout << endl;

    // Vector2f v(5.0,6.0);
    // float* vvv = eigen_mat_test();
    // for(size_t i = 0; i< 2; ++i)
    //     cout << vvv[i] << "," << endl;
    vector<float> xxx;
    xxx.reserve(100);
    xxx.push_back(0.5);
    cout << xxx.capacity() << endl;
    cout << xxx.size() << endl;
    xxx.clear();
    cout << xxx.capacity() << endl;
    cout << xxx.size() << endl;
}


/*
 *  P2OS for ROS
 *  Copyright (C) 2009
 *     David Feil-Seifer, Brian Gerkey, Kasper Stoy,
 *      Richard Vaughan, & Andrew Howard
 *
 *
 *  This program is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 */

#include <stdio.h>
#include <limits.h>
#include <math.h>  /* rint(3) */
#include <sys/types.h>
#include <stdlib.h> /* for abs() */
#include <unistd.h>

#include "../include/p2os_driver/sip.h"
#include "tf/tf.h"
#include "tf/transform_datatypes.h"
#include <sstream>
#include <boost/assign/list_of.hpp>

void SIP::FillStandard(ros_p2os_data_t* data)
{
  ///////////////////////////////////////////////////////////////
  // odometry

  double px, py, pa;

  // initialize position to current offset
  px = this->x_offset / 1e3;
  py = this->y_offset / 1e3;
  // now transform current position by rotation if there is one
  // and add to offset
  if(this->angle_offset != 0)
  {
    double rot = DTOR(this->angle_offset);    // convert rotation to radians
    px +=  ((this->xpos/1e3) * cos(rot) -
                               (this->ypos/1e3) * sin(rot));
    py +=  ((this->xpos/1e3) * sin(rot) +
                               (this->ypos/1e3) * cos(rot));
    pa = DTOR(this->angle_offset + angle);
  }
  else
  {
    px += this->xpos / 1e3;
    py += this->ypos / 1e3;
    pa = DTOR(this->angle);
  }

  // timestamps get set in the p2os::StandardSIPPutData fcn
  data->position.header.frame_id = "odom";
  data->position.child_frame_id = "base_link";

  data->position.pose.pose.position.x = px;
  data->position.pose.pose.position.y = py;
  data->position.pose.pose.position.z = 0.0;
  data->position.pose.pose.orientation = tf::createQuaternionMsgFromYaw(pa);

    // add rates
  data->position.twist.twist.linear.x = ((lvel + rvel) / 2) / 1e3;
  data->position.twist.twist.linear.y = 0.0;
  data->position.twist.twist.angular.z = ((double)(rvel-lvel)/(2.0/PlayerRobotParams[param_idx].DiffConvFactor));

  data->position.pose.covariance = boost::assign::list_of	(1e-3) (0)    (0)   (0)   (0)   (0)
                                                          (0)    (1e-3) (0)   (0)   (0)   (0)
                                                          (0)    (0)    (1e6) (0)   (0)   (0)
                                                          (0)    (0)    (0)   (1e6) (0)   (0)
                                                          (0)    (0)    (0)   (0)   (1e6) (0)
                                                          (0)    (0)    (0)   (0)   (0)   (1e3) ;

	data->position.twist.covariance = boost::assign::list_of (1e-3) (0)    (0)   (0)   (0)   (0)
                                                           (0)    (1e-3) (0)   (0)   (0)   (0)
                                                           (0)    (0)    (1e6) (0)   (0)   (0)
                                                           (0)    (0)    (0)   (1e6) (0)   (0)
                                                           (0)    (0)    (0)   (0)   (1e6) (0)
                                                           (0)    (0)    (0)   (0)   (0)   (1e3) ;

  //publish transform
  data->odom_trans.header = data->position.header;
  data->odom_trans.child_frame_id = data->position.child_frame_id;
  data->odom_trans.transform.translation.x = px;
  data->odom_trans.transform.translation.y = py;
  data->odom_trans.transform.translation.z = 0;
  data->odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(pa);

  // battery
  data->batt.voltage = battery / 10.0;

  // motor state
  // The below will tell us if the motors are currently moving or not, it does
  // not tell us whether they have been enabled
  // data->motors.state = (status & 0x01);
	data->motors.state = (motors_enabled & 0x01);
  /*
  ///////////////////////////////////////////////////////////////
  // compass
  memset(&(data->compass),0,sizeof(data->compass));
  data->compass.pos.pa = DTOR(this->compass);
  */

  ///////////////////////////////////////////////////////////////
  // sonar
  data->sonar.ranges_count = static_cast<int>(sonarreadings);
  data->sonar.ranges.clear();
  for(int i=0; i < data->sonar.ranges_count; i++)
    data->sonar.ranges.push_back(sonars[i] / 1e3);

  ///////////////////////////////////////////////////////////////
  // gripper
  unsigned char gripState = timer;
  if ((gripState & 0x01) && (gripState & 0x02) && !(gripState & 0x04))
  {
      data->gripper.grip.state = PLAYER_GRIPPER_STATE_ERROR;
      data->gripper.grip.dir = 0;
  }
  else if(gripState & 0x04)
  {
    data->gripper.grip.state = PLAYER_GRIPPER_STATE_MOVING;
    if(gripState & 0x01)
      data->gripper.grip.dir = 1;
    if(gripState & 0x02)
      data->gripper.grip.dir = -1;
  }
  else if(gripState & 0x01)
  {
    data->gripper.grip.state = PLAYER_GRIPPER_STATE_OPEN;
    data->gripper.grip.dir = 0;
  }
  else if(gripState & 0x02)
  {
    data->gripper.grip.state = PLAYER_GRIPPER_STATE_CLOSED;
    data->gripper.grip.dir = 0;
  }

  // Reset data to false
  data->gripper.grip.inner_beam = false;
  data->gripper.grip.outer_beam = false;
  data->gripper.grip.left_contact = false;
  data->gripper.grip.right_contact = false;

  if (digin & 0x08)
  {
    data->gripper.grip.inner_beam = true;
  }
  if (digin & 0x04)
  {
    data->gripper.grip.outer_beam = true;
  }
  if (!(digin & 0x10))
  {
    data->gripper.grip.left_contact = true;
  }
  if (!(digin & 0x20))
  {
    data->gripper.grip.right_contact = true;
  }

  // lift
  data->gripper.lift.dir = 0;

  if ((gripState & 0x10) && (gripState & 0x20) && !(gripState & 0x40))
  {
      // In this case, the lift is somewhere in between, so
      // must be at an intermediate carry position. Use last commanded position
      data->gripper.lift.state = PLAYER_ACTARRAY_ACTSTATE_IDLE;
      data->gripper.lift.position = lastLiftPos;
  }
  else if (gripState & 0x40)  // Moving
  {
      data->gripper.lift.state = PLAYER_ACTARRAY_ACTSTATE_MOVING;
      // There is no way to know where it is for sure, so use last commanded
      // position.
      data->gripper.lift.position = lastLiftPos;
      if (gripState & 0x10)
        data->gripper.lift.dir = 1;
      else if (gripState & 0x20)
        data->gripper.lift.dir = -1;
  }
  else if (gripState & 0x10)  // Up
  {
      data->gripper.lift.state = PLAYER_ACTARRAY_ACTSTATE_IDLE;
      data->gripper.lift.position = 1.0f;
      data->gripper.lift.dir = 0;
  }
  else if (gripState & 0x20)  // Down
  {
      data->gripper.lift.state = PLAYER_ACTARRAY_ACTSTATE_IDLE;
      data->gripper.lift.position = 0.0f;
      data->gripper.lift.dir = 0;
  }
  else    // Assume stalled
  {
      data->gripper.lift.state = PLAYER_ACTARRAY_ACTSTATE_STALLED;
  }
  // Store the last lift position
  lastLiftPos = data->gripper.lift.position;

  /*
  ///////////////////////////////////////////////////////////////
  // bumper
  unsigned int bump_count = PlayerRobotParams[param_idx].NumFrontBumpers + PlayerRobotParams[param_idx].NumRearBumpers;
  if (data->bumper.bumpers_count != bump_count)
  {
    data->bumper.bumpers_count = bump_count;
    delete [] data->bumper.bumpers;
    data->bumper.bumpers = new uint8_t[bump_count];
  }
  int j = 0;
  for(int i=PlayerRobotParams[param_idx].NumFrontBumpers-1;i>=0;i--)
    data->bumper.bumpers[j++] =
      (unsigned char)((this->frontbumpers >> i) & 0x01);
  for(int i=PlayerRobotParams[param_idx].NumRearBumpers-1;i>=0;i--)
    data->bumper.bumpers[j++] =
      (unsigned char)((this->rearbumpers >> i) & 0x01);
  */

  ///////////////////////////////////////////////////////////////
  // digital I/O
  data->dio.count = 8;
  data->dio.bits = (unsigned int)this->digin;

  ///////////////////////////////////////////////////////////////
  // analog I/O
  //TODO: should do this smarter, based on which analog input is selected
  data->aio.voltages_count = (unsigned char)1;
  // if (!data->aio.voltages)
  //   data->aio.voltages = new float[1];
  // data->aio.voltages[0] = (this->analog / 255.0) * 5.0;
  data->aio.voltages.clear();
  data->aio.voltages.push_back((this->analog / 255.0) * 5.0);
}

int SIP::PositionChange( unsigned short from, unsigned short to )
{
  int diff1, diff2;

  /* find difference in two directions and pick shortest */
  if ( to > from ) {
    diff1 = to - from;
    diff2 = - ( from + 4096 - to );
  }
  else {
    diff1 = to - from;
    diff2 = 4096 - from + to;
  }

  if ( abs(diff1) < abs(diff2) )
    return(diff1);
  else
    return(diff2);

}

void SIP::Print()
{
  int i;

  ROS_DEBUG("lwstall:%d rwstall:%d\n", lwstall, rwstall);

  std::stringstream front_bumper_info;
  for(int i=0;i<5;i++) {
    front_bumper_info << " "
                      << static_cast<int>((frontbumpers >> i) & 0x01 );
  }
  ROS_DEBUG("Front bumpers:%s", front_bumper_info.str().c_str());
  std::stringstream rear_bumper_info;
  for(int i=0;i<5;i++) {
    rear_bumper_info << " "
                     << static_cast<int>((rearbumpers >> i) & 0x01 );
  }
  ROS_DEBUG("Rear bumpers:%s", rear_bumper_info.str().c_str());

  ROS_DEBUG("status: 0x%x analog: %d param_id: %d ", status, analog, param_idx);
  std::stringstream status_info;
  for(i=0;i<11;i++) {
    status_info << " "
               << static_cast<int>((status >> (7-i) ) & 0x01);
  }
  ROS_DEBUG("status:%s", status_info.str().c_str());
  std::stringstream digin_info;
  for(i=0;i<8;i++) {
    digin_info << " "
               << static_cast<int>((digin >> (7-i) ) & 0x01);
  }
  ROS_DEBUG("digin:%s", digin_info.str().c_str());
  std::stringstream digout_info;
  for(i=0;i<8;i++) {
    digout_info << " "
               << static_cast<int>((digout >> (7-i) ) & 0x01);
  }
  ROS_DEBUG("digout:%s", digout_info.str().c_str());
  ROS_DEBUG("battery: %d compass: %d sonarreadings: %d\n", battery,
            compass, sonarreadings);
  ROS_DEBUG("xpos: %d ypos:%d ptu:%hu timer:%hu\n", xpos, ypos, ptu, timer);
  ROS_DEBUG("angle: %d lvel: %d rvel: %d control: %d\n", angle, lvel,
            rvel, control);

  PrintSonars();
  PrintArmInfo ();
  PrintArm ();
}

void SIP::PrintSonars()
{
  if(sonarreadings <= 0)
    return;
  std::stringstream sonar_info;
  for(int i = 0; i < sonarreadings; i++){
    sonar_info << " " << static_cast<int>(sonars[i]);
  }
  ROS_DEBUG("Sonars: %s", sonar_info.str().c_str());
}

void SIP::PrintArm ()
{
	ROS_DEBUG ("Arm power is %s\tArm is %sconnected\n", (armPowerOn ? "on" : "off"), (armConnected ? "" : "not "));
	ROS_DEBUG ("Arm joint status:\n");
	for (int ii = 0; ii < 6; ii++)
		ROS_DEBUG ("Joint %d   %s   %d\n", ii + 1, (armJointMoving[ii] ? "Moving " : "Stopped"), armJointPos[ii]);
}

void SIP::PrintArmInfo ()
{
	ROS_DEBUG ("Arm version:\t%s\n", armVersionString);
	ROS_DEBUG ("Arm has %d joints:\n", armNumJoints);
	ROS_DEBUG ("  |\tSpeed\tHome\tMin\tCentre\tMax\tTicks/90\n");
	for (int ii = 0; ii < armNumJoints; ii++)
		ROS_DEBUG ("%d |\t%d\t%d\t%d\t%d\t%d\t%d\n", ii, armJoints[ii].speed, armJoints[ii].home, armJoints[ii].min, armJoints[ii].centre, armJoints[ii].max, armJoints[ii].ticksPer90);
}

void SIP::ParseStandard( unsigned char *buffer )
{
  int cnt = 0, change;
  unsigned short newxpos, newypos;

  status = buffer[cnt];
  cnt += sizeof(unsigned char);
  /*
   * Remember P2OS uses little endian:
   * for a 2 byte short (called integer on P2OS)
   * byte0 is low byte, byte1 is high byte
   * The following code is host-machine endian independant
   * Also we must or (|) bytes together instead of casting to a
   * short * since on ARM architectures short * must be even byte aligned!
   * You can get away with this on a i386 since shorts * can be
   * odd byte aligned. But on ARM, the last bit of the pointer will be ignored!
   * The or'ing will work on either arch.
   */
  newxpos = ((buffer[cnt] | (buffer[cnt+1] << 8))
	     & 0xEFFF) % 4096; /* 15 ls-bits */

  if (xpos!=INT_MAX) {
    change = (int) rint(PositionChange( rawxpos, newxpos ) *
			PlayerRobotParams[param_idx].DistConvFactor);
    if (abs(change)>100)
      ROS_DEBUG("invalid odometry change [%d]; odometry values are tainted", change);
    else
      xpos += change;
  }
  else
    xpos = 0;
  rawxpos = newxpos;
  cnt += sizeof(short);

  newypos = ((buffer[cnt] | (buffer[cnt+1] << 8))
	     & 0xEFFF) % 4096; /* 15 ls-bits */

  if (ypos!=INT_MAX) {
    change = (int) rint(PositionChange( rawypos, newypos ) *
			PlayerRobotParams[param_idx].DistConvFactor);
    if (abs(change)>100)
      ROS_DEBUG("invalid odometry change [%d]; odometry values are tainted", change);
    else
      ypos += change;
  }
  else
    ypos = 0;
  rawypos = newypos;
  cnt += sizeof(short);

  angle = (short)
    rint(((short)(buffer[cnt] | (buffer[cnt+1] << 8))) *
	 PlayerRobotParams[param_idx].AngleConvFactor * 180.0/M_PI);
  cnt += sizeof(short);

  lvel = (short)
    rint(((short)(buffer[cnt] | (buffer[cnt+1] << 8))) *
	 PlayerRobotParams[param_idx].VelConvFactor);
  cnt += sizeof(short);

  rvel = (short)
    rint(((short)(buffer[cnt] | (buffer[cnt+1] << 8))) *
	 PlayerRobotParams[param_idx].VelConvFactor);
  cnt += sizeof(short);

  battery = buffer[cnt];
  cnt += sizeof(unsigned char);
  ROS_DEBUG( "battery value: %d", battery );

  lwstall = buffer[cnt] & 0x01;
  rearbumpers = buffer[cnt] >> 1;
  cnt += sizeof(unsigned char);

  rwstall = buffer[cnt] & 0x01;
  frontbumpers = buffer[cnt] >> 1;
  cnt += sizeof(unsigned char);

  control = (short)
    rint(((short)(buffer[cnt] | (buffer[cnt+1] << 8))) *
	 PlayerRobotParams[param_idx].AngleConvFactor);
  cnt += sizeof(short);

  ptu = (buffer[cnt] | (buffer[cnt+1] << 8));
	motors_enabled = buffer[cnt];
	sonar_flag = buffer[cnt+1];
  cnt += sizeof(short);

  //compass = buffer[cnt]*2;
  if(buffer[cnt] != 255 && buffer[cnt] != 0 && buffer[cnt] != 181)
    compass = (buffer[cnt]-1)*2;
  cnt += sizeof(unsigned char);

  unsigned char numSonars=buffer[cnt];
  cnt+=sizeof(unsigned char);

  if(numSonars > 0)
  {
    //find the largest sonar index supplied
    unsigned char maxSonars=sonarreadings;
    for(unsigned char i=0;i<numSonars;i++)
    {
      unsigned char sonarIndex=buffer[cnt+i*(sizeof(unsigned char)+sizeof(unsigned short))];
      if((sonarIndex+1)>maxSonars) maxSonars=sonarIndex+1;
    }

    //if necessary make more space in the array and preserve existing readings
    if(maxSonars>sonarreadings)
    {
      unsigned short *newSonars=new unsigned short[maxSonars];
      for(unsigned char i=0;i<sonarreadings;i++)
        newSonars[i]=sonars[i];
      if(sonars!=NULL) delete[] sonars;
      sonars=newSonars;
      sonarreadings=maxSonars;
    }

    //update the sonar readings array with the new readings
    for(unsigned char i=0;i<numSonars;i++)
    {
    sonars[buffer[cnt]]=   (unsigned short)
      rint((buffer[cnt+1] | (buffer[cnt+2] << 8)) *
	   PlayerRobotParams[param_idx].RangeConvFactor);
      cnt+=sizeof(unsigned char)+sizeof(unsigned short);
  }
  }

  timer = (buffer[cnt] | (buffer[cnt+1] << 8));
  cnt += sizeof(short);

  analog = buffer[cnt];
  cnt += sizeof(unsigned char);

  digin = buffer[cnt];
  cnt += sizeof(unsigned char);

  digout = buffer[cnt];
  cnt += sizeof(unsigned char);
  // for debugging:
  Print();
  // PrintSonars();
}

/** Parse a SERAUX SIP packet.  For a CMUcam, this will have blob
 **  tracking messages in the format (all one-byte values, no spaces):
 **
 **      255 M mx my x1 y1 x2 y2 pixels confidence  (10-bytes)
 **
 ** Or color info messages of the format:
 **
 **      255 S Rval Gval Bval Rvar Gvar Bvar    (8-bytes)
 */
void SIP::ParseSERAUX( unsigned char *buffer )
{
  unsigned char type = buffer[1];
  if (type != SERAUX && type != SERAUX2)
  {
    // Really should never get here...
    ROS_ERROR("Attempt to parse non SERAUX packet as serial data.\n");
    return;
  }

  int len  = (int)buffer[0]-3;		// Get the string length

  /* First thing is to find the beginning of last full packet (if possible).
  ** If there are less than CMUCAM_MESSAGE_LEN*2-1 bytes (19), we're not
  ** guaranteed to have a full packet.  If less than CMUCAM_MESSAGE_LEN
  ** bytes, it is impossible to have a full packet.
  ** To find the beginning of the last full packet, search between bytes
  ** len-17 and len-8 (inclusive) for the start flag (255).
  */
  int ix;
  for (ix = (len>18 ? len-17 : 2); ix<=len-8; ix++)
    if (buffer[ix] == 255)
      break;		// Got it!
  if (len < 10 || ix > len-8) {
    ROS_ERROR("Failed to get a full blob tracking packet.\n");
    return;
  }

  // There is a special 'S' message containing the tracking color info
  if (buffer[ix+1] == 'S')
  {
     // Color information (track color)
     ROS_DEBUG("Tracking color (RGB):  %d %d %d\n"
            "       with variance:  %d %d %d\n",
              buffer[ix+2], buffer[ix+3], buffer[ix+4],
              buffer[ix+5], buffer[ix+6], buffer[ix+7]);
     blobcolor = buffer[ix+2]<<16 | buffer[ix+3]<<8 | buffer[ix+4];
     return;
  }

  // Tracking packets with centroid info are designated with a 'M'
  if (buffer[ix+1] == 'M')
  {
     // Now it's easy.  Just parse the packet.
     blobmx	= buffer[ix+2];
     blobmy	= buffer[ix+3];
     blobx1	= buffer[ix+4];
     bloby1	= buffer[ix+5];
     blobx2	= buffer[ix+6];
     bloby2	= buffer[ix+7];
     blobconf	= buffer[ix+9];
     // Xiaoquan Fu's calculation for blob area (max 11297).
     blobarea	= (bloby2 - bloby1 +1)*(blobx2 - blobx1 + 1)*blobconf/255;
     return;
  }

  ROS_ERROR("Unknown blob tracker packet type: %c\n", buffer[ix+1]);
  return;
}

// Parse a set of gyro measurements.  The buffer is formatted thusly:
//     length (2 bytes), type (1 byte), count (1 byte)
// followed by <count> pairs of the form:
//     rate (2 bytes), temp (1 byte)
// <rate> falls in [0,1023]; less than 512 is CCW rotation and greater
// than 512 is CW
void
SIP::ParseGyro(unsigned char* buffer)
{
  // Get the message length (account for the type byte and the 2-byte
  // checksum)
  int len  = (int)buffer[0]-3;

  unsigned char type = buffer[1];
  if(type != GYROPAC)
  {
    // Really should never get here...
    ROS_ERROR("Attempt to parse non GYRO packet as gyro data.\n");
    return;
  }

  if(len < 1)
  {
    ROS_DEBUG("Couldn't get gyro measurement count");
    return;
  }

  // get count
  int count = (int)buffer[2];

  // sanity check
  if((len-1) != (count*3))
  {
    ROS_DEBUG("Mismatch between gyro measurement count and packet length");
    return;
  }

  // Actually handle the rate values.  Any number of things could (and
  // probably should) be done here, like filtering, calibration, conversion
  // from the gyro's arbitrary units to something meaningful, etc.
  //
  // As a first cut, we'll just average all the rate measurements in this
  // set, and ignore the temperatures.
  float ratesum = 0;
  int bufferpos = 3;
  unsigned short rate;
  unsigned char temp;
  for(int i=0; i<count; i++)
  {
    rate = (unsigned short)(buffer[bufferpos++]);
    rate |= buffer[bufferpos++] << 8;
    temp = bufferpos++;

    ratesum += rate;
  }

  int32_t average_rate = (int32_t)rint(ratesum / (float)count);

  // store the result for sending
  gyro_rate = average_rate;
}

void SIP::ParseArm (unsigned char *buffer)
{
	int length = (int) buffer[0] - 2;

	if (buffer[1] != ARMPAC)
	{
		ROS_ERROR("Attempt to parse a non ARM packet as arm data.\n");
		return;
	}

	if (length < 1 || length != 9)
	{
		ROS_DEBUG ("ARMpac length incorrect size");
		return;
	}

	unsigned char status = buffer[2];
	if (status & 0x01)
		armPowerOn = true;		// Power is on
	else
		armPowerOn = false;		// Power is off

	if (status & 0x02)
		armConnected = true;	// Connection is established
	else
		armConnected = false;	// Connection is not established

	unsigned char motionStatus = buffer[3];
	if (motionStatus & 0x01)
		armJointMoving[0] = true;
	if (motionStatus & 0x02)
		armJointMoving[1] = true;
	if (motionStatus & 0x04)
		armJointMoving[2] = true;
	if (motionStatus & 0x08)
		armJointMoving[3] = true;
	if (motionStatus & 0x10)
		armJointMoving[4] = true;
	if (motionStatus & 0x20)
		armJointMoving[5] = true;

	memcpy (armJointPos, &buffer[4], 6);
	memset (armJointPosRads, 0, 6 * sizeof (double));
}

void SIP::ParseArmInfo (unsigned char *buffer)
{
	int length = (int) buffer[0] - 2;
	if (buffer[1] != ARMINFOPAC)
	{
		ROS_ERROR ("Attempt to parse a non ARMINFO packet as arm info.\n");
		return;
	}

	if (length < 1)
	{
		ROS_DEBUG ("ARMINFOpac length bad size");
		return;
	}

	// Copy the version string
	if (armVersionString)
		free (armVersionString);
        // strndup() isn't available everywhere (e.g., Darwin)
	//armVersionString = strndup ((char*) &buffer[2], length);		// Can't be any bigger than length
        armVersionString = (char*)calloc(length+1,sizeof(char));
        assert(armVersionString);
        strncpy(armVersionString,(char*)&buffer[2], length);
	int dataOffset = strlen (armVersionString) + 3;		// +1 for packet size byte, +1 for packet ID, +1 for null byte

	armNumJoints = buffer[dataOffset];
	if (armJoints)
		delete[] armJoints;
	if (armNumJoints <= 0)
		return;
	armJoints = new ArmJoint[armNumJoints];
	dataOffset += 1;
	for (int ii = 0; ii < armNumJoints; ii++)
	{
		armJoints[ii].speed = buffer[dataOffset + (ii * 6)];
		armJoints[ii].home = buffer[dataOffset + (ii * 6) + 1];
		armJoints[ii].min = buffer[dataOffset + (ii * 6) + 2];
		armJoints[ii].centre = buffer[dataOffset + (ii * 6) + 3];
		armJoints[ii].max = buffer[dataOffset + (ii * 6) + 4];
		armJoints[ii].ticksPer90 = buffer[dataOffset + (ii * 6) + 5];
	}
}


// Copyright (c) 2014-2017 The Bitcoin Core developers
// Copyright (c) 2018 Equibit Group AG
// Distributed under the MIT software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.

#include <chainparams.h>
#include <validation.h>
#include <net.h>

#include <test/test_bitcoin.h>

#include <boost/signals2/signal.hpp>
#include <boost/test/unit_test.hpp>

BOOST_FIXTURE_TEST_SUITE(main_tests, TestingSetup)

#ifdef BUILD_BTC
static void TestBlockSubsidyHalvings(const Consensus::Params& consensusParams)
{
    int maxHalvings = 64;
    CAmount nInitialSubsidy = 50 * COIN;

    CAmount nPreviousSubsidy = nInitialSubsidy * 2; // for height == 0
    BOOST_CHECK_EQUAL(nPreviousSubsidy, nInitialSubsidy * 2);
    for (int nHalvings = 0; nHalvings < maxHalvings; nHalvings++) {
        int nHeight = nHalvings * consensusParams.nSubsidyHalvingInterval;
        CAmount nSubsidy = GetBlockSubsidy(nHeight, consensusParams);
        BOOST_CHECK(nSubsidy <= nInitialSubsidy);
        BOOST_CHECK_EQUAL(nSubsidy, nPreviousSubsidy / 2);
        nPreviousSubsidy = nSubsidy;
    }
    BOOST_CHECK_EQUAL(GetBlockSubsidy(maxHalvings * consensusParams.nSubsidyHalvingInterval, consensusParams), 0);
}

static void TestBlockSubsidyHalvings(int nSubsidyHalvingInterval)
{
    Consensus::Params consensusParams;
    consensusParams.nSubsidyHalvingInterval = nSubsidyHalvingInterval;
    TestBlockSubsidyHalvings(consensusParams);
}

BOOST_AUTO_TEST_CASE(block_subsidy_test)
{
    const auto chainParams = CreateChainParams(CBaseChainParams::MAIN);
    TestBlockSubsidyHalvings(chainParams->GetConsensus()); // As in main
    TestBlockSubsidyHalvings(150); // As in regtest
    TestBlockSubsidyHalvings(1000); // Just another interval
}

BOOST_AUTO_TEST_CASE(subsidy_limit_test)
{
    const auto chainParams = CreateChainParams(CBaseChainParams::MAIN);
    CAmount nSum = 0;
    for (int nHeight = 0; nHeight < 14000000; nHeight += 1000) {
        CAmount nSubsidy = GetBlockSubsidy(nHeight, chainParams->GetConsensus());
        BOOST_CHECK(nSubsidy <= 50 * COIN);
        nSum += nSubsidy * 1000;
        BOOST_CHECK(MoneyRange(nSum));
    }
    BOOST_CHECK_EQUAL(nSum, 2099999997690000ULL);
}

bool ReturnFalse() { return false; }
bool ReturnTrue() { return true; }

BOOST_AUTO_TEST_CASE(test_combiner_all)
{
    boost::signals2::signal<bool (), CombinerAll> Test;
    BOOST_CHECK(Test());
    Test.connect(&ReturnFalse);
    BOOST_CHECK(!Test());
    Test.connect(&ReturnTrue);
    BOOST_CHECK(!Test());
    Test.disconnect(&ReturnFalse);
    BOOST_CHECK(Test());
    Test.disconnect(&ReturnTrue);
    BOOST_CHECK(Test());
}
#else  // BUILD_OCN

static const CAmount    MAX_COINBASE = 52.5 * COIN;

static void TestBlockSubsidyCurve(const Consensus::Params& consensusParams)
{
    // Pre-calculated block mining reward for test
    static std::map<int, CAmount> blockNumToMiningReward = {
                                                            std::pair<int, CAmount>(0       , GENESIS_BLOCK_REWARD),
                                                            std::pair<int, CAmount>(1       , FIRST_BLOCK_REWARD),
                                                            std::pair<int, CAmount>(52500   , 506341245),
                                                            std::pair<int, CAmount>(105000  , 827458486),
                                                            std::pair<int, CAmount>(157500  , 1322666848),
                                                            std::pair<int, CAmount>(210000  , 2041088800),
                                                            std::pair<int, CAmount>(262500  , 2983908560),
                                                            std::pair<int, CAmount>(315000  , 4032576406),
                                                            std::pair<int, CAmount>(367500  , 4904234072),
                                                            std::pair<int, CAmount>(419999  , 5249999999),
                                                            std::pair<int, CAmount>(420000  , 5250000000),
                                                            std::pair<int, CAmount>(420001  , 5249999999),
                                                            std::pair<int, CAmount>(472500  , 4904234072),
                                                            std::pair<int, CAmount>(525000  , 4032576406),
                                                            std::pair<int, CAmount>(577500  , 2983908560),
                                                            std::pair<int, CAmount>(630000  , 2041088800),
                                                            std::pair<int, CAmount>(682500  , 1322666848),
                                                            std::pair<int, CAmount>(735000  , 827458486),
                                                            std::pair<int, CAmount>(787500  , 506341245),
                                                            std::pair<int, CAmount>(840000  , 305670953),
                                                            std::pair<int, CAmount>(892500  , 183023491),
                                                            std::pair<int, CAmount>(945000  , 109050395),
                                                            std::pair<int, CAmount>(997500  , 64785313),
                                                            std::pair<int, CAmount>(1050000 , 38421164),
                                                            std::pair<int, CAmount>(1102500 , 22762315),
                                                            std::pair<int, CAmount>(1155000 , 13477113),
                                                            std::pair<int, CAmount>(1207500 , 7976642),
                                                            std::pair<int, CAmount>(1260000 , 4720090),
                                                            std::pair<int, CAmount>(1312500 , 2792707),
                                                            std::pair<int, CAmount>(1365000 , 1652220),
                                                           };

    std::map<int, CAmount>::iterator itr_Heights = blockNumToMiningReward.begin();
    std::map<int, CAmount>::iterator itr_HeightMiningRewardFinder;

    while (itr_Heights != blockNumToMiningReward.end())
    {
        int nHeightTest = itr_Heights->first / consensusParams.nSubsidyAccelerationFactor;

        itr_HeightMiningRewardFinder = blockNumToMiningReward.find(nHeightTest);

        if (itr_HeightMiningRewardFinder != blockNumToMiningReward.end())
        {
            CAmount nSubsidy = GetBlockSubsidy(nHeightTest, consensusParams);
            if (nHeightTest > 1)
            {
                BOOST_CHECK(nSubsidy <= MAX_COINBASE);
            }
//            if (nSubsidy != itr_HeightMiningRewardFinder->second)
//                std::cout << "\nnHeight = " << nHeightTest << "\tnSubC = " << nSubsidy << "\tnSubR= " << itr_HeightMiningRewardFinder->second;
            BOOST_CHECK_EQUAL(nSubsidy, itr_HeightMiningRewardFinder->second);
        }
        ++ itr_Heights;
    }
}

BOOST_AUTO_TEST_CASE(block_subsidy_test)
{
    const auto mainchainParams = CreateChainParams(CBaseChainParams::MAIN);
    BOOST_CHECK_EQUAL(mainchainParams->GetConsensus().nSubsidyAccelerationFactor, 1);   // As in mainNet
    TestBlockSubsidyCurve(mainchainParams->GetConsensus());

    const auto testchainParams = CreateChainParams(CBaseChainParams::TESTNET);          // As in testNet
    BOOST_CHECK_EQUAL(testchainParams->GetConsensus().nSubsidyAccelerationFactor, 1);
    // no need to run the TestBlockSubsidyCurve for testNet as its similar to mainNet

    const auto regchainParams = CreateChainParams(CBaseChainParams::REGTEST);
    BOOST_CHECK_EQUAL(regchainParams->GetConsensus().nSubsidyAccelerationFactor, 1400); // As in regTest
    TestBlockSubsidyCurve(regchainParams->GetConsensus());
}

BOOST_AUTO_TEST_CASE(subsidy_limit_test)
{
    CAmount nSum = 0;
    const auto chainParams = CreateChainParams(CBaseChainParams::MAIN);
    //std::cout << "\n\n   subsidy_limit_test ( " << MAX_COINBASE <<" ) " << "   Acc.Factor = " << chainParams->GetConsensus().nSubsidyAccelerationFactor;

    for (int nHeight = 1; nHeight < 2500002; nHeight += 1000)
    {
        CAmount nSubsidy = GetBlockSubsidy(nHeight, chainParams->GetConsensus());
        nSum += nSubsidy;
        //std::cout << "\nnHeight = " << nHeight << "\tnSubsidy = " << nSubsidy << "\tnSum= " << nSum ;
        if (nHeight > 1 )
        {
            BOOST_CHECK(nSubsidy <= MAX_COINBASE);
        }
        BOOST_CHECK(MoneyRange(nSum));
    }
    BOOST_CHECK_EQUAL(nSum, 133094593368744ULL);  // Total mining rewards of block_1, block_1001, block_2001, .... block_2500001
}
#endif // END_BUILD

BOOST_AUTO_TEST_SUITE_END()


//
// Copyright (c) Leonid Seniukov. All rights reserved.
// Licensed under the MIT license. See LICENSE file in the project root for details.
//

#include <boost/filesystem.hpp>
#include "absolutePoseEstimation/rotationAveraging/RotationAverager.h"
#include "absolutePoseEstimation/rotationAveraging/RotationRobustOptimizerCreator.h"
#include "absolutePoseEstimation/translationAveraging/TranslationMeasurement.h"
#include "absolutePoseEstimation/rotationAveraging/RotationMeasurement.h"
#include "absolutePoseEstimation/translationAveraging/TranslationAverager.h"

#include "bundleAdjustment/BundleAdjuster.h"
#include "bundleAdjustment/BundleAdjusterCreator.h"

#include "sparsePointCloud/CloudProjectorCreator.h"
#include "sparsePointCloud/PointClassifierCreator.h"
#include "sparsePointCloud/ProjectableInfo.h"

#include "computationHandlers/AbsolutePosesComputationHandler.h"
#include "computationHandlers/TimerClockNow.h"

namespace gdr {

    void AbsolutePosesComputationHandler::computePointClasses() {

        std::vector<ProjectableInfo> posesForCloudProjector;
        posesForCloudProjector.reserve(getNumberOfPoses());

        for (int i = 0; i < getNumberOfPoses(); ++i) {
            const auto &pose = connectedComponent->getVertex(i);
            posesForCloudProjector.emplace_back(
                    ProjectableInfo(
                            pose.getAbsolutePoseSE3(),
                            pose.getCamera(),
                            pose.getIndex(),
                            pose.getPathRGBImage(),
                            pose.getPathDImage()
                    ));
        }

        cloudProjector = CloudProjectorCreator::getCloudProjector();
        cloudProjector->setCameraPoses(posesForCloudProjector);

        const auto &matchesBetweenPoints = connectedComponent->getInlierObservedPoints();
        for (const auto &vectorOfMatches: matchesBetweenPoints) {

            std::vector<std::pair<int, int>> poseAndLocalIndices;
            poseAndLocalIndices.push_back(vectorOfMatches.first.first);
            poseAndLocalIndices.push_back(vectorOfMatches.second.first);

            pointMatcher->insertPointsWithNewClasses(poseAndLocalIndices);
        }

        // unordered map's Key is local index
        std::vector<std::unordered_map<int, KeyPointInfo>>
                keyPointInfoByPoseNumAndLocalInd(pointMatcher->getNumberOfPoses());

        for (const auto &vectorOfMatches: matchesBetweenPoints) {

            for (int i = 0; i < 2; ++i) {
                const auto &fullPointInfo = (i == 0) ? (vectorOfMatches.first) : (vectorOfMatches.second);

                const auto &poseNumAndLocalInd = fullPointInfo.first;
                const auto &foundIt = keyPointInfoByPoseNumAndLocalInd[poseNumAndLocalInd.first].find(
                        poseNumAndLocalInd.second);
                if (foundIt != keyPointInfoByPoseNumAndLocalInd[poseNumAndLocalInd.first].end()) {
                    assert(foundIt->second == fullPointInfo.second);
                } else {
                    keyPointInfoByPoseNumAndLocalInd[poseNumAndLocalInd.first].insert(
                            std::make_pair(poseNumAndLocalInd.second, fullPointInfo.second));
                }
            }
        }

        auto pointClasses = pointMatcher->assignPointClasses();

        for (int pointIncrementor = 0; pointIncrementor < pointClasses.size(); ++pointIncrementor) {
            int pointClassNumber = pointClasses[pointIncrementor];
            std::pair<int, int> poseNumberAndLocalIndex = pointMatcher->getPoseNumberAndLocalIndex(pointIncrementor);
            std::vector<KeyPointInfo> keyPointInfo;
            keyPointInfo.emplace_back(
                    keyPointInfoByPoseNumAndLocalInd[poseNumberAndLocalIndex.first][poseNumberAndLocalIndex.second]);
            cloudProjector->addPoint(pointClassNumber, keyPointInfo);
        }
    }

    int AbsolutePosesComputationHandler::getNumberOfPoses() const {
        return connectedComponent->getNumberOfPoses();
    }

    std::vector<RotationMeasurement> AbsolutePosesComputationHandler::getRelativeRotationsVector() const {

        std::vector<RotationMeasurement> relativeRotationsToReturn;

        for (int indexFromDest = 0; indexFromDest < getPoseGraph().size(); ++indexFromDest) {
            for (const auto &relativePose: getPoseGraph().getRelativePosesFrom(indexFromDest)) {

                int indexToToBeTransformed = relativePose.getIndexTo();

                assert(indexFromDest == relativePose.getIndexFrom());

                relativeRotationsToReturn.emplace_back(RotationMeasurement(
                        relativePose.getRelativeRotation(), indexFromDest, indexToToBeTransformed
                ));
            }
        }

        return relativeRotationsToReturn;
    }

    std::vector<SO3> AbsolutePosesComputationHandler::performRotationAveraging() {

        timeStartRotationAveraging = timerGetClockTimeNow();
        std::vector<SO3> absoluteRotations = RotationAverager::shanonAveraging(
                getRelativeRotationsVector(),
                getIndexFixedPose(),
                getPathRelativePoseFile());

        for (int i = 0; i < getNumberOfPoses(); ++i) {
            connectedComponent->setRotation(i, SO3(absoluteRotations[i].getRotationSophus()));
        }

        timeEndRotationAveraging = timerGetClockTimeNow();

        return absoluteRotations;
    }

    std::vector<SO3> AbsolutePosesComputationHandler::performRotationRobustOptimization() {

        std::vector<SO3> shonanOptimizedAbsolutePoses;

        for (const auto &vertexPose: connectedComponent->getVertices()) {
            shonanOptimizedAbsolutePoses.emplace_back(SO3(vertexPose.getRotationQuat()));
        }

        assert(shonanOptimizedAbsolutePoses.size() == getNumberOfPoses());

        std::vector<RotationMeasurement> relativeRotationsAfterICP;

        for (int indexFrom = 0; indexFrom < getNumberOfPoses(); ++indexFrom) {
            for (const auto &knownRelativePose: connectedComponent->getConnectionsFromVertex(indexFrom)) {
                assert(indexFrom == knownRelativePose.getIndexFrom());

                if (knownRelativePose.getIndexFrom() < knownRelativePose.getIndexTo()) {
                    relativeRotationsAfterICP.emplace_back(
                            RotationMeasurement(knownRelativePose.getRelativeRotation(),
                                                knownRelativePose.getIndexFrom(),
                                                knownRelativePose.getIndexTo()));
                }
            }
        }

        timeStartRobustRotationOptimization = timerGetClockTimeNow();
        std::unique_ptr<RotationRobustOptimizer> rotationOptimizer =
                RotationRobustOptimizerCreator::getRefiner(
                        RotationRobustOptimizerCreator::RobustParameterType::DEFAULT);

        std::vector<SO3> optimizedPosesRobust =
                rotationOptimizer->getOptimizedOrientation(shonanOptimizedAbsolutePoses,
                                                           relativeRotationsAfterICP,
                                                           connectedComponent->getPoseIndexWithMaxConnectivity());
        timeEndRobustRotationOptimization = timerGetClockTimeNow();

        assert(getNumberOfPoses() == optimizedPosesRobust.size());

        for (int i = 0; i < getNumberOfPoses(); ++i) {
            connectedComponent->setRotation(i, optimizedPosesRobust[i]);
        }

        return optimizedPosesRobust;

    }

    std::vector<Eigen::Vector3d> AbsolutePosesComputationHandler::performTranslationAveraging() {

        std::vector<TranslationMeasurement> relativeTranslations;
        std::vector<SE3> absolutePoses = connectedComponent->getPoses();
        int indexFixedToZero = connectedComponent->getPoseIndexWithMaxConnectivity();

        for (int indexFrom = 0; indexFrom < getNumberOfPoses(); ++indexFrom) {
            for (const auto &knownRelativePose: connectedComponent->getConnectionsFromVertex(indexFrom)) {
                assert(indexFrom == knownRelativePose.getIndexFrom());

                if (knownRelativePose.getIndexFrom() < knownRelativePose.getIndexTo()) {
                    relativeTranslations.emplace_back(
                            TranslationMeasurement(knownRelativePose.getRelativeTranslation(),
                                                   knownRelativePose.getIndexFrom(),
                                                   knownRelativePose.getIndexTo()));
                }
            }
        }

        timeStartTranslationAveraging = timerGetClockTimeNow();

        std::vector<Eigen::Vector3d> optimizedAbsoluteTranslationsIRLS = TranslationAverager::recoverTranslations(
                relativeTranslations,
                absolutePoses,
                indexFixedToZero).toVectorOfVectors();

        assert(optimizedAbsoluteTranslationsIRLS[indexFixedToZero].norm() < std::numeric_limits<double>::epsilon());

        bool successIRLS = true;

        // Now run IRLS with PCG answer as init solution
        optimizedAbsoluteTranslationsIRLS = TranslationAverager::recoverTranslationsIRLS(
                relativeTranslations,
                absolutePoses,
                optimizedAbsoluteTranslationsIRLS,
                indexFixedToZero,
                successIRLS).toVectorOfVectors();

        timeEndTranslationAveraging = timerGetClockTimeNow();

        assert(optimizedAbsoluteTranslationsIRLS[indexFixedToZero].norm() < std::numeric_limits<double>::epsilon());

        assert(getNumberOfPoses() == optimizedAbsoluteTranslationsIRLS.size());

        for (int i = 0; i < getNumberOfPoses(); ++i) {
            connectedComponent->setTranslation(i, optimizedAbsoluteTranslationsIRLS[i]);
        }

        return optimizedAbsoluteTranslationsIRLS;
    }

    std::vector<SE3> AbsolutePosesComputationHandler::performBundleAdjustmentUsingDepth() {
        int maxNumberOfPointsToShow = -1;
        computePointClasses();

        int indexFixedToZero = connectedComponent->getPoseIndexWithMaxConnectivity();
        std::vector<Point3d> observedPoints = cloudProjector->computedPointsGlobalCoordinates();
        std::vector<std::pair<SE3, CameraRGBD>> posesAndCameraParams;

        assert(!connectedComponent->getVertices().empty());
        for (const auto &vertexPose: connectedComponent->getVertices()) {
            posesAndCameraParams.emplace_back(std::make_pair(vertexPose.getAbsolutePoseSE3(), vertexPose.getCamera()));
        }

        std::vector<double> errorsBefore;
        std::vector<cv::Mat> shownResidualsBefore;

        if (saveDebugImages) {
            shownResidualsBefore = cloudProjector->showPointsReprojectionError(observedPoints,
                                                                               "before",
                                                                               errorsBefore,
                                                                               connectedComponent->getVertex(
                                                                                0).getCamera(),
                                                                               maxNumberOfPointsToShow);
        }

        timeStartBundleAdjustment = timerGetClockTimeNow();

        std::unique_ptr<BundleAdjuster> bundleAdjuster =
                BundleAdjusterCreator::getBundleAdjuster(BundleAdjusterCreator::BundleAdjustmentType::USE_DEPTH_INFO);

        bool isUsableBA = true;
        std::vector<SE3> posesOptimized = bundleAdjuster->optimizePointsAndPoses(observedPoints,
                                                                                posesAndCameraParams,
                                                                                cloudProjector->getKeyPointInfoByPoseNumberAndPointClass(),
                                                                                indexFixedToZero,
                                                                                isUsableBA);

        timeEndBundleAdjustment = timerGetClockTimeNow();

        assert(posesOptimized.size() == getNumberOfPoses());

        for (int i = 0; i < getNumberOfPoses(); ++i) {
            connectedComponent->setPoseSE3(i, posesOptimized[i]);
        }

        std::vector<double> errorsAfter;

        cloudProjector->setPoses(connectedComponent->getPoses());
        cloudProjector->setPoints(bundleAdjuster->getOptimizedPoints());

        if (saveDebugImages) {
            auto shownResidualsAfter = cloudProjector->showPointsReprojectionError(observedPoints,
                                                                                "after",
                                                                                errorsAfter,
                                                                                connectedComponent->getVertex(
                                                                                0).getCamera(),
                                                                                maxNumberOfPointsToShow);

            assert(shownResidualsAfter.size() == shownResidualsBefore.size());

            std::string pathToRGBDirectoryToSave = "shownResiduals";
            boost::filesystem::path pathToRemove(pathToRGBDirectoryToSave);

            boost::filesystem::remove_all(pathToRemove);
            boost::filesystem::create_directories(pathToRemove);

            int counterMedianErrorGotWorse = 0;
            int counterMedianErrorGotBetter = 0;
            int counterSumMedianError = 0;

            for (int i = 0; i < shownResidualsAfter.size(); ++i) {

                boost::filesystem::path pathToSave = pathToRemove;
                bool medianErrorGotLessAfterBA = (errorsBefore[i] > errorsAfter[i]);

                if (medianErrorGotLessAfterBA) {
                    ++counterMedianErrorGotBetter;
                } else {
                    ++counterMedianErrorGotWorse;
                }
                ++counterSumMedianError;

                std::string betterOrWorseResults = medianErrorGotLessAfterBA ? " " : " [WORSE] ";
                std::string nameImageReprojErrors =
                        std::to_string(i) + betterOrWorseResults + " quantils: " + std::to_string(errorsBefore[i]) +
                        " -> " + std::to_string(errorsAfter[i]) + ".png";
                pathToSave.append(nameImageReprojErrors);
                cv::Mat stitchedImageResiduals;
                std::vector<cv::DMatch> matches1to2;
                std::vector<cv::KeyPoint> keyPointsToShowFirst;
                std::vector<cv::KeyPoint> keyPointsToShowSecond;
                cv::Mat stitchedImage;
                cv::drawMatches(shownResidualsBefore[i], {},
                                shownResidualsAfter[i], {},
                                matches1to2,
                                stitchedImage);
                cv::imwrite(pathToSave.string(), stitchedImage);
            }

            assert(counterMedianErrorGotWorse + counterMedianErrorGotBetter == counterSumMedianError);
        }

        return posesOptimized;
    }

    AbsolutePosesComputationHandler::AbsolutePosesComputationHandler(
            std::unique_ptr<ConnectedComponentPoseGraph> &connectedComponentPoseGraph) {

        connectedComponent = std::move(connectedComponentPoseGraph);
        pointMatcher = PointClassifierCreator::getPointClassifier();
        pointMatcher->setNumberOfPoses(getNumberOfPoses());
    }

    std::set<int> AbsolutePosesComputationHandler::initialIndices() const {
        return connectedComponent->initialIndices();
    }

    const std::vector<VertexPose> &AbsolutePosesComputationHandler::getVertices() const {
        return connectedComponent->getVertices();
    }

    bool AbsolutePosesComputationHandler::getSaveDebugImages() const {
        return saveDebugImages;
    }

    void AbsolutePosesComputationHandler::setSaveDebugImages(bool saveImages) {
        saveDebugImages = saveImages;
    }

    std::vector<double> AbsolutePosesComputationHandler::getPosesTimestamps() const {
        std::vector<double> timestampsToReturn;

        const auto &vertices = getVertices();

        for (const auto &vertex: vertices) {
            timestampsToReturn.emplace_back(vertex.getTimestamp());
        }

        assert(timestampsToReturn.size() == getNumberOfPoses());
        assert(!timestampsToReturn.empty());

        return timestampsToReturn;
    }

    std::vector<SE3> AbsolutePosesComputationHandler::getPosesSE3() const {
        std::vector<SE3> posesSE3;

        for (const auto &vertex: getVertices()) {
            posesSE3.emplace_back(vertex.getAbsolutePoseSE3());
        }

        assert(posesSE3.size() == getNumberOfPoses());
        return posesSE3;
    }

    const PoseGraph &AbsolutePosesComputationHandler::getPoseGraph() const {
        return connectedComponent->getPoseGraph();
    }

    int AbsolutePosesComputationHandler::getIndexFixedPose() const {
        return connectedComponent->getPoseIndexWithMaxConnectivity();
    }

    PosesForEvaluation AbsolutePosesComputationHandler::getPosesForEvaluation() const {
        return connectedComponent->getPosesForEvaluation();
    }

    PosesForEvaluation AbsolutePosesComputationHandler::getPosesForEvaluation(const SE3 &poseFixedInversed) const {
        return connectedComponent->getPosesForEvaluation(poseFixedInversed);
    }

    void AbsolutePosesComputationHandler::setRelativePosesFilePath(const std::string &pathRelPoses) {
        pathRelativePosesFile = pathRelPoses;
    }

    std::string AbsolutePosesComputationHandler::getPathRelativePoseFile() const {
        return pathRelativePosesFile;
    }

    std::stringstream AbsolutePosesComputationHandler::printTimeBenchmarkInfo() const {
        std::chrono::duration<double> timeRotationAveraging = std::chrono::duration_cast<std::chrono::duration<double>>(
                timeEndRotationAveraging - timeStartRotationAveraging);
        std::chrono::duration<double> timeRotationRobust = std::chrono::duration_cast<std::chrono::duration<double>>(
                timeEndRobustRotationOptimization - timeStartRobustRotationOptimization);
        std::chrono::duration<double> timeTranslationAveraging = std::chrono::duration_cast<std::chrono::duration<double>>(
                timeEndTranslationAveraging - timeStartTranslationAveraging);
        std::chrono::duration<double> timeBundleAdjustment = std::chrono::duration_cast<std::chrono::duration<double>>(
                timeEndBundleAdjustment - timeStartBundleAdjustment);

        std::stringstream benchmarkTimeInfo;

        benchmarkTimeInfo << "          Rotation Averaging: " << timeRotationAveraging.count() << std::endl;
        benchmarkTimeInfo << "          Robust Rotation Optimization: " << timeRotationRobust.count() << std::endl;
        benchmarkTimeInfo << "          Translation Averaging: " << timeTranslationAveraging.count() << std::endl;
        benchmarkTimeInfo << "          Bundle Adjustment: " << timeBundleAdjustment.count() << std::endl;

        return benchmarkTimeInfo;
    }
}


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

#ifndef NETKET_DIAGONALDENSITYMATRIX_HPP
#define NETKET_DIAGONALDENSITYMATRIX_HPP

#include <Eigen/Dense>
#include <algorithm>
#include <complex>
#include <fstream>
#include <utility>
#include <vector>
#include "Machine/abstract_machine.hpp"
#include "Utils/lookup.hpp"
#include "Utils/random_utils.hpp"
#include "density_matrix_machine.hpp"

namespace netket {
class DiagonalDensityMatrix : public AbstractMachine {
  using VisibleChangeInfo = std::pair<std::vector<int>, std::vector<double>>;

  AbstractDensityMatrix &density_matrix_;

 public:
  explicit DiagonalDensityMatrix(AbstractDensityMatrix &dm)
      : AbstractMachine(dm.GetHilbertPhysicalShared()), density_matrix_(dm){};

  const AbstractDensityMatrix &GetFullDensityMatrix() const noexcept {
    return density_matrix_;
  }

 private:
  /**
   * Doubles a visible configuration v so that it represents an element on the
   * diagonal of a density matrix.
   * @param v is a visible configuration
   * @return the vertical concatentation of [v, v]
   */
  VisibleType DoubleVisibleConfig(const VisibleConstType v) const {
    VisibleType v2(2 * v.rows());
    v2.head(v.rows()) = v;
    v2.tail(v.rows()) = v;

    return v2;
  }

  VisibleChangeInfo DoubleVisibleChangeInfo(const std::vector<int> &tochange,
                                            const std::vector<double> &newconf,
                                            int offset) const {
    std::vector<int> tochange_doubled(tochange.size() * 2);
    std::vector<double> newconf_doubled(newconf.size() * 2);

    // Copy tochange on the first half of tochange_doubled and copy + offset on
    // the other half.
    std::copy(tochange.begin(), tochange.end(), tochange_doubled.begin());
    std::copy(tochange.begin(), tochange.end(),
              tochange_doubled.begin() + tochange.size());
    for (auto tcd = tochange_doubled.begin() + tochange.size();
         tcd != tochange_doubled.end(); ++tcd) {
      *tcd += offset;
    }

    std::copy(newconf.begin(), newconf.end(), newconf_doubled.begin());
    std::copy(newconf.begin(), newconf.end(),
              newconf_doubled.begin() + newconf.size());

    return VisibleChangeInfo(tochange_doubled, newconf_doubled);
  };

 public:
  Complex LogVal(VisibleConstType v) override {
    return density_matrix_.LogVal(DoubleVisibleConfig(v));
  }

  Complex LogVal(VisibleConstType v, const LookupType &lt) override {
    return density_matrix_.LogVal(DoubleVisibleConfig(v), lt);
  }

  void InitLookup(VisibleConstType v, LookupType &lt) override {
    return density_matrix_.InitLookup(DoubleVisibleConfig(v), lt);
  }

  void UpdateLookup(VisibleConstType v, const std::vector<int> &tochange,
                    const std::vector<double> &newconf,
                    LookupType &lt) override {
    VisibleChangeInfo d_changes =
        DoubleVisibleChangeInfo(tochange, newconf, v.size());
    return density_matrix_.UpdateLookup(DoubleVisibleConfig(v), d_changes.first,
                                        d_changes.second, lt);
  }

  VectorType LogValDiff(
      VisibleConstType v, const std::vector<std::vector<int>> &tochange,
      const std::vector<std::vector<double>> &newconf) override {
    auto tochange_d = std::vector<std::vector<int>>(tochange.size());
    auto newconf_d = std::vector<std::vector<double>>(newconf.size());
    // double every element in tochange and newconf
    {
      auto tc = tochange.begin();
      auto nc = newconf.begin();
      int i = 0;
      while (tc != tochange.end()) {
        auto d_changes = DoubleVisibleChangeInfo(*tc, *nc, v.size());
        tochange_d[i] = d_changes.first;
        newconf_d[i] = d_changes.second;
      }
    }
    return density_matrix_.LogValDiff(DoubleVisibleConfig(v), tochange_d,
                                      newconf_d);
  }

  Complex LogValDiff(VisibleConstType v, const std::vector<int> &tochange,
                     const std::vector<double> &newconf,
                     const LookupType &lt) override {
    VisibleChangeInfo d_changes =
        DoubleVisibleChangeInfo(tochange, newconf, v.size());
    return density_matrix_.LogValDiff(DoubleVisibleConfig(v), d_changes.first,
                                      d_changes.second, lt);
  }

  VectorType DerLog(VisibleConstType v) override {
    return density_matrix_.DerLog(DoubleVisibleConfig(v));
  }

  VectorType DerLog(VisibleConstType v, const LookupType &lt) override {
    return density_matrix_.DerLog(DoubleVisibleConfig(v), lt);
  }

  VectorType DerLogChanged(VisibleConstType v, const std::vector<int> &tochange,
                           const std::vector<double> &newconf) override {
    VisibleChangeInfo d_changes =
        DoubleVisibleChangeInfo(tochange, newconf, v.size());
    return density_matrix_.DerLogChanged(DoubleVisibleConfig(v),
                                         d_changes.first, d_changes.second);
  }

  int Npar() const override { return density_matrix_.Npar(); }

  VectorType GetParameters() override {
    return density_matrix_.GetParameters();
  }

  void SetParameters(VectorConstRefType pars) override {
    return density_matrix_.SetParameters(pars);
  }

  void InitRandomPars(int seed, double sigma) override {
    return density_matrix_.InitRandomPars(seed, sigma);
  }

  int Nvisible() const override { return density_matrix_.Nvisible(); }

  bool IsHolomorphic() const noexcept override {
    return density_matrix_.IsHolomorphic();
  }

  void Save(const std::string &filename) const override {
    return density_matrix_.Save(filename);
  }

  void Load(const std::string &filename) override {
    return density_matrix_.Load(filename);
  }
};
}  // namespace netket

#endif  // NETKET_DIAGONALDENSITYMATRIX_HPP


/*

Copyright (c) 2005-2021, University of Oxford.
All rights reserved.

University of Oxford means the Chancellor, Masters and Scholars of the
University of Oxford, having an administrative office at Wellington
Square, Oxford OX1 2JD, UK.

This file is part of Chaste.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
 * Redistributions of source code must retain the above copyright notice,
   this list of conditions and the following disclaimer.
 * Redistributions in binary form must reproduce the above copyright notice,
   this list of conditions and the following disclaimer in the documentation
   and/or other materials provided with the distribution.
 * Neither the name of the University of Oxford nor the names of its
   contributors may be used to endorse or promote products derived from this
   software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

*/

#include "ToroidalHoneycombMeshGenerator.hpp"

#include <boost/foreach.hpp>
#include "RandomNumberGenerator.hpp"
#include "MathsCustomFunctions.hpp"
#include "ChasteSyscalls.hpp"

ToroidalHoneycombMeshGenerator::ToroidalHoneycombMeshGenerator(unsigned numNodesAlongWidth, unsigned numNodesAlongDepth, double widthScaleFactor, double depthScaleFactor)
{
    mpMesh = nullptr;
    mDomainWidth = numNodesAlongWidth*widthScaleFactor;
    mDomainDepth = 0.5*sqrt(3)*numNodesAlongDepth*depthScaleFactor;
    mNumCellWidth = numNodesAlongWidth; //*1 because cells are considered to be size one
    mNumCellLength = numNodesAlongDepth;
    mMeshFilename = "mesh";

    // The code below won't work in parallel
    assert(PetscTools::IsSequential());

    // An older version of the constructor might allow the wrong argument through to the scale factor
    assert(widthScaleFactor > 0.0);
    assert(depthScaleFactor > 0.0);

    // Get a unique mesh filename
    std::stringstream pid;
    pid << getpid();

    OutputFileHandler output_file_handler("2D_temporary_honeycomb_mesh_"+ pid.str());

    unsigned num_nodes_along_width = mNumCellWidth;
    unsigned num_nodes_along_depth = mNumCellLength;
    double horizontal_spacing = mDomainWidth / (double)num_nodes_along_width;
    double vertical_spacing = mDomainDepth / (double)num_nodes_along_depth;

    unsigned num_nodes            = num_nodes_along_width*num_nodes_along_depth;
    unsigned num_elem_along_width = num_nodes_along_width-1;
    unsigned num_elem_along_depth = num_nodes_along_depth-1;
    unsigned num_elem             = 2*num_elem_along_width*num_elem_along_depth;
    unsigned num_edges            = 3*num_elem_along_width*num_elem_along_depth + num_elem_along_width + num_elem_along_depth;

    double x0 = 0.0;
    double y0 = 0.0;

    // Write node file
    out_stream p_node_file = output_file_handler.OpenOutputFile(mMeshFilename+".node");
    (*p_node_file) << std::scientific;
    //(*p_node_file) << std::setprecision(20);
    (*p_node_file) << num_nodes << "\t2\t0\t1" << std::endl;

    unsigned node = 0;
    for (unsigned i=0; i<num_nodes_along_depth; i++)
    {
        for (unsigned j=0; j<num_nodes_along_width; j++)
        {
            unsigned boundary = 0;
            if ((i==0) || (i==num_nodes_along_depth-1))
            {
                boundary = 1;
            }

            double x = x0 + horizontal_spacing*((double)j + 0.25*(1.0+ SmallPow(-1.0,i+1)));
            double y = y0 + vertical_spacing*(double)i;

            // Avoid floating point errors which upset OffLatticeSimulation
            if ((y<0.0) && (y>-1e-12))
            {
                // Difficult to cover - just corrects floating point errors that have occurred from time to time!
                // LCOV_EXCL_START
                y = 0.0;
                // LCOV_EXCL_STOP
            }

            (*p_node_file) << node++ << "\t" << x << "\t" << y << "\t" << boundary << std::endl;
        }
    }
    p_node_file->close();

    // Write element file and edge file
    out_stream p_elem_file = output_file_handler.OpenOutputFile(mMeshFilename+".ele");
    (*p_elem_file) << std::scientific;

    out_stream p_edge_file = output_file_handler.OpenOutputFile(mMeshFilename+".edge");
    (*p_node_file) << std::scientific;

    (*p_elem_file) << num_elem << "\t3\t0" << std::endl;
    (*p_edge_file) << num_edges << "\t1" << std::endl;

    unsigned elem = 0;
    unsigned edge = 0;
    for (unsigned i=0; i<num_elem_along_depth; i++)
    {
        for (unsigned j=0; j < num_elem_along_width; j++)
        {
            unsigned node0 =     i*num_nodes_along_width + j;
            unsigned node1 =     i*num_nodes_along_width + j+1;
            unsigned node2 = (i+1)*num_nodes_along_width + j;

            if (i%2 != 0)
            {
                node2 = node2 + 1;
            }

            (*p_elem_file) << elem++ << "\t" << node0 << "\t" << node1 << "\t" << node2 << std::endl;

            unsigned horizontal_edge_is_boundary_edge = 0;
            unsigned vertical_edge_is_boundary_edge = 0;
            if (i==0)
            {
                horizontal_edge_is_boundary_edge = 1;
            }

            (*p_edge_file) << edge++ << "\t" << node0 << "\t" << node1 << "\t" << horizontal_edge_is_boundary_edge << std::endl;
            (*p_edge_file) << edge++ << "\t" << node1 << "\t" << node2 << "\t" << 0 << std::endl;
            (*p_edge_file) << edge++ << "\t" << node2 << "\t" << node0 << "\t" << vertical_edge_is_boundary_edge << std::endl;

            node0 = i*num_nodes_along_width + j + 1;

            if (i%2 != 0)
            {
                node0 = node0 - 1;
            }
            node1 = (i+1)*num_nodes_along_width + j+1;
            node2 = (i+1)*num_nodes_along_width + j;

            (*p_elem_file) << elem++ << "\t" << node0 << "\t" << node1 << "\t" << node2 << std::endl;
        }
    }

    for (unsigned i=0; i<num_elem_along_depth; i++)
    {
        unsigned node0, node1;

        if (i%2==0)
        {
             node0 = (i+1)*num_nodes_along_width - 1;
             node1 = (i+2)*num_nodes_along_width - 1;
        }
        else
        {
            node0 = (i+1)*num_nodes_along_width;
            node1 = (i)*num_nodes_along_width;
        }
        (*p_edge_file) << edge++ << "\t" << node0 << "\t" << node1 << "\t" << 1 << std::endl;
    }

    for (unsigned j=0; j<num_elem_along_width; j++)
    {
        unsigned node0 = num_nodes_along_width*(num_nodes_along_depth-1) + j;
        unsigned node1 = num_nodes_along_width*(num_nodes_along_depth-1) + j+1;
        (*p_edge_file) << edge++ << "\t" << node1 << "\t" << node0 << "\t" << 1 << std::endl;
    }

    p_elem_file->close();
    p_edge_file->close();

    // Having written the mesh to file, now construct it using TrianglesMeshReader.
    // Nested scope so the reader closes files before we delete them below.
    {
        TrianglesMeshReader<2,2> mesh_reader(output_file_handler.GetOutputDirectoryFullPath() + mMeshFilename);
        mpMesh = new Toroidal2dMesh(mDomainWidth,mDomainDepth);
        mpMesh->ConstructFromMeshReader(mesh_reader);
    }

    // Make the mesh Toroidal (we use Triangle library mode inside this ReMesh call)
    mpMesh->ReMesh();

    // Delete the temporary files
    output_file_handler.FindFile("").Remove();

    // The original files have been deleted, it is better if the mesh object forgets about them
    mpMesh->SetMeshHasChangedSinceLoading();
}

MutableMesh<2,2>* ToroidalHoneycombMeshGenerator::GetMesh()
{
    EXCEPTION("A Toroidal mesh was created but a normal mesh is being requested.");
    return mpMesh; // Not really
}

Toroidal2dMesh* ToroidalHoneycombMeshGenerator::GetToroidalMesh()
{
    return (Toroidal2dMesh*) mpMesh;
}