#pragma once

#include <cmath>
#include <cstdint>
#include <iostream>
#include <stdexcept>
#include <string>
#include <vector>

#include <Eigen/Geometry>
#include <common_robotics_utilities/maybe.hpp>
#include <common_robotics_utilities/serialization.hpp>
#include <common_robotics_utilities/utility.hpp>

namespace common_robotics_utilities
{
namespace voxel_grid
{
class GridIndex
{
private:
  int64_t x_ = -1;
  int64_t y_ = -1;
  int64_t z_ = -1;

public:
  GridIndex() : x_(-1), y_(-1), z_(-1) {}

  GridIndex(const int64_t x, const int64_t y, const int64_t z)
      : x_(x), y_(y), z_(z) {}

  const int64_t& X() const { return x_; }

  const int64_t& Y() const { return y_; }

  const int64_t& Z() const { return z_; }

  int64_t& X() { return x_; }

  int64_t& Y() { return y_; }

  int64_t& Z() { return z_; }

  bool operator==(const GridIndex& other) const
  {
    return (X() == other.X() && Y() == other.Y() && Z() == other.Z());
  }

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

class GridSizes
{
private:
  double cell_x_size_ = 0.0;
  double cell_y_size_ = 0.0;
  double cell_z_size_ = 0.0;
  double inv_cell_x_size_ = 0.0;
  double inv_cell_y_size_ = 0.0;
  double inv_cell_z_size_ = 0.0;
  double x_size_ = 0.0;
  double y_size_ = 0.0;
  double z_size_ = 0.0;
  int64_t num_x_cells_ = 0;
  int64_t num_y_cells_ = 0;
  int64_t num_z_cells_ = 0;
  int64_t stride1_ = 0;
  int64_t stride2_ = 0;
  bool valid_ = false;

  static void CheckPositiveValid(const double param)
  {
    if (param <= 0.0)
    {
      throw std::invalid_argument("param must be positive and non-zero");
    }
    if (std::isnan(param))
    {
      throw std::invalid_argument("param must not be NaN");
    }
    if (std::isinf(param) != 0)
    {
      throw std::invalid_argument("param must not be INF");
    }
  }

  static void CheckPositiveValid(const int64_t param)
  {
    if (param <= 0)
    {
      throw std::invalid_argument("param must be positive and non-zero");
    }
  }

public:
  static uint64_t Serialize(
      const GridSizes& sizes, std::vector<uint8_t>& buffer)
  {
    return sizes.SerializeSelf(buffer);
  }

  static serialization::Deserialized<GridSizes> Deserialize(
      const std::vector<uint8_t>& buffer, const uint64_t starting_offset)
  {
    GridSizes temp_sizes;
    const uint64_t bytes_read
        = temp_sizes.DeserializeSelf(buffer, starting_offset);
    return serialization::MakeDeserialized(temp_sizes, bytes_read);
  }

  GridSizes(const double cell_size,
            const double x_size,
            const double y_size,
            const double z_size)
      : GridSizes(cell_size, cell_size, cell_size, x_size, y_size, z_size) {}

  GridSizes(const double cell_size,
            const int64_t num_x_cells,
            const int64_t num_y_cells,
            const int64_t num_z_cells)
      : GridSizes(cell_size, cell_size, cell_size,
                  num_x_cells, num_y_cells, num_z_cells) {}

  GridSizes(const double cell_x_size,
            const double cell_y_size,
            const double cell_z_size,
            const double x_size,
            const double y_size,
            const double z_size)
  {
    // Safety check
    CheckPositiveValid(cell_x_size);
    CheckPositiveValid(cell_y_size);
    CheckPositiveValid(cell_z_size);
    CheckPositiveValid(x_size);
    CheckPositiveValid(y_size);
    CheckPositiveValid(z_size);
    // Set
    cell_x_size_ = cell_x_size;
    cell_y_size_ = cell_y_size;
    cell_z_size_ = cell_z_size;
    inv_cell_x_size_ = 1.0 / cell_x_size_;
    inv_cell_y_size_ = 1.0 / cell_y_size_;
    inv_cell_z_size_ = 1.0 / cell_z_size_;
    num_x_cells_
        = static_cast<int64_t>(std::ceil(x_size / cell_x_size));
    num_y_cells_
        = static_cast<int64_t>(std::ceil(y_size / cell_y_size));
    num_z_cells_
        = static_cast<int64_t>(std::ceil(z_size / cell_z_size));
    x_size_ = static_cast<double>(num_x_cells_) * cell_x_size_;
    y_size_ = static_cast<double>(num_y_cells_) * cell_y_size_;
    z_size_ = static_cast<double>(num_z_cells_) * cell_z_size_;
    stride1_ = num_y_cells_ * num_z_cells_;
    stride2_ = num_z_cells_;
    valid_ = true;
  }

  GridSizes(const double cell_x_size,
            const double cell_y_size,
            const double cell_z_size,
            const int64_t num_x_cells,
            const int64_t num_y_cells,
            const int64_t num_z_cells)
  {
    // Safety check
    CheckPositiveValid(cell_x_size);
    CheckPositiveValid(cell_y_size);
    CheckPositiveValid(cell_z_size);
    CheckPositiveValid(num_x_cells);
    CheckPositiveValid(num_y_cells);
    CheckPositiveValid(num_z_cells);
    // Set
    cell_x_size_ = cell_x_size;
    cell_y_size_ = cell_y_size;
    cell_z_size_ = cell_z_size;
    inv_cell_x_size_ = 1.0 / cell_x_size_;
    inv_cell_y_size_ = 1.0 / cell_y_size_;
    inv_cell_z_size_ = 1.0 / cell_z_size_;
    num_x_cells_ = num_x_cells;
    num_y_cells_ = num_y_cells;
    num_z_cells_ = num_z_cells;
    x_size_ = static_cast<double>(num_x_cells_) * cell_x_size_;
    y_size_ = static_cast<double>(num_y_cells_) * cell_y_size_;
    z_size_ = static_cast<double>(num_z_cells_) * cell_z_size_;
    stride1_ = num_y_cells_ * num_z_cells_;
    stride2_ = num_z_cells_;
    valid_ = true;
  }

  GridSizes()
  {
    cell_x_size_ = 0.0;
    cell_y_size_ = 0.0;
    cell_z_size_ = 0.0;
    inv_cell_x_size_ = 0.0;
    inv_cell_y_size_ = 0.0;
    inv_cell_z_size_ = 0.0;
    num_x_cells_ = 0;
    num_y_cells_ = 0;
    num_z_cells_ = 0;
    x_size_ = 0.0;
    y_size_ = 0.0;
    z_size_ = 0.0;
    stride1_ = 0.0;
    stride2_ = 0.0;
    valid_ = false;
  }

  bool Valid() const { return valid_; }

  bool UniformCellSize() const
  {
    return ((cell_x_size_ == cell_y_size_) && (cell_x_size_ == cell_z_size_));
  }

  uint64_t SerializeSelf(std::vector<uint8_t>& buffer) const
  {
    const uint64_t start_buffer_size = buffer.size();
    // Serialize everything needed to reproduce the grid sizes
    serialization::SerializeMemcpyable<double>(CellXSize(), buffer);
    serialization::SerializeMemcpyable<double>(CellYSize(), buffer);
    serialization::SerializeMemcpyable<double>(CellZSize(), buffer);
    serialization::SerializeMemcpyable<int64_t>(NumXCells(), buffer);
    serialization::SerializeMemcpyable<int64_t>(NumYCells(), buffer);
    serialization::SerializeMemcpyable<int64_t>(NumZCells(), buffer);
    // Figure out how many bytes were written
    const uint64_t end_buffer_size = buffer.size();
    const uint64_t bytes_written = end_buffer_size - start_buffer_size;
    return bytes_written;
  }

  uint64_t DeserializeSelf(const std::vector<uint8_t>& buffer,
                           const uint64_t starting_offset)
  {
    uint64_t current_position = starting_offset;
    const auto cell_x_size_deserialized
        = serialization::DeserializeMemcpyable<double>(buffer,
                                                       current_position);
    const double cell_x_size = cell_x_size_deserialized.Value();
    current_position += cell_x_size_deserialized.BytesRead();
    const auto cell_y_size_deserialized
        = serialization::DeserializeMemcpyable<double>(buffer,
                                                       current_position);
    const double cell_y_size = cell_y_size_deserialized.Value();
    current_position += cell_y_size_deserialized.BytesRead();
    const auto cell_z_size_deserialized
        = serialization::DeserializeMemcpyable<double>(buffer,
                                                       current_position);
    const double cell_z_size = cell_z_size_deserialized.Value();
    current_position += cell_z_size_deserialized.BytesRead();
    const auto num_x_cells_deserialized
        = serialization::DeserializeMemcpyable<int64_t>(buffer,
                                                        current_position);
    const int64_t num_x_cells = num_x_cells_deserialized.Value();
    current_position += num_x_cells_deserialized.BytesRead();
    const auto num_y_cells_deserialized
        = serialization::DeserializeMemcpyable<int64_t>(buffer,
                                                        current_position);
    const int64_t num_y_cells = num_y_cells_deserialized.Value();
    current_position += num_y_cells_deserialized.BytesRead();
    const auto num_z_cells_deserialized
        = serialization::DeserializeMemcpyable<int64_t>(buffer,
                                                        current_position);
    const int64_t num_z_cells = num_z_cells_deserialized.Value();
    current_position += num_z_cells_deserialized.BytesRead();
    try
    {
      *this = GridSizes(cell_x_size, cell_y_size, cell_z_size,
                        num_x_cells, num_y_cells, num_z_cells);
    }
    catch (const std::invalid_argument&)
    {
      // The only reason the constructor can fail is if the params are
      // default-initialized or invalid. We make sure to zero and set invalid.
      *this = GridSizes();
    }
    // Figure out how many bytes were read
    const uint64_t bytes_read = current_position - starting_offset;
    return bytes_read;
  }

  double CellXSize() const { return cell_x_size_; }

  double CellYSize() const { return cell_y_size_; }

  double CellZSize() const { return cell_z_size_; }

  Eigen::Vector3d CellSizes() const
  {
    return Eigen::Vector3d(cell_x_size_, cell_y_size_, cell_z_size_);
  }

  double InvCellXSize() const { return inv_cell_x_size_; }

  double InvCellYSize() const { return inv_cell_y_size_; }

  double InvCellZSize() const { return inv_cell_z_size_; }

  int64_t NumXCells() const { return num_x_cells_; }

  int64_t NumYCells() const { return num_y_cells_; }

  int64_t NumZCells() const { return num_z_cells_; }

  Eigen::Matrix<int64_t, 3, 1> NumCells() const
  {
    Eigen::Matrix<int64_t, 3, 1> num_cells;
    num_cells << num_x_cells_, num_y_cells_, num_z_cells_;
    return num_cells;
  }

  int64_t TotalCells() const
  {
    return num_x_cells_ * num_y_cells_ * num_z_cells_;
  }

  double XSize() const { return x_size_; }

  double YSize() const { return y_size_; }

  double ZSize() const { return z_size_; }

  Eigen::Vector3d Sizes() const
  {
    return Eigen::Vector3d(x_size_, y_size_, z_size_);
  }

  int64_t Stride1() const { return stride1_; }

  int64_t Stride2() const { return stride2_; }

  bool IndexInBounds(const int64_t x_index,
                     const int64_t y_index,
                     const int64_t z_index) const
  {
    if (x_index >= 0 && y_index >= 0 && z_index >= 0
        && x_index < NumXCells() && y_index < NumYCells()
        && z_index < NumZCells())
    {
      return true;
    }
    else
    {
      return false;
    }
  }

  bool IndexInBounds(const GridIndex& index) const
  {
    if (index.X() >= 0 && index.Y() >= 0 && index.Z() >= 0
        && index.X() < NumXCells() && index.Y() < NumYCells()
        && index.Z() < NumZCells())
    {
      return true;
    }
    else
    {
      return false;
    }
  }

  int64_t GetDataIndex(const int64_t x_index,
                       const int64_t y_index,
                       const int64_t z_index) const
  {
    return (x_index * Stride1()) + (y_index * Stride2()) + z_index;
  }

  int64_t GetDataIndex(const GridIndex& index) const
  {
    return (index.X() * Stride1()) + (index.Y() * Stride2()) + index.Z();
  }

  GridIndex GetGridIndexFromDataIndex(const int64_t data_index) const
  {
    const int64_t x_idx = data_index / Stride1();
    const int64_t remainder = data_index % Stride1();
    const int64_t y_idx = remainder / Stride2();
    const int64_t z_idx = remainder % Stride2();
    return GridIndex(x_idx, y_idx, z_idx);
  }

  Eigen::Vector4d IndexToLocationInGridFrame(const GridIndex& index) const
  {
    return IndexToLocationInGridFrame(index.X(), index.Y(), index.Z());
  }

  Eigen::Vector4d IndexToLocationInGridFrame(const int64_t x_index,
                                             const int64_t y_index,
                                             const int64_t z_index) const
  {
    const Eigen::Vector4d point_in_grid_frame(
          CellXSize() * (static_cast<double>(x_index) + 0.5),
          CellYSize() * (static_cast<double>(y_index) + 0.5),
          CellZSize() * (static_cast<double>(z_index) + 0.5),
          1.0);
    return point_in_grid_frame;
  }

  bool operator==(const GridSizes& other) const
  {
    return (NumXCells() == other.NumXCells()
            && NumYCells() == other.NumYCells()
            && NumZCells() == other.NumZCells()
            && CellXSize() == other.CellXSize()
            && CellYSize() == other.CellYSize()
            && CellZSize() == other.CellZSize());
  }

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

// Forward-declare for use in GridQuery.
template<typename T, typename BackingStore=std::vector<T>>
class VoxelGridBase;

// While this looks like a std::optional<T>, it *does not own* the item of T,
// unlike std::optional<T>, since it needs to pass the caller a const/mutable
// reference to the item in the voxel grid.
template<typename T>
class GridQuery : public ReferencingMaybe<T>
{
private:
  template<typename Item, typename BackingStore> friend class VoxelGridBase;

  // This constructor is private because users should not be able to create
  // GridQuery<T> with a value on their own, creation should only be possible
  // within a VoxelGridBase<T> to which the GridQuery<T> references.
  explicit GridQuery(T& item) : ReferencingMaybe<T>(item) {}

public:
  GridQuery() : ReferencingMaybe<T>() {}
};

/// This is the base class for all voxel grid classes. It is pure virtual to
/// force the implementation of certain necessary functions (cloning, access,
/// derived-class memeber de/serialization) in concrete implementations. This is
/// the class to inherit from if you want a VoxelGrid-like type. If all you want
/// is a voxel grid of T, see VoxelGrid<T, BackingStore> below.
template<typename T, typename BackingStore>
class VoxelGridBase
{
private:
  Eigen::Isometry3d origin_transform_ = Eigen::Isometry3d::Identity();
  Eigen::Isometry3d inverse_origin_transform_ = Eigen::Isometry3d::Identity();
  T default_value_;
  T oob_value_;
  BackingStore data_;
  GridSizes sizes_;
  bool initialized_ = false;

  T& AccessIndex(const int64_t& data_index)
  {
    if ((data_index >= 0) && (data_index < static_cast<int64_t>(data_.size())))
    {
      // Note: do not refactor to use .at(), since not all vector-like
      // implementations implement it (ex thrust::host_vector<T>).
      return data_[data_index];
    }
    else
    {
      throw std::out_of_range("data_index out of range");
    }
  }

  const T& AccessIndex(const int64_t& data_index) const
  {
    if ((data_index >= 0) && (data_index < static_cast<int64_t>(data_.size())))
    {
      // Note: do not refactor to use .at(), since not all vector-like
      // implementations implement it (ex thrust::host_vector<T>).
      return data_[data_index];
    }
    else
    {
      throw std::out_of_range("data_index out of range");
    }
  }

  void SetContents(const T& value)
  {
    data_.clear();
    data_.resize(sizes_.TotalCells(), value);
  }

  uint64_t BaseSerializeSelf(
      std::vector<uint8_t>& buffer,
      const serialization::Serializer<T>& value_serializer) const
  {
    const uint64_t start_buffer_size = buffer.size();
    // Serialize the transform
    serialization::SerializeIsometry3d(origin_transform_, buffer);
    // Serialize the default value
    value_serializer(default_value_, buffer);
    // Serialize the OOB value
    value_serializer(oob_value_, buffer);
    // Serialize the data
    serialization::SerializeVectorLike<T, BackingStore>(
          data_, buffer, value_serializer);
    // Serialize the grid sizes
    GridSizes::Serialize(sizes_, buffer);
    // Serialize the initialized
    serialization::SerializeMemcpyable<uint8_t>(
        static_cast<uint8_t>(initialized_), buffer);
    // Figure out how many bytes were written
    const uint64_t end_buffer_size = buffer.size();
    const uint64_t bytes_written = end_buffer_size - start_buffer_size;
    return bytes_written;
  }

  uint64_t BaseDeserializeSelf(
      const std::vector<uint8_t>& buffer, const uint64_t starting_offset,
      const serialization::Deserializer<T>& value_deserializer)
  {
    uint64_t current_position = starting_offset;
    // Deserialize the transforms
    const auto origin_transform_deserialized
        = serialization::DeserializeIsometry3d(buffer, current_position);
    origin_transform_ = origin_transform_deserialized.Value();
    current_position += origin_transform_deserialized.BytesRead();
    inverse_origin_transform_ = origin_transform_.inverse();
    // Deserialize the default value
    const auto default_value_deserialized
        = value_deserializer(buffer, current_position);
    default_value_ = default_value_deserialized.Value();
    current_position += default_value_deserialized.BytesRead();
    // Deserialize the OOB value
    const auto oob_value_deserialized
        = value_deserializer(buffer, current_position);
    oob_value_ = oob_value_deserialized.Value();
    current_position += oob_value_deserialized.BytesRead();
    // Deserialize the data
    const auto data_deserialized
        = serialization::DeserializeVectorLike<T, BackingStore>(
              buffer, current_position, value_deserializer);
    data_ = data_deserialized.Value();
    current_position += data_deserialized.BytesRead();
    // Deserialize the cell sizes
    const auto sizes_deserialized
        = GridSizes::Deserialize(buffer, current_position);
    sizes_ = sizes_deserialized.Value();
    current_position += sizes_deserialized.BytesRead();
    if (sizes_.TotalCells() != static_cast<int64_t>(data_.size()))
    {
      throw std::runtime_error("sizes_.NumCells() != data_.size()");
    }
    // Deserialize the initialized
    const auto initialized_deserialized
        = serialization::DeserializeMemcpyable<uint8_t>(buffer,
                                                        current_position);
    initialized_ = static_cast<bool>(initialized_deserialized.Value());
    current_position += initialized_deserialized.BytesRead();
    if (sizes_.Valid() != initialized_)
    {
      throw std::runtime_error("sizes_.Valid() != initialized_");
    }
    // Figure out how many bytes were read
    const uint64_t bytes_read = current_position - starting_offset;
    return bytes_read;
  }

  bool OnMutableAccess(const GridIndex& index)
  {
    return OnMutableAccess(index.X(), index.Y(), index.Y());
  }

protected:
  // These are pure-virtual in the base class to force their implementation in
  // derived classes.

  /// Do the work necessary for Clone() to copy the current object.
  virtual VoxelGridBase<T, BackingStore>* DoClone() const = 0;

  /// Serialize any derived-specific members into the provided buffer.
  virtual uint64_t DerivedSerializeSelf(
      std::vector<uint8_t>& buffer,
      const serialization::Serializer<T>& value_serializer) const = 0;

  /// Deserialize any derived-specific members from the provided buffer.
  virtual uint64_t DerivedDeserializeSelf(
      const std::vector<uint8_t>& buffer, const uint64_t starting_offset,
      const serialization::Deserializer<T>& value_deserializer) = 0;

  /// Callback on any mutable access to the grid. Return true/false to allow or
  /// disallow access to the grid. For example, this can be used to prohibit
  /// changes to a non-const grid, or to invalidate a cache if voxels are
  /// modified.
  virtual bool OnMutableAccess(const int64_t x_index,
                               const int64_t y_index,
                               const int64_t z_index) = 0;

public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  VoxelGridBase(const Eigen::Isometry3d& origin_transform,
                const GridSizes& sizes,
                const T& default_value,
                const T& oob_value)
  {
    Initialize(origin_transform, sizes, default_value, oob_value);
  }

  VoxelGridBase(const GridSizes& sizes,
                const T& default_value,
                const T& oob_value)
  {
    Initialize(sizes, default_value, oob_value);
  }

  VoxelGridBase()
  {
    origin_transform_ = Eigen::Isometry3d::Identity();
    inverse_origin_transform_ = origin_transform_.inverse();
    utility::RequireAlignment(origin_transform_, 16u);
    utility::RequireAlignment(inverse_origin_transform_, 16u);
    data_.clear();
    initialized_ = false;
  }

  virtual ~VoxelGridBase() {}

  void Initialize(const Eigen::Isometry3d& origin_transform,
                  const GridSizes& sizes,
                  const T& default_value,
                  const T& oob_value)
  {
    if (sizes.Valid())
    {
      sizes_ = sizes;
      origin_transform_ = origin_transform;
      inverse_origin_transform_ = origin_transform_.inverse();
      utility::RequireAlignment(origin_transform_, 16u);
      utility::RequireAlignment(inverse_origin_transform_, 16u);
      default_value_ = default_value;
      oob_value_ = oob_value;
      SetContents(default_value_);
      initialized_ = true;
    }
    else
    {
      throw std::invalid_argument("sizes is not valid");
    }
  }

  void Initialize(const GridSizes& sizes,
                  const T& default_value,
                  const T& oob_value)
  {
    const Eigen::Translation3d origin_translation(-sizes.XSize() * 0.5,
                                                  -sizes.YSize() * 0.5,
                                                  -sizes.ZSize() * 0.5);
    const Eigen::Isometry3d origin_transform
        = origin_translation * Eigen::Quaterniond::Identity();
    Initialize(origin_transform, sizes, default_value, oob_value);
  }

  VoxelGridBase<T, BackingStore>* Clone() const
  {
    return DoClone();
  }

  uint64_t SerializeSelf(
      std::vector<uint8_t>& buffer,
      const serialization::Serializer<T>& value_serializer) const
  {
    return BaseSerializeSelf(buffer, value_serializer)
        + DerivedSerializeSelf(buffer, value_serializer);
  }

  uint64_t DeserializeSelf(
      const std::vector<uint8_t>& buffer, const uint64_t starting_offset,
      const serialization::Deserializer<T>& value_deserializer)
  {
    uint64_t current_position = starting_offset;
    current_position
        += BaseDeserializeSelf(buffer, starting_offset, value_deserializer);
    current_position
        += DerivedDeserializeSelf(buffer, current_position, value_deserializer);
    // Figure out how many bytes were read
    const uint64_t bytes_read = current_position - starting_offset;
    return bytes_read;
  }

  bool IsInitialized() const
  {
    return initialized_;
  }

  void ResetWithDefault()
  {
    SetContents(default_value_);
  }

  void ResetWithNewValue(const T& new_value)
  {
    SetContents(new_value);
  }

  void ResetWithNewDefault(const T& new_default)
  {
    default_value_ = new_default;
    SetContents(default_value_);
  }

  bool IndexInBounds(const int64_t x_index,
                     const int64_t y_index,
                     const int64_t z_index) const
  {
    return sizes_.IndexInBounds(x_index, y_index, z_index);
  }

  bool IndexInBounds(const GridIndex& index) const
  {
    return sizes_.IndexInBounds(index);
  }

  bool LocationInBounds(const double x,
                        const double y,
                        const double z) const
  {
    const GridIndex index = LocationToGridIndex(x, y, z);
    return sizes_.IndexInBounds(index);
  }

  bool LocationInBounds3d(const Eigen::Vector3d& location) const
  {
    const GridIndex index = LocationToGridIndex3d(location);
    return sizes_.IndexInBounds(index);
  }

  bool LocationInBounds4d(const Eigen::Vector4d& location) const
  {
    const GridIndex index = LocationToGridIndex4d(location);
    return sizes_.IndexInBounds(index);
  }

  GridQuery<const T> GetImmutable3d(const Eigen::Vector3d& location) const
  {
    return GetImmutable(LocationToGridIndex3d(location));
  }

  GridQuery<const T> GetImmutable4d(const Eigen::Vector4d& location) const
  {
    return GetImmutable(LocationToGridIndex4d(location));
  }

  GridQuery<const T> GetImmutable(const double x,
                                  const double y,
                                  const double z) const
  {
    const Eigen::Vector4d location(x, y, z, 1.0);
    return GetImmutable4d(location);
  }

  GridQuery<const T> GetImmutable(const GridIndex& index) const
  {
    if (sizes_.IndexInBounds(index))
    {
      return GridQuery<const T>(AccessIndex(sizes_.GetDataIndex(index)));
    }
    else
    {
      return GridQuery<const T>();
    }
  }

  GridQuery<const T> GetImmutable(const int64_t x_index,
                                  const int64_t y_index,
                                  const int64_t z_index) const
  {
    if (sizes_.IndexInBounds(x_index, y_index, z_index))
    {
      return GridQuery<const T>(
            AccessIndex(sizes_.GetDataIndex(x_index, y_index, z_index)));
    }
    else
    {
      return GridQuery<const T>();
    }
  }

  GridQuery<T> GetMutable3d(const Eigen::Vector3d& location)
  {
    return GetMutable(LocationToGridIndex3d(location));
  }

  GridQuery<T> GetMutable4d(const Eigen::Vector4d& location)
  {
    return GetMutable(LocationToGridIndex4d(location));
  }

  GridQuery<T> GetMutable(const double x,
                          const double y,
                          const double z)
  {
    const Eigen::Vector4d location(x, y, z, 1.0);
    return GetMutable4d(location);
  }

  GridQuery<T> GetMutable(const GridIndex& index)
  {
    if (sizes_.IndexInBounds(index) && OnMutableAccess(index))
    {
      return GridQuery<T>(AccessIndex(sizes_.GetDataIndex(index)));
    }
    else
    {
      return GridQuery<T>();
    }
  }

  GridQuery<T> GetMutable(const int64_t x_index,
                          const int64_t y_index,
                          const int64_t z_index)
  {
    if (sizes_.IndexInBounds(x_index, y_index, z_index) &&
        OnMutableAccess(x_index, y_index, z_index))
    {
      return GridQuery<T>(
            AccessIndex(sizes_.GetDataIndex(x_index, y_index, z_index)));
    }
    else
    {
      return GridQuery<T>();
    }
  }

  bool SetValue3d(const Eigen::Vector3d& location, const T& value)
  {
    return SetValue(LocationToGridIndex3d(location), value);
  }

  bool SetValue4d(const Eigen::Vector4d& location, const T& value)
  {
    return SetValue(LocationToGridIndex4d(location), value);
  }

  bool SetValue(const double x,
                const double y,
                const double z,
                const T& value)
  {
    const Eigen::Vector4d location(x, y, z, 1.0);
    return SetValue4d(location, value);
  }

  bool SetValue(const GridIndex& index, const T& value)
  {
    if (sizes_.IndexInBounds(index) && OnMutableAccess(index))
    {
      AccessIndex(sizes_.GetDataIndex(index)) = value;
      return true;
    }
    else
    {
      return false;
    }
  }

  bool SetValue(const int64_t x_index,
                const int64_t y_index,
                const int64_t z_index,
                const T& value)
  {
    if (sizes_.IndexInBounds(x_index, y_index, z_index) &&
        OnMutableAccess(x_index, y_index, z_index))
    {
      AccessIndex(sizes_.GetDataIndex(x_index, y_index, z_index)) = value;
      return true;
    }
    else
    {
      return false;
    }
  }

  bool SetValue3d(const Eigen::Vector3d& location, T&& value)
  {
    return SetValue(LocationToGridIndex3d(location), value);
  }

  bool SetValue4d(const Eigen::Vector4d& location, T&& value)
  {
    return SetValue(LocationToGridIndex4d(location), value);
  }

  bool SetValue(const double x,
                const double y,
                const double z,
                T&& value)
  {
    const Eigen::Vector4d location(x, y, z, 1.0);
    return SetValue4d(location, value);
  }

  bool SetValue(const GridIndex& index, T&& value)
  {
    if (sizes_.IndexInBounds(index) && OnMutableAccess(index))
    {
      AccessIndex(sizes_.GetDataIndex(index)) = value;
      return true;
    }
    else
    {
      return false;
    }
  }

  bool SetValue(const int64_t x_index,
                const int64_t y_index,
                const int64_t z_index,
                T&& value)
  {
    if (sizes_.IndexInBounds(x_index, y_index, z_index) &&
        OnMutableAccess(x_index, y_index, z_index))
    {
      AccessIndex(sizes_.GetDataIndex(x_index, y_index, z_index)) = value;
      return true;
    }
    else
    {
      return false;
    }
  }

  const GridSizes& GetGridSizes() const { return sizes_; }

  double GetXSize() const { return sizes_.XSize(); }

  double GetYSize() const { return sizes_.YSize(); }

  double GetZSize() const { return sizes_.ZSize(); }

  Eigen::Vector3d GetSizes() const { return sizes_.Sizes(); }

  Eigen::Vector3d GetCellSizes() const { return sizes_.CellSizes(); }

  bool HasUniformCellSize() const { return sizes_.UniformCellSize(); }

  const T& GetDefaultValue() const { return default_value_; }

  const T& GetOOBValue() const { return oob_value_; }

  void SetDefaultValue(const T& default_value)
  {
    default_value_ = default_value;
  }

  void SetOOBValue(const T& oob_value) { oob_value_ = oob_value; }

  int64_t GetTotalCells() const { return sizes_.TotalCells(); }

  int64_t GetNumXCells() const { return sizes_.NumXCells(); }

  int64_t GetNumYCells() const { return sizes_.NumYCells(); }

  int64_t GetNumZCells() const { return sizes_.NumZCells(); }

  Eigen::Matrix<int64_t, 3, 1> GetNumCells() const { return sizes_.NumCells(); }

  const Eigen::Isometry3d& GetOriginTransform() const
  {
    return origin_transform_;
  }

  const Eigen::Isometry3d& GetInverseOriginTransform() const
  {
    return inverse_origin_transform_;
  }

  void UpdateOriginTransform(const Eigen::Isometry3d& origin_transform)
  {
    origin_transform_ = origin_transform;
    inverse_origin_transform_ = origin_transform_.inverse();
  }

  GridIndex LocationInGridFrameToGridIndex(const double x,
                                           const double y,
                                           const double z) const
  {
    return LocationInGridFrameToGridIndex4d(Eigen::Vector4d(x, y, z, 1.0));
  }

  GridIndex LocationInGridFrameToGridIndex3d(
      const Eigen::Vector3d& location) const
  {
    const int64_t x_cell
        = static_cast<int64_t>(location.x() * sizes_.InvCellXSize());
    const int64_t y_cell
        = static_cast<int64_t>(location.y() * sizes_.InvCellYSize());
    const int64_t z_cell
        = static_cast<int64_t>(location.z() * sizes_.InvCellZSize());
    return GridIndex(x_cell, y_cell, z_cell);
  }

  GridIndex LocationInGridFrameToGridIndex4d(
      const Eigen::Vector4d& location) const
  {
    if (location(3) == 1.0)
    {
      const int64_t x_cell
          = static_cast<int64_t>(location(0) * sizes_.InvCellXSize());
      const int64_t y_cell
          = static_cast<int64_t>(location(1) * sizes_.InvCellYSize());
      const int64_t z_cell
          = static_cast<int64_t>(location(2) * sizes_.InvCellZSize());
      return GridIndex(x_cell, y_cell, z_cell);
    }
    else
    {
      throw std::invalid_argument("location(3) != 1");
    }
  }

  GridIndex LocationToGridIndex(const double x,
                                const double y,
                                const double z) const
  {
    return LocationToGridIndex4d(Eigen::Vector4d(x, y, z, 1.0));
  }

  GridIndex LocationToGridIndex3d(const Eigen::Vector3d& location) const
  {
    const Eigen::Vector3d point_in_grid_frame
        = GetInverseOriginTransform() * location;
    return LocationInGridFrameToGridIndex3d(point_in_grid_frame);
  }

  GridIndex LocationToGridIndex4d(const Eigen::Vector4d& location) const
  {
    const Eigen::Vector4d point_in_grid_frame
        = GetInverseOriginTransform() * location;
    return LocationInGridFrameToGridIndex4d(point_in_grid_frame);
  }

  Eigen::Vector4d GridIndexToLocationInGridFrame(const GridIndex& index) const
  {
    return sizes_.IndexToLocationInGridFrame(index);
  }

  Eigen::Vector4d GridIndexToLocationInGridFrame(const int64_t x_index,
                                                 const int64_t y_index,
                                                 const int64_t z_index) const
  {
    return sizes_.IndexToLocationInGridFrame(x_index, y_index, z_index);
  }

  Eigen::Vector4d GridIndexToLocation(const GridIndex& index) const
  {
    return GetOriginTransform() * GridIndexToLocationInGridFrame(index);
  }

  Eigen::Vector4d GridIndexToLocation(const int64_t x_index,
                                      const int64_t y_index,
                                      const int64_t z_index) const
  {
    return GetOriginTransform()
        * GridIndexToLocationInGridFrame(x_index, y_index, z_index);
  }

  int64_t GridIndexToDataIndex(const int64_t x_index,
                               const int64_t y_index,
                               const int64_t z_index) const
  {
    return sizes_.GetDataIndex(x_index, y_index, z_index);
  }

  int64_t GridIndexToDataIndex(const GridIndex& index) const
  {
    return sizes_.GetDataIndex(index);
  }

  GridIndex DataIndexToGridIndex(const int64_t data_index) const
  {
    return sizes_.GetGridIndexFromDataIndex(data_index);
  }

  BackingStore& GetMutableRawData() { return data_; }

  const BackingStore& GetImmutableRawData() const { return data_; }

  bool SetRawData(const BackingStore& data)
  {
    const int64_t expected_length = GetTotalCells();
    if (static_cast<int64_t>(data.size()) == expected_length)
    {
      data_ = data;
      return true;
    }
    else
    {
      std::cerr << "Failed to load internal data - expected "
                << expected_length << " got " << data.size() << std::endl;
      return false;
    }
  }

  uint64_t HashDataIndex(const int64_t x_index,
                         const int64_t y_index,
                         const int64_t z_index) const
  {
    return static_cast<uint64_t>(
        GridIndexToDataIndex(x_index, y_index, z_index));
  }

  uint64_t HashDataIndex(const GridIndex& index) const
  {
    return static_cast<uint64_t>(GridIndexToDataIndex(index));
  }
};

/// If you want a VoxelGrid<T> this is the class to use. Since you should never
/// inherit from it, this class is final.
template<typename T, typename BackingStore=std::vector<T>>
class VoxelGrid final : public VoxelGridBase<T, BackingStore>
{
private:
  VoxelGridBase<T, BackingStore>* DoClone() const override
  {
    return new VoxelGrid<T, BackingStore>(
        static_cast<const VoxelGrid<T, BackingStore>&>(*this));
  }

  uint64_t DerivedSerializeSelf(
      std::vector<uint8_t>& buffer,
      const serialization::Serializer<T>& value_serializer) const override
  {
    UNUSED(buffer);
    UNUSED(value_serializer);
    return 0;
  }

  uint64_t DerivedDeserializeSelf(
      const std::vector<uint8_t>& buffer, const uint64_t starting_offset,
      const serialization::Deserializer<T>& value_deserializer) override
  {
    UNUSED(buffer);
    UNUSED(starting_offset);
    UNUSED(value_deserializer);
    return 0;
  }

  bool OnMutableAccess(const int64_t x_index,
                       const int64_t y_index,
                       const int64_t z_index) override
  {
    UNUSED(x_index);
    UNUSED(y_index);
    UNUSED(z_index);
    return true;
  }

public:
  static uint64_t Serialize(
      const VoxelGrid<T, BackingStore>& grid, std::vector<uint8_t>& buffer,
      const serialization::Serializer<T>& value_serializer)
  {
    return grid.SerializeSelf(buffer, value_serializer);
  }

  static serialization::Deserialized<VoxelGrid<T, BackingStore>> Deserialize(
      const std::vector<uint8_t>& buffer, const uint64_t starting_offset,
      const serialization::Deserializer<T>& value_deserializer)
  {
    VoxelGrid<T, BackingStore> temp_grid;
    const uint64_t bytes_read
        = temp_grid.DeserializeSelf(buffer, starting_offset,
                                    value_deserializer);
    return serialization::MakeDeserialized(temp_grid, bytes_read);
  }

  VoxelGrid(const Eigen::Isometry3d& origin_transform,
            const GridSizes& sizes,
            const T& default_value)
      : VoxelGridBase<T, BackingStore>(
          origin_transform, sizes, default_value, default_value) {}

  VoxelGrid(const Eigen::Isometry3d& origin_transform,
            const GridSizes& sizes,
            const T& default_value,
            const T& oob_value)
      : VoxelGridBase<T, BackingStore>(
          origin_transform, sizes, default_value, oob_value) {}

  VoxelGrid(const GridSizes& sizes, const T& default_value)
      : VoxelGridBase<T, BackingStore>(sizes, default_value, default_value) {}

  VoxelGrid(const GridSizes& sizes, const T& default_value, const T& oob_value)
      : VoxelGridBase<T, BackingStore>(sizes, default_value, oob_value) {}

  VoxelGrid() : VoxelGridBase<T, BackingStore>() {}
};
}  // namespace voxel_grid
}  // namespace common_robotics_utilities

namespace std
{
template <>
struct hash<common_robotics_utilities::voxel_grid::GridIndex>
{
  std::size_t operator()(
      const common_robotics_utilities::voxel_grid::GridIndex& index) const
  {
    std::size_t hash_val = 0;
    common_robotics_utilities::utility::hash_combine(
        hash_val, index.X(), index.Y(), index.Z());
    return hash_val;
  }
};
}

inline std::ostream& operator<<(
    std::ostream& strm,
    const common_robotics_utilities::voxel_grid::GridIndex& index)
{
  strm << "GridIndex: (" << index.X() << "," << index.Y() << "," << index.Z()
       << ")";
  return strm;
}


// -----------------------------------------------------------------------
// RTToolbox - DKFZ radiotherapy quantitative evaluation library
//
// (c) Copyright 2007, DKFZ, Heidelberg, Germany
// ALL RIGHTS RESERVED
//
// THIS FILE CONTAINS CONFIDENTIAL AND PROPRIETARY INFORMATION OF DKFZ.
// ANY DUPLICATION, MODIFICATION, DISTRIBUTION, OR
// DISCLOSURE IN ANY FORM, IN WHOLE, OR IN PART, IS STRICTLY PROHIBITED
// WITHOUT THE PRIOR EXPRESS WRITTEN PERMISSION OF DKFZ.
//
//------------------------------------------------------------------------

// this file defines the rttbCoreTests for the test driver
// and all it expects is that you have a function called RegisterTests

#include <fstream>

#include "litCheckMacros.h"
#include "rttbBioModel.h"
#include "rttbTCPLQModel.h"
#include "rttbNTCPLKBModel.h"

#include <boost/make_shared.hpp>
#include <boost/shared_ptr.hpp>
#include "boost/filesystem.hpp"

#include "rttbModelXMLWriter.h"

namespace rttb
{

	namespace testing
	{
		static std::string readFile(const std::string& filename);
		int ModelsIOTest(int argc, char* argv[])
		{
			std::string expectedxmlfilenametcpleq;
			std::string expectedxmlfilenamentcplk;

			if (argc > 2)
			{
				expectedxmlfilenametcpleq = argv[1];
				expectedxmlfilenamentcplk = argv[2];
			}

			typedef core::DVH::DataDifferentialType DataDifferentialType;

			PREPARE_DEFAULT_TEST_REPORTING;

			//generate artificial DVH and corresponding statistical values
			DoseTypeGy binSize = DoseTypeGy(0.1);
			DoseVoxelVolumeType voxelVolume = 8;

			DataDifferentialType aDataDifferential;

			DoseCalcType value = 0;
			DVHVoxelNumber numberOfVoxels = 0;

			// creat default values
			for (int i = 0; i < 98; i++)
			{
				value = 0;
				numberOfVoxels += value;
				aDataDifferential.push_back(value);
			}

			aDataDifferential.push_back(10);
			aDataDifferential.push_back(20);

			const IDType structureID = "myStructure";
			const IDType doseID = "myDose";
			const IDType voxelizationID = "myVoxelization";

			core::DVH::Pointer dvhPtr = boost::make_shared<core::DVH>(aDataDifferential, binSize,
			                               voxelVolume, structureID,
			                               doseID, voxelizationID);

			//test TCP LQ Model
			models::BioModelParamType alpha = 0.35;
			models::BioModelParamType beta = 0.023333333333333;
			models::BioModelParamType roh = 10000000;
			int numFractions = 8;

			boost::shared_ptr<rttb::models::TCPLQModel> tcplq = boost::make_shared<rttb::models::TCPLQModel>
			        (dvhPtr, roh, numFractions, alpha / beta, alpha, 0.08);

			std::string filename = "BioModeltcpleqIOTest.xml";
			rttb::io::models::ModelXMLWriter writer = rttb::io::models::ModelXMLWriter(filename, tcplq, false);
			CHECK_NO_THROW(writer.writeModel());
			CHECK_EQUAL(boost::filesystem::exists(filename), true);

			std::string defaultAsIs = readFile(filename);
			std::string defaultExpected = readFile(expectedxmlfilenametcpleq);
			//Does not work due to double imprecisions. As long as we don't have a ModelXMLReader, a check does not makes sense. See T21832
			//CHECK_EQUAL(defaultAsIs, defaultExpected);
			CHECK_EQUAL(std::remove(filename.c_str()), 0);

			//test NTCPLKBModel
			models::BioModelParamType aVal = 10;
			models::BioModelParamType mVal = 0.16;
			models::BioModelParamType d50Val = 35;

			boost::shared_ptr<rttb::models::NTCPLKBModel> ntcplk =
			    boost::make_shared<rttb::models::NTCPLKBModel>(dvhPtr, d50Val, mVal, aVal);
			filename = "BioModelntcplkIOTest.xml";
			rttb::io::models::ModelXMLWriter writer2 = rttb::io::models::ModelXMLWriter(filename, ntcplk);
			CHECK_NO_THROW(writer2.writeModel());
			CHECK_EQUAL(boost::filesystem::exists(filename), true);

			defaultAsIs = readFile(filename);
			defaultExpected = readFile(expectedxmlfilenamentcplk);
			//Does not work due to double imprecisions. As long as we don't have a ModelXMLReader, a check does not makes sense. See T21832
			//CHECK_EQUAL(defaultAsIs, defaultExpected);
			CHECK_EQUAL(std::remove(filename.c_str()), 0);
			std::string dvhFilename = "dvhfor" + ntcplk->getModelType() + ".xml";
			CHECK_EQUAL(std::remove(dvhFilename.c_str()), 0);

			RETURN_AND_REPORT_TEST_SUCCESS;
		}

		std::string readFile(const std::string& filename)
		{
			std::ifstream fileStream(filename.c_str());
			std::string content((std::istreambuf_iterator<char>(fileStream)),
			                    (std::istreambuf_iterator<char>()));
			return content;
		}
	}
}


/*
 * Market.cc
 *
 *  Created on: Apr 13, 2014
 *      Author: wilfeli
 */

#include <list>
#include <algorithm>
#include <functional>
#include <iostream>

#include <boost/random.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/random/uniform_int_distribution.hpp>

#include "W.h"
#include "Market.h"
#include "ISeller.h"
#include "IBuyer.h"
#include "Contract.h"
#include "Message.h"
#include "Agent.h"
#include "Tools.h"
#include "Parameters.h"

//general code to find equilibrium

MarketL::MarketL(W &w_, std::string mode): w(w_){
};

MarketL::MarketL(W &w_): w(w_){
	w.AddMarketL(this);
};

void
MarketL::AddSellerL(ISellerL* agent) { sellers.push_back(agent); };

void
MarketL::AddBuyerL(IBuyerL* agent) { buyers.push_back(agent); };

void
MarketL::_collect(){
	//clears previous asks
	asks.clear();

	//collect asks
	//for each seller, call
	for(auto seller=sellers.begin();seller != sellers.end(); ++seller){
		//?collect them only once
		asks.push_back((**seller).ac_L());
	};

	bids.clear();
	for(auto buyer=buyers.begin();buyer != buyers.end(); ++buyer){
		//?collect them only once
		bids.push_back((**buyer).ac_L());
	};
};

void
MarketL::_demand(){

	//clean previous demand_curve
	for (auto dc=demand_curve.begin(); dc!=demand_curve.end(); ++dc){
		dc->second->demand_curve_point_bids.clear();
        delete dc->second;
	};
	demand_curve.clear();

	//form list of cut-off prices
	std::list<double> p_t;
	long N_steps = 0;

	typedef std::map<double,DemandCurveBid*>::iterator ITER_DC;
	typedef decltype(demand_curve.rend()) REV_ITER_DC;

	ITER_DC p_stop;
	REV_ITER_DC rev_p_stop;
	double p;

	double key;

	for (auto bid = bids.begin(); bid != bids.end(); ++bid){
		//only for active agents
//		if (reinterpret_cast<Agent>(*(**bid).issuer).status != 0){
//		if (reinterpret_cast<Agent*>((**bid).issuer)->status != 0){
		if ((**bid).issuer->get_status() != 0){
			//get size of demand curve
			N_steps = (**bid).demand_curve.rows();

			//iterate over each row
			for (int i = 0; i < N_steps; ++i){
				//check that p isn't in the map
				//key equal to p
				key = (**bid).demand_curve(i,0);
				auto dcbidin = demand_curve.find(key);
				if (dcbidin == demand_curve.end()){
					//create new entry
					DemandCurveBid* dcbidnew = new DemandCurveBid();
					dcbidnew->p = key;
					dcbidnew->q += (**bid).demand_curve(i,1);
					dcbidnew->demand_curve_point_bids.push_back(std::make_pair(i, *bid));

					//add new entry to the map and get a pointer to it
					std::pair<ITER_DC,bool>
					dcbid_pos = demand_curve.insert(std::pair<double, DemandCurveBid*>(key,dcbidnew));

					//add q to accommodate inserting point
					//create pointer to the current price for the bid
					dcbidin = dcbid_pos.first;

					if (++dcbid_pos.first != demand_curve.end()){
						dcbidnew->q += dcbid_pos.first->second->q;
						//add all other agents' bids
						dcbidnew->demand_curve_point_bids.insert(dcbidnew->demand_curve_point_bids.end(),
																dcbid_pos.first->second->demand_curve_point_bids.begin(),
																dcbid_pos.first->second->demand_curve_point_bids.end());
					};

				}else{
					dcbidin->second->q += (**bid).demand_curve(i,1);
					dcbidin->second->demand_curve_point_bids.push_back(std::make_pair(i, *bid));

				};

				//update curve for demand with q
				p_stop = demand_curve.end();
				rev_p_stop = demand_curve.rend();

				if (i > 0){
					p = (**bid).demand_curve(i-1,0);
					p_stop = demand_curve.find(p);
				}

				if (p_stop == demand_curve.end()){
					rev_p_stop = demand_curve.rend();
				}else{
					rev_p_stop = std::reverse_iterator<ITER_DC>(p_stop);
				};

				if (dcbidin == demand_curve.begin()){

				}else{
					dcbidin--;
				};

				//add q to past p until previous cut-off
				for (std::reverse_iterator<ITER_DC> rev_dcbid (dcbidin);
						rev_dcbid != rev_p_stop; ++rev_dcbid){
					rev_dcbid->second->q += (**bid).demand_curve(i,1);
					rev_dcbid->second->demand_curve_point_bids.push_back(std::make_pair(i, *bid));
				};

			};
		};
	};

//	//print all q
//	for (auto iter:demand_curve){
//		std::cout << iter.first << "," << iter.second->q << "\n";
//	};

};

void
MarketL::_supply(){

	//clean previous supply_curve
	for (auto sc=supply_curve.begin(); sc!=supply_curve.end(); ++sc){
		sc->second->supply_curve_point_asks.clear();
        delete sc->second;
	};
	supply_curve.clear();

	//form list of cut-off prices
	std::list<double> p_t;
	long N_steps = 0;

	typedef std::map<double,SupplyCurveAsk*>::iterator ITER_sc;
	typedef decltype(supply_curve.rend()) REV_ITER_sc;

	ITER_sc p_stop;
	REV_ITER_sc rev_p_stop;
	double p = 0.0;

	double key;

	for (auto ask = asks.begin(); ask != asks.end(); ++ask){
		//only for active agents
//		if (reinterpret_cast<Agent>(*(**ask).issuer).status != 0){
		if ((**ask).issuer->get_status() != 0){
			//get size of supply curve
			N_steps = (**ask).supply_curve.rows();

			//iterate over each row
			for (long i = N_steps - 1; i >= 0; --i){
				//check that p isn't in the map
				//key equal to p
				key = (**ask).supply_curve(i,0);
				auto scaskin = supply_curve.find(key);
				if (scaskin == supply_curve.end()){
					//create new entry
					SupplyCurveAsk* scasknew = new SupplyCurveAsk();
					scasknew->p = key;
					scasknew->q += (**ask).supply_curve(i,1);
					scasknew->supply_curve_point_asks.push_back(std::make_pair(i, *ask));

					//add new entry to the map and get a pointer to it
					std::pair<ITER_sc,bool>
					scask_pos = supply_curve.insert(std::pair<double, SupplyCurveAsk*>(key,scasknew));

					//add q to accommodate inserting point
					//create pointer to the current price for the ask
					scaskin = scask_pos.first;

					if (scaskin != supply_curve.begin()){
						scaskin--;

						scasknew->q += scaskin->second->q;
						//add all other agents' asks
						scasknew->supply_curve_point_asks.insert(scasknew->supply_curve_point_asks.end(),
																scaskin->second->supply_curve_point_asks.begin(),
																scaskin->second->supply_curve_point_asks.end());
					};

				}else{
					scaskin->second->q += (**ask).supply_curve(i,1);
					scaskin->second->supply_curve_point_asks.push_back(std::make_pair(i, *ask));

				};

				//update curve for supply with q
				p_stop = supply_curve.end();
				rev_p_stop = supply_curve.rend();

				if (i < N_steps - 1){
					p = (**ask).supply_curve(i + 1,0);
					p_stop = supply_curve.find(p);
				}

				if (p_stop == supply_curve.end()){
					rev_p_stop = supply_curve.rend();
				}else{
					rev_p_stop = std::reverse_iterator<ITER_sc>(p_stop);
				};

				std::reverse_iterator<ITER_sc> rev_until (scaskin);

				//move to the next element
				rev_until--;

				//add q to future p until next cut-off
				for (std::reverse_iterator<ITER_sc> rev_scask (p_stop);
						rev_scask != rev_until; ++rev_scask){
					rev_scask->second->q += (**ask).supply_curve(i,1);
					rev_scask->second->supply_curve_point_asks.push_back(std::make_pair(i, *ask));
				};

			};
		};
	};

//	//print all q
//	for (auto iter:supply_curve){
//		std::cout << iter.first << "," << iter.second->q << "\n";
//	};

};

void
MarketL::_eq(){
    bool FLAG_ASK = false;
    bool FLAG_BID = false;
	bool FLAG_EQ = false; //equilibrium
	bool FLAG_S_H_D = false; //supply higher than demand
	bool FLAG_END = false; //indicator that needs to stop iteration
	double p_eq = 0.0;

	std::vector<double> p_all;

	//make list of all prices
	for (auto ask=supply_curve.begin(); ask != supply_curve.end(); ++ask){
		p_all.push_back(ask->first);
	};

	for (auto bid=demand_curve.begin(); bid != demand_curve.end(); ++bid){
		p_all.push_back(bid->first);
	};

	std::sort(p_all.begin(), p_all.end());

	//remove duplicates
	auto p_end = std::unique(p_all.begin(), p_all.end());

	auto p_i = p_end;
	--p_i;

    //check that have demand and supply
    if (supply_curve.begin() != supply_curve.end()){
        FLAG_ASK = true;
    };

    if (demand_curve.begin() != demand_curve.end()){
        FLAG_BID = true;
    };

    if (FLAG_BID && FLAG_ASK){
        while (( !FLAG_EQ ) && (!FLAG_END)){

            double q_s = 0.0;
            double q_d = 0.0;

            auto d_i = _i_p_demand((*p_i));
            auto s_i = _i_p_supply((*p_i));

            if (d_i != demand_curve.end()){
                q_d = _i_p_demand((*p_i))->second->q;
            };

            if (s_i != supply_curve.end()){
                q_s = _i_p_supply((*p_i))->second->q;
            };

            if (q_s >= q_d){
                FLAG_S_H_D = true;
            }else{
                if ((FLAG_S_H_D) && (p_i != std::prev(p_end))){
                    FLAG_EQ = true;
                    FLAG_S_H_D = false;
                    p_eq = *p_i;

    //                if (w.param->SIMULATION_MODE != "test"){

                        //but if supply is now zero ? - take next price
                        if (q_s <= 0.0){
                            //take previous price
                            p_eq = *std::next(p_i);
                        };
    //                };
                };
                if (p_i == std::prev(p_end)) {
                    FLAG_END = true;
                    FLAG_EQ = true; //call it equilibrium
                    FLAG_S_H_D = false;
                    p_eq = *p_i;
                };
            };

            if ((p_i == p_all.begin()) && (!FLAG_END && !FLAG_EQ)){
                if (!supply_curve.empty() && !demand_curve.empty()){
                    p_eq = (supply_curve.begin()->first + demand_curve.rbegin()->first)/2;
                    FLAG_EQ = true;
                }else{
                    p_eq = *p_all.begin();
                };
                FLAG_END = true;
            };

            --p_i;
        };
    }else{
//        p_eq = std::numeric_limits<double>::signaling_NaN();
        p_eq = 0.0;
    };

	//save price to market

	market_price.push_back(new MarketPrice(p_eq, FLAG_EQ));

//	//print all q
//	for (auto iter:supply_curve){
//		std::cout << iter.first << "," << iter.second->q << std::endl;
//	};
//
//	//print all q
//	for (auto iter:demand_curve){
//		std::cout << iter.first << "," << iter.second->q << std::endl;
//	};
//
//
//
//	std::cout << p_eq << std::endl;

};

void
MarketL::step(){

	_collect();
	_demand();
	_supply();
	_eq();

	if ((market_price.back()->FLAG_EQ) && (!supply_curve.empty() && !demand_curve.empty())) {
		_match();
	};

}

void
MarketC::step(){

	_collect();
	_demand();
	_supply();
	_eq();

	if ((market_price.back()->FLAG_EQ) && (!supply_curve.empty() && !demand_curve.empty())) {
		_match();
	};

}

void
MarketL::_match(){

	//get sellers at that price point
	//randomize them
	//iterate over them
	double price = market_price.back()->p;

	//find position for the price

    auto scask = _i_p_supply(price);
    //shuffle the suppliers
    using std::placeholders::_1;

    if (scask != supply_curve.end()) {

        if (w.param->SIMULATION_MODE != "test"){
            std::random_shuffle(scask->second->supply_curve_point_asks.begin(),
                    scask->second->supply_curve_point_asks.end(),
                    std::bind(&MarketL::random_shuffle,
                            this, std::placeholders::_1));
        }else{
            Tools::random_shuffle(scask->second->supply_curve_point_asks.begin(),
                                scask->second->supply_curve_point_asks.end(),
                                w.myrng);
        };

        for (auto ask:scask->second->supply_curve_point_asks){
            //check how much is selling
            //construct a message for an agent
            MessageMarketLCheckAsk* mes = new MessageMarketLCheckAsk (this, price);
            double q_ask = dynamic_cast<ISellerL*>(ask.second->issuer)->_mas_q_sell(mes);

            //clean memory
            delete mes;

            //find buyers for this price
            auto dcbid = _i_p_demand(price);

            if (dcbid != demand_curve.end()){

                if (w.param->SIMULATION_MODE != "test"){
                    //randomize buyers
                    std::random_shuffle(dcbid->second->demand_curve_point_bids.begin(),
                            dcbid->second->demand_curve_point_bids.end(),
                            std::bind(&MarketL::random_shuffle,
                                                this, std::placeholders::_1));
                }else{
                    //randomize buyers
                    Tools::random_shuffle(dcbid->second->demand_curve_point_bids.begin(),
                                        dcbid->second->demand_curve_point_bids.end(),
                                        w.myrng);

                };

                for (auto bid:dcbid->second->demand_curve_point_bids){
                    MessageMarketLCheckBid* mes = new MessageMarketLCheckBid (this, price);
                    double q_bid = dynamic_cast<IBuyerL*> (bid.second->issuer)->_mas_q_buy(mes);

                    //clean memory
                    delete mes;

                    if (q_bid > 0.0){
                        double q_bought = 0.0;
                        if (q_ask > 0.0){
                            if (q_bought < q_bid){

                                double q_sell = std::min(q_bid - q_bought, q_ask);

                                bool FLAG_CLEARED = _clear(dynamic_cast<ISellerL*>(ask.second->issuer),
                                                        dynamic_cast<IBuyerL*>(bid.second->issuer),
                                                    price,
                                                    q_sell);

                                if (FLAG_CLEARED){
                                    q_bought += q_sell;
                                    q_ask -= q_sell;
                                }else{
                                    break;
                                };
                            };
                        }else{
                            break;
                        };
                    };
                };
            };
        };
    };
};

bool
MarketL::_clear(ISellerL* seller, IBuyerL* buyer, double price, double q){

	bool FLAG_CLEARED = true;

	MessageMarketLSellC* mes = new MessageMarketLSellC(this, buyer, price, q);

	seller->_sell_asCHK(mes);

	delete mes;

	return FLAG_CLEARED;

};

void
MarketL::_clear_C(ContractHK* contract){

	contract->holder->_buy_asCHK(contract);

};

MarketL::SupplyIter
MarketL::_i_p_supply(double p){
	//handle no finding and last element

	MarketL::SupplyIter supply_iter ;//= supply_curve.begin();
//	bool FLAG_P = false;

	//find first price that is lower
	std::reverse_iterator<MarketL::SupplyIter> supply_rev (supply_curve.end());

	while ((supply_rev != supply_curve.rend()) && (supply_rev->first > p)){
		supply_rev++;
	};

	if ((supply_rev.base() == supply_curve.end()) && (supply_curve.end()!=supply_curve.begin())){
		supply_iter = std::prev(supply_curve.end());
	}else{
		if (supply_rev == supply_curve.rend()){
			supply_iter = supply_curve.end();
		}else{
			supply_iter = supply_rev.base();
		};
	};

	return supply_iter;
};

MarketL::DemandIter
MarketL::_i_p_demand(double p){

	//find first price that is higher

	MarketL::DemandIter demand_iter = demand_curve.begin();

	while ((demand_iter->first < p) && (demand_iter != demand_curve.end())){
		demand_iter++;
	};

	return demand_iter;
};

int
MarketL::random_shuffle(int i){
	typedef decltype(w.rng) RandomGeneratorType;

	//use world random to get generator
	boost::uniform_int<> distribution(0, i - 1);
	boost::variate_generator< RandomGeneratorType&, boost::uniform_int<> >
	    MarketL_shuffle( w.rng, distribution );

	return MarketL_shuffle();

};

MarketC::MarketC(W &w_):MarketL(w_, "derived"){
	w.AddMarketC(this);

};

void
MarketC::_collect(){
	//collect asks
	//for each seller, call
	asks.clear();
	for(auto seller=sellers.begin();seller != sellers.end(); ++seller){
		//?collect them only once
		asks.push_back((**seller).ac_C());
	};

	bids.clear();
	for(auto buyer=buyers.begin();buyer != buyers.end(); ++buyer){
		//?collect them only once
		bids.push_back((**buyer).ac_C());
	};
};

/**
 *
 * factor out who goes first and make it property of the market
 *
 *
 */

void
MarketC::_match(){

	//get sellers at that price point
	//randomize them
	//iterate over them
	double price = market_price.back()->p;

	//find position for the price
    auto scask = _i_p_supply(price);

    //find buyers for this price
	auto dcbid = _i_p_demand(price);

    using std::placeholders::_1;

    if ((scask != supply_curve.end()) && (dcbid != demand_curve.end())) {

        if (w.param->SIMULATION_MODE != "test"){
            //randomize buyers
            std::random_shuffle(dcbid->second->demand_curve_point_bids.begin(),
                    dcbid->second->demand_curve_point_bids.end(),
                    std::bind(&MarketL::random_shuffle,
                                        this, std::placeholders::_1));

            //shuffle the suppliers
            //check that have some suppliers
            std::random_shuffle(scask->second->supply_curve_point_asks.begin(),
                    scask->second->supply_curve_point_asks.end(),
                    std::bind(&MarketL::random_shuffle,
                            this, std::placeholders::_1));
        }else{
            //randomize buyers
            Tools::random_shuffle(dcbid->second->demand_curve_point_bids.begin(),
                                dcbid->second->demand_curve_point_bids.end(),
                                w.myrng);

            //shuffle the suppliers
            //check that have some suppliers
            Tools::random_shuffle(scask->second->supply_curve_point_asks.begin(),
                                scask->second->supply_curve_point_asks.end(),
                                w.myrng);

        };

		for (auto bid:dcbid->second->demand_curve_point_bids){
			MessageMarketCCheckBid* mes = new MessageMarketCCheckBid (this, price);
			double q_bid = dynamic_cast<IBuyerC*>(bid.second->issuer)->_mas_q_buy(mes);

			//clean memory
			delete mes;

			if (q_bid > 0.0)
			{
				double q_bought = 0.0;

				for (auto ask:scask->second->supply_curve_point_asks){
					//check how much is selling
					//construct a message for an agent
					MessageMarketCCheckAsk* mes = new MessageMarketCCheckAsk (this, price);
					double q_ask = dynamic_cast<ISellerC*>(ask.second->issuer)->_mas_q_sell(mes);
					//clean memory
					delete mes;

					if (q_ask > 0.0){
						if (q_bought < q_bid){

							double q_sell = std::min(q_bid - q_bought, q_ask);

							bool FLAG_CLEARED = _clear(dynamic_cast<ISellerC*>(ask.second->issuer),
													dynamic_cast<IBuyerC*>(bid.second->issuer),
												price,
												q_sell);

							if (FLAG_CLEARED){
								q_bought += q_sell;
								q_ask -= q_sell;
							}else{
                                break;
                            };
						}else{
                            break;
                        };
					};
				};
			};
		};
    };
};

bool
MarketC::_clear(ISellerC* seller, IBuyerC* buyer, double price, double q){
	bool FLAG_CLEARED = true;

	MessageMarketCSellGoodC* mes = new MessageMarketCSellGoodC(this, buyer, price, q);

	FLAG_CLEARED = seller->_sell_asGC(mes);

	delete mes;

	MessagePSSendPayment* mes_ps = new MessagePSSendPayment(seller, price*q, "buy_asGC");

	if (FLAG_CLEARED){
		FLAG_CLEARED = buyer->_PS_send_payment(mes_ps);
	};

	delete mes_ps;

	return FLAG_CLEARED;

};

void
MarketC::AddSellerC(ISellerC* agent) { sellers.push_back(agent); };

void
MarketC::AddBuyerC(IBuyerC* agent) { buyers.push_back(agent); };


#include <boost/iterator/function_input_iterator.hpp>
#include <iostream>
#include <random>

typedef struct{
    typedef int result_type;
    result_type operator() (){
        return std::rand();
    };
} generator;

int main(int argc, char *argv[]){
    auto g = generator();
    boost::function_input_iterator<generator, int> begin(g, 0);
    boost::function_input_iterator<generator, int> end(g, 10);

    // Output 10 random integer numbers.
    std::copy(begin, end, std::ostream_iterator<int>(std::cout, " "));

    return EXIT_SUCCESS;
}


#ifndef ALGOL_ALGORITHMS_RECURSION_PERMUTATION_HPP
#define ALGOL_ALGORITHMS_RECURSION_PERMUTATION_HPP

#include <array>
#include <string>
#include <functional>
#include <algorithm>
#include "stl2/concepts.hpp"
#include <boost/coroutine2/all.hpp>

namespace algol::algorithms::recursion {
  namespace concepts = std::experimental::ranges;

  template <typename T>
  using pull_type = typename boost::coroutines2::coroutine<T>::pull_type;

  template <typename T>
  using push_type = typename boost::coroutines2::coroutine<T>::push_type;

  namespace detail {
    template <concepts::Iterator It>
    class iterator_range {
    public:
      iterator_range (It begin, It end) : begin_(begin), end_(end)
      {}

      It begin ()
      { return begin_; }

      It end ()
      { return end_; }

    private:
      It begin_;
      It end_;
    };

    /**
     * \brief generates the n! permutations of [first, last) range one at time
     * \details it uses coroutines to generate a permutation at time
     * \complexity O(N!)
     * \precondition first >= last
     * \postcondition every permutation of the range [first, last) is generated
     * \tparam ForwardIt iterator type for [first, last) range
     * \param sink computed permutation
     * \param begin iterator to the first element of the whole range
     * \param first iterator to the first element of the recursive range
     * \param last iterator to the one past last element of the range
     */
    template <concepts::ForwardIterator ForwardIt>
    void permute (push_type<iterator_range<ForwardIt>>& sink, ForwardIt begin, ForwardIt first, ForwardIt last)
    {
      using namespace std::literals::string_literals;

      if (first > last)
        throw std::domain_error{"the range [first, last) is invalid"s};

      if (first == last) {
        sink(iterator_range(begin, last));
        return;
      }
      for (auto it = first; it != last; ++it) {
        std::iter_swap(first, it);
        permute(sink, begin, std::next(first), last);
        std::iter_swap(first, it);
      }
    }
  }

  /**
   * \brief generates every n! permutation of the n elements of the range
   * \details iterate over every permutation of [first, last) range
   * \complexity O(N!)
   * \precondition first >= last
   * \postcondition every permutation of the range [first, last) is generated
   * \tparam ForwardIt iterator type for [first, last) range
   * \param first iterator to the first element of the range
   * \param last iterator to the one past last element of the range
   * \return a range of iterator that define a permutation
   */
  template <concepts::ForwardIterator ForwardIt>
  pull_type<detail::iterator_range<ForwardIt>> permutations (ForwardIt first, ForwardIt last)
  {
    using std::placeholders::_1;
    pull_type<detail::iterator_range<ForwardIt>> source {std::bind(detail::permute<ForwardIt>, _1, first, first, last)};
    return source;
  }
}

#endif //ALGOL_ALGORITHMS_RECURSION_PERMUTATION_HPP


//==============================================================================
//         Copyright 2003 - 2011   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_CORE_UTILITY_LAGGED_FIBONACCI2281STREAM_HPP_INCLUDED
#define NT2_CORE_UTILITY_LAGGED_FIBONACCI2281STREAM_HPP_INCLUDED

#include <nt2/core/utility/config.hpp>
#include <nt2/core/utility/randstream.hpp>
#include <boost/random/lagged_fibonacci.hpp>

namespace nt2
{
  class lagged_fibonacci2281stream_ : public randstream_
  {
    public:
    virtual void rand (double* data, std::size_t i0, std::size_t i1);
    virtual void rand (float*  data, std::size_t i0, std::size_t i1);
    virtual void randn(double* data, std::size_t i0, std::size_t i1);
    virtual void randn(float*  data, std::size_t i0, std::size_t i1);
    virtual void seed (uint32_t s);

    virtual ~lagged_fibonacci2281stream_();

    private:
    boost::random::lagged_fibonacci2281 generator_;
  };

  nt2::randstream_* lagged_fibonacci2281stream();
}

#endif


/*=============================================================================
    Copyright (c) 2002-2010 Joel de Guzman

    Distributed under the Boost Software License, Version 1.0. (See accompanying
    file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
=============================================================================*/
///////////////////////////////////////////////////////////////////////////////
//
//  A parser for summing a comma-separated list of numbers using phoenix.
//
//  [ JDG June 28, 2002 ]   spirit1
//  [ JDG March 24, 2007 ]  spirit2
//
///////////////////////////////////////////////////////////////////////////////

#include <boost/config/warning_disable.hpp>
//[tutorial_adder_includes
#include <boost/spirit/include/qi.hpp>
#include <boost/spirit/include/phoenix_core.hpp>
#include <boost/spirit/include/phoenix_operator.hpp>
#include <iostream>
#include <string>
//]

namespace client
{
    //[tutorial_adder_using
    namespace qi = boost::spirit::qi;
    namespace ascii = boost::spirit::ascii;
    namespace phoenix = boost::phoenix;

    using qi::double_;
    using qi::_1;
    using ascii::space;
    using phoenix::ref;
    //]

    ///////////////////////////////////////////////////////////////////////////
    //  Our adder
    ///////////////////////////////////////////////////////////////////////////

    //[tutorial_adder
    template <typename Iterator>
    bool adder(Iterator first, Iterator last, double& n)
    {
        bool r = qi::phrase_parse(first, last,

            //  Begin grammar
            (
                double_[ref(n) = _1] >> *(',' >> double_[ref(n) += _1])
            )
            ,
            //  End grammar

            space);

        if (first != last) // fail if we did not get a full match
            return false;
        return r;
    }
    //]
}

////////////////////////////////////////////////////////////////////////////
//  Main program
////////////////////////////////////////////////////////////////////////////
int
main()
{
    std::cout << "/////////////////////////////////////////////////////////\n\n";
    std::cout << "\t\tA parser for summing a list of numbers...\n\n";
    std::cout << "/////////////////////////////////////////////////////////\n\n";

    std::cout << "Give me a comma separated list of numbers.\n";
    std::cout << "The numbers are added using Phoenix.\n";
    std::cout << "Type [q or Q] to quit\n\n";

    std::string str;
    while (getline(std::cin, str))
    {
        if (str.empty() || str[0] == 'q' || str[0] == 'Q')
            break;

        double n;
        if (client::adder(str.begin(), str.end(), n))
        {
            std::cout << "-------------------------\n";
            std::cout << "Parsing succeeded\n";
            std::cout << str << " Parses OK: " << std::endl;

            std::cout << "sum = " << n;
            std::cout << "\n-------------------------\n";
        }
        else
        {
            std::cout << "-------------------------\n";
            std::cout << "Parsing failed\n";
            std::cout << "-------------------------\n";
        }
    }

    std::cout << "Bye... :-) \n\n";
    return 0;
}


// ManualParserSolution.cpp : Defines the entry point for the console application.
//

#include "stdafx.h"
#include <boost/optional/optional_io.hpp>

#include "../ManualLexer/CalcLexer.h"
#include "../ManualParser/CalcParser.h"

using namespace std;
using namespace calc;

int main()
{
	string inputString;
	getline(cin, inputString);

	CalcLexer lexer = CalcLexer(inputString);
	CCalcParser parser = CCalcParser(lexer);

	double parsedNumber = parser.Calculate();
	cout << "Parsed: " << parsedNumber << endl;

    return 0;
}


#include <gtest/gtest.h>
#include <ka/algorithm.hpp>
#include <string>
#include <vector>
#include <deque>
#include <list>
#include <forward_list>
#include <map>
#include <unordered_map>
#include <set>
#include <unordered_set>
#include <boost/container/static_vector.hpp>
#include <boost/container/small_vector.hpp>
#include <boost/container/slist.hpp>
#include <boost/container/flat_set.hpp>
#include <boost/container/flat_map.hpp>

template<typename T>
struct EraseIfSequence : testing::Test {
};

using sequences = testing::Types<
  std::string, std::vector<int>, std::deque<int>, std::list<int>,
  std::forward_list<int>, boost::container::static_vector<int, 9>,
  boost::container::small_vector<int, 9>, boost::container::slist<int>>;

TYPED_TEST_SUITE(EraseIfSequence, sequences);

struct even_t {
  /// Arithmetic N
  template<typename N>
  bool operator()(N n) const {
    return n % N{2} == N{0};
  }
};

TYPED_TEST(EraseIfSequence, SequencesEmpty) {
  using S = TypeParam;
  S s;
  ka::erase_if(s, even_t{});
  EXPECT_EQ(S{}, s);
}

TYPED_TEST(EraseIfSequence, SequencesPredicateAlwaysFalse) {
  using S = TypeParam;
  S s{1, 3, 5};
  ka::erase_if(s, even_t{});
  EXPECT_EQ((S{1, 3, 5}), s);
}

TYPED_TEST(EraseIfSequence, SequencesPredicateAlwaysTrue) {
  using S = TypeParam;
  S s{4, 6, 8, 10};
  ka::erase_if(s, even_t{});
  EXPECT_EQ(S{}, s);
}

TYPED_TEST(EraseIfSequence, SequencesStandardCase) {
  using S = TypeParam;
  S s{1, 4, 6, 8, 3};
  ka::erase_if(s, even_t{});
  EXPECT_EQ((S{1, 3}), s);
}

template<typename T>
struct EraseIfAssociativeSequence : testing::Test {};

using associative_sequences = testing::Types<
  std::map<int, char>, std::multimap<int, char>, std::unordered_map<int, char>,
  boost::container::flat_map<int, char>, boost::container::flat_multimap<int, char>>;

TYPED_TEST_SUITE(EraseIfAssociativeSequence, associative_sequences);

struct even_key_t {
  /// Pair<Arithmetic, _> T
  template<typename T>
  bool operator()(T const& pair) const {
    using N = decltype(pair.first);
    return pair.first % N{2} == N{0};
  }
};

TYPED_TEST(EraseIfAssociativeSequence, AssociativeSequencesEmpty) {
  using S = TypeParam;
  S s;
  ka::erase_if(s, even_key_t{});
  EXPECT_EQ(S{}, s);
}

TYPED_TEST(EraseIfAssociativeSequence, AssociativeSequencesPredicateAlwaysFalse) {
  using S = TypeParam;
  S s{{1, 'a'}, {3, 'c'}, {5, 'e'}};
  ka::erase_if(s, even_key_t{});
  EXPECT_EQ((S{{1, 'a'}, {3, 'c'}, {5, 'e'}}), s);
}

TYPED_TEST(EraseIfAssociativeSequence, AssociativeSequencesPredicateAlwaysTrue) {
  using S = TypeParam;
  S s{{4, 'd'}, {6, 'f'}, {8, 'h'}, {10, 'j'}};
  ka::erase_if(s, even_key_t{});
  EXPECT_EQ(S{}, s);
}

TYPED_TEST(EraseIfAssociativeSequence, AssociativeSequencesStandardCase) {
  using S = TypeParam;
  S s{{1, 'a'}, {4, 'd'}, {6, 'f'}, {8, 'h'}, {3, 'c'}};
  ka::erase_if(s, even_key_t{});
  EXPECT_EQ((S{{1, 'a'}, {3, 'c'}}), s);
}

template<typename T>
struct EraseIfSet : testing::Test {};

using sets = testing::Types<
  std::set<int>, std::multiset<int>, std::unordered_set<int>,
  boost::container::flat_set<int>, boost::container::flat_multiset<int>>;

TYPED_TEST_SUITE(EraseIfSet, sets);

TYPED_TEST(EraseIfSet, SetsEmpty) {
  using S = TypeParam;
  S s;
  ka::erase_if(s, even_t{});
  EXPECT_EQ(S{}, s);
}

TYPED_TEST(EraseIfSet, SetsPredicateAlwaysFalse) {
  using S = TypeParam;
  S s{1, 3, 5, 3};
  ka::erase_if(s, even_t{});
  EXPECT_EQ((S{1, 3, 5, 3}), s);
}

TYPED_TEST(EraseIfSet, SetsPredicateAlwaysTrue) {
  using S = TypeParam;
  S s{4, 6, 8, 4, 10, 6};
  ka::erase_if(s, even_t{});
  EXPECT_EQ(S{}, s);
}

TYPED_TEST(EraseIfSet, SetsPredicateStandardCase) {
  using S = TypeParam;
  S s{1, 4, 1, 6, 6, 8, 3, 8, 3};
  ka::erase_if(s, even_t{});
  EXPECT_EQ((S{1, 1, 3, 3}), s);
}


#include <fstream>
#include <boost/timer.hpp>
#include <opencv2/imgcodecs.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/viz.hpp>

#include "myslam/config.h"
#include "myslam/visual_odometry.h"

int main(int argc, char **argv)
{
    if (argc != 2)
    {
        cout << "Need a parameter file" << endl;
        return 1;
    }

    myslam::Config::setParameterFile(argv[1]);
    myslam::VisualOdometry::Ptr vo(new myslam::VisualOdometry);

    string dataset_dir = myslam::Config::get<string>("dataset_dir");
    cout << "dataset: " << dataset_dir << endl;

    ifstream fin(dataset_dir + "/associate.txt");
    if (!fin)
    {
        cout << "No data association file" << endl;
        return 1;
    }

    vector<string> rgb_files, depth_files;
    vector<double> rgb_times, depth_times;

    while (!fin.eof())
    {
        string rgb_time, rgb_file, depth_time, depth_file;
        fin >> rgb_time >> rgb_file >> depth_time >> depth_file;
        rgb_times.push_back(atof(rgb_time.c_str()));
        depth_times.push_back(atof(depth_time.c_str()));
        rgb_files.push_back(dataset_dir + "/" + rgb_file);
        depth_files.push_back(dataset_dir + "/" + depth_file);

        if (fin.good() == true)
            break;
    }

    myslam::Camera::Ptr camera(new myslam::Camera);

    cv::viz::Viz3d vis("Visual Odometry");
    cv::viz::WCoordinateSystem world_coor(1.0), camera_coor(0.5);
    cv::Point3d cam_pos(0, -1, -1), camera_focal_point(0, 0, 0), cam_y_dir(0, 1, 0);
    cv::Affine3d cam_pose = cv::viz::makeCameraPose(cam_pos, camera_focal_point, cam_y_dir);
    vis.setViewerPose(cam_pose);

    world_coor.setRenderingProperty(cv::viz::LINE_WIDTH, 2.0);
    camera_coor.setRenderingProperty(cv::viz::LINE_WIDTH, 1.0);

    vis.showWidget("World", world_coor);
    vis.showWidget("Camera", camera_coor);

    cout << "read RGB files: " << rgb_files.size() << endl;

    for(int i = 0;i<rgb_files.size();i++){
        Mat color = cv::imread(rgb_files[i]);
        Mat depth = cv::imread(depth_files[i], -1);

        if(color.data == nullptr || depth.data == nullptr)
            break;

        myslam::Frame::Ptr pFrame = myslam::Frame::createFrame();
        pFrame->camera_ = camera;
        pFrame->color_ = color;
        pFrame->depth_ = depth;
        pFrame->time_stamp_ = rgb_times[i];

        boost::timer timer;
        vo->addFrame(pFrame);
        cout << "VO add frame time cost: " << timer.elapsed() << endl;

        if(vo->state_ == myslam::VisualOdometry::LOST)
            break;

        SE3 T_c_w = pFrame->T_w_c_.inverse(); //  pFrame->T_c_w_: transform from world to camera
        cv::Affine3d M(
            cv::Affine3d::Mat3(
                T_c_w.rotation_matrix()(0,0), T_c_w.rotation_matrix()(0,1), T_c_w.rotation_matrix()(0,2),
                T_c_w.rotation_matrix()(1,0), T_c_w.rotation_matrix()(1,1), T_c_w.rotation_matrix()(1,2),
                T_c_w.rotation_matrix()(2,0), T_c_w.rotation_matrix()(2,1), T_c_w.rotation_matrix()(2,2)
            ),
            cv::Affine3d::Vec3(
                T_c_w.translation()(0,0), T_c_w.translation()(1,0), T_c_w.translation()(2,0)

            )
        );

        cv::imshow("image", color);
        cv::waitKey(1);
        vis.setWidgetPose("camera", M);
        vis.spinOnce();
    }
}


#include <franka/robot.h>
#include <franka/model.h>
#include <pinocchio/parsers/urdf.hpp>
#include <pinocchio/algorithm/rnea.hpp>
#include <pinocchio/algorithm/kinematics.hpp>
#include <Eigen/Dense>
#include <sys/types.h>
#include <sys/stat.h>
#include <unistd.h>
#include <iostream>
#include <limits>
#include <algorithm>

struct GravityCase
{
    Eigen::Matrix<double, 7, 1> q = Eigen::Matrix<double, 7, 1>::Zero();
    Eigen::Matrix<double, 3, 1> g = Eigen::Matrix<double, 3, 1>::Zero();
    Eigen::Matrix<double, 7, 1> gravity = Eigen::Matrix<double, 7, 1>::Zero();
};

struct CoriolisCase
{
    Eigen::Matrix<double, 7, 1> q = Eigen::Matrix<double, 7, 1>::Zero();
    Eigen::Matrix<double, 7, 1> dq = Eigen::Matrix<double, 7, 1>::Zero();
    Eigen::Matrix<double, 7, 1> coriolis = Eigen::Matrix<double, 7, 1>::Zero();
};

std::array<double, 3> array(const Eigen::Matrix<double, 3, 1> &v)
{
    std::array<double, 3> r;
    Eigen::Matrix<double, 3, 1>::Map(&r[0]) = v;
    return r;
}

std::array<double, 7> array(const Eigen::Matrix<double, 7, 1> &v)
{
    std::array<double, 7> r;
    Eigen::Matrix<double, 7, 1>::Map(&r[0]) = v;
    return r;
}

Eigen::Matrix<double, 7, 1> vector(const std::array<double, 7> &v)
{
    return Eigen::Matrix<double, 7, 1>::Map(&v[0]);
}

Eigen::Matrix<double, 7, 1> compute_emulator_gravity(pinocchio::Model &model, pinocchio::Data &data, const Eigen::Matrix<double, 7, 1> &q, const Eigen::Matrix<double, 3, 1> &gravity)
{
    model.gravity.linear_impl() = gravity;
    return pinocchio::computeGeneralizedGravity(model, data, q);
}

Eigen::Matrix<double, 7, 1> compute_emulator_coriolis(pinocchio::Model &model, pinocchio::Data &data, const Eigen::Matrix<double, 7, 1> &q, const Eigen::Matrix<double, 7, 1> &dq)
{
    return pinocchio::computeCoriolisMatrix(model, data, q, dq) * dq;
}

int _main(int argc, char **argv)
{
    if (argc != 2)
    {
        std::cout << "Usage: ./franka_emulator_library_calibrator <IP>" << std::endl;
        return 1;
    }

    //Init robot
    franka::Robot real_robot(argv[1]);
    franka::Model real_model = real_robot.loadModel();

    //Init model
    pinocchio::Model emulator_model;
    struct stat model_stat;
    if (stat("model/franka_emulator.urdf", &model_stat) == 0) pinocchio::urdf::buildModel("model/franka_emulator.urdf", emulator_model);
    else if (stat("../model/franka_emulator.urdf", &model_stat) == 0) pinocchio::urdf::buildModel("../model/franka_emulator.urdf", emulator_model);
    else throw std::runtime_error("Could not find model file");
    pinocchio::Data emulator_data(emulator_model);

    //Generating test cases
    std::vector<GravityCase> gravity_cases;
    std::vector<CoriolisCase> coriolis_cases;
    gravity_cases.reserve(3 * 3*3*3*3*3*3*3);
    coriolis_cases.reserve(7 * 3*3*3*3*3*3*3);
    for (int j0 = -1; j0 <= 1; j0++)
    for (int j1 = -1; j1 <= 1; j1++)
    for (int j2 = -1; j2 <= 1; j2++)
    for (int j3 = -1; j3 <= 1; j3++)
    for (int j4 = -1; j4 <= 1; j4++)
    for (int j5 = -1; j5 <= 1; j5++)
    for (int j6 = -1; j6 <= 1; j6++)
    {
        Eigen::Matrix<double, 7, 1> q;
        q(0) = j0 * M_PI / 4;
        q(1) = j1 * M_PI / 4;
        q(2) = j2 * M_PI / 4;
        q(3) = -M_PI/2 + j3 * M_PI/4;
        q(4) = j4 * M_PI / 4;
        q(5) = M_PI/4 + j5 * M_PI/4;
        q(6) = j6 * M_PI / 4;
        for (int g = 0; g < 3; g++)
        {
            GravityCase cas;
            cas.q = q;
            cas.g(g) = -10.0;
            cas.gravity = vector(real_model.gravity(array(q), 0.0, std::array<double, 3>({0,0,0}), array(cas.g)));
            gravity_cases.push_back(cas);
        }
        for (int c = 0; c < 7; c++)
        {
            CoriolisCase cas;
            cas.q = q;
            cas.dq(c) = 1.0;
            cas.coriolis = vector(real_model.coriolis(array(q), array(cas.dq), std::array<double, 9>({0,0,0,0,0,0,0,0,0}), 0.0, std::array<double, 3>({0,0,0})));
            coriolis_cases.push_back(cas);
        }
    }

    //Extracting parametes
    const unsigned int nparameters = 10;
    const unsigned int mass0 = 0;
    const unsigned int inertia0 = 1;
    const unsigned int position0 = 7;
    Eigen::VectorXd parameters(7*nparameters); //7 links * (mass + 6 inertia + 3 position)
    for (unsigned int link = 0; link < 7; link++)
    {
        parameters(nparameters*link + mass0) = emulator_model.inertias[link+1].mass();
        for (unsigned int inertia = 0; inertia < 6; inertia++) parameters(nparameters*link + inertia0 + inertia) = emulator_model.inertias[link+1].inertia().data()[inertia];
        for (unsigned int position = 0; position < 3; position++) parameters(nparameters*link + position0 + position) = emulator_model.inertias[link+1].lever()[position];
    }
    Eigen::VectorXd previous_parameters = parameters;
    double previous_error_norm = std::numeric_limits<double>::quiet_NaN();

    //Run Newton method
    const unsigned int gravity_error0 = 0;
    const unsigned int coriolis_error0 = 7*gravity_cases.size();
    Eigen::VectorXd error(7*gravity_cases.size() + 7*coriolis_cases.size());
    Eigen::MatrixXd derivative(7*gravity_cases.size() + 7*coriolis_cases.size(), 7*nparameters);
    error.setZero();
    derivative.setZero();
    for (unsigned int step = 0;; step++)
    {
        //Computing error
        std::cout << "Calibration step " << step << std::endl;
        std::cout << "Computing error of gravity()" << std::endl;
        for (unsigned int cas = 0; cas < gravity_cases.size(); cas++)
        {
            Eigen::Matrix<double, 7, 1> emulator_gravity = compute_emulator_gravity(emulator_model, emulator_data, gravity_cases[cas].q, gravity_cases[cas].g);
            error.block<7,1>(gravity_error0 + 7*cas,0) = emulator_gravity - gravity_cases[cas].gravity;
        }
        std::cout << "Computing error of coriolis()" << std::endl;
        for (unsigned int cas = 0; cas < coriolis_cases.size(); cas++)
        {
            Eigen::Matrix<double, 7, 1> emulator_coriolis = compute_emulator_coriolis(emulator_model, emulator_data, coriolis_cases[cas].q, coriolis_cases[cas].dq);
            error.block<7,1>(coriolis_error0 + 7*cas,0) = emulator_coriolis - coriolis_cases[cas].coriolis;
        }
        std::cout << "Error 2-norm:        " << error.norm() << std::endl;
        std::cout << "Error 1-norm:        " << error.lpNorm<1>() << std::endl;
        std::cout << "Error infinity-norm: " << error.lpNorm<Eigen::Infinity>() << std::endl;
        if (error.norm() >= previous_error_norm) break;
        previous_parameters = parameters;
        previous_error_norm = error.norm();

        //Computing derivative
        std::cout << "Computing derivative of gravity() in mass" << std::endl;
        for (unsigned int cas = 0; cas < gravity_cases.size(); cas++)
            for (unsigned int link = 0; link < 7; link++)
            {
                const double mass_epsillon = 0.01;
                double previous_mass = emulator_model.inertias[link+1].mass();
                emulator_model.inertias[link+1].mass() = previous_mass + mass_epsillon;
                Eigen::Matrix<double, 7, 1> upper = compute_emulator_gravity(emulator_model, emulator_data, gravity_cases[cas].q, gravity_cases[cas].g);
                emulator_model.inertias[link+1].mass() = std::max(previous_mass - mass_epsillon, 0.0);
                Eigen::Matrix<double, 7, 1> lower = compute_emulator_gravity(emulator_model, emulator_data, gravity_cases[cas].q, gravity_cases[cas].g);
                emulator_model.inertias[link+1].mass() = previous_mass;
                derivative.block<7,1>(gravity_error0 + 7*cas, nparameters*link + mass0) = (upper - lower) / (mass_epsillon + std::min(mass_epsillon, previous_mass));
            }

        std::cout << "Computing derivative of gravity() in position" << std::endl;
        for (unsigned int cas = 0; cas < gravity_cases.size(); cas++)
            for (unsigned int link = 0; link < 7; link++)
                for (unsigned int position = 0; position < 3; position++)
                {
                    const double position_epsillon = 0.01;
                    double previous_position = emulator_model.inertias[link+1].lever()[position];
                    emulator_model.inertias[link+1].lever()[position] = previous_position + position_epsillon;
                    Eigen::Matrix<double, 7, 1> upper = compute_emulator_gravity(emulator_model, emulator_data, gravity_cases[cas].q, gravity_cases[cas].g);
                    emulator_model.inertias[link+1].lever()[position] = previous_position - position_epsillon;
                    Eigen::Matrix<double, 7, 1> lower = compute_emulator_gravity(emulator_model, emulator_data, gravity_cases[cas].q, gravity_cases[cas].g);
                    emulator_model.inertias[link+1].lever()[position] = previous_position;
                    derivative.block<7,1>(gravity_error0 + 7*cas, nparameters*link + position0 + position) = (upper - lower) / (2 * position_epsillon);
                }

        std::cout << "Computing derivative of coriolis() in mass" << std::endl;
        for (unsigned int cas = 0; cas < coriolis_cases.size(); cas++)
            for (unsigned int link = 0; link < 7; link++)
            {
                const double mass_epsillon = 0.01;
                double previous_mass = emulator_model.inertias[link+1].mass();
                emulator_model.inertias[link+1].mass() = previous_mass + mass_epsillon;
                Eigen::Matrix<double, 7, 1> upper = compute_emulator_coriolis(emulator_model, emulator_data, coriolis_cases[cas].q, coriolis_cases[cas].dq);
                emulator_model.inertias[link+1].mass() = std::max(previous_mass - mass_epsillon, 0.0);
                Eigen::Matrix<double, 7, 1> lower = compute_emulator_coriolis(emulator_model, emulator_data, coriolis_cases[cas].q, coriolis_cases[cas].dq);
                emulator_model.inertias[link+1].mass() = previous_mass;
                derivative.block<7,1>(coriolis_error0 + 7*cas, nparameters*link + mass0) = (upper - lower) / (mass_epsillon + std::min(mass_epsillon, previous_mass));
            }

        std::cout << "Computing derivative of coriolis() in inertia" << std::endl;
        for (unsigned int cas = 0; cas < coriolis_cases.size(); cas++)
            for (unsigned int link = 0; link < 7; link++)
                for (unsigned int inertia = 0; inertia < 6; inertia++)
                {
                    const double inertia_epsillon = 0.01;
                    bool central = inertia == 0 || inertia == 2 || inertia == 5;
                    double previous_inertia = emulator_model.inertias[link+1].inertia().data()[inertia];
                    emulator_model.inertias[link+1].inertia().data()[inertia] = previous_inertia + inertia_epsillon;
                    Eigen::Matrix<double, 7, 1> upper = compute_emulator_coriolis(emulator_model, emulator_data, coriolis_cases[cas].q, coriolis_cases[cas].dq);
                    emulator_model.inertias[link+1].inertia().data()[inertia] = (central ? std::max(previous_inertia - inertia_epsillon, 0.0) : (previous_inertia - inertia_epsillon));
                    Eigen::Matrix<double, 7, 1> lower = compute_emulator_coriolis(emulator_model, emulator_data, coriolis_cases[cas].q, coriolis_cases[cas].dq);
                    emulator_model.inertias[link+1].inertia().data()[inertia] = previous_inertia;
                    derivative.block<7,1>(coriolis_error0 + 7*cas, nparameters*link + inertia0 + inertia) = (upper - lower) / (inertia_epsillon + (central ? std::min(inertia_epsillon, previous_inertia) : inertia_epsillon));
                }

        std::cout << "Computing derivative of coriolis() in position" << std::endl;
        for (unsigned int cas = 0; cas < coriolis_cases.size(); cas++)
            for (unsigned int link = 0; link < 7; link++)
                for (unsigned int position = 0; position < 3; position++)
                {
                    const double position_epsillon = 0.01;
                    double previous_position = emulator_model.inertias[link+1].lever()[position];
                    emulator_model.inertias[link+1].lever()[position] = previous_position + position_epsillon;
                    Eigen::Matrix<double, 7, 1> upper = compute_emulator_coriolis(emulator_model, emulator_data, coriolis_cases[cas].q, coriolis_cases[cas].dq);
                    emulator_model.inertias[link+1].lever()[position] = previous_position - position_epsillon;
                    Eigen::Matrix<double, 7, 1> lower = compute_emulator_coriolis(emulator_model, emulator_data, coriolis_cases[cas].q, coriolis_cases[cas].dq);
                    emulator_model.inertias[link+1].lever()[position] = previous_position;
                    derivative.block<7,1>(coriolis_error0 + 7*cas, nparameters*link + position0 + position) = (upper - lower) / (2 * position_epsillon);
                }

        //Updating parameters
        std::cout << "Computing parameters for next step" << std::endl;
        parameters -= derivative.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(error);
        std::cout << "Applying parameters" << std::endl;
        for (unsigned int link = 0; link < 7; link++)
        {
            if (parameters(nparameters*link + mass0) < 0.0) parameters(nparameters*link + mass0) = 0.0;
            emulator_model.inertias[link+1].mass() = parameters(nparameters*link + mass0);
            for (unsigned int inertia = 0; inertia < 6; inertia++)
            {
                bool central = inertia == 0 || inertia == 2 || inertia == 5;
                if (central && parameters(nparameters*link + inertia0 + inertia) < 0.0) parameters(nparameters*link + inertia0 + inertia) = 0.0;
                emulator_model.inertias[link+1].inertia().data()[inertia] = parameters(nparameters*link + inertia0 + inertia);
            }
            for (unsigned int position = 0; position < 3; position++) emulator_model.inertias[link+1].lever()[position] = parameters(nparameters*link + position0 + position);
        }
    }

    //Printing parameters
    std::cout << "Calibration finished" << std::endl;
    std::cout.precision(std::numeric_limits<double>::max_digits10);
    for (unsigned int link = 0; link < 7; link++)
    {
        std::cout << "<link name=\"panda_link" << link+1 << "\">" << std::endl;

        std::cout << "<origin rpy=\"0 0 0\" xyz=\"";
        std::cout << previous_parameters(nparameters*link + position0) << " ";
        std::cout << previous_parameters(nparameters*link + position0 + 1) << " ";
        std::cout << previous_parameters(nparameters*link + position0 + 2) << "\"/>" << std::endl;

        std::cout << "<mass value=\"" << previous_parameters(nparameters*link + mass0) << "\"/>" << std::endl;

        std::cout << "<inertia ";
        std::cout << "ixx=\"" << previous_parameters(nparameters*link + inertia0) << "\" ";
        std::cout << "ixy=\"" << previous_parameters(nparameters*link + inertia0 + 1) << "\" ";
        std::cout << "ixz=\"" << previous_parameters(nparameters*link + inertia0 + 3) << "\" ";
        std::cout << "iyy=\"" << previous_parameters(nparameters*link + inertia0 + 2) << "\" ";
        std::cout << "iyz=\"" << previous_parameters(nparameters*link + inertia0 + 4) << "\" ";
        std::cout << "izz=\"" << previous_parameters(nparameters*link + inertia0 + 5) << "\"/>" << std::endl;

        std::cout << std::endl;
    }
    return 0;
}

int main(int argc, char **argv)
{
    try
    {
        return _main(argc, argv);
    }
    catch (std::exception &e)
    {
        std::cout << "Exception occured: " << e.what() << std::endl;
        return 1;
    }
}