// 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 NQS_METROPOLISEXCHANGEPT_HPP
#define NQS_METROPOLISEXCHANGEPT_HPP

#include <mpi.h>
#include <Eigen/Dense>
#include <iostream>
#include "Utils/parallel_utils.hpp"
#include "Utils/random_utils.hpp"
#include "abstract_sampler.hpp"

namespace nqs {

// Metropolis sampling generating local exchanges
// Parallel tempering is also used
class MetropolisExchangePt : public AbstractSampler {
  // number of visible units
  const int nv_;

  const int nrep_;
  std::vector<double> beta_;

  // states of visible units
  // for each sampled temperature
  std::vector<Eigen::VectorXd> v_;

  Eigen::VectorXd accept_;
  Eigen::VectorXd moves_;

  int mynode_;
  int totalnodes_;

  // clusters to do updates
  std::vector<std::vector<int>> clusters_;

  // Look-up tables
  std::vector<typename AbstractMachine::LookupType> lt_;

  int sweep_size_;

 public:
  MetropolisExchangePt(const AbstractGraph &graph, AbstractMachine &psi,
                       int dmax = 1, int nreplicas = 1)
      : AbstractSampler(psi), nv_(GetHilbert().Size()), nrep_(nreplicas) {
    Init(graph, dmax);
  }

  void Init(const AbstractGraph &graph, int dmax) {
    MPI_Comm_size(MPI_COMM_WORLD, &totalnodes_);
    MPI_Comm_rank(MPI_COMM_WORLD, &mynode_);

    v_.resize(nrep_);
    for (int i = 0; i < nrep_; i++) {
      v_[i].resize(nv_);
    }

    for (int i = 0; i < nrep_; i++) {
      beta_.push_back(1. - double(i) / double(nrep_));
    }

    lt_.resize(nrep_);

    accept_.resize(2 * nrep_);
    moves_.resize(2 * nrep_);

    GenerateClusters(graph, dmax);

    Reset(true);

    // Always use odd sweep size to avoid possible ergodicity problems
    if (nv_ % 2 == 0) {
      sweep_size_ = nv_ + 1;
    } else {
      sweep_size_ = nv_;
    }

    InfoMessage() << "Metropolis sampler with parallel tempering is ready "
                  << std::endl;
    InfoMessage() << nrep_ << " replicas are being used" << std::endl;
    InfoMessage() << dmax << " is the maximum distance for exchanges"
                  << std::endl;
  }

  template <class Graph>
  void GenerateClusters(Graph &graph, int dmax) {
    auto dist = graph.AllDistances();

    assert(int(dist.size()) == nv_);

    for (int i = 0; i < nv_; i++) {
      for (int j = 0; j < nv_; j++) {
        if (dist[i][j] <= dmax && i != j) {
          clusters_.push_back({i, j});
        }
      }
    }
  }

  void Reset(bool initrandom = false) override {
    if (initrandom) {
      for (int i = 0; i < nrep_; i++) {
        GetHilbert().RandomVals(v_[i], this->GetRandomEngine());
      }
    }

    for (int i = 0; i < nrep_; i++) {
      GetMachine().InitLookup(v_[i], lt_[i]);
    }

    accept_ = Eigen::VectorXd::Zero(2 * nrep_);
    moves_ = Eigen::VectorXd::Zero(2 * nrep_);
  }

  // Exchange sweep at given temperature
  void LocalExchangeSweep(int rep) {
    std::vector<int> tochange(2);
    std::uniform_real_distribution<double> distu;
    std::uniform_int_distribution<int> distcl(0, clusters_.size() - 1);

    std::vector<double> newconf(2);

    for (int i = 0; i < sweep_size_; i++) {
      int rcl = distcl(this->GetRandomEngine());
      assert(rcl < int(clusters_.size()));
      int si = clusters_[rcl][0];
      int sj = clusters_[rcl][1];

      assert(si < nv_ && sj < nv_);

      if (std::abs(v_[rep](si) - v_[rep](sj)) >
          std::numeric_limits<double>::epsilon()) {
        tochange = clusters_[rcl];
        newconf[0] = v_[rep](sj);
        newconf[1] = v_[rep](si);

        auto explo =
            std::exp(beta_[rep] * GetMachine().LogValDiff(v_[rep], tochange,
                                                          newconf, lt_[rep]));
        double ratio = this->GetMachineFunc()(explo);

        if (ratio > distu(this->GetRandomEngine())) {
          accept_(rep) += 1;
          GetMachine().UpdateLookup(v_[rep], tochange, newconf, lt_[rep]);
          GetHilbert().UpdateConf(v_[rep], tochange, newconf);
        }
      }

      moves_(rep) += 1;
    }
  }

  void Sweep() override {
    // First we do local exchange sweeps
    for (int i = 0; i < nrep_; i++) {
      LocalExchangeSweep(i);
    }

    // Tempearture exchanges
    std::uniform_real_distribution<double> distribution(0, 1);

    for (int r = 1; r < nrep_; r += 2) {
      if (ExchangeProb(r, r - 1) > distribution(this->GetRandomEngine())) {
        Exchange(r, r - 1);
        accept_(nrep_ + r) += 1.;
        accept_(nrep_ + r - 1) += 1;
      }
      moves_(nrep_ + r) += 1.;
      moves_(nrep_ + r - 1) += 1;
    }

    for (int r = 2; r < nrep_; r += 2) {
      if (ExchangeProb(r, r - 1) > distribution(this->GetRandomEngine())) {
        Exchange(r, r - 1);
        accept_(nrep_ + r) += 1.;
        accept_(nrep_ + r - 1) += 1;
      }
      moves_(nrep_ + r) += 1.;
      moves_(nrep_ + r - 1) += 1;
    }
  }

  // computes the probability to exchange two replicas
  double ExchangeProb(int r1, int r2) {
    const double lf1 = 2 * std::real(GetMachine().LogVal(v_[r1], lt_[r1]));
    const double lf2 = 2 * std::real(GetMachine().LogVal(v_[r2], lt_[r2]));

    return std::exp((beta_[r1] - beta_[r2]) * (lf2 - lf1));
  }

  void Exchange(int r1, int r2) {
    std::swap(v_[r1], v_[r2]);
    std::swap(lt_[r1], lt_[r2]);
  }

  const Eigen::VectorXd &Visible() const noexcept override { return v_[0]; }

  void SetVisible(const Eigen::VectorXd &v) override { v_[0] = v; }

  AbstractMachine::VectorType DerLogVisible() override {
    return GetMachine().DerLog(v_[0], lt_[0]);
  }

  Eigen::VectorXd Acceptance() const override {
    Eigen::VectorXd acc = accept_;
    for (int i = 0; i < acc.size(); i++) {
      acc(i) /= moves_(i);
    }
    return acc;
  }
};

}  // namespace nqs

#endif


/*
 * (C) Copyright 2020 Met Office UK
 *
 * This software is licensed under the terms of the Apache Licence Version 2.0
 * which can be obtained at http://www.apache.org/licenses/LICENSE-2.0.
 */

#include "ufo/filters/MetOfficeBuddyPairFinder.h"

#include <algorithm>

#include "oops/util/Logger.h"
#include "ufo/filters/MetOfficeBuddyCheckParameters.h"
#include "ufo/filters/MetOfficeBuddyCollectorV1.h"
#include "ufo/filters/MetOfficeBuddyCollectorV2.h"
#include "ufo/utils/RecursiveSplitter.h"

#include <boost/make_unique.hpp>

namespace ufo {

namespace {

float zonalBandWidth(int numBands) {
  return 180.0f / numBands;
}

}  // namespace

MetOfficeBuddyPairFinder::MetOfficeBuddyPairFinder(const MetOfficeBuddyCheckParameters &options,
                           const std::vector<float> &latitudes,
                           const std::vector<float> &longitudes,
                           const std::vector<util::DateTime> &datetimes,
                           const std::vector<float> *pressures,
                           const std::vector<int> &stationIds)
  : options_(options), latitudes_(latitudes), longitudes_(longitudes),
    datetimes_(datetimes), pressures_(pressures), stationIds_(stationIds)
{}

std::vector<MetOfficeBuddyPair> MetOfficeBuddyPairFinder::findBuddyPairs(
    const std::vector<size_t> & validObsIds) {
  std::vector<int> validObsIdsInSortOrder;
  std::vector<int> bandLbounds;
  sortObservations(validObsIds, validObsIdsInSortOrder, bandLbounds);
  return pairObservations(validObsIdsInSortOrder, bandLbounds);
}

void MetOfficeBuddyPairFinder::sortObservations(const std::vector<size_t> & validObsIds,
                                                std::vector<int> &validObsIdsInSortOrder,
                                                std::vector<int> &bandLbounds)
{
  // Initialize output parameters

  validObsIdsInSortOrder.clear();
  validObsIdsInSortOrder.reserve(validObsIds.size());
  bandLbounds.assign(options_.numZonalBands + 1, 0);

  // Identify the band containing each valid observation

  const float bandWidth = zonalBandWidth(options_.numZonalBands);
  std::vector<int> bandIndices(validObsIds.size());
  for (size_t validObsIndex = 0; validObsIndex < validObsIds.size(); ++validObsIndex) {
    const size_t obsId = validObsIds[validObsIndex];
    // Use 89.999 to round up values, e.g. ranges are 70.0 to 74.999
    // (There may be a more elegant way to do this, but use this one for compatibility with OPS.)
    bandIndices[validObsIndex] =
        std::max(0, static_cast<int>((89.999f - latitudes_[obsId]) / bandWidth));
  }

  // Sort observations

  RecursiveSplitter splitter(validObsIds.size());
  splitter.groupBy(bandIndices);
  splitter.sortGroupsBy(
        [this, &validObsIds](size_t obsIndexA, size_t obsIndexB)
        {
          size_t obsIdA = validObsIds[obsIndexA];
          size_t obsIdB = validObsIds[obsIndexB];
          if (pressures_ != nullptr)
            return std::make_tuple(longitudes_[obsIdA], -latitudes_[obsIdA],
                                   (*pressures_)[obsIdA], datetimes_[obsIdA]) <
                   std::make_tuple(longitudes_[obsIdB], -latitudes_[obsIdB],
                                   (*pressures_)[obsIdB], datetimes_[obsIdB]);
          else
            return std::make_tuple(longitudes_[obsIdA], -latitudes_[obsIdA], datetimes_[obsIdA]) <
                   std::make_tuple(longitudes_[obsIdB], -latitudes_[obsIdB], datetimes_[obsIdB]);
        });

  // Fill the validObsIdsInSortOrder and bandLbounds vectors

  int currentBandIndex = -1;
  for (auto group : splitter.groups()) {
    for (size_t validObsIndex : group) {
      const size_t obsId = validObsIds[validObsIndex];
      for (int band = currentBandIndex + 1; band <= bandIndices[validObsIndex]; ++band) {
        bandLbounds[band] = validObsIdsInSortOrder.size();
      }
      currentBandIndex = bandIndices[validObsIndex];
      validObsIdsInSortOrder.push_back(obsId);
    }
  }
  for (int band = currentBandIndex + 1; band < bandLbounds.size(); ++band) {
    bandLbounds[band] = validObsIds.size();
  }

  oops::Log::trace() << "Buddy check: " << validObsIds.size() << " input observations" << std::endl;
}

std::vector<MetOfficeBuddyPair> MetOfficeBuddyPairFinder::pairObservations(
    const std::vector<int> &validObsIdsInSortOrder,
    const std::vector<int> &bandLbounds) {

  std::vector<MetOfficeBuddyPair> pairs;

  // Initialise variables
  const float bandWidth = zonalBandWidth(options_.numZonalBands);
  // eqn 3.1
  const float searchDLat = Constants::rad2deg * options_.searchRadius / Constants::mean_earth_rad;
  const float searchDLatB = searchDLat + 0.5f * bandWidth;
  const int numSearchBands = static_cast<int>(searchDLat / bandWidth) + 1;

  typedef std::vector<int>::const_iterator ObsIdIt;
  typedef std::vector<int>::const_reverse_iterator ObsIdRevIt;

  std::vector<ObsIdIt> bandBegins(options_.numZonalBands);
  std::vector<ObsIdIt> bandEnds(options_.numZonalBands);

  for (int bandIndex = 0; bandIndex < options_.numZonalBands; ++bandIndex) {
    bandBegins[bandIndex] = validObsIdsInSortOrder.begin() + bandLbounds[bandIndex];
    bandEnds[bandIndex] = validObsIdsInSortOrder.begin() + bandLbounds[bandIndex + 1];
  }

  std::vector<ObsIdIt> firstObsToCheckInBands(options_.numZonalBands);

  // Collects buddies of a single observation. When we're done with that observation, the collected
  // list of buddies is extracted into 'pairs' and the collector is reset.
  std::unique_ptr<MetOfficeBuddyCollector> buddyCollector = makeBuddyCollector();

  // Iterate over all bands
  for (int jBandA = 0; jBandA < options_.numZonalBands; ++jBandA) {
    const float lonSearchRangeHalfWidth = getLongitudeSearchRangeHalfWidth(jBandA, bandWidth);

    const int firstBandToSearch = jBandA;
    const int lastBandToSearch = std::min(options_.numZonalBands.value() - 1,
                                          jBandA + numSearchBands);

    firstObsToCheckInBands = bandBegins;

    // Iterate over observations in (jBandA)th band
    for (ObsIdIt obsIdItA = bandBegins[jBandA]; obsIdItA != bandEnds[jBandA]; ++obsIdItA) {
      const int obsIdA = *obsIdItA;

      const float minLonToCheck = longitudes_[obsIdA] - lonSearchRangeHalfWidth;
      const float maxLonToCheck = longitudes_[obsIdA] + lonSearchRangeHalfWidth;

      buddyCollector->reset(obsIdA);

      // Iterate over bands that may contain buddies
      for (int jBandB = firstBandToSearch; jBandB <= lastBandToSearch; ++jBandB) {
        float midBandLatB = 90.0f - bandWidth * (jBandB + 0.5f);
        if (std::abs(latitudes_[obsIdA] - midBandLatB) > searchDLatB)
          continue;

        ObsIdIt firstObsIdItB = firstObsToCheckInBands[jBandB];
        if (jBandA == jBandB)
          firstObsIdItB = obsIdItA + 1;  // Iterate only over observations following observation A.

        // First loop: look for buddies at longitudes [minLonToCheck, maxLonToCheck]
        bool firstObsInSearchRangeFound = false;
        for (ObsIdIt obsIdItB = firstObsIdItB; obsIdItB != bandEnds[jBandB]; ++obsIdItB) {
          const int obsIdB = *obsIdItB;
          if (longitudes_[obsIdB] < minLonToCheck)
            continue;
          if (longitudes_[obsIdB] > maxLonToCheck)
            break;
          if (!firstObsInSearchRangeFound) {
            firstObsToCheckInBands[jBandB] = obsIdItB;
            firstObsInSearchRangeFound = true;
          }

          buddyCollector->examinePotentialBuddy(obsIdB);
          if (buddyCollector->foundEnoughBuddies())
            goto FinishProcessingObsA;
          if (buddyCollector->foundEnoughBuddiesInCurrentBand())
            goto FinishProcessingBandB;
        }

        // Optional second loop: look for buddies at longitudes [-180, maxLonToCheck - 360]
        if (maxLonToCheck > 180 && (jBandA != jBandB || lonSearchRangeHalfWidth < 180)) {
          // Observation A is near band end (+180); wrap around and check the band start too.
          float wrappedMaxLonToCheck = maxLonToCheck - 360;
          for (ObsIdIt obsIdItB = bandBegins[jBandB]; obsIdItB != bandEnds[jBandB]; ++obsIdItB) {
            const int obsIdB = *obsIdItB;
            if (longitudes_[obsIdB] > wrappedMaxLonToCheck ||
                longitudes_[obsIdB] >= minLonToCheck /* visited already in the first loop */)
              break;

            buddyCollector->examinePotentialBuddy(obsIdB);
            if (buddyCollector->foundEnoughBuddies())
              goto FinishProcessingObsA;
            if (buddyCollector->foundEnoughBuddiesInCurrentBand())
              goto FinishProcessingBandB;
          }
        }

        // Optional third loop: look for buddies at longitudes [minLonToCheck + 360, 180]
        if (minLonToCheck < -180 && jBandA != jBandB) {
          // Observation A is near band start (-180); wrap around and check the band end too.
          float wrappedMinLonToCheck = minLonToCheck + 360;
          for (ObsIdRevIt obsIdItB(bandEnds[jBandB]), reverseBandEnd(bandBegins[jBandB]);
               obsIdItB != reverseBandEnd; ++obsIdItB) {
            const int obsIdB = *obsIdItB;
            if (longitudes_[obsIdB] < wrappedMinLonToCheck ||
                longitudes_[obsIdB] <= maxLonToCheck /* visited already in the first loop */)
              break;

            buddyCollector->examinePotentialBuddy(obsIdB);
            if (buddyCollector->foundEnoughBuddies())
              goto FinishProcessingObsA;
            if (buddyCollector->foundEnoughBuddiesInCurrentBand())
              goto FinishProcessingBandB;
          }
        }

FinishProcessingBandB:
        buddyCollector->startProcessingNextBand();
      }  // end of secondary loop over bands (jBandB)
FinishProcessingObsA:
      buddyCollector->appendBuddyPairsTo(pairs);
    }  // end of main loop over observations (obsIdItA)
  }  // end of main loop over bands (jBandA)

  oops::Log::trace() << "Found " << pairs.size() << " buddy pairs.\n";

  return pairs;
}

std::unique_ptr<MetOfficeBuddyCollector> MetOfficeBuddyPairFinder::makeBuddyCollector() const {
  if (options_.useLegacyBuddyCollector)
    return boost::make_unique<MetOfficeBuddyCollectorV1>(options_, latitudes_,
                                                         longitudes_, stationIds_);
  else
    return boost::make_unique<MetOfficeBuddyCollectorV2>(options_, latitudes_,
                                                         longitudes_, stationIds_);
}

float MetOfficeBuddyPairFinder::getLongitudeSearchRangeHalfWidth(int bandIndex,
                                                                 float bandWidth) const {
  const float earthRadius = Constants::mean_earth_rad;
  const float deg2rad = static_cast<float>(Constants::deg2rad);
  const float rad2deg = static_cast<float>(Constants::rad2deg);

  const float midBandLatA = 90.0f - bandWidth * (bandIndex + 0.5f);
  // eqn 3.2a
  const float rad = earthRadius * std::cos((std::abs(midBandLatA) + bandWidth * 0.5f) * deg2rad);
  if (rad <= 10.0f)
    return 360.0f;  // Adjacent to pole
  else
    return rad2deg * options_.searchRadius / rad;  // eqn 3.2b
}

}  // namespace ufo


//  Boost Sort library tests for integer_sort and float_sort details.

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

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

#include <boost/cstdint.hpp>
#include <boost/sort/spreadsort/detail/spreadsort_common.hpp>
#include <boost/sort/spreadsort/detail/integer_sort.hpp>
#include <boost/sort/spreadsort/detail/float_sort.hpp>
#include <boost/sort/spreadsort/detail/string_sort.hpp>
#include <boost/sort/spreadsort/float_sort.hpp>
// Include unit test framework
#include <boost/test/included/test_exec_monitor.hpp>
#include <boost/test/test_tools.hpp>
#include <vector>

#include <iostream>

using namespace std;
using namespace boost::sort::spreadsort;
using namespace boost::sort::spreadsort::detail;

namespace {

struct int_right_shift {
  int operator()(const int x, const unsigned offset) const {
    return x >> offset;
  }
};

struct float_right_shift {
  int operator()(const float x, const unsigned offset) const {
    return float_mem_cast<float, int>(x) >> offset;
  }
};

const int max_int_bits = sizeof(boost::uintmax_t) * 8;
const int max_size_bits = sizeof(size_t) * 8;
const boost::uintmax_t one = 1;

// spreadsort won't recurse for inputs smaller than min_count.
const int int_min_log_count =
  (std::min)((int)int_log_finishing_count,
             (int)int_log_mean_bin_size + int_log_min_split_count);
const int float_min_log_count =
  (std::min)((int)float_log_finishing_count,
             (int)float_log_mean_bin_size + float_log_min_split_count);
const unsigned absolute_min_count = (std::min)(1 << int_min_log_count,
                                               1 << float_min_log_count);

// Verify that roughlog2 is floor(log base 2) + 1.
void roughlog2_test()
{
  for (boost::uintmax_t i = 0; i < max_int_bits; ++i) {
    BOOST_CHECK(detail::rough_log_2_size(one << i) == i + 1);
    BOOST_CHECK(detail::rough_log_2_size((one << i) - 1) == i);
  }
}

// Test the worst-case performance handling, and assure that is using the
// correct formula for the worst-case number of radix iterations.
template<unsigned log_mean_bin_size, unsigned log_min_split_count,
         unsigned log_finishing_count>
void get_min_count_test()
{
  const unsigned min_log_size = log_mean_bin_size + log_min_split_count;
  size_t prev_min_count = absolute_min_count;
  for (int log_range = 0; log_range <= max_int_bits; ++log_range) {
    size_t min_count = get_min_count<log_mean_bin_size, log_min_split_count,
                                     log_finishing_count>(log_range);
    BOOST_CHECK(min_count >= prev_min_count);
    prev_min_count = min_count;
    // When the range is really small, the radix sort will complete in one
    // iteration and worst-case handling doesn't apply.  The code below
    // guarantees the worst-case number of radix sorting iteration.
    if (log_range > min_log_size) {
      BOOST_CHECK(min_count >= (1 << min_log_size));
      int iterations = rough_log_2_size(min_count) - min_log_size;
      BOOST_CHECK(iterations >= 1);
      int base_iterations = max_splits - log_min_split_count;
      int covered_log_range = 0;
      if (iterations > base_iterations) {
        covered_log_range += max_splits * (iterations - base_iterations);
      } else {
        base_iterations = iterations;
      }
      // sum of n to n + x = ((x + 1) * (n + (n + x)))/2 + log_mean_bin_size
      covered_log_range +=
        (base_iterations * (log_min_split_count * 2 + base_iterations - 1))/2 +
        log_mean_bin_size;
      BOOST_CHECK(covered_log_range >= log_range);
      BOOST_CHECK(covered_log_range - max_splits < log_range);
    }
  }
}

// Test the decision of how many pieces to split up the radix sort into
// (roughly 2^(log_range - log_divisor)) to make sure the results are logical.
void get_log_divisor_test()
{
  for (int log_range = 0; log_range <= max_int_bits; ++log_range) {
    int prev_log_divisor = max_int_bits +
      (std::max)((int)int_log_mean_bin_size, (int)float_log_mean_bin_size);
    for (int log_count = 0; log_count < max_size_bits; ++log_count) {
      size_t count = (one << log_count) - 1;
      BOOST_CHECK(rough_log_2_size(count) == (unsigned)log_count);
      int log_divisor =
        get_log_divisor<int_log_mean_bin_size>(count, log_range);
      // Only process counts >= int_log_finishing_count in this function.
      if (count >= absolute_min_count)
        BOOST_CHECK(log_divisor <= log_range);
      // More pieces should be used the larger count is.
      BOOST_CHECK(log_divisor <= prev_log_divisor);
      prev_log_divisor = log_divisor;
      BOOST_CHECK(log_divisor >= 0);
      if (log_range > log_count) {
        BOOST_CHECK(log_range - log_divisor <= max_splits);
      } else if (log_range <= max_finishing_splits) {
        BOOST_CHECK(log_divisor == 0);
      }
    }
  }
}

// Verify that is_sorted_or_find_extremes returns true if the data is sorted,
// and otherwise returns the actual min and max.
void is_sorted_or_find_extremes_test()
{
  vector<int> input;
  input.push_back(3);
  input.push_back(5);
  input.push_back(1);
  // Test a sorted input.
  vector<int> sorted_input(input);
  std::sort(sorted_input.begin(), sorted_input.end());
  vector<int>::iterator max, min;
  BOOST_CHECK(detail::is_sorted_or_find_extremes(sorted_input.begin(),
                                                 sorted_input.end(), max, min));
  // Test an unsorted input.
  BOOST_CHECK(!detail::is_sorted_or_find_extremes(input.begin(), input.end(),
                                                  max, min));
  BOOST_CHECK(*min == 1);
  BOOST_CHECK(*max == 5);
  // Test the comparison function version.
  BOOST_CHECK(detail::is_sorted_or_find_extremes(sorted_input.begin(),
                                                 sorted_input.end(), max, min,
                                                 std::less<int>()));
  BOOST_CHECK(!detail::is_sorted_or_find_extremes(sorted_input.begin(),
                                                  sorted_input.end(),
                                                  max, min,
                                                  std::greater<int>()));
  BOOST_CHECK(*min == 5);
  BOOST_CHECK(*max == 1);

  // Test with floats
  vector<float> float_input;
  float_input.push_back(.3f);
  float_input.push_back(4.0f);
  float_input.push_back(.1f);
  vector<float> sorted_float_input(float_input);
  std::sort(sorted_float_input.begin(), sorted_float_input.end());
  // Test cast_float_iter
  int cast_min = detail::cast_float_iter<int, vector<float>::iterator>(
                     sorted_float_input.begin());
  int cast_max = detail::cast_float_iter<int, vector<float>::iterator>(
                     sorted_float_input.end() - 1);
  BOOST_CHECK(cast_min == float_right_shift()(.1f, 0));
  BOOST_CHECK(cast_max == float_right_shift()(4.0f, 0));
  // Test a sorted input
  int div_max, div_min;
  BOOST_CHECK(detail::is_sorted_or_find_extremes(sorted_float_input.begin(),
                                                 sorted_float_input.end(),
                                                 div_max, div_min));
  // Test an unsorted input.
  BOOST_CHECK(!detail::is_sorted_or_find_extremes(float_input.begin(),
                                                  float_input.end(),
                                                  div_max, div_min));
  BOOST_CHECK(div_min == cast_min);
  BOOST_CHECK(div_max == cast_max);

  // Test with a right_shift functor.
  BOOST_CHECK(detail::is_sorted_or_find_extremes(sorted_float_input.begin(),
                                                 sorted_float_input.end(),
                                                 div_max, div_min,
                                                 float_right_shift()));
  // Test an unsorted input.
  BOOST_CHECK(!detail::is_sorted_or_find_extremes(float_input.begin(),
                                                  float_input.end(), div_max,
                                                  div_min,
                                                  float_right_shift()));
  BOOST_CHECK(div_min == float_right_shift()(.1f, 0));
  BOOST_CHECK(div_max == float_right_shift()(4.0f, 0));
}

// Make sure bins are created correctly.
void size_bins_test() {
  size_t bin_sizes[1 << detail::max_finishing_splits];
  bin_sizes[0] = 1;
  bin_sizes[2] = 7;
  const int old_bin_value = 7;
  std::vector<int> old_bins;
  old_bins.push_back(old_bin_value);
  std::vector<vector<int>::iterator> bin_cache;
  bin_cache.push_back(old_bins.begin());
  unsigned cache_offset = 1;
  unsigned cache_end;
  const unsigned bin_count = 2;
  std::vector<int>::iterator *new_cache_start =
    size_bins(bin_sizes, bin_cache, cache_offset, cache_end, bin_count);
  BOOST_CHECK((new_cache_start - &bin_cache[0]) == cache_offset);
  BOOST_CHECK(bin_sizes[0] == 0);
  BOOST_CHECK(bin_sizes[1] == 0);
  BOOST_CHECK(bin_sizes[2] == 7);  // shouldn't modify past bin_count
  BOOST_CHECK(cache_end == 3);
  BOOST_CHECK(bin_cache.size() == cache_end);
  BOOST_CHECK(old_bins[0] == old_bin_value);
}

// Test the specialized 3-way swap loops.
void swap_loop_test() {
  size_t bin_sizes[1 << detail::max_finishing_splits];
  bin_sizes[0] = bin_sizes[1] = 2;
  bin_sizes[2] = 1;

  // test integer swap loop
  vector<int> ints;
  const int int_div_min = 3;
  const int int_log_divisor = 1;
  const unsigned int_offset = int_div_min << int_log_divisor;
  ints.push_back(2 + int_offset);
  ints.push_back(1 + int_offset); // stays in place
  ints.push_back(4 + int_offset);
  ints.push_back(3 + int_offset);
  ints.push_back(0 + int_offset);
  vector<vector<int>::iterator> int_bin_vector;
  int_bin_vector.push_back(ints.begin());
  int_bin_vector.push_back(int_bin_vector[0] + bin_sizes[0]);
  int_bin_vector.push_back(int_bin_vector[1] + bin_sizes[1]);
  vector<int>::iterator next_int_bin_start = int_bin_vector[0];
  vector<int>::iterator *int_bins = &int_bin_vector[0];
  int_right_shift integer_right_shift;
  swap_loop(int_bins, next_int_bin_start, 0, integer_right_shift, bin_sizes,
            int_log_divisor, int_div_min);
  for (unsigned i = 0; i < ints.size(); ++i) {
    BOOST_CHECK(ints[i] == int(int_offset + i));
  }
  BOOST_CHECK(next_int_bin_start == ints.begin() + bin_sizes[0]);

  // test float swap loop
  vector<float> floats;
  const int float_four_as_int = float_mem_cast<float, int>(4.0f);
  const int float_log_divisor =
    rough_log_2_size(float_mem_cast<float, int>(5.0f) - float_four_as_int);
  const int float_div_min = float_four_as_int >> float_log_divisor;
  floats.push_back(6.0f);
  floats.push_back(5.0f); // stays in place
  floats.push_back(8.0f);
  floats.push_back(7.0f);
  floats.push_back(4.0f);
  vector<vector<float>::iterator> float_bin_vector;
  float_bin_vector.push_back(floats.begin());
  float_bin_vector.push_back(float_bin_vector[0] + bin_sizes[0]);
  float_bin_vector.push_back(float_bin_vector[1] + bin_sizes[1]);
  vector<float>::iterator next_float_bin_start = float_bin_vector[0];
  vector<float>::iterator *float_bins = &float_bin_vector[0];
  float_swap_loop(float_bins, next_float_bin_start, 0, bin_sizes,
                  float_log_divisor, float_div_min);
  for (unsigned i = 0; i < floats.size(); ++i) {
    BOOST_CHECK(floats[i] == 4.0f + i);
  }
  BOOST_CHECK(next_float_bin_start == floats.begin() + bin_sizes[0]);
}

} // end anonymous namespace

// test main
int test_main( int, char*[] )
{
  roughlog2_test();
  get_min_count_test<int_log_mean_bin_size, int_log_min_split_count,
                    int_log_finishing_count>();
  get_min_count_test<float_log_mean_bin_size, float_log_min_split_count,
                    float_log_finishing_count>();
  get_log_divisor_test();
  is_sorted_or_find_extremes_test();
  size_bins_test();
  swap_loop_test();
  return 0;
}


///////////////////////////////////////////////////////////////////////////////
//  Copyright 2016 John Maddock. 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)

#include <boost/multiprecision/cpp_int.hpp>
#include <boost/multiprecision/cpp_dec_float.hpp>
#include <boost/multiprecision/cpp_bin_float.hpp>

boost::multiprecision::cpp_rational rationalfromStr(const char* str)
{
   boost::multiprecision::cpp_dec_float_50 d1(str);
   boost::multiprecision::cpp_rational     result(d1); // <--- eats CPU forever
   return result;
}

boost::multiprecision::cpp_rational rationalfromStr2(const char* str)
{
   boost::multiprecision::cpp_bin_float_50 d1(str);
   boost::multiprecision::cpp_rational     result(d1); // <--- eats CPU forever
   return result;
}

int main()
{
   // This example is OK.
   {
      boost::multiprecision::cpp_rational expected = 1;
      BOOST_MP_ASSERT(expected == rationalfromStr("1"));
   }
   // This example is OK.
   {
      boost::multiprecision::cpp_rational expected = boost::multiprecision::cpp_rational(25) / boost::multiprecision::cpp_rational(10);
      BOOST_MP_ASSERT(expected == rationalfromStr("2.5"));
   }
   // This example is OK.
   {
      boost::multiprecision::cpp_rational expected = boost::multiprecision::cpp_rational(5) / boost::multiprecision::cpp_rational(1000);
      BOOST_MP_ASSERT(expected == rationalfromStr("0.005"));
   }
   // This example is OK.
   {
      boost::multiprecision::cpp_rational expected = 0;
      BOOST_MP_ASSERT(expected == boost::multiprecision::cpp_rational("0")); // direct cpp_rational from str is OK.
   }
   // This example fails.
   {
      boost::multiprecision::cpp_rational expected = 0;
      // reachable code
      BOOST_MP_ASSERT(expected == rationalfromStr("0")); // cpp_rational from cpp_dec_float_50 is not OK.
            // unreachable code
   }
   {
      boost::multiprecision::cpp_rational expected = 0;
      // reacheble code
      BOOST_MP_ASSERT(expected == rationalfromStr2("0")); // cpp_rational from cpp_dec_float_50 is not OK.
          // unreachable code
   }
   return 0;
}


#pragma once
#include <vector>
#include <queue>
#include <Eigen/Geometry>
#include <boost/optional.hpp>
#include <OpenMesh/Core/Mesh/TriMesh_ArrayKernelT.hh>
#include <OpenMesh/Core/Mesh/PolyConnectivity.hh>
#include "../vector_convert.hh"
#include "../FaceBaryCoordT.hh"
#include "../../eigen_util.hh"
#include "../../util.hh"
#include "../DerivedPtrHolder.hh"

namespace kt84 {

/*
    Implementation of a surface mesh parameterization algorithm descibed in the following articles:
        Ryan Schmidt, Cindy Grimm, and Brian Wyvill.
        Interactive decal compositing with discrete exponential maps.
        ACM SIGGRAPH 2006.

        Ryan Schmidt and Karan Singh.
        Drag, Drop, and Clone: An Interactive Interface for Surface Composition.
        Technical Report CSRG-611, Department of Computer Science, University of Toronto, 2010.

    NOTE: The algorithm is implemented in a slightly different way; the uv coordiante of currently processed vertex q is estimated by rotating
    its local frame to that of its adjacent vertex p that has been processed already, which is in contrast to what is claimed in the original
    SIGGRAPH paper (see sentences below equation (3)). This modification seems to provide better robustness especially when parameterizing highly
    curved surface regions (probably because normals at distant locations are very different from the seed's normal).

    parameters:
        seed(s)                 seed vertex id
        seed(s)_basis_u         3D vector in object space corresponding to unit basis vector of u-coordinate.
                                (its length determines the overall scaling of the parameterization)
        seed(s)_uv              seed uv coordinate
        dist_max                termination criteria based on object-space distance
        uv_min, uv_max          termination criteria based on parameter-space bounding box
*/

struct ExpMap_VertexTraits {
    struct Data {
        bool            paramed;
        double          dist;
        double          weight;
        Eigen::Vector2d uv;
        Eigen::Vector2d basis_u;              // represents u-coordiante basis vector in object space.
                                            // (i.e., its length determines scaling of parameterization.)
                                            // living on the tangent plane defined by intrinsic bases

        Data()
            : paramed()
            , dist   (util::dbl_max())
            , weight ()
            , uv     (Eigen::Vector2d::Zero())
            , basis_u(Eigen::Vector2d::Zero())
        {}
    } expmap;
};

struct ExpMap_FaceTraits {
    bool expmap_paramed;

    ExpMap_FaceTraits()
        : expmap_paramed()
    {}
};

namespace internal {
    template <class TMeshBase>
    void expmap_compute_common(TMeshBase* mesh,
                               const std::vector<OpenMesh::PolyConnectivity::VHandle>& seeds,
                               const std::vector<Eigen::Vector3d                    >& seeds_basis_u,
                               const std::vector<Eigen::Vector2d                    >& seeds_uv,
                               double dist_max,
                               const Eigen::Vector2d& uv_min,
                               const Eigen::Vector2d& uv_max,
                               std::vector<OpenMesh::PolyConnectivity::FHandle>& expmap_paramed_faces)
    {
        // helper functions ==============================================================================================
        auto get_intr_basis = [] (const Eigen::Vector3d& normal) -> Eigen::Vector3d {
            // get unit basis vector orthogonal to input vector in intrinsical manner
            double nx = std::abs(normal.x());
            double ny = std::abs(normal.y());
            double nz = std::abs(normal.z());
            Eigen::Vector3d basis =
                nx < ny && ny < nz ? Eigen::Vector3d::UnitX() :
                ny < nz            ? Eigen::Vector3d::UnitY() :
                                     Eigen::Vector3d::UnitZ();
            eigen_util::orthonormalize(normal, basis);
            return basis;
        };

        auto compute_rotation = [get_intr_basis] (const Eigen::Vector3d& p_normal,
                                                  const Eigen::Vector3d& q_normal) -> Eigen::Rotation2Dd
            // compute 2D rotation which will be used to rotate basis_u of p to estimate that of q
        {
            // get intrinsic basis vectors of p, q
            auto p_intr_basis = get_intr_basis(p_normal);
            auto q_intr_basis = get_intr_basis(q_normal);

            // step 1: compute 3D rotation that aligns q_normal with p_normal. apply it to q_basis.
            auto rot1_axis = q_normal.cross(p_normal);
            if (!rot1_axis.isZero()) {
                rot1_axis.normalize();
                double rot1_angle = util::acos_clamped(q_normal.dot(p_normal));
                q_intr_basis = Eigen::AngleAxisd(rot1_angle, rot1_axis) * q_intr_basis;
            }

            // step 2: compute 2D rotation that makes (rotated) q_basis aligned with p_basis.
            double rot2_angle = util::acos_clamped(q_intr_basis.dot(p_intr_basis));
            if (q_intr_basis.cross(p_intr_basis).dot(p_normal) < 0)     // negative rotation
                rot2_angle *= -1.0f;

            return Eigen::Rotation2Dd(rot2_angle);
        };

        auto compute_uv = [get_intr_basis] (const Eigen::Vector3d& p_point ,
                                            const Eigen::Vector3d& p_normal,
                                            const Eigen::Vector2d& p_uv,
                                            const Eigen::Vector2d& p_basis_u,
                                            const Eigen::Vector3d& q_point ) -> Eigen::Vector2d
            // computes uv for point q using p's point, normal, uv, basis_u
        {
            auto p_intr_basis_x = get_intr_basis(p_normal);
            auto p_intr_basis_y = p_normal.cross(p_intr_basis_x);

            // project (rotate) pq into tangent plane
            Eigen::Vector3d pq = q_point - p_point;
            Eigen::Vector2d local_uv(pq.dot(p_intr_basis_x), pq.dot(p_intr_basis_y));
            local_uv *= pq.norm() / local_uv.norm();

            // encode 2D local_uv using propagated (non-unit) basis vectors
            auto p_basis_v = eigen_util::rotate90(p_basis_u);
            local_uv = Eigen::Vector2d(p_basis_u.dot(local_uv), p_basis_v.dot(local_uv));

            // account for scale difference
            local_uv /= p_basis_u.squaredNorm();

            Eigen::Vector2d q_uv = p_uv + local_uv;

            return q_uv;
        };
        // ============================================================================================== helper functions

        // clear all vertex data
        for (auto v : mesh->vertices())
            mesh->data(v).expmap = ExpMap_VertexTraits::Data();

        // priority queue for propagation front
        struct QueueElement {
            OpenMesh::PolyConnectivity::VHandle vhandle;
            double dist;

            QueueElement() : dist(){}
            QueueElement(OpenMesh::PolyConnectivity::VHandle vhandle_, double dist_) : vhandle(vhandle_) , dist(dist_) {}
            bool operator<(const QueueElement& rhs) const { return dist > rhs.dist; }       // note: smaller distance gets higher priority
        };
        std::priority_queue<QueueElement> candidates;

        // init
        for (size_t i = 0; i < seeds.size(); ++i) {
            auto seed = seeds[i];
            auto& seed_data = mesh->data(seed).expmap;

            Eigen::Vector3d seed_normal       = o2e(mesh->normal(seed));
            Eigen::Vector3d seed_intr_basis_x = get_intr_basis(seed_normal);
            Eigen::Vector3d seed_intr_basis_y = seed_normal.cross(seed_intr_basis_x);
            Eigen::Vector3d seed_basis_u      = seeds_basis_u[i];

            seed_data.weight  += 1;
            seed_data.dist     = 0;
            seed_data.uv      += seeds_uv[i];
            seed_data.basis_u += Eigen::Vector2d(seed_intr_basis_x.dot(seed_basis_u), seed_intr_basis_y.dot(seed_basis_u));

            candidates.push(QueueElement(seed, 0));
        }

        while (!candidates.empty()) {
            auto p = candidates.top().vhandle;
            candidates.pop();

            auto& p_data   = mesh->data(p).expmap;
            auto  p_point  = o2e(mesh->point (p));
            auto  p_normal = o2e(mesh->normal(p));

            if (p_data.paramed) continue;
            p_data.paramed = true;

            p_data.uv      /= p_data.weight;
            p_data.basis_u /= p_data.weight;

            if (dist_max < p_data.dist) continue;

            if (!Eigen::AlignedBox2d(uv_min, uv_max).contains(p_data.uv)) continue;

            for (auto q = mesh->vv_iter(p); q.is_valid(); ++q) {
                auto& q_data   = mesh->data(*q).expmap;
                auto  q_point  = o2e(mesh->point (*q));
                auto  q_normal = o2e(mesh->normal(*q));

                if (q_data.paramed) continue;

                // distance update
                double dist_pq = (p_point - q_point).norm();
                if (p_data.dist + dist_pq < q_data.dist)
                    q_data.dist = p_data.dist + dist_pq;

                // weight based on distance between p and q
                double weight = 1.0f / std::pow(dist_pq, 0.25);
                q_data.weight += weight;

                // estimate of data by weighted averaging
                q_data.uv      += weight * compute_uv(p_point, p_normal, p_data.uv, p_data.basis_u, q_point);
                q_data.basis_u += weight * (compute_rotation(p_normal, q_normal) * p_data.basis_u);            // be sure to use parentheses! otherwise Eigen will happily go funny:)

                candidates.push(QueueElement(*q, q_data.dist));
            }
        }

        // collect faces with its vertices all paramed
        expmap_paramed_faces.clear();
        expmap_paramed_faces.reserve(mesh->n_faces());
        for (auto f : mesh->faces()) {
            mesh->data(f).expmap_paramed = true;
            for (auto v = mesh->fv_iter(f); v.is_valid(); ++v) {
                if (!mesh->data(*v).expmap.paramed) {
                    mesh->data(f).expmap_paramed = false;
                    break;
                }
            }
            if (mesh->data(f).expmap_paramed)
                expmap_paramed_faces.push_back(f);
        }
    }
}

template <class TMeshBase, class TMesh>
struct ExpMap : public DerivedPtrHolder<TMesh, ExpMap<TMeshBase, TMesh>> {
    std::vector<typename TMeshBase::FHandle> expmap_paramed_faces;

    void expmap_compute(
        const std::vector<typename TMeshBase::VHandle>& seeds,
        const std::vector<Eigen::Vector3d            >& seeds_basis_u,
        const std::vector<Eigen::Vector2d            >& seeds_uv,
        double dist_max,
        const Eigen::Vector2d& uv_min = Eigen::Vector2d::Constant(-std::numeric_limits<double>::max()),
        const Eigen::Vector2d& uv_max = Eigen::Vector2d::Constant( std::numeric_limits<double>::max()))
    {
        TMesh* mesh = DerivedPtrHolder<TMesh, ExpMap<TMeshBase, TMesh>>::derived_ptr;
        internal::expmap_compute_common(mesh, seeds, seeds_basis_u, seeds_uv, dist_max, uv_min, uv_max, expmap_paramed_faces);
    }
    void expmap_compute(
        typename TMeshBase::VHandle seed,
        Eigen::Vector3d             seed_basis_u,
        Eigen::Vector2d             seed_uv,
        double dist_max,
        const Eigen::Vector2d& uv_min = Eigen::Vector2d::Constant(-std::numeric_limits<double>::max()),
        const Eigen::Vector2d& uv_max = Eigen::Vector2d::Constant( std::numeric_limits<double>::max()))
    {
        expmap_compute(
            std::vector<typename TMeshBase::VHandle>(1, seed        ),
            std::vector<Eigen::Vector3d            >(1, seed_basis_u),
            std::vector<Eigen::Vector2d            >(1, seed_uv     ),
            dist_max,
            uv_min,
            uv_max);
    }
};

// specialization for triangle meshes, with functionality to convert on-surface position (face id + barycentric coord) into uv coordinates.
template <class TTrait, class TMesh>
struct ExpMap<OpenMesh::TriMesh_ArrayKernelT<TTrait>, TMesh> : public DerivedPtrHolder<TMesh, ExpMap<OpenMesh::TriMesh_ArrayKernelT<TTrait>, TMesh>> {
    typedef OpenMesh::TriMesh_ArrayKernelT<TTrait> MeshBase;

    std::vector<typename MeshBase::FHandle> expmap_paramed_faces;

    void expmap_compute(
        const std::vector<typename MeshBase::VHandle>& seeds,
        const std::vector<Eigen::Vector3d           >& seeds_basis_u,
        const std::vector<Eigen::Vector2d           >& seeds_uv,
        double dist_max,
        const Eigen::Vector2d& uv_min = Eigen::Vector2d::Constant(-std::numeric_limits<double>::max()),
        const Eigen::Vector2d& uv_max = Eigen::Vector2d::Constant( std::numeric_limits<double>::max()))
    {
        TMesh* mesh = get_mesh();
        internal::expmap_compute_common(mesh, seeds, seeds_basis_u, seeds_uv, dist_max, uv_min, uv_max, expmap_paramed_faces);
    }
    void expmap_compute(
        typename MeshBase::VHandle seed,
        Eigen::Vector3d            seed_basis_u,
        Eigen::Vector2d            seed_uv,
        double dist_max,
        const Eigen::Vector2d& uv_min = Eigen::Vector2d::Constant(-std::numeric_limits<double>::max()),
        const Eigen::Vector2d& uv_max = Eigen::Vector2d::Constant( std::numeric_limits<double>::max()))
    {
        expmap_compute(
            std::vector<typename MeshBase::VHandle>(1, seed        ),
            std::vector<Eigen::Vector3d           >(1, seed_basis_u),
            std::vector<Eigen::Vector2d           >(1, seed_uv     ),
            dist_max,
            uv_min,
            uv_max);
    }
    void expmap_compute(
        const std::vector<FaceBaryCoord  >& seeds,
        const std::vector<Eigen::Vector3d>& seeds_basis_u,
        const std::vector<Eigen::Vector2d>& seeds_uv,
        double dist_max,
        const Eigen::Vector2d& uv_min = Eigen::Vector2d::Constant(-std::numeric_limits<double>::max()),
        const Eigen::Vector2d& uv_max = Eigen::Vector2d::Constant( std::numeric_limits<double>::max()))
    {
        TMesh* mesh = get_mesh();

        std::vector<typename MeshBase::VHandle> seeds2;
        std::vector<Eigen::Vector3d           > seeds2_basis_u;
        std::vector<Eigen::Vector2d           > seeds2_uv;

        for (size_t i = 0; i < seeds.size(); ++i) {
            auto& seed         = seeds        [i];
            auto& seed_basis_u = seeds_basis_u[i];
            auto& seed_uv      = seeds_uv     [i];

            Eigen::Vector3d p = o2e(seed.blend_point (*mesh));
            Eigen::Vector3d n = o2e(seed.blend_normal(*mesh));

            Eigen::Vector3d seed_basis_v = n.cross(seed_basis_u);

            double offset_scaling = 1 / seed_basis_u.squaredNorm();

            for (auto v = mesh->fv_iter(seed.f); v; ++v) {
                Eigen::Vector3d pq = o2e(mesh->point(v)) - p;
                Eigen::Vector2d uv_offset(pq.dot(seed_basis_u), pq.dot(seed_basis_v));
                uv_offset *= offset_scaling;

                seeds2        .push_back(v);
                seeds2_basis_u.push_back(seed_basis_u);
                seeds2_uv     .push_back(seed_uv + uv_offset);
            }
        }

        expmap_compute(seeds2, seeds2_basis_u, seeds2_uv, dist_max, uv_min, uv_max);
    }
    void expmap_compute(
        FaceBaryCoord   seed,
        Eigen::Vector3d seed_basis_u,
        Eigen::Vector2d seed_uv,
        double dist_max,
        const Eigen::Vector2d& uv_min = Eigen::Vector2d::Constant(-std::numeric_limits<double>::max()),
        const Eigen::Vector2d& uv_max = Eigen::Vector2d::Constant( std::numeric_limits<double>::max()))
    {
        expmap_compute(
            std::vector<FaceBaryCoord  >(1, seed        ),
            std::vector<Eigen::Vector3d>(1, seed_basis_u),
            std::vector<Eigen::Vector2d>(1, seed_uv     ),
            dist_max,
            uv_min,
            uv_max);
    }
    // conversion from on-surface position to uv coordinates
    boost::optional<Eigen::Vector2d> fbc_to_uv(const FaceBaryCoord& fbc) const {
        TMesh* mesh = get_mesh();

        if (!mesh->data(fbc.f).expmap_paramed)
            return boost::none;

        return fbc.blend_value<MeshBase, Eigen::Vector2d>(
            *mesh,
            [](const MeshBase& mesh, typename MeshBase::VHandle v) { return mesh.data(v).expmap.uv; }
        );
    }
    boost::optional<Eigen::Vector2d> fbc_to_uv(typename MeshBase::FHandle f, const BaryCoord& bc) const {
        return fbc_to_uv(FaceBaryCoord(f, bc));
    }
    FaceBaryCoord uv_to_fbc(Eigen::Vector2d uv) const {
        TMesh* mesh = get_mesh();

        for (auto f : expmap_paramed_faces) {
            // compute bounding box of this face
            Eigen::AlignedBox2d box;
            std::vector<Eigen::Vector2d> face_uv;
            for (auto v = mesh->cfv_iter(f); v.is_valid(); ++v) {
                auto expmap_uv = mesh->data(*v).expmap.uv;
                face_uv.push_back(expmap_uv);
                box.extend(expmap_uv);
            }

            // skip if uv is outside the bounding box
            if (!box.contains(uv)) continue;

            // compute barycentric coordinate
            // (1 - hit.u - hit.v) * face_uv[0] + hit.u * face_uv[1] + hit.v * face_uv[2] = uv
            // | face_uv[1] - face_uv[0], face_uv[2] - face_uv[0] | * |hit.u| = | uv - face_uv[0] |
            // |                                                  |   |hit.v|   |                 |
            Eigen::Matrix2d A;
            A << face_uv[1] - face_uv[0], face_uv[2] - face_uv[0];
            Eigen::Vector2d hit_uv = A.inverse() * (uv - face_uv[0]);

            // skip if barycentric coordinate is negative
            BaryCoord bc(1 - hit_uv[0] - hit_uv[1], hit_uv[0], hit_uv[1]);
            if (!bc.is_all_positive()) continue;

            // return result
            return FaceBaryCoord(f, bc);
        }

        return FaceBaryCoord();
    }
private:
    TMesh* get_mesh() const { return DerivedPtrHolder<TMesh, ExpMap<OpenMesh::TriMesh_ArrayKernelT<TTrait>, TMesh>>::derived_ptr; }
};

}


#include "utility.h"
#include "bagReader.h"
#include "featureExtraction.h"
#include "imageProjection.h"
#include "imuPreintegration.h"
#include "mapOptmization.h"

#include <ros/ros.h>
#include <rosbag/view.h>

#include <boost/foreach.hpp>

#define foreach BOOST_FOREACH

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

    // Initialize ROS
    ros::init(argc, argv, "main_loam");

    // Create node and set loop rate
    ros::NodeHandle n;
    ros::Rate loop_rate(10);

    // Publish correct imu data at /imu_correct
    ros::Publisher imu_correct_pub = n.advertise<sensor_msgs::Imu>("/imu_correct", 1000);

    // LIO-SAM class definitions
    FeatureExtraction FE;
    mapOptimization MO;
    ImageProjection IP;
    TransformFusion TF;
    IMUPreintegration ImuP;

    // Load bag file
    bagReader bgr;
    bgr.load(std::string(argv[1]));

    // Read each message in bag file
    foreach(rosbag::MessageInstance m, bgr.view){

        if(m.getTopic() == "/imu_raw"){
                sensor_msgs::Imu::ConstPtr s = m.instantiate<sensor_msgs::Imu>();
                if (s != NULL){
                    // Handle raw IMU values using LIO-SAM class functions
                    ImuP.imuHandler(s);
                    IP.imuHandler(s);
                }
            }

        if(m.getTopic() == "/imu_correct"){
            sensor_msgs::Imu::ConstPtr s = m.instantiate<sensor_msgs::Imu>();
            if (s != NULL){
                // Publish correct imu data
                imu_correct_pub.publish(m.instantiate<sensor_msgs::Imu>());
            }
        }

        if(m.getTopic() == "/points_raw"){
            sensor_msgs::PointCloud2ConstPtr s = m.instantiate<sensor_msgs::PointCloud2>();
            if (s != NULL){
                // Handle point cloud data using LIO-SAM class function
                IP.cloudHandler(s);
            }
        }

        ros::spinOnce();
        loop_rate.sleep();
    }

}


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

#include <boost/numeric/ublas/matrix_proxy.hpp>

#include "determinant.hpp"
#include "SiconosVector.hpp"
#include "SimpleMatrix.hpp"
#include "BlockMatrixIterators.hpp"
#include "BlockMatrix.hpp"

#include "SiconosAlgebra.hpp"

using namespace Siconos;

//=======================
//       get norm
//=======================

double SimpleMatrix::normInf() const
{
  if (_num == 1)
    return norm_inf(*mat.Dense);
  else if (_num == 2)
    return norm_inf(*mat.Triang);
  else if (_num == 3)
    return norm_inf(*mat.Sym);
  else if (_num == 4)
    return norm_inf(*mat.Sparse);
  else if (_num == 5)
    return norm_inf(*mat.Banded);
  else if (_num == 6)
    return 0;
  else // if(_num==7)
    return 1;
}

void SimpleMatrix::normInfByColumn(SP::SiconosVector vIn) const
{
  if (_num == 1)
  {
    if (vIn->size() != size(1))
      RuntimeException::selfThrow("SimpleMatrix::normInfByColumn: the given vector does not have the right length");
    DenseVect tmpV = DenseVect(size(0));
    for (unsigned int i = 0; i < size(1); i++)
    {
       ublas::noalias(tmpV) = ublas::column(*mat.Dense, i);
       (*vIn)(i) = norm_inf(tmpV);
    }
  }
  else
    RuntimeException::selfThrow("SimpleMatrix::normInfByColumn: not implemented for data other than DenseMat");
}
//=======================
//       determinant
//=======================

double SimpleMatrix::det() const
{
  if (_num == 1)
    return determinant(*mat.Dense);
  else if (_num == 2)
    return determinant(*mat.Triang);
  else if (_num == 3)
    return determinant(*mat.Sym);
  else if (_num == 4)
    return determinant(*mat.Sparse);
  else if (_num == 5)
    return determinant(*mat.Banded);
  else if (_num == 6)
    return 0;
  else // if(_num==7)
    return 1;
}

void SimpleMatrix::trans()
{
  switch (_num)
  {
  case 1:
    *mat.Dense = ublas::trans(*mat.Dense);
    break;
  case 2:
    SiconosMatrixException::selfThrow("SimpleMatrix::trans() failed, the matrix is triangular matrix and can not be transposed in place.");
    break;
  case 3:
    break;
  case 4:
    *mat.Sparse = ublas::trans(*mat.Sparse);
    break;
  case 5:
    *mat.Banded = ublas::trans(*mat.Banded);
    break;
  case 6:
    break;
  case 7:
    break;
  }
  resetLU();
}

void SimpleMatrix::trans(const SiconosMatrix &m)
{
  if (m.isBlock())
    SiconosMatrixException::selfThrow("SimpleMatrix::trans(m) failed, not yet implemented for m being a BlockMatrix.");

  if (&m == this)
    trans();//SiconosMatrixException::selfThrow("SimpleMatrix::trans(m) failed, m = this, use this->trans().");
  else
  {
    unsigned int numM = m.num();
    switch (numM)
    {
    case 1:
      if (_num != 1)
        SiconosMatrixException::selfThrow("SimpleMatrix::trans(m) failed, try to transpose a dense matrix into another type.");
      noalias(*mat.Dense) = ublas::trans(*m.dense());
      break;
    case 2:
      if (_num != 1)
        SiconosMatrixException::selfThrow("SimpleMatrix::trans(m) failed, try to transpose a triangular matrix into a non-dense one.");
      noalias(*mat.Dense) = ublas::trans(*m.triang());
      break;
    case 3:
      *this = m;
      break;
    case 4:
      if (_num == 1)
        noalias(*mat.Dense) = ublas::trans(*m.sparse());
      else if (_num == 4)
        noalias(*mat.Sparse) = ublas::trans(*m.sparse());
      else
        SiconosMatrixException::selfThrow("SimpleMatrix::trans(m) failed, try to transpose a sparse matrix into a forbidden type (not dense nor sparse).");
      break;
    case 5:
      if (_num == 1)
        noalias(*mat.Dense) = ublas::trans(*m.banded());
      else if (_num == 5)
        noalias(*mat.Banded) = ublas::trans(*m.banded());
      else
        SiconosMatrixException::selfThrow("SimpleMatrix::trans(m) failed, try to transpose a banded matrix into a forbidden type (not dense nor banded).");
      break;
    case 6:
      *this = m;
      break;
    case 7:
      *this = m;
    }
    // unsigned int tmp = _dimRow;
    // _dimRow = _dimCol;
    // _dimCol = tmp;
    resetLU();
  }
}

const SimpleMatrix matrix_pow(const SimpleMatrix& m, unsigned int power)
{
  if (m.isBlock())
    SiconosMatrixException::selfThrow("Matrix, pow function: not yet implemented for BlockMatrix.");
  if ( m.size(0) != m.size(1))
    SiconosMatrixException::selfThrow("matrix_pow(SimpleMatrix), matrix is not square.");

  if (power > 0)
  {
    unsigned int num = m.num();
    if (num == 1)
    {
      DenseMat p = *m.dense();
      for (unsigned int i = 1; i < power; i++)
        p = prod(p, *m.dense());
      return p;
    }
    else if (num == 2)
    {
      TriangMat t = *m.triang();
      for (unsigned int i = 1; i < power; i++)
        t = prod(t, *m.triang());
      return t;
    }
    else if (num == 3)
    {
      SymMat s = *m.sym();
      for (unsigned int i = 1; i < power; i++)
        s = prod(s, *m.sym());
      return s;
    }
    else if (num == 4)
    {
      SparseMat sp = *m.sparse();
      for (unsigned int i = 1; i < power; i++)
        sp = prod(sp, *m.sparse());
      return sp;
    }
    else if (num == 5)
    {
      DenseMat b = *m.banded();
      for (unsigned int i = 1; i < power; i++)
        b = prod(b, *m.banded());
      return b;
    }
    else if (num == 6)
    {
      ZeroMat z(m.size(0), m.size(1));
      return z;
    }
    else // if (num==7)
    {
      IdentityMat I(m.size(0), m.size(1));;
      return I;
    }
  }
  else// if(power == 0)
  {
    IdentityMat I = ublas::identity_matrix<double>(m.size(0), m.size(1));
    return I;
  }
}

/*
The following code inverts the matrix input using LU-decomposition with backsubstitution of unit vectors. Reference: Numerical Recipies in C, 2nd ed., by Press, Teukolsky, Vetterling & Flannery.

you can solve Ax=b using three lines of ublas code:

permutation_matrix<> piv;
lu_factorize(A, piv);
lu_substitute(A, piv, x);

*/
#ifndef INVERT_MATRIX_HPP
#define INVERT_MATRIX_HPP

// REMEMBER to update "lu.hpp" header includes from boost-CVS
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/vector_proxy.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/triangular.hpp>
#include <boost/numeric/ublas/lu.hpp>
#include <boost/numeric/ublas/io.hpp>

/* Matrix inversion routine.
Uses lu_factorize and lu_substitute in uBLAS to invert a matrix */
template<class T, class U, class V>
bool InvertMatrix(const ublas::matrix<T, U, V>& input, ublas::matrix<T, U, V>& inverse)
{
  using namespace boost::numeric::ublas;
  typedef permutation_matrix<std::size_t> pmatrix;
// create a working copy of the input
  matrix<T, U, V> A(input);
// create a permutation matrix for the LU-factorization
  pmatrix pm(A.size1());

// perform LU-factorization
  int res = lu_factorize(A,pm);
  if (res != 0) return false;

// create identity matrix of "inverse"
  inverse.assign(ublas::identity_matrix<T>(A.size1()));

// backsubstitute to get the inverse
  lu_substitute(A, pm, inverse);

  return true;
}

#endif //INVERT_MATRIX_HPP

void invertMatrix(const SimpleMatrix& input, SimpleMatrix& output)
{
  InvertMatrix(*input.dense(), *output.dense());
}

/* XXX Find out if we can use an elementwise ublas operation */
SP::SiconosVector compareMatrices(const SimpleMatrix& data, const SimpleMatrix& ref)
{
  SimpleMatrix diff(data.size(0), data.size(1));
  SP::SiconosVector res(new SiconosVector(data.size(1)));
  diff = data - ref;
  for (unsigned int i = 0; i < data.size(0); ++i)
  {
    for (unsigned int j = 0; j < data.size(1); ++j)
      diff(i, j) /= 1 + fabs(ref(i, j));
  }
  diff.normInfByColumn(res);
  return res;

}


// Copyright András Vukics 2006–2020. Distributed under the Boost Software License, Version 1.0. (See accompanying file LICENSE.txt)
#include "ExampleMode.h"

#include "ElementLiouvillean.h"
#include "Tridiagonal.tcc"
#include "TridiagonalHamiltonian.h"

#include <boost/bind.hpp>

using boost::bind;

void aJump   (StateVectorLow&, double kappa_nPlus1);
void aDagJump(StateVectorLow&, double kappa_n     );

double aJumpRate   (const LazyDensityOperator&, double kappa_nPlus1);
double aDagJumpRate(const LazyDensityOperator&, double kappa_n     );

basic::PumpedLossyMode::PumpedLossyMode(double delta, double kappa, dcomp eta, double nTh, size_t cutoff)
  : Free(cutoff,{},
         {
           CF{"(kappa*(2*nTh+1),delta)",dcomp(kappa*(2*nTh+1),delta),     cutoff },
           CF{"eta"                    ,eta                         ,sqrt(cutoff)}
         }),
    TridiagonalHamiltonian<1,false>(dcomp(-kappa*(2*nTh+1),delta)*nop(cutoff)
                                    +
                                    tridiagPlusHC_overI(conj(eta)*aop(cutoff))),
    ElementLiouvilleanStrategies<1,2>(JumpStrategies(bind(aJump   ,_1,kappa*(nTh+1)),
                                                     bind(aDagJump,_1,kappa* nTh   )),
                                      JumpRateStrategies(bind(aJumpRate   ,_1,kappa*(nTh+1)),
                                                         bind(aDagJumpRate,_1,kappa* nTh   )),
                                      "Mode",{"photon loss","photon absorption"}),
    ElementAveraged<1>("PumpedLossyMode",{"<number operator>","real(<ladder operator>)","imag(\")"})
{
  getParsStream()<<"Pumped lossy mode";
}

auto basic::PumpedLossyMode::average_v(NoTime, const LazyDensityOperator& matrix) const -> const Averages
{
  auto averages(initializedAverages());

  averages(0)=aJumpRate(matrix,1);

  for (int n=1; n<int(matrix.getDimension()); n++) {
    double sqrtn=sqrt(n);
    dcomp offdiag(matrix(n)(n-1));
    averages(1)+=sqrtn*real(offdiag);
    averages(2)+=sqrtn*imag(offdiag);
  }

  return averages;

}

const Tridiagonal::Diagonal mainDiagonal(dcomp z, size_t cutoff);

basic::PumpedLossyModeIP::PumpedLossyModeIP(double delta, double kappa, dcomp eta, double nTh, size_t cutoff)
  : Free(cutoff,
         {
           RF{"kappa*(2*nTh+1)",kappa*(2*nTh+1),cutoff},
           RF{"delta",delta,1}
         },
         {CF{"eta",eta,sqrt(cutoff)}}),
    FreeExact<false>(cutoff),
    TridiagonalHamiltonian<1,true>(furnishWithFreqs(tridiagPlusHC_overI(conj(eta)*aop(cutoff)),
                                                    mainDiagonal(dcomp(kappa*(2*nTh+1),-delta),cutoff))),
    ElementLiouvilleanStrategies<1,2>(JumpStrategies(bind(aJump   ,_1,kappa*(nTh+1)),
                                                     bind(aDagJump,_1,kappa* nTh   )),
                                      JumpRateStrategies(bind(aJumpRate   ,_1,kappa*(nTh+1)),
                                                         bind(aDagJumpRate,_1,kappa* nTh   )),
                                      "Mode",{"photon loss","photon absorption"}),
    ElementAveraged<1>("PumpedLossyMode",{"<number operator>","real(<ladder operator>)","imag(\")"}),
    z_(kappa*(2*nTh+1),-delta)
{}

void basic::PumpedLossyModeIP::updateU(OneTime dtDid) const
{
  getDiagonal()=exp(-z_*(dtDid*blitz::tensor::i));
}

// PumpedLossyModeIP::average_v exactly the same as PumpedLossyMode::average_v above
auto basic::PumpedLossyModeIP::average_v(NoTime, const LazyDensityOperator& matrix) const -> const Averages
{
  auto averages(initializedAverages());

  averages(0)=aJumpRate(matrix,1);

  for (int n=1; n<int(matrix.getDimension()); n++) {
    double sqrtn=sqrt(n);
    dcomp offdiag(matrix(n)(n-1));
    averages(1)+=sqrtn*real(offdiag);
    averages(2)+=sqrtn*imag(offdiag);
  }

  return averages;

}

hierarchical::ModeBase::ModeBase(double kappa, double nTh, size_t cutoff)
  : Free(cutoff,{RF{"kappa*(2*nTh+1)",kappa*(2*nTh+1),cutoff}}),
    ElementLiouvillean<1,2>("Mode",{"photon loss","photon absorption"}),
    ElementAveraged<1>("PumpedLossyMode",{"<number operator>","real(<ladder operator>)","imag(\")"}),
    kappa_(kappa), nTh_(nTh)
{}

void hierarchical::ModeBase::doActWithJ(NoTime, StateVectorLow& psi, LindbladNo<0>) const
{
  aJump(psi,kappa_*(nTh_+1));
}

void hierarchical::ModeBase::doActWithJ(NoTime, StateVectorLow& psi, LindbladNo<1>) const
{
  aDagJump(psi,kappa_*nTh_);
}

double hierarchical::ModeBase::rate(NoTime, const LazyDensityOperator& matrix, LindbladNo<0>) const
{
  return aJumpRate(matrix,kappa_*(nTh_+1));
}

double hierarchical::ModeBase::rate(NoTime, const LazyDensityOperator& matrix, LindbladNo<1>) const
{
  return aDagJumpRate(matrix,kappa_*nTh_);
}

// ModeBase::average_v exactly the same as PumpedLossyMode::average_v above
auto hierarchical::ModeBase::average_v (NoTime, const hierarchical::ModeBase::LazyDensityOperator& matrix) const -> const Averages
{
  auto averages(initializedAverages());

  averages(0)=aJumpRate(matrix,1);

  for (int n=1; n<int(matrix.getDimension()); n++) {
    double sqrtn=sqrt(n);
    dcomp offdiag(matrix(n)(n-1));
    averages(1)+=sqrtn*real(offdiag);
    averages(2)+=sqrtn*imag(offdiag);
  }

  return averages;

}

hierarchical::PumpedLossyMode::PumpedLossyMode(double delta, double kappa, dcomp eta, double nTh, size_t cutoff)
  : ModeBase(kappa,nTh,cutoff),
    TridiagonalHamiltonian<1,false>(dcomp(-kappa*(2*nTh+1),delta)*nop(cutoff)
                                    +
                                    tridiagPlusHC_overI(conj(eta)*aop(cutoff)))
{}

hierarchical::PumpedLossyModeIP::PumpedLossyModeIP(double delta, double kappa, dcomp eta, double nTh, size_t cutoff)
  : ModeBase(kappa,nTh,cutoff),
    FreeExact<false>(cutoff),
    TridiagonalHamiltonian<1,true>(furnishWithFreqs(tridiagPlusHC_overI(conj(eta)*aop(cutoff)),
                                                    mainDiagonal(dcomp(kappa*(2*nTh+1),-delta),cutoff))),
    z_(kappa*(2*nTh+1),-delta)
{}

// PumpedLossyModeIP::updateU exactly the same as above
void hierarchical::PumpedLossyModeIP::updateU(OneTime dtDid) const
{
  getDiagonal()=exp(-z_*(dtDid*blitz::tensor::i));
}


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
/*!
 * \file
**/
#ifndef BOOST_SIMD_PREDICATES_FUNCTIONS_IS_EVEN_HPP_INCLUDED
#define BOOST_SIMD_PREDICATES_FUNCTIONS_IS_EVEN_HPP_INCLUDED
#include <boost/simd/include/functor.hpp>
#include <boost/dispatch/include/functor.hpp>

/*!
 * \ingroup boost_simd_predicates
 * \defgroup boost_simd_predicates_is_even is_even
 *
 * \par Description
 * Returns True<result_type>() or False<result_type>() according a0 is even or not.
 * A floating number is even if it is a  flint
 * and divided by two it is still a flint
 * \par
 * A flint is a 'floating integer' i.e. a floating number
 * representing an integer value
 * \par
 * Be conscious that all sufficiently great floating points values are even...
 *
 * \par Header file
 *
 * \code
 * #include <nt2/include/functions/is_even.hpp>
 * \endcode
 *
 *
 * \synopsis
 *
 * \code
 * namespace boost::simd
 * {
 *   template <class A0>
 *     meta::call<tag::is_even_(A0)>::type
 *     is_even(const A0 & a0);
 * }
 * \endcode
 *
 * \param a0 the unique parameter of is_even
 *
 * \return an integer value
 *
 * \par Notes
 * In SIMD mode, this function acts elementwise on the inputs vectors elements
 * \par
 * This is a predicate operation. Such operations return bool in scalar mode,
 * but not in SIMD mode.
 * \par
 * The return type in SIMD mode is the common type of the parameters and is
 * a 'signed boolean' type. This means that in this case True has all its bits
 * sets to one. This is to facilitate masking operations. You are invited to
 * consult the rationale.
 *
**/

namespace boost { namespace simd { namespace tag
  {
    /*!
     * \brief Define the tag is_even_ of functor is_even
     *        in namespace boost::simd::tag for toolbox boost.simd.predicates
    **/
    struct is_even_ : ext::elementwise_<is_even_> { typedef ext::elementwise_<is_even_> parent; };
  }
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::is_even_, is_even, 1)
} }

#endif

// modified by jt the 25/12/2010


#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/io/pcd_io.h>
#include <pcl/features/fpfh.h>
#include <pcl/features/normal_3d.h>
#include <pcl/features/moment_of_inertia_estimation.h>
#include <boost/foreach.hpp>
#include <algorithm>
#include <Eigen/Core>
#include <Eigen/Eigen>
#include <Eigen/Dense>
#include <sys/types.h>
#include <dirent.h>
#include <errno.h>
#include <vector>
#include <string>
#include <iostream>
#include <fstream>
using namespace std;

class Viewer
{
public:
  Viewer()
    : viewer_(new pcl::visualization::PCLVisualizer( "Point Cloud Viewer" )),
      dir_("."),
      buffer_cloud_(new pcl::PointCloud<pcl::PointXYZI>)
  {
    getdir(dir_, files_);
  }

  void run()
  {
    viewer_->registerKeyboardCallback( &Viewer::keyboard_callback, *this );
    viewer_->setBackgroundColor (255, 255, 255);
    string filename = files_.back();
    std::cout << filename << std::endl;
    files_.pop_back();
    std::cout << "==================================" << std::endl;
    std::cout << "Now opening file is : " << filename << std::endl;
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
    pcl::io::loadPCDFile (filename, *cloud);
    //buffer_cloud_ = cloud->makeShared();
    pcl::io::loadPCDFile (filename, *buffer_cloud_);
    std::cout << "cloud points : " << cloud->points.size() << std::endl;
    viewer_->addPointCloud(cloud, "Cloud");

    while (!viewer_->wasStopped ())
    {
      viewer_->spinOnce (100);
      boost::this_thread::sleep (boost::posix_time::microseconds (100000));
    }
  }

private:

  void keyboard_callback( const pcl::visualization::KeyboardEvent& event, void* )
  {
    if( event.getKeyCode() && event.keyDown() ){

      if(event.getKeyCode() == 'y' || event.getKeyCode() == 'n'){
        std::cout << "Key : " << event.getKeyCode() << std::endl;
        //ここで特徴量の計算
        std::vector<float> features;
        feature_calculation(buffer_cloud_, features);
        if (event.getKeyCode() == 'y') {
          output_features(1, features);
        }else if(event.getKeyCode() == 'n')
        {
          output_features(0, features);
        }
        //新しいポイントクラウドの読み込み
        string filename = files_.back();
        std::cout << "==================================" << std::endl;
        std::cout << "Now opening file is : " << filename << std::endl;
        files_.pop_back();
        pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
        pcl::io::loadPCDFile (filename, *cloud);
        pcl::io::loadPCDFile (filename, *buffer_cloud_);
        //buffer_cloud_ = cloud->makeShared();
        std::cout << "cloud points : " << cloud->points.size() << std::endl;
        viewer_->updatePointCloud(cloud, "Cloud");

      }
    }
  }

  void feature_calculation(pcl::PointCloud<pcl::PointXYZI>::Ptr cloud,
                           std::vector<float> &features)
  {
    int n;
    float f21,f22,f23,f24,f25,f26;
    float v1, v2, v3;
    double v11, v22, v33;
    float f31,f32,f33,f34,f35,f36,f37;
    pcl::PointXYZI min_point;
    pcl::PointXYZI max_point;
    std::vector<float> moment_of_inertia;
    std::vector<float> eccentricity;
    Eigen::Matrix3f cmat;
    Eigen::Vector4f xyz_centroid;
    EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;
    pcl::MomentOfInertiaEstimation <pcl::PointXYZI> feature_extractor;
    feature_extractor.setInputCloud (cloud);
    feature_extractor.compute ();
    feature_extractor.getMomentOfInertia (moment_of_inertia);
    feature_extractor.getEccentricity (eccentricity);
    feature_extractor.getAABB (min_point, max_point);
    pcl::compute3DCentroid (*cloud, xyz_centroid);
    pcl::computeCovarianceMatrix (*cloud, xyz_centroid, covariance_matrix);
    n = cloud->points.size();
    f21 = covariance_matrix (0,0) / (n - 1);
    f22 = covariance_matrix (0,1) / (n - 1);
    f23 = covariance_matrix (0,2) / (n - 1);
    f24 = covariance_matrix (1,1) / (n - 1);
    f25 = covariance_matrix (1,2) / (n - 1);
    f26 = covariance_matrix (2,2) / (n - 1);
    cmat (0,0) = f21;
    cmat (0,1) = f22;
    cmat (0,2) = f23;
    cmat (1,0) = f22;
    cmat (1,1) = f24;
    cmat (1,2) = f25;
    cmat (2,0) = f23;
    cmat (2,1) = f25;
    cmat (2,2) = f26;
    Eigen::EigenSolver<Eigen::MatrixXf> es(cmat, false);
    complex<double> lambda1 = es.eigenvalues()[0];
    complex<double> lambda2 = es.eigenvalues()[1];
    complex<double> lambda3 = es.eigenvalues()[2];
    v11 = lambda1.real();
    v22 = lambda2.real();
    v33 = lambda3.real();
    std::vector<double> v;
    v.push_back (v11);
    v.push_back (v22);
    v.push_back (v33);
    std::sort(v.begin(), v.end() );
    v3 = v[0];
    v2 = v[1];
    v1 = v[2];
    v.clear();

    double nnn = v1 * v2 * v3;
    f31 = (v1 - v2) / v1;
    f32 = (v2 - v3) / v1;
    f33 = v3 / v1;
    f34 = pow(nnn, 1.0 / 3.0);
    f35 = (v1 - v3) / v1;
    f36 = -(v1 * log(v1) + v2 * log(v2) + v3 * log(v3));
    f37 = v3 / (v1 + v2 + v3);

    features.push_back(f21);
    features.push_back(f22);
    features.push_back(f23);
    features.push_back(f24);
    features.push_back(f25);
    features.push_back(f26);
    features.push_back(f31);
    features.push_back(f32);
    features.push_back(f33);
    features.push_back(f34);
    features.push_back(f35);
    features.push_back(f36);
    features.push_back(f37);

  }

  void output_features(int label, std::vector<float> features)
  {
    output_file_.open("train.csv", ios::ate | ios::app);
    output_file_ << label;
    for (int i = 0; i < features.size(); ++i) {
      output_file_ << " " << i+1 << ":" << features[i];
    }
    output_file_ << "\n";
    output_file_.close();
  }

  int getdir (string dir, vector<string> &files)
  {
    DIR *dp;
    struct dirent *dirp;
    if((dp  = opendir(dir.c_str())) == NULL) {
        cout << "Error(" << errno << ") opening " << dir << endl;
        return errno;
    }

    while ((dirp = readdir(dp)) != NULL) {
        files.push_back(string(dirp->d_name));
    }
    closedir(dp);
    sort( files_.begin(), files_.end() );
    for (size_t i = 0; i < files_.size(); ++i) {
      std::cout << files_[i] << std::endl;
    }

    return 0;
  }

  boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer_;
  string dir_;
  vector<string> files_;
  pcl::PointCloud<pcl::PointXYZI>::Ptr buffer_cloud_;
  std::ofstream output_file_;
};

int main( int argc, char* argv[] )
{
  Viewer viewer;
  viewer.run();

  return 0;
}


#include <iostream>
#include <type_traits>
#include <boost/type_index.hpp>

using boost::typeindex::type_id;

template<typename T1, typename T2>
void type_traits_example(T1& v1, T2& /*v2*/)
{
    std::cout << "\n" << __PRETTY_FUNCTION__ << "\n";
    std::cout << "is_unsigned_v<T1> = " << std::is_unsigned_v<T1> << "\n";
    std::cout << "is_same_v<T1,T2>  = " << std::is_same_v<T1,T2> << "\n";

    using t1_nonconst_t = std::remove_const_t<T1>;
    static_assert(!std::is_const_v<t1_nonconst_t>, "remove_const_t<T1> returned non-const T");
    std::cout << "remove_const_t<t1_nonconst_t> = "
        << type_id<t1_nonconst_t>().pretty_name().c_str() << "\n";

    using v1_nonconst_t = std::remove_const_t<decltype(v1)>;
    static_assert(!std::is_const_v<v1_nonconst_t>,
                  "remove_const_t<decltype(v1)> returned const T");
    std::cout << "decltype(v1) = "
        << type_id<decltype(v1)>().pretty_name().c_str() << "\n"
        << "remove_const_t<v1_nonconst_T> = "
        << type_id<v1_nonconst_t>().pretty_name().c_str() << "\n";
}

int main()
{
    const auto u = unsigned{42};
    auto       i = int{-42};
    type_traits_example(u, i);
    type_traits_example(i, u);
}


// Copyright (c) 2016
// Author: Chrono Law
#include <std.hpp>
using namespace std;

#include <boost/generator_iterator.hpp>
#include <boost/random.hpp>
using namespace boost;

///////////////////////////////////////

void case1()
{
    boost::rand48 rng;

    auto iter = make_generator_iterator(rng);

    for (int i = 0;i < 5; ++i)
    {
        cout << *++iter << ",";
    }

}

///////////////////////////////////////

int main()
{
    std::cout << "hello generator_iterator" << std::endl;

    case1();
}


// HEADER
#include <path_follower/controller/robotcontroller_potential_field_TT.h>

// THIRD PARTY
#include <tf/tf.h>

// PROJECT
#include <path_follower/utils/pose_tracker.h>
#include <path_follower/utils/obstacle_cloud.h>
#include <path_follower/parameters/path_follower_parameters.h>
#include <path_follower/collision_avoidance/collision_avoider.h>
#include <cslibs_navigation_utilities/MathHelper.h>

// SYSTEM
#include <boost/algorithm/clamp.hpp>

#include <path_follower/factory/controller_factory.h>

REGISTER_ROBOT_CONTROLLER(RobotController_Potential_Field_TT, potential_field_TT, default_collision_avoider);

RobotController_Potential_Field_TT::RobotController_Potential_Field_TT():
    RobotController_Potential_Field()
{
}

void RobotController_Potential_Field_TT::setGoalPosition()
{

    Eigen::Vector3d current_pose = pose_tracker_->getRobotPose();
    double x_meas = current_pose[0];
    double y_meas = current_pose[1];
    double theta_meas = current_pose[2];

    ///PERSON FOLLOWING
    double s_diff = std::numeric_limits<double>::max();

    // desired distance behind the person in path coordinates
    double s_dist = 2.0;
    tf::Point goal (0,0,0);
    for(int i = proj_ind_; i < ((int)path_interpol.n())-1 ; i++){
        if(fabs(fabs(path_interpol.s(path_interpol.n()-1) - path_interpol.s(i)) - s_dist) < s_diff){
            s_diff = fabs(fabs(path_interpol.s(path_interpol.n()-1) - path_interpol.s(i)) - s_dist);
            tf::Point goal_tmp(path_interpol.p(i), path_interpol.q(i), 0.0);
            goal = goal_tmp;
        }
        if(fabs(fabs(path_interpol.s(path_interpol.n()-1) - path_interpol.s(proj_ind_)) - s_dist) < 1e-1){
            vn_ = 0.0;
        }
    }

    mGoalPosX = goal.getX();
    mGoalPosY = goal.getY();

    double dx = mGoalPosX - x_meas;
    double dy = mGoalPosY - y_meas;

    FAttX = opt_.kAtt()*(dx*cos(-theta_meas) - dy*sin(-theta_meas));
    FAttY = opt_.kAtt()*(dx*sin(-theta_meas) + dy*cos(-theta_meas));

}


#ifndef _DSExprImpl_Hpp_
#define _DSExprImpl_Hpp_

#include <iosfwd>
#include <string>

#include <boost/function.hpp>

#include <Lintel/Double.hpp>

#include <DataSeries/DSExpr.hpp>
#include <DataSeries/GeneralField.hpp>

#define YY_DECL                                                         \
    DSExprImpl::Parser::token_type                                      \
    DSExprScanlex(DSExprImpl::Parser::semantic_type *yylval, void * yyscanner)

namespace DSExprImpl {

    using namespace std;
    using namespace boost;

    // TODO: make valGV to do general value calculations.

    class ExprNumericConstant : public DSExpr {
      public:
        // TODO: consider parsing the string as both a double and an
        // int64 to get better precision.
        ExprNumericConstant(double v) : val(v) {}

        virtual expr_type_t getType() {
            return t_Numeric;
        }

        virtual double valDouble() { return val; }
        virtual int64_t valInt64() { return static_cast<int64_t>(val); }
        virtual bool valBool() { return val ? true : false; }
        virtual const string valString() {
            FATAL_ERROR("no automatic coercion of numeric constants to string");
        }

        virtual void dump(ostream &out);
        virtual bool isNull() {
            return false;
        }
      private:
        double val;
    };

    class ExprField : public DSExpr {
      public:
        ExprField(ExtentSeries &series, const string &fieldname);

        virtual ~ExprField() {
            delete field;
        };

        virtual expr_type_t getType() {
            if (field->getType() == ExtentType::ft_variable32) {
                return t_String;
            } else {
                return t_Numeric;
            }
        }

        virtual double valDouble() {
            return field->valDouble();
        }
        virtual int64_t valInt64() {
            return GeneralValue(*field).valInt64();
        }
        virtual bool valBool() {
            return GeneralValue(*field).valBool();
        }
        virtual const string valString() {
            return GeneralValue(*field).valString();
        }
        virtual bool isNull() {
            return field->isNull();
        }

        virtual void dump(ostream &out);

      private:
        GeneralField *field;
        string fieldname;
    };

    class ExprStrLiteral : public DSExpr {
      public:
        ExprStrLiteral(const string &l);

        virtual ~ExprStrLiteral() {}

        virtual expr_type_t getType() {
            return t_String;
        }

        virtual double valDouble() {
            FATAL_ERROR("no automatic coercion of string literals to double");
        }
        virtual int64_t valInt64() {
            FATAL_ERROR("no automatic coercion of string literals to integer");
        }
        virtual bool valBool() {
            FATAL_ERROR("no automatic coercion of string literals to boolean");
        }
        virtual const string valString() {
            return s;
        }
        virtual bool isNull() {
            return false;
        }

        virtual void dump(ostream &out);

      private:
        string s;
    };

    class ExprUnary : public DSExpr {
      public:
        ExprUnary(DSExpr *_subexpr)
        : subexpr(_subexpr) {}
        virtual ~ExprUnary() {
            delete subexpr;
        }

        virtual void dump(ostream &out);

        virtual string opname() const {
            FATAL_ERROR("either override dump or defined opname");
        }

        virtual bool isNull() {
            return subexpr->isNull();
        }
      protected:
        DSExpr *subexpr;
    };

    class ExprBinary : public DSExpr {
      public:
        ExprBinary(DSExpr *_left, DSExpr *_right)
                : left(_left), right(_right) {}
        virtual ~ExprBinary() {
            delete left;
            delete right;
        }

        bool either_string() const
        {
            return ((left->getType() == t_String) ||
                    (right->getType() == t_String));
        }

        virtual void dump(ostream &out);

        virtual string opname() const {
            FATAL_ERROR("either override dump or defined opname");
        }

        virtual bool isNull() {
            return left->isNull() || right->isNull();
        }
      protected:
        DSExpr *left, *right;
    };

    class ExprMinus : public ExprUnary {
      public:
        ExprMinus(DSExpr *subexpr)
        : ExprUnary(subexpr) {}

        virtual expr_type_t getType() {
            return t_Numeric;
        }

        virtual double valDouble() {
            return - subexpr->valDouble();
        }
        virtual int64_t valInt64() {
            return - subexpr->valInt64();
        }
        virtual bool valBool() {
            FATAL_ERROR("the unary - operation is not well defined on booleans");
        }
        virtual const string valString() {
            FATAL_ERROR("the unary - operation is not well defined on strings");
        }

        virtual string opname() const { return string("-"); }
    };

    class ExprAdd : public ExprBinary {
      public:
        ExprAdd(DSExpr *left, DSExpr *right) :
                ExprBinary(left,right) {}

        virtual expr_type_t getType() {
            return t_Numeric;
        }

        virtual double valDouble() {
            return left->valDouble() + right->valDouble();
        }
        virtual int64_t valInt64() {
            // TODO: add check for overflow?
            return left->valInt64()+ right->valInt64();
        }
        virtual bool valBool() {
            FATAL_ERROR("the + operation is not well defined on booleans");
        }
        virtual const string valString() {
            return left->valString() + right->valString();
        }

        virtual string opname() const { return string("+"); }
    };

    class ExprSubtract : public ExprBinary {
      public:
        ExprSubtract(DSExpr *left, DSExpr *right) :
                ExprBinary(left,right) {}
        virtual expr_type_t getType() {
            return t_Numeric;
        }
        virtual double valDouble() {
            return left->valDouble() - right->valDouble();
        }
        virtual int64_t valInt64() {
            return left->valInt64() - right->valInt64();
        }
        virtual bool valBool() {
            FATAL_ERROR("the - operation is not well defined on booleans");
        }
        virtual const string valString() {
            FATAL_ERROR("the - operation is not well defined on strings");
        }

        virtual string opname() const { return string("-"); }
    };

    class ExprMultiply : public ExprBinary {
      public:
        ExprMultiply(DSExpr *left, DSExpr *right) :
                ExprBinary(left,right) {}

        virtual expr_type_t getType() {
            return t_Numeric;
        }

        virtual double valDouble() {
            return left->valDouble() * right->valDouble();
        }
        virtual int64_t valInt64() {
            return left->valInt64() * right->valInt64();
        }
        virtual bool valBool() {
            // TODO-reviewer: this one could be well defined as "and"; we could define + as or.
            // opinions?
            FATAL_ERROR("the * operation is not well defined on booleans");
        }
        virtual const string valString() {
            FATAL_ERROR("the * operation is not well defined on strings");
        }

        virtual string opname() const { return string("*"); }
    };

    class ExprDivide : public ExprBinary {
      public:
        ExprDivide(DSExpr *left, DSExpr *right) :
                ExprBinary(left,right) {}

        virtual expr_type_t getType() {
            return t_Numeric;
        }

        virtual double valDouble() {
            return left->valDouble() / right->valDouble();
        }
        virtual int64_t valInt64() {
            return left->valInt64() / right->valInt64();
        }
        virtual bool valBool() {
            FATAL_ERROR("the / operation is not well defined on booleans");
        }
        virtual const string valString() {
            FATAL_ERROR("the / operation is not well defined on strings");
        }

        virtual string opname() const { return string("/"); }
    };

    class ExprEq : public ExprBinary {
      public:
        ExprEq(DSExpr *l, DSExpr *r)
                : ExprBinary(l,r) {}
        virtual expr_type_t getType() {
            return t_Bool;
        }
        virtual double valDouble() {
            FATAL_ERROR("evaluating == as a double is not well defined");
        }
        virtual int64_t valInt64() {
            FATAL_ERROR("evaluating == as an int64 is not well defined");
        }
        virtual bool valBool() {
            if (either_string()) {
                return (left->valString() == right->valString());
            } else {
                return Double::eq(left->valDouble(), right->valDouble());
            }
        }
        virtual const string valString() {
            FATAL_ERROR("evaluating == as a string is not well defined");
        }

        virtual string opname() const { return string("=="); }
    };

    class ExprNeq : public ExprBinary {
      public:
        ExprNeq(DSExpr *l, DSExpr *r)
                : ExprBinary(l,r) {}

        virtual expr_type_t getType() {
            return t_Bool;
        }

        virtual double valDouble() {
            FATAL_ERROR("evaluating != as a double is not well defined");
        }
        virtual int64_t valInt64() {
            FATAL_ERROR("evaluating != as an int64 is not well defined");
        }
        virtual bool valBool() {
            if (either_string()) {
                return (left->valString() != right->valString());
            } else {
                return !Double::eq(left->valDouble(), right->valDouble());
            }
        }
        virtual const string valString() {
            FATAL_ERROR("evaluating != as a string is not well defined");
        }

        virtual string opname() const { return string("!="); }
    };

    class ExprGt : public ExprBinary {
      public:
        ExprGt(DSExpr *l, DSExpr *r)
                : ExprBinary(l,r) {}

        virtual expr_type_t getType() {
            return t_Bool;
        }

        virtual double valDouble() {
            FATAL_ERROR("evaluating > as a double is not well defined");
        }
        virtual int64_t valInt64() {
            FATAL_ERROR("evaluating > as an int64 is not well defined");
        }
        virtual bool valBool() {
            if (either_string()) {
                return (left->valString() > right->valString());
            } else {
                return Double::gt(left->valDouble(), right->valDouble());
            }
        }
        virtual const string valString() {
            FATAL_ERROR("evaluating > as a string is not well defined");
        }

        virtual string opname() const { return string(">"); }
    };

    class ExprLt : public ExprBinary {
      public:
        ExprLt(DSExpr *l, DSExpr *r)
                : ExprBinary(l,r) {}
        virtual expr_type_t getType() {
            return t_Bool;
        }
        virtual double valDouble() {
            FATAL_ERROR("evaluating < as a double is not well defined");
        }
        virtual int64_t valInt64() {
            FATAL_ERROR("evaluating < as an int64 is not well defined");
        }
        virtual bool valBool() {
            if (either_string()) {
                return (left->valString() < right->valString());
            } else {
                return Double::lt(left->valDouble(), right->valDouble());
            }
        }
        virtual const string valString() {
            FATAL_ERROR("evaluating < as a string is not well defined");
        }

        virtual string opname() const { return string("<"); }
    };

    class ExprGeq : public ExprBinary {
      public:
        ExprGeq(DSExpr *l, DSExpr *r)
                : ExprBinary(l,r) {}

        virtual expr_type_t getType() {
            return t_Bool;
        }

        virtual double valDouble() {
            FATAL_ERROR("evaluating >= as a double is not well defined");
        }
        virtual int64_t valInt64() {
            FATAL_ERROR("evaluating >= as an int64 is not well defined");
        }
        virtual bool valBool() {
            if (either_string()) {
                return (left->valString() >= right->valString());
            } else {
                return Double::geq(left->valDouble(), right->valDouble());
            }
        }
        virtual const string valString() {
            FATAL_ERROR("evaluating >= as a string is not well defined");
        }

        virtual string opname() const { return string(">="); }
    };

    class ExprLeq : public ExprBinary {
      public:
        ExprLeq(DSExpr *l, DSExpr *r)
                : ExprBinary(l,r) {}

        virtual expr_type_t getType() {
            return t_Bool;
        }

        virtual double valDouble() {
            FATAL_ERROR("evaluating <= as a double is not well defined");
        }
        virtual int64_t valInt64() {
            FATAL_ERROR("evaluating <= as a int64 is not well defined");
        }
        virtual bool valBool() {
            if (either_string()) {
                return (left->valString() <= right->valString());
            } else {
                return Double::leq(left->valDouble(), right->valDouble());
            }
        }
        virtual const string valString() {
            FATAL_ERROR("evaluating <= as a string is not well defined");
        }

        virtual string opname() const { return string("<="); }
    };

    class ExprLor : public ExprBinary {
      public:
        ExprLor(DSExpr *l, DSExpr *r)
                : ExprBinary(l,r) {}

        virtual expr_type_t getType() {
            return t_Bool;
        }

        virtual double valDouble() {
            FATAL_ERROR("evaluating || as a double is not well defined");
        }
        virtual int64_t valInt64() {
            FATAL_ERROR("evaluating || as an int64 is not well defined");
        }
        virtual bool valBool() {
            return (left->valBool() || right->valBool());
        }
        virtual const string valString() {
            FATAL_ERROR("evaluating || as a string is not well defined");
        }

        virtual string opname() const { return string("||"); }
    };

    class ExprLand : public ExprBinary {
      public:
        ExprLand(DSExpr *l, DSExpr *r)
                : ExprBinary(l,r) {}
        virtual expr_type_t getType() {
            return t_Bool;
        }
        virtual double valDouble() {
            FATAL_ERROR("evaluating && as a double is not well defined");
        }
        virtual int64_t valInt64() {
            FATAL_ERROR("evaluating && as an int64 is not well defined");
        }
        virtual bool valBool() {
            return (left->valBool() && right->valBool());
        }
        virtual const string valString() {
            FATAL_ERROR("evaluating && as a string is not well defined");
        }

        virtual string opname() const { return string("&&"); }
    };

    class ExprLnot : public ExprUnary {
      public:
        ExprLnot(DSExpr *subexpr)
        : ExprUnary(subexpr) {}

        virtual expr_type_t getType() {
            return t_Bool;
        }

        virtual double valDouble() {
            FATAL_ERROR("evaluating ! as a double is not well defined");
        }
        virtual int64_t valInt64() {
            FATAL_ERROR("evaluating ! as an int64 is not well defined");
        }
        virtual bool valBool() {
            return !(subexpr->valBool());
        }
        virtual const string valString() {
            FATAL_ERROR("evaluating ! as a string is not well defined");
        }

        virtual string opname() const { return string("!"); }
    };

    class ExprFnTfracToSeconds : public ExprUnary {
      public:
        ExprFnTfracToSeconds(DSExpr *subexpr)
        : ExprUnary(subexpr)
        {}

        virtual expr_type_t getType() {
            return t_Numeric;
        }

        virtual double valDouble() {
            return subexpr->valInt64() / 4294967296.0;
        }
        virtual int64_t valInt64() {
            FATAL_ERROR("shouldn't try to get valInt64 using fn.TfracToSeconds, it drops too much precision");
        }
        virtual bool valBool() {
            FATAL_ERROR("shouldn't try to get valBool using fn.TfracToSeconds, it drops too much precision");
        }
        virtual const string valString() {
            FATAL_ERROR("shouldn't try to get valString using fn.TfracToSeconds, definition unclear");
        }

        virtual void dump(ostream &out) {
            out << "fn.TfracToSeconds(";
            subexpr->dump(out);
            out << ")";
        }
    };

    class ExprFunctionApplication : public DSExpr {
      public:
        ExprFunctionApplication(const string &fnname, const DSExpr::List &fnargs)
                : name(fnname), args(fnargs)
        {}
        virtual ~ExprFunctionApplication();

        virtual expr_type_t getType() {
            return t_Unknown;
        }

        virtual double valDouble() {
            return 0.0;
        }
        virtual int64_t valInt64() {
            return 0;
        }
        virtual bool valBool() {
            return false;
        }
        virtual const string valString() {
            return "";
        }

        virtual void dump(ostream &out);

        virtual bool isNull() {
            FATAL_ERROR("TODO: figure out why we instantiate this class directly");
        }

      private:
        const string name;
        DSExpr::List args;
    };

    class Driver {
      public:
        typedef DSExprParser::Selector Selector;
        typedef DSExprParser::FieldNameToSelector FieldNameToSelector;

        Driver(ExtentSeries &series)
        : expr(NULL), series(&series), field_name_to_selector(), scanner_state(NULL),
          current_fnargs()
        { }

        Driver(const FieldNameToSelector &field_name_to_selector)
        : expr(NULL), series(NULL), field_name_to_selector(field_name_to_selector),
          scanner_state(NULL), current_fnargs()
        {
            SINVARIANT(!!field_name_to_selector);
        }

        ExprField *makeExprField(const string &field_name) {
            if (series == NULL) {
                Selector tmp = field_name_to_selector(field_name);
                INVARIANT(tmp.first != NULL, format("field_name '%s' is not defined") % field_name);
                return new ExprField(*tmp.first, tmp.second);
            } else {
                return new ExprField(*series, field_name);
            }
        }
        ~Driver();

        void doit(const string &str);

        void startScanning(const string &str);
        void finishScanning();

        DSExpr *expr;
        ExtentSeries *series;
        const FieldNameToSelector field_name_to_selector;
        void *scanner_state;
        DSExpr::List current_fnargs;
    };

};

#endif