#include "ces.h"

#include <vector>
#include <sstream>
#include <boost/shared_ptr.hpp>

#include "aligner.h"
#include "lattice.h"
#include "viterbi_envelope.h"
#include "error_surface.h"

using boost::shared_ptr;
using namespace std;

const bool minimize_segments = true;    // if adjacent segments have equal scores, merge them

void ComputeErrorSurface(const SentenceScorer& ss, const ViterbiEnvelope& ve, ErrorSurface* env, const ScoreType type, const Hypergraph& hg) {
  vector<WordID> prev_trans;
  const vector<shared_ptr<Segment> >& ienv = ve.GetSortedSegs();
  env->resize(ienv.size());
  ScoreP prev_score;
  int j = 0;
  for (int i = 0; i < ienv.size(); ++i) {
    const Segment& seg = *ienv[i];
    vector<WordID> trans;
    if (type == AER) {
      vector<bool> edges(hg.edges_.size(), false);
      seg.CollectEdgesUsed(&edges);  // get the set of edges in the viterbi
                                     // alignment
      ostringstream os;
      const string* psrc = ss.GetSource();
      if (psrc == NULL) {
        cerr << "AER scoring in VEST requires source, but it is missing!\n";
        abort();
      }
      size_t pos = psrc->rfind(" ||| ");
      if (pos == string::npos) {
        cerr << "Malformed source for AER: expected |||\nINPUT: " << *psrc << endl;
        abort();
      }
      Lattice src;
      Lattice ref;
      LatticeTools::ConvertTextOrPLF(psrc->substr(0, pos), &src);
      LatticeTools::ConvertTextOrPLF(psrc->substr(pos + 5), &ref);
      AlignerTools::WriteAlignment(src, ref, hg, &os, true, 0, &edges);
      string tstr = os.str();
      TD::ConvertSentence(tstr.substr(tstr.rfind(" ||| ") + 5), &trans);
    } else {
      seg.ConstructTranslation(&trans);
    }
    // cerr << "Scoring: " << TD::GetString(trans) << endl;
    if (trans == prev_trans) {
      if (!minimize_segments) {
        assert(prev_score); // if this fails, it means
	                    // the decoder can generate null translations
        ErrorSegment& out = (*env)[j];
        out.delta = prev_score->GetZero();
        out.x = seg.x;
	++j;
      }
      // cerr << "Identical translation, skipping scoring\n";
    } else {
      ScoreP score = ss.ScoreCandidate(trans);
      // cerr << "score= " << score->ComputeScore() << "\n";
      ScoreP cur_delta_p = score->GetZero();
      Score* cur_delta = cur_delta_p.get();
      // just record the score diffs
      if (!prev_score)
        prev_score = score->GetZero();

      score->Subtract(*prev_score, cur_delta);
      prev_trans.swap(trans);
      prev_score = score;
      if ((!minimize_segments) || (!cur_delta->IsAdditiveIdentity())) {
        ErrorSegment& out = (*env)[j];
        out.delta = cur_delta_p;
        out.x = seg.x;
        ++j;
      }
    }
  }
  // cerr << " In segments: " << ienv.size() << endl;
  // cerr << "Out segments: " << j << endl;
  assert(j > 0);
  env->resize(j);
}


/*
 * GridMap.cpp
 *
 *  Created on: Jul 14, 2014
 *      Author: Péter Fankhauser
 *	 Institute: ETH Zurich, ANYbotics
 */

#include "grid_map_core/GridMap.hpp"

#include "grid_map_core/CubicInterpolation.hpp"
#include "grid_map_core/GridMapMath.hpp"
#include "grid_map_core/SubmapGeometry.hpp"
#include "grid_map_core/iterators/GridMapIterator.hpp"

#include <Eigen/Dense>

#include <math.h>
#include <algorithm>
#include <cassert>
#include <iostream>
#include <stdexcept>

using namespace std;
using namespace grid_map;

namespace grid_map {

GridMap::GridMap(const std::vector<std::string>& layers) {
  position_.setZero();
  length_.setZero();
  resolution_ = 0.0;
  size_.setZero();
  startIndex_.setZero();
  timestamp_ = 0;
  layers_ = layers;

  for (auto& layer : layers_) {
    data_.insert(std::pair<std::string, Matrix>(layer, Matrix()));
  }
}

GridMap::GridMap() : GridMap(std::vector<std::string>()) {}

void GridMap::setGeometry2(const Length& length, const double resolution, const Position& position) {
  assert(length(0) > 0.0);
  assert(length(1) > 0.0);
  assert(resolution > 0.0);

  Size size;
  size(0) = (int) (round(length(0) / resolution));  // There is no round() function in Eigen.
  size(1) = (int) (round(length(1) / resolution));

  // std::cout << "size in setGeometry2 before: " << size_ << std::endl;
  // size_ = size;
  // std::cout << "size in setGeometry2 after: " << size_ << std::endl;
  // for (auto& data : data_) {
  //   std::cout << "data.second.size1: " << data.second.size() << " ==> ";
  //   data.second.resize(size_(0), size_(1));
  //   std::cout << "data.second.size2: " << data.second.size() << std::endl;
  //   data.second.setConstant(NAN);
  // }

  resize(size);
  clearAll();

  resolution_ = resolution;
  length_ = (size_.cast<double>() * resolution_).matrix();
  position_ = position;
  startIndex_.setZero();

  return;
}
void GridMap::setGeometry2(const SubmapGeometry& geometry) {
  setGeometry2(geometry.getLength(), geometry.getResolution(), geometry.getPosition());
}

void GridMap::setGeometry(const Length& length, const double resolution, const Position& position) {
  assert(length(0) > 0.0);
  assert(length(1) > 0.0);
  assert(resolution > 0.0);

  Size size;
  size(0) = static_cast<int>(round(length(0) / resolution));  // There is no round() function in Eigen.
  size(1) = static_cast<int>(round(length(1) / resolution));
  // std::cout << "size in setGeometry1 before: " << size_ << std::endl;
  resize(size);
  // std::cout << "size in setGeometry1 after: " << size_ << std::endl;
  clearAll();

  resolution_ = resolution;
  length_ = (size_.cast<double>() * resolution_).matrix();
  position_ = position;
  startIndex_.setZero();

  return;
}

void GridMap::setGeometry(const SubmapGeometry& geometry) {
  setGeometry(geometry.getLength(), geometry.getResolution(), geometry.getPosition());
}

void GridMap::setBasicLayers(const std::vector<std::string>& basicLayers) {
  basicLayers_ = basicLayers;
}

const std::vector<std::string>& GridMap::getBasicLayers() const {
  return basicLayers_;
}

bool GridMap::hasBasicLayers() const {
  return basicLayers_.size() > 0;
}

bool GridMap::hasSameLayers(const GridMap& other) const {
  for (const auto& layer : layers_) {
    if (!other.exists(layer)) {
      return false;
    }
  }
  return true;
}

void GridMap::add(const std::string& layer, const double value) {
  add(layer, Matrix::Constant(size_(0), size_(1), value));
}

void GridMap::add(const std::string& layer, const Matrix& data) {
  assert(size_(0) == data.rows());
  assert(size_(1) == data.cols());

  if (exists(layer)) {
    // Type exists already, overwrite its data.
    data_.at(layer) = data;
  } else {
    // Type does not exist yet, add type and data.
    data_.insert(std::pair<std::string, Matrix>(layer, data));
    layers_.push_back(layer);
  }
}

bool GridMap::exists(const std::string& layer) const {
  return !(data_.find(layer) == data_.end());
}

const Matrix& GridMap::get(const std::string& layer) const {
  try {
    return data_.at(layer);
  } catch (const std::out_of_range& exception) {
    throw std::out_of_range("GridMap::get(...) : No map layer '" + layer + "' available.");
  }
}

Matrix& GridMap::get(const std::string& layer) {
  try {
    return data_.at(layer);
  } catch (const std::out_of_range& exception) {
    throw std::out_of_range("GridMap::get(...) : No map layer of type '" + layer + "' available.");
  }
}

const Matrix& GridMap::operator[](const std::string& layer) const {
  return get(layer);
}

Matrix& GridMap::operator[](const std::string& layer) {
  return get(layer);
}

bool GridMap::erase(const std::string& layer) {
  const auto dataIterator = data_.find(layer);
  if (dataIterator == data_.end()) {
    return false;
  }
  data_.erase(dataIterator);

  const auto layerIterator = std::find(layers_.begin(), layers_.end(), layer);
  if (layerIterator == layers_.end()) {
    return false;
  }
  layers_.erase(layerIterator);

  const auto basicLayerIterator = std::find(basicLayers_.begin(), basicLayers_.end(), layer);
  if (basicLayerIterator != basicLayers_.end()) {
    basicLayers_.erase(basicLayerIterator);
  }

  return true;
}

const std::vector<std::string>& GridMap::getLayers() const {
  return layers_;
}

float& GridMap::atPosition(const std::string& layer, const Position& position) {
  Index index;
  if (getIndex(position, index)) {
    return at(layer, index);
  }
  throw std::out_of_range("GridMap::atPosition(...) : Position is out of range.");
}

float GridMap::atPosition(const std::string& layer, const Position& position, InterpolationMethods interpolationMethod) const {

  bool skipNextSwitchCase = false;
  switch (interpolationMethod) {
    case InterpolationMethods::INTER_CUBIC_CONVOLUTION: {
      float value;
      if (atPositionBicubicConvolutionInterpolated(layer, position, value)) {
        return value;
      } else {
        interpolationMethod = InterpolationMethods::INTER_LINEAR;
        skipNextSwitchCase = true;
      }
    }
    case InterpolationMethods::INTER_CUBIC: {
      if (!skipNextSwitchCase) {
        float value;
        if (atPositionBicubicInterpolated(layer, position, value)) {
          return value;
        } else {
          interpolationMethod = InterpolationMethods::INTER_LINEAR;
        }
      }
    }
    case InterpolationMethods::INTER_LINEAR: {
      float value;
      if (atPositionLinearInterpolated(layer, position, value))
        return value;
      else
        interpolationMethod = InterpolationMethods::INTER_NEAREST;
    }
    case InterpolationMethods::INTER_NEAREST: {
      Index index;
      if (getIndex(position, index)) {
        return at(layer, index);
      } else
        throw std::out_of_range("GridMap::atPosition(...) : Position is out of range.");
      break;
    }
    default:
      throw std::runtime_error(
          "GridMap::atPosition(...) : Specified "
          "interpolation method not implemented.");
  }
}

float& GridMap::at(const std::string& layer, const Index& index) {
  try {
    return data_.at(layer)(index(0), index(1));
  } catch (const std::out_of_range& exception) {
    throw std::out_of_range("GridMap::at(...) : No map layer '" + layer + "' available.");
  }
}

float GridMap::at(const std::string& layer, const Index& index) const {
  try {
    return data_.at(layer)(index(0), index(1));
  } catch (const std::out_of_range& exception) {
    throw std::out_of_range("GridMap::at(...) : No map layer '" + layer + "' available.");
  }
}

bool GridMap::getIndex(const Position& position, Index& index) const {
  return getIndexFromPosition(index, position, length_, position_, resolution_, size_, startIndex_);
}

bool GridMap::getPosition(const Index& index, Position& position) const {
  return getPositionFromIndex(position, index, length_, position_, resolution_, size_, startIndex_);
}

bool GridMap::isInside(const Position& position) const {
  return checkIfPositionWithinMap(position, length_, position_);
}

bool GridMap::isValid(DataType value) const {
  return isfinite(value);
}

bool GridMap::isValid(const Index& index) const {
  return isValid(index, basicLayers_);
}

bool GridMap::isValid(const Index& index, const std::string& layer) const {
  return isValid(at(layer, index));
}

bool GridMap::isValid(const Index& index, const std::vector<std::string>& layers) const {
  if (layers.empty()) {
    return false;
  }
  for (const auto& layer : layers) {
    if (!isValid(index, layer)) {
      return false;
    }
  }
  return true;
}

bool GridMap::getPosition3(const std::string& layer, const Index& index, Position3& position) const {
  const auto value = at(layer, index);
  if (!isValid(value)) {
    return false;
  }
  Position position2d;
  getPosition(index, position2d);
  position.head(2) = position2d;
  position.z() = value;
  return true;
}

bool GridMap::getVector(const std::string& layerPrefix, const Index& index, Eigen::Vector3d& vector) const {
  Eigen::Vector3d temp{at(layerPrefix + "x", index), at(layerPrefix + "y", index), at(layerPrefix + "z", index)};
  if (!isValid(temp[0]) || !isValid(temp[1]) || !isValid(temp[2])) {
    return false;
  } else {
    vector = temp;
    return true;
  }
}

GridMap GridMap::getSubmap(const Position& position, const Length& length, bool& isSuccess, int flag)  const {
  Index index;
  return getSubmap(position, length, index, isSuccess, flag);
}

GridMap GridMap::getSubmap(const Position& position, const Length& length, Index& indexInSubmap, bool& isSuccess, int flag)  const{
  // Submap the generate.
  GridMap submap(layers_);
  submap.setBasicLayers(basicLayers_);
  submap.setTimestamp(timestamp_);
  submap.setFrameId(frameId_);

  // std::cout << "submap1" << std::endl;
  // Get submap geometric information.
  SubmapGeometry submapInformation(*this, position, length, isSuccess);
  if (!isSuccess) {
    return GridMap(layers_);
  }
  submap.setGeometry(submapInformation);
  submap.startIndex_.setZero();  // Because of the way we copy the data below.

  // Copy data.
  std::vector<BufferRegion> bufferRegions;
  // std::cout << "submapInformation.getStartIndex(): " << submapInformation.getStartIndex() << std::endl;
  // std::cout << "size_: " << size_ << std::endl;
  // std::cout << "startIndex_: " << startIndex_ << std::endl;
  // std::cout << "submap.getSize(): " << submap.getSize() << std::endl;
  if (!getBufferRegionsForSubmap(bufferRegions, submapInformation.getStartIndex(), submap.getSize(), size_, startIndex_)) {
    cout << "Cannot access submap of this size." << endl;
    isSuccess = false;
    return GridMap(layers_);
  }

  // std::cout << "GridMap::getSubmap() | bufferRegions.size(): " << bufferRegions.size() << "\n";
  // std::cout << "GridMap::getSubmap() | data_.size(): " << data_.size() << "\n";
  // std::cout << "GridMap::getSubmap() | submap.data_.size(): " << submap.data_.size() << "\n";

  for (const auto& data : data_) {
    for (const auto& bufferRegion : bufferRegions) {
      Index index = bufferRegion.getStartIndex();
      Size size = bufferRegion.getSize();
      // std::cout << "data.first, bufferRegion index, size: " << data.first << " | " << index[0] << "," << index[1] << " ; " << size[0] << "," << size[1] << std::endl;
      // std::cout << "submap.data_[" << data.first << "].size(): " << submap.data_[data.first].size() << std::endl;
      // std::cout << "data.second.block(index...).size(): " << data.second.block(index(0), index(1), size(0), size(1)).size() << std::endl;
      // std::cout << "data.second.size(): " << data.second.size() << std::endl;
      // std::cout << "isValid: " << isValid(index, data.first) << std::endl;
      // std::cout << "isValid(0,0): " << isValid(Index(0,0), data.first) << std::endl;

      if (bufferRegion.getQuadrant() == BufferRegion::Quadrant::TopLeft) {
        submap.data_[data.first].topLeftCorner(size(0), size(1)) = data.second.block(index(0), index(1), size(0), size(1));
      } else if (bufferRegion.getQuadrant() == BufferRegion::Quadrant::TopRight) {
        submap.data_[data.first].topRightCorner(size(0), size(1)) = data.second.block(index(0), index(1), size(0), size(1));
      } else if (bufferRegion.getQuadrant() == BufferRegion::Quadrant::BottomLeft) {
        submap.data_[data.first].bottomLeftCorner(size(0), size(1)) = data.second.block(index(0), index(1), size(0), size(1));
      } else if (bufferRegion.getQuadrant() == BufferRegion::Quadrant::BottomRight) {
        submap.data_[data.first].bottomRightCorner(size(0), size(1)) = data.second.block(index(0), index(1), size(0), size(1));
      }
    }
  }

  isSuccess = true;
  // std::cout << "GridMap::getSubmap submap2" << std::endl;
  //
  // std::cout << "this->printInfo in GridMap::getSUbmap: ====\n";
  // printInfo();

  // submap.flag = flag;
  // std::cout << "GridMap::getSubmap submap.printInfo() ===\n";
  // submap.printInfo();

  return submap;
}

GridMap GridMap::getTransformedMap(const Eigen::Isometry3d& transform, const std::string& heightLayerName, const std::string& newFrameId,
                                   const double sampleRatio) const {
  // Check if height layer is valid.
  if (!exists(heightLayerName)) {
    throw std::out_of_range("GridMap::getTransformedMap(...) : No map layer '" + heightLayerName + "' available.");
  }

  // Initialization.
  std::vector<Position3> positionSamples;
  Position3 center;
  Index newIndex;

  const double sampleLength = resolution_ * sampleRatio;

  // Find edges in new coordinate frame.
  const double halfLengthX = length_.x() * 0.5;
  const double halfLengthY = length_.y() * 0.5;
  const Position3 topLeftCorner(position_.x() + halfLengthX, position_.y() + halfLengthY, 0.0);
  const Position3 topRightCorner(position_.x() + halfLengthX, position_.y() - halfLengthY, 0.0);
  const Position3 bottomLeftCorner(position_.x() - halfLengthX, position_.y() + halfLengthY, 0.0);
  const Position3 bottomRightCorner(position_.x() - halfLengthX, position_.y() - halfLengthY, 0.0);

  std::vector<Position3> newEdges;
  newEdges.reserve(4);
  newEdges.push_back(transform * topLeftCorner);
  newEdges.push_back(transform * topRightCorner);
  newEdges.push_back(transform * bottomLeftCorner);
  newEdges.push_back(transform * bottomRightCorner);

  // Find new grid center.
  Position3 newCenter = Position3::Zero();
  for (const auto& newEdge : newEdges) {
    newCenter += newEdge;
  }
  newCenter *= 0.25;

  // Find new grid length.
  Length maxLengthFromCenter = Length(0.0, 0.0);
  for (const auto& newEdge : newEdges) {
    Position3 positionCenterToEdge = newEdge - newCenter;
    maxLengthFromCenter.x() = std::fmax(std::fabs(positionCenterToEdge.x()), maxLengthFromCenter.x());
    maxLengthFromCenter.y() = std::fmax(std::fabs(positionCenterToEdge.y()), maxLengthFromCenter.y());
  }
  Length newLength = 2.0 * maxLengthFromCenter;

  // Create new grid map.
  GridMap newMap(layers_);
  newMap.setBasicLayers(basicLayers_);
  newMap.setTimestamp(timestamp_);
  newMap.setFrameId(newFrameId);
  newMap.setGeometry(newLength, resolution_, Position(newCenter.x(), newCenter.y()));
  newMap.startIndex_.setZero();

  for (GridMapIterator iterator(*this); !iterator.isPastEnd(); ++iterator) {
    // Get position at current index.
    if (!getPosition3(heightLayerName, *iterator, center)) {
      continue;
    }

    // Sample four points around the center cell.
    positionSamples.clear();

    if (sampleRatio > 0.0) {
      positionSamples.reserve(5);
      positionSamples.push_back(center);
      positionSamples.push_back(Position3(center.x() - sampleLength, center.y(), center.z()));
      positionSamples.push_back(Position3(center.x() + sampleLength, center.y(), center.z()));
      positionSamples.push_back(Position3(center.x(), center.y() - sampleLength, center.z()));
      positionSamples.push_back(Position3(center.x(), center.y() + sampleLength, center.z()));
    } else {
      positionSamples.push_back(center);
    }

    // Transform the sampled points and register to the new map.
    for (const auto& position : positionSamples) {
      const Position3 transformedPosition = transform * position;

      // Get new index.
      if (!newMap.getIndex(Position(transformedPosition.x(), transformedPosition.y()), newIndex)) {
        continue;
      }

      // Check if we have already assigned a value (preferably larger height
      // values -> inpainting).
      const auto newExistingValue = newMap.at(heightLayerName, newIndex);
      if (!std::isnan(newExistingValue) && newExistingValue > transformedPosition.z()) {
        continue;
      }

      // Copy the layers.
      for (const auto& layer : layers_) {
        const auto currentValueInOldGrid = at(layer, *iterator);
        auto& newValue = newMap.at(layer, newIndex);
        if (layer == heightLayerName) {
          newValue = transformedPosition.z();
        }  // adjust height
        else {
          newValue = currentValueInOldGrid;
        }  // re-assign
      }
    }
  }

  return newMap;
}

void GridMap::setPosition(const Position& position) {
  position_ = position;
}

bool GridMap::move(const Position& position, std::vector<BufferRegion>& newRegions) {
  Index indexShift;
  Position positionShift = position - position_;
  getIndexShiftFromPositionShift(indexShift, positionShift, resolution_);
  Position alignedPositionShift;
  getPositionShiftFromIndexShift(alignedPositionShift, indexShift, resolution_);

  // Delete fields that fall out of map (and become empty cells).
  for (int i = 0; i < indexShift.size(); i++) {
    if (indexShift(i) != 0) {
      if (abs(indexShift(i)) >= getSize()(i)) {
        // Entire map is dropped.
        clearAll();
        newRegions.push_back(BufferRegion(Index(0, 0), getSize(), BufferRegion::Quadrant::Undefined));
      } else {
        // Drop cells out of map.
        int sign = (indexShift(i) > 0 ? 1 : -1);
        int startIndex = startIndex_(i) - (sign < 0 ? 1 : 0);
        int endIndex = startIndex - sign + indexShift(i);
        int nCells = abs(indexShift(i));
        int index = (sign > 0 ? startIndex : endIndex);
        wrapIndexToRange(index, getSize()(i));

        if (index + nCells <= getSize()(i)) {
          // One region to drop.
          if (i == 0) {
            clearRows(index, nCells);
            newRegions.push_back(BufferRegion(Index(index, 0), Size(nCells, getSize()(1)), BufferRegion::Quadrant::Undefined));
          } else if (i == 1) {
            clearCols(index, nCells);
            newRegions.push_back(BufferRegion(Index(0, index), Size(getSize()(0), nCells), BufferRegion::Quadrant::Undefined));
          }
        } else {
          // Two regions to drop.
          int firstIndex = index;
          int firstNCells = getSize()(i) - firstIndex;
          if (i == 0) {
            clearRows(firstIndex, firstNCells);
            newRegions.push_back(BufferRegion(Index(firstIndex, 0), Size(firstNCells, getSize()(1)), BufferRegion::Quadrant::Undefined));
          } else if (i == 1) {
            clearCols(firstIndex, firstNCells);
            newRegions.push_back(BufferRegion(Index(0, firstIndex), Size(getSize()(0), firstNCells), BufferRegion::Quadrant::Undefined));
          }

          int secondIndex = 0;
          int secondNCells = nCells - firstNCells;
          if (i == 0) {
            clearRows(secondIndex, secondNCells);
            newRegions.push_back(BufferRegion(Index(secondIndex, 0), Size(secondNCells, getSize()(1)), BufferRegion::Quadrant::Undefined));
          } else if (i == 1) {
            clearCols(secondIndex, secondNCells);
            newRegions.push_back(BufferRegion(Index(0, secondIndex), Size(getSize()(0), secondNCells), BufferRegion::Quadrant::Undefined));
          }
        }
      }
    }
  }

  // Update information.
  startIndex_ += indexShift;
  wrapIndexToRange(startIndex_, getSize());
  position_ += alignedPositionShift;

  // Check if map has been moved at all.
  return (indexShift.any() != 0);
}

bool GridMap::move(const Position& position) {
  std::vector<BufferRegion> newRegions;
  return move(position, newRegions);
}

bool GridMap::addDataFrom(const GridMap& other, bool extendMap, bool overwriteData, bool copyAllLayers, std::vector<std::string> layers) {
  // Set the layers to copy.
  if (copyAllLayers) layers = other.getLayers();

  // Resize map.
  if (extendMap) extendToInclude(other);

  // Check if all layers to copy exist and add missing layers.
  for (const auto& layer : layers) {
    if (std::find(layers_.begin(), layers_.end(), layer) == layers_.end()) {
      add(layer);
    }
  }
  // Copy data.
  for (GridMapIterator iterator(*this); !iterator.isPastEnd(); ++iterator) {
    if (isValid(*iterator) && !overwriteData) continue;
    Position position;
    getPosition(*iterator, position);
    Index index;
    if (!other.isInside(position)) continue;
    other.getIndex(position, index);
    for (const auto& layer : layers) {
      if (!other.isValid(index, layer)) continue;
      at(layer, *iterator) = other.at(layer, index);
    }
  }

  return true;
}

bool GridMap::extendToInclude(const GridMap& other) {
  // Get dimension of maps.
  Position topLeftCorner(position_.x() + length_.x() / 2.0, position_.y() + length_.y() / 2.0);
  Position bottomRightCorner(position_.x() - length_.x() / 2.0, position_.y() - length_.y() / 2.0);
  Position topLeftCornerOther(other.getPosition().x() + other.getLength().x() / 2.0, other.getPosition().y() + other.getLength().y() / 2.0);
  Position bottomRightCornerOther(other.getPosition().x() - other.getLength().x() / 2.0,
                                  other.getPosition().y() - other.getLength().y() / 2.0);
  // Check if map needs to be resized.
  bool resizeMap = false;
  Position extendedMapPosition = position_;
  Length extendedMapLength = length_;
  if (topLeftCornerOther.x() > topLeftCorner.x()) {
    extendedMapPosition.x() += (topLeftCornerOther.x() - topLeftCorner.x()) / 2.0;
    extendedMapLength.x() += topLeftCornerOther.x() - topLeftCorner.x();
    resizeMap = true;
  }
  if (topLeftCornerOther.y() > topLeftCorner.y()) {
    extendedMapPosition.y() += (topLeftCornerOther.y() - topLeftCorner.y()) / 2.0;
    extendedMapLength.y() += topLeftCornerOther.y() - topLeftCorner.y();
    resizeMap = true;
  }
  if (bottomRightCornerOther.x() < bottomRightCorner.x()) {
    extendedMapPosition.x() -= (bottomRightCorner.x() - bottomRightCornerOther.x()) / 2.0;
    extendedMapLength.x() += bottomRightCorner.x() - bottomRightCornerOther.x();
    resizeMap = true;
  }
  if (bottomRightCornerOther.y() < bottomRightCorner.y()) {
    extendedMapPosition.y() -= (bottomRightCorner.y() - bottomRightCornerOther.y()) / 2.0;
    extendedMapLength.y() += bottomRightCorner.y() - bottomRightCornerOther.y();
    resizeMap = true;
  }
  // Resize map and copy data to new map.
  if (resizeMap) {
    GridMap mapCopy = *this;
    setGeometry(extendedMapLength, resolution_, extendedMapPosition);
    // Align new map with old one.
    Vector shift = position_ - mapCopy.getPosition();
    shift.x() = std::fmod(shift.x(), resolution_);
    shift.y() = std::fmod(shift.y(), resolution_);
    if (std::abs(shift.x()) < resolution_ / 2.0) {
      position_.x() -= shift.x();
    } else {
      position_.x() += resolution_ - shift.x();
    }
    if (size_.x() % 2 != mapCopy.getSize().x() % 2) {
      position_.x() += -std::copysign(resolution_ / 2.0, shift.x());
    }
    if (std::abs(shift.y()) < resolution_ / 2.0) {
      position_.y() -= shift.y();
    } else {
      position_.y() += resolution_ - shift.y();
    }
    if (size_.y() % 2 != mapCopy.getSize().y() % 2) {
      position_.y() += -std::copysign(resolution_ / 2.0, shift.y());
    }
    // Copy data.
    for (GridMapIterator iterator(*this); !iterator.isPastEnd(); ++iterator) {
      if (isValid(*iterator)) continue;
      Position position;
      getPosition(*iterator, position);
      Index index;
      if (!mapCopy.isInside(position)) continue;
      mapCopy.getIndex(position, index);
      for (const auto& layer : layers_) {
        at(layer, *iterator) = mapCopy.at(layer, index);
      }
    }
  }
  return true;
}

void GridMap::setTimestamp(const Time timestamp) {
  timestamp_ = timestamp;
}

Time GridMap::getTimestamp() const {
  return timestamp_;
}

void GridMap::resetTimestamp() {
  timestamp_ = 0.0;
}

void GridMap::setFrameId(const std::string& frameId) {
  frameId_ = frameId;
}

const std::string& GridMap::getFrameId() const {
  return frameId_;
}

const Length& GridMap::getLength() const {
  return length_;
}

const Position& GridMap::getPosition() const {
  return position_;
}

double GridMap::getResolution() const {
  return resolution_;
}

const Size& GridMap::getSize() const {
  return size_;
}

void GridMap::setStartIndex(const Index& startIndex) {
  startIndex_ = startIndex;
}

const Index& GridMap::getStartIndex() const {
  return startIndex_;
}

bool GridMap::isDefaultStartIndex() const {
  return (startIndex_ == 0).all();
}

void GridMap::convertToDefaultStartIndex() {
  if (isDefaultStartIndex()) return;

  std::vector<BufferRegion> bufferRegions;
  if (!getBufferRegionsForSubmap(bufferRegions, startIndex_, size_, size_, startIndex_)) {
    throw std::out_of_range("Cannot access submap of this size.");
  }

  for (auto& data : data_) {
    auto tempData(data.second);
    for (const auto& bufferRegion : bufferRegions) {
      Index index = bufferRegion.getStartIndex();
      Size size = bufferRegion.getSize();

      if (bufferRegion.getQuadrant() == BufferRegion::Quadrant::TopLeft) {
        tempData.topLeftCorner(size(0), size(1)) = data.second.block(index(0), index(1), size(0), size(1));
      } else if (bufferRegion.getQuadrant() == BufferRegion::Quadrant::TopRight) {
        tempData.topRightCorner(size(0), size(1)) = data.second.block(index(0), index(1), size(0), size(1));
      } else if (bufferRegion.getQuadrant() == BufferRegion::Quadrant::BottomLeft) {
        tempData.bottomLeftCorner(size(0), size(1)) = data.second.block(index(0), index(1), size(0), size(1));
      } else if (bufferRegion.getQuadrant() == BufferRegion::Quadrant::BottomRight) {
        tempData.bottomRightCorner(size(0), size(1)) = data.second.block(index(0), index(1), size(0), size(1));
      }
    }
    data.second = tempData;
  }

  startIndex_.setZero();
}

Position GridMap::getClosestPositionInMap(const Position& position) const {
  if (getSize().x() < 1u || getSize().y() < 1u) {
    return position_;
  }

  if (isInside(position)) {
    return position;
  }

  Position positionInMap = position;

  // Find edges.
  const double halfLengthX = length_.x() * 0.5;
  const double halfLengthY = length_.y() * 0.5;
  const Position3 topLeftCorner(position_.x() + halfLengthX, position_.y() + halfLengthY, 0.0);
  const Position3 topRightCorner(position_.x() + halfLengthX, position_.y() - halfLengthY, 0.0);
  const Position3 bottomLeftCorner(position_.x() - halfLengthX, position_.y() + halfLengthY, 0.0);
  const Position3 bottomRightCorner(position_.x() - halfLengthX, position_.y() - halfLengthY, 0.0);

  // Find constraints.
  const double maxX = topRightCorner.x();
  const double minX = bottomRightCorner.x();
  const double maxY = bottomLeftCorner.y();
  const double minY = bottomRightCorner.y();

  // Clip to box constraints and correct for indexing precision.
  // Points on the border can lead to invalid indices because the cells represent half open intervals, i.e. [...).
  positionInMap.x() = std::fmin(positionInMap.x(), maxX - std::numeric_limits<double>::epsilon());
  positionInMap.y() = std::fmin(positionInMap.y(), maxY - std::numeric_limits<double>::epsilon());

  positionInMap.x() = std::fmax(positionInMap.x(), minX + std::numeric_limits<double>::epsilon());
  positionInMap.y() = std::fmax(positionInMap.y(), minY + std::numeric_limits<double>::epsilon());

  return positionInMap;
}

void GridMap::clear(const std::string& layer) {
  try {
    data_.at(layer).setConstant(NAN);
  } catch (const std::out_of_range& exception) {
    throw std::out_of_range("GridMap::clear(...) : No map layer '" + layer + "' available.");
  }
}

void GridMap::clearBasic() {
  for (auto& layer : basicLayers_) {
    clear(layer);
  }
}

void GridMap::clearAll() {
  for (auto& data : data_) {
    data.second.setConstant(NAN);
  }
}

void GridMap::clearRows(unsigned int index, unsigned int nRows) {
  for (auto& layer : layers_) {
    data_.at(layer).block(index, 0, nRows, getSize()(1)).setConstant(NAN);
  }
}

void GridMap::clearCols(unsigned int index, unsigned int nCols) {
  for (auto& layer : layers_) {
    data_.at(layer).block(0, index, getSize()(0), nCols).setConstant(NAN);
  }
}

bool GridMap::atPositionLinearInterpolated(const std::string& layer, const Position& position, float& value) const {
  Position point;
  Index indices[4];
  bool idxTempDir;
  size_t idxShift[4];

  getIndex(position, indices[0]);
  getPosition(indices[0], point);

  if (position.x() >= point.x()) {
    indices[1] = indices[0] + Index(-1, 0);  // Second point is above first point.
    idxTempDir = true;
  } else {
    indices[1] = indices[0] + Index(+1, 0);
    idxTempDir = false;
  }
  if (position.y() >= point.y()) {
    indices[2] = indices[0] + Index(0, -1);  // Third point is right of first point.
    if (idxTempDir) {
      idxShift[0] = 0;
      idxShift[1] = 1;
      idxShift[2] = 2;
      idxShift[3] = 3;
    } else {
      idxShift[0] = 1;
      idxShift[1] = 0;
      idxShift[2] = 3;
      idxShift[3] = 2;
    }

  } else {
    indices[2] = indices[0] + Index(0, +1);
    if (idxTempDir) {
      idxShift[0] = 2;
      idxShift[1] = 3;
      idxShift[2] = 0;
      idxShift[3] = 1;
    } else {
      idxShift[0] = 3;
      idxShift[1] = 2;
      idxShift[2] = 1;
      idxShift[3] = 0;
    }
  }
  indices[3].x() = indices[1].x();
  indices[3].y() = indices[2].y();

  const Size& mapSize = getSize();
  const size_t bufferSize = mapSize(0) * mapSize(1);
  const size_t startIndexLin = getLinearIndexFromIndex(startIndex_, mapSize);
  const size_t endIndexLin = startIndexLin + bufferSize;
  const auto& layerMat = operator[](layer);
  float f[4];

  for (size_t i = 0; i < 4; ++i) {
    const size_t indexLin = getLinearIndexFromIndex(indices[idxShift[i]], mapSize);
    if ((indexLin < startIndexLin) || (indexLin > endIndexLin)) return false;
    f[i] = layerMat(indexLin);
  }

  getPosition(indices[idxShift[0]], point);
  const Position positionRed = (position - point) / resolution_;
  const Position positionRedFlip = Position(1., 1.) - positionRed;

  value = f[0] * positionRedFlip.x() * positionRedFlip.y() + f[1] * positionRed.x() * positionRedFlip.y() +
          f[2] * positionRedFlip.x() * positionRed.y() + f[3] * positionRed.x() * positionRed.y();
  return true;
}

void GridMap::resize(const Index& size) {
  size_ = size;
  for (auto& data : data_) {
    data.second.resize(size_(0), size_(1));
  }
}

bool GridMap::atPositionBicubicConvolutionInterpolated(const std::string& layer, const Position& position,
                                            float& value) const
{
  double interpolatedValue = 0.0;
  if (!bicubic_conv::evaluateBicubicConvolutionInterpolation(*this, layer, position, &interpolatedValue)) {
    return false;
  }

  if (!std::isfinite(interpolatedValue)) {
    return false;
  }
  value = interpolatedValue;

  return true;
}

bool GridMap::atPositionBicubicInterpolated(const std::string& layer, const Position& position,
                                            float& value) const
{
  double interpolatedValue = 0.0;
  if (!bicubic::evaluateBicubicInterpolation(*this, layer, position, &interpolatedValue)) {
    return false;
  }

  if (!std::isfinite(interpolatedValue)) {
    return false;
  }
  value = interpolatedValue;

  return true;

}

}  // namespace grid_map


/**
 * Source file for class Ledger
 * Please see Ledger.h for documentation
 */

#include "statsdcc/ledger.h"

#include <boost/lexical_cast.hpp>

#include <algorithm>
#include <cmath>
#include <cstring>

#include "statsdcc/chrono.h"
#include "statsdcc/configs/aggregator_config.h"
#include "statsdcc/logger.h"

namespace statsdcc {

// buffers the metric
void Ledger::buffer(const std::string& metric) {
  const char *metric_csty = metric.c_str();

  char name_buffer[MAX_TOKEN_BUFFER_SIZE] = {0};
  char type_buffer[3] = {0};
  char metric_value_buffer[MAX_TOKEN_BUFFER_SIZE] = {0};
  double metric_value = -std::numeric_limits<double>::max();
  double sample_rate = 1;
  MetricType type = MetricType::counter;

  // record number of calls to buffer method
  ++this->statsd_metrics["metrics_processed"];

  // setup the names for the stats stored in counters
  std::string bad_lines_seen = ::config->name + ".bad_lines_seen";

  sscanf(metric_csty,
         "%" MAX_TOKEN_STRING_LENGTH "[^:]:" \
         "%" MAX_TOKEN_STRING_LENGTH "[^|]|" \
         "%2[^|]|"                           \
         "@%lf",
         name_buffer, metric_value_buffer, type_buffer, &sample_rate);

  // trim leading spaces if any
  char *tmp_type = type_buffer;
  while (*tmp_type == ' ' && *tmp_type != '\0')
    ++tmp_type;
  std::string metric_name(std::move(name_buffer));
  std::string metric_type_str(tmp_type);

  // track bad lines
  // using find() intead of == so that we can ignore trailing spaces
  bool bad_line = false;
  if ("" == metric_type_str) {
    bad_line = true;
  } else if (metric_type_str.find("ms") == 0) {
    type = MetricType::timer;
    try {
      metric_value = boost::lexical_cast<double>(metric_value_buffer);
    } catch(...) {
      bad_line = true;
    }
  } else if (metric_type_str.find("c") == 0) {
    type = MetricType::counter;
    try {
      metric_value = boost::lexical_cast<double>(metric_value_buffer);
    } catch(...) {
      bad_line = true;
    }
  } else if (metric_type_str.find("g") == 0) {
    type = MetricType::gauge;
    try {
      metric_value = boost::lexical_cast<double>(metric_value_buffer);
    } catch(...) {
      bad_line = true;
    }
  } else if (metric_type_str.find("s") == 0) {
    type = MetricType::set;
    bad_line = false;
  } else {
    try {
      metric_value = boost::lexical_cast<double>(metric_value_buffer);
    } catch(...) {
      bad_line = true;
    }
  }

  if (bad_line) {
    ::logger->info("Bad line: " + metric);
    ++this->counters[bad_lines_seen];
    return;
  }

  ++this->frequency[metric_name];

  switch (type) {
    case MetricType::timer:
      this->timer_counters[metric_name] += (1 / sample_rate);
      this->timers[metric_name].push_back(metric_value);
      break;

    case MetricType::gauge:
      {
        // check if +/- is specified
        char char_after_colon = metric_csty[metric.find_first_of(":") + 1];
        if (('+' == char_after_colon) || ('-' == char_after_colon)) {
          this->gauges[metric_name] += metric_value;
        } else {
          this->gauges[metric_name] = metric_value;
        }
        break;
      }
    case MetricType::set:
      this->sets[metric_name].insert(metric_value_buffer);
      break;

    default:
      this->counters[metric_name] += metric_value * (1 / sample_rate);
      break;
  }
}

// Aggregates buffered metrics
void Ledger::process() {
  std::uint64_t start_time = chrono::unixtime_ms();

  // process counters
  // calculate "per second" rate
  for (auto counter_itr = this->counters.cbegin();
      counter_itr != this->counters.cend();
      ++counter_itr) {
    this->counter_rates[counter_itr->first] =
      counter_itr->second / ::config->frequency;
  }

  // process timers
  for (auto timer_key_value_pair_itr = this->timers.cbegin();
      timer_key_value_pair_itr != this->timers.cend();
      ++timer_key_value_pair_itr) {
    std::unordered_map<std::string, double> current_timer_data;
    std::string key = timer_key_value_pair_itr->first;

    if (key.length() <= 0) {
      current_timer_data["count"] = current_timer_data["count_ps"] = 0;
    } else {
      // get sorted values
      std::vector<double> values(timer_key_value_pair_itr->second);
      std::sort(values.begin(), values.end());

      // get count, sum, mean, min, and max
      int count = values.size();
      double min = values.front();
      double max = values.back();
      double sum = 0;
      for (auto value_itr = values.cbegin();
          value_itr != values.cend();
          ++value_itr) {
        sum += *value_itr;
      }
      double mean = sum / count;

      // initialize sum, mean, and threshold boundary
      double threshold_boundary = max;

      for (auto threshold_itr = ::config->percentiles.cbegin();
          threshold_itr != ::config->percentiles.cend();
          ++threshold_itr) {
        double pct =
          (*threshold_itr < 0) ? -(*threshold_itr) : (*threshold_itr);
         // Int is potentially too short since we could have more than 65K metrics in the flush window
         unsigned long num_in_threshold = 0;

        if (count > 1) {
          num_in_threshold = round(pct / 100 * count);
          if (0 == num_in_threshold) {
            continue;
          }
          threshold_boundary = values[num_in_threshold - 1];
        }

        double pct_sum = 0;
        for (unsigned long i = 0; i < num_in_threshold; ++i) {
            pct_sum += values[i];
        }
        double pct_mean = pct_sum / num_in_threshold;

        // generate pct name
        char clean_pct[17] = {0};
        snprintf(clean_pct, sizeof(clean_pct), "%g", pct);
        for (unsigned int i = 0; i < strlen(clean_pct); ++i) {
          clean_pct[i] = ('.' == clean_pct[i]) ? '_' : clean_pct[i];
        }

        current_timer_data[std::string("upper_") + std::string(clean_pct)] = threshold_boundary;
        current_timer_data[std::string("count_") + std::string(clean_pct)] = num_in_threshold;
        current_timer_data[std::string("sum_") + std::string(clean_pct)] = pct_sum;
        current_timer_data[std::string("mean_") + std::string(clean_pct)] = pct_mean;

      }  // foreach percentile

      current_timer_data["upper"] = max;
      current_timer_data["lower"] = min;
      current_timer_data["count"] = this->timer_counters[key];
      current_timer_data["mean"] = mean;
    }

    this->timer_data[key] = current_timer_data;
  }  // foreach metric

  this->statsd_metrics["processing_time"] =
    chrono::unixtime_ms() - start_time;
}

}  // namespace statsdcc


//---------------------------------------------------------------------------//
// Copyright (c) 2018-2020 Mikhail Komarov <nemo@nil.foundation>
// Copyright (c) 2020 Ilias Khairullin <ilias@nil.foundation>
//
// MIT License
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//---------------------------------------------------------------------------//

#ifndef CRYPTO3_PUBKEY_MODES_CREATE_KEY_HPP
#define CRYPTO3_PUBKEY_MODES_CREATE_KEY_HPP

#include <type_traits>
#include <iterator>

#include <boost/assert.hpp>
#include <boost/concept_check.hpp>

#include <boost/range/concepts.hpp>

#include <nil/crypto3/pubkey/secret_sharing.hpp>
#include <nil/crypto3/pubkey/dkg.hpp>

#include <nil/crypto3/pubkey/private_key.hpp>

#include <nil/crypto3/pubkey/algorithm/deal_shares.hpp>
#include <nil/crypto3/pubkey/algorithm/deal_share.hpp>
#include <nil/crypto3/pubkey/algorithm/verify_share.hpp>

namespace nil {
    namespace crypto3 {
        //
        // CoeffsIterator - coefficients of polynomial
        //
        template<typename Scheme, typename CoeffsIterator, typename Number,
                 typename SecretSharingScheme = typename pubkey::private_key<Scheme>::sss_public_key_group_type,
                 typename ValueType = typename std::iterator_traits<CoeffsIterator>::value_type,
                 typename SecretSharingScheme::template check_coeff_type<ValueType> = true>
        inline typename std::enable_if<
            std::is_same<pubkey::shamir_sss<typename SecretSharingScheme::group_type>, SecretSharingScheme>::value ||
                std::is_same<pubkey::feldman_sss<typename SecretSharingScheme::group_type>, SecretSharingScheme>::value,
            std::pair<pubkey::public_key<Scheme>, std::vector<pubkey::private_key<Scheme>>>>::type
            create_key(CoeffsIterator first, CoeffsIterator last, Number n) {
            BOOST_CONCEPT_ASSERT((boost::InputIteratorConcept<CoeffsIterator>));

            using privkeys_type = std::vector<pubkey::private_key<Scheme>>;
            using sss_no_key_ops_type = typename pubkey::private_key<Scheme>::sss_public_key_no_key_ops_type;

            typename sss_no_key_ops_type::shares_type shares =
                nil::crypto3::deal_shares<SecretSharingScheme>(first, last, n);
            privkeys_type privkeys;
            for (const auto &s : shares) {
                privkeys.emplace_back(s);
            }
            auto PK = pubkey::public_key<Scheme>(sss_no_key_ops_type::get_public_coeffs(first, last).front());
            return std::make_pair(PK, privkeys);
        }

        //
        // CoeffsRange - coefficients of polynomial
        //
        template<typename Scheme, typename CoeffsRange, typename Number,
                 typename SecretSharingScheme = typename pubkey::private_key<Scheme>::sss_public_key_group_type,
                 typename ValueType = typename std::iterator_traits<typename CoeffsRange::iterator>::value_type,
                 typename SecretSharingScheme::template check_coeff_type<ValueType> = true>
        inline typename std::enable_if<
            std::is_same<pubkey::shamir_sss<typename SecretSharingScheme::group_type>, SecretSharingScheme>::value ||
                std::is_same<pubkey::feldman_sss<typename SecretSharingScheme::group_type>, SecretSharingScheme>::value,
            std::pair<pubkey::public_key<Scheme>, std::vector<pubkey::private_key<Scheme>>>>::type
            create_key(const CoeffsRange &r, Number n) {
            BOOST_RANGE_CONCEPT_ASSERT((boost::SinglePassRangeConcept<const CoeffsRange>));
            return create_key<Scheme>(r.begin(), r.end(), n);
        }

        //
        // InputIterator - public representation values of polynomial's coefficients
        //
        template<typename Scheme, typename CoeffsIterator, typename Number,
                 typename SecretSharingScheme = typename pubkey::private_key<Scheme>::sss_public_key_group_type,
                 typename ValueType = typename std::iterator_traits<CoeffsIterator>::value_type,
                 typename SecretSharingScheme::template check_public_coeff_type<ValueType> = true>
        inline typename std::enable_if<
            std::is_same<pubkey::feldman_sss<typename SecretSharingScheme::group_type>, SecretSharingScheme>::value,
            pubkey::private_key<Scheme>>::type
            create_key(CoeffsIterator first,
                       CoeffsIterator last,
                       typename SecretSharingScheme::share_type share,
                       Number n) {
            BOOST_CONCEPT_ASSERT((boost::InputIteratorConcept<CoeffsIterator>));

            using privkey_type = pubkey::private_key<Scheme>;

            assert(static_cast<bool>(nil::crypto3::verify_share<SecretSharingScheme>(first, last, share)));
            return privkey_type(share);
        }

        //
        // PublicCoeffsRange - public representation values of polynomial's coefficients
        //
        template<typename Scheme, typename PublicCoeffsRange, typename Number,
                 typename SecretSharingScheme = typename pubkey::private_key<Scheme>::sss_public_key_group_type,
                 typename ValueType = typename std::iterator_traits<typename PublicCoeffsRange::iterator>::value_type,
                 typename SecretSharingScheme::template check_public_coeff_type<ValueType> = true>
        inline typename std::enable_if<
            std::is_same<pubkey::feldman_sss<typename SecretSharingScheme::group_type>, SecretSharingScheme>::value,
            pubkey::private_key<Scheme>>::type
            create_key(const PublicCoeffsRange &r, typename SecretSharingScheme::share_type share, Number n) {
            BOOST_RANGE_CONCEPT_ASSERT((boost::SinglePassRangeConcept<const PublicCoeffsRange>));
            return create_key<Scheme>(r.begin(), r.end(), share, n);
        }

        //
        // CoeffsIterator - coefficients of polynomial
        // InputIterator2 - participants' weights
        //
        template<typename Scheme, typename CoeffsIterator, typename WeightsIterator,
                 typename SecretSharingScheme = typename pubkey::private_key<Scheme>::sss_public_key_group_type,
                 typename ValueType1 = typename std::iterator_traits<CoeffsIterator>::value_type,
                 typename ValueType2 = typename std::iterator_traits<WeightsIterator>::value_type,
                 typename SecretSharingScheme::template check_coeff_type<ValueType1> = true,
                 typename SecretSharingScheme::template check_weight_type<ValueType2> = true>
        inline typename std::enable_if<
            std::is_same<pubkey::weighted_shamir_sss<typename SecretSharingScheme::group_type>,
                         SecretSharingScheme>::value,
            std::pair<pubkey::public_key<Scheme>, std::vector<pubkey::private_key<Scheme>>>>::type
            create_key(CoeffsIterator first1, CoeffsIterator last1, WeightsIterator first2, WeightsIterator last2) {
            BOOST_CONCEPT_ASSERT((boost::InputIteratorConcept<CoeffsIterator>));
            BOOST_CONCEPT_ASSERT((boost::InputIteratorConcept<WeightsIterator>));

            using privkeys_type = std::vector<pubkey::private_key<Scheme>>;
            using sss_no_key_ops_type = typename pubkey::private_key<Scheme>::sss_public_key_no_key_ops_type;

            typename sss_no_key_ops_type::shares_type shares = nil::crypto3::deal_shares<SecretSharingScheme>(
                first1, last1, first2, last2, std::distance(first2, last2));
            privkeys_type privkeys;
            for (const auto &s : shares) {
                privkeys.emplace_back(s, std::distance(first1, last1));
            }
            auto PK = pubkey::public_key<Scheme>(sss_no_key_ops_type::get_public_coeffs(first1, last1).front(),
                                                 std::distance(first2, last2));
            return std::make_pair(PK, privkeys);
        }

        //
        // CoeffsRange - coefficients of polynomial
        // WeightsRange - participants' weights
        //
        template<typename Scheme, typename CoeffsRange, typename WeightsRange,
                 typename SecretSharingScheme = typename pubkey::private_key<Scheme>::sss_public_key_group_type,
                 typename ValueType1 = typename std::iterator_traits<typename CoeffsRange::iterator>::value_type,
                 typename ValueType2 = typename std::iterator_traits<typename WeightsRange::iterator>::value_type,
                 typename SecretSharingScheme::template check_coeff_type<ValueType1> = true,
                 typename SecretSharingScheme::template check_weight_type<ValueType2> = true>
        inline typename std::enable_if<
            std::is_same<pubkey::weighted_shamir_sss<typename SecretSharingScheme::group_type>,
                         SecretSharingScheme>::value,
            std::pair<pubkey::public_key<Scheme>, std::vector<pubkey::private_key<Scheme>>>>::type
            create_key(const CoeffsRange &r1, const WeightsRange &r2) {
            BOOST_RANGE_CONCEPT_ASSERT((boost::SinglePassRangeConcept<const CoeffsRange>));
            BOOST_RANGE_CONCEPT_ASSERT((boost::SinglePassRangeConcept<const WeightsRange>));
            return create_key<Scheme>(r1.begin(), r1.end(), r2.begin(), r2.end());
        }

        //
        // PublicCoeffsIterators - public representation values of polynomials' coefficients of other participants
        // SharesIterator - shares generated by other participants
        //
        template<typename Scheme, typename PublicCoeffsIterators, typename SharesIterator, typename Number,
                 typename SecretSharingScheme = typename pubkey::private_key<Scheme>::sss_public_key_group_type,
                 typename ValueType1 = typename std::iterator_traits<
                     typename std::iterator_traits<PublicCoeffsIterators>::value_type::iterator>::value_type,
                 typename ValueType2 = typename std::iterator_traits<SharesIterator>::value_type,
                 typename SecretSharingScheme::template check_public_coeff_type<ValueType1> = true,
                 typename SecretSharingScheme::template check_share_type<ValueType2> = true>
        inline typename std::enable_if<
            std::is_same<pubkey::pedersen_dkg<typename SecretSharingScheme::group_type>, SecretSharingScheme>::value,
            std::pair<pubkey::public_key<Scheme>, pubkey::private_key<Scheme>>>::type
            create_key(PublicCoeffsIterators first1, PublicCoeffsIterators last1, SharesIterator first2,
                       SharesIterator last2, Number n) {
            BOOST_CONCEPT_ASSERT((boost::InputIteratorConcept<PublicCoeffsIterators>));
            BOOST_RANGE_CONCEPT_ASSERT((
                boost::SinglePassRangeConcept<const typename std::iterator_traits<PublicCoeffsIterators>::value_type>));
            BOOST_CONCEPT_ASSERT((boost::InputIteratorConcept<SharesIterator>));
            assert(n == std::distance(first1, last1));
            assert(n == std::distance(first2, last2));

            using privkey_type = pubkey::private_key<Scheme>;

            auto it_coeffs = first1;
            auto it_share = first2;
            typename SecretSharingScheme::public_element_type PK = SecretSharingScheme::public_element_type::zero();
            while (it_coeffs != last1 && it_share != last2) {
                assert(static_cast<bool>(nil::crypto3::verify_share<SecretSharingScheme>(*it_coeffs, *it_share)));
                PK = PK + *((*it_coeffs).begin());
                it_coeffs++;
                it_share++;
            }
            return std::make_pair(pubkey::public_key<Scheme>(PK),
                                  privkey_type(static_cast<typename SecretSharingScheme::share_type>(
                                      nil::crypto3::deal_share<SecretSharingScheme>(first2, last2))));
        }

        //
        // PublicCoeffsRanges - public representation values of polynomials' coefficients of other participants
        // SharesRange - shares generated by other participants
        //
        template<typename Scheme, typename PublicCoeffsRanges, typename SharesRange, typename Number,
                 typename SecretSharingScheme = typename pubkey::private_key<Scheme>::sss_public_key_group_type,
                 typename ValueType1 = typename std::iterator_traits<typename std::iterator_traits<
                     typename PublicCoeffsRanges::iterator>::value_type::iterator>::value_type,
                 typename ValueType2 = typename std::iterator_traits<typename SharesRange::iterator>::value_type,
                 typename SecretSharingScheme::template check_public_coeff_type<ValueType1> = true,
                 typename SecretSharingScheme::template check_share_type<ValueType2> = true>
        inline typename std::enable_if<
            std::is_same<pubkey::pedersen_dkg<typename SecretSharingScheme::group_type>, SecretSharingScheme>::value,
            std::pair<pubkey::public_key<Scheme>, pubkey::private_key<Scheme>>>::type
            create_key(const PublicCoeffsRanges &r, const SharesRange &shares, Number n) {
            BOOST_RANGE_CONCEPT_ASSERT((boost::SinglePassRangeConcept<const PublicCoeffsRanges>));
            BOOST_RANGE_CONCEPT_ASSERT(
                (boost::SinglePassRangeConcept<
                    const typename std::iterator_traits<typename PublicCoeffsRanges::iterator>::value_type>));
            BOOST_RANGE_CONCEPT_ASSERT((boost::SinglePassRangeConcept<const SharesRange>));
            return create_key<Scheme>(r.begin(), r.end(), shares.begin(), shares.end(), n);
        }
    }    // namespace crypto3
}    // namespace nil

#endif    // include guard


//  Created by jimmy on 2017-01-18.
//  Copyright (c) 2017 Nowhere Planet. All rights reserved.
//

#ifndef __btrf_tree__
#define __btrf_tree__

// backtracking decision tree
// a forest has multiple independent trees
#include <stdio.h>
#include <algorithm>
// flann
#include <flann/util/heap.h>
#include <flann/util/result_set.h>
#include <flann/flann.hpp>
// Eigen
#include <Eigen/Dense>

#include "btrf_util.hpp"
#include "dt_random.hpp"

using std::vector;
using Eigen::VectorXf;
using flann::BranchStruct;

class BTRFTreeNode;

class BTRFTree
{
private:
    friend class BTRFForest;

    typedef flann::L2<float> Distance;
    typedef Distance::ResultType DistanceType;
    typedef Distance::ElementType ElementType;

    typedef BTRFTreeNode Node;
    typedef BTRFTreeNode* NodePtr;
    typedef BranchStruct<NodePtr, DistanceType > BranchSt;
    typedef BranchSt* Branch;

    typedef SCRFRandomSample FeatureType;
    typedef BTRFTreeParameter TreeParameter;

    NodePtr root_;    // tree root
    TreeParameter tree_param_; // tree parameter

    Distance distance_;   // the distance functor
    int leaf_node_num_;   // total leaf node number
    vector<NodePtr> leaf_nodes_;   // leaf node for back tracking

    DTRandom rnd_generator_;   // random number generator

public:
    BTRFTree();
    ~BTRFTree();

    // build a decision tree using training examples
    // features: sampled image pixel locations
    // labels: 3D location
    // indices: index of samples
    // rgb_images: training RGB images, 8bit
    // param: tree parameter
    bool buildTree(const vector<FeatureType> & features,
                   const vector<VectorXf> & labels,
                   const vector<unsigned int> & indices,
                   const vector<cv::Mat> & rgb_images,
                   const TreeParameter & param);

    // predict 3D locaiton of a pixel in an image
    // feature: input, testing feature location
    // rgb_image: input, an RGB image
    // max_check: input, back tracking parameter, >= 1
    // pred: output predicted 3D location
    // dist: output local patch descriptor distance
    bool predict(const FeatureType & feature,
                 const cv::Mat & rgb_image,
                 const int max_check,
                 VectorXf & pred,
                 float & dist) const;

    // for model save/load, each row is a descriptor, row index is leaf node index
    void getLeafNodeDescriptor(Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> & data);
    void setLeafNodeDescriptor(const Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> & data);

    // tree parameters
    const TreeParameter & getTreeParameter(void) const {return tree_param_;}
    void setTreeParameter(const TreeParameter & param) {tree_param_ = param;}

private:
    // build tree implementation
    bool buildTreeImpl(const vector<FeatureType> & features,
                       const vector<VectorXf> & labels,
                       const vector<cv::Mat> & rgb_images,
                       const vector<unsigned int> & indices,
                       NodePtr node);

    // optimize split parameter
    // left_indices: example index in leaf child node, output
    // right_indices: example index in right child node, output
    // split_param: split parameter, output
    double optimizeRandomFeature(const vector<FeatureType> & features,
                                 const vector<VectorXf> & labels,
                                 const vector<cv::Mat> & rgbImages,
                                 const vector<unsigned int> & indices,
                                 const int depth,
                                 vector<unsigned int> & left_indices,
                                 vector<unsigned int> & right_indices,
                                 RandomSplitParameter & split_param) const;

    // optimize threshold of the random feature
    double optimizeThreshold(const vector<FeatureType> & features,
                             const vector<VectorXf> & labels,
                             const vector<cv::Mat> & rgbImages,
                             const vector<unsigned int> & indices,
                             const int depth,
                             RandomSplitParameter & split_param,
                             vector<unsigned int> & left_indices,
                             vector<unsigned int> & right_indices) const;
    // generate leaf node parameters
    bool setLeafNode(const vector<FeatureType> & features,
                     const vector<VectorXf> & labels,
                     const vector<unsigned int> & indices,
                     NodePtr node);

    // record leaf node point in an array for O(1) access
    void hashLeafNode();

    void recordLeafNodes(NodePtr node,
                         vector<NodePtr> & leafNodes,
                         int & leafnode_index);

    // priority search tree, this function is modifed from flann
    // result_set: search result
    // vec: local descriptor
    // node: current tree node
    // check_count: number of checked leaf node
    // max_check  : threshold of backtracking
    // heap: priority heap
    // sample: testing sample
    // rgb_image: testing image
    void searchLevel(flann::ResultSet<DistanceType>  & result_set,
                     const ElementType* vec,
                     const NodePtr node,
                     int & check_count,
                     const int max_check,
                     flann::Heap<BranchSt>* heap,
                     const FeatureType & sample,
                     const cv::Mat & rgb_image) const;

    // from CVPR2015 paper
    // "Exploiting uncertainty in regression forests for accurate camera relocalization"
    // depth adapted RGB pixel comparison feature
    // image: an RGB image
    // feat: feature location
    // split: offset and color channel
    // return: pixel comparision feature
    static double computeRandomFeature(const cv::Mat & rgb_image,
                                       const FeatureType * feat,
                                       const RandomSplitParameter & split);

};

#endif /* defined(__btrf_tree__) */


#ifndef PYTHONIC_INCLUDE_NUMPY_TAN_HPP
#define PYTHONIC_INCLUDE_NUMPY_TAN_HPP

#include "pythonic/include/utils/functor.hpp"
#include "pythonic/include/types/ndarray.hpp"
#include "pythonic/include/utils/numpy_traits.hpp"
#include <boost/simd/function/tan.hpp>

PYTHONIC_NS_BEGIN

namespace numpy
{
#define NUMPY_NARY_FUNC_NAME tan
#define NUMPY_NARY_FUNC_SYM boost::simd::tan
#include "pythonic/include/types/numpy_nary_expr.hpp"
}
PYTHONIC_NS_END

#endif


// This file is part of snark, a generic and flexible library for robotics research
// Copyright (c) 2011 The University of Sydney
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
// 1. Redistributions of source code must retain the above copyright
//    notice, this list of conditions and the following disclaimer.
// 2. 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.
// 3. Neither the name of the University of Sydney nor the
//    names of its contributors may be used to endorse or promote products
//    derived from this software without specific prior written permission.
//
// NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE
// GRANTED BY THIS LICENSE.  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 "convert.h"
#include "traits.h"

#include <comma/csv/stream.h>
#include <Eigen/Dense>

namespace {

    using namespace snark::imaging;

    typedef std::vector< double > channels;

    // conversion does not have to be a linear operation, but many are; these function simplify setting up such conversions
    /*
      commented-out to remove a compiler warning (-Wunused-function)
    channels linear_combination( const channels & i, const Eigen::Vector3d & before, const Eigen::Matrix3d & m, const Eigen::Vector3d & after )
    {
        channels t = { i[0] + before(0), i[1] + before(1), i[2] + before(2) };
        return channels( { m(0,0) * t[0] + m(0,1) * t[1] + m(0,2) * t[2] + after(0)
                         , m(1,0) * t[0] + m(1,1) * t[1] + m(1,2) * t[2] + after(1)
                         , m(2,0) * t[0] + m(2,1) * t[1] + m(2,2) * t[2] + after(2) } );
    }

    channels linear_combination( const channels & i, const Eigen::Vector3d & before, const Eigen::Matrix3d & m )
    {
        channels t = { i[0] + before(0), i[1] + before(1), i[2] + before(2) };
        return channels( { m(0,0) * t[0] + m(0,1) * t[1] + m(0,2) * t[2]
                         , m(1,0) * t[0] + m(1,1) * t[1] + m(1,2) * t[2]
                         , m(2,0) * t[0] + m(2,1) * t[1] + m(2,2) * t[2] } );
    }
    */
    channels linear_combination( const channels & i, const Eigen::Matrix3d & m, const Eigen::Vector3d & after )
    {
        return channels( { m(0,0) * i[0] + m(0,1) * i[1] + m(0,2) * i[2] + after(0)
                         , m(1,0) * i[0] + m(1,1) * i[1] + m(1,2) * i[2] + after(1)
                         , m(2,0) * i[0] + m(2,1) * i[1] + m(2,2) * i[2] + after(2) } );
    }

    channels linear_combination( const channels & i, const Eigen::Matrix3d & m )
    {
        return channels( { m(0,0) * i[0] + m(0,1) * i[1] + m(0,2) * i[2]
                         , m(1,0) * i[0] + m(1,1) * i[1] + m(1,2) * i[2]
                         , m(2,0) * i[0] + m(2,1) * i[1] + m(2,2) * i[2] } );
    }

    typedef std::function< channels ( const channels & p ) > C;
    // TODO:
    // - outr can be duplicate to the explicit convert call in convert (or the other way around); sort out
    typedef std::pair< colorspace::cspace, range > half_key_t;
    typedef std::pair< half_key_t, half_key_t > conversion_key_t;
    typedef std::map< conversion_key_t, C > conversion_map_t;

    C scale( range inr, range outr )
    {
        double uin, lin, uout, lout;
        switch ( inr ) {
            case ub : uin = limits< ub >::upper(); lin = limits< ub >::lower(); break;
            case uw : uin = limits< uw >::upper(); lin = limits< uw >::lower(); break;
            case ui : uin = limits< ui >::upper(); lin = limits< ui >::lower(); break;
            case f  : uin = limits< f  >::upper(); lin = limits< f  >::lower(); break;
            case d  : uin = limits< d  >::upper(); lin = limits< d  >::lower(); break;
            default:
                COMMA_THROW( comma::exception, "unknown input range '" << stringify::from( inr ) << "'" );
        }

        switch ( outr ) {
            case ub : uout = limits< ub >::upper(); lout = limits< ub >::lower(); break;
            case uw : uout = limits< uw >::upper(); lout = limits< uw >::lower(); break;
            case ui : uout = limits< ui >::upper(); lout = limits< ui >::lower(); break;
            case f  : uout = limits< f  >::upper(); lout = limits< f  >::lower(); break;
            case d  : uout = limits< d  >::upper(); lout = limits< d  >::lower(); break;
            default:
                COMMA_THROW( comma::exception, "unknown output range '" << stringify::from( outr ) << "'" );
        }

        double factor = ( uout - lout ) / ( uin - lin );
        return [ factor, lin, lout ]( const channels & rhs ){
            return channels( { factor * ( rhs[0] - lin ) + lout
                             , factor * ( rhs[1] - lin ) + lout
                             , factor * ( rhs[2] - lin ) + lout } ); };
    }

    const conversion_map_t & conversions()
    {
        static conversion_map_t m;
        m[ std::make_pair( std::make_pair( colorspace::rgb, ub ), std::make_pair( colorspace::rgb,   ub ) ) ] =
            []( const channels & i ){ return linear_combination( i, (Eigen::Matrix3d() << 1, 0, 0, 0, 1, 0, 0, 0, 1).finished() ); };
        m[ std::make_pair( std::make_pair( colorspace::rgb, f  ), std::make_pair( colorspace::ypbpr, f  ) ) ] =
            []( const channels & i ){ return linear_combination( i, (Eigen::Matrix3d() << 0.299, 0.587, 0.114, -0.168736, -0.331264, 0.5, 0.5, -0.418688, -0.081312).finished(), (Eigen::Vector3d() << 0, 0.5, 0.5).finished() ); };
        m[ std::make_pair( std::make_pair( colorspace::rgb, d  ), std::make_pair( colorspace::ypbpr, d  ) ) ] = m[ std::make_pair( std::make_pair( colorspace::rgb, f  ), std::make_pair( colorspace::ypbpr, f  ) ) ];
        m[ std::make_pair( std::make_pair( colorspace::rgb, f  ), std::make_pair( colorspace::ypbpr, d  ) ) ] = m[ std::make_pair( std::make_pair( colorspace::rgb, f  ), std::make_pair( colorspace::ypbpr, f  ) ) ];
        m[ std::make_pair( std::make_pair( colorspace::rgb, d  ), std::make_pair( colorspace::ypbpr, f  ) ) ] = m[ std::make_pair( std::make_pair( colorspace::rgb, f  ), std::make_pair( colorspace::ypbpr, f  ) ) ];
        m[ std::make_pair( std::make_pair( colorspace::rgb, f  ), std::make_pair( colorspace::ycbcr, ub ) ) ] =
            []( const channels & i ){ return linear_combination( i, (Eigen::Matrix3d() << 65.481, 128.553, 24.966, -37.797, -74.203, 112.0, 112.0, -93.786, -18.214).finished(), (Eigen::Vector3d() << 16, 128, 128).finished() ); };
        m[ std::make_pair( std::make_pair( colorspace::rgb, d  ), std::make_pair( colorspace::ycbcr, ub ) ) ] = m[ std::make_pair( std::make_pair( colorspace::rgb, f  ), std::make_pair( colorspace::ycbcr, ub ) ) ];
        // this differs from commonly presented values, e.g., wiki@YCbCr page, that rounds numbers to 3 decimal digits
        m[ std::make_pair( std::make_pair( colorspace::rgb, ub ), std::make_pair( colorspace::ycbcr, ub ) ) ] =
            []( const channels & i ){ return linear_combination( i, (Eigen::Matrix3d() << 65.481, 128.553, 24.966, -37.797, -74.203, 112.0, 112.0, -93.786, -18.214).finished() / 255., (Eigen::Vector3d() << 16, 128, 128).finished() ); };
        {
            m[ std::make_pair( std::make_pair( colorspace::ycbcr, ub ), std::make_pair( colorspace::rgb, ub ) ) ] =
                []( const channels & i) {
                    return channels( { 255/219. * ( i[0] - 16 )                                               + 255/112.*0.701             * ( i[2] - 128 )
                                     , 255/219. * ( i[0] - 16 ) - 255/112.*0.886*0.114/0.587 * ( i[1] - 128 ) - 255/112.*0.701*0.299/0.587 * ( i[2] - 128 )
                                     , 255/219. * ( i[0] - 16 ) + 255/112.*0.886             * ( i[1] - 128 )                                               } );
                };
        }
        return m;
    }

    C select( const conversion_map_t & m, const colorspace & inc, range inr, const colorspace & outc, range outr, bool recurse = true )
    {
        // the logic for searching for the conversion method:
        // 0. check for non-conversions: same colorspace, different (or even same) range; handle explicitly
        if ( inc.value == outc.value )
        {
            if ( inr == outr ) { return []( const channels & c ) { return c; }; }
            return scale( inr, outr );
        }
        // 1. if there is a direct conversion from (inc, inr) to (outc, outr) use it (data are stored as doubles on both in and out)
        {
            const conversion_key_t & key = std::make_pair( std::make_pair( inc.value, inr ), std::make_pair( outc.value, outr ) );
            conversion_map_t::const_iterator i = m.find( key );
            if ( i != m.end() ) { return i->second; }
        }
        // 2. if there is a conversion from (inc, inr) to (outc, default-range-of-outc) use it, then change range of outc (chain conversions)
        {
            range outdef = colorspace::default_range( outc.value );
            const conversion_key_t & key = std::make_pair( std::make_pair( inc.value, inr ), std::make_pair( outc.value, outdef ) );
            conversion_map_t::const_iterator i = m.find( key );
            if ( i != m.end() ) { return [ i, outdef, outr ]( const channels & c ){ return scale( outdef, outr )( i->second( c ) ); }; }
        }
        // 3. if there is a conversion from (inc, default-range-of-inc) to (outc, outr), use it after changing range of inc
        {
            range indef = colorspace::default_range( inc.value );
            const conversion_key_t & key = std::make_pair( std::make_pair( inc.value, indef ), std::make_pair( outc.value, outr ) );
            conversion_map_t::const_iterator i = m.find( key );
            if ( i != m.end() ) { return [ i, inr, indef ]( const channels & c ){ return i->second( scale( inr, indef )( c ) ); }; }
        }
        // 4. if there is a conversion from (inc, default-range-of-inc) to (outc, default-range-of-outc), change range of inc, apply conversion, then change range of outc
        {
            range indef = colorspace::default_range( inc.value );
            range outdef = colorspace::default_range( outc.value );
            const conversion_key_t & key = std::make_pair( std::make_pair( inc.value, indef ), std::make_pair( outc.value, outdef ) );
            conversion_map_t::const_iterator i = m.find( key );
            if ( i != m.end() ) { return [ i, inr, indef, outdef, outr ]( const channels & c ){ return scale( outdef, outr )( i->second( scale( inr, indef )( c ) ) ); }; }
        }
        // 5. if there is a conversion from (inc, inr ) to (rgb, some-range-of-rgb) and also conversion from (rgb, some-range-of-rgb) to (outc, outr)
        //    ( covering all possible intermediate conversions through a recursive call), convert through the intermediate rgb
        {
            if ( recurse )
            {
                for ( range r : { ub, f } )
                {
                    try {
                        // do not stuck in infinite recursion
                        C c0 = select( m, inc, inr, colorspace::rgb, r, false );
                        C c1 = select( m, colorspace::rgb, r, outc, outr, false );
                        return [ c0, c1 ]( const channels & c ){ return c1( c0( c ) ); };
                    }
                    catch ( comma::exception & )
                    {}
                }
            }
        }
        // 6. if all the above fails, throw
        COMMA_THROW( comma::exception, "conversion from colorspace " << inc << ", range " << stringify::from( inr ) << " to colorspace " << outc << ", range " << stringify::from( outr ) << " is not known" );
    }

    typedef std::function< void ( const comma::csv::options & csv, const C & c ) > F;

    template< colorspace::cspace inc, range inr, colorspace::cspace outc, range outr, typename outt >
    void convert( const comma::csv::options & csv, const C & c )
    {
        pixel< double, inr > sample_in;
        comma::csv::input_stream< pixel< double, inr > > is( std::cin, csv, sample_in );
        comma::csv::options output_csv;
        output_csv.full_xpath = false;
        output_csv.flush = csv.flush;
        if( csv.binary() ) { output_csv.format( comma::csv::format::value< pixel< outt, outr > >() ); }
        pixel< outt, outr > sample_out;
        comma::csv::output_stream< pixel< outt, outr > > os( std::cout, output_csv, sample_out );
        comma::csv::tied< pixel< double, inr >, pixel< outt, outr > > tied( is, os );
        pixel< double, outr > op;
        while( is.ready() || std::cin.good() )
        {
            const pixel< double, inr > * ip = is.read();
            if( !ip ) { break; }
            c( ip->channel ).swap( op.channel );
            tied.append( pixel< outt, outr >( op ) );
            if ( output_csv.flush ) { std::cout.flush(); }
        }
    }

    template< colorspace::cspace inc, range inr, colorspace::cspace outc, range outr, range outt >
    F resolve_( typename std::enable_if< std::is_integral< typename range_traits< outr >::value_t >::value, typename range_traits< outr >::value_t >::type outr_v
              , typename std::enable_if< std::is_integral< typename range_traits< outt >::value_t >::value, typename range_traits< outt >::value_t >::type outt_v )
    {
        if ( outr_v == ub && outt_v == ub ) { return convert< inc, inr, outc, ub, typename range_traits< ub >::value_t >; }
        if ( outr_v == ub && outt_v == uw ) { return convert< inc, inr, outc, ub, typename range_traits< uw >::value_t >; }
        if ( outr_v == ub && outt_v == ui ) { return convert< inc, inr, outc, ub, typename range_traits< ui >::value_t >; }
        if ( outr_v == uw && outt_v == uw ) { return convert< inc, inr, outc, uw, typename range_traits< uw >::value_t >; }
        if ( outr_v == uw && outt_v == ui ) { return convert< inc, inr, outc, uw, typename range_traits< ui >::value_t >; }
        if ( outr_v == ui && outt_v == ui ) { return convert< inc, inr, outc, ui, typename range_traits< ui >::value_t >; }
        COMMA_THROW( comma::exception, "unsupported combination of output range " << stringify::from( outr ) << " and type " << stringify::from( outt ) );
    }

    template< colorspace::cspace inc, range inr, colorspace::cspace outc, range outr, range outt >
    F resolve_( typename std::enable_if< std::is_floating_point< typename range_traits< outr >::value_t >::value, typename range_traits< outr >::value_t >::type outr_v
              , typename std::enable_if< std::is_integral< typename range_traits< outt >::value_t >::value, typename range_traits< outt >::value_t >::type outt_v )
    {
        COMMA_THROW( comma::exception, "cannot use integer output type " << stringify::from( outt ) << " for " << stringify::from( outr ) << " output range" );
    }

    template< colorspace::cspace inc, range inr, colorspace::cspace outc, range outr, range outt >
    F resolve_( typename std::enable_if< std::is_integral< typename range_traits< outr >::value_t >::value, typename range_traits< outr >::value_t >::type outr_v
              , typename std::enable_if< std::is_floating_point< typename range_traits< outt >::value_t >::value, typename range_traits< outt >::value_t >::type outt_v )
    {
        return convert< inc, inr, outc, outr, typename range_traits< outt >::value_t >;
    }

    template< colorspace::cspace inc, range inr, colorspace::cspace outc, range outr, range outt >
    F resolve_( typename std::enable_if< std::is_floating_point< typename range_traits< outr >::value_t >::value, typename range_traits< outr >::value_t >::type outr_v
              , typename std::enable_if< std::is_floating_point< typename range_traits< outt >::value_t >::value, typename range_traits< outt >::value_t >::type outt_v )
    {
        return convert< inc, inr, outc, outr, typename range_traits< outt >::value_t >;
    }

    template< colorspace::cspace inc, range inr, colorspace::cspace outc, range outr, range outt >
    struct outt_
    {
        static F dispatch()
        {
            // shall not attempt instantiating if:
            // - outr is integer, outt is integer and outr > outt
            // - outr is float, outt is integer
            typename range_traits< outr >::value_t outr_v( outr );
            typename range_traits< outt >::value_t outt_v( outt );
            return resolve_< inc, inr, outc, outr, outt >( outr_v, outt_v );
        }
    };

    template< colorspace::cspace inc, range inr, colorspace::cspace outc, range outr >
    struct outr_
    {
        static F dispatch( range outt )
        {
            switch ( outt )
            {
                case ub: return outt_< inc, inr, outc, outr, ub >::dispatch( ); break;
                case uw: return outt_< inc, inr, outc, outr, uw >::dispatch( ); break;
                case ui: return outt_< inc, inr, outc, outr, ui >::dispatch( ); break;
                case f:  return outt_< inc, inr, outc, outr, f  >::dispatch( ); break;
                case d:  return outt_< inc, inr, outc, outr, d  >::dispatch( ); break;
                default:
                    COMMA_THROW( comma::exception, "unknown output storage format '" << stringify::from( outt ) << "'" );
            }
        }
    };

    template< colorspace::cspace inc, range inr, colorspace::cspace outc >
    struct outc_
    {
        static F dispatch( range outr, range outt )
        {
            switch ( outr )
            {
                case ub: return outr_< inc, inr, outc, ub >::dispatch( outt ); break;
                case uw: return outr_< inc, inr, outc, uw >::dispatch( outt ); break;
                case ui: return outr_< inc, inr, outc, ui >::dispatch( outt ); break;
                case f:  return outr_< inc, inr, outc, f  >::dispatch( outt ); break;
                case d:  return outr_< inc, inr, outc, d  >::dispatch( outt ); break;
                default:
                    COMMA_THROW( comma::exception, "unknown output range '" << stringify::from( outr ) << "'" );
            }
        }
    };

    template< colorspace::cspace inc, range inr >
    struct inr_
    {
        static F dispatch( const colorspace & outc, range outr, range outt )
        {
            switch ( outc.value )
            {
                case colorspace::rgb:   return outc_< inc, inr, colorspace::rgb   >::dispatch( outr, outt ); break;
                case colorspace::ypbpr: return outc_< inc, inr, colorspace::ypbpr >::dispatch( outr, outt ); break;
                case colorspace::ycbcr: return outc_< inc, inr, colorspace::ycbcr >::dispatch( outr, outt ); break;
                default:
                    COMMA_THROW( comma::exception, "unknown colorspace to '" << std::string( outc ) << "'" );
            }
        }
    };

    template< colorspace::cspace inc >
    struct inc_
    {
        static F dispatch( range inr, const colorspace & outc, range outr, range outt )
        {
            switch ( inr )
            {
                case ub: return inr_< inc, ub >::dispatch( outc, outr, outt ); break;
                case uw: return inr_< inc, uw >::dispatch( outc, outr, outt ); break;
                case ui: return inr_< inc, ui >::dispatch( outc, outr, outt ); break;
                case f:  return inr_< inc, f  >::dispatch( outc, outr, outt ); break;
                case d:  return inr_< inc, d  >::dispatch( outc, outr, outt ); break;
                default:
                    COMMA_THROW( comma::exception, "unknown input range '" << stringify::from( inr ) << "'" );
            }
        }
    };

} // anonymous

namespace snark { namespace imaging {

    converter::F converter::dispatch( const colorspace & inc, range inr, const colorspace & outc, range outr, range outt )
    {
        const C & conv = select( conversions(), inc, inr, outc, outr );

        switch ( inc.value )
        {
            case colorspace::rgb:   return std::bind( inc_< colorspace::rgb   >::dispatch( inr, outc, outr, outt ), std::placeholders::_1, conv ); break;
            case colorspace::ypbpr: return std::bind( inc_< colorspace::ypbpr >::dispatch( inr, outc, outr, outt ), std::placeholders::_1, conv ); break;
            case colorspace::ycbcr: return std::bind( inc_< colorspace::ycbcr >::dispatch( inr, outc, outr, outt ), std::placeholders::_1, conv ); break;
            default:
                COMMA_THROW( comma::exception, "unknown colorspace from '" << std::string( inc ) << "'" );
        }
    }

    void converter::list( std::ostream & os )
    {
        const auto & allc = { colorspace( colorspace::rgb ), colorspace( colorspace::ycbcr ), colorspace( colorspace::ypbpr ) };
        const auto & allr = { ub, uw, ui, f, d };
        for ( const auto & inc : allc )
        {
            for ( auto inr : allr )
            {
                for ( const auto & outc : allc )
                {
                    for ( auto outr : allr )
                    {
                        try
                        {
                            select( conversions(), inc, inr, outc, outr );
                            os << inc << ',' << stringify::from( inr ) << ',' << outc << ',' << stringify::from( outr ) << std::endl;
                        }
                        catch ( comma::exception & )
                        {}
                    }
                }
            }
        }
    }

} } // namespace snark { namespace imaging {


/*    Copyright (c) 2010-2018, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 */

#include <boost/make_shared.hpp>
#include <memory>
#include <boost/bind.hpp>

#include "Tudat/Astrodynamics/Aerodynamics/aerodynamics.h"
#include "Tudat/Astrodynamics/Aerodynamics/flightConditions.h"
#include "Tudat/Astrodynamics/Aerodynamics/standardAtmosphere.h"
#include "Tudat/Astrodynamics/BasicAstrodynamics/oblateSpheroidBodyShapeModel.h"
#include "Tudat/Mathematics/BasicMathematics/mathematicalConstants.h"

namespace tudat
{

namespace aerodynamics
{

//! Constructor, sets objects and functions from which relevant environment and state variables are retrieved.
FlightConditions::FlightConditions( const std::shared_ptr< basic_astrodynamics::BodyShapeModel > shapeModel,
                  const std::shared_ptr< reference_frames::AerodynamicAngleCalculator >
                  aerodynamicAngleCalculator ):
    shapeModel_( shapeModel ),
    aerodynamicAngleCalculator_( aerodynamicAngleCalculator ),
    currentTime_( TUDAT_NAN )
{
    // Link body-state function.
    bodyCenteredPseudoBodyFixedStateFunction_ = std::bind(
                &reference_frames::AerodynamicAngleCalculator::getCurrentAirspeedBasedBodyFixedState, aerodynamicAngleCalculator_ );

    // Check if given body shape is an oblate spheroid and set geodetic latitude function if so
    if( std::dynamic_pointer_cast< basic_astrodynamics::OblateSpheroidBodyShapeModel >( shapeModel ) != nullptr )
    {
        geodeticLatitudeFunction_ = std::bind(
                    &basic_astrodynamics::OblateSpheroidBodyShapeModel::getGeodeticLatitude,
                    std::dynamic_pointer_cast< basic_astrodynamics::OblateSpheroidBodyShapeModel >( shapeModel ),
                    std::placeholders::_1, 1.0E-4 );
    }
}

//! Function to update all flight conditions.
void FlightConditions::updateConditions( const double currentTime )
{
    if( !( currentTime == currentTime_ ) )
    {
        currentTime_ = currentTime;

        // Update aerodynamic angles (but not angles w.r.t. body-fixed frame).
        if( aerodynamicAngleCalculator_!= nullptr )
        {
            aerodynamicAngleCalculator_->update( currentTime, false );
        }

        // Calculate state of vehicle in global frame and corotating frame.
        currentBodyCenteredAirspeedBasedBodyFixedState_ = bodyCenteredPseudoBodyFixedStateFunction_( );
    }
}

//! Constructor, sets objects and functions from which relevant environment and state variables are retrieved.
AtmosphericFlightConditions::AtmosphericFlightConditions(
        const std::shared_ptr< aerodynamics::AtmosphereModel > atmosphereModel,
        const std::shared_ptr< basic_astrodynamics::BodyShapeModel > shapeModel,
        const std::shared_ptr< AerodynamicCoefficientInterface > aerodynamicCoefficientInterface,
        const std::shared_ptr< reference_frames::AerodynamicAngleCalculator > aerodynamicAngleCalculator,
        const std::function< double( const std::string& ) > controlSurfaceDeflectionFunction ):
    FlightConditions( shapeModel, aerodynamicAngleCalculator ),
    atmosphereModel_( atmosphereModel ),
    aerodynamicCoefficientInterface_( aerodynamicCoefficientInterface ),
    controlSurfaceDeflectionFunction_( controlSurfaceDeflectionFunction )
{
    // Check if atmosphere requires latitude and longitude update.
    if( std::dynamic_pointer_cast< aerodynamics::StandardAtmosphere >( atmosphereModel_ ) == nullptr )
    {
        updateLatitudeAndLongitudeForAtmosphere_ = 1;
    }
    else
    {
        updateLatitudeAndLongitudeForAtmosphere_ = 0;
    }
    isLatitudeAndLongitudeSet_ = 0;

    if( updateLatitudeAndLongitudeForAtmosphere_ && aerodynamicAngleCalculator_== nullptr )
    {
        throw std::runtime_error( "Error when making flight conditions, angles are to be updated, but no calculator is set" );
    }
}

//! Function to set custom dependency of aerodynamic coefficients
void AtmosphericFlightConditions::setAerodynamicCoefficientsIndependentVariableFunction(
        const AerodynamicCoefficientsIndependentVariables independentVariable,
        const std::function< double( ) > coefficientDependency )
{
    if( ( independentVariable == mach_number_dependent ) ||
            ( independentVariable == angle_of_attack_dependent ) ||
            ( independentVariable == angle_of_sideslip_dependent )||
            ( independentVariable == altitude_dependent ) )
    {
        throw std::runtime_error(
                    std::string( "Error when setting aerodynamic coefficient function dependency, value of parameter " ) +
                    std::to_string( independentVariable ) +
                    std::string(", will not  be used." ) );
    }
    else
    {
        customCoefficientDependencies_[ independentVariable ] = coefficientDependency;
    }
}

//! Function to update all flight conditions.
void AtmosphericFlightConditions::updateConditions( const double currentTime )
{
    if( !( currentTime == currentTime_ ) )
    {
        currentTime_ = currentTime;

        // Update aerodynamic angles (but not angles w.r.t. body-fixed frame).
        if( aerodynamicAngleCalculator_!= nullptr )
        {
            aerodynamicAngleCalculator_->update( currentTime, false );
        }

        // Calculate state of vehicle in global frame and corotating frame.
        currentBodyCenteredAirspeedBasedBodyFixedState_ = bodyCenteredPseudoBodyFixedStateFunction_( );

        updateAerodynamicCoefficientInput( );

        // Update angles from aerodynamic to body-fixed frame (if relevant).
        if( aerodynamicAngleCalculator_!= nullptr )
        {
            aerodynamicAngleCalculator_->update( currentTime, true );
            updateAerodynamicCoefficientInput( );
        }

        // Update aerodynamic coefficients.
        aerodynamicCoefficientInterface_->updateFullCurrentCoefficients(
                    aerodynamicCoefficientIndependentVariables_, controlSurfaceAerodynamicCoefficientIndependentVariables_,
                    currentTime_ );
    }
}

//! Function to (compute and) retrieve the value of an independent variable of aerodynamic coefficients
double AtmosphericFlightConditions::getAerodynamicCoefficientIndependentVariable(
        const AerodynamicCoefficientsIndependentVariables independentVariableType,
        const std::string& secondaryIdentifier )
{
    double currentIndependentVariable;
    switch( independentVariableType )
    {
    //Calculate Mach number if needed.
    case mach_number_dependent:
        currentIndependentVariable = getCurrentMachNumber( );
        break;
        //Get angle of attack if needed.
    case angle_of_attack_dependent:
        if( aerodynamicAngleCalculator_== nullptr )
        {
            throw std::runtime_error( "Error, aerodynamic angle calculator is nullptr, but require angle of attack" );
        }
        currentIndependentVariable = aerodynamicAngleCalculator_->getAerodynamicAngle(
                    reference_frames::angle_of_attack );
        break;
        //Get angle of sideslip if needed.
    case angle_of_sideslip_dependent:
        if( aerodynamicAngleCalculator_== nullptr )
        {
            throw std::runtime_error( "Error, aerodynamic angle calculator is nullptr, but require angle of sideslip" );
        }
        currentIndependentVariable = aerodynamicAngleCalculator_->getAerodynamicAngle(
                    reference_frames::angle_of_sideslip );
        break;
    case altitude_dependent:
        currentIndependentVariable = getCurrentAltitude( );
        break;
    case control_surface_deflection_dependent:
    {
        try
        {
            currentIndependentVariable = controlSurfaceDeflectionFunction_( secondaryIdentifier );
        }
        catch( std::runtime_error )
        {
            throw std::runtime_error( "Error, control surface " + secondaryIdentifier + "not recognized when updating coefficients" );
        }
        break;
    }
    default:
        if( customCoefficientDependencies_.count( independentVariableType ) == 0 )
        {
            throw std::runtime_error( "Error, did not recognize aerodynamic coefficient dependency "
                                      + std::to_string( independentVariableType ) );
        }
        else
        {
            currentIndependentVariable = customCoefficientDependencies_.at( independentVariableType )( );
        }
    }

    return currentIndependentVariable;
}

//! Function to update the independent variables of the aerodynamic coefficient interface
void AtmosphericFlightConditions::updateAerodynamicCoefficientInput( )
{
    aerodynamicCoefficientIndependentVariables_.clear( );
    // Calculate independent variables for aerodynamic coefficients.
    for( unsigned int i = 0; i < aerodynamicCoefficientInterface_->getNumberOfIndependentVariables( ); i++ )
    {
        aerodynamicCoefficientIndependentVariables_.push_back(
                    getAerodynamicCoefficientIndependentVariable(
                        aerodynamicCoefficientInterface_->getIndependentVariableName( i ) ) );
    }

    controlSurfaceAerodynamicCoefficientIndependentVariables_.clear( );
    for( unsigned int i = 0; i < aerodynamicCoefficientInterface_->getNumberOfControlSurfaces( ); i++ )
    {
        std::string currentControlSurface = aerodynamicCoefficientInterface_->getControlSurfaceName( i );
        for( unsigned int j = 0; j < aerodynamicCoefficientInterface_->getNumberOfControlSurfaceIndependentVariables( currentControlSurface ); j++ )
        {
            controlSurfaceAerodynamicCoefficientIndependentVariables_[ currentControlSurface ].push_back(
                        getAerodynamicCoefficientIndependentVariable(
                            aerodynamicCoefficientInterface_->getControlSurfaceIndependentVariableName(
                                currentControlSurface, j ), currentControlSurface ) );
        }
    }
}

//! Function to set the angle of attack to trimmed conditions.
std::shared_ptr< TrimOrientationCalculator > setTrimmedConditions(
        const std::shared_ptr< AtmosphericFlightConditions > flightConditions )
{
    // Create trim object.
    std::shared_ptr< TrimOrientationCalculator > trimOrientation =
            std::make_shared< TrimOrientationCalculator >(
                flightConditions->getAerodynamicCoefficientInterface( ) );

    // Create angle-of-attack function from trim object.
    std::function< std::vector< double >( ) > untrimmedIndependentVariablesFunction =
            std::bind( &AtmosphericFlightConditions::getAerodynamicCoefficientIndependentVariables,
                         flightConditions );
    std::function< std::map< std::string, std::vector< double > >( ) > untrimmedControlSurfaceIndependentVariablesFunction =
            std::bind( &AtmosphericFlightConditions::getControlSurfaceAerodynamicCoefficientIndependentVariables,
                         flightConditions );
    flightConditions->getAerodynamicAngleCalculator( )->setOrientationAngleFunctions(
                std::bind( &TrimOrientationCalculator::findTrimAngleOfAttackFromFunction, trimOrientation,
                             untrimmedIndependentVariablesFunction, untrimmedControlSurfaceIndependentVariablesFunction ) );

    return trimOrientation;
}

} // namespace aerodynamics

} // namespace tudat