#include "nexus/backend/caffe_model.h"

#include <glog/logging.h>

#include <boost/filesystem.hpp>
#include <boost/make_shared.hpp>
#include <fstream>
#include <opencv2/opencv.hpp>
#include <sstream>

#include "nexus/backend/slice.h"
#include "nexus/backend/utils.h"
#include "nexus/common/image.h"
#include "nexus/proto/control.pb.h"

namespace fs = boost::filesystem;

namespace nexus {
namespace backend {

CaffeModel::CaffeModel(int gpu_id, const ModelInstanceConfig& config)
    : ModelInstance(gpu_id, config) {
  CHECK(model_info_["cfg_file"]) << "Missing cfg_file in the model info";
  CHECK(model_info_["weight_file"]) << "Missing weight_file in the model info";
  CHECK(model_info_["mean_file"] || model_info_["mean_value"])
      << "Missing mean_file or mean_value in the model info";
  // load caffe model
  fs::path model_dir = fs::path(model_info_["model_dir"].as<std::string>());
  fs::path cfg_path = model_dir / model_info_["cfg_file"].as<std::string>();
  fs::path weight_path =
      model_dir / model_info_["weight_file"].as<std::string>();
  CHECK(fs::exists(cfg_path)) << "cfg file " << cfg_path << " doesn't exist";
  CHECK(fs::exists(weight_path))
      << "weight file " << weight_path << " doesn't exist";

  // init gpu device
  caffe::Caffe::SetDevice(gpu_id);
  caffe::Caffe::set_mode(caffe::Caffe::GPU);

  // load network
  net_.reset(new caffe::ServeNet<float>(cfg_path.string(), max_batch_));
  net_->CopyTrainedLayersFrom(weight_path.string());
  // get input and output shape
  // NOTE: currently we only consider single input and single output
  CHECK_EQ(net_->num_inputs(), 1)
      << "CaffeModel only support caffe model that has single input";
  CHECK_EQ(net_->num_outputs(), 1)
      << "CaffeModel only support caffe model that has single output";
  input_shape_.set_dims(std::vector<int>(net_->input_blobs()[0]->shape()));
  output_shape_.set_dims(std::vector<int>(net_->output_blobs()[0]->shape()));
  input_blob_idx_ = net_->input_blob_indices()[0];
  output_blob_name_ = net_->blob_names()[net_->output_blob_indices()[0]];

  // get the single input and output size
  input_size_ = input_shape_.NumElements(1);
  output_size_ = output_shape_.NumElements(1);
  image_height_ = input_shape_.dim(2);
  image_width_ = input_shape_.dim(3);

  LOG(INFO) << "Model " << model_session_id_ << ": input shape " << input_shape_
            << " (" << input_size_ << "), output shape " << output_shape_
            << " (" << output_size_ << ")";

  // Set up data transformer
  caffe::TransformationParameter transform_param;
  if (model_info_["scale"]) {
    transform_param.set_scale(model_info_["scale"].as<float>());
  }
  transform_param.set_crop_size(image_height_);
  if (model_info_["mean_file"]) {
    fs::path mean_file = model_dir / model_info_["mean_file"].as<std::string>();
    transform_param.set_mean_file(mean_file.string());
  } else {
    const YAML::Node& mean_values = model_info_["mean_value"];
    CHECK(mean_values.IsSequence())
        << "mean_value in the config is not sequence";
    for (uint i = 0; i < mean_values.size(); ++i) {
      transform_param.add_mean_value(mean_values[i].as<float>());
    }
  }
  transformer_.reset(
      new caffe::DataTransformer<float>(transform_param, net_->phase()));

  // whether enbable prefix batching
  if (model_info_["prefix_layer"]) {
    prefix_layer_ = model_info_["prefix_layer"].as<std::string>();
    prefix_index_ = net_->layer_index_by_name(prefix_layer_);
    LOG(INFO) << "Prefix layer up to " << prefix_layer_ << "(" << prefix_index_
              << ")";
  } else {
    prefix_layer_ = "";
    prefix_index_ = -1;
  }
  // load classnames
  if (model_info_["class_names"]) {
    fs::path cns_path =
        model_dir / model_info_["class_names"].as<std::string>();
    LoadClassnames(cns_path.string(), &classnames_);
  }
}

Shape CaffeModel::InputShape() { return input_shape_; }

std::unordered_map<std::string, Shape> CaffeModel::OutputShapes() {
  return {{output_blob_name_, output_shape_}};
}

ArrayPtr CaffeModel::CreateInputGpuArray() {
  boost::shared_ptr<caffe::Blob<float> > blob;
  if (input_blobs_.empty()) {
    blob = net_->blobs()[input_blob_idx_];
  } else {
    blob = boost::make_shared<caffe::Blob<float> >(input_shape_.dims());
  }
  size_t nfloats = max_batch_ * input_size_;
  auto buf = std::make_shared<Buffer>(blob->mutable_gpu_data(),
                                      nfloats * sizeof(float), gpu_device_);
  auto arr = std::make_shared<Array>(DT_FLOAT, nfloats, buf);
  arr->set_tag(input_blobs_.size());
  input_blobs_.push_back(blob);
  return arr;
}

std::unordered_map<std::string, ArrayPtr> CaffeModel::GetOutputGpuArrays() {
  size_t nfloats = max_batch_ * output_size_;
  auto blob = net_->output_blobs()[0];
  auto buf = std::make_shared<Buffer>(blob->mutable_gpu_data(),
                                      nfloats * sizeof(float), gpu_device_);
  auto arr = std::make_shared<Array>(DT_FLOAT, nfloats, buf);
  return {{output_blob_name_, arr}};
}

void CaffeModel::Preprocess(std::shared_ptr<Task> task) {
  auto prepare_image = [&](cv::Mat& image) {
    auto in_arr = std::make_shared<Array>(DT_FLOAT, input_size_, cpu_device_);
    cv::Mat resized_image;
    cv::resize(image, resized_image, cv::Size(image_width_, image_height_));
    std::vector<int> blob_shape = input_shape_.dims();
    blob_shape[0] = 1;
    caffe::Blob<float> blob(blob_shape);
    blob.data()->set_cpu_data(in_arr->Data<void>());
    transformer_->Transform(resized_image, &blob);
    task->AppendInput(in_arr);
  };

  const auto& query = task->query;
  const auto& input_data = query.input();
  switch (input_data.data_type()) {
    case DT_IMAGE: {
      cv::Mat cv_img_bgr = DecodeImage(input_data.image(), CO_BGR);
      if (query.window_size() > 0) {
        for (int i = 0; i < query.window_size(); ++i) {
          const auto& rect = query.window(i);
          cv::Mat crop_img = cv_img_bgr(cv::Rect(rect.left(), rect.top(),
                                                 rect.right() - rect.left(),
                                                 rect.bottom() - rect.top()));
          prepare_image(crop_img);
        }
      } else {
        prepare_image(cv_img_bgr);
      }
      break;
    }
    default:
      task->result.set_status(INPUT_TYPE_INCORRECT);
      task->result.set_error_message("Input type incorrect: " +
                                     DataType_Name(input_data.data_type()));
      break;
  }
}

void CaffeModel::Forward(std::shared_ptr<BatchTask> batch_task) {
  auto blob = input_blobs_[batch_task->GetInputArray()->tag()];
  // reshape input blob to current batch size
  size_t batch = batch_task->batch_size();
  std::vector<int> input_shape = input_shape_.dims();
  input_shape[0] = batch;
  blob->Reshape(input_shape);
  // Replace input blob in the network by the corresponding blob
  net_->set_blob(input_blob_idx_, blob);
  // We don't need to reshape the network, because during the forwarding
  // Caffe will reshape every layers based on the input batch size
  const caffe::Blob<float>* output_blob = net_->Forward()[0];
  // Copy to output array in the batch_task
  auto out_arr = batch_task->GetOutputArray(output_blob_name_);
  Memcpy(out_arr->Data<void>(), cpu_device_, output_blob->gpu_data(),
         gpu_device_, output_blob->count() * sizeof(float));
  batch_task->SliceOutputBatch(
      {{output_blob_name_, Slice(batch, output_size_)}});
}

void CaffeModel::Postprocess(std::shared_ptr<Task> task) {
  const QueryProto& query = task->query;
  QueryResultProto* result = &task->result;
  result->set_status(CTRL_OK);
  for (auto& output : task->outputs) {
    auto out_arr = output->arrays.at(output_blob_name_);
    float* out_data = out_arr->Data<float>();
    if (type() == "classification") {
      if (classnames_.empty()) {
        PostprocessClassification(query, out_data, output_size_, result);
      } else {
        PostprocessClassification(query, out_data, output_size_, result,
                                  &classnames_);
      }
    } else {
      std::ostringstream oss;
      oss << "Unsupported model type " << type() << " for " << framework();
      result->set_status(MODEL_TYPE_NOT_SUPPORT);
      result->set_error_message(oss.str());
      break;
    }
  }
}

void CaffeModel::LoadClassnames(const std::string& filepath) {
  std::ifstream fs(filepath);
  CHECK(fs.good()) << "Classname file " << filepath << " doesn't exist";
  std::string line;
  while (std::getline(fs, line)) {
    classnames_.push_back(line);
  }
  fs.close();
}

}  // namespace backend
}  // namespace nexus


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

// ---
// Author: Zachary Kann

// Implementation of ParticleGroup class. See include/z_atom_group.hpp for
// more details about the class.

#include "z_particle_group.hpp"
#include <boost/lexical_cast.hpp>
#include <boost/format.hpp>
#include "z_string.hpp"
#include "z_vec.hpp"
#include <algorithm>

void ParticleGroup::CalculateElectricField(const ParticleGroup& other_group,
                              const arma::rowvec& box,
                              const arma::imat& nearby_molecules,
                              arma::rowvec& dx) {
  for (int i_other = 0; i_other < other_group.size(); ++i_other) {
    if (included_in_field_[other_group.indices(i_other)])
      continue;
    else
      included_in_field_[other_group.indices(i_other)] = true;
    for (int i_atom = 0; i_atom < group_size_; ++i_atom) {
      if (nearby_molecules(i_atom, other_group.index_to_molecule(i_other)) == 0)
        continue;
      //Skip neutral atoms
      if (std::abs(other_group.charge(i_other) < 0.0001)) continue;
      FindDxNoShift(dx, position(i_atom), other_group.position(i_other), box);
      double r2 = arma::dot(dx,dx);
      electric_fields_.row(i_atom) +=
          other_group.charge(i_other)*dx/r2/std::sqrt(r2);
    }
  }
}

void ParticleGroup::CalculateElectricField(const ParticleGroup& other_group,
                              const arma::rowvec& box,
                              const double cutoff_squared,
                              arma::rowvec& dx) {
  for (int i_other = 0; i_other < other_group.size(); ++i_other) {
    if (included_in_field_[other_group.indices(i_other)])
      continue;
    else
      included_in_field_[other_group.indices(i_other)] = true;
    for (int i_atom = 0; i_atom < group_size_; ++i_atom) {
      // Ignore contribution from same molecule
      if (other_group.index_to_molecule(i_other) == index_to_molecule(i_other))
        continue;
      //Skip neutral atoms
      if (std::abs(other_group.charge(i_other) < 0.0001)) continue;
      FindDxNoShift(dx, position(i_atom), other_group.position(i_other), box);
      double r2 = arma::dot(dx,dx);
      if (r2 > cutoff_squared) continue;
      electric_fields_.row(i_atom) +=
          other_group.charge(i_other)*dx/r2/std::sqrt(r2);
    }
  }
}

void ParticleGroup::MarkNearbyAtoms(const ParticleGroup& other_group,
                                const arma::rowvec& box,
                                const double cutoff_squared,
                                arma::imat& nearby_atoms) const {
  nearby_atoms = arma::zeros<arma::imat>(group_size_,
                                         other_group.num_molecules());
  arma::rowvec dx;
  for (int i_atom = 0; i_atom < group_size_; ++i_atom) {
    const int molecule_number = index_to_molecule_[i_atom];
    for (std::vector<int>::const_iterator i_other =
             other_group.molecule_position_markers_.begin();
         i_other != other_group.molecule_position_markers_.end(); ++i_other) {
      if (molecule_number == other_group.index_to_molecule(*i_other)) continue;
      FindDxNoShift(dx, position(i_atom), other_group.position(*i_other), box);
      double r2 = arma::dot(dx,dx);
      if (r2 < cutoff_squared)
        nearby_atoms(i_atom,*i_other) = 1;
    }
  }
}

void ParticleGroup::FindClusters(const double cutoff_distance_squared,
                             const arma::rowvec& box,
                             Histogram& clusters) const {
  arma::irowvec in_current_cluster;
  arma::irowvec in_any_cluster = arma::zeros<arma::irowvec>(group_size_);
  arma::rowvec dx;
  for (int i_atom = 0; i_atom < group_size_; ++i_atom) {
    if (in_any_cluster(i_atom)) continue;
    in_current_cluster.zeros();
    in_current_cluster(i_atom) = 1;
    in_any_cluster(i_atom) = 1;
    for (int i_other = 0; i_other < group_size_; ++i_other) {
      if (in_any_cluster(i_other)) continue;
      FindDxNoShift(dx, position(i_atom), position(i_other), box);
      double r2 = arma::dot(dx,dx);
      if (r2 > cutoff_distance_squared) continue;
      in_current_cluster(i_other) = 1;
      in_any_cluster(i_other) = 1;
      FindCluster(cutoff_distance_squared, box, dx, in_current_cluster,
                  in_any_cluster, i_other);
    }
    clusters.Add(arma::sum(in_current_cluster));
  }
}

void ParticleGroup::FindNearestk(const arma::rowvec& point, const arma::rowvec& box,
                             const int exclude_molecule, const int k,
                             arma::rowvec& dx, arma::rowvec& r2,
                             arma::irowvec& nearest) const {
  for (int i_atom = 0; i_atom < group_size_; ++i_atom) {
    if (index_to_molecule_[i_atom] == exclude_molecule) {
      r2(i_atom) = std::numeric_limits<double>::max();
    } else {
      FindDxNoShift(dx, point, positions_.row(i_atom), box);
      r2(i_atom) = arma::dot(dx,dx);
    }
  }
  arma::uword min_index;
  for (int i = 0; i < k; ++i) {
    r2.min(min_index);
    r2(min_index) = std::numeric_limits<double>::max();
    nearest(i) = min_index;
  }
}

void ParticleGroup::FindCluster(const double cutoff_distance_squared,
                             const arma::rowvec& box, arma::rowvec& dx,
                             arma::irowvec& in_current_cluster,
                             arma::irowvec& in_any_cluster,
                             const int start_atom) const {
  for (int i_other = start_atom + 1; i_other < group_size_; ++i_other) {
    if (in_any_cluster(i_other)) continue;
    FindDxNoShift(dx, position(start_atom), position(i_other), box);
    double r2 = arma::dot(dx,dx);
    if (r2 > cutoff_distance_squared) continue;
    in_current_cluster(i_other) = 1;
    in_any_cluster(i_other) = 1;
    FindCluster(cutoff_distance_squared, box, dx, in_current_cluster,
                  in_any_cluster, i_other);
  }
}


// Distributed under the MIT License.
// See LICENSE.txt for details.

#pragma once

#include <boost/functional/hash.hpp>
#include <cstddef>
#include <map>
#include <optional>
#include <tuple>
#include <type_traits>
#include <utility>

#include "DataStructures/FixedHashMap.hpp"
#include "Domain/Structure/Direction.hpp"
#include "Domain/Structure/ElementId.hpp"
#include "Domain/Structure/MaxNumberOfNeighbors.hpp"
#include "NumericalAlgorithms/Spectral/Mesh.hpp"
#include "Parallel/InboxInserters.hpp"
#include "Time/TimeStepId.hpp"
#include "Utilities/TMPL.hpp"

namespace evolution::dg::Tags {
/*!
 * \brief The inbox tag for boundary correction communication and DG-subcell
 * ghost zone cells.
 *
 * The stored data consists of the following (in argument order of the tuple):
 *
 * 1. the mesh of the neighboring element's face (not the mortar mesh!)
 * 2. the variables at the ghost zone cells for finite difference/volume
 *    reconstruction
 * 3. the data on the mortar needed for computing the boundary corrections (e.g.
 *    fluxes, characteristic speeds, conserved variables)
 * 4. the TimeStepId beyond which the boundary terms are no longer valid, when
 *    using local time stepping.
 *
 * The TimeStepId is the neighboring element's next time step. When using local
 * time stepping, the neighbor's boundary data is valid up until this time,
 * which may include multiple local time steps. By receiving and storing the
 * neighbor time step, the local element knows whether or not it should remove
 * boundary data and expect new data to be sent from the neighbor.
 *
 * The ghost cell data (second element of the tuple) will be valid whenever a
 * DG-subcell scheme is being used. Whenever a DG-subcell scheme is being used,
 * elements using DG and not FD/FV always send both the ghost cells and boundary
 * correction data together. Elements using FD/FV send the ghost cells first
 * followed by the boundary correction data once the element has received all
 * neighbor ghost cell data. Note that the second send/receive only modifies the
 * flux and the TimeStepId used for the flux validity range.
 *
 * When only a DG scheme (not a DG-subcell scheme) is used the ghost cell data
 * will never be valid.
 *
 * In the DG-subcell scheme this tag is used both for communicating the ghost
 * cell data needed for the FD/FV reconstruction step and the data needed for
 * the boundary corrections.
 * - For an element using DG, both ghost cells and boundary corrections are
 *   sent using a single communication. After receiving all neighbor
 *   boundary corrections the element can finish computing the time step.
 *   The ghost cell data from neighbors is unused.
 * - For an element using FD/FV, first the ghost cells are sent. Once all
 *   neighboring ghost cell data is received, reconstruction is done and the
 *   boundary terms are computed and sent to the neighbors. After receiving all
 *   neighbor boundary corrections the element can finish computing the time
 *   step.
 * - Whether or not an extra communication is needed when an element switches
 *   from DG to FD/FV depends on how exactly the decision to switch is
 *   implemented. If the volume terms are integrated and verified to be
 *   valid before a DG element sends ghost cell and bonudary data then no
 *   additional communication is needed when switching from DG to FD/FV. In this
 *   case a second check of the data that includes the boundary correction needs
 *   to be done. If the second check determines a switch from DG to FD/FV is
 *   needed, we can continue to use the DG fluxes since the evolution in the
 *   small was valid, thus avoiding an additional communication. However, to
 *   fully guarantee physical realizability a second communication or evolving
 *   the neighboring ghost cells needs to be done. We have not yet decided how
 *   to deal with the possible need for an additional communication since it
 *   also depends on whether or not we decide to couple to Voronoi instead of
 *   just Neumann neighbors.
 * - The data for the inbox tags is erased after the boundary correction is
 *   complete and the solution has been verified to be valid at the new time
 *   step. The ghost cells could be invalidated immediately after
 *   reconstruction, thus using the ghost cell data after reconstruction is
 *   complete is considered undefined behavior. That is, we make no guarantee as
 *   to whether or not it will work.
 * - The reason for minimizing the number of communications rather than having a
 *   more uniform implementation between DG and FD/FV is that it is the number
 *   of communications that adds the most overhead, not the size of each
 *   communication. Thus, one large communication is cheaper than several small
 *   communications.
 */
template <size_t Dim>
struct BoundaryCorrectionAndGhostCellsInbox {
  using stored_type =
      std::tuple<Mesh<Dim - 1>, std::optional<std::vector<double>>,
                 std::optional<std::vector<double>>, ::TimeStepId>;

 public:
  using temporal_id = TimeStepId;
  using type = std::map<
      TimeStepId,
      FixedHashMap<maximum_number_of_neighbors(Dim),
                   std::pair<Direction<Dim>, ElementId<Dim>>, stored_type,
                   boost::hash<std::pair<Direction<Dim>, ElementId<Dim>>>>>;

  template <typename Inbox, typename ReceiveDataType>
  static void insert_into_inbox(const gsl::not_null<Inbox*> inbox,
                                const temporal_id& time_step_id,
                                ReceiveDataType&& data) {
    auto& current_inbox = (*inbox)[time_step_id];
    auto& [mesh, ghost_cell_data, boundary_data, boundary_data_validity_range] =
        data.second;
    (void)ghost_cell_data;

    if (auto it = current_inbox.find(data.first); it != current_inbox.end()) {
      auto& [current_mesh, current_ghost_cell_data, current_boundary_data,
             current_boundary_data_validity_range] = it->second;
      // We have already received some data at this time. Receiving data twice
      // at the same time should only occur when receiving fluxes after having
      // previously received ghost cells. We sanity check that the data we
      // already have is the ghost cells and that we have not yet received flux
      // data.
      ASSERT(current_ghost_cell_data.has_value(),
             "Have not yet received ghost cells at time step "
                 << time_step_id
                 << " but the inbox entry already exists. This is a bug in the "
                    "ordering of the actions.");
      ASSERT(not current_boundary_data.has_value(),
             "The fluxes have already been received at time step "
                 << time_step_id
                 << ". They are either being received for a second time, there "
                    "is a bug in the ordering of the actions (though a "
                    "different ASSERT should've caught that), or the incorrect "
                    "temporal ID is being sent.");

      ASSERT(current_mesh == mesh,
             "The mesh being received for the fluxes is different than the "
             "mesh received for the ghost cells. Mesh for fluxes: "
                 << mesh << " mesh for ghost cells " << current_mesh);

      // We always move here since we take ownership of the data and moves
      // implicitly decay to copies
      current_boundary_data = std::move(boundary_data);
      current_boundary_data_validity_range = boundary_data_validity_range;
    } else {
      // We have not received ghost cells or fluxes at this time.
      if (not current_inbox.insert(std::forward<ReceiveDataType>(data))
                  .second) {
        ERROR("Failed to insert data to receive at instance '"
              << time_step_id
              << "' with tag 'BoundaryCorrectionAndGhostCellsInbox'.\n");
      }
    }
  }
};
}  // namespace evolution::dg::Tags


//
// Copyright © 2017 Arm Ltd. All rights reserved.
// SPDX-License-Identifier: MIT
//

#include <boost/test/unit_test.hpp>
#include "armnnOnnxParser/IOnnxParser.hpp"
#include  "ParserPrototxtFixture.hpp"

BOOST_AUTO_TEST_SUITE(OnnxParser)

// A MatMul in isolation, not connected to an add. Should result in a non-biased FullyConnected layer.
struct MatMulFixture : public armnnUtils::ParserPrototxtFixture<armnnOnnxParser::IOnnxParser>
{
    MatMulFixture()
    {
        m_Prototext = R"(
                    ir_version: 3
                    producer_name:  "CNTK "
                    producer_version:  "2.5.1 "
                    domain:  "ai.cntk "
                    model_version: 1
                    graph {
                      name:  "CNTKGraph "
                      input {
                         name: "Input"
                         type {
                           tensor_type {
                             elem_type: 1
                             shape {
                               dim {
                                 dim_value: 1
                               }
                               dim {
                                 dim_value: 1
                               }
                             }
                           }
                         }
                       }
                       input {
                          name: "Const"
                          type {
                            tensor_type {
                              elem_type: 1
                              shape {
                                dim {
                                  dim_value: 1
                                }
                                dim {
                                  dim_value: 1
                                }
                              }
                            }
                          }
                        }
                        initializer {
                          dims: 1
                          dims: 1
                          data_type: 1
                          float_data: 17.0
                          name: "Const"
                       }
                       node {
                           input: "Input"
                           input: "Const"
                           output: "Output"
                           name: "SimpleMatmul"
                           op_type: "MatMul"
                       }
                      output {
                           name:  "Output"
                           type {
                              tensor_type {
                                elem_type: 1
                                shape {
                                  dim {
                                     dim_value: 1
                                  }
                                  dim {
                                     dim_value: 1
                                  }
                                }
                              }
                           }
                       }
                    }
                    opset_import {
                       version: 7
                     })";

        Setup();
    }
};

BOOST_FIXTURE_TEST_CASE(MatMul, MatMulFixture)
{
    RunTest<1>({{"Input", { 2 }}}, {{"Output", { 34 }}});
}

// In Onnx fully connected layers are expressed as a MatMul followed by an Add.
// The OnnxParser must detect this case and convert them to a FullyConnected layer.
struct FullyConnectedFixture : public armnnUtils::ParserPrototxtFixture<armnnOnnxParser::IOnnxParser>
{
    FullyConnectedFixture()
    {
        m_Prototext = R"(
                    ir_version: 3
                    producer_name:  "CNTK "
                    producer_version:  "2.5.1 "
                    domain:  "ai.cntk "
                    model_version: 1
                    graph {
                      name:  "CNTKGraph "
                      input {
                         name: "Input"
                         type {
                           tensor_type {
                             elem_type: 1
                             shape {
                               dim {
                                 dim_value: 1
                               }
                               dim {
                                 dim_value: 1
                               }
                             }
                           }
                         }
                       }
                       input {
                          name: "Weight"
                          type {
                            tensor_type {
                              elem_type: 1
                              shape {
                                dim {
                                  dim_value: 1
                                }
                                dim {
                                  dim_value: 1
                                }
                              }
                            }
                          }
                        }
                        initializer {
                          dims: 1
                          dims: 1
                          data_type: 1
                          float_data: 2
                          name: "Weight"
                       }
                       input {
                          name: "Bias"
                          type {
                            tensor_type {
                              elem_type: 1
                              shape {
                                dim {
                                  dim_value: 1
                                }
                              }
                            }
                          }
                        }
                        initializer {
                          dims: 1
                          data_type: 1
                          float_data: 1
                          name: "Bias"
                       }
                       node {
                           input: "Input"
                           input: "Weight"
                           output: "AddInput"
                           name: "FCMatmul"
                           op_type: "MatMul"
                       }
                       node {
                           input: "AddInput"
                           input: "Bias"
                           output: "Output"
                           name: "FCAdd"
                           op_type: "Add"
                       }
                       value_info {
                            name: "AddInput"
                            type {
                              tensor_type {
                                elem_type: 1
                                shape {
                                  dim {
                                    dim_value: 1
                                  }
                                  dim {
                                    dim_value: 1
                                  }
                                }
                              }
                            }
                          }
                      output {
                           name:  "Output"
                           type {
                              tensor_type {
                                elem_type: 1
                                shape {
                                  dim {
                                     dim_value: 1
                                  }
                                  dim {
                                     dim_value: 1
                                  }
                                }
                              }
                           }
                       }
                    }
                    opset_import {
                       version: 7
                     })";

        Setup();
    }
};

BOOST_FIXTURE_TEST_CASE(FullyConnected, FullyConnectedFixture)
{
    RunTest<1>({{"Input", { 3 }}}, {{"Output", { 7 }}});
}

// Similar to FullyConnectedFixture, but this time the MatMul's output is used by two Adds. This should result
// in two FullyConnected layers being created.
//      I
//      |
//      M -- C
//     / \'
// C-- A  A -- C
//     \ /
//      A
struct MatMulUsedInTwoFcFixture : public armnnUtils::ParserPrototxtFixture<armnnOnnxParser::IOnnxParser>
{
    MatMulUsedInTwoFcFixture()
    {
        m_Prototext = R"(
                    ir_version: 3
                    producer_name:  "CNTK "
                    producer_version:  "2.5.1 "
                    domain:  "ai.cntk "
                    model_version: 1
                    graph {
                      name:  "CNTKGraph "
                      input {
                         name: "Input"
                         type {
                           tensor_type {
                             elem_type: 1
                             shape {
                               dim {
                                 dim_value: 1
                               }
                               dim {
                                 dim_value: 1
                               }
                             }
                           }
                         }
                       }
                       input {
                          name: "Weight"
                          type {
                            tensor_type {
                              elem_type: 1
                              shape {
                                dim {
                                  dim_value: 1
                                }
                                dim {
                                  dim_value: 1
                                }
                              }
                            }
                          }
                        }
                        initializer {
                          dims: 1
                          dims: 1
                          data_type: 1
                          float_data: 2
                          name: "Weight"
                       }
                       input {
                          name: "Bias"
                          type {
                            tensor_type {
                              elem_type: 1
                              shape {
                                dim {
                                  dim_value: 1
                                }
                              }
                            }
                          }
                        }
                        initializer {
                          dims: 1
                          data_type: 1
                          float_data: 1
                          name: "Bias"
                       }
                       input {
                          name: "Bias_1"
                          type {
                            tensor_type {
                              elem_type: 1
                              shape {
                                dim {
                                  dim_value: 1
                                }
                              }
                            }
                          }
                        }
                        initializer {
                          dims: 1
                          data_type: 1
                          float_data: 10.0
                          name: "Bias_1"
                       }
                       node {
                           input: "Input"
                           input: "Weight"
                           output: "AddInput"
                           name: "FCMatmul"
                           op_type: "MatMul"
                       }
                       node {
                           input: "AddInput"
                           input: "Bias"
                           output: "AddOutput"
                           name: "FCAdd"
                           op_type: "Add"
                       }
                       node {
                           input: "AddInput"
                           input: "Bias_1"
                           output: "AddOutput_1"
                           name: "FCAdd_1"
                           op_type: "Add"
                       }
                       node {
                           input: "AddOutput"
                           input: "AddOutput_1"
                           output: "Output"
                           name: "FinalAdd"
                           op_type: "Add"
                       }
                       value_info {
                            name: "AddInput"
                            type {
                              tensor_type {
                                elem_type: 1
                                shape {
                                  dim {
                                    dim_value: 1
                                  }
                                  dim {
                                    dim_value: 1
                                  }
                                }
                              }
                            }
                          }
                      value_info {
                           name:  "AddOutput"
                           type {
                              tensor_type {
                                elem_type: 1
                                shape {
                                  dim {
                                     dim_value: 1
                                  }
                                  dim {
                                     dim_value: 1
                                  }
                                }
                              }
                           }
                       }
                       value_info {
                            name:  "AddOutput_1"
                            type {
                               tensor_type {
                                 elem_type: 1
                                 shape {
                                   dim {
                                      dim_value: 1
                                   }
                                   dim {
                                      dim_value: 1
                                   }
                                 }
                               }
                            }
                        }
                        output {
                             name:  "Output"
                             type {
                                tensor_type {
                                  elem_type: 1
                                  shape {
                                    dim {
                                       dim_value: 1
                                    }
                                    dim {
                                       dim_value: 1
                                    }
                                  }
                                }
                             }
                         }
                    }
                    opset_import {
                       version: 7
                     })";

        Setup();
    }
};

BOOST_FIXTURE_TEST_CASE(MatMulUsedInTwoFc, MatMulUsedInTwoFcFixture)
{
    RunTest<1>({{"Input", { 3 }}}, {{"Output", { 23 }}});
}

// Similar to MatMulUsedInTwoFc, but this time the Adds are 'staggered' (see diagram), which means that only one
// FullyConnected layer can be created (the other should just be an Add).
//        I
//        |
//        M -- C1
//       / \'
// C2 -- A  |
//       \ /
//        A
struct MatMulUsedInTwoFcStaggeredFixture : public armnnUtils::ParserPrototxtFixture<armnnOnnxParser::IOnnxParser>
{
    MatMulUsedInTwoFcStaggeredFixture()
    {
        m_Prototext = R"(
                    ir_version: 3
                    producer_name:  "CNTK "
                    producer_version:  "2.5.1 "
                    domain:  "ai.cntk "
                    model_version: 1
                    graph {
                      name:  "CNTKGraph "
                      input {
                         name: "Input"
                         type {
                           tensor_type {
                             elem_type: 1
                             shape {
                               dim {
                                 dim_value: 1
                               }
                               dim {
                                 dim_value: 1
                               }
                             }
                           }
                         }
                       }
                       input {
                          name: "Weight"
                          type {
                            tensor_type {
                              elem_type: 1
                              shape {
                                dim {
                                  dim_value: 1
                                }
                                dim {
                                  dim_value: 1
                                }
                              }
                            }
                          }
                        }
                        initializer {
                          dims: 1
                          dims: 1
                          data_type: 1
                          float_data: 2
                          name: "Weight"
                       }
                       input {
                          name: "Bias"
                          type {
                            tensor_type {
                              elem_type: 1
                              shape {
                                dim {
                                  dim_value: 1
                                }
                              }
                            }
                          }
                        }
                        initializer {
                          dims: 1
                          data_type: 1
                          float_data: 1
                          name: "Bias"
                       }
                        node {
                           input: "Input"
                           input: "Weight"
                           output: "AddInput"
                           name: "MatmulFC&NFC"
                           op_type: "MatMul"
                       }
                       node {
                           input: "AddInput"
                           input: "Bias"
                           output: "AddOutput"
                           name: "FCAdd"
                           op_type: "Add"
                       }

                       node {
                           input: "AddInput"
                           input: "AddOutput"
                           output: "Output"
                           name: "FinalAdd"
                           op_type: "Add"
                       }
                       value_info {
                            name: "AddInput"
                            type {
                              tensor_type {
                                elem_type: 1
                                shape {
                                  dim {
                                    dim_value: 1
                                  }
                                  dim {
                                    dim_value: 1
                                  }
                                }
                              }
                            }
                          }
                      value_info {
                           name:  "AddOutput"
                           type {
                              tensor_type {
                                elem_type: 1
                                shape {
                                  dim {
                                     dim_value: 1
                                  }
                                  dim {
                                     dim_value: 1
                                  }
                                }
                              }
                           }
                       }
                       output {
                             name:  "Output"
                             type {
                                tensor_type {
                                  elem_type: 1
                                  shape {
                                    dim {
                                       dim_value: 1
                                    }
                                    dim {
                                       dim_value: 1
                                    }
                                  }
                                }
                             }
                         }
                    }
                    opset_import {
                       version: 7
                     })";
        Setup();
    }
};

BOOST_FIXTURE_TEST_CASE(MatMulUsedInTwoFcStaggered, MatMulUsedInTwoFcStaggeredFixture)
{
    RunTest<1>({{"Input", { 3 }}}, {{"Output", { 13 }}});
}

BOOST_AUTO_TEST_SUITE_END()


#include "ros/ros.h"
#include <nav_msgs/Odometry.h>
// PCL
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/io/ply_io.h>
#include <pcl/point_types.h>
#include <pcl/octree/octree.h>
#include <pcl/filters/passthrough.h>
#include <pcl/filters/project_inliers.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/common/transforms.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/approximate_voxel_grid.h>
#include <pcl/point_types.h>
#include <pcl/registration/ndt.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <boost/thread/thread.hpp>
#include <boost/filesystem.hpp>
#include <thread>
namespace fs = boost::filesystem;

int get_filenames(const std::string& dir, std::vector<std::string>& filenames)
{
    fs::path path(dir);
    if (!fs::exists(path))
    {
        return -1;
    }

    fs::directory_iterator end_iter;
    for (fs::directory_iterator iter(path); iter!=end_iter; ++iter)
    {
        if (fs::is_regular_file(iter->status()))
        {
            filenames.push_back(iter->path().string());
        }

        // if (fs::is_directory(iter->status()))
        // {
        //     get_filenames(iter->path().string(), filenames);
        // }
    }

    return filenames.size();
}

Eigen::Matrix4f pose_to_eigen(float px,float py,float pz,float qx,float qy,float qz,float qw)
{
  //RotationMatrix to Quaterniond
  Eigen::Quaternionf q;
  q.x() = qx;
  q.y() = qy;
  q.z() = qz;
  q.w() = qw;
  //Quaterniond to RotationMatrix
  Eigen::Matrix3f Rx = q.toRotationMatrix();

  // laser ->world
  Eigen::Affine3f laser_to_world = Eigen::Affine3f::Identity();
  laser_to_world.translation() << px, py, pz;
  laser_to_world.linear() = Rx;

  // world -> laser
  Eigen::Affine3f world_to_laser= Eigen::Affine3f::Identity();
  world_to_laser.matrix() = laser_to_world.matrix().inverse();

  Eigen::Matrix4f max = laser_to_world.matrix();

  return max;
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "ndt_node");

  ROS_INFO("coloring_node started...");
  std::string root_path,map_path;
  float px,py,pz,qx,qy,qz,qw;
  ros::NodeHandle nh_("~");
  nh_.param<std::string>("root_path", root_path, "");
  nh_.param<std::string>("map_path", map_path, "");
  nh_.param<float>("pos_x", px,0);
  nh_.param<float>("pos_y", py,0);
  nh_.param<float>("pos_z", pz,0);
  nh_.param<float>("qua_x", qx,0);
  nh_.param<float>("qua_y", qy,0);
  nh_.param<float>("qua_z", qz,0);
  nh_.param<float>("qua_w", qw,1);
  //init guess
  Eigen::Matrix4f init_pose = pose_to_eigen(px,py,pz,qx,qy,qz,qw);
  // load map
  pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud(new pcl::PointCloud<pcl::PointXYZ>);
    if (pcl::io::loadPLYFile<pcl::PointXYZ>(map_path, *target_cloud) == -1)
    {
        PCL_ERROR("Couldn't read file room_scan1.pcd \n");
        return (-1);
    }
    std::cout << "Loaded " << target_cloud->size() << " data points from room_scan1.pcd" << std::endl;
  //read laser pcd
  std::vector<std::string> pcd_names;
  get_filenames(root_path+"/pcd",pcd_names);
  pcl::PointCloud<pcl::PointXYZ>::Ptr livox_cloud (new pcl::PointCloud<pcl::PointXYZ>);
  for (size_t i = 0; i < pcd_names.size(); i++)
  {
    std::string fileName = pcd_names[i];
    // load pcd file
    pcl::PointCloud<pcl::PointXYZ>::Ptr frame_ptr (new pcl::PointCloud<pcl::PointXYZ>);
      if(pcl::io::loadPCDFile<pcl::PointXYZ>(fileName, *frame_ptr)==-1) {
          std::cout << "load cloud failed!" << std::endl;
      }
    *livox_cloud=*livox_cloud+*frame_ptr;
  }

  //将输入的扫描过滤到原始尺寸的大概10%以提高匹配的速度。
    pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_cloud(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::ApproximateVoxelGrid<pcl::PointXYZ> approximate_voxel_filter;
    approximate_voxel_filter.setLeafSize(0.05, 0.05, 0.05);
    approximate_voxel_filter.setInputCloud(livox_cloud);
    approximate_voxel_filter.filter(*filtered_cloud);
    std::cout << "Filtered cloud contains " << filtered_cloud->size()
        << " data points from room_scan2.pcd" << std::endl;
    //初始化正态分布变换（NDT）
    pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> ndt;
    //设置依赖尺度NDT参数
    //为终止条件设置最小转换差异
    ndt.setTransformationEpsilon(0.01);
    //为More-Thuente线搜索设置最大步长
    ndt.setStepSize(0.1);
    //设置NDT网格结构的分辨率（VoxelGridCovariance）
    ndt.setResolution(1.0);
    //设置匹配迭代的最大次数
    ndt.setMaximumIterations(35);
    // 设置要配准的点云
    ndt.setInputCloud(filtered_cloud);
    //设置点云配准目标
    ndt.setInputTarget(target_cloud);
    //计算需要的刚体变换以便将输入的点云匹配到目标点云
    pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud(new pcl::PointCloud<pcl::PointXYZ>);
    ndt.align(*output_cloud, init_pose);
    std::cout << "Normal Distributions Transform has converged:" << ndt.hasConverged()
        << " score: " << ndt.getFitnessScore() << std::endl;
        std::cout << "Normal Distributions Transform:" << ndt.getFinalTransformation() << std::endl;

    Eigen::Matrix4f t_localizer(Eigen::Matrix4f::Identity());
    t_localizer = ndt.getFinalTransformation();
    Eigen::Affine3f A;
    A = t_localizer;
    Eigen::Matrix3f rotation_matrix = A.linear();
    Eigen::Quaternionf q(rotation_matrix);
    std::cout << "Normal Distributions quaternion:" << q.x() << " " << q.y()<< " " << q.z() << " " << q.w() << std::endl;
    Eigen::Vector3f eulerAngle=q.matrix().eulerAngles(0,1,2);
    std::cout << "Normal eulerAngle:" << eulerAngle[0] * 180 / M_PI << " " << eulerAngle[1] * 180 / M_PI<< " " << eulerAngle[2] * 180 / M_PI  << std::endl;

    //使用创建的变换对未过滤的输入点云进行变换
    pcl::transformPointCloud(*livox_cloud, *output_cloud, ndt.getFinalTransformation());
    //保存转换的输入点云
    pcl::io::savePCDFileASCII(root_path+"/transformed.pcd", *output_cloud);

  ros::Rate rate(10);
  while(ros::ok())
  {

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

  return 0;
}


/// \file   test_walrasian_market.cpp
///
/// \brief
///
/// \authors    Maarten P. Scholl
/// \date       2019-10-08
/// \copyright  Copyright 2017-2019 The Institute for New Economic Thinking,
///             Oxford Martin School, University of Oxford
///
///             Licensed under the Apache License, Version 2.0 (the "License");
///             you may not use this file except in compliance with the License.
///             You may obtain a copy of the License at
///
///                 http://www.apache.org/licenses/LICENSE-2.0
///
///             Unless required by applicable law or agreed to in writing,
///             software distributed under the License is distributed on an "AS
///             IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either
///             express or implied. See the License for the specific language
///             governing permissions and limitations under the License.
///
///             You may obtain instructions to fulfill the attribution
///             requirements in CITATION.cff
///
#include <tuple>
#include <utility>

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE walrasian_market

#include <boost/test/included/unit_test.hpp>

#include <esl/economics/finance/company.hpp>
#include <esl/computation/environment.hpp>
#include <esl/economics/markets/walras/price_setter.hpp>
#include <esl/law/jurisdictions.hpp>
#include <esl/simulation/model.hpp>
#include <esl/data/log.hpp>

#include <esl/economics/markets/walras/quote_message.hpp>

using namespace esl;
using namespace esl::economics;
using namespace esl::economics::markets;
using namespace esl::economics::finance;
using namespace esl::economics::markets::walras;
using namespace esl::law;
using esl::economics::company;

using std::map;
using std::vector;

///
/// \brief  A simple test trader order with constant wealth allocation
///
struct test_trader_order
: public differentiable_order_message
{
    ///
    /// \brief  Total wealth of the agent to be distributed
    ///
    double capital;

    ///
    /// \brief  Allocation fractions
    ///
    map<identity<law::property>, double> allocation;

    ///
    /// \brief  Supply of long positions and securities lending contracts
    ///
    //map<identity<law::property>, quantity> supply;

    // inherit base class constructor
   // using differentiable_order_message::differentiable_order_message;

    ///
    /// \param capital
    /// \param allocation
    /// \param supply
    /// \param q
    test_trader_order( double capital
                     , map<identity<law::property>, double> allocation
                     , const walras::quote_message &q = walras::quote_message())

    : differentiable_order_message(q.sender, q.recipient)
    , capital(capital)
    , allocation(move(allocation))
    {

    }

    ///
    /// \brief  computes excess demand for each property individually
    /// \param quotes   for each property id, a tuple of the quote and differentiable scalar
    /// \return
    map<identity<law::property>, variable> excess_demand(const map<identity<law::property>, std::tuple<quote, variable>> &quotes)
    const override
    {
        map<identity<law::property>, variable> excess_demand_;
        for(const auto &[k, v]: allocation) {
            auto quote_price_ = static_cast<double>(std::get<0>(quotes.find(k)->second));
            double supply_ = 0;
            auto iterator_ = supply.find(k);
            if(supply.end() != iterator_) {
                supply_ = double(std::get<0>(iterator_->second) - std::get<1>(iterator_->second));
            }
            excess_demand_.insert({k, (v * capital) - supply_ * (quote_price_ * std::get<1>(quotes.find(k)->second))});
        }
        return excess_demand_;
    }
};

struct test_constant_demand_trader
: public shareholder
{
    // set trader wealth for this test
    double wealth = 1000.00;

    test_constant_demand_trader(const identity<shareholder> &i)
    : agent(i)
    , economics::finance::shareholder(i)
    {

    }

    simulation::time_point act( simulation::time_interval step
                              , std::seed_seq &seed
                              ) override
    {
        (void) seed;
        for(auto [k, message_]: inbox) {
            switch(message_->type){
            case walras::quote_message::code:
                auto quote_ = std::dynamic_pointer_cast<walras::quote_message>(message_);
                map<identity<property>, double> allocation;
                size_t assets_ = quote_->proposed.size();
                size_t denominator_ =  (assets_ * (1 + assets_))/2;
                size_t a = 1;
                for(auto [k, q]: quote_->proposed){
                    allocation.emplace(k->identifier,  double(a) / denominator_ );
                    ++a;
                }

                // make an order message
                auto message_ = this->template create_message<test_trader_order>( quote_->sender
                                                                                , step.lower
                                                                                , wealth
                                                                                , allocation
                                                                                , *quote_
                                                                                );
                message_->sent = step.lower;
                for(auto [k, q]: quote_->proposed){
                    message_->supply.emplace(k->identifier, std::make_tuple(500, quantity(0)));
                }
            }
        }
        return step.upper;
    }
};

BOOST_AUTO_TEST_SUITE(ESL)

///
/// \brief  Tests that a market quotes the right number of prices for property,
///         and that these are delivered to participants.
///
BOOST_AUTO_TEST_CASE(walras_market_quote_multiple_assets)
{
    computation::environment environment_;
    simulation::model model_(environment_, simulation::parameter::parametrization(0, 0, 10));

    vector<std::tuple<std::shared_ptr<company>, share_class>> share_classes_;

    typedef std::tuple<identity<company>, share_class> key_t;
    map<key_t, identity<law::property>> stocks_;

    // set the number of traded assets and create a container
    law::property_map<quote> traded_assets_;
    size_t assets = 2;

    std::vector<std::shared_ptr<property>> properties_;

    // create the assets, by creating the companies that issued the shares first
    for(size_t a = 0; a < assets; ++a) {
        auto company_identifier_ = model_.template create_identifier<company>();

        // create a US company
        auto company_ = std::make_shared<company>(company_identifier_, law::jurisdictions::US);

        // create a share issue for the company for several shares
        auto main_issue_ = share_class();
        company_->shares_outstanding[main_issue_] = 1'000;

        // create stocks from the shares
        for(const auto &[share_, quantity] : company_->shares_outstanding) {
            (void) quantity;
            auto stock_ = std::make_shared<stock>(*company_, share_);

            // suggest to the market that $1.00 is the starting price
            traded_assets_.insert({stock_, quote(price::approximate(1.00, currencies::USD))});
            share_classes_.emplace_back(make_tuple(company_, share_));

            // create a key to store these in associative datastructures
            auto key_ = std::make_tuple<identity<company>, share_class>(*company_, share_class(share_));
            stocks_.insert({key_, stock_->identifier});

            properties_.push_back(stock_);
        }
    }

    auto market_ = model_.template create<price_setter>();
    market_->traded_properties = traded_assets_;

    // create a collection with trading agents who are also shareholders
    vector<std::shared_ptr<test_constant_demand_trader>> participants_ =
        {   model_.template create<test_constant_demand_trader>()
        ,   model_.template create<test_constant_demand_trader>()
        };

    for(auto &p : participants_) {
        std::cout << "test_constant_demand_trader " << p->identifier << std::endl;
        // add the trading agents to the market participant list
        market_->participants.insert((*p));

        // notify agents about the stock and key
        for(const auto &[k, v]: stocks_) {
            p->stocks.insert(std::make_pair(k, v));
        }

        // give agents an endowment of stocks
        for(const auto &[k, v]: traded_assets_) {
            (void)v;
            (*p).shareholder::owner<stock>::properties.insert(std::dynamic_pointer_cast<stock>(k),quantity(500));
        }

        std::cout << "created " << share_classes_.size() << " share classes" << std::endl;

        // notify company of new shareholders
        for(auto &share: share_classes_) {
            auto c = std::get<0>(share);
            map<share_class, std::uint64_t> holdings_;
            holdings_.emplace(std::get<1>(share), 500);
            c->shareholders.emplace(*p, holdings_);
        }
    }

    // during the first timestep, the market sends out quotes, and nothing else happens
    model_.step({0, 1});

    // expect: prices unchanged from initial price given above
    BOOST_TEST(std::get<price>(market_->traded_properties.find(properties_[0])->second.type) == price(100, currencies::USD));
    BOOST_TEST(std::get<price>(market_->traded_properties.find(properties_[1])->second.type) == price(100, currencies::USD));

    // we have the first market interaction: prices are formed by the market agent
    model_.step({1, 2});

    // prices are updated
    BOOST_TEST(std::get<price>(market_->traded_properties.find(properties_[0])->second.type) == price( 66, currencies::USD));
    BOOST_TEST(std::get<price>(market_->traded_properties.find(properties_[1])->second.type) == price(133, currencies::USD));

    // the agent does not know the new prices until after one time step
    BOOST_CHECK_EQUAL(participants_[0]->prices.size(), assets);
    BOOST_TEST(participants_[0]->prices.find(properties_[0])->second == price(100, currencies::USD));
    BOOST_TEST(participants_[0]->prices.find(properties_[1])->second == price(100, currencies::USD));
    //log(notice) << participants_[0]->prices << std::endl;

    // the market runs again
    model_.step({2, 3});
    // the agent now knows the previous prices from step [1, 2)
    //log(notice) << participants_[0]->prices << std::endl;
    BOOST_TEST(participants_[0]->prices.find(properties_[0])->second == price( 66, currencies::USD));
    BOOST_TEST(participants_[0]->prices.find(properties_[1])->second == price(133, currencies::USD));

    // the market prices are computed again
    BOOST_TEST(std::get<price>(market_->traded_properties.find(properties_[0])->second.type) == price( 66, currencies::USD));
    BOOST_TEST(std::get<price>(market_->traded_properties.find(properties_[1])->second.type) == price(133, currencies::USD));
}

///
/// \brief  Tests that a market quotes the right number of prices for property,
///         and that these are delivered to participants.
///
    BOOST_AUTO_TEST_CASE(walras_market_quote_single_asset)
    {
        computation::environment environment_;
        simulation::model model_(environment_, simulation::parameter::parametrization(0, 0, 10));

        vector<std::tuple<std::shared_ptr<company>, share_class>> share_classes_;

        typedef std::tuple<identity<company>, share_class> key_t;
        map<key_t, identity<law::property>> stocks_;

        // set the number of traded assets and create a container
        law::property_map<quote> traded_assets_;
        size_t assets = 1;

        std::vector<std::shared_ptr<property>> properties_;

        // create the assets, by creating the companies that issued the shares first
        for(size_t a = 0; a < assets; ++a) {
            auto company_identifier_ = model_.template create_identifier<company>();

            // create a US company
            auto company_ = std::make_shared<company>(company_identifier_, law::jurisdictions::US);

            // create a share issue for the company for several shares
            auto main_issue_ = share_class();
            company_->shares_outstanding[main_issue_] = 1'000;

            // create stocks from the shares
            for(const auto &[share_, quantity] : company_->shares_outstanding) {
                (void) quantity;
                auto stock_ = std::make_shared<stock>(*company_, share_);

                // suggest to the market that $1.00 is the starting price
                traded_assets_.insert({stock_, quote(price::approximate(1.00, currencies::USD))});
                share_classes_.emplace_back(make_tuple(company_, share_));

                // create a key to store these in associative datastructures
                auto key_ = std::make_tuple<identity<company>, share_class>(*company_, share_class(share_));
                stocks_.insert({key_, stock_->identifier});

                properties_.push_back(stock_);
            }
        }

        auto market_ = model_.template create<price_setter>();
        market_->traded_properties = traded_assets_;

        // create a collection with trading agents who are also shareholders
        vector<std::shared_ptr<test_constant_demand_trader>> participants_ =
            {   model_.template create<test_constant_demand_trader>()
                ,   model_.template create<test_constant_demand_trader>()
            };

        for(auto &p : participants_) {
            std::cout << "test_constant_demand_trader " << p->identifier << std::endl;
            // add the trading agents to the market participant list
            market_->participants.insert((*p));

            // notify agents about the stock and key
            for(const auto &[k, v]: stocks_) {
                p->stocks.insert(std::make_pair(k, v));
            }

            // give agents an endowment of stocks
            for(const auto &[k, v]: traded_assets_) {
                (void)v;
                (*p).shareholder::owner<stock>::properties.insert(std::dynamic_pointer_cast<stock>(k),quantity(500));
            }

            std::cout << "created " << share_classes_.size() << " share classes" << std::endl;

            // notify company of new shareholders
            for(auto &share: share_classes_) {
                auto c = std::get<0>(share);
                map<share_class, std::uint64_t> holdings_;
                holdings_.emplace(std::get<1>(share), 500);
                c->shareholders.emplace(*p, holdings_);
            }
        }

        // during the first timestep, the market sends out quotes, and nothing else happens
        model_.step({0, 1});

        // expect: prices unchanged from initial price given above
        BOOST_TEST(std::get<price>(market_->traded_properties.find(properties_[0])->second.type) == price(100, currencies::USD));

        // we have the first market interaction: prices are formed by the market agent
        model_.step({1, 2});

        // prices are updated
        BOOST_TEST(std::get<price>(market_->traded_properties.find(properties_[0])->second.type) == price(200, currencies::USD));

        // the agent does not know the new prices until after one time step
        BOOST_CHECK_EQUAL(participants_[0]->prices.size(), assets);
        BOOST_TEST(participants_[0]->prices.find(properties_[0])->second == price(100, currencies::USD));
        //log(notice) << participants_[0]->prices << std::endl;

        // the market runs again
        model_.step({2, 3});
        // the agent now knows the previous prices from step [1, 2)
        //log(notice) << participants_[0]->prices << std::endl;
        BOOST_TEST(participants_[0]->prices.find(properties_[0])->second == price(200, currencies::USD));

        // the market prices are computed again
        BOOST_TEST(std::get<price>(market_->traded_properties.find(properties_[0])->second.type) == price(200, currencies::USD));
    }

BOOST_AUTO_TEST_SUITE_END()  // ESL


#include <iostream>
#include <cstdlib>
#include <ctime>
#include <cmath>
#include <cstring>

#include <sstream>
#include <vector>
#include <string>
#include <set>
#include <iostream>
#include <fstream>
#include <time.h>
#include <limits.h>
//#include <atomic>
#include <chrono>

#include <stdint.h>
#include <stdlib.h>
#include <sys/stat.h>
#include <functional>

#include <getopt.h>

#include "../common/Common.hpp"
//#include "utils.hpp"
#include "../utils/fastq.hpp"
#include "../kmer/Kmer.hpp"
#include "../hash/HashTables.hpp"

#include "../utils/alglib-3.15.0/src/statistics.h"
#include "../utils/alglib-3.15.0/src/ap.h"

#include "../kmer/kmc_api/kmc_file.h"
#include "../kmer/kmc_reader.hpp"

#include "../utils/threadpool.hpp"
#include <boost/bind.hpp>
//#include <boost/thread.hpp>
using namespace std;
//using namespace boost;
using namespace boost::threadpool;
using namespace std::chrono;

// structs for getopt
struct SA_ProgramOptions {
  size_t k;
  string input1;
  string input2;
  string output1;
  string output2;
  int mode;
  unsigned int num_threads;
  int max_memory;
  int count_min;
  bool verbose;
  bool kmc;
  bool bin;
  float kmer_vote; //ratio of unique kmers in a read to be claimed as unique read
  float pvalue_thresh;
  vector<string> files;

  SA_ProgramOptions() : k(0), mode(1), num_threads(8), max_memory(12), count_min(2), verbose(false), kmc(false), bin(true), kmer_vote(0.5), pvalue_thresh(0.005) {}
};

void SA_PrintUsage() {
  cerr << "SA " << SA_VERSION << endl << endl;
  cerr << "Compare frequencies of k-mers [from KMC library] and saves differential reads results for each metagenome in comparison" << endl << endl;
  cerr << "Usage: SA -k -a -b -o -p [options]";
  cerr << endl << endl <<
	"-k, --kmer-size=INT             Size of k-mers, at most " << (int) (Kmer::MAX_K-1)<< endl <<
	"-a, --input1=STRING             Input filename for metagenome group A" << endl <<
	"-b, --input2=STRING             Input filename for metagenome group B" << endl <<
	"-o, --output1=STRING            Prefix for output of metagenome A" << endl <<
	"-p, --output2=STRING            Prefix for output of metagenome B" << endl <<
	"-v, --kmer-vote=FLOAT           Percentage threshold of differential k-mers in distinctive reads <default 0.5>" << endl <<
	"-t, --number-threads=INT        Number of threads for running KMC etc. <default 8>" << endl <<
	"-m, --max-memory=INT            Max memory for running KMC <default 12>" << endl <<
	"-c, --count-min=INT            Min threshold of k-mer count for running KMC <default 2>" << endl <<
	"-q, --pvalue-thresh=FLOAT       For U-test <default 0.005>" << endl <<
	"-M, --mode=INT                  1 or 2 <default 1>" << endl <<
	"    --kmc                       Call KMC (if you do not have kmc output; make sure kmc is available in the current environment)" << endl <<
	"    --no-bin                    Do not generate kmer_count.bin (already existed)" << endl <<
    "    --verbose                   Print messages during run" << endl << endl
    ;
}

void SA_ParseOptions(int argc, char **argv, SA_ProgramOptions &opt) {
  int verbose_flag = 0;
  int kmc_flag = 0;
  int bin_flag = 0;
  const char* opt_string = "k:o:p:a:b:v:q:m:c:t:M:";
  static struct option long_options[] =
  {
    {"verbose", no_argument,  &verbose_flag, 1},
	{"kmc", no_argument, &kmc_flag, 1},
	{"no-bin", no_argument, &bin_flag, 1},
	{"kmer-size", required_argument, 0, 'k'},
    {"output1", required_argument, 0, 'o'},
	{"output2", required_argument, 0, 'p'},
	{"input1", required_argument, 0, 'a'},
	{"input2", required_argument, 0, 'b'},
	{"kmer-vote", required_argument, 0, 'v'},
	{"pvalue-thresh", required_argument, 0, 'q'},
	{"max-memory", required_argument, 0, 'm'},
	{"count-min", required_argument, 0, 'c'},
	{"num-threads", required_argument, 0, 't'},
    {"mode", required_argument, 0, 'M'},
	{0,0,0,0}
  };

  int option_index = 0;
  int c;
  stringstream ss;
  while (true) {
    c = getopt_long(argc,argv,opt_string, long_options, &option_index);

    if (c == -1) {
      break;
    }

    switch (c) {
	case 'k':
	  opt.k = atoi(optarg);
	  break;
	case 'o':
      opt.output1 = optarg;
      break;
	case 'p':
	  opt.output2 = optarg;
	  break;
	case 'a':
	  opt.input1 = optarg;
	  break;
	case 'b':
	  opt.input2 = optarg;
	  break;
	case 'v':
	  opt.kmer_vote = atof(optarg);
	  break;
	case 'q':
	  opt.pvalue_thresh = atof(optarg);
	  break;
	case 'M':
	  opt.mode = atoi(optarg);
	  break;
    	case 't':
	  opt.num_threads = atoi(optarg);
	  break;
	case 'm':
	  opt.max_memory = atoi(optarg);
	  break;
	case 'c':
          opt.count_min = atoi(optarg);
          break;
	default: break;
    }
  }

  if (verbose_flag) {
	  opt.verbose = true;
  }

  if (kmc_flag) {
	  opt.kmc = true;
  }

  if (bin_flag) {
	  opt.bin = false;
  }
}

bool SA_CheckOptions(SA_ProgramOptions &opt) {
  bool ret = true;

  if (opt.k <= 0 || opt.k >= MAX_KMER_SIZE) {
	  cerr << "Error, invalid value for kmer-size: " << opt.k << endl;
	  cerr << "Values must be between 1 and " << (MAX_KMER_SIZE-1) << endl;
	  ret = false;
  }

  /*
  if (opt.files1.size() == 0 || opt.files2.size() == 0) {
    cerr << "Need to specify files for input" << endl;
    ret = false;
  } else {
    struct stat stFileInfo;
    vector<string>::const_iterator it;
    int intStat;
    for(it = opt.files1.begin(); it != opt.files1.end(); ++it) {
      intStat = stat(it->c_str(), &stFileInfo);
      if (intStat != 0) {
	cerr << "Error: file not found, " << *it << endl;
	ret = false;
      }
    }
	for(it = opt.files2.begin(); it != opt.files2.end(); ++it) {
      intStat = stat(it->c_str(), &stFileInfo);
      if (intStat != 0) {
	cerr << "Error: file not found, " << *it << endl;
	ret = false;
      }
    }

  }
  */

  //TODO: check if we have permission to write to outputfile

  return ret;

}

void SA_PrintSummary(const SA_ProgramOptions &opt) {
  cout << "Mode: " << opt.mode << endl;
  cout << "kmer size: " << Kmer::k << endl;
  cout << "Using kmer_vote: " << opt.kmer_vote << endl;
  cout << "Using pvalue_thresh: " << opt.pvalue_thresh << endl;
  cout << "Writing kmer_count.bin? " << std::boolalpha << opt.bin << endl;
}

//added by Mingjie on 02/12/2015
//test kmer count in batches
void BatchTest(uint16_t **ary_count, int tot_sample, uint64_t batch_size, streamoff batch_offset, int num_sample1, int num_sample2, vector<uint64_t> &v_kmers, float pvalue_thresh, const vector<Kmer> &kvec, uset_t *g_kmer1_ptr, uset_t *g_kmer2_ptr, uset_t *large_kmer_count_ptr) {
	double freq_array1[num_sample1], freq_array2[num_sample2];
	double bothtails, lefttail, righttail, freq;
	int numZero;
	uint16_t cnt;
	uint64_t tot_cnt;
	uint64_t num_kmers;
	alglib::real_1d_array array_s1, array_s2;

	bothtails = 0;
	lefttail  = 0;
	righttail = 0;

	Kmer km;
	for(uint64_t i=0; i<batch_size; i++) {
		km = kvec[batch_offset+i];
		tot_cnt = 0;
		numZero = 0;
		for(int j=0; j<tot_sample; j++){
			cnt = ary_count[j][i];
			//cout << cnt << endl;
			tot_cnt += cnt;
			if (cnt == 0){
				numZero++;
			}
			num_kmers = v_kmers[j];
			freq = double(cnt)*1000000/num_kmers;

			if(j<num_sample1) {
				freq_array1[j] = freq;
			} else {
				freq_array2[j-num_sample1] = freq;
			}
		}
		//char tmp[1024];
		//km.toString(tmp);
		//cout << tmp << endl;
		//exit(1);
		//if (tot_cnt > tot_sample) {
		array_s1.setcontent(num_sample1, freq_array1);
		array_s2.setcontent(num_sample2, freq_array2);
		alglib::mannwhitneyutest(array_s1, num_sample1, array_s2, num_sample2, bothtails, lefttail, righttail);
		if(numZero < tot_sample * 0.7 ){
			if(lefttail <= pvalue_thresh) {
				g_kmer2_ptr->insert(km);
			} else if (righttail <= pvalue_thresh) {
				g_kmer1_ptr->insert(km);
			}
			if (tot_cnt > tot_sample*2  ){
				large_kmer_count_ptr->insert(km);
			}
		}
	}
}

void SA(int argc, char **argv) {

	SA_ProgramOptions opt;
	SA_ParseOptions(argc,argv,opt);
	ckhmap_t kmap, *kmap_ptr;
	kmap_ptr = &kmap;
  uset_t large_kmer_count, *large_kmer_count_ptr;
  large_kmer_count_ptr = &large_kmer_count;
	size_t kmer_coverage;
	size_t kmap_size;
	std::string filename;
	vector<string> samples1, samples2, samples;
	vector<string> kmc_names1, kmc_names2, kmc_names;
	vector<size_t> v_kmers; //kmer_cov for each sample
	int num_sample1, num_sample2, tot_sample;
	uset_t g_kmer1, g_kmer2, *g_kmer1_ptr, *g_kmer2_ptr;  //differential kmers for two groups in comparison
	g_kmer1_ptr = &g_kmer1;
	g_kmer2_ptr = &g_kmer2;
	vector<Kmer> kvec;

	alglib::real_1d_array array_s1, array_s2;

	int cap_threads = 6;
	steady_clock::time_point t1, t2, t3, t4, t5, t6;

	if (argc < 2) {
		SA_PrintUsage();
		exit(1);
	}

	/*
	if (!SA_CheckOptions(opt)) {
	CountBF_PrintUsage();
	exit(1);
	} */

	// set static global k-value
	Kmer::set_k(opt.k);

	if (opt.verbose) {
		SA_PrintSummary(opt);
	}

	int act_threads;
	act_threads = opt.num_threads > cap_threads ? cap_threads:opt.num_threads;

	GetInput(opt.input1, samples1, kmc_names1);
	GetInput(opt.input2, samples2, kmc_names2);

	samples = samples1;
	samples.insert(samples.end(), samples2.begin(), samples2.end());
	kmc_names = kmc_names1;
	kmc_names.insert(kmc_names.end(), kmc_names2.begin(), kmc_names2.end());

	num_sample1 = samples1.size();
	num_sample2 = samples2.size();
	tot_sample = samples.size();
	if (opt.verbose) {
		cout << endl << "# samples in group 1: " << num_sample1 << endl << "# samples in group 2: " << num_sample2 << endl;
	}
  if (opt.bin){
		pool tp(opt.num_threads);
		buildKHtable();

  }
	else {
		//read in total number of kmers in each sample
		ifstream logStream("kmer_count.log");
		string line;
		getline(logStream, line);
		istringstream ss(line);
		ss >> kmap_size;
		for (int i = 0; i < tot_sample; i++) {
			ss >> kmer_coverage;
			v_kmers.push_back(kmer_coverage);
		}
	}

	//read kmers in kvec to restore the original kmer order in kmap
	ifstream kmer_file("kmer_set.hex");
	kvec.resize(kmap_size);
	uint8_t bytes[(Kmer::MAX_K)/4];
	size_t idx_k = 0;
	for (size_t i = 0; i < kmap_size; i++) {
		kmer_file.read(reinterpret_cast<char*> (&bytes[0]), sizeof(bytes[0])*(Kmer::MAX_K)/4);
		Kmer km(bytes);
		kvec[idx_k] = km;
		idx_k ++;
	}
	t4 = steady_clock::now();

	ifstream inStream("kmer_count.bin", ios::binary);

	const uint64_t batch_thresh = 10000000; //if changed, remember to change 2D array parameter vec_count in readHT and batchTest
	uint64_t batch_size;

	//initialize 2D array
	uint16_t** ary_count = new uint16_t*[tot_sample];
	for (int i=0; i < tot_sample; i++) {
		ary_count[i] = new uint16_t[batch_thresh];
	}

	if (opt.verbose) {
	  cout << "\n...Loading and testing in batches..." << endl;
	}

	//load and test kmer count info in batches
	uint64_t kcnt_rem = kmap_size;
	streamoff batch_offset = 0;
	while(kcnt_rem >= batch_thresh) {
	  batch_size = batch_thresh;

	  ReadHT(inStream, tot_sample, kmap_size, ary_count, batch_size, batch_offset);
	  BatchTest(ary_count, tot_sample, batch_size, batch_offset, num_sample1, num_sample2, v_kmers, opt.pvalue_thresh, kvec, g_kmer1_ptr, g_kmer2_ptr, large_kmer_count_ptr);

	  //cout << g_kmer1.size() << "\t" << g_kmer2.size() <<endl;

	  batch_offset += batch_size;
	  kcnt_rem -= batch_size;
	  if (opt.verbose) {
		  cout << "# loaded kmers: " << batch_offset << endl;
	  }

	}
	if(kcnt_rem > 0 && kcnt_rem < batch_thresh) {
	  batch_size = kcnt_rem;
	  ReadHT(inStream, tot_sample, kmap_size, ary_count, batch_size, batch_offset);
	  BatchTest(ary_count, tot_sample, batch_size, batch_offset, num_sample1, num_sample2, v_kmers, opt.pvalue_thresh, kvec, g_kmer1_ptr, g_kmer2_ptr, large_kmer_count_ptr);
	}

	for (int i = 0; i < tot_sample; ++i) {
		delete [] ary_count[i];
	}
	delete [] ary_count;
	inStream.close();

	t5 = steady_clock::now();
	auto duration45 = duration_cast<std::chrono::minutes>(t5 - t4).count();
	cout << "BatchTesting takes " << duration45 << " minutes\n";

	if (opt.verbose) {
	  cout << "\n# differential kmers in group 1: " << g_kmer1.size() << endl;
	  cout << "# differential kmers in group 2: " << g_kmer2.size() << endl;
	}

	//deallocate the memory of kvec
	vector<Kmer>().swap(kvec);

	if (opt.verbose) {
	  cout << "\n...Writing distinctive reads..."  << endl;
	}

	//unsigned int write_threads;
	//write_threads = opt.num_threads > cap_threads ? opt.num_threads : act_threads;
	pool tp(opt.num_threads);

	if (opt.verbose) {
		cout << "start " << tp.size() << " threads" << endl;
	}

	for(vector<string>::iterator it=samples1.begin(); it!=samples1.end(); ++it) {
	  vector<string> tmp;
	  tmp.push_back(*it);

	  const char * basep = (*it).c_str();
	  std::string base(basename(basep));

		filename = std::string(opt.output1) + ("_" + base);
		if (opt.verbose) {
			cout << "writing to " << filename << endl;
		}
		ReadExtract(g_kmer1_ptr, tmp, filename, opt.kmer_vote, opt.verbose, tp, opt.num_threads, large_kmer_count_ptr);
	  tmp.clear();
	}

	for(vector<string>::iterator it=samples2.begin(); it!=samples2.end(); ++it) {
	  vector<string> tmp;
	  tmp.push_back(*it);

	  const char * basep = (*it).c_str();
	  std::string base(basename(basep));
		filename = std::string(opt.output2) + ("_" + base);
		if (opt.verbose) {
			cout << "writing to " << filename << endl;
		}
		ReadExtract(g_kmer2_ptr, tmp, filename, opt.kmer_vote, opt.verbose, tp, opt.num_threads, large_kmer_count_ptr);
	  tmp.clear();
	}

	t6 = steady_clock::now();
	auto duration56 = duration_cast<std::chrono::minutes>(t6 - t5).count();
	cout << "ReadExtract takes " << duration56 << " minutes\n";

}


/*
 * main.cpp
 *
 *  Created on: 28 May 2016
 *      Author: Minh Ngo @ 3DUniversum
 */
#include <iostream>
#include <boost/format.hpp>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/features/integral_image_normal.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/common/transforms.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/surface/marching_cubes.h>
#include <pcl/surface/marching_cubes_hoppe.h>
#include <pcl/filters/passthrough.h>
#include <pcl/surface/poisson.h>
#include <pcl/surface/impl/texture_mapping.hpp>
#include <pcl/features/normal_3d_omp.h>
#include <pcl/filters/filter.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/features/normal_3d.h>
#include <pcl/surface/gp3.h>
#include <pcl/search/kdtree.h>
#include <eigen3/Eigen/Core>
#include <pcl/surface/marching_cubes_rbf.h>
#include <pcl/io/pcd_io.h>
#include <pcl/io/ply_io.h>
#include <pcl/features/normal_3d_omp.h>
#include <pcl/surface/mls.h>
#include <opencv2/opencv.hpp>
#include <opencv2/core/mat.hpp>
#include <opencv2/core/eigen.hpp>
#include <vtkFillHolesFilter.h>
#include "Frame3D/Frame3D.h"
#include "pcl/surface/vtk_smoothing/vtk_utils.h"
#include <vtkPolyData.h>

pcl::PointCloud<pcl::PointXYZ>::Ptr mat2IntegralPointCloud(const cv::Mat &depth_mat, const float focal_length, const float max_depth) {
    // This function converts a depth image to a point cloud
    assert(depth_mat.type() == CV_16U);
    pcl::PointCloud<pcl::PointXYZ>::Ptr point_cloud(new pcl::PointCloud<pcl::PointXYZ>());
    const int half_width = depth_mat.cols / 2;
    const int half_height = depth_mat.rows / 2;
    const float inv_focal_length = 1.0 / focal_length;
    point_cloud->points.reserve(depth_mat.rows * depth_mat.cols);
    for (int y = 0; y < depth_mat.rows; y++)
    {
        for (int x = 0; x < depth_mat.cols; x++)
        {
            float z = depth_mat.at<ushort>(cv::Point(x, y)) * 0.001;
            if (z < max_depth && z > 0)
            {
                point_cloud->points.emplace_back(static_cast<float>(x - half_width) * z * inv_focal_length,
                                                 static_cast<float>(y - half_height) * z * inv_focal_length,
                                                 z);
            }
            else
            {
                point_cloud->points.emplace_back(x, y, NAN);
            }
        }
    }
    point_cloud->width = depth_mat.cols;
    point_cloud->height = depth_mat.rows;
    return point_cloud;
}

pcl::PointCloud<pcl::PointNormal>::Ptr computeNormals(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud) {
    // This function computes normals given a point cloud
    // !! Please note that you should remove NaN values from the pointcloud after computing the surface normals.
    pcl::PointCloud<pcl::PointNormal>::Ptr cloud_normals(new pcl::PointCloud<pcl::PointNormal>); // Output datasets
    pcl::IntegralImageNormalEstimation<pcl::PointXYZ, pcl::PointNormal> ne;
    ne.setNormalEstimationMethod(ne.AVERAGE_3D_GRADIENT);
    ne.setInputCloud(cloud);
    ne.compute(*cloud_normals);
    pcl::copyPointCloud(*cloud, *cloud_normals);
    return cloud_normals;
}

pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformPointCloud(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud, const Eigen::Matrix4f &transform) {
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformed_cloud(new pcl::PointCloud<pcl::PointXYZRGB>());
    pcl::transformPointCloud(*cloud, *transformed_cloud, transform);
    return transformed_cloud;
}

template <class T>
typename pcl::PointCloud<T>::Ptr transformPointCloudNormals(typename pcl::PointCloud<T>::Ptr cloud, const Eigen::Matrix4f &transform) {
    typename pcl::PointCloud<T>::Ptr transformed_cloud(new typename pcl::PointCloud<T>());
    pcl::transformPointCloudWithNormals(*cloud, *transformed_cloud, transform);
    return transformed_cloud;
}

pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr mergingPointClouds(std::vector<Frame3D> frames)
{
    std::cout << "Merging point clouds" << std::endl;
    pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr modelCloud(new pcl::PointCloud<pcl::PointXYZRGBNormal>);

    for (Frame3D frame: frames)
    {
        cv::Mat depthImage = frame.depth_image_;
        double focalLength = frame.focal_length_;
        const Eigen::Matrix4f cameraPose = frame.getEigenTransform();

        pcl::PointCloud<pcl::PointXYZ>::Ptr pointCloud = mat2IntegralPointCloud(depthImage, focalLength, 1);
        pcl::PointCloud<pcl::PointNormal>::Ptr pointCloudWithNormals = computeNormals(pointCloud);

        // remove NaNs from normals
        std::vector<int> indices_normals;
        pcl::PointCloud<pcl::PointNormal>::Ptr pointCloudWithNormalsFiltered(new pcl::PointCloud<pcl::PointNormal>);
        pcl::removeNaNNormalsFromPointCloud<pcl::PointNormal>(*pointCloudWithNormals, *pointCloudWithNormalsFiltered, indices_normals);

        pointCloudWithNormalsFiltered = transformPointCloudNormals<pcl::PointNormal>(pointCloudWithNormalsFiltered, cameraPose);
        pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud_xyzrgbnormal(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
        copyPointCloud(*pointCloudWithNormalsFiltered, *cloud_xyzrgbnormal);

        *modelCloud += *cloud_xyzrgbnormal;
    }

    return modelCloud;
}

// Different methods of constructing mesh
enum CreateMeshMethod {
    PoissonSurfaceReconstruction = 0,
    MarchingCubes = 1
};

// Create mesh from point cloud using one of above methods
pcl::PolygonMesh createMesh(pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr pointCloud, CreateMeshMethod method) {
    std::cout << "Creating meshes" << std::endl;

    // The variable for the constructed mesh

    pcl::PolygonMesh::Ptr triangles(new pcl::PolygonMesh);

    // TODO(Student): Call Poisson Surface Reconstruction. ~ 5 lines.
    // TODO: Experiment with different depth
    if (method == PoissonSurfaceReconstruction)
    {
        pcl::Poisson<pcl::PointXYZRGBNormal>::Ptr poisson(new pcl::Poisson<pcl::PointXYZRGBNormal>);
        poisson->setDepth(5);
        // poisson->setSamplesPerNode(5);

        poisson->setInputCloud(pointCloud);
        poisson->reconstruct(*triangles);
    }
    else if (method == MarchingCubes)
    {

        // TODO(Student): Call Marching Cubes Reconstruction. ~ 5 lines.
        // TODO: Experiment with different versions and parameters of marching cubes
        pcl::search::KdTree<pcl::PointXYZRGBNormal>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZRGBNormal>);
        tree->setInputCloud(pointCloud);

        pcl::MarchingCubesHoppe<pcl::PointXYZRGBNormal> mc;
        mc.setInputCloud(pointCloud);
        // mc.setPercentageExtendGrid(0.1);
        mc.setGridResolution(100, 100, 100);
        mc.setSearchMethod(tree);
        mc.reconstruct(*triangles);
    }

    return *triangles;
}

int median(std::vector<int> &v) {
    size_t n = v.size() / 2;
    std::nth_element(v.begin(), v.begin() + n, v.end());
    return v[n];
}

pcl::PolygonMesh addTexture(std::vector<Frame3D> frames, pcl::PolygonMesh triangles, int average_colors, bool reverse)
{
    // average_colors = 0:
    // average_colors = 1: average color for every vertex
    // average_colors = 2: median color for every vertex
    // reverse is used for processing frames in reversed order - useful when average_colors = 0

    Eigen::Vector2f uv_coordinates;

    pcl::PointCloud<pcl::PointXYZ>::Ptr point_cloud(new pcl::PointCloud<pcl::PointXYZ>());
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr point_cloud_coloured(new pcl::PointCloud<pcl::PointXYZRGB>());

    pcl::fromPCLPointCloud2(triangles.cloud, *point_cloud);
    pcl::fromPCLPointCloud2(triangles.cloud, *point_cloud_coloured);

    pcl::TextureMapping<pcl::PointXYZ> texture;

    int n_vertices = point_cloud_coloured->points.size();
    // sum of color intensities. first index: 0->b, 1->g, 2->r, 3->count
    // second index: vertex index.
    // In this way we can cumulate pixel intensities by channel and calculate an average
    // WARNING!!! the number of points may be too high, and the huge dimension of the array could cause the program to crash
    int* sums[4];//[point_cloud_coloured->points.size()];
    // std::vector<int> r[n_vertices], g[n_vertices], b[n_vertices];
    std::vector<int> *r, *g, *b;
    if (average_colors == 1){
        for(int i = 0; i < 4; i++){
            sums[i] = new int[point_cloud_coloured->points.size()];
        }
    }
    else if (average_colors == 2){
        // allocate only when needed, otherwise risk crashing
        r = new std::vector<int>[n_vertices];
        g = new std::vector<int>[n_vertices];
        b = new std::vector<int>[n_vertices];
        for(int i = 0; i < n_vertices; i++){
            b[i] = std::vector<int>(0);
            g[i] = std::vector<int>(0);
            r[i] = std::vector<int>(0);
        }
    }

    if (reverse){ // we might want to process frames in reverse order, as without averaging this impacts the final coloring
        std::reverse(frames.begin(), frames.end());
    }
    for (Frame3D frame: frames)
    {
        const Eigen::Matrix4f cameraPose = frame.getEigenTransform();

        cv::Mat rgbImage;
        cv::resize(frame.rgb_image_, rgbImage, frame.depth_image_.size());

        const Eigen::Matrix4f cameraPoseInv = cameraPose.inverse().eval();
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_inv(new pcl::PointCloud<pcl::PointXYZ>());
        pcl::transformPointCloud(*point_cloud, *cloud_inv, cameraPoseInv);

        pcl::TextureMapping<pcl::PointXYZ>::Octree::Ptr octree(new pcl::TextureMapping<pcl::PointXYZ>::Octree(0.01));
        octree->setInputCloud(cloud_inv);
        octree->addPointsFromInputCloud();

        pcl::texture_mapping::Camera camera;
        camera.focal_length = frame.focal_length_;
        camera.height = rgbImage.size().height;
        camera.width = rgbImage.size().width;

        // iterate over every vertex, and color the point cloud accordingly (or save results for averaging later)
        for (int k = 0; k < n_vertices; k++) {
            const pcl::PointXYZ vertices_points = cloud_inv->points.at(k);
            if (!texture.isPointOccluded(vertices_points, octree)) {
                texture.getPointUVCoordinates(vertices_points, camera, uv_coordinates);
                if (uv_coordinates[0] >= 0 && uv_coordinates[1] >= 0) {
                    int cols = round(uv_coordinates.x() * camera.width);
                    int rows = round(camera.height - uv_coordinates.y() * camera.height);

                    cv::Vec3b source_pixel = rgbImage.at<cv::Vec3b>(rows, cols);

                    if (average_colors == 0){ // replace the colors; sensitive to frame processing order
                        point_cloud_coloured->points.at(k).b = source_pixel[0];
                        point_cloud_coloured->points.at(k).g = source_pixel[1];
                        point_cloud_coloured->points.at(k).r = source_pixel[2];
                    }
                    else if (average_colors == 1){ // for calculating the mean colors
                        sums[0][k] += source_pixel[0];
                        sums[1][k] += source_pixel[1];
                        sums[2][k] += source_pixel[2];
                        sums[3][k] += 1;
                    }
                    else if (average_colors == 2){ // append to a vector to calculate the median
                        b[k].push_back(source_pixel[0]);
                        g[k].push_back(source_pixel[1]);
                        r[k].push_back(source_pixel[2]);
                    }
                }
            }
        }

    }
    // calculate average colors for every point
    if (average_colors == 1){
        for(int i = 0; i < point_cloud_coloured->points.size(); i++){
            if (sums[3][i] > 0){ // check the vertex has been processed
                point_cloud_coloured->points.at(i).b = (u_int8_t)(sums[0][i] / sums[3][i]);
                point_cloud_coloured->points.at(i).g = (u_int8_t)(sums[1][i] / sums[3][i]);
                point_cloud_coloured->points.at(i).r = (u_int8_t)(sums[2][i] / sums[3][i]);
            }
        }
    }
    // calculate median colors for every point
    else if (average_colors == 2){
         for(int i = 0; i < point_cloud_coloured->points.size(); i++){
            if (b[i].size() > 0){ // check the vertex has been processed
                // if (b[i].size() > 2) // for printing the number of views that see the same vertex
                    // std::cout << b[i].size() << std::endl;
                point_cloud_coloured->points.at(i).b = median(b[i]);
                point_cloud_coloured->points.at(i).g = median(g[i]);
                point_cloud_coloured->points.at(i).r = median(r[i]);
            }
        }
    }

    pcl::toPCLPointCloud2(*point_cloud_coloured, triangles.cloud);
    return triangles;
}

// Given a PolygonMesh input, fills its holes using vtk functions
// The input mesh is modified as it is passed by reference
void fillPolygonMeshHoles(pcl::PolygonMesh &triangles, float holeSize)
{
    vtkSmartPointer<vtkPolyData> vtkMesh = vtkSmartPointer<vtkPolyData>::New();
    pcl::VTKUtils::mesh2vtk(triangles, vtkMesh);

    vtkSmartPointer<vtkFillHolesFilter> fillHolesFilter = vtkSmartPointer<vtkFillHolesFilter>::New();
    fillHolesFilter->SetInputData(vtkMesh);
    fillHolesFilter->SetHoleSize(holeSize);
    fillHolesFilter->Update();

    pcl::VTKUtils::vtk2mesh(fillHolesFilter->GetOutput(), triangles);
}
// to run the file, use
// make -j2 && ./final ../../3dframes 1 t
// where first parameter is 0 for poisson, 1 for marching cubes
// second parameter is t for adding texture (color), any other value for not adding texture
int main(int argc, char *argv[])
{
    if (argc != 4)
    {
        std::cout << "./final [3DFRAMES PATH] [RECONSTRUCTION MODE] [TEXTURE_MODE]" << std::endl;

        return 0;
    }

    const CreateMeshMethod reconMode = static_cast<CreateMeshMethod>(std::stoi(argv[2]));

    // Loading 3D frames
    std::vector<Frame3D> frames = Frame3D::loadFrames(boost::str(boost::format("%s") % argv[1]));

    pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr texturedCloud;
    pcl::PolygonMesh triangles;

    // Create one point cloud by merging all frames with texture using
    // the rgb images from the frames
    texturedCloud = mergingPointClouds(frames);

    // Create a mesh from the textured cloud using a reconstruction method,
    // Poisson Surface or Marching Cubes
    triangles = createMesh(texturedCloud, reconMode);

    fillPolygonMeshHoles(triangles, 100.0f); // the hole size parameter is almost insignificant, in our case all values > 1 lead to the same results

    if (argv[3][0] == 't')
    {
        // SECTION 4: Coloring 3D Model
        std::cout << "Start adding color to the model" << std::endl;
        triangles = addTexture(frames, triangles, 0, 0);

    }

    // Sample code for visualization.

    // Show viewer
    std::cout << "Finished adding color to the model" << std::endl;
    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));

    // Add mesh
    viewer->setBackgroundColor(1, 1, 1);
    viewer->addPolygonMesh(triangles, "meshes", 0);
    viewer->addCoordinateSystem(1.0);
    viewer->initCameraParameters();

    // Keep viewer open
    while (!viewer->wasStopped())
    {
        viewer->spinOnce(100);
        boost::this_thread::sleep(boost::posix_time::microseconds(100000));
    }

    return 0;
}