#pragma once
//===----------------------------------------------------------------------===//
#include <dtl/bitmap/util/plain_bitmap.hpp>
#include <dtl/bitmap/util/plain_bitmap_iter.hpp>
#include <dtl/dtl.hpp>

#include <boost/dynamic_bitset.hpp>

#include <cstddef>
#include <type_traits>
#include <vector>
//===----------------------------------------------------------------------===//
namespace dtl {
//===----------------------------------------------------------------------===//
/// Un-Aligned Hybrid: An RLE compressed representation of a bitmap of length N.
/// Unlike WAH or BBC, the encoding is not word or byte aligned.
template<typename _word_type = u32>
class uah {

protected:
  using word_type = typename std::remove_cv<_word_type>::type;
  static_assert(std::is_integral<word_type>::value,
      "The word type must be an integral type.");
  static_assert(!std::is_same<word_type, bool>::value,
      "The word type must not be a boolean.");

  static constexpr std::size_t word_bitlength = sizeof(word_type) * 8;
  static constexpr std::size_t max_fill_length = (1ull << (word_bitlength - 2)) - 1;
  /// The number of bits that can be stored in a literal word.
  static constexpr std::size_t payload_bit_cnt = word_bitlength - 1;
  static constexpr word_type all_ones = word_type(~word_type(0));

  /// The encoded bitmap.
  std::vector<word_type> data_;
  /// The length of the encoded bitmap.
  std::size_t encoded_bitmap_length_ = 0;

  std::size_t remaining_bit_cnt_in_last_literal_word_ = 0;

  /// Returns true if the given word is a literal word.
  static inline constexpr u1
  is_literal_word(const word_type w) noexcept {
    return (w & word_type(1)) == 0;
  }

  /// Returns true if the given word is a fill word.
  static inline constexpr u1
  is_fill_word(const word_type w) noexcept {
    return (w & word_type(1)) == 1;
  }

  /// Extract the fill value from a fill word.
  static inline constexpr u1
  extract_fill_value(const word_type w) noexcept {
    assert(is_fill_word(w));
    return (w & word_type(2)) != 0;
  }

  /// Extract the fill length from a fill word.
  static inline constexpr std::size_t
  extract_fill_length(const word_type w) noexcept {
    assert(is_fill_word(w));
    return w >> 2;
  }

  /// Creates a fill word.
  static inline constexpr word_type
  make_fill_word(u1 val, const std::size_t len) noexcept {
    assert(len <= max_fill_length);
    word_type w = val ? word_type(3) : word_type(1);
    w |= len << 2;
    return w;
  }

  /// Initialize the compressed bitmap with a 0- or 1-run of length 0.
  inline void
  init(u1 val) noexcept {
    assert(data_.size() == 0);
    data_.push_back(make_fill_word(val, 0));
  }

  /// Append a 0- or 1-run of the given length to the compressed bitmap.
  inline void
  append_run(u1 val, std::size_t len) {
    assert(data_.size() > 0);
    auto& w = data_.back();
    if (is_fill_word(w)) {
      const auto last_fill_len = extract_fill_length(w);
      const auto last_fill_val = extract_fill_value(w);
      if (last_fill_val == val) {
        //===--------------------------------------------------------------===//
        // The run that is to be appended is of a SAME KIND as the
        // previous one.
        //===--------------------------------------------------------------===//

        // Extend the previous fill word.
        auto extend_by = std::min(max_fill_length - last_fill_len, len);
        w = make_fill_word(val, last_fill_len + extend_by);
        encoded_bitmap_length_ += extend_by;
        if (extend_by == len) {
          return; // Done.
        }
        // The previous fill word exceeded its max length. Thus, we have to
        // append a new fill word.
        len -= extend_by;
      }
      else {
        //===--------------------------------------------------------------===//
        // The run that is to be appended is of a DIFFERENT KIND than the
        // previous one.
        //===--------------------------------------------------------------===//

        // Try to save space by converting the last word into a literal word.
        if (last_fill_len < payload_bit_cnt) {
          // Convert the last word into a literal word.
          auto literal_word = last_fill_val
              ? (all_ones >> (word_bitlength - last_fill_len)) << 1
              : word_type(0);
          auto remaining_bits = payload_bit_cnt - last_fill_len;
          // Append the current run value.
          auto extend_by = std::min(remaining_bits, len);
          auto bits_to_append = val
              ? all_ones >> (word_bitlength - extend_by)
              : word_type(0);
          literal_word |=
              bits_to_append << (1 + payload_bit_cnt - remaining_bits);
          w = literal_word;
          encoded_bitmap_length_ += extend_by;
          remaining_bit_cnt_in_last_literal_word_ = remaining_bits - extend_by;
          if (extend_by == len) {
            return; // Done.
          }
          // The previous fill word exceeded its max length. Thus, we have to
          // append a new fill word.
          len -= extend_by;
        }
      }
    }
    else {
      // Append to the last literal word.
      if (remaining_bit_cnt_in_last_literal_word_ > 0) {
        auto extend_by = std::min(remaining_bit_cnt_in_last_literal_word_, len);
        if (val == true) {
          auto bits_to_append = all_ones >> (word_bitlength - extend_by);
          w |= bits_to_append
              << (word_bitlength - remaining_bit_cnt_in_last_literal_word_);
        }
        remaining_bit_cnt_in_last_literal_word_ -= extend_by;
        encoded_bitmap_length_ += extend_by;
        if (extend_by == len) {
          return; // Done.
        }
        len -= extend_by;
        // Could not fit the entire run into the last literal word.
      }
    }

    // Append remaining bits of the current run as a fill word.
    while (len > 0) {
      const auto fill_len = std::min(std::size_t(max_fill_length), len);
      data_.emplace_back(make_fill_word(val, fill_len));
      encoded_bitmap_length_ += fill_len;
      len -= fill_len;
    }
    remaining_bit_cnt_in_last_literal_word_ = 0;
  }

  /// Append a 0-run of the given length to the compressed bitmap.
  inline void
  append_zero_run(std::size_t len) {
    append_run(false, len);
  }

  /// Append a 1-run of the given length to the compressed bitmap.
  inline void
  append_one_run(const std::size_t len) {
    append_run(true, len);
  }

public:
  uah() = default;

  explicit uah(const boost::dynamic_bitset<$u32>& in) {
    // Obtain a 1-run iterator for the input bitmap.
    dtl::plain_bitmap_iter<boost::dynamic_bitset<$u32>> it(in);
    // Append runs to the compressed bitmap.
    std::size_t i = 0;
    if (it.pos() > 0) {
      // The bitmap starts with a 0-run.
      init(false);
      auto len = it.pos() - i;
      append_zero_run(len);
      i += len;
    }
    else {
      // The bitmap starts with a 0-run.
      init(true);
    }
    while (!it.end()) {
      auto len = it.length();
      append_one_run(len);
      i += len;
      it.next();
      if (!it.end()) {
        len = it.pos() - i;
        append_zero_run(len);
        i += len;
      }
    }
    if (i < in.size()) {
      append_zero_run(in.size() - i);
    }
    // Try to reduce the memory consumption.
    shrink();
  }

  ~uah() = default;
  uah(const uah& other) = default;
  uah(uah&& other) noexcept = default;
  uah& operator=(const uah& other) = default;
  uah& operator=(uah&& other) noexcept = default;

  /// Return the size in bytes.
  std::size_t __forceinline__
  size_in_bytes() const {
    return data_.size() * sizeof(word_type) /* size of the compressed bitmap */
        + sizeof(encoded_bitmap_length_); /* bit-length of the original bitmap */
  }

  /// Returns the size of the bitmap.
  std::size_t __forceinline__
  size() const {
    return encoded_bitmap_length_;
  }

  /// Conversion to an plain bitmap. // TODO remove
  dtl::plain_bitmap<$u64>
  to_plain_bitmap() {
    dtl::plain_bitmap<$u64> ret(encoded_bitmap_length_, false);
    std::size_t i = 0;
    for (std::size_t word_idx = 0; word_idx < data_.size(); ++word_idx) {
      auto& w = data_[word_idx];
      if (is_fill_word(w)) {
        auto val = extract_fill_value(w);
        auto len = extract_fill_length(w);
        ret.set(i, i + len, val);
        i += len;
      }
      else {
        const std::size_t cnt = word_idx == data_.size() - 1
            ? payload_bit_cnt - remaining_bit_cnt_in_last_literal_word_
            : payload_bit_cnt;
        for (std::size_t k = 0; k < cnt; ++k) {
          ret.set(i + k, dtl::bits::bit_test(w, k + 1));
        }
        i += cnt;
      }
    }
    if (i < encoded_bitmap_length_) {
      ret.clear(i, encoded_bitmap_length_);
    }
    return std::move(ret);
  }

  static std::string
  name() {
    return "uah" + std::to_string(word_bitlength);
  }

  /// Returns the value of the bit at the position pos.
  u1 __forceinline__
  test(const std::size_t pos) const {
    std::size_t i = 0;
    std::size_t word_idx = 0;
    // Find the corresponding word.
    for (; word_idx < data_.size(); ++word_idx) {
      auto& w = data_[word_idx];
      if (is_literal_word(w)) {
        if (pos >= i + payload_bit_cnt) {
          i += payload_bit_cnt;
          continue;
        }
        else {
          return dtl::bits::bit_test(w, pos - i + 1); // TODO optimize
        }
      }
      else {
        auto fill_len = extract_fill_length(w);
        if (pos >= i + fill_len) {
          i += fill_len;
          continue;
        }
        else {
          return extract_fill_value(w);
        }
      }
    }
    return false;
  }

  /// Try to reduce the memory consumption. This function is supposed to be
  /// called after the bitmap has been modified.
  __forceinline__ void
  shrink() {
    data_.shrink_to_fit();
  }

  //===--------------------------------------------------------------------===//
  /// 1-run iterator
  class iter {
    const uah& outer_;

    std::size_t word_idx_;
    std::size_t in_word_idx_;

    //===------------------------------------------------------------------===//
    // Iterator state
    //===------------------------------------------------------------------===//
    /// Points to the beginning of a 1-run.
    $u64 pos_;
    /// The length of the current 1-run.
    $u64 length_;
    //===------------------------------------------------------------------===//

  public:
    explicit __forceinline__
    iter(const uah& outer)
        : outer_(outer),
          word_idx_(0),
          in_word_idx_(0),
          pos_(0), length_(0) {
      const auto word_cnt = outer_.data_.size();
      word_type w;
      while (word_idx_ < word_cnt) {
        w = outer_.data_[word_idx_];
        if (is_fill_word(w)) {
          if (extract_fill_value(w) == false) {
            pos_ += extract_fill_length(w);
          }
          else {
            length_ = extract_fill_length(w);
            in_word_idx_ = 0;
            break;
          }
        }
        else {
          const word_type payload = w >> 1;
          if (payload == 0) {
            pos_ += payload_bit_cnt;
          }
          else {
            const std::size_t b = dtl::bits::tz_count(payload);
            std::size_t e = b + 1;
            for (; e < payload_bit_cnt; ++e) {
              u1 is_set = dtl::bits::bit_test(payload, e);
              if (!is_set) break;
            }
            pos_ += b;
            length_ = e - b;
            in_word_idx_ = e;
            break;
          }
        }
        ++word_idx_;
      }
      if (word_idx_ == word_cnt) {
        pos_ = outer_.encoded_bitmap_length_;
        length_ = 0;
      }
      else {
        if (is_fill_word(w)) {
          ++word_idx_;
          in_word_idx_ = 0;
        }
      }
    }

    /// Forward the iterator to the next 1-run.
    void __forceinline__
    next() {
      pos_ += length_;
      length_ = 0;
      const auto word_cnt = outer_.data_.size();
      word_type w;
      while (word_idx_ < word_cnt) {
        w = outer_.data_[word_idx_];
        if (is_fill_word(w)) {
          if (extract_fill_value(w) == false) {
            pos_ += extract_fill_length(w);
          }
          else {
            length_ = extract_fill_length(w);
            break;
          }
        }
        else {
          if (in_word_idx_ < payload_bit_cnt) { // TODO decode the entire literal word at once.
            const word_type payload = w >> (1 + in_word_idx_);
            if (payload == 0) {
              pos_ += payload_bit_cnt - in_word_idx_;
            }
            else {
              const std::size_t b = dtl::bits::tz_count(payload);
              std::size_t e = b + 1;
              for (; e < (payload_bit_cnt - in_word_idx_); ++e) {
                u1 is_set = dtl::bits::bit_test(payload, e);
                if (!is_set) break;
              }
              pos_ += b;
              length_ = e - b;
              in_word_idx_ += e;
              break;
            }
          }
        }
        ++word_idx_;
        in_word_idx_ = 0;
      }
      if (word_idx_ == word_cnt) {
        pos_ = outer_.encoded_bitmap_length_;
        length_ = 0;
      }
      else {
        if (is_fill_word(w)) {
          ++word_idx_;
          in_word_idx_ = 0;
        }
      }
    }

    /// Forward the iterator to the desired position.
    void __forceinline__
    skip_to(const std::size_t to_pos) {
      assert(pos_ <= to_pos);
      if (to_pos >= outer_.encoded_bitmap_length_) {
        pos_ = outer_.encoded_bitmap_length_;
        length_ = 0;
        return;
      }
      // Call next until the desired position has been reached.
      while (!end() && pos() + length() <= to_pos) {
        next();
      }
      // Adjust the current position and run length.
      if (!end() && pos() < to_pos) {
        length_ -= to_pos - pos_;
        pos_ = to_pos;
      }
    }

    u1 __forceinline__
    end() const noexcept {
      return length_ == 0;
    }

    u64 __forceinline__
    pos() const noexcept {
      return pos_;
    }

    u64 __forceinline__
    length() const noexcept {
      return length_;
    }
  };
  //===--------------------------------------------------------------------===//

  using skip_iter_type = iter;
  using scan_iter_type = iter;

  /// Returns a 1-run iterator.
  skip_iter_type __forceinline__
  it() const {
    return skip_iter_type(*this);
  }

  /// Returns a 1-run iterator.
  scan_iter_type __forceinline__
  scan_it() const {
    return scan_iter_type(*this);
  }

  /// Returns the name of the instance including the most important parameters
  /// in JSON.
  std::string
  info() const {
    return "{\"name\":\"" + name() + "\""
        + ",\"n\":" + std::to_string(encoded_bitmap_length_)
        + ",\"size\":" + std::to_string(size_in_bytes())
        + ",\"word_size\":" + std::to_string(sizeof(word_type))
        + "}";
  }

  // For debugging purposes.
  void
  print(std::ostream& os) const {
    os << "word idx | word type | content" << std::endl;
    for (std::size_t i = 0; i < data_.size(); ++i) {
      os << std::setw(8) << i << " | ";
      auto& w = data_[i];
      if (is_fill_word(w)) {
        os << "fill      | ";
        os << extract_fill_length(w) << " x ";
        os << (extract_fill_value(w) ? "'1'" : "'0'");
        os << std::endl;
      }
      else {
        os << "literal   | '";
        for (std::size_t k = 1; k < word_bitlength; ++k) {
          os << (dtl::bits::bit_test(w, k) ? "1" : "0");
        }
        os << "'";
        os << std::endl;
      }
    }
  }
};
//===----------------------------------------------------------------------===//
/// UAH compressed representation of a bitmap of length N using 8-bit words.
using uah8 = uah<u8>;
/// UAH compressed representation of a bitmap of length N using 16-bit words.
using uah16 = uah<u16>;
/// UAH compressed representation of a bitmap of length N using 32-bit words.
using uah32 = uah<u32>;
/// UAH compressed representation of a bitmap of length N using 64-bit words.
using uah64 = uah<u64>;
//===----------------------------------------------------------------------===//
} // namespace dtl


/*
 Copyright (C) 2017 Quaternion Risk Management Ltd
 All rights reserved.

 This file is part of ORE, a free-software/open-source library
 for transparent pricing and risk analysis - http://opensourcerisk.org

 ORE is free software: you can redistribute it and/or modify it
 under the terms of the Modified BSD License.  You should have received a
 copy of the license along with this program.
 The license is also available online at <http://opensourcerisk.org>

 This program is distributed on the basis that it will form a useful
 contribution to risk analytics and model standardisation, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/

#include <boost/lexical_cast.hpp>
#include <boost/timer.hpp>
#include <orea/cube/inmemorycube.hpp>
#include <orea/engine/stresstest.hpp>
#include <orea/engine/valuationengine.hpp>
#include <orea/scenario/clonescenariofactory.hpp>
#include <ored/utilities/log.hpp>
#include <ql/errors.hpp>
#include <ql/instruments/forwardrateagreement.hpp>
#include <ql/instruments/makeois.hpp>
#include <ql/instruments/makevanillaswap.hpp>
#include <ql/math/solvers1d/newtonsafe.hpp>
#include <ql/pricingengines/capfloor/bacheliercapfloorengine.hpp>
#include <ql/pricingengines/capfloor/blackcapfloorengine.hpp>
#include <ql/pricingengines/swap/discountingswapengine.hpp>
#include <ql/termstructures/yield/oisratehelper.hpp>
#include <qle/instruments/crossccybasisswap.hpp>
#include <qle/instruments/deposit.hpp>
#include <qle/instruments/fxforward.hpp>
#include <qle/pricingengines/crossccyswapengine.hpp>
#include <qle/pricingengines/depositengine.hpp>
#include <qle/pricingengines/discountingfxforwardengine.hpp>

#include <iomanip>
#include <iostream>

using namespace QuantLib;
using namespace QuantExt;
using namespace std;
using namespace ore::data;

namespace ore {
namespace analytics {

StressTest::StressTest(const boost::shared_ptr<ore::data::Portfolio>& portfolio,
                       boost::shared_ptr<ore::data::Market>& market, const string& marketConfiguration,
                       const boost::shared_ptr<ore::data::EngineData>& engineData,
                       boost::shared_ptr<ScenarioSimMarketParameters>& simMarketData,
                       const boost::shared_ptr<StressTestScenarioData>& stressData, const Conventions& conventions,
                       boost::shared_ptr<ScenarioFactory> scenarioFactory, bool continueOnError) {

    LOG("Build Simulation Market");
    boost::shared_ptr<ScenarioSimMarket> simMarket =
        boost::make_shared<ScenarioSimMarket>(market, simMarketData, conventions, marketConfiguration, continueOnError);

    LOG("Build Stress Scenario Generator");
    Date asof = market->asofDate();
    boost::shared_ptr<Scenario> baseScenario = simMarket->baseScenario();
    scenarioFactory = scenarioFactory ? scenarioFactory : boost::make_shared<CloneScenarioFactory>(baseScenario);
    boost::shared_ptr<StressScenarioGenerator> scenarioGenerator =
        boost::make_shared<StressScenarioGenerator>(stressData, baseScenario, simMarketData, scenarioFactory);
    simMarket->scenarioGenerator() = scenarioGenerator;

    LOG("Build Engine Factory");
    map<MarketContext, string> configurations;
    configurations[MarketContext::pricing] = marketConfiguration;
    boost::shared_ptr<EngineFactory> factory = boost::make_shared<EngineFactory>(engineData, simMarket, configurations);

    LOG("Reset and Build Portfolio");
    portfolio->reset();
    portfolio->build(factory);

    LOG("Build the cube object to store sensitivities");
    boost::shared_ptr<NPVCube> cube = boost::make_shared<DoublePrecisionInMemoryCube>(
        asof, portfolio->ids(), vector<Date>(1, asof), scenarioGenerator->samples());

    boost::shared_ptr<DateGrid> dg = boost::make_shared<DateGrid>(
        "1,0W"); // TODO - extend the DateGrid interface so that it can actually take a vector of dates as input
    vector<boost::shared_ptr<ValuationCalculator>> calculators;
    calculators.push_back(boost::make_shared<NPVCalculator>(simMarketData->baseCcy()));
    ValuationEngine engine(asof, dg, simMarket);
    LOG("Run Stress Scenarios");
    /*ostringstream o;
    o.str("");
    o << "Stress scenarios " << portfolio->size() << " x " << dg->size() << " x " << scenarioGenerator->samples() <<
    "... ";
    auto progressBar = boost::make_shared<SimpleProgressBar>(o.str());
    auto progressLog = boost::make_shared<ProgressLog>("Building scenarios...");
    engine.registerProgressIndicator(progressBar);
    engine.registerProgressIndicator(progressLog);*/
    engine.buildCube(portfolio, cube, calculators);

    /*****************
     * Collect results
     */
    baseNPV_.clear();
    shiftedNPV_.clear();
    delta_.clear();
    labels_.clear();
    for (Size i = 0; i < portfolio->size(); ++i) {
        Real npv0 = cube->getT0(i, 0);
        string id = portfolio->trades()[i]->id();
        trades_.insert(id);
        baseNPV_[id] = npv0;
        for (Size j = 0; j < scenarioGenerator->samples(); ++j) {
            string label = scenarioGenerator->scenarios()[j]->label();
            Real npv = cube->get(i, 0, j, 0);
            pair<string, string> p(id, label);
            shiftedNPV_[p] = npv;
            delta_[p] = npv - npv0;
            labels_.insert(label);
        }
    }
    LOG("Stress testing done");
}

void StressTest::writeReport(const boost::shared_ptr<ore::data::Report>& report, Real outputThreshold) {

    report->addColumn("TradeId", string());
    report->addColumn("ScenarioLabel", string());
    report->addColumn("Base NPV", double(), 2);
    report->addColumn("Scenario NPV", double(), 2);
    report->addColumn("Sensitivity", double(), 2);

    for (auto data : shiftedNPV_) {
        string id = data.first.first;
        string factor = data.first.second;
        Real npv = data.second;
        Real base = baseNPV_[id];
        Real sensi = npv - base;
        if (fabs(sensi) > outputThreshold) {
            report->next();
            report->add(id);
            report->add(factor);
            report->add(base);
            report->add(npv);
            report->add(sensi);
        }
    }

    report->end();
}
} // namespace analytics
} // namespace ore


#ifndef MESH_H
#define MESH_H

#include "array"
#include "vector"

#pragma clang diagnostic push
#pragma clang diagnostic ignored "-Wunused-variable"
#pragma clang diagnostic ignored "-Wunused-parameter"
#pragma clang diagnostic ignored "-Wmissing-braces"
#include "dccrg.hpp"
#include "dccrg_cartesian_geometry.hpp"
#pragma clang diagnostic pop

#include "cell.hpp"
#include "parameters.hpp"
#include "common.h"

#include <Eigen/Dense>

using namespace std;
using namespace dccrg;
using namespace Eigen;

typedef Array<double, 4, 1> vec4;

// class to handle neighborhood cells
class Neighborhood_Cube
{
public:

    std::array<vec4, 27> fields;

    vec4& operator [](const std::size_t i){return this->fields[i];}
    const vec4& operator [](const std::size_t i) const {return this->fields[i];}

    void init_currents()
    {
        for(int ijk=0; ijk<27; ijk++) {
            fields[ijk] = vec4::Zero();
        }
        return;
    };

    int index(int i, int j, int k) {
        return (i+1) + (j+1)*3 + (k+1)*3*3;
    };

    void add_J(int i, int j, int k, vec4 dJ)
    {
        int ijk = this->index(i,j,k);

        #ifdef DEBUG
        cout << "  addJ: " << i << j << k << " -> " << ijk
        << " A" << dJ << " B " << fields[ijk] << endl;
        #endif

        fields[ijk] += dJ;
    };

    vec4 J(int i, int j, int k) {
        return this->fields[index(i,j,k)];
    };

	void distribute(dccrg::Dccrg<Cell, dccrg::Cartesian_Geometry>& grid, uint64_t cell);
};

class Mesh
{
public:

    // MPI parameters
    int
        rank = 0;

    // Deposit currents into the mesh
	void sort_particles_into_cells(dccrg::Dccrg<Cell, dccrg::Cartesian_Geometry>& grid);

    // Deposit currents into the mesh
	void deposit_currents(dccrg::Dccrg<Cell, dccrg::Cartesian_Geometry>& grid);

    // Collect and compute staggered Yee currents
	void yee_currents(dccrg::Dccrg<Cell, dccrg::Cartesian_Geometry>& grid);

    /* Shift fields from staggered Yee to nodal lattice
       in practice we interpolate linearly between neighboring Yee values
       TODO FIXME
    */
	void nodal_fields(dccrg::Dccrg<Cell, dccrg::Cartesian_Geometry>& grid);

};

#endif


/********************************************************
  Stanford Driving Software
  Copyright (c) 2011 Stanford University
  All rights reserved.

  Redistribution and use in source and binary forms, with
  or without modification, are permitted provided that the
  following conditions are met:

* Redistributions of source code must retain the above
  copyright notice, this list of conditions and the
  following disclaimer.
* Redistributions in binary form must reproduce the above
  copyright notice, this list of conditions and the
  following disclaimer in the documentation and/or other
  materials provided with the distribution.
* The names of the contributors may not be used to endorse
  or promote products derived from this software
  without specific prior written permission.

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

#include <pluginlib/class_list_macros.h>
#include <Project3D.h>
#include <velodyne/Packet.h>
#include <velodyne/Projected.h>
#include <boost/thread.hpp>
#include <velodyne.h>
#include <global.h>
#include <pcl/point_types.h>
#include <pcl_ros/point_cloud.h>

using namespace std;

namespace velodyne
{

  PLUGINLIB_DECLARE_CLASS(velodyne, Project3D, velodyne::Project3D, nodelet::Nodelet);

  Project3D::Project3D()
  {

  }

  Project3D::~Project3D()
  {
  }

  void Project3D::onInit()
  {
    string cal_file, int_file;
    ros::NodeHandle& private_nh = getPrivateNodeHandle();
    private_nh.getParam("/driving/velodyne/cal_file", cal_file);
    private_nh.getParam("/driving/velodyne/int_file", int_file);
    readCalibration(cal_file, int_file);
    projected_pub_ = private_nh.advertise<velodyne::Projected>
        ("/driving/velodyne/projected", 4096);
    points_pub_ = private_nh.advertise<pcl::PointCloud<pcl::PointXYZI> >
        ("/driving/velodyne/rawpoints", 4096);
    packet_sub_ = private_nh.subscribe("/driving/velodyne/packet", 4096,
        &Project3D::projectPacket, this);
    NODELET_INFO("Ready to project points");
  }

  void Project3D::readCalibration(string cal_file, string int_file)
  {
    if(!config_.readCalibration(cal_file))
    {
      NODELET_ERROR_STREAM("Could not open calibration file " <<
          cal_file << string("."));
      throw VLRException("Could not open calibration file " +
          cal_file + string("."));
    }

    if(!config_.readIntensity(int_file))
    {
      NODELET_ERROR_STREAM("Could not open intensity file " <<
          int_file << string("."));
      throw VLRException("Could not open intensity file " +
          int_file + string("."));
    }
  }

  void Project3D::projectPacket(const velodyne::PacketPtr& packet)
  {
    double x, y, z;
    double distance, distance1, cosVertAngle, sinVertAngle, cosRotAngle, sinRotAngle, hOffsetCorr, vOffsetCorr;
    double xyDistance; //, distanceCorr, shortOffset, longOffset;

    velodyne::Projected projected;
    pcl::PointCloud<pcl::PointXYZI> cloud;
    for (int b = 0; b < velodyne::Packet::NUM_BLOCKS; b++)
    {
      for (int l = 0; l < velodyne::Block::NUM_BEAMS; l++)
      {
        // laser is global laser id, 0-64
        // l is block laser id, 0-32
        int laser = l + velodyne::Block::NUM_BEAMS * packet->block[b].block;
        if ((packet->block[b].laser[l].distance == 0 ||
              packet->block[b].laser[l].distance * VELODYNE_TICKS_TO_METER > 110 ||
              isnan(config_.sin_rot_angle[packet->block[b].encoder][laser])))
        {
          projected.block[b].point[l].x = 0;
          projected.block[b].point[l].y = 0;
          projected.block[b].point[l].z = 0;
          projected.block[b].laser[l].distance = 0;
        }
        else
        {
          distance1 = packet->block[b].laser[l].distance * VELODYNE_TICKS_TO_METER;
          distance = config_.range_offsetX[laser] * distance1 + config_.range_offset[laser];

          cosVertAngle = config_.cos_vert_angle[laser];
          sinVertAngle = config_.sin_vert_angle[laser];
          cosRotAngle = config_.cos_rot_angle[packet->block[b].encoder][laser];
          sinRotAngle = config_.sin_rot_angle[packet->block[b].encoder][laser];
          hOffsetCorr = config_.h_offset[laser];
          vOffsetCorr = config_.v_offset[laser];

          xyDistance = distance * cosVertAngle;

          x = xyDistance * cosRotAngle - hOffsetCorr * sinRotAngle;
          y = xyDistance * sinRotAngle + hOffsetCorr * cosRotAngle;
          z = (xyDistance / cosVertAngle) * sinVertAngle + vOffsetCorr;

          projected.block[b].point[l].x = x;
          projected.block[b].point[l].y = y;
          projected.block[b].point[l].z = z;
          projected.block[b].laser[l].distance = (uint16_t) (xyDistance * 100);
          projected.block[b].laser[l].intensity = config_.intensity_map[config_.inv_beam_order[laser]][packet->block[b].laser[l].intensity];
          pcl::PointXYZI point;
          point.x = x;
          point.y = y;
          point.z = z;
          point.intensity = projected.block[b].laser[l].intensity;
          cloud.push_back(point);
        }
      }
      projected.block[b].block = packet->block[b].block;
      projected.block[b].encoder = packet->block[b].encoder;
    }
    projected.spin_count = packet->spin_count;
    projected.header.stamp = packet->header.stamp;
    projected.header.frame_id = "Velodyne";
    cloud.header.frame_id = "Velodyne";
    cloud.header.stamp = ros::Time::now();
    points_pub_.publish(cloud);
    projected_pub_.publish(projected);
  }
} // namespace velodyne


//#include <ndt_registration/ndt_matcher_p2d.h>
//#include <ndt_registration/ndt_matcher_d2d_2d.h>
#include <ndt_registration/ndt_matcher_d2d.h>
#include <ndt_map/ndt_map.h>
#include <ndt_map/pointcloud_utils.h>

#include "pcl/point_cloud.h"
#include "pcl/io/pcd_io.h"
#include <cstdio>
#include <Eigen/Eigen>
#include <Eigen/Geometry>

#include <iostream>
#include <sstream>

using namespace std;

int
main (int argc, char** argv)
{
    if(argc!=9) {
	std::cout<<"usage "<<argv[0]<<" x y z r p t cloud1.wrl cloud2.wrl\n";
	return -1;
    }

    istringstream roll_c(argv[4]),pitch_c(argv[5]),yaw_c(argv[6]),xoffset_c(argv[1]),yoffset_c(argv[2]),zoffset_c(argv[3]);
    double roll,pitch,yaw,xoffset,yoffset,zoffset;
    roll_c >> roll;
    pitch_c >> pitch;
    yaw_c >> yaw;
    xoffset_c >> xoffset;
    yoffset_c >> yoffset;
    zoffset_c >> zoffset;

    printf("X %f Y %f Z %f Roll %f Pitch %f Yaw %f \n",xoffset,yoffset,zoffset,roll,pitch,yaw);
    pcl::PointCloud<pcl::PointXYZ> cloud, cloud_offset, cloud_trans;
    char fname[50];
    FILE *fout;
    double __res[] = {0.5, 1, 2, 4};
    std::vector<double> resolutions (__res, __res+sizeof(__res)/sizeof(double));

    struct timeval tv_start,tv_end,tv_reg_start,tv_reg_end;

    Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> Tout;
    Tout.setIdentity();
    //printf("Working");
    if(argc == 9)
    {

        gettimeofday(&tv_start,NULL);
        //we do a single scan to scan registration
        //TODO fix these to load pcd files
	if (pcl::io::loadPCDFile<pcl::PointXYZ> (argv[7], cloud) == -1) //* load the file
	{
	    std::cerr<<"Couldn't read file\n";
	    return (-1);
	}
	if (pcl::io::loadPCDFile<pcl::PointXYZ> (argv[8], cloud_offset) == -1) //* load the file
	{
	    std::cerr<<"Couldn't read file\n";
	    return (-1);
	}

        Tout =  Eigen::Translation<double,3>(xoffset,yoffset,zoffset)*
            Eigen::AngleAxis<double>(roll,Eigen::Vector3d::UnitX()) *
            Eigen::AngleAxis<double>(pitch,Eigen::Vector3d::UnitY()) *
            Eigen::AngleAxis<double>(yaw,Eigen::Vector3d::UnitZ()) ;

	//lslgeneric::NDTMatcherD2D_2D<pcl::PointXYZ,pcl::PointXYZ> matcherD2D(false, false, resolutions);
	perception_oru::NDTMatcherD2D matcherD2D(false, false, resolutions);
	cloud_trans = cloud_offset;
        bool ret = matcherD2D.match(cloud,cloud_offset,Tout,true);

	std::cout<<"Transform: \n"<<Tout.matrix()<<std::endl;

	perception_oru::transformPointCloudInPlace(Tout,cloud_trans);
	pcl::PointCloud<pcl::PointXYZRGB> cloud_comb;
	pcl::PointXYZRGB red(255,0,0);
	for(int i=0; i<cloud.points.size(); ++i ) {
	    red.x = cloud.points[i].x;
	    red.y = cloud.points[i].y;
	    red.z = cloud.points[i].z;
	    cloud_comb.points.push_back(red);
	}
	pcl::PointXYZRGB green(0,200,0);
	for(int i=0; i<cloud_offset.points.size(); ++i ) {
	    green.x = cloud_offset.points[i].x;
	    green.y = cloud_offset.points[i].y;
	    green.z = cloud_offset.points[i].z;
	    cloud_comb.points.push_back(green);
	}
	pcl::PointXYZRGB blue(10,20,200);
	for(int i=0; i<cloud_trans.points.size(); ++i ) {
	    blue.x = cloud_trans.points[i].x;
	    blue.y = cloud_trans.points[i].y;
	    blue.z = cloud_trans.points[i].z;
	    cloud_comb.points.push_back(blue);
	}
	cloud_comb.width=1;
	cloud_comb.height=cloud_comb.points.size();
	cloud_comb.is_dense = false;
	pcl::io::savePCDFileBinary ("test_pcd.pcd", cloud_comb);

    }
}


//
//  astar.hpp
//  indigo-bondorder
//
//  Created by Welsh, Ivan on 30/10/17.
//  Copyright © 2017 Allison Group. All rights reserved.
//

#ifndef ASTAR_HPP
#define ASTAR_HPP

#include <map>
#include <queue>
#include <deque>
#include <vector>

#include <boost/dynamic_bitset.hpp>

#include "electron_optimisation_algorithm.hpp"

namespace indigo_bondorder {
  namespace algorithm {

    typedef boost::dynamic_bitset<> VertMask;

    struct AStarQueueItem {
      ElnDist distribution;
      VertMask unchangeable, calculable, new_calculable;
      Score path_cost, heuristic_cost, parent_path_cost;
      uint16_t nbr_start_idx;
      uint8_t nbr_count;
    };

    typedef std::shared_ptr<AStarQueueItem> p_AStarQueueItem;

    class ItemCompare {
    public:
      bool operator()(const p_AStarQueueItem a, const p_AStarQueueItem b);
    };

    class PriorityQueue :
    public std::priority_queue<p_AStarQueueItem, std::vector<p_AStarQueueItem>,
    ItemCompare > {
    public:
      void reserve(size_t sz) { this->c.reserve(sz); }
      size_t max_size() const { return this->c.max_size(); }
      void clear() { this->c.clear(); }
    };

    class AStarOptimisation : public ElectronOptimisationAlgorithm {
    private:
      PriorityQueue queue_;

      std::vector<ElnVertex> pos2vert_;
      std::map<ElnVertex, size_t>  vert2pos_;

      std::vector<uint8_t> uniqueIDs_;
      std::vector<VertMask> requiredUnchangeables_;
      std::map<MolVertPair, ElnVertex> idsToVertex_;
      std::map<ElnVertex, ElnVertProp*> vertexProperties_;
      size_t vertMaskSize_;
      size_t maximumQueueSize_ = 0;
      size_t sizePerItem_;

    private:
      AStarOptimisation() = default;

    public:
      AStarOptimisation(ElectronOpt* parent);

    public:
      void Run() override;
      size_t GetMaximumQueueSize() { return maximumQueueSize_; }
      size_t GetSizePerQueueItem() { return sizePerItem_; }

    private:
      void PopulateUniqueIDs();
      void PopulateUnchangeables();
      void PopulateInitialDistribution(p_AStarQueueItem d);
      void PopulateNeighbourDistribution(p_AStarQueueItem parent, p_AStarQueueItem child);
      void Initalise();
      void DetermineCalculable(p_AStarQueueItem d);
      void CalculatePathEnergy(p_AStarQueueItem d);
      void CalculateHeuristicEnergy(p_AStarQueueItem d);
      void PromiscuousHeuristic(p_AStarQueueItem d);
      void AbstemiousHeuristic(p_AStarQueueItem d);
      void GenerateNeighbourDistributions(p_AStarQueueItem d, std::vector<ElnDist>* out_nbrs);

    };
  }
}

#endif /* ASTAR_HPP */


#include <stdlib.h>
#include <stdio.h>
#include <vector>
#include <string>
#include <sstream>
#include <iostream>
#include <fstream>
#include <iomanip>
#include <stdexcept>
#include <functional>
#include <random>
#include <Eigen/Geometry>
#include <visualization_msgs/Marker.h>
#include "arc_utilities/eigen_helpers.hpp"
#include "arc_utilities/eigen_helpers_conversions.hpp"
#include "arc_utilities/pretty_print.hpp"
#include "arc_utilities/voxel_grid.hpp"
#include "arc_utilities/simple_rrt_planner.hpp"
#include "uncertainty_planning_core/simple_pid_controller.hpp"
#include "uncertainty_planning_core/simple_uncertainty_models.hpp"
#include "uncertainty_planning_core/uncertainty_contact_planning.hpp"
#include "uncertainty_planning_core/simple_robot_models.hpp"
#include "uncertainty_planning_core/simple_samplers.hpp"
#include "uncertainty_planning_core/uncertainty_planning_core.hpp"
#include "fast_kinematic_simulator/fast_kinematic_simulator.hpp"
#include "fast_kinematic_simulator/simulator_environment_builder.hpp"
#include "uncertainty_planning_examples/config_common.hpp"

#ifndef UR5_LINKED_COMMON_CONFIG_HPP
#define UR5_LINKED_COMMON_CONFIG_HPP

#ifdef LIMIT_UR5_JOINT_LIMITS
    #define UR5_JOINT_LIMITS M_PI
#else
    #define UR5_JOINT_LIMITS (M_PI * 2.0)
#endif

#ifndef OVERRIDE_FIXED_RESOLUTION
    #define RESOLUTION 0.03125
#endif

namespace ur5_linked_common_config
{
    typedef simple_robot_models::SimpleJointModel SJM;
    typedef simple_robot_models::SimpleLinkedConfiguration SLC;

    inline uncertainty_planning_core::PLANNING_AND_EXECUTION_OPTIONS GetDefaultOptions()
    {
        uncertainty_planning_core::PLANNING_AND_EXECUTION_OPTIONS options;
        options.clustering_type = uncertainty_contact_planning::CONVEX_REGION_SIGNATURE;
        options.planner_time_limit = 120.0;
        options.goal_bias = 0.1;
        options.step_size = M_PI;
        options.step_duration = 10.0;
        options.goal_probability_threshold = 0.51;
        options.goal_distance_threshold = M_PI_4;
        options.connect_after_first_solution = 0.0;
        options.signature_matching_threshold = 0.99;
        options.distance_clustering_threshold = M_PI_2;
        options.feasibility_alpha = 0.75;
        options.variance_alpha = 0.75;
        options.edge_attempt_count = 50u;
        options.num_particles = 24u;
        options.use_contact = true;
        options.use_reverse = true;
        options.use_spur_actions = true;
        options.max_exec_actions = 1000u;
        options.max_policy_exec_time = 0.0;
        options.num_policy_simulations = 1u;
        options.num_policy_executions = 1u;
        options.policy_action_attempt_count = 100u;
        options.debug_level = 0;
        options.planner_log_file = "/tmp/ur5_planner_log.txt";
        options.policy_log_file = "/tmp/ur5_policy_log.txt";
        options.planned_policy_file = "/tmp/ur5_planned_policy.policy";
        options.executed_policy_file = "/dev/null";
        return options;
    }

    inline uncertainty_planning_core::PLANNING_AND_EXECUTION_OPTIONS GetOptions()
    {
        return uncertainty_planning_core::GetOptions(GetDefaultOptions());
    }

    inline config_common::TASK_CONFIG_PARAMS GetDefaultExtraOptions()
    {
        return config_common::TASK_CONFIG_PARAMS(0.03125, 25.0, 0.0, 0.0, "baxter_blocked_test_mod_env");
    }

    inline config_common::TASK_CONFIG_PARAMS GetExtraOptions()
    {
        return config_common::GetOptions(GetDefaultExtraOptions());
    }

    inline simple_robot_models::LINKED_ROBOT_CONFIG GetDefaultRobotConfig(const config_common::TASK_CONFIG_PARAMS& options)
    {
        const double env_resolution = options.environment_resolution;
        const double kp = 1.0;
        const double ki = 0.0;
        const double kd = 0.1;
        const double i_clamp = 0.0;
        const double velocity_limit = env_resolution * 2.0;
        const double max_sensor_noise = options.sensor_error;
        const double max_actuator_noise = options.actuator_error;
        const simple_robot_models::LINKED_ROBOT_CONFIG robot_config(kp, ki, kd, i_clamp, velocity_limit, max_sensor_noise, max_actuator_noise);
        return robot_config;
    }

    inline Eigen::Isometry3d GetBaseTransform()
    {
        const Eigen::Isometry3d base_transform = Eigen::Translation3d(0.0, 0.0, 0.0) * Eigen::Quaterniond(Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitZ()));
        return base_transform;
    }

    inline SLC MakeUR5ArmConfiguration(const std::vector<double>& joint_values)
    {
        assert(joint_values.size() == 6);
        SLC arm_configuration(6);
        const double shoulder_pan_joint = joint_values[0];
        arm_configuration[0] = SJM(std::pair<double, double>(-UR5_JOINT_LIMITS, UR5_JOINT_LIMITS), shoulder_pan_joint, SJM::REVOLUTE); // shoulder_pan_joint
        const double shoulder_lift_joint = joint_values[1];
        arm_configuration[1] = SJM(std::pair<double, double>(-UR5_JOINT_LIMITS, UR5_JOINT_LIMITS), shoulder_lift_joint, SJM::REVOLUTE); // shoulder_lift_joint
        const double elbow_joint = joint_values[2];
        arm_configuration[2] = SJM(std::pair<double, double>(-UR5_JOINT_LIMITS, UR5_JOINT_LIMITS), elbow_joint, SJM::REVOLUTE); // elbow_joint
        const double wrist_1_joint = joint_values[3];
        arm_configuration[3] = SJM(std::pair<double, double>(-UR5_JOINT_LIMITS, UR5_JOINT_LIMITS), wrist_1_joint, SJM::REVOLUTE); // wrist_1_joint
        const double wrist_2_joint = joint_values[4];
        arm_configuration[4] = SJM(std::pair<double, double>(-UR5_JOINT_LIMITS, UR5_JOINT_LIMITS), wrist_2_joint, SJM::REVOLUTE); // wrist_2_joint
        const double wrist_3_joint = joint_values[5];
        arm_configuration[5] = SJM(std::pair<double, double>(-UR5_JOINT_LIMITS, UR5_JOINT_LIMITS), wrist_3_joint, SJM::REVOLUTE); // wrist_3_joint
        return arm_configuration;
    }

    inline std::pair<SLC, SLC> GetStartAndGoal()
    {
        // Define the goals of the plan
        const SLC goal = MakeUR5ArmConfiguration(std::vector<double>{0.0, 0.0, 0.0, 0.0, 0.0, 0.0});
        const SLC start = MakeUR5ArmConfiguration(std::vector<double>{0.0, 0.0, 0.0, 0.0, 0.0, 0.0});
        return std::make_pair(start, goal);
    }

    inline SLC GetReferenceConfiguration()
    {
        const SLC reference_configuration = MakeUR5ArmConfiguration(std::vector<double>{0.0, 0.0, 0.0, 0.0, 0.0, 0.0});
        return reference_configuration;
    }

    inline std::vector<double> GetJointUncertaintyParams(const config_common::TASK_CONFIG_PARAMS& options)
    {
        std::vector<double> uncertainty_params(6, options.actuator_error);
        return uncertainty_params;
    }

    inline std::vector<double> GetJointDistanceWeights()
    {
        const std::vector<double> max_velocities = {1.0, 1.0, 1.0, 1.0, 1.0, 1.0};
        std::vector<double> distance_weights(max_velocities.size(), 0.0);
        for (size_t idx = 0; idx < max_velocities.size(); idx++)
        {
            distance_weights[idx] = 1.0 / max_velocities[idx];
        }
        return distance_weights;
    }

    inline void MakeCylinderPoints(const double radius, const double length, const double resolution, const Eigen::Vector3d& axis, const std::shared_ptr<EigenHelpers::VectorVector4d>& points)
    {
        UNUSED(radius);
        UNUSED(length);
        UNUSED(axis);
        points->push_back(Eigen::Vector4d(0.0, 0.0, 0.0, 1.0));
        points->push_back(Eigen::Vector4d(resolution, 0.0, 0.0, 1.0));
        points->push_back(Eigen::Vector4d(0.0, resolution, 0.0, 1.0));
        points->push_back(Eigen::Vector4d(0.0, resolution * 2.0, 0.0, 1.0));
        points->push_back(Eigen::Vector4d(0.0, 0.0, resolution, 1.0));
        points->push_back(Eigen::Vector4d(0.0, 0.0, resolution * 2.0, 1.0));
        points->push_back(Eigen::Vector4d(0.0, 0.0, resolution * 3.0, 1.0));
    }

    inline std::shared_ptr<EigenHelpers::VectorVector4d> GetLinkPoints(const std::string& link_name)
    {
        if (link_name == "base_link")
        {
            std::shared_ptr<EigenHelpers::VectorVector4d> link_points(new EigenHelpers::VectorVector4d());
            MakeCylinderPoints(0.06, 0.05, RESOLUTION, Eigen::Vector3d::UnitZ(), link_points);
            return link_points;
        }
        else if (link_name == "shoulder_link")
        {
            std::shared_ptr<EigenHelpers::VectorVector4d> link_points(new EigenHelpers::VectorVector4d());
            MakeCylinderPoints(0.06, 0.05, RESOLUTION, Eigen::Vector3d::UnitZ(), link_points);
            return link_points;
        }
        else if (link_name == "upper_arm_link")
        {
            std::shared_ptr<EigenHelpers::VectorVector4d> link_points(new EigenHelpers::VectorVector4d());
            MakeCylinderPoints(0.06, 0.05, RESOLUTION, Eigen::Vector3d::UnitZ(), link_points);
            return link_points;
        }
        else if (link_name == "forearm_link")
        {
            std::shared_ptr<EigenHelpers::VectorVector4d> link_points(new EigenHelpers::VectorVector4d());
            MakeCylinderPoints(0.06, 0.05, RESOLUTION, Eigen::Vector3d::UnitZ(), link_points);
            return link_points;
        }
        else if (link_name == "wrist_1_link")
        {
            std::shared_ptr<EigenHelpers::VectorVector4d> link_points(new EigenHelpers::VectorVector4d());
            MakeCylinderPoints(0.6, 0.12, RESOLUTION, Eigen::Vector3d::UnitY(), link_points);
            return link_points;
        }
        else if (link_name == "wrist_2_link")
        {
            std::shared_ptr<EigenHelpers::VectorVector4d> link_points(new EigenHelpers::VectorVector4d());
            MakeCylinderPoints(0.6, 0.12, RESOLUTION, Eigen::Vector3d::UnitZ(), link_points);
            return link_points;
        }
        else if (link_name == "wrist_3_link")
        {
            std::shared_ptr<EigenHelpers::VectorVector4d> link_points(new EigenHelpers::VectorVector4d());
            MakeCylinderPoints(0.05, 0.0823, RESOLUTION, Eigen::Vector3d::UnitY(), link_points);
            return link_points;
        }
        else if (link_name == "ee_link")
        {
            std::shared_ptr<EigenHelpers::VectorVector4d> link_points(new EigenHelpers::VectorVector4d());
            MakeCylinderPoints(RESOLUTION * 0.5, RESOLUTION * 0.5, RESOLUTION, Eigen::Vector3d::UnitX(), link_points);
            return link_points;
        }
        else
        {
            throw std::invalid_argument("Invalid link name");
        }
    }

    typedef uncertainty_planning_core::LinkedActuatorModel UR5JointActuatorModel;

    inline simple_robot_models::SimpleLinkedRobot<UR5JointActuatorModel> GetRobot(const Eigen::Isometry3d& base_transform, const simple_robot_models::LINKED_ROBOT_CONFIG& joint_config, const std::vector<double>& joint_uncertainty_params, const std::vector<double>& joint_distance_weights)
    {
        const double shoulder_pan_joint_noise = joint_uncertainty_params[0];
        const double shoulder_lift_joint_noise = joint_uncertainty_params[1];
        const double elbow_joint_noise = joint_uncertainty_params[2];
        const double wrist_1_joint_noise = joint_uncertainty_params[3];
        const double wrist_2_joint_noise = joint_uncertainty_params[4];
        const double wrist_3_joint_noise = joint_uncertainty_params[5];
        // Make the robot model
        simple_robot_models::RobotLink base_link(GetLinkPoints("base_link"), "base_link");
        simple_robot_models::RobotLink shoulder_link(GetLinkPoints("shoulder_link"), "shoulder_link");
        simple_robot_models::RobotLink upper_arm_link(GetLinkPoints("upper_arm_link"), "upper_arm_link");
        simple_robot_models::RobotLink forearm_link(GetLinkPoints("forearm_link"), "forearm_link");
        simple_robot_models::RobotLink wrist_1_link(GetLinkPoints("wrist_1_link"), "wrist_1_link");
        simple_robot_models::RobotLink wrist_2_link(GetLinkPoints("wrist_2_link"), "wrist_2_link");
        simple_robot_models::RobotLink wrist_3_link(GetLinkPoints("wrist_3_link"), "wrist_3_link");
        simple_robot_models::RobotLink ee_link(GetLinkPoints("ee_link"), "ee_link");
        // Collect the links
        const std::vector<simple_robot_models::RobotLink> links = {base_link, shoulder_link, upper_arm_link, forearm_link, wrist_1_link, wrist_2_link, wrist_3_link, ee_link};
        // Set allowed self-collisions (i.e. collisions that actually can't happen, so if they occur, they are numerical issues)
        const std::vector<std::pair<size_t, size_t>> allowed_self_collisions = {std::pair<size_t, size_t>(0, 1), std::pair<size_t, size_t>(1, 2), std::pair<size_t, size_t>(2, 3), std::pair<size_t, size_t>(3, 4), std::pair<size_t, size_t>(4, 5), std::pair<size_t, size_t>(5, 6), std::pair<size_t, size_t>(6, 7)};
        // Make the reference configuration
        const SLC reference_configuration = GetReferenceConfiguration();
        // Make the joints
        // Shoulder pan
        simple_robot_models::RobotJoint<UR5JointActuatorModel> shoulder_pan_joint;
        shoulder_pan_joint.name = "shoulder_pan_joint";
        shoulder_pan_joint.parent_link_index = 0;
        shoulder_pan_joint.child_link_index = 1;
        shoulder_pan_joint.joint_axis = Eigen::Vector3d::UnitZ();
        shoulder_pan_joint.joint_transform = Eigen::Translation3d(0.0, 0.0, 0.089159) * EigenHelpers::QuaternionFromUrdfRPY(0.0, 0.0, 0.0);
        shoulder_pan_joint.joint_model = reference_configuration[0];
        simple_robot_models::LINKED_ROBOT_CONFIG shoulder_pan_joint_config = joint_config;
        shoulder_pan_joint_config.velocity_limit = 3.15;
        const UR5JointActuatorModel shoulder_pan_joint_model(std::abs(shoulder_pan_joint_noise), shoulder_pan_joint_config.velocity_limit);
        shoulder_pan_joint.joint_controller = simple_robot_models::JointControllerGroup<UR5JointActuatorModel>(shoulder_pan_joint_config, shoulder_pan_joint_model);
        // Shoulder lift
        simple_robot_models::RobotJoint<UR5JointActuatorModel> shoulder_lift_joint;
        shoulder_lift_joint.name = "shoulder_lift_joint";
        shoulder_lift_joint.parent_link_index = 1;
        shoulder_lift_joint.child_link_index = 2;
        shoulder_lift_joint.joint_axis = Eigen::Vector3d::UnitY();
        shoulder_lift_joint.joint_transform = Eigen::Translation3d(0.0, 0.13585, 0.0) * EigenHelpers::QuaternionFromUrdfRPY(0.0, M_PI_2, 0.0);
        shoulder_lift_joint.joint_model = reference_configuration[1];
        simple_robot_models::LINKED_ROBOT_CONFIG shoulder_lift_joint_config = joint_config;
        shoulder_lift_joint_config.velocity_limit = 3.15;
        const UR5JointActuatorModel shoulder_lift_joint_model(std::abs(shoulder_lift_joint_noise), shoulder_lift_joint_config.velocity_limit);
        shoulder_lift_joint.joint_controller = simple_robot_models::JointControllerGroup<UR5JointActuatorModel>(shoulder_lift_joint_config, shoulder_lift_joint_model);
        // Elbow
        simple_robot_models::RobotJoint<UR5JointActuatorModel> elbow_joint;
        elbow_joint.name = "elbow_joint";
        elbow_joint.parent_link_index = 2;
        elbow_joint.child_link_index = 3;
        elbow_joint.joint_axis = Eigen::Vector3d::UnitY();
        elbow_joint.joint_transform = Eigen::Translation3d(0.0, -0.1197, 0.42500) * EigenHelpers::QuaternionFromUrdfRPY(0.0, 0.0, 0.0);
        elbow_joint.joint_model = reference_configuration[2];
        simple_robot_models::LINKED_ROBOT_CONFIG elbow_joint_config = joint_config;
        elbow_joint_config.velocity_limit = 3.15;
        const UR5JointActuatorModel elbow_joint_model(std::abs(elbow_joint_noise), elbow_joint_config.velocity_limit);
        elbow_joint.joint_controller = simple_robot_models::JointControllerGroup<UR5JointActuatorModel>(elbow_joint_config, elbow_joint_model);
        // Wrist 1
        simple_robot_models::RobotJoint<UR5JointActuatorModel> wrist_1_joint;
        wrist_1_joint.name = "wrist_1_joint";
        wrist_1_joint.parent_link_index = 3;
        wrist_1_joint.child_link_index = 4;
        wrist_1_joint.joint_axis = Eigen::Vector3d::UnitY();
        wrist_1_joint.joint_transform = Eigen::Translation3d(0.0, 0.0, 0.39225) * EigenHelpers::QuaternionFromUrdfRPY(0.0, M_PI_2, 0.0);
        wrist_1_joint.joint_model = reference_configuration[3];
        simple_robot_models::LINKED_ROBOT_CONFIG wrist_1_joint_config = joint_config;
        wrist_1_joint_config.velocity_limit = 3.2;
        const UR5JointActuatorModel wrist_1_joint_model(std::abs(wrist_1_joint_noise), wrist_1_joint_config.velocity_limit);
        wrist_1_joint.joint_controller = simple_robot_models::JointControllerGroup<UR5JointActuatorModel>(wrist_1_joint_config, wrist_1_joint_model);
        // Wrist 2
        simple_robot_models::RobotJoint<UR5JointActuatorModel> wrist_2_joint;
        wrist_2_joint.name = "wrist_2_joint";
        wrist_2_joint.parent_link_index = 4;
        wrist_2_joint.child_link_index = 5;
        wrist_2_joint.joint_axis = Eigen::Vector3d::UnitZ();
        wrist_2_joint.joint_transform = Eigen::Translation3d(0.0, 0.093, 0.0) * EigenHelpers::QuaternionFromUrdfRPY(0.0, 0.0, 0.0);
        wrist_2_joint.joint_model = reference_configuration[4];
        simple_robot_models::LINKED_ROBOT_CONFIG wrist_2_joint_config = joint_config;
        wrist_2_joint_config.velocity_limit = 3.2;
        const UR5JointActuatorModel wrist_2_joint_model(std::abs(wrist_2_joint_noise), wrist_2_joint_config.velocity_limit);
        wrist_2_joint.joint_controller = simple_robot_models::JointControllerGroup<UR5JointActuatorModel>(wrist_2_joint_config, wrist_2_joint_model);
        // Wrist 3
        simple_robot_models::RobotJoint<UR5JointActuatorModel> wrist_3_joint;
        wrist_3_joint.name = "wrist_3_joint";
        wrist_3_joint.parent_link_index = 5;
        wrist_3_joint.child_link_index = 6;
        wrist_3_joint.joint_axis = Eigen::Vector3d::UnitY();
        wrist_3_joint.joint_transform = Eigen::Translation3d(0.0, 0.0, 0.09465) * EigenHelpers::QuaternionFromUrdfRPY(0.0, 0.0, 0.0);
        wrist_3_joint.joint_model = reference_configuration[5];
        simple_robot_models::LINKED_ROBOT_CONFIG wrist_3_joint_config = joint_config;
        wrist_3_joint_config.velocity_limit = 3.2;
        const UR5JointActuatorModel wrist_3_joint_model(std::abs(wrist_3_joint_noise), wrist_3_joint_config.velocity_limit);
        wrist_3_joint.joint_controller = simple_robot_models::JointControllerGroup<UR5JointActuatorModel>(wrist_3_joint_config, wrist_3_joint_model);
        // Fixed wrist->EE joint
        simple_robot_models::RobotJoint<UR5JointActuatorModel> ee_fixed_joint;
        ee_fixed_joint.name = "ee_fixed_joint";
        ee_fixed_joint.parent_link_index = 6;
        ee_fixed_joint.child_link_index = 7;
        ee_fixed_joint.joint_axis = Eigen::Vector3d::UnitZ();
        ee_fixed_joint.joint_transform = Eigen::Translation3d(0.0, 0.0823, 0.0) * EigenHelpers::QuaternionFromUrdfRPY(0.0, 0.0, M_PI_2);
        ee_fixed_joint.joint_model = SJM(std::make_pair(0.0, 0.0), 0.0, SJM::FIXED);
        // We don't need an uncertainty model for a fixed joint
        ee_fixed_joint.joint_controller = simple_robot_models::JointControllerGroup<UR5JointActuatorModel>(joint_config);
        // Collect the joints
        const std::vector<simple_robot_models::RobotJoint<UR5JointActuatorModel>> joints = {shoulder_pan_joint, shoulder_lift_joint, elbow_joint, wrist_1_joint, wrist_2_joint, wrist_3_joint, ee_fixed_joint};
        const simple_robot_models::SimpleLinkedRobot<UR5JointActuatorModel> robot(base_transform, links, joints, allowed_self_collisions, reference_configuration, joint_distance_weights);
        return robot;
    }

    inline uncertainty_planning_core::LinkedSamplerPtr GetSampler()
    {
        // Make the sampler
        const SLC reference_configuration = GetReferenceConfiguration();
        return uncertainty_planning_core::LinkedSamplerPtr(new simple_samplers::SimpleLinkedBaseSampler<uncertainty_planning_core::PRNG>(reference_configuration));
    }

    inline uncertainty_planning_core::LinkedSimulatorPtr GetSimulator(const config_common::TASK_CONFIG_PARAMS& options, const int32_t debug_level)
    {
        const simulator_environment_builder::EnvironmentComponents environment_components = simulator_environment_builder::BuildCompleteEnvironment(options.environment_id, options.environment_resolution);
        const fast_kinematic_simulator::SolverParameters solver_params = fast_kinematic_simulator::GetDefaultSolverParameters();
        return fast_kinematic_simulator::MakeLinkedSimulator(environment_components.GetEnvironment(), environment_components.GetEnvironmentSDF(), environment_components.GetSurfaceNormalsGrid(), solver_params, options.simulation_controller_frequency, debug_level);
    }
}

#endif // UR5_LINKED_COMMON_CONFIG_HPP


/********************************************************
  Stanford Driving Software
  Copyright (c) 2011 Stanford University
  All rights reserved.

  Redistribution and use in source and binary forms, with
  or without modification, are permitted provided that the
  following conditions are met:

* Redistributions of source code must retain the above
  copyright notice, this list of conditions and the
  following disclaimer.
* Redistributions in binary form must reproduce the above
  copyright notice, this list of conditions and the
  following disclaimer in the documentation and/or other
  materials provided with the distribution.
* The names of the contributors may not be used to endorse
  or promote products derived from this software
  without specific prior written permission.

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

#include <track_manager.h>
#include <boost/filesystem.hpp>
#include "multibooster_support2.h"

using namespace std;
using boost::shared_ptr;
using namespace track_manager;
using namespace pipeline;
using namespace Eigen;

// Object* buildObject(const vector< shared_ptr<DescriptorNode> >& dns, int label) {
//   Object* obj = new Object;
//   obj->label_ = label;
//   obj->descriptors_ = vector<descriptor>(dns.size());

//   for(size_t i = 0; i < dns.size(); ++i) {
//     descriptor& desc = obj->descriptors_[i];
//     if(dns[i]->getDescriptor()) {
//       desc.vector = new VectorXf();
//       *desc.vector = *dns[i]->getDescriptor(); //TODO: Make multibooster take a shared_ptr.
//       desc.length_squared = desc.vector->dot(*desc.vector);
//     }
//     else {
//       desc.vector = NULL;
//       desc.length_squared = -1;
//     }
//   }

//   return obj;
//}

// Object* getDescriptorsForCloud2(DescriptorPipeline& dp, const sensor_msgs::PointCloud& roscloud, bool debug = false, bool timing = false) {
//   shared_ptr<MatrixXf> cloud((MatrixXf*)NULL);
//   shared_ptr<VectorXf> intensity((VectorXf*)NULL);
//   rosToEigen(roscloud, &cloud, &intensity);
//   dp.setInput(cloud, intensity);

//   timeval start, end;
//   gettimeofday(&start, NULL);

//   dp.compute();
//   vector< shared_ptr<DescriptorNode> > dns = dp.getOutputDescriptorNodes(0);

//   Object* obj = new Object;
//   obj->descriptors_ = vector<descriptor>(dns.size());
//   for(size_t i = 0; i < dns.size(); ++i) {
//     descriptor& desc = obj->descriptors_[i];
//     if(dns[i]->getDescriptor()) {
//       desc.vector = new VectorXf();
//       *desc.vector = *dns[i]->getDescriptor(); //TODO: Make multibooster take a shared_ptr.
//       desc.length_squared = desc.vector->dot(*desc.vector);
//     }
//     else {
//       desc.vector = NULL;
//       desc.length_squared = -1;
//     }
//   }
//   dp.flush();

//   gettimeofday(&end, NULL);
//   if(timing)
//     cout << (end.tv_sec - start.tv_sec) * 1000. + (end.tv_usec - start.tv_usec) / 1000. << " ms to compute descriptors and copy into object." << endl;
//   return obj;
// }

void accumulateStats(MultiBooster* mb, const TrackManager& tm, PerfStats* stats, vector< shared_ptr<Track> >* misclassified) {
  size_t max_num_clouds = 0;
  for(size_t i = 0; i < tm.tracks_.size(); ++i) {
    if(tm.tracks_[i]->clouds_.size() > max_num_clouds)
      max_num_clouds = tm.tracks_[i]->clouds_.size();
  }
  cout << "Building pipeline..."; cout.flush();
  int num_threads = 1;
  ClassifierPipeline cp(mb, num_threads);
  cout << " done." << endl;

  timeval start, end;
  gettimeofday(&start, NULL);
  int num_clouds = 0;
  for(size_t i = 0; i < tm.tracks_.size(); ++i) {
    if(i % 100 == 0) {
      cout << "."; cout.flush();
    }
    //cout << "Working on track " << i << " / " << tm.tracks_.size() << endl;
    Track& tr = *tm.tracks_[i];

    int label;
    if(tr.label_.compare("unlabeled") == 0)
      continue;
    if(tr.label_.compare("background") == 0)
      label = -1;
    else
      label = mb->class_map_.toId(tr.label_);

    vector< shared_ptr<MatrixXf> > clouds(tr.clouds_.size());
    vector< shared_ptr<VectorXf> > intensities(tr.clouds_.size());
    for(size_t j = 0; j < tr.clouds_.size(); ++j) {
      ++num_clouds;
      shared_ptr<MatrixXf> cloud((MatrixXf*)NULL);
      shared_ptr<VectorXf> intensity((VectorXf*)NULL);
      rosToEigen(*tr.clouds_[j], &cloud, &intensity);
      clouds[j] = cloud;
      intensities[j] = intensity;
    }

    timeval start2, end2;
    gettimeofday(&start2, NULL);
    vector<VectorXf> responses = cp.classify(clouds, intensities);
    gettimeofday(&end2, NULL);
    cout << ((end2.tv_sec - start2.tv_sec) * 1000. + (end2.tv_usec - start2.tv_usec) / 1000.) / (float)clouds.size() << " ms per object." << endl;

    // -- Get the prediction and increment statistics.
    VectorXf track_prediction = mb->prior_;
    assert(responses.size() == tr.clouds_.size());
    for(size_t j = 0; j < responses.size(); ++j) {
      track_prediction += 2*responses[j] - mb->prior_;
    }
    cout << track_prediction.transpose() << endl;
    stats->incrementStats(label, track_prediction);

    // -- If we were wrong, set the track aside to be saved.
    int prediction = 0;
    float val = track_prediction.maxCoeff(&prediction);
    if(val < 0)
      prediction = -1;
    if(label != prediction)
      misclassified->push_back(tm.tracks_[i]);
  }
  gettimeofday(&end, NULL);
  cout << "Descriptor computation and classification of " << num_clouds << " clouds took "
       << end.tv_sec - start.tv_sec << " seconds, mean time per cloud is " << (double)(end.tv_sec - start.tv_sec) / (double)num_clouds << endl;
}

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

  if(argc < 3) {
    cout << "Usage: " << argv[0] << " CLASSIFIER TRACK_MANAGER [TRACK_MANAGER ...]" << endl;
    return 1;
  }

  cout << "Loading multibooster " << argv[1] << endl;
  MultiBooster* mb = new MultiBooster(argv[1]);
  if(getenv("WC_LIMIT"))
    mb->wc_limiter_ = atoi(getenv("WC_LIMIT"));

  mb->applyNewMappings(mb->class_map_, getDescriptorNames());
  cout << mb->status(false) << endl;

  PerfStats stats(mb->class_map_);
  for(int i = 2; i < argc; ++i) {
    cout << "Loading track manager " << argv[i] << endl;
    cerr << "Loading track manager " << argv[i] << endl;
    TrackManager tm(argv[i]);
    cout << "Loaded " << tm.tracks_.size() << " tracks." << endl;
    if(tm.tracks_.size() == 0) {
      cerr << "0 tracks.  Skipping." << endl;
      continue;
    }
    vector< shared_ptr<Track> > misclassified;
    accumulateStats(mb, tm, &stats, &misclassified);

    boost::filesystem::path path(argv[i]);
    string stem = path.filename().substr(0, path.filename().size() - 3);
    string savename = "misclassified/" + stem + ".tm";
    cout << misclassified.size() << " misclassified tracks.  Saving to " << savename << endl;
    TrackManager mc(misclassified);
    int foo = system("mkdir -p misclassified");     --foo; // This is to avoid a warning.
    mc.save(savename);
  }

  cout << stats.statString() << endl;
  delete mb;
  return 0;
}


// Copyright 2021 Tier IV, Inc.
//
// 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 SCENE_MODULE__OCCLUSION_SPOT__GRID_UTILS_HPP_
#define SCENE_MODULE__OCCLUSION_SPOT__GRID_UTILS_HPP_

#include <grid_map_core/GridMap.hpp>
#include <grid_map_core/iterators/LineIterator.hpp>
#include <grid_map_core/iterators/PolygonIterator.hpp>
#include <opencv2/opencv.hpp>
#include <scene_module/occlusion_spot/geometry.hpp>

#include <nav_msgs/msg/occupancy_grid.hpp>

#include <boost/geometry.hpp>

#include <lanelet2_core/geometry/Lanelet.h>
#include <lanelet2_core/primitives/Lanelet.h>

#include <vector>

namespace behavior_velocity_planner
{
namespace grid_utils
{
namespace occlusion_cost_value
{
static constexpr int NO_INFORMATION = -1;
static constexpr int FREE_SPACE = 0;
static constexpr int UNKNOWN = 50;
static constexpr int OCCUPIED = 100;
}  // namespace occlusion_cost_value

struct GridParam
{
  int free_space_max;  // maximum value of a freespace cell in the occupancy grid
  int occupied_min;    // minimum value of an occupied cell in the occupancy grid
};
struct OcclusionSpotSquare
{
  grid_map::Index index;        // index of the anchor
  grid_map::Position position;  // position of the anchor
  int side_size;                // number of cells for each side of the square
};
// @brief structure representing a OcclusionSpot on the OccupancyGrid
struct OcclusionSpot
{
  double distance_along_lanelet;
  lanelet::ConstLanelet lanelet;
  lanelet::BasicPoint2d position;
};
//!< @brief Return true
// if the given cell is a occlusion_spot square of size min_size*min_size in the given grid
bool isOcclusionSpotSquare(
  OcclusionSpotSquare & occlusion_spot, const grid_map::Matrix & grid_data,
  const grid_map::Index & cell, const int side_size, const grid_map::Size & grid_size);
//!< @brief Find all occlusion spots inside the given lanelet
void findOcclusionSpots(
  std::vector<grid_map::Position> & occlusion_spot_positions, const grid_map::GridMap & grid,
  const lanelet::BasicPolygon2d & polygon, const double min_size);
//!< @brief Return true if the path between the two given points is free of occupied cells
bool isCollisionFree(
  const grid_map::GridMap & grid, const grid_map::Position & p1, const grid_map::Position & p2);
//!< @brief get the corner positions of the square described by the given anchor
void getCornerPositions(
  std::vector<grid_map::Position> & corner_positions, const grid_map::GridMap & grid,
  const OcclusionSpotSquare & occlusion_spot_square);
void imageToOccupancyGrid(const cv::Mat & cv_image, nav_msgs::msg::OccupancyGrid * occupancy_grid);
void toQuantizedImage(
  const nav_msgs::msg::OccupancyGrid & occupancy_grid, cv::Mat * cv_image, const GridParam & param);
void denoiseOccupancyGridCV(
  nav_msgs::msg::OccupancyGrid & occupancy_grid, grid_map::GridMap & grid_map,
  const GridParam & param);
}  // namespace grid_utils
}  // namespace behavior_velocity_planner

#endif  // SCENE_MODULE__OCCLUSION_SPOT__GRID_UTILS_HPP_


#include <exe/inputParser.h>

#include <base/cgal_typedefs.h>
#include <IO/fileIO.h>
#include <IO/ttIO.h>
#include <IO/ethIO.h>
#ifdef COLMAP
#include <IO/colmapIO.h>
#endif
#include <util/helper.h>
#include <util/geometricOperations.h>

#include <processing/meshProcessing.h>
#include <processing/edgeManifoldness.h>
#include <processing/graphCut.h>

#include <processing/pointSetProcessing.h>
#include <processing/normalAndSensorProcessing.h>
#include <processing/evaluation.h>
#include <processing/rayTracingFacet.h>

#include <learning/learning.h>
#include <learning/learningMath.h>
#include <learning/learningRayTracing.h>
#include <learning/learningRayTracingGroundTruth.h>
#include <learning/learningIO_bin.h>

#include <CGAL/optimal_bounding_box.h>
#include <boost/filesystem.hpp>
using namespace boost::filesystem;

/////////////////////////////////////////////////////////////////////
///////////////////////// CONTROL FUNCTIONS /////////////////////////
/////////////////////////////////////////////////////////////////////
int extractFeatures(dirHolder& dir, dataHolder& data, runningOptions& options, exportOptions& exportO){

    ///////////////////////////////
    ///////// FILE NAMING /////////
    ///////////////////////////////
    options.scoring="_"+options.scoring;
    if(dir.read_file.empty())
        dir.read_file = dir.write_file;
    if(dir.write_file.empty())
        dir.write_file = dir.read_file;

    ///////////////////////////////
    ///////// IMPORT SCAN /////////
    ///////////////////////////////
    // sampling input
    if(options.data_source == "ply"){
        importPLYPoints(dir, data);
    }
    else if(options.data_source == "npz"){
        importNPZ(dir, data);
    }
//    #ifdef COLMAP
//    else if(options.data_source == "colmap"){
//        readColmapFiles(dir, data);
//    }
//    #endif
    #ifdef OpenMVS
    else if(options.data_source == "omvs"){
        if(importOMVSScene(dir, data))
            return 1;
    }
    #endif
    else{
        cout << "ERROR: not a valid reconstruction input" << endl;
        return 1;
    }

    // ground truth
    if(!dir.gt_poly_file.empty()){
        if(dir.gt_poly_file.substr(dir.gt_poly_file.length() - 3 ) == "off"){
            if(importOFFMesh(dir.path+dir.gt_poly_file, data.gt_poly))
                return 1;
//            CGAL::Polygon_mesh_processing::keep_largest_connected_components(data.gt_poly, 1);
        }
        #ifdef RECONBENCH
        else if(dir.gt_poly_file.substr(dir.gt_poly_file.length() - 3 ) == "mpu"){
            if(importImplicit(dir, data))
                return 1;
        }
        #endif
        else{
            cerr << "\nnot a valid ending for a ground truth file. choose either .mpu or .off" << endl;
            return 1;
        }
        assert(data.gt_poly.size_of_vertices() > 0);
        options.ground_truth = 1;
    }

    ///////////////////////////////////
    ///////// PREPROCESS DATA /////////
    ///////////////////////////////////
    if(!dir.transformation_file.empty()){
        // import translation matrix
//        if(importTransformationMatrix(dir,data))
//            return 1;
        if(applyTransformationMatrix(data))
            return 1;
    }
    // calc gt obb
    if(!options.gt_isclosed){
        cout << "\nMake bounding box of open GT for cropping input..." << endl;
        assert(options.ground_truth);
        // get centroid of ground truth
        getOrientedBoundingBox(data.gt_poly,data.bb_array,data.bb_surface_mesh);
        data.gt_centroid = getBoundingBoxCentroid(data.bb_array,1000.0);
        cout << "\t-ground truth centroid for ray tracing: " << data.gt_centroid << "-1000" << endl;
        clipWithBoundingBox(data.points,data.infos,data.bb_surface_mesh);

    }

    if(options.scale > 0.0)
        standardizePointSet(dir, data, options.scale);

//    if(options.scoring == "_rt")
//        orderSensors(data);
//    else
//        cout << "\nSensors not ordered for ray-tracing" << endl;

    if(exportO.cameras){
        dir.suffix = "_cameras";
        exportCameraCenter(dir, data);
    }
    // export the scanned points
    if(exportO.toply){
        exportO.color = data.has_color;
        exportO.normals = data.has_normal;
        exportPLY(dir, data.points, data.infos, exportO);
    }

    if(exportO.tonpz){
        toXTensor(data);
        exportNPZ(dir,data);
    }

    //////////////////////////////////////
    /////// DELAUNAY TRIANGULATION ///////
    //////////////////////////////////////
    if(options.Dt_epsilon > 0.0)
        makeAdaptiveDelaunayWithInfo(data, options.Dt_epsilon);
    else
        makeDelaunayWithInfo(data);

//    cout << "Mean edge length after scaling: " << calcMeanEdgeLength(data) << endl;

    ///////////////////////////////////
    /////// TETRAHEDRON SCORING ///////
    ///////////////////////////////////
    // make an index, necessary for graphExport e.g.
    options.make_global_cell_idx=1;
    indexDelaunay(data, options);

    if(!options.ground_truth){
        // this constructs the features which are exported to the cell ray graph,
        // important to note the difference between score, and features, where score is not used in any way in the learning.
        if(options.scale == 0.0)
            cout << "\nConsider turning on scaling with --sn, if your learning data is not yet scaled to a unit cube!" << endl;
        assert(data.has_sensor);
        learning::rayTracing(data.Dt, options);
        // this is only for making a reconstruction of a lrt, but has no influence on the features or the learning
//        learning::aggregateScoreAndLabel(data.Dt);
    }
    else{
        assert(data.has_sensor);
        learning::rayTracing(data.Dt, options);
        if(dir.gt_poly_file.substr(dir.gt_poly_file.length() - 3 ) == "off"){
            if(options.gt_isclosed){
                if(labelObjectWithClosedGroundTruth(dir,data, options.number_of_points_per_cell))
                    return 1;
            }
            else{
                if(labelObjectWithOpenGroundTruth(dir,data, options.number_of_points_per_cell))
                    return 1;
            }
        }
        #ifdef RECONBENCH
        if(dir.gt_poly_file.substr(dir.gt_poly_file.length() - 3 ) == "mpu"){
            labelObjectWithImplicit(data, options.number_of_points_per_cell);
        }
        #endif

    }

    // get tet index per eval point
    if(!dir.occ_file.empty()){
        if(importOccPoints(dir,data))
            return 1;
        point2TetraIndex(data);
    }

    ////////////////////////////
    ////////// EXPORT //////////
    ////////////////////////////
    // export _eval.npz file, with eval points, occupancy and point2tetIndex
    if(!dir.occ_file.empty())
        exportOccPoints(dir,data);

    //// export features and labels
    // check if labels directory exists, if not create it
    path lpath(dir.path);
    lpath /= string("dgnn");
    if(!is_directory(lpath))
        create_directory(lpath);
    // export the features and labels
    auto ge = graphExporter(dir, data.Dt, options);
    ge.run(true);

    ///// export the 3DT as npz
    options.make_global_cell_idx=1;
    options.make_global_vertex_idx=1;
    options.make_finite_cell_idx=0;
    options.make_finite_vertex_idx=0;
    indexDelaunay(data, options);
    export3DT(dir,data);

    if(exportO.interface){
        for(auto cit = data.Dt.all_cells_begin(); cit != data.Dt.all_cells_end(); cit++){
            if(data.Dt.is_infinite(cit)){
                cit->info().gc_label = 1;
                continue;
            }
            // this means untraversed cells and 50/50 cells will be labelled as inside
            cit->info().gc_label = cit->info().outside_score > cit->info().inside_score ? 1 : 0;
        }
        exportInterface(dir, data, options, exportO);
    }

    return 0;
}

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

    cliParser ip("feat");
    if(ip.parse(argc, argv))
        return 1;
    if(ip.getInput())
        return 1;
    if(ip.getOutput())
        return 1;
    if(ip.getFeat())
        return 1;

    auto start = std::chrono::high_resolution_clock::now();
    cout << "\n-----FEATURE EXTRACTION-----" << endl;
    cout << "\nWorking dir set to:\n\t-" << ip.dh.path << endl;

    dataHolder data;
    if(extractFeatures(ip.dh, data, ip.ro, ip.eo))
        return 1;

    auto stop = std::chrono::high_resolution_clock::now();
    auto duration = std::chrono::duration_cast<std::chrono::seconds>(stop - start);
    cout << "\n-----FEATURE EXTRACTION FINISHED in "<< duration.count() << "s -----\n" << endl;

    return 0;

}


/**
 * Copyright (c) 2011-2017 libbitcoin developers (see AUTHORS)
 *
 * This file is part of libbitcoin.
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Affero General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Affero General Public License for more details.
 *
 * You should have received a copy of the GNU Affero General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */
#include <bitcoin/bitcoin/wallet/stealth_address.hpp>

#include <algorithm>
#include <cstdint>
#include <iostream>
#include <boost/program_options.hpp>
#include <bitcoin/bitcoin/formats/base_58.hpp>
#include <bitcoin/bitcoin/math/checksum.hpp>
#include <bitcoin/bitcoin/math/elliptic_curve.hpp>
#include <bitcoin/bitcoin/math/hash.hpp>
#include <bitcoin/bitcoin/math/stealth.hpp>
#include <bitcoin/bitcoin/utility/assert.hpp>
#include <bitcoin/bitcoin/utility/binary.hpp>
#include <bitcoin/bitcoin/utility/data.hpp>

namespace libbitcoin {
namespace wallet {

using namespace bc::chain;

static constexpr uint8_t version_size = sizeof(uint8_t);
static constexpr uint8_t options_size = sizeof(uint8_t);
static constexpr uint8_t number_keys_size = sizeof(uint8_t);
static constexpr uint8_t number_sigs_size = sizeof(uint8_t);
static constexpr uint8_t filter_length_size = sizeof(uint8_t);
static constexpr uint8_t max_spend_key_count = max_uint8;

// wiki.unsystem.net/index.php/DarkWallet/Stealth#Address_format
// [version:1=0x2a][options:1][scan_pubkey:33][N:1][spend_pubkey_1:33]..
// [spend_pubkey_N:33][number_signatures:1][prefix_number_bits:1]
// [filter:prefix_number_bits / 8, round up][checksum:4]
// Estimate assumes N = 0 and prefix_length = 0:
constexpr size_t min_address_size = version_size + options_size +
    ec_compressed_size + number_keys_size + number_sigs_size +
    filter_length_size + checksum_size;

// Document the assumption that the prefix is defined with an 8 bit block size.
static_assert(binary::bits_per_block == byte_bits,
    "The stealth prefix must use an 8 bit block size.");

const uint8_t stealth_address::mainnet_p2kh = 0x2a;
const uint8_t stealth_address::reuse_key_flag = 1 << 0;
const size_t stealth_address::min_filter_bits = 1 * byte_bits;
const size_t stealth_address::max_filter_bits = sizeof(uint32_t) * byte_bits;

stealth_address::stealth_address()
  : valid_(false), version_(0), scan_key_(null_compressed_point),
    spend_keys_(), signatures_(0), filter_()
{
}

stealth_address::stealth_address(const stealth_address& other)
  : valid_(other.valid_), version_(other.version_), scan_key_(other.scan_key_),
    spend_keys_(other.spend_keys_), signatures_(other.signatures_),
    filter_(other.filter_)
{
}

stealth_address::stealth_address(const std::string& encoded)
  : stealth_address(from_string(encoded))
{
}

stealth_address::stealth_address(const data_chunk& decoded)
  : stealth_address(from_stealth(decoded))
{
}

stealth_address::stealth_address(const binary& filter,
    const ec_compressed& scan_key, const point_list& spend_keys,
    uint8_t signatures, uint8_t version)
  : stealth_address(from_stealth(filter, scan_key, spend_keys, signatures,
        version))
{
}

stealth_address::stealth_address(uint8_t version, const binary& filter,
    const ec_compressed& scan_key, const point_list& spend_keys,
    uint8_t signatures)
  : valid_(true), filter_(filter), scan_key_(scan_key),
    spend_keys_(spend_keys), signatures_(signatures), version_(version)
{
}

// Factories.
// ----------------------------------------------------------------------------

stealth_address stealth_address::from_string(const std::string& encoded)
{
    data_chunk decoded;
    return decode_base58(decoded, encoded) ? stealth_address(decoded) :
        stealth_address();
}

// This is the stealth address parser.
stealth_address stealth_address::from_stealth(const data_chunk& decoded)
{
    // Size is guarded until we get to N.
    auto required_size = min_address_size;
    if (decoded.size() < required_size || !verify_checksum(decoded))
        return{};

    // [version:1 = 0x2a]
    auto iterator = decoded.begin();
    const auto version = *iterator;

    // [options:1]
    ++iterator;
    const auto options = *iterator;
    if (options > reuse_key_flag)
        return{};

    // [scan_pubkey:33]
    ++iterator;
    auto scan_key_begin = iterator;
    iterator += ec_compressed_size;
    ec_compressed scan_key;
    std::copy_n(scan_key_begin, ec_compressed_size, scan_key.begin());

    // [N:1]
    auto number_spend_pubkeys = *iterator;
    ++iterator;

    // Adjust and retest required size. for pubkey list.
    required_size += number_spend_pubkeys * ec_compressed_size;
    if (decoded.size() < required_size)
        return{};

    // We don't explicitly save 'reuse', instead we add to spend_keys_.
    point_list spend_keys;
    if (options == reuse_key_flag)
        spend_keys.push_back(scan_key);

    // [spend_pubkey_1:33]..[spend_pubkey_N:33]
    ec_compressed point;
    for (auto key = 0; key < number_spend_pubkeys; ++key)
    {
        auto spend_key_begin = iterator;
        iterator += ec_compressed_size;
        std::copy_n(spend_key_begin, ec_compressed_size, point.begin());
        spend_keys.push_back(point);
    }

    // [number_signatures:1]
    const auto signatures = *iterator;
    ++iterator;

    // [prefix_number_bits:1]
    const auto filter_bits = *iterator;
    if (filter_bits > max_filter_bits)
        return{};

    // [prefix:prefix_number_bits / 8, round up]
    ++iterator;
    const auto filter_bytes = (filter_bits + (byte_bits - 1)) / byte_bits;

    // Adjust and retest required size.
    required_size += filter_bytes;
    if (decoded.size() != required_size)
        return{};

    // Deserialize the filter bytes/blocks.
    const data_chunk raw_filter(iterator, iterator + filter_bytes);
    const binary filter(filter_bits, raw_filter);
    return{ filter, scan_key, spend_keys, signatures, version };
}

// This corrects signature and spend_keys.
stealth_address stealth_address::from_stealth(const binary& filter,
    const ec_compressed& scan_key, const point_list& spend_keys,
    uint8_t signatures, uint8_t version)
{
    // Ensure there is at least one spend key.
    auto spenders = spend_keys;
    if (spenders.empty())
        spenders.push_back(scan_key);

    // Guard against too many keys.
    const auto spend_keys_size = spenders.size();
    if (spend_keys_size > max_spend_key_count)
        return{};

    // Guard against prefix too long.
    auto prefix_number_bits = filter.size();
    if (prefix_number_bits > max_filter_bits)
        return{};

    // Coerce signatures to a valid range.
    const auto maximum = signatures == 0 || signatures > spend_keys_size;
    const auto coerced = maximum ? static_cast<uint8_t>(spend_keys_size) :
        signatures;

    // Parameter order is used to change the constructor signature.
    return{ version, filter, scan_key, spenders, coerced };
}

// Cast operators.
// ----------------------------------------------------------------------------

stealth_address::operator const bool() const
{
    return valid_;
}

stealth_address::operator const data_chunk() const
{
    return to_chunk();
}

// Serializer.
// ----------------------------------------------------------------------------

std::string stealth_address::encoded() const
{
    return encode_base58(to_chunk());
}

uint8_t stealth_address::version() const
{
    return version_;
}

// Accessors.
// ----------------------------------------------------------------------------

const binary& stealth_address::filter() const
{
    return filter_;
}

const ec_compressed& stealth_address::scan_key() const
{
    return scan_key_;
}

uint8_t stealth_address::signatures() const
{
    return signatures_;
}

const point_list& stealth_address::spend_keys() const
{
    return spend_keys_;
}

// Methods.
// ----------------------------------------------------------------------------

data_chunk stealth_address::to_chunk() const
{
    data_chunk address;
    address.push_back(version());
    address.push_back(options());
    extend_data(address, scan_key_);

    // Spend_pubkeys must have been guarded against a max size of 255.
    auto number_spend_pubkeys = static_cast<uint8_t>(spend_keys_.size());

    // Adjust for key reuse.
    if (reuse_key())
        --number_spend_pubkeys;

    address.push_back(number_spend_pubkeys);

    // Serialize the spend keys, excluding any that match the scan key.
    for (const auto& key : spend_keys_)
        if (key != scan_key_)
            extend_data(address, key);

    address.push_back(signatures_);

    // The prefix must be guarded against a size greater than 32
    // so that the bitfield can convert into uint32_t and sized by uint8_t.
    const auto prefix_number_bits = static_cast<uint8_t>(filter_.size());

    // Serialize the prefix bytes/blocks.
    address.push_back(prefix_number_bits);
    extend_data(address, filter_.blocks());

    append_checksum(address);
    return address;
}

// Helpers.
// ----------------------------------------------------------------------------

bool stealth_address::reuse_key() const
{
    // If the spend_keys_ contains the scan_key_ then the key is reused.
    return std::find(spend_keys_.begin(), spend_keys_.end(), scan_key_) !=
        spend_keys_.end();
}

uint8_t stealth_address::options() const
{
    // There is currently only one option.
    return reuse_key() ? reuse_key_flag : 0x00;
}

// Operators.
// ----------------------------------------------------------------------------

stealth_address& stealth_address::operator=(const stealth_address& other)
{
    valid_ = other.valid_;
    version_ = other.version_;
    scan_key_ = other.scan_key_;
    spend_keys_ = other.spend_keys_;
    signatures_ = other.signatures_;
    filter_ = other.filter_;
    return *this;
}

bool stealth_address::operator<(const stealth_address& other) const
{
    return encoded() < other.encoded();
}

bool stealth_address::operator==(const stealth_address& other) const
{
    return valid_ == other.valid_ && version_ == other.version_ &&
        scan_key_ == other.scan_key_&& spend_keys_ == other.spend_keys_ &&
        signatures_ == other.signatures_ && filter_ == other.filter_;
}

bool stealth_address::operator!=(const stealth_address& other) const
{
    return !(*this == other);
}

std::istream& operator>>(std::istream& in, stealth_address& to)
{
    std::string value;
    in >> value;
    to = stealth_address(value);

    if (!to)
    {
        using namespace boost::program_options;
        BOOST_THROW_EXCEPTION(invalid_option_value(value));
    }

    return in;
}

std::ostream& operator<<(std::ostream& out, const stealth_address& of)
{
    out << of.encoded();
    return out;
}

} // namespace wallet
} // namespace libbitcoin


#include "CalibTracker/SiStripLorentzAngle/interface/LA_Filler_Fitter.h"
#include "CalibTracker/SiStripCommon/interface/TTREE_FOREACH_ENTRY.hh"
#include "DataFormats/SiStripDetId/interface/StripSubdetector.h"

#include <cmath>
#include <boost/lexical_cast.hpp>

void LA_Filler_Fitter::fill(TTree* tree, Book& book) const {
  TTREE_FOREACH_ENTRY(tree) {
    TFE_MAX(maxEvents_);
    TFE_PRINTSTATUS;
    std::vector<unsigned> PLEAF(tsostrackmulti, tree);
    std::vector<unsigned> PLEAF(clusterdetid, tree);
    std::vector<unsigned> PLEAF(clusterwidth, tree);
    std::vector<float> PLEAF(clustervariance, tree);
    std::vector<float> PLEAF(tsosdriftx, tree);
    std::vector<float> PLEAF(tsosdriftz, tree);
    std::vector<float> PLEAF(tsoslocaltheta, tree);
    std::vector<float> PLEAF(tsoslocalphi, tree);
    std::vector<float> PLEAF(tsosglobalZofunitlocalY, tree);

    const unsigned N(clusterdetid.size());
    std::vector<float> BdotY(N, 0);
    if (!ensembleBins_) {
      std::vector<float> PLEAF(tsosBdotY, tree);
      swap(BdotY, tsosBdotY);
    }
    std::vector<float> localy(N, 0);
    if (localYbin_) {
      std::vector<float> PLEAF(tsoslocaly, tree);
      swap(localy, tsoslocaly);
    }
    std::vector<unsigned> seedstrip(N, 0);
    if (stripsPerBin_) {
      std::vector<unsigned> PLEAF(clusterseedstrip, tree);
      swap(seedstrip, clusterseedstrip);
    }

    for (unsigned i = 0; i < N; i++) {
      const SiStripDetId detid(clusterdetid[i]);
      if (tsostrackmulti[i] != 1 ||
          (detid.subDetector() != SiStripDetId::TIB && detid.subDetector() != SiStripDetId::TOB))
        continue;

      const int sign = tsosglobalZofunitlocalY[i] < 0 ? -1 : 1;
      const float tthetaL = sign * tsosdriftx[i] / tsosdriftz[i];
      const float tthetaT = sign * tan(tsoslocaltheta[i]) * cos(tsoslocalphi[i]);

      fill_one_cluster(book,
                       granularity(detid, tthetaL, TFE_index, localy[i], seedstrip[i] % 128),
                       clusterwidth[i],
                       clustervariance[i],
                       tthetaL,
                       tthetaT,
                       fabs(BdotY[i]));
    }
  }
}

void LA_Filler_Fitter::fill_one_cluster(Book& book,
                                        const poly<std::string>& gran,
                                        const unsigned width,
                                        const float variance,
                                        const float tthetaL,
                                        const float tthetaT,
                                        const float BdotY) const {
  book.fill(tthetaL, gran + "_reconstruction", 360, -1.0, 1.0);
  book.fill(tthetaT - tthetaL, gran + allAndOne(width), 360, -1.0, 1.0);
  book.fill(tthetaT - tthetaL, variance, gran + varWidth(width), 360, -1.0, 1.0);
  if (methods_ & WIDTH)
    book.fill(tthetaT, width, gran + method(WIDTH), 81, -0.6, 0.6);
  if (!ensembleBins_) {
    book.fill(BdotY, gran + "_field", 101, 1, 5);
    book.fill(width, gran + "_width", 10, 0, 10);
  }
}

poly<std::string> LA_Filler_Fitter::allAndOne(const unsigned width) const {
  poly<std::string> a1("_all");
  if (width == 1)
    a1 *= "_w1";
  return a1;
}

poly<std::string> LA_Filler_Fitter::varWidth(const unsigned width) const {
  poly<std::string> vw;
  vw++;
  if (width == 2 && methods_ & (AVGV2 | RMSV2))
    vw *= method(AVGV2, false);
  if (width == 3 && methods_ & (AVGV3 | RMSV3))
    vw *= method(AVGV3, false);
  return vw;
}

poly<std::string> LA_Filler_Fitter::granularity(const SiStripDetId detid,
                                                const float tthetaL,
                                                const Long64_t TFE_index,
                                                const float localy,
                                                const unsigned apvstrip) const {
  poly<std::string> gran;
  gran += subdetLabel(detid);
  if (byLayer_)
    gran *= layerLabel(detid);
  if (byModule_)
    gran *= moduleLabel(detid);
  if (localYbin_)
    gran += (localy < 0 ? "_yM" : "_yP") + std::to_string(abs((int)(localy / localYbin_ + (localy < 0 ? -1 : 0))));
  if (stripsPerBin_)
    gran += "_strip" +
            std::to_string(
                (unsigned)((0.5 + ((apvstrip / 64) ? (127 - apvstrip) : apvstrip) / stripsPerBin_) * stripsPerBin_));
  if (ensembleBins_) {
    gran +=
        "_ensembleBin" + std::to_string((int)(ensembleBins_ * (tthetaL - ensembleLow_) / (ensembleUp_ - ensembleLow_)));
    gran += "";
    if (ensembleSize_)
      gran *= "_sample" + std::to_string(TFE_index % ensembleSize_);
  }
  return gran;
}

std::string LA_Filler_Fitter::subdetLabel(const SiStripDetId detid) {
  return detid.subDetector() == SiStripDetId::TOB ? "TOB" : "TIB";
}
std::string LA_Filler_Fitter::moduleLabel(const SiStripDetId detid) {
  return subdetLabel(detid) + "_module" + std::to_string(detid());
}
std::string LA_Filler_Fitter::layerLabel(const SiStripDetId detid) const {
  const bool isTIB = detid.subdetId() == StripSubdetector::TIB;
  unsigned layer = isTIB ? tTopo_->tibLayer(detid) : tTopo_->tobLayer(detid);
  bool stereo = isTIB ? tTopo_->tibStereo(detid) : tTopo_->tobStereo(detid);

  return subdetLabel(detid) + "_layer" + std::to_string(layer) + (stereo ? "s" : "a");
}