/*
 * s2slayer.cpp
 *
 * Feed-forward:
 *    z(l) = a(l-1) * w + b // where * (vector, matrix) multiplication
 *    a(l) = activation(z(l))
 *
 * Back propagation:
 *    delta(l) = elem_prod(gradient(C, a(l)), activation_derivative(z(l)))
 *    dC/db(l) = delta(l)
 *    dC/dw(l) = a(l-1) * delta(l)
 *    gradient(C, a(l - 1)) = transp(w) * delta(l)
 */
#include <boost/assert.hpp>

#include "core/utils.h"
#include "core/random.h"
#include "core/functions.h"
#include "lstmlayer.h"
#include "s2slayer.h"

using namespace std;
using namespace boost;
using namespace yann;

namespace yann {

////////////////////////////////////////////////////////////////////////////////////////////////
//
// Seq2SeqLayer_Context implementation
//
class Seq2SeqLayer_Context :
    public Layer::Context
{
  typedef Layer::Context Base;

  friend class Seq2SeqLayer;

public:
  Seq2SeqLayer_Context(
      unique_ptr<Layer::Context> encoder_ctx,
      unique_ptr<Layer::Context> decoder_ctx) :
    Base(decoder_ctx->get_output()) // decoder output is our output
  {
    YANN_CHECK(encoder_ctx);
    YANN_CHECK(decoder_ctx);
    _encoder_ctx = std::move(encoder_ctx);
    _decoder_ctx = std::move(decoder_ctx);
  }

  inline bool is_valid() const
  {
    return _encoder_ctx && _decoder_ctx;
  }

  // Layer::Context overwrites
  virtual void start_epoch()
  {
    YANN_CHECK(is_valid());

    Base::start_epoch();

    _encoder_ctx->start_epoch();
    _decoder_ctx->start_epoch();
  }

  virtual void reset_state()
  {
    YANN_CHECK(is_valid());

    Base::reset_state();

    _encoder_ctx->reset_state();
    _decoder_ctx->reset_state();
  }
protected:
  unique_ptr<Layer::Context> _encoder_ctx;
  unique_ptr<Layer::Context> _decoder_ctx;
}; // class Seq2SeqLayer_Context

////////////////////////////////////////////////////////////////////////////////////////////////
//
// Seq2SeqLayer_TrainingContext implementation
//
class Seq2SeqLayer_TrainingContext :
    public Seq2SeqLayer_Context
{
  typedef Seq2SeqLayer_Context Base;
  friend class Seq2SeqLayer;

public:
  Seq2SeqLayer_TrainingContext(
      unique_ptr<Layer::Context> encoder_ctx,
      unique_ptr<Layer::Context> decoder_ctx) :
    Base(std::move(encoder_ctx), std::move(decoder_ctx))
  {
    _decoder_input_gradient.resize(_decoder_ctx->get_output_size());
    _decoder_output_gradient.resize(_decoder_ctx->get_output_size());
  }

protected:
  Vector _decoder_input_gradient;
  Vector _decoder_output_gradient;
}; // class Seq2SeqLayer_TrainingContext

}; // namespace yann

////////////////////////////////////////////////////////////////////////////////////////////////
//
// yann::Seq2SeqLayer implementation
//
std::unique_ptr<Seq2SeqLayer> yann::Seq2SeqLayer::create_lstm(
      const MatrixSize & input_size,
      const MatrixSize & output_size,
      const std::unique_ptr<ActivationFunction> & gate_activation_function,
      const std::unique_ptr<ActivationFunction> & io_activation_function)
{
  YANN_CHECK_GT(input_size, 0);
  YANN_CHECK_GT(output_size, 0);
  YANN_CHECK(gate_activation_function);
  YANN_CHECK(io_activation_function);

  auto encoder = make_unique<LstmLayer>(input_size, output_size);
  YANN_CHECK(encoder);
  encoder->set_activation_functions(gate_activation_function, io_activation_function);

  auto decoder = make_unique<LstmLayer>(output_size, output_size);
  YANN_CHECK(decoder);
  decoder->set_activation_functions(gate_activation_function, io_activation_function);

  return make_unique<Seq2SeqLayer>(std::move(encoder), std::move(decoder));
}

std::unique_ptr<Seq2SeqLayer> yann::Seq2SeqLayer::create_lstm(
    const MatrixSize & input_size,
    const MatrixSize & output_size,
    const std::unique_ptr<ActivationFunction> & activation_function)
{
  YANN_CHECK(activation_function);
  return create_lstm(input_size, output_size, activation_function, activation_function);
}

yann::Seq2SeqLayer::Seq2SeqLayer(std::unique_ptr<Layer> encoder, std::unique_ptr<Layer> decoder) :
    _encoder(std::move(encoder)),
    _decoder(std::move(decoder))
{
  YANN_CHECK_EQ(_encoder->get_output_size(), _decoder->get_input_size());
}

yann::Seq2SeqLayer::~Seq2SeqLayer()
{
}

// Layer overwrites
bool yann::Seq2SeqLayer::is_valid() const
{
  if(!Base::is_valid()) {
    return false;
  }
  if(!_encoder || !_encoder->is_valid()) {
    return false;
  }
  if(!_decoder || !_decoder->is_valid()) {
    return false;
  }
  return true;
}

std::string yann::Seq2SeqLayer::get_name() const
{
  return "Seq2SeqLayer";
}

string yann::Seq2SeqLayer::get_info() const
{
  YANN_CHECK(is_valid());

  ostringstream oss;
  oss << Base::get_info()
      << " encoder: " << _encoder->get_info()
      << ", decoder: " << _decoder->get_info()
  ;
  return oss.str();
}

bool yann::Seq2SeqLayer::is_equal(const Layer & other, double tolerance) const
{
  if(!Base::is_equal(other, tolerance)) {
    return false;
  }
  auto the_other = dynamic_cast<const Seq2SeqLayer*>(&other);
  if(the_other == nullptr) {
    return false;
  }
  if(is_valid() != the_other->is_valid()) {
    return false;
  }
  if(_encoder && !_encoder->is_equal(*the_other->_encoder, tolerance)) {
    return false;
  }
  if(_decoder && !_decoder->is_equal(*the_other->_decoder, tolerance)) {
    return false;
  }
  return true;
}

MatrixSize yann::Seq2SeqLayer::get_input_size() const
{
  return _encoder ? _encoder->get_input_size() : 0;
}

MatrixSize yann::Seq2SeqLayer::get_output_size() const
{
  return _decoder ? _decoder->get_output_size() : 0;
}

unique_ptr<Layer::Context> yann::Seq2SeqLayer::create_context(const MatrixSize & batch_size) const
{
  YANN_CHECK(is_valid());

  auto encoder_ctx = _encoder->create_context(batch_size);
  YANN_CHECK(encoder_ctx);
  auto decoder_ctx = _decoder->create_context(batch_size);
  YANN_CHECK(decoder_ctx);

  return make_unique<Seq2SeqLayer_Context>(std::move(encoder_ctx), std::move(decoder_ctx));
}
unique_ptr<Layer::Context> yann::Seq2SeqLayer::create_context(const RefVectorBatch & output) const
{
  YANN_CHECK(is_valid());

  auto encoder_ctx = _encoder->create_context(get_batch_size(output));
  YANN_CHECK(encoder_ctx);
  auto decoder_ctx = _decoder->create_context(output); // decoder output is our output
  YANN_CHECK(decoder_ctx);

  return make_unique<Seq2SeqLayer_Context>(std::move(encoder_ctx), std::move(decoder_ctx));
}
unique_ptr<Layer::Context> yann::Seq2SeqLayer::create_training_context(
    const MatrixSize & batch_size, const std::unique_ptr<Layer::Updater> & updater) const
{
  YANN_CHECK(is_valid());
  YANN_CHECK(updater);

  auto encoder_ctx = _encoder->create_training_context(batch_size, updater);
  YANN_CHECK(encoder_ctx);
  auto decoder_ctx = _decoder->create_training_context(batch_size, updater);
  YANN_CHECK(decoder_ctx);

  return make_unique<Seq2SeqLayer_TrainingContext>(std::move(encoder_ctx), std::move(decoder_ctx));
}
unique_ptr<Layer::Context> yann::Seq2SeqLayer::create_training_context(
    const RefVectorBatch & output, const std::unique_ptr<Layer::Updater> & updater) const
{
  YANN_CHECK(is_valid());
  YANN_CHECK(updater);

  auto encoder_ctx = _encoder->create_training_context(get_batch_size(output), updater);
  YANN_CHECK(encoder_ctx);
  auto decoder_ctx = _decoder->create_training_context(output, updater); // decoder output is our output
  YANN_CHECK(decoder_ctx);

  return make_unique<Seq2SeqLayer_TrainingContext>(std::move(encoder_ctx), std::move(decoder_ctx));
}

template<typename InputType>
void yann::Seq2SeqLayer::feedforward_internal(
    const InputType & input,
    Context * context,
    enum OperationMode mode) const
{
  auto ctx = dynamic_cast<Seq2SeqLayer_Context *>(context);
  YANN_CHECK(ctx);
  YANN_CHECK(ctx->is_valid());
  YANN_CHECK(is_valid());

  YANN_CHECK_LT(0, get_batch_size(input));
  YANN_CHECK_LE(get_batch_size(input), get_batch_size(ctx->get_output()));
  YANN_CHECK_EQ(get_batch_item_size(input), get_input_size());

  //
  // we assume all input is given to us "at once" no incremental feedforward
  //
  auto & encoder_ctx = ctx->_encoder_ctx;
  auto & decoder_ctx = ctx->_decoder_ctx;

  // feed input into encoder, overwrite the output
  _encoder->feedforward(input, encoder_ctx.get(), Operation_Assign);
  auto encoder_output_size = get_batch_size(encoder_ctx->get_output());
  YANN_SLOW_CHECK_GT(encoder_output_size, 0);
  auto state = get_batch(encoder_ctx->get_output(), encoder_output_size - 1); // last output

  // feed state into decoder and then feed prev output
  YANN_CHECK_EQ(mode, Operation_Assign); // we don't support anything else because we re-use output buffer
  for(MatrixSize ii = 0; ii < ctx->get_batch_size(); ++ii) {
    if(ii > 0) {
      auto prev_output = get_batch(decoder_ctx->get_output(), ii - 1);
      _decoder->feedforward(prev_output, decoder_ctx.get(), Operation_Assign);
    } else {
      _decoder->feedforward(state, decoder_ctx.get(), Operation_Assign);
    }
  }
}

void yann::Seq2SeqLayer::feedforward(
    const RefConstVectorBatch & input,
    Context * context,
    enum OperationMode mode) const
{
  feedforward_internal(input, context, mode);
}

void yann::Seq2SeqLayer::feedforward(
    const RefConstSparseVectorBatch & input,
    Context * context,
    enum OperationMode mode) const
{
  feedforward_internal(input, context, mode);
}

template<typename InputType>
void yann::Seq2SeqLayer::backprop_internal(
    const RefConstVectorBatch & gradient_output,
    const InputType & input,
    optional<RefVectorBatch> gradient_input,
    Context * context) const
{
  auto ctx = dynamic_cast<Seq2SeqLayer_TrainingContext *>(context);
  YANN_CHECK(ctx);
  YANN_CHECK(ctx->is_valid());
  YANN_CHECK(is_valid());
  YANN_CHECK_GT(get_batch_size(gradient_output), 0);
  YANN_CHECK_EQ(get_batch_item_size(gradient_output), get_output_size());
  YANN_CHECK_EQ(get_batch_size(input), get_batch_size(gradient_output));
  YANN_CHECK_EQ(get_batch_item_size(input), get_input_size());
  YANN_CHECK_EQ(get_batch_item_size(input), get_input_size());
  YANN_CHECK(!gradient_input || is_same_size(input, *gradient_input));

  //
  // we assume all output is given to us "at once" no incremental backprop
  //
  auto & encoder_ctx = ctx->_encoder_ctx;
  YANN_SLOW_CHECK(encoder_ctx);
  auto & decoder_ctx = ctx->_decoder_ctx;
  YANN_SLOW_CHECK(decoder_ctx);

  auto encoder_output_size = get_batch_size(encoder_ctx->get_output());
  YANN_SLOW_CHECK_GT(encoder_output_size, 0);
  auto state = get_batch(encoder_ctx->get_output(), encoder_output_size - 1); // last output

  // first we propagate back the output for decoder
  auto & decoder_input_gradient = ctx->_decoder_input_gradient;
  auto & decoder_output_gradient = ctx->_decoder_output_gradient;
  YANN_SLOW_CHECK(is_same_size(decoder_input_gradient, decoder_output_gradient));

  decoder_input_gradient.setZero();
  decoder_output_gradient.setZero();

  const auto batch_size = get_batch_size(gradient_output);
  for(MatrixSize ii = batch_size - 1; ii >= 0; --ii) {
    decoder_output_gradient += get_batch(gradient_output, ii);
    if(ii > 0) {
      auto decoder_prev_output = get_batch(decoder_ctx->get_output(), ii - 1);
      _decoder->backprop(
          decoder_output_gradient, // gradient_output
          decoder_prev_output,     // input
          make_optional<RefVectorBatch>(decoder_input_gradient),  // gradient_input
          decoder_ctx.get());
      swap(decoder_input_gradient, decoder_output_gradient);
    } else {
      _decoder->backprop(
          decoder_output_gradient, // gradient_output
          state,                   // input
          make_optional<RefVectorBatch>(decoder_input_gradient),  // gradient_input
          decoder_ctx.get());
    }
    // decoder_input_gradient contains the state gradient
  }

  // now propagate the gradient for encoder
  decoder_output_gradient.setZero();
  for(MatrixSize ii = batch_size - 1; ii >= 0; --ii) {
    auto gradient_in = gradient_input ? make_optional<RefVectorBatch>(get_batch(*gradient_input, ii)) : boost::none;
    InputType in = input.block(ii, 0, 1, input.cols()); // TODO: RowMajor, switch to get_batch()

    _encoder->backprop(
        decoder_input_gradient, // gradient_output
        in,   // input
        gradient_in,            // gradient_input
        encoder_ctx.get());
    if(ii == batch_size - 1) {
      decoder_input_gradient.setZero(); // we don't care about encoder output except the last vector
    }
  }
}

void yann::Seq2SeqLayer::backprop(
    const RefConstVectorBatch & gradient_output,
    const RefConstVectorBatch & input,
    optional<RefVectorBatch> gradient_input,
    Context * context) const
{
 backprop_internal(gradient_output, input, gradient_input, context);
}

void yann::Seq2SeqLayer::backprop(
    const RefConstVectorBatch & gradient_output,
    const RefConstSparseVectorBatch & input,
    optional<RefVectorBatch> gradient_input,
    Context * context) const
{
  backprop_internal<RefConstSparseVectorBatch>(gradient_output, input, gradient_input, context);
}

void yann::Seq2SeqLayer::init(enum InitMode mode, boost::optional<InitContext> init_context)
{
  YANN_CHECK(is_valid());
  _encoder->init(mode, init_context);
  _decoder->init(mode, init_context);
}

void yann::Seq2SeqLayer::update(Context * context, const size_t & tests_num)
{
  auto ctx = dynamic_cast<Seq2SeqLayer_TrainingContext *>(context);
  YANN_CHECK(ctx);
  YANN_CHECK(ctx->is_valid());
  YANN_CHECK(is_valid());

  _encoder->update(ctx->_encoder_ctx.get(), tests_num);
  _decoder->update(ctx->_decoder_ctx.get(), tests_num);
}

// the format is (e:<encoder>,d:<decoder>)
void yann::Seq2SeqLayer::read(std::istream & is)
{
  YANN_CHECK(is_valid());

  Base::read(is);

  read_char(is, '(');
  read_object(is, "e", *_encoder);
  read_char(is, ',');
  read_object(is, "d", *_decoder);
  read_char(is, ')');
}

// the format is (e:<encoder>,d:<decoder>)
void yann::Seq2SeqLayer::write(std::ostream & os) const
{
  YANN_CHECK(is_valid());

  Base::write(os);

  os << "(";
  write_object(os, "e", *_encoder);
  os << ",";
  write_object(os, "d", *_decoder);
  os << ")";
}


/*******************************************************************************
 * Copyright (c) 2018-, UT-Battelle, LLC.
 * All rights reserved. This program and the accompanying materials
 * are made available under the terms of the MIT License
 * which accompanies this distribution.
 *
 * Contributors:
 *   Alexander J. McCaskey - initial API and implementation
 *   Thien Nguyen - implementation
 *******************************************************************************/
#include "qcor_observable.hpp"

#include "ObservableTransform.hpp"
#include "xacc.hpp"
// #include "xacc_quantum_gate_api.hpp"
#include <spdlog/fmt/fmt.h>
#include <Eigen/Dense>

#include <algorithm>
#include <cassert>

#include "CompositeInstruction.hpp"
#include "FermionOperator.hpp"
#include "PauliOperator.hpp"
#include "qalloc.hpp"
#include "qcor_ir.hpp"
#include "qcor_pimpl_impl.hpp"
#include "xacc_internal_compiler.hpp"
#include "xacc_quantum_gate_api.hpp"
#include "xacc_service.hpp"

namespace qcor {

// ---------------- Operator ---------------------- //

// Internal hidden implementation
class Operator::OperatorImpl
    : public commutative_ring<OperatorImpl>,
      public equality_comparable<OperatorImpl>,
      public commutative_multipliable<OperatorImpl, double>,
      public commutative_multipliable<OperatorImpl, std::complex<double>> {
  friend class Operator;

 private:
  enum operation { PlusEquals, MinusEqual, StarEqual };

  // This function performs sub-class specific algebraic operation
  // with another operator instance, or scalar value (double, complex)
  template <typename T>
  struct cast_and_apply {
    void operator()(std::shared_ptr<xacc::Observable> obs, operation _op,
                    const OperatorImpl &other) {
      auto input_casted = std::dynamic_pointer_cast<T>(obs);
      auto other_casted = std::dynamic_pointer_cast<T>(other.op);
      assert(input_casted && other_casted &&
             "Invalid Operator sub-types for arithmetic operation.");

      switch (_op) {
        case operation::PlusEquals:
          input_casted->operator+=(*other_casted.get());
          break;
        case operation::MinusEqual:
          input_casted->operator-=(*other_casted.get());
          break;
        case operation::StarEqual:
          input_casted->operator*=(*other_casted.get());
          break;
        default:
          std::cout << "Invalid Selection\n";
          exit(0);
          break;
      }
    }

    void operator()(std::shared_ptr<xacc::Observable> obs,
                    const double &value) {
      auto input_casted = std::dynamic_pointer_cast<T>(obs);
      assert(input_casted &&
             "Invalid Operator sub-types for arithmetic operation.");
      input_casted->operator*=(value);
    }

    void operator()(std::shared_ptr<xacc::Observable> obs,
                    const std::complex<double> &value) {
      auto input_casted = std::dynamic_pointer_cast<T>(obs);
      assert(input_casted &&
             "Invalid Operator sub-types for arithmetic operation.");
      input_casted->operator*=(value);
    }
  };

  // Apply the desired operation between this operator an the given other operator
  OperatorImpl &apply(const operation &_op, const OperatorImpl &other) {
    if (type == "pauli") {
      cast_and_apply<xacc::quantum::PauliOperator>()(op, _op, other);
    } else if (type == "fermion") {
      cast_and_apply<xacc::quantum::FermionOperator>()(op, _op, other);
    } else {
      // FIXME / TODO As we add more Operator types, update
      // this if/else section
      error("Invalid Operator type for the given algebraic operation.");
    }
    return *this;
  }

  // Apply the desired operation between this operator an the given scalar double
  OperatorImpl &apply(const double &d) {
    if (type == "pauli") {
      cast_and_apply<xacc::quantum::PauliOperator>()(op, d);
    } else if (type == "fermion") {
      cast_and_apply<xacc::quantum::FermionOperator>()(op, d);
    } else {
      // FIXME / TODO As we add more Operator types, update
      // this if/else section
      error("Invalid Operator type for the given algebraic operation.");
    }
    return *this;
  }

  // Apply the desired operation between this operator an the given scalar complex
  OperatorImpl &apply(const std::complex<double> &d) {
    if (type == "pauli") {
      cast_and_apply<xacc::quantum::PauliOperator>()(op, d);
    } else if (type == "fermion") {
      cast_and_apply<xacc::quantum::FermionOperator>()(op, d);
    } else {
      // FIXME / TODO As we add more Operator types, update
      // this if/else section
      error("Invalid Operator type for the given algebraic operation.");
    }
    return *this;
  }

  std::string type;
  std::shared_ptr<xacc::Observable> op;

 public:
  // Internal impl constructed from HetMap of options, or string-like expression
  OperatorImpl() = default;
  OperatorImpl(const std::string &_type, const std::string &expr)
      : type(_type) {
    op = xacc::quantum::getObservable(type, expr);
  }

  OperatorImpl(const std::string &name, xacc::HeterogeneousMap &options) {
    auto tmp_op = xacc::quantum::getObservable(name, options);
    auto obs_str = tmp_op->toString();

    if (obs_str.find("^") != std::string::npos) {

      op = xacc::quantum::getObservable("fermion", obs_str);
      type = "fermion";

    } else if (obs_str.find("X") != std::string::npos ||
               obs_str.find("Y") != std::string::npos ||
               obs_str.find("Z") != std::string::npos) {
      op = xacc::quantum::getObservable("pauli", obs_str);
      type = "pauli";
    }
  }

  OperatorImpl(const std::string &_type, std::shared_ptr<xacc::Observable> _op)
      : type(_type), op(_op) {}
  OperatorImpl(const OperatorImpl &other) : type(other.type), op(other.op) {}

  // Implement internal Algebraic API
  OperatorImpl &operator+=(const OperatorImpl &v) noexcept {
    return apply(operation::PlusEquals, v);
  }

  OperatorImpl &operator-=(const OperatorImpl &v) noexcept {
    return apply(operation::MinusEqual, v);
  }
  OperatorImpl &operator*=(const OperatorImpl &v) noexcept {
    return apply(operation::StarEqual, v);
  }
  OperatorImpl &operator*=(const double v) noexcept { return apply(v); }
  OperatorImpl &operator*=(const std::complex<double> v) noexcept {
    return apply(v);
  }

  bool operator==(const OperatorImpl &v) noexcept {
    if (type == "pauli") {
      auto casted = std::dynamic_pointer_cast<xacc::quantum::PauliOperator>(op);
      auto other_casted =
          std::dynamic_pointer_cast<xacc::quantum::PauliOperator>(v.op);
      assert(casted && other_casted && "Invalid types for Operator == check.");
      return casted->operator==(*other_casted.get());
    } else if (type == "fermion") {
      auto casted =
          std::dynamic_pointer_cast<xacc::quantum::FermionOperator>(op);
      auto other_casted =
          std::dynamic_pointer_cast<xacc::quantum::FermionOperator>(v.op);
      assert(casted && other_casted && "Invalid types for Operator == check.");
      return casted->operator==(*other_casted.get());
    } else {
      // FIXME / TODO As we add more Operator types, update
      // this if/else section
    }
    return false;
  }

  std::vector<Operator> getSubTerms() {
    std::vector<Operator> ret;
    for (auto sub_term : op->getSubTerms()) {
      ret.emplace_back(OperatorImpl(type, sub_term));
    }
    return ret;
  }

  std::vector<Operator> getNonIdentitySubTerms() {
    std::vector<Operator> ret;
    for (auto sub_term : op->getNonIdentitySubTerms()) {
      ret.emplace_back(OperatorImpl(type, sub_term));
    }
    return ret;
  }

  Operator getIdentitySubTerm() {
    auto id_term = op->getIdentitySubTerm();
    if (!id_term) {
      // THROW AN ERROR.
     error("There is no identity sub term. exiting.");
    }
    return Operator(OperatorImpl(type, op->getIdentitySubTerm()));
  }

  bool hasIdentitySubTerm() { return op->getIdentitySubTerm() != nullptr; }
  std::complex<double> coefficient() { return op->coefficient(); }

  std::vector<SparseElement> to_sparse_matrix() {
    auto sp_el = op->to_sparse_matrix();
    std::vector<SparseElement> ret;
    for (auto el : sp_el) {
      ret.emplace_back(el.row(), el.col(), el.coeff());
    }
    return ret;
  }

  Operator commutator(Operator &other) {
    return Operator(OperatorImpl(type, op->commutator(other.m_internal->op)));
  }

  std::pair<std::vector<int>, std::vector<int>> toBinaryVectors(
      const int nQubits) {
    assert(type == "pauli" && "toBinaryVectors only works for pauli operators");
    return std::dynamic_pointer_cast<xacc::quantum::PauliOperator>(op)
        ->toBinaryVectors(nQubits);
  }

  void mapQubitSites(std::map<int, int> &siteMap) {
    assert(type == "pauli" && "mapQubitSites only works for pauli operators");
    std::dynamic_pointer_cast<xacc::quantum::PauliOperator>(op)->mapQubitSites(
        siteMap);
  }
};

Operator &Operator::operator=(const Operator &other) {
  m_internal->op = other.m_internal->op;
  m_internal->type = other.m_internal->type;
  return *this;
}

Operator::Operator(const std::string &name, xacc::HeterogeneousMap &options)
    : m_internal(name, options) {}
Operator::Operator(const std::string &type, const std::string &expr)
    : m_internal(type, expr) {}
Operator::Operator(const Operator &op)
    : m_internal(op.m_internal.operator->()->type,
                 op.m_internal.operator->()->op->toString()) {}
Operator::Operator(const OperatorImpl &&impl)
    : m_internal(impl.type, impl.op) {}

Operator::Operator() = default;
Operator::~Operator() = default;

std::shared_ptr<xacc::Identifiable> Operator::get_as_opaque() {
  return std::dynamic_pointer_cast<xacc::Identifiable>(m_internal->op);
}

Operator &Operator::operator+=(const Operator &v) noexcept {
  OperatorImpl *other = v.m_internal.operator->();
  m_internal->operator+=(*other);
  return *this;
}

Operator &Operator::operator-=(const Operator &v) noexcept {
  OperatorImpl *other = v.m_internal.operator->();
  m_internal->operator-=(*other);
  return *this;
}
Operator &Operator::operator*=(const Operator &v) noexcept {
  OperatorImpl *other = v.m_internal.operator->();
  m_internal->operator*=(*other);
  return *this;
}
bool Operator::operator==(const Operator &v) noexcept {
  OperatorImpl *other = v.m_internal.operator->();
  return m_internal->operator==(*other);
}
Operator &Operator::operator*=(const double v) noexcept {
  m_internal->operator*=(v);
  return *this;
}
Operator &Operator::operator*=(const std::complex<double> v) noexcept {
  m_internal->operator*=(v);
  return *this;
}

int Operator::nQubits() { return m_internal->op->nBits(); }

std::pair<std::vector<int>, std::vector<int>> Operator::toBinaryVectors(
    const int nQubits) {
  return m_internal->toBinaryVectors(nQubits);
}
void Operator::mapQubitSites(std::map<int, int> &siteMap) {
  return m_internal->mapQubitSites(siteMap);
}

std::vector<std::shared_ptr<CompositeInstruction>> Operator::observe(
    std::shared_ptr<CompositeInstruction> program) {
  auto as_xacc = program->as_xacc();
  auto cis = m_internal->op->observe(as_xacc);
  std::vector<std::shared_ptr<CompositeInstruction>> ret;
  for (auto ci : cis) {
    ret.emplace_back(std::make_shared<CompositeInstruction>(
        std::dynamic_pointer_cast<xacc::Identifiable>(ci)));
  }

  return ret;
}

std::vector<Operator> Operator::getSubTerms() {
  return m_internal->getSubTerms();
}

std::vector<Operator> Operator::getNonIdentitySubTerms() {
  return m_internal->getNonIdentitySubTerms();
}
bool Operator::hasIdentitySubTerm() { return m_internal->hasIdentitySubTerm(); }

std::string Operator::toString() const { return m_internal->op->toString(); }
Operator Operator::getIdentitySubTerm() {
  return m_internal->getIdentitySubTerm();
}
std::complex<double> Operator::coefficient() {
  return m_internal->coefficient();
}

std::vector<Operator::SparseElement> Operator::to_sparse_matrix() {
  return m_internal->to_sparse_matrix();
}

Operator Operator::commutator(Operator &op) {
  return m_internal->commutator(op);
}

Operator Operator::transform(const std::string &type,
                             xacc::HeterogeneousMap m) {
  auto transformed =
      xacc::getService<xacc::ObservableTransform>(type)->transform(
          m_internal->op);
  auto new_type =
      std::dynamic_pointer_cast<xacc::quantum::FermionOperator>(transformed)
          ? "fermion"
          : "pauli";
  return Operator(OperatorImpl(new_type, transformed));
}

void __internal_exec_observer(
    xacc::AcceleratorBuffer *b,
    std::vector<std::shared_ptr<CompositeInstruction>> v) {
  // auto vv = v.get()->operator->()->program;
  std::vector<std::shared_ptr<xacc::CompositeInstruction>> tmp;
  std::transform(v.begin(), v.end(), std::back_inserter(tmp),
                 [](std::shared_ptr<CompositeInstruction> c)
                     -> std::shared_ptr<xacc::CompositeInstruction> {
                   return c->as_xacc();
                 });
  xacc::internal_compiler::execute(b, tmp);
}

Operator operator+(double coeff, Operator op) {
  return Operator("pauli", fmt::format("{}", coeff)) + op;
}
Operator operator+(Operator op, double coeff) {
  return op + Operator("pauli", fmt::format("{}", coeff));
}

Operator operator-(double coeff, Operator op) {
  return Operator("pauli", fmt::format("{}", coeff)) - op;
}

Operator operator-(Operator op, double coeff) {
  return op - Operator("pauli", fmt::format("{}", coeff));
}

Operator adag(int idx) {
  return Operator("fermion", fmt::format("1.0 {}^", idx));
}
Operator a(int idx) { return Operator("fermion", fmt::format("1.0 {}", idx)); }

Operator X(int idx) { return Operator("pauli", fmt::format("X{}", idx)); }
Operator Y(int idx) { return Operator("pauli", fmt::format("Y{}", idx)); }
Operator Z(int idx) { return Operator("pauli", fmt::format("Z{}", idx)); }

Operator allZs(const int nQubits) {
  auto ret = Z(0);
  for (int i = 1; i < nQubits; i++) {
    ret *= Z(i);
  }
  return ret;
}

Operator SP(int idx) {
  std::complex<double> imag(0.0, 1.0);
  return X(idx) + imag * Y(idx);
}

Operator SM(int idx) {
  std::complex<double> imag(0.0, 1.0);
  return X(idx) - imag * Y(idx);
}

Eigen::MatrixXcd get_dense_matrix(Operator &op) {
  auto mat_el = op.to_sparse_matrix();
  auto size = std::pow(2, op.nBits());
  Eigen::MatrixXcd mat = Eigen::MatrixXcd::Zero(size, size);
  for (auto el : mat_el) {
    mat(el.row(), el.col()) = el.coeff();
  }
  return mat;
}

Operator createOperator(const std::string &repr) {
  if (!xacc::isInitialized())
    xacc::internal_compiler::compiler_InitializeXACC();
  return qcor::Operator("pauli", repr);
}

Operator createOperator(const std::string &name, const std::string &repr) {
  if (!xacc::isInitialized())
    xacc::internal_compiler::compiler_InitializeXACC();
  return qcor::Operator(name, repr);
}

Operator createOperator(const std::string &name, HeterogeneousMap &&options) {
  return createOperator(name, options);
}
Operator createOperator(const std::string &name, HeterogeneousMap &options) {
  return qcor::Operator(name, options);
}

Operator createObservable(const std::string &repr) {
  return createOperator(repr);
}

Operator createObservable(const std::string &name, const std::string &repr) {
  return createOperator(name, repr);
}

Operator operatorTransform(const std::string &type, Operator &op) {
  return op.transform(type);
}

Operator _internal_python_createObservable(const std::string &name,
                                           const std::string &repr) {
  return createOperator(name, repr);
}

namespace __internal__ {
std::map<std::size_t, Operator> cached_observables = {};

std::vector<std::shared_ptr<CompositeInstruction>> observe(
    Operator &obs, std::shared_ptr<CompositeInstruction> program) {
  return obs.observe(program);
}
}  // namespace __internal__

double observe(std::shared_ptr<CompositeInstruction> program, Operator &obs,
               xacc::internal_compiler::qreg &q) {
  // Observe the program
  auto v = obs.observe(program);

  std::vector<std::shared_ptr<xacc::CompositeInstruction>> tmp;
  std::transform(v.begin(), v.end(), std::back_inserter(tmp),
                 [](std::shared_ptr<CompositeInstruction> c)
                     -> std::shared_ptr<xacc::CompositeInstruction> {
                   return c->as_xacc();
                 });
  xacc::internal_compiler::execute(q.results(), tmp);

  // We want to contract q children buffer
  // exp-val-zs with obs term coeffs
  return q.weighted_sum(
      std::dynamic_pointer_cast<xacc::Observable>(obs.get_as_opaque()).get());
}

}  // namespace qcor

std::ostream &operator<<(std::ostream &os, qcor::Operator const &m) {
  return os << m.toString();
}


#include <kr_serial_interface/comm_types.h>
#include <kr_serial_interface/decode_msgs.h>

#include <Eigen/Geometry>

namespace kr_mav_msgs
{
bool decodeOutputData(const std::vector<uint8_t> &data, kr_mav_msgs::OutputData &output)
{
  struct OUTPUT_DATA output_data;
  if(data.size() != sizeof(output_data))
    return false;

  memcpy(&output_data, &data[0], sizeof(output_data));
  output.loop_rate = output_data.loop_rate;
  output.voltage = output_data.voltage / 1e3f;

  const double roll = output_data.roll / 1e2f * M_PI / 180;
  const double pitch = output_data.pitch / 1e2f * M_PI / 180;
  const double yaw = output_data.yaw / 1e2f * M_PI / 180;
  // Asctec (2012 firmware) uses  Z-Y-X convention
  Eigen::Quaternionf q = Eigen::AngleAxisf(yaw, Eigen::Vector3f::UnitZ()) *
                         Eigen::AngleAxisf(pitch, Eigen::Vector3f::UnitY()) *
                         Eigen::AngleAxisf(roll, Eigen::Vector3f::UnitX());
  output.orientation.w = q.w();
  output.orientation.x = q.x();
  output.orientation.y = q.y();
  output.orientation.z = q.z();

  output.angular_velocity.x = output_data.ang_vel[0] * 0.0154f * M_PI / 180;
  output.angular_velocity.y = output_data.ang_vel[1] * 0.0154f * M_PI / 180;
  output.angular_velocity.z = output_data.ang_vel[2] * 0.0154f * M_PI / 180;

  output.linear_acceleration.x = output_data.acc[0] / 1e3f * 9.81f;
  output.linear_acceleration.y = output_data.acc[1] / 1e3f * 9.81f;
  output.linear_acceleration.z = output_data.acc[2] / 1e3f * 9.81f;

  output.pressure_dheight = output_data.dheight / 1e3f;
  output.pressure_height = output_data.height / 1e3f;

  output.magnetic_field.x = output_data.mag[0] / 2500.0f;
  output.magnetic_field.y = output_data.mag[1] / 2500.0f;
  output.magnetic_field.z = output_data.mag[2] / 2500.0f;

  for(int i = 0; i < 8; i++)
  {
    output.radio_channel[i] = output_data.radio[i];
  }

  // Asctec firmware uses the following rotor numbering convention:
  //   *1*    Front
  // 3     4
  //    2
  //
  // But we want:
  //   *1*    Front
  // 2     4
  //    3
  const int motors_map[] = {0, 2, 1, 3};
  for(int i = 0; i < 4; i++)
  {
    // The following conversion is from
    // http://wiki.asctec.de/display/AR/List+of+all+predefined+variables%2C+commands+and+parameters
    //
    // motorRPM = 1075+m*37.625
    //
    // Note: If m == 0, the motors are not spinning.
    int m = output_data.rpm[motors_map[i]];
    output.motor_rpm[i] = (m == 0 ? 0 : 1075) + m * 37.625;
  }

  output.seq = output_data.seq;

  return true;
}

bool decodeStatusData(const std::vector<uint8_t> &data, kr_mav_msgs::StatusData &status)
{
  struct STATUS_DATA status_data;
  if(data.size() != sizeof(status_data))
    return false;
  memcpy(&status_data, &data[0], sizeof(status_data));

  status.loop_rate = status_data.loop_rate;
  status.voltage = status_data.voltage / 1e3f;
  status.seq = status_data.seq;

  return true;
}

}  // namespace kr_mav_msgs


#include <iostream>
#include <list>
#include <map>

#include <Eigen/Dense>

#include <openmht/multi/MHT.h>
#include <openmht/plot/Plot.h>

#include <boost/random.hpp>
#include <boost/random/variate_generator.hpp>
#include <boost/math/distributions/uniform.hpp>
#include <boost/random/normal_distribution.hpp>

using std::cout;
using std::endl;

class Contact {
public:
     int id;
     Eigen::Vector2d position;
     Eigen::Vector2d velocity;
};

boost::mt19937 gener_;
boost::normal_distribution<> normal_dist_(0,1.0);
boost::variate_generator<boost::mt19937&,
                         boost::normal_distribution<> > rng_normal(gener_,normal_dist_);

void step_dynamics(std::list<Contact> &contacts, double dt)
{
     for (std::list<Contact>::iterator it = contacts.begin();
          it != contacts.end(); it++) {
          it->position = it->position + it->velocity*dt;
     }
}

Contact add_noise(Contact &contact)
{
     Contact c = contact;
     c.position(0,0) = c.position(0,0) + rng_normal();
     c.position(1,0) = c.position(1,0) + rng_normal();
     return c;
}

int main(int argc, char *argv[])
{
     rng_normal.engine().seed(static_cast<unsigned int>(std::time(0)));
     rng_normal.distribution().reset();

     int num_contacts = 3;
     std::list<Contact> contacts;
     for (int i = 0; i < num_contacts; i++) {
          Contact c;
          c.id = i;
          c.position << rng_normal()*10, rng_normal()*10;
          c.velocity << rng_normal()*1, rng_normal()*1;
          contacts.push_back(c);
     }

     for (int i = 0; i < 100; i++) {
          cout << rng_normal() << endl;
     }

     double t0 = 0;
     double dt = 1;
     double tend = 20;

     openmht::Plot plot;
     std::map<int, std::list<Eigen::Vector2d> > truth, measured, tracked;

     openmht::MHT mht;
     mht.set_dt(dt);
     for (double t = t0; t < tend; t += dt) {

          // Convert from your list of 2D detection points to a list of
          std::list<openmht::Measurement> m_list;
          for (std::list<Contact>::iterator it = contacts.begin();
               it != contacts.end(); it++) {

               Contact c = add_noise(*it);

               openmht::Measurement m;
               m.set_position(c.position);
               m_list.push_back(m);

               // Save the measured position for plotting later
               measured[c.id].push_back(c.position);
          }

          // Process the list of measurements with MHT
          mht.process_measurements(m_list);

          // Get a list of the fused entities / contacts:
          std::list<openmht::Entity> ents = mht.entities();

          // Save the filtered locations of each track
          for (std::list<openmht::Entity>::iterator it = ents.begin();
               it != ents.end(); it++) {
               tracked[it->id()].push_back(it->position());
          }

          // Save the truth tracks:
          for (std::list<Contact>::iterator it = contacts.begin();
               it != contacts.end(); it++) {
               truth[it->id].push_back(it->position);
          }

          // Plot the results:
          std::string title = "Tracks";
          std::list< std::map<int, std::list<Eigen::Vector2d> > > lists;
          std::list<std::string> labels;
          std::list<std::string> styles;
          std::string options = "";
          std::list<std::string> objects;

          lists.push_back(truth);
          labels.push_back("Truth");
          styles.push_back("linespoints");

          lists.push_back(measured);
          labels.push_back("Measured");
          styles.push_back("linespoints");

          lists.push_back(tracked);
          labels.push_back("Tracked");
          styles.push_back("linespoints");

          plot.plot(lists, title, labels, styles, options, objects, false);

          // Step dynamics:
          step_dynamics(contacts,dt);
     }
     plot.wait();
}


/*! \file swaptionSmileMurexExport.hpp
    \brief Export swaption volatility structure to a xml file readable by Murex
	since Murex expects absolute volatilities but stores spreads w.r.t. a reference atm surface
	it is possible to import this reference atm surface from xml and use it to compute absolute
	volatilities via reference atm + given volstructure smile spread
	Peter Caspers
*/

#include <ql/quantlib.hpp>
#include <iostream>
#include <fstream>
#include <boost/property_tree/detail/rapidxml.hpp>
#include <map>

#ifndef quantlib_swaptionSmileMurexExport_hpp
#define quantlib_swaptionSmileMurexExport_hpp

using namespace boost;
using namespace std;

namespace QuantLib {

		/*! Exporter is set up by swaption volatility structure, xml output path,
		    Murex specific nickname (e.g. MDS), date (e.g. TODAY), swap (e.g. EUR SW VOL),
			option names, swap names (must match exactly Murex names, e.g. 3M, 6M, 1Y, including case sensitivity),
			strike spreads (e.g. -0.02, -0.01, -0.05, 0.025, 0.0, 0.025, 0.05, 0.01, 0.02),
			optionally an in path for the reference atm surface which is used to compute absolute volatilities from smile spreads */

	bool swaptionSmileMurexExport(boost::shared_ptr<SwaptionVolatilityStructure> vol,
				string outPath, string nickname, string date, string swap,
				vector<string>& optionNames, vector<string>& swapNames,
				vector<Period>& optionTenors, vector<Period>& swapTenors,
				vector<double> strikeSpreads, string referenceAtmInPath="");

}

#endif


#include <Eigen/Eigen>

#include <calibu/Calibu.h>
#include <pangolin/pangolin.h>
#include <pangolin/glcuda.h>
#include <pangolin/glvbo.h>
#include <SceneGraph/SceneGraph.h>
#include <sophus/se3.hpp>

#include <kangaroo/kangaroo.h>
#include <kangaroo/BoundedVolume.h>
#include <kangaroo/MarchingCubes.h>
#include <kangaroo/extra/BaseDisplayCuda.h>
#include <kangaroo/extra/BaselineFromCamModel.h>
#include <kangaroo/extra/DisplayUtils.h>
#include <kangaroo/extra/ImageSelect.h>
#include <kangaroo/extra/Handler3dGpuDepth.h>
#include <kangaroo/extra/RpgCameraOpen.h>
#include <kangaroo/extra/SavePPM.h>
#include <kangaroo/extra/SaveMeshlab.h>

#ifdef HAVE_CVARS
#include <kangaroo/extra/CVarHelpers.h>
#endif // HAVE_CVARS

template<typename T>
bool isFinite(const T& x)
{
  return x==x;
}

int main(int argc, char* argv[])
{
  // Initialise window
  pangolin::View& container = SetupPangoGLWithCuda(1024, 768);
  SceneGraph::GLSceneGraph::ApplyPreferredGlSettings();

  // Open video device
  hal::Camera video = OpenRpgCamera(argc,argv);

  // Capture first image
  std::shared_ptr<hal::ImageArray> images = hal::ImageArray::Create();

  // Open video device
  const bool use_colour = video.NumChannels() == 2;

  const int w = video.Width();
  const int h = video.Height();

  const int MaxLevels = 4;
  const int its[] = {1,0,2,3};

  // Load Camera intrinsics from file
  roo::ImageIntrinsics K;
  double baseline_m = 0.08; // Default.

  GetPot clArgs(argc, argv);
  const std::string filename = clArgs.follow("","-cmod");
  if (filename.empty()) {

    std::cout << "No camera model provided. Using generic camera model based on image dimensions." << std::endl;
    const double depth_focal = w * 570.342/640.0;;
    K = roo::ImageIntrinsics(depth_focal,depth_focal, w/2.0 - 0.5, h/2.0 - 0.5 );
  } else {
    std::shared_ptr<calibu::Rig<double>> rig = calibu::ReadXmlRig(filename);
    Eigen::Matrix3f cam_model = rig->cameras_[0]->K().cast<float>();
    K = roo::ImageIntrinsics(cam_model(0,0), cam_model(1,1), cam_model(0,2), cam_model(1,2));

    if (use_colour) {
      // Estimate baseline.
      Eigen::Matrix3d RDFvision;
      RDFvision << 1,0,0,  0,1,0,  0,0,1;
      const Sophus::SE3d T_rl = T_rlFromCamModelRDF(rig->cameras_[0], rig->cameras_[1], RDFvision);

      baseline_m = T_rl.translation().norm();
    }
  }

  // Near and far planes for bounding box.
  const double knear = 0.4;
  const double kfar = 4;

  //    // DepthSense
  //    const double baseline_m = 0.02;
  //    const roo::ImageIntrinsics K(224.501999, 230.494003, w/2.0 - 0.5, h/2.0 - 0.5 );
  //    const double knear = 0.15;
  //    const double kfar = 2.0;

  const float volrad = 2.0;
  const int volres = 256;

  roo::BoundingBox reset_bb(make_float3(-volrad,-volrad,knear), make_float3(volrad,volrad,knear+2*volrad));
  //    roo::BoundingBox reset_bb(make_float3(-volrad,-volrad,-volrad), make_float3(volrad,volrad,volrad));

#ifdef HAVE_CVARS
  CVarUtils::AttachCVar<roo::BoundingBox>("BoundingBox", &reset_bb);
#endif // HAVE_CVARS

  // Camera (rgb) to depth
  Eigen::Vector3d c_d(baseline_m,0,0);
  Sophus::SE3d T_cd = Sophus::SE3d(Sophus::SO3d(),c_d).inverse();

  roo::Image<unsigned short, roo::TargetDevice, roo::Manage> dKinect(w,h);
  roo::Image<uchar3, roo::TargetDevice, roo::Manage> drgb(w,h);
  roo::Image<float, roo::TargetDevice, roo::Manage> dKinectMeters(w,h);
  roo::Pyramid<float, MaxLevels, roo::TargetDevice, roo::Manage> kin_d(w,h);
  roo::Pyramid<float4, MaxLevels, roo::TargetDevice, roo::Manage> kin_v(w,h);
  roo::Pyramid<float4, MaxLevels, roo::TargetDevice, roo::Manage> kin_n(w,h);
  roo::Image<float4, roo::TargetDevice, roo::Manage>  dDebug(w,h);
  roo::Image<unsigned char, roo::TargetDevice, roo::Manage> dScratch(w*sizeof(roo::LeastSquaresSystem<float,12>),h);

  roo::Pyramid<float, MaxLevels, roo::TargetDevice, roo::Manage> ray_i(w,h);
  roo::Pyramid<float, MaxLevels, roo::TargetDevice, roo::Manage> ray_d(w,h);
  roo::Pyramid<float4, MaxLevels, roo::TargetDevice, roo::Manage> ray_n(w,h);
  roo::Pyramid<float4, MaxLevels, roo::TargetDevice, roo::Manage> ray_v(w,h);
  roo::Pyramid<float4, MaxLevels, roo::TargetDevice, roo::Manage> ray_c(w,h);
  roo::BoundedVolume<roo::SDF_t, roo::TargetDevice, roo::Manage> vol(volres,volres,volres,reset_bb);
  roo::BoundedVolume<float, roo::TargetDevice, roo::Manage> colorVol(volres,volres,volres,reset_bb);

  std::vector<std::unique_ptr<KinectKeyframe> > keyframes;
  roo::Mat<roo::ImageKeyframe<uchar3>,10> kfs;

  SceneGraph::GLSceneGraph glgraph;
  SceneGraph::GLAxis glcamera(0.1);
  SceneGraph::GLAxisAlignedBox glboxfrustum;
  SceneGraph::GLAxisAlignedBox glboxvol;

  glboxvol.SetBounds(roo::ToEigen(vol.bbox.Min()), roo::ToEigen(vol.bbox.Max()) );
  glgraph.AddChild(&glcamera);
  glgraph.AddChild(&glboxvol);
  glgraph.AddChild(&glboxfrustum);

  pangolin::OpenGlRenderState s_cam(
        pangolin::ProjectionMatrixRDF_TopLeft(w,h,K.fu,K.fv,K.u0,K.v0,0.1,1000),
        pangolin::ModelViewLookAtRDF(0,0,-2,0,0,0,0,-1,0)
        );

  pangolin::Var<bool> run("ui.run", true, true);

  pangolin::Var<bool> showcolor("ui.show color", false, true);
  pangolin::Var<bool> viewonly("ui.view only", false, true);
  pangolin::Var<bool> fuse("ui.fuse", true, true);
  pangolin::Var<bool> reset("ui.reset", true, false);

  pangolin::Var<int> show_level("ui.Show Level", 0, 0, MaxLevels-1);

  // TODO: This needs to be a function of the inverse depth
  pangolin::Var<int> biwin("ui.size",3, 1, 20);
  pangolin::Var<float> bigs("ui.gs",1.5, 1E-3, 5);
  pangolin::Var<float> bigr("ui.gr",0.1, 1E-6, 0.2);

  pangolin::Var<bool> pose_refinement("ui.Pose Refinement", true, true);
  pangolin::Var<float> icp_c("ui.icp c",0.1, 1E-3, 1);
  pangolin::Var<float> trunc_dist_factor("ui.trunc vol factor",2, 1, 4);

  pangolin::Var<float> max_w("ui.max w", 1000, 1E-2, 1E3);
  pangolin::Var<float> mincostheta("ui.min cos theta", 0.1, 0, 1);

  pangolin::Var<bool> save_kf("ui.Save KF", false, false);
  pangolin::Var<float> rgb_fl("ui.RGB focal length", 535.7,400,600);
  pangolin::Var<float> max_rmse("ui.Max RMSE",0.10,0,0.5);
  pangolin::Var<float> rmse("ui.RMSE",0);

  pangolin::ActivateDrawPyramid<float,MaxLevels> adrayimg(ray_i, GL_LUMINANCE32F_ARB, true, true);
  pangolin::ActivateDrawPyramid<float4,MaxLevels> adraycolor(ray_c, GL_RGBA32F, true, true);
  pangolin::ActivateDrawPyramid<float4,MaxLevels> adraynorm(ray_n, GL_RGBA32F, true, true);
  //    pangolin::ActivateDrawPyramid<float,MaxLevels> addepth( kin_d, GL_LUMINANCE32F_ARB, false, true);
  pangolin::ActivateDrawPyramid<float4,MaxLevels> adnormals( kin_n, GL_RGBA32F_ARB, false, true);
  pangolin::ActivateDrawImage<float4> addebug( dDebug, GL_RGBA32F_ARB, false, true);

  Handler3DDepth<float,roo::TargetDevice> rayhandler(ray_d[0], s_cam, pangolin::AxisNone);
  SetupContainer(container, 4, (float)w/h);
  container[0].SetDrawFunction(std::ref(adrayimg))
      .SetHandler(&rayhandler);
  container[1].SetDrawFunction(SceneGraph::ActivateDrawFunctor(glgraph, s_cam))
      .SetHandler( new pangolin::Handler3D(s_cam, pangolin::AxisNone) );
  container[2].SetDrawFunction(std::ref(use_colour?adraycolor:adraynorm))
      .SetHandler(&rayhandler);
  container[3].SetDrawFunction(std::ref(adnormals));

  Sophus::SE3d T_wl;

  pangolin::RegisterKeyPressCallback(' ', [&reset,&viewonly]() { reset = true; viewonly=false;} );
  pangolin::RegisterKeyPressCallback('l', [&vol,&viewonly]() {LoadPXM("save.vol", vol); viewonly = true;} );
//    pangolin::RegisterKeyPressCallback('s', [&vol,&colorVol,&keyframes,&rgb_fl,w,h]() {SavePXM("save.vol", vol); SaveMeshlab(vol,keyframes,rgb_fl,rgb_fl,w/2,h/2); } );
  pangolin::RegisterKeyPressCallback('s', [&vol,&colorVol]() {roo::SaveMesh("mesh",vol,colorVol); } );
//  pangolin::RegisterKeyPressCallback('s', [&vol]() {SavePXM("save.vol", vol); } );

  for(long frame=-1; !pangolin::ShouldQuit();)
  {
    const bool go = !viewonly && (frame==-1 || run);

    if(Pushed(save_kf)) {
      KinectKeyframe* kf = new KinectKeyframe(w,h,T_cd * T_wl.inverse());
      //            kf->img.CopyFrom(roo::Image<uchar3, roo::TargetHost>((uchar3*)imgs[0].ptr,imgs[0].w,imgs[0].h,imgs[0].pitch));
      keyframes.push_back( std::unique_ptr<KinectKeyframe>(kf) );
    }

    if(go) {
      if(video.Capture(*images)) {
//        dKinect.CopyFrom(roo::Image<unsigned short, roo::TargetHost>((unsigned short*)images->at(0)->data(), images->at(0)->Width(), images->at(0)->Height()));
        dKinectMeters.CopyFrom(roo::Image<float, roo::TargetHost>((float*)images->at(0)->data(), images->at(0)->Width(), images->at(0)->Height()));
        if(use_colour) {
          drgb.CopyFrom(roo::Image<uchar3, roo::TargetHost>((uchar3*)images->at(1)->data(), images->at(1)->Width(), images->at(1)->Height() ));
        }
//        roo::ElementwiseScaleBias<float,unsigned short,float>(dKinectMeters, dKinect, 1.0f/1000.0f);
        roo::ElementwiseScaleBias<float,float,float>(dKinectMeters, dKinectMeters, 1.0f/1000.0f);
        roo::BilateralFilter<float,float>(kin_d[0],dKinectMeters,bigs,bigr,biwin,0.2);

        roo::BoxReduceIgnoreInvalid<float,MaxLevels,float>(kin_d);
        for(int l=0; l<MaxLevels; ++l) {
          roo::DepthToVbo<float>(kin_v[l], kin_d[l], K[l] );
          roo::NormalsFromVbo(kin_n[l], kin_v[l]);
        }

        frame++;
      }
    }

    const float trunc_dist = trunc_dist_factor*length(vol.VoxelSizeUnits());

    float f_rmse = rmse;
    if(Pushed(reset) || !std::isfinite(f_rmse) ) {
      T_wl = Sophus::SE3d();

      vol.bbox = reset_bb;
      //            roo::SdfReset(vol, trunc_dist );
      roo::SdfReset(vol, std::numeric_limits<float>::quiet_NaN() );
      keyframes.clear();

      colorVol.bbox = reset_bb;
      roo::SdfReset(colorVol);

      // Fuse first kinect frame in.
      const float trunc_dist = trunc_dist_factor*length(vol.VoxelSizeUnits());
      if(use_colour) {
        roo::SdfFuse(vol, colorVol, kin_d[0], kin_n[0], T_wl.inverse().matrix3x4(), K, drgb, (T_cd * T_wl.inverse()).matrix3x4(), roo::ImageIntrinsics(rgb_fl, drgb), trunc_dist, max_w, mincostheta );
      }else{
        roo::SdfFuse(vol, kin_d[0], kin_n[0], T_wl.inverse().matrix3x4(), K, trunc_dist, max_w, mincostheta );
      }
    }

    if(viewonly) {

      Sophus::SE3d T_vw(s_cam.GetModelViewMatrix());
      const roo::BoundingBox roi(T_vw.inverse().matrix3x4(), w, h, K, 0, 50);
      roo::BoundedVolume<roo::SDF_t> work_vol = vol.SubBoundingVolume( roi );
      roo::BoundedVolume<float> work_colorVol = colorVol.SubBoundingVolume( roi );
      if(work_vol.IsValid()) {
        if(showcolor) {
          roo::RaycastSdf(ray_d[0], ray_n[0], ray_i[0], work_vol, work_colorVol, T_vw.inverse().matrix3x4(), K, 0.1, 50, trunc_dist, true );
        }else{
          roo::RaycastSdf(ray_d[0], ray_n[0], ray_i[0], work_vol, T_vw.inverse().matrix3x4(), K, 0.1, 50, trunc_dist, true );
        }

        if(keyframes.size() > 0) {
          // populate kfs
          for( int k=0; k< kfs.Rows(); k++)
          {
            if(k < keyframes.size()) {
              kfs[k].img = keyframes[k]->img;
              kfs[k].T_iw = keyframes[k]->T_iw.matrix3x4();
              kfs[k].K = roo::ImageIntrinsics(rgb_fl, kfs[k].img);
            }else{
              kfs[k].img.ptr = 0;
            }
          }
          roo::TextureDepth<float4,uchar3,10>(ray_c[0], kfs, ray_d[0], ray_n[0], ray_i[0], T_vw.inverse().matrix3x4(), K);
        }
      }
    }else{
      bool tracking_good = true;

      const roo::BoundingBox roi(roo::BoundingBox(T_wl.matrix3x4(), w, h, K, knear,kfar));
      roo::BoundedVolume<roo::SDF_t> work_vol = vol.SubBoundingVolume( roi );
      if(work_vol.IsValid()) {
        //                roo::RaycastSdf(ray_d[0], ray_n[0], ray_i[0], work_vol, T_wl.matrix3x4(), fu, fv, u0, v0, knear, kfar, true );
        //                roo::BoxReduceIgnoreInvalid<float,MaxLevels,float>(ray_d);
        for(int l=0; l<MaxLevels; ++l) {
          if(its[l] > 0) {
            const roo::ImageIntrinsics Kl = K[l];
            if(showcolor) {
              roo::RaycastSdf(ray_d[l], ray_n[l], ray_i[l], work_vol, colorVol, T_wl.matrix3x4(), Kl, knear,kfar, trunc_dist, true );
            }else{
              roo::RaycastSdf(ray_d[l], ray_n[l], ray_i[l], work_vol, T_wl.matrix3x4(), Kl, knear,kfar, trunc_dist, true );
            }
            roo::DepthToVbo<float>(ray_v[l], ray_d[l], Kl );
            //                    roo::DepthToVbo(ray_v[l], ray_d[l], Kl.fu, Kl.fv, Kl.u0, Kl.v0 );
            //                    roo::NormalsFromVbo(ray_n[l], ray_v[l]);
          }
        }

        if(pose_refinement && frame > 0) {
          Sophus::SE3d T_lp;

          //                    const int l = show_level;
          //                    roo::RaycastSdf(ray_d[l], ray_n[l], ray_i[l], work_vol, T_wl.matrix3x4(), fu/(1<<l), fv/(1<<l), w/(2 * 1<<l) - 0.5, h/(2 * 1<<l) - 0.5, knear,kfar, true );
          //                    roo::DepthToVbo(ray_v[l], ray_d[l], fu/(1<<l), fv/(1<<l), w/(2.0f * (1<<l)) - 0.5, h/(2.0f * (1<<l)) - 0.5 );

          for(int l=MaxLevels-1; l >=0; --l)
          {
            const Eigen::Matrix3d Kdepth = K[l].Matrix();

            for(int i=0; i<its[l]; ++i ) {
              const Eigen::Matrix<double, 3,4> mKT_lp = Kdepth * T_lp.matrix3x4();
              const Eigen::Matrix<double, 3,4> mT_pl = T_lp.inverse().matrix3x4();
              roo::LeastSquaresSystem<float,6> lss = roo::PoseRefinementProjectiveIcpPointPlane(
                    kin_v[l], ray_v[l], ray_n[l], mKT_lp, mT_pl, icp_c, dScratch, dDebug.SubImage(0,0,w>>l,h>>l)
                    );

              Eigen::Matrix<double,6,6> sysJTJ = lss.JTJ;
              Eigen::Matrix<double,6,1> sysJTy = lss.JTy;

              // Add a week prior on our pose
              const double motionSigma = 0.2;
              const double depthSigma = 0.1;
              sysJTJ += (depthSigma / motionSigma) * Eigen::Matrix<double,6,6>::Identity();

              rmse = sqrt(lss.sqErr / lss.obs);
              tracking_good = rmse < max_rmse;

              if(l == MaxLevels-1 && MaxLevels > 1) {
                // Solve for rotation only
                Eigen::FullPivLU<Eigen::Matrix<double,3,3> > lu_JTJ( sysJTJ.block<3,3>(3,3) );
                Eigen::Matrix<double,3,1> x = -1.0 * lu_JTJ.solve( sysJTy.segment<3>(3) );
                T_lp = T_lp * Sophus::SE3d(Sophus::SO3d::exp(x), Eigen::Vector3d(0,0,0) );
              }else{
                Eigen::FullPivLU<Eigen::Matrix<double,6,6> > lu_JTJ( sysJTJ );
                Eigen::Matrix<double,6,1> x = -1.0 * lu_JTJ.solve( sysJTy );
                if( isFinite(x) ) {
                  T_lp = T_lp * Sophus::SE3d::exp(x);
                }
              }

            }
          }

          if(tracking_good) {
            T_wl = T_wl * T_lp.inverse();
          }
        }
      }

      if(pose_refinement && fuse) {
        if(tracking_good) {
          const roo::BoundingBox roi(T_wl.matrix3x4(), w, h, K, knear,kfar);
          roo::BoundedVolume<roo::SDF_t> work_vol = vol.SubBoundingVolume( roi );
          roo::BoundedVolume<float> work_colorVol = colorVol.SubBoundingVolume( roi );
          if(work_vol.IsValid()) {
            const float trunc_dist = trunc_dist_factor*length(vol.VoxelSizeUnits());
            if(use_colour) {
              roo::SdfFuse(work_vol, work_colorVol, kin_d[0], kin_n[0], T_wl.inverse().matrix3x4(), K, drgb, (T_cd * T_wl.inverse()).matrix3x4(), roo::ImageIntrinsics(rgb_fl, drgb), trunc_dist, max_w, mincostheta );
            }else{
              roo::SdfFuse(work_vol, kin_d[0], kin_n[0], T_wl.inverse().matrix3x4(), K, trunc_dist, max_w, mincostheta );
            }
          }
        }
      }
    }

    glcamera.SetPose(T_wl.matrix());

    roo::BoundingBox bbox_work(T_wl.matrix3x4(), w, h, K.fu, K.fv, K.u0, K.v0, knear,kfar);
    bbox_work.Intersect(vol.bbox);
    glboxfrustum.SetBounds(roo::ToEigen(bbox_work.Min()), roo::ToEigen(bbox_work.Max()) );

    //        {
    //            CudaScopedMappedPtr var(cbo);
    //            roo::Image<uchar4> dCbo((uchar4*)*var,w,h);
    //            roo::ConvertImage<uchar4,float4>(dCbo,kin_n[0]);
    //        }

    //        {
    //            CudaScopedMappedPtr var(vbo);
    //            roo::Image<float4> dVbo((float4*)*var,w,h);
    //            dVbo.CopyFrom(kin_v[0]);
    //        }

    /////////////////////////////////////////////////////////////
    // Draw
    addebug.SetImage(dDebug.SubImage(0,0,w>>show_level,h>>show_level));
    //        addepth.SetImageScale(scale);
    //        addepth.SetLevel(show_level);
    adnormals.SetLevel(show_level);
    adrayimg.SetLevel(viewonly? 0 : show_level);
    //        adraynorm.SetLevel(show_level);

    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
    glColor3f(1,1,1);
    pangolin::FinishFrame();
  }
}


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_POW_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_POW_HPP_INCLUDED
#include <boost/simd/detail/overload.hpp>

#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/function/any.hpp>
#include <boost/simd/function/if_else.hpp>
#include <boost/simd/function/is_flint.hpp>
#include <boost/simd/function/is_ltz.hpp>
#include <boost/simd/function/is_odd.hpp>
#include <boost/simd/function/logical_and.hpp>
#include <boost/simd/function/logical_andnot.hpp>
#include <boost/simd/function/multiplies.hpp>
#include <boost/simd/function/if_neg.hpp>
#include <boost/simd/function/pow_abs.hpp>
#include <boost/simd/function/shift_right.hpp>
#include <boost/simd/function/sqr.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;
  BOOST_DISPATCH_OVERLOAD_IF( pow_
                           , (typename A0,typename X)
                           , (detail::is_native<X>)
                           , bd::cpu_
                           , bs::pack_<bd::floating_<A0>,X>
                           , bs::pack_<bd::floating_<A0>,X>
                           )
  {
    BOOST_FORCEINLINE A0 operator()( const A0& a0, const A0& a1) BOOST_NOEXCEPT
    {
      auto nega0 = is_negative(a0);
      A0 z = pow_abs(a0, a1);
      z =  if_neg(logical_and(is_odd(a1), nega0), z);
      auto invalid =  logical_andnot(nega0, logical_or(is_flint(a1), is_inf(a1)));
      z = if_else(invalid, Nan<A0>(), z);
      return z;
    }
  };

  BOOST_DISPATCH_OVERLOAD_IF(pow_
                          , (typename A0,typename A1,typename X)
                          , (detail::is_native<X>)
                          , bd::cpu_
                          , bs::pack_<bd::arithmetic_<A0>, X>
                          , bs::pack_<bd::uint_<A1>, X>
                          )
   {
      BOOST_FORCEINLINE A0 operator()(const A0& a0, const A1& a1 ) const BOOST_NOEXCEPT
      {
        A0 base = a0;
        A1 exp = a1;
        A0 result = One<A0>();
        while(bs::any(exp))
        {
          result *= if_else(is_odd(exp), base, One<A0>());
          exp =  exp >> 1;
          base = sqr(base);
        }
        return result;
       }
   };

  BOOST_DISPATCH_OVERLOAD_IF ( pow_
                          , (typename A0, typename A1, typename X)
                          , (detail::is_native<X>)
                          , bd::cpu_
                          , bs::pack_< bd::arithmetic_<A0>, X>
                          , bd::constant_< bd::uint_<A1>>
                          )
  {
    using result_type = A0;

    BOOST_FORCEINLINE result_type operator() ( A0 const& a0, A1) const BOOST_NOEXCEPT
    {
      return pow_expander<A1::value>::call(a0);
    }
  };

  BOOST_DISPATCH_OVERLOAD_IF ( pow_
                          , (typename A0, typename A1, typename X)
                          , (detail::is_native<X>)
                          , bd::cpu_
                          , bs::pack_< bd::floating_<A0>, X>
                          , bd::constant_< bd::int_<A1>>
                          )
  {
    using result_type = A0;

    BOOST_FORCEINLINE result_type operator() ( A0 const& a0, A1) const BOOST_NOEXCEPT
    {
      return eval(a0, boost::mpl::bool_<(A1::value >= 0)>());
    }

    BOOST_FORCEINLINE result_type eval( A0 const& a0, boost::mpl::true_) const BOOST_NOEXCEPT
    {
      return pow_expander<A1::value>::call(a0);
    }

    BOOST_FORCEINLINE result_type eval( A0 const& a0, boost::mpl::false_) const BOOST_NOEXCEPT
    {
      return pow_expander<-A1::value>::call(rec(a0));
    }
  };

  BOOST_DISPATCH_OVERLOAD_IF ( pow_
                          , (typename A0, typename A1, typename X)
                          , (detail::is_native<X>)
                          , bd::cpu_
                          , bs::pack_< bd::arithmetic_<A0>, X>
                          , bd::scalar_< bd::uint_<A1>>
                          )
  {
    using result_type = A0;

    A0 operator() ( A0 const& a0, A1 const& a1) const BOOST_NOEXCEPT
    {
      A0 base = a0;
      A1 exp = a1;

      result_type result = One<result_type>();
      while(exp)
      {
        if(is_odd(exp))
          result *= base;
        exp >>= 1;
        base = sqr(base);
      }

      return result;
    }
  };

  BOOST_DISPATCH_OVERLOAD_IF ( pow_
                          , (typename A0, typename A1, typename X)
                          , (detail::is_native<X>)
                          , bd::cpu_
                          , bs::pack_< bd::integer_<A0>, X>
                          , bs::pack_< bd::int_<A1>, X>
                          )
  {
    using result_type = A0;

    BOOST_FORCEINLINE A0 operator() ( A0 const& a0, A1 const& a1) const BOOST_NOEXCEPT
    {
      BOOST_ASSERT_MSG( boost::simd::assert_all(a1 >= 0), "integral pow with signed exponent" );

      using u_t =  bd::as_integer_t<A1, unsigned>;
      return pow(a0, bitwise_cast<u_t>(a1));
    }
  };

  BOOST_DISPATCH_OVERLOAD_IF ( pow_
                          , (typename A0, typename A1, typename X)
                          , (detail::is_native<X>)
                          , bd::cpu_
                          , bs::pack_< bd::floating_<A0>, X>
                          , bs::pack_< bd::int_<A1>, X>
                          )
  {
    using result_type = A0;

    A0 operator() ( A0 const& a0, A1 const& a1) const BOOST_NOEXCEPT
    {
      using u_t =  bd::as_integer_t<A1, unsigned>;
      auto ltza1 = is_ltz(a1);
      A0 p = pow(a0, bitwise_cast<u_t>(if_neg(ltza1, a1)));
      return if_else(ltza1, rec(p), p);
    }
  };

  BOOST_DISPATCH_OVERLOAD_IF( pow_
                            , (typename A0,typename X)
                            , (detail::is_native<X>)
                            , bs::raw_tag
                            , bd::cpu_
                            , bs::pack_<bd::floating_<A0>,X>
                            , bs::pack_<bd::floating_<A0>,X>
                           )
  {
    BOOST_FORCEINLINE A0 operator()(const raw_tag &,
                                    const A0& a0, const A0& a1) BOOST_NOEXCEPT
    {
      auto nega0 = is_negative(a0);
      A0 z = raw_(pow_abs(a0, a1));
      z =  if_neg(logical_and(is_odd(a1), nega0), z);
      auto invalid =  logical_andnot(nega0, logical_or(is_flint(a1), is_inf(a1)));
      z = if_else(invalid, Nan<A0>(), z);
      return z;
    }
  };
} } }

#endif


/* g++ final.cpp -o final `pkg-config --libs opencv` -larmadillo -lwiringPi */
#include <iostream>
#include <armadillo>
#include <string>
#include "opencv2/core/core.hpp"
#include "opencv2/core/utility.hpp"
#include "opencv2/imgproc/imgproc.hpp"
#include "opencv2/imgcodecs.hpp"
#include "opencv2/highgui/highgui.hpp"
#include <vector>
#include <fstream>
#include <math.h>
#include <ctime>

#include <errno.h>
#include <string.h>
#include <wiringPi.h>
#include <wiringSerial.h>

//add OpenCV and Armadillo namespaces
using namespace cv;
using namespace std;
using namespace arma;

//	Functions for UART
void uart_ch(char ch)
{
	int fd ;

	if ((fd = serialOpen ("/dev/ttyAMA0", 9600)) < 0)
	{
		fprintf (stderr, "Unable to open serial device: %s\n", strerror (errno));
		return;
	}
	serialPutchar(fd,ch);
	serialClose(fd);
}

void uart_str(char *str)
{
	while(*str) uart_ch(*str++);
}

class LineFinder {
private:
    // original image
    cv::Mat img;

    // vector containing the end points
    // of the detected lines
    vector<Vec4i> lines;

    // accumulator resolution parameters
    double deltaRho;
    double deltaTheta;

    // minimum number of votes that a line
    // 선으로 간주되기 전에 받아야 하는 투표 최소 개수 (threshold)
    int minVote;

    // 선의 최소 길이
    double minLength;

    // 선을 따라가는 최대 허용 간격(gap)
    double maxGap;
public:
    // Default accumulator resolution is 1 pixel by 1 degree
    // no gap, no mimimum length
    LineFinder() : deltaRho(1), deltaTheta(CV_PI/180), minVote(10), minLength(0.), maxGap(0.) {}

    // Set the minimum number of votes
    void setMinVote(int minv) {
        minVote = minv;
    }

    // Set line length and gap
    void setLineLengthAndGap(double length, double gap) {
        minLength = length;
        maxGap = gap;
    }

    // Apply probabilistic Hough Transform
    vector<Vec4i> findLines(cv::Mat& binary) {
        lines.clear();
        HoughLinesP(binary, lines, deltaRho, deltaTheta, minVote, minLength, maxGap);
        return lines;
    }

    // Draw the detected lines on an image
    void drawDetectedLines(cv::Mat &image, Scalar color = Scalar(0, 0, 255)) {
        // Draw the lines
        vector<Vec4i>::const_iterator it2 = lines.begin();
        while (it2 != lines.end()) {
            if((*it2)[0] != (*it2)[2]) {
                Point pt1((*it2)[0], (*it2)[1]);
                Point pt2((*it2)[2], (*it2)[3]);
                line(image, pt1, pt2, color, 2);
            }
            ++it2;
        }
    }

    //일관성 없는 방향 라인 제거
    vector<Vec4i> removeLinesOfInconsistentOrientations(const cv::Mat &orientations, double percentage, double delta) {
        vector<Vec4i>::iterator it = lines.begin();
        // check all lines
        while (it != lines.end()) {
            // end points
            int x1= (*it)[0];
            int y1= (*it)[1];
            int x2= (*it)[2];
            int y2= (*it)[3];

            // line orientation + 90o to get the parallel line
            double ori1 = atan2(static_cast<double>(y1 - y2), static_cast<double>(x1 - x2)) + CV_PI / 2; // double 형으로 변환하는 함수를 호출!
            if (ori1 > CV_PI) ori1 = ori1 - 2 * CV_PI;

            double ori2 = atan2(static_cast<double>(y2 - y1), static_cast<double>(x2 - x1)) + CV_PI / 2;
            if (ori2 > CV_PI) ori2 = ori2 - 2 * CV_PI;

            // for all points on the line
            LineIterator lit(orientations, Point(x1, y1), Point(x2, y2));
            int i, count = 0;
            for(i = 0, count = 0; i < lit.count; i++, ++lit) {
                float ori = *(reinterpret_cast<float *>(*lit)); // lineiterate의 값들을 float형으로 인식해라!

                // is line orientation similar to gradient orientation ?
                if (min(fabs(ori - ori1), fabs(ori - ori2)) < delta) // fabs 부동소수점 숫자의 절대값 반환
                    count++;
            }
            double consistency = count / static_cast<double>(i); // 일관성

            // set to zero lines of inconsistent orientation
            if (consistency < percentage) {
                (*it)[0] = (*it)[1] = (*it)[2] = (*it)[3] = 0;
            }
            ++it;
        }
        return lines;
    }
};

class EdgeDetector {
private:
    // original image
    cv::Mat img;

    // 16-bit signed int image
    cv::Mat sobel;

    // Aperture size of the Sobel kernel
    int aperture; // 조리개??

    // Sobel magnitude
    cv::Mat sobelMagnitude;

    // Sobel orientation
    cv::Mat sobelOrientation;
public:
    EdgeDetector() : aperture(3) {}

    // Compute the Sobel
    void computeSobel(const cv::Mat& image) {
        cv::Mat sobelX;
        cv::Mat sobelY;

        // Compute Sobel
        Sobel(image, sobelX, CV_32F, 1, 0, aperture);
        Sobel(image, sobelY, CV_32F, 0, 1, aperture);

        // Compute magnitude and orientation
        cartToPolar(sobelX, sobelY, sobelMagnitude, sobelOrientation);
    }

    // Get Sobel orientation
    cv::Mat getOrientation() {
        return sobelOrientation;
    }
};

class vanishingPt {
	public:
	cv::Mat image;
	cv::Mat frame;
	vector< vector<int> > points;
	mat A, b, prevRes;
	mat Atemp, btemp, res, aug, error, soln;
	// store minimum length for lines to be considered while estimating vanishing point
    // define minimum length requirement for any line
    int minlength = image.cols * image.cols * 0.001;

	// temporary vector for intermediate storage
	vector<int> temp;

	// store (x1, y1) and (x2, y2) endpoints for each line segment
	vector<cv::Vec4i> lines_std;

	// to store intermediate errors
	double temperr;

	// constructor to set video/webcam and find vanishing point
	vanishingPt() {}
	void init() {
		for(int i = 0; i < lines_std.size(); i++) {
			// ignore if almost vertical
			if (abs(lines_std[i][0] - lines_std[i][2]) < 10 || abs(lines_std[i][1] - lines_std[i][3]) < 10) // check if almost vertical
				continue;
			// ignore shorter lines (x1-x2)^2 + (y2-y1)^2 < minlength
			if (((lines_std[i][0] - lines_std[i][2]) * (lines_std[i][0] - lines_std[i][2]) + (lines_std[i][1] - lines_std[i][3]) * (lines_std[i][1] - lines_std[i][3])) < minlength)
				continue;
            /*
            // ignore (0, x)
            double slope = abs(lines_std[i][1] - lines_std[i][3]) / abs(lines_std[i][0] - lines_std[i][2]);
            if(slope < 0.001) continue;
            */

            // store valid lines' endpoints for calculations
			for(int j = 0; j < 4; j++) {
				temp.push_back(lines_std[i][j]);
			}
			points.push_back(temp);
			temp.clear();
		}
		cout << "Detected:" << lines_std.size() << endl;
		cout << "Filtered:" << points.size() << endl;
	}

	void makeLines() {
		// to solve Ax = b for x
	    A = zeros<mat>(points.size(), 2);
	    b = zeros<mat>(points.size(), 1);

	    // convert given end-points of line segment into a*x + b*y = c format for calculations
	    // do for each line segment detected
	    for(int i = 0; i < points.size(); i++) {
			A(i, 0) = -(points[i][3] - points[i][1]);			// -(y2-y1)
			A(i, 1) = (points[i][2] - points[i][0]);				// x2-x1
			b(i, 0) = A(i, 0) * points[i][0] + A(i, 1) * points[i][1];	// -(y2-y1)*x1 + (x2-x1)*y1 = x2*y1 - x1*y2
	    }
	}

	// estimate the vanishing point
	void eval() {
		// stores the estimated co-ordinates of the vanishing point with respect to the image
		soln = zeros<mat>(2, 1);

        // theta
        int theta = 0;
        double delta = 0.0;

        // buffer for UART
        char buffer;

		// initialize error
		double err = 9999999999;

		// calculate point of intersection of every pair of lines and
		// find the sum of distance from all other lines
		// select the point which has the minimum sum of distance
		for(int i = 0; i < points.size(); i++) {
			for(int j = 0; j < points.size(); j++) {
				if(i >= j)
				continue;

				// armadillo vector
				uvec indices;

				// store indices of lines to be used for calculation
				indices << i << j;

				// extract the rows with indices specified in uvec indices
				// stores the ith and jth row of matrices A and b into Atemp and btemp respectively
				// hence creating a 2x2 matrix for calculating point of intersection
				Atemp = A.rows(indices);
				btemp = b.rows(indices);

				// if lines are parallel then skip
				if(arma::rank(Atemp) != 2)
					continue;

				// solves for 'x' in A*x = b
				res = calc(Atemp, btemp);

				if(res.n_rows == 0 || res.n_cols == 0)
					continue;

				// calculate error assuming perfect intersection is
				error = A * res - b;

				// reduce size of error
				error = error / 1000;

				// to store intermediate error values
				temperr = 0;

				// summation of errors
				for(int i = 0; i < error.n_rows; i++)
                    temperr += (error(i, 0) * error(i, 0)) / 1000;

                // scale errors to prevent any overflows
				temperr /= 1000000;

				// if current error is smaller than previous min error then update the solution (point)
				if(err > temperr) {
					soln = res;
					err = temperr;
				}
			}
		}

		cout<<"\n\nResult:\n"<<soln(0,0)<<","<<soln(1,0)<<"\nError:"<<err<<"\n\n";
/*
		// draw a circle to visualize the approximate vanishing point
		if(soln(0, 0) > 0 && soln(0, 0) < image.cols && soln(1, 0) > 0 && soln(1, 0) < image.rows) {
			cv::circle(frame, Point(soln(0, 0), soln(1, 0)), 15, cv::Scalar(0, 0, 255), 2);
            Point start(240, 320);
            Point end(soln(0, 0), soln(1, 0));
            line(frame, start, end, cv::Scalar(0, 0, 255), 2);
            theta = atan2(320 - soln(1, 0), soln(0, 0) - 240) * (180 / CV_PI);
            cout << theta << endl;
            // toDo: use previous frame's result to reduce calculations and stabilize the region of vanishing point
            prevRes = soln;
        }
        else {
            cv::circle(frame, Point(prevRes(0, 0), prevRes(1, 0)), 15, cv::Scalar(0, 0, 255), 2);
            Point start(240, 320);
            Point end(prevRes(0, 0), prevRes(1, 0));
            line(frame, start, end, cv::Scalar(0, 0, 255), 2);
            theta = atan2(320 - prevRes(1, 0), prevRes(0, 0) - 240) * (180 / CV_PI);
            cout << theta << endl;
        }
        if(theta < 0) {
            theta += 360;
        }
		if(theta > 120)		theta = 120;
		else if(theta < 90)	theta = 90;
        buffer = theta;
        uart_ch(buffer);

        cv::namedWindow("Line");
		cv::imshow("Line", frame);

		// flush the vector
		points.clear();
*/
        // draw a circle to visualize the approximate vanishing point
		if(soln(0, 0) > 0 && soln(0, 0) < image.cols && soln(1, 0) > 0 && soln(1, 0) < image.rows) {
			cv::circle(frame, Point(soln(0, 0), soln(1, 0)), 15, cv::Scalar(0, 0, 255), 2);
            if(soln(0, 0) < 240) {
                // 좌회전 연산
                delta = ((240 - soln(0, 0)) / 240) * 120 * ((130 - soln(1, 0)) / 65);
            }/*
            else if(soln(0, 0) >= 230 && soln(0, 0) < 250) {
                // 직진
                delta = 0;
            }*/
            else if(soln(0, 0) >= 240) {
                // 우회전 연산
                delta = -((240 - soln(0, 0)) / 240) * 120 * ((130 - soln(1, 0)) / 65);
            }
            // toDo: use previous frame's result to reduce calculations and stabilize the region of vanishing point
            prevRes = soln;
        }
        else {
            cv::circle(frame, Point(prevRes(0, 0), prevRes(1, 0)), 15, cv::Scalar(0, 0, 255), 2);
            if(prevRes(0, 0) < 240) {
                // 좌회전 연산
                delta = ((240 - prevRes(0, 0)) / 240) * 120 * ((130 - prevRes(1, 0)) / 65);
            }/*
            else if(prevRes(0, 0) >= 230 && prevRes(0, 0) < 250) {
                // 직진
                delta = 0;
            }*/
            else if(prevRes(0, 0) >= 240) {
                // 우회전 연산
                delta = -((240 - prevRes(0, 0)) / 240) * 120 * ((130 - prevRes(1, 0)) / 65);
            }
        }

        buffer = (int)delta + 60;
        cout << (int)buffer << endl;
        uart_ch(buffer);

        cv::namedWindow("Line");
		cv::imshow("Line", frame);

		// flush the vector
		points.clear();
	}

	//function to calculate and return the intersection point
	mat calc(mat A, mat b) {
	    mat x = zeros<mat>(2, 1);
		solve(x, A, b);
	    return x;
	}
};

int main()
{
	// make object
    vanishingPt vp;

    // to calculate fps
    clock_t begin, end;

    // read from video file on disk
    // to read from webcam initialize as: cap = VideoCapture(int device_id);
    VideoCapture cap(0);
    cv::Mat source;

    if(cap.isOpened()) { // check if camera/ video stream is available
        // get first frame to intialize the values
        cap.read(vp.frame);
        vp.image = cv::Mat(cv::Size(vp.frame.rows, vp.frame.cols), CV_8UC1, 0.0);
    }

    while(cap.isOpened()) { // check if camera/ video stream is available
        cap >> source;
        cv::resize(source, source, cv::Size(480, 320));
        if(!cap.grab())
            continue;

        // to calculate fps
        begin = clock();

        cv::Mat ROI(source, Rect(0, 240, 480, 80));
        cv::Mat tmp = ROI;
        medianBlur(tmp, tmp, 5);

        // Compute Sobel
        EdgeDetector ed;
        ed.computeSobel(tmp);

        // Apply Canny algorithm
        cv::Mat contours;
        Canny(tmp, contours, 150, 200); // 125, 200

        cv::namedWindow("contours");
        imshow("contours", contours);

        // Create LineFinder instance
        LineFinder ld;
        ld.setLineLengthAndGap(50, 20); // 50, 20
        ld.setMinVote(60); // 60

        // Detect lines
        vector<Vec4i> li = ld.findLines(contours);

        // remove inconsistent lines
        // ld.removeLinesOfInconsistentOrientations(ed.getOrientation(), 0.4, 0.1); // 0.4  0.1

        ld.drawDetectedLines(tmp);

        for(int i = 0; i < li.size(); i++){
            for(int j = 0; j < 4; j++){
                if(j == 1 || j == 3) li[i][j] += 240;
 //               else if(j == 0 || j == 2) li[i][j] += 80;
                cout << li[i][j] << " ";
            }
            cout << endl;
        }

        source.copyTo(tmp);

        vp.lines_std = li;

        // it's advised not to modify image stored in the buffer structure of the opencv.
        vp.frame = tmp.clone();

        cv::cvtColor(vp.frame, vp.image, cv::COLOR_BGR2GRAY);

        // resize frame to 480x320
        cv::resize(vp.image, vp.image, cv::Size(480, 320));
        cv::resize(vp.frame, vp.frame, cv::Size(480, 320));

        // equalize histogram
        cv::equalizeHist(vp.image, vp.image);

        // initialize the line segment matrix in format y = m*x + c
        vp.init();

        // draw lines on image and display
        vp.makeLines();

        // approximate vanishing point
        vp.eval();

        // to calculate fps
        end = clock();
        cout << "fps: " << 1 / (double(end - begin) / CLOCKS_PER_SEC) << endl;

        // hit 'esc' to exit program
        int k = cv::waitKey(1);
        if (k == 27)
            break;
    }
    cv::destroyAllWindows();
    return 0;
}


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2020 Tinko Bartels, Berlin, Germany.

// Contributed and/or modified by Tinko Bartels,
//   as part of Google Summer of Code 2020 program.

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

#ifndef BOOST_GEOMETRY_EXTENSIONS_GENERIC_ROBUST_PREDICATES_STRATEGIES_CARTESIAN_DETAIL_STAGE_A_HPP
#define BOOST_GEOMETRY_EXTENSIONS_GENERIC_ROBUST_PREDICATES_STRATEGIES_CARTESIAN_DETAIL_STAGE_A_HPP

#include <boost/mp11/list.hpp>
#include <boost/mp11/algorithm.hpp>
#include <boost/mp11/set.hpp>

#include <boost/geometry/extensions/generic_robust_predicates/strategies/cartesian/detail/expression_tree.hpp>
#include <boost/geometry/extensions/generic_robust_predicates/strategies/cartesian/detail/coefficient_list.hpp>
#include <boost/geometry/extensions/generic_robust_predicates/strategies/cartesian/detail/error_bound.hpp>
#include <boost/geometry/extensions/generic_robust_predicates/strategies/cartesian/detail/interval_error_bound.hpp>

#include <boost/geometry/extensions/generic_robust_predicates/strategies/cartesian/detail/semi_static_filter.hpp>
#include <boost/geometry/extensions/generic_robust_predicates/strategies/cartesian/detail/almost_static_filter.hpp>
#include <boost/geometry/extensions/generic_robust_predicates/strategies/cartesian/detail/static_filter.hpp>

namespace boost { namespace geometry
{

namespace detail { namespace generic_robust_predicates
{

template <typename Expression, typename CalculationType>
struct stage_a_error_bound_impl
{
private:
    using ct = CalculationType;
    using root = Expression;
    using stack = typename boost::mp11::mp_unique<post_order<Expression>>;
    using evals = typename boost::mp11::mp_remove_if<stack, is_leaf>;
    using interim_evals = typename boost::mp11::mp_remove
        <
            boost::mp11::mp_remove_if<stack, is_leaf>,
            root
        >;
    using interim_errors = evals_error<interim_evals>;
    using final_children = add_children
        <
            boost::mp11::mp_second
                <
                    boost::mp11::mp_map_find
                        <
                            interim_errors,
                            typename root::left
                        >
                >,
            boost::mp11::mp_second
                <
                    boost::mp11::mp_map_find
                        <
                            interim_errors,
                            typename root::right
                        >
                >
        >;
    using final_children_ltp = error_map_list_to_product<final_children>;
    using final_children_ltp_abs = abs_all<final_children_ltp>;
    using final_children_sum_fold = error_map_balanced_sum_up<final_children_ltp_abs>;
    using final_coeff = coeff_round
        <
            div_by_1_m_eps
                <
                    mult_by_1_p_eps
                        <
                            boost::mp11::mp_second<final_children_sum_fold>
                        >
                >
        >;
    static constexpr ct final_coeff_value = eval_eps_polynomial<ct, final_coeff>::value;

    struct final_coeff_constant : public static_constant_interface<ct>
    {
        static constexpr ct value = final_coeff_value;
        static constexpr bool non_negative = true;
    };

    using error_expression_variable = boost::mp11::mp_front<final_children_sum_fold>;
public:
    using type = product<final_coeff_constant, error_expression_variable>;
};

template
<
    typename Expression,
    typename CalculationType
>
using stage_a_error_bound =
    typename stage_a_error_bound_impl<Expression, CalculationType>::type;

template
<
    typename Expression,
    typename CalculationType
>
using stage_a_semi_static = semi_static_filter
        <
            Expression,
            CalculationType,
            stage_a_error_bound<Expression, CalculationType>
        >;

template
<
    typename Expression,
    typename CalculationType
>
using stage_a_static = static_filter
        <
            Expression,
            CalculationType,
            interval<stage_a_error_bound<Expression, CalculationType>>
        >;

template
<
    typename Expression,
    typename CalculationType
>
using stage_a_almost_static = almost_static_filter
        <
            Expression,
            CalculationType,
            stage_a_static<Expression, CalculationType>
        >;

}} // namespace detail::generic_robust_predicates

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_EXTENSIONS_GENERIC_ROBUST_PREDICATES_STRATEGIES_CARTESIAN_DETAIL_STAGE_A_HPP


//
// Created by David Wise on 24/03/2017.
//

#include <iostream>
#include "donorClass.h"
#include <stdio.h>
#include <Eigen/Dense>
//#include "gnuplot-iostream/gnuplot-iostream.h"
#include <fstream>

using namespace std;

int main() {

// Initialise a donor with required nuclear spin and hyperfine coupling
	Donor phos;
    phos.initialise(0.5, 7.29e-26);

// Create a savefile for the eigen values
    std::ofstream saveFile("donorsgotest.txt");

// Create matrices to store eigenvalues (need (nucSpin + 0.5) * 4)
    MatrixXcd eig1;
    MatrixXcd eig2;
    MatrixXcd eig3;
    MatrixXcd eig4;
    MatrixXcd fields;

// Loop through a number of steps in magnetic field finding eigenvalues at each step
    int numSteps = 1000;
    for (double incr = 0; incr <numSteps; ++incr) {
        double maxField = 1;
        eig1.resize(numSteps, 1);
        eig2.resize(numSteps, 1);
        eig3.resize(numSteps, 1);
        eig4.resize(numSteps, 1);
        fields.resize(numSteps, 1);
        MatrixXcd Eigs = phos.getEigs(incr);
        eig1(incr) = Eigs(0);
        eig2(incr) = Eigs(1);
        eig4(incr) = Eigs(2);
        eig3(incr) = Eigs(3);
        fields(incr) = (maxField/numSteps)*incr;
    };

// Save eigenvalues to file
    saveFile << "Fields \n" << fields << "\n Eigs 1 \n" << eig1 << "\n Eigs 2 \n" << eig2 << "\n Eigs 3 \n" << eig3 << "\n Eigs 4 \n" << eig4;
    saveFile.close();

	return 0;
}


/**
* PS Move API - An interface for the PS Move Motion Controller
* Copyright (c) 2011, 2012 Thomas Perl <m@thp.io>
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
*    1. Redistributions of source code must retain the above copyright
*       notice, this list of conditions and the following disclaimer.
*
*    2. Redistributions in binary form must reproduce the above copyright
*       notice, this list of conditions and the following disclaimer in the
*       documentation and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
**/

/* General math utility functions */
#ifndef __PSMOVE_ADVANCED_MATH_H
#define __PSMOVE_ADVANCED_MATH_H

//-- includes -----
#include "psmove_platform_config.h"

#include <Eigen/Core>
#include <Eigen/Geometry>

#include "psmove_math.h"

//-- macros -----
#define assert_vector3f_is_normalized(v) assert(is_nearly_equal(v.squaredNorm(), 1.f, k_normal_epsilon))

#endif // __PSMOVE_ADVANCED_MATH_H


/*

PICCANTE
The hottest HDR imaging library!
http://vcg.isti.cnr.it/piccante

Copyright (C) 2014
Visual Computing Laboratory - ISTI CNR
http://vcg.isti.cnr.it
First author: Francesco Banterle

This Source Code Form is subject to the terms of the Mozilla Public
License, v. 2.0. If a copy of the MPL was not distributed with this
file, You can obtain one at http://mozilla.org/MPL/2.0/.

*/

#ifndef PIC_ALGORITHMS_POISSON_IMAGE_EDITING_HPP
#define PIC_ALGORITHMS_POISSON_IMAGE_EDITING_HPP

#include <vector>

#include "../base.hpp"
#include "../image.hpp"
#include "../util/std_util.hpp"
#include "../filtering/filter_laplacian.hpp"

#ifndef PIC_DISABLE_EIGEN

#ifndef PIC_EIGEN_NOT_BUNDLED
    #include "../externals/Eigen/Sparse"
    #include "../externals/Eigen/src/SparseCore/SparseMatrix.h"
#else
    #include <Eigen/Sparse>
    #include <Eigen/src/SparseCore/SparseMatrix.h>
#endif

#endif

namespace pic {

#ifndef PIC_DISABLE_EIGEN
/**
 * @brief computePoissonImageEditing
 * @param source
 * @param target
 * @param mask
 * @param ret
 * @return
 */
PIC_INLINE Image *computePoissonImageEditing(Image *source, Image *target, bool *mask, Image *ret = NULL)
{
    if((source == NULL) || (target == NULL) || (mask == NULL)) {
        return NULL;
    }

    //allocate the output
    if(ret == NULL) {
        ret = target->clone();
    }

    int width  = target->width;
    int height = target->height;

    #ifdef PIC_DEBUG
        printf("Init matrix...");
    #endif

    Image *lap_source = FilterLaplacian::execute(source, NULL);

    std::vector< Eigen::Triplet< double > > tL;

    //indices pass
    int *index = new int[width * height];
    int count = 0;
    for(int i = 0; i < height; i++) {
        int tmpI = i * width;

        for(int j = 0; j < width; j++) {
            int indI = tmpI + j;

            if(mask[indI]) {
                index[indI] = count;
                count++;
            } else {
                index[indI] = 0;
            }
        }
    }

    //matrix A pass
    count = 0;
    for(int i = 0; i < height; i++) {
        int tmpI = i * width;

        for(int j = 0; j < width; j++) {
            int indI = tmpI + j;

            if(mask[indI]) {
                if((j + 1) < (width - 1)) {
                    if(mask[indI + 1]) {
                        tL.push_back(Eigen::Triplet< double > (count, index[indI + 1], -1.0));
                    }
                }

                if((j - 1) > -1) {
                    if(mask[indI - 1]) {
                        tL.push_back(Eigen::Triplet< double > (count, index[indI - 1], -1.0));
                    }
                }

                if((i + 1) < (height - 1)) {
                    if(mask[indI + width]) {
                        tL.push_back(Eigen::Triplet< double > (count, index[indI + width], -1.0));
                    }
                }

                if((i - 1) > -1) {
                    if(mask[indI - width]) {
                        tL.push_back(Eigen::Triplet< double > (count, index[indI - width], -1.0));
                    }
                }

                tL.push_back(Eigen::Triplet< double > (count, count , 4.0));

                count++;
            }
        }
    }

    int tot = count;
    Eigen::SparseMatrix<double> A = Eigen::SparseMatrix<double>(tot, tot);
    A.setFromTriplets(tL.begin(), tL.end());

    #ifdef PIC_DEBUG
        printf("Ok\n");
    #endif

    //solve the linear system for each color channel
    Eigen::SimplicialCholesky<Eigen::SparseMatrix<double> > solver(A);

    for(int k=0; k< target->channels; k++) {

        Eigen::VectorXd b, x;
        b = Eigen::VectorXd::Zero(tot);

        //assign values to b
        int count = 0;
        for(int i = 0; i < height; i++) {
            int tmpI = i * width;

            for(int j = 0; j < width; j++) {
                int indI = (tmpI + j);

                if(mask[indI]) {

                    b[count] = -(*lap_source)(j, i)[k];

                    if((j + 1) < (width - 1)) {
                        if(!mask[indI + 1]) {
                            b[count] += (*target)(j + 1, i)[k];
                        }
                    }

                    if((j - 1) > -1) {
                        if(!mask[indI - 1]) {
                            b[count] += (*target)(j - 1, i)[k];
                        }
                    }

                    if((i + 1) < (height - 1)) {
                        if(!mask[indI + width]) {
                            b[count] += (*target)(j, i + 1)[k];
                        }
                    }

                    if((i - 1) > -1) {
                        if(!mask[indI - width]) {
                           b[count] += (*target)(j, i - 1)[k];
                        }
                    }

                    count++;
                }
            }
        }

        x = solver.solve(b);

        if(solver.info() != Eigen::Success) {
            #ifdef PIC_DEBUG
                printf("SOLVER FAILED!\n");
            #endif

            return NULL;
        }

        #ifdef PIC_DEBUG
            printf("SOLVER SUCCESS!\n");
        #endif

        count = 0;
        for(int i = 0; i < height; i++) {
            int tmpI = i * width;

            for(int j = 0; j < width; j++) {
                int indI = (tmpI + j);

                if(mask[indI]) {
                    float val = float(x(count));
                    (*ret)(j, i)[k] = val > 0.0f ? val : 0.0f;
                    count++;
                }
            }
        }
    }

    delete_s(lap_source);
    delete_vec_s(index);

    return ret;
}
#endif

} // end namespace pic

#endif /* PIC_ALGORITHMS_POISSON_IMAGE_EDITING_HPP */


// Copyright 2004 The Trustees of Indiana University.

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

//  Authors: Douglas Gregor
//           Andrew Lumsdaine
#ifndef BOOST_GRAPH_BETWEENNESS_CENTRALITY_CLUSTERING_HPP
#define BOOST_GRAPH_BETWEENNESS_CENTRALITY_CLUSTERING_HPP

#include <boost/graph/betweenness_centrality.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/pending/indirect_cmp.hpp>
#include <algorithm>
#include <vector>
#include <boost/property_map.hpp>

namespace boost {

/** Threshold termination function for the betweenness centrality
 * clustering algorithm.
 */
template<typename T>
struct bc_clustering_threshold
{
  typedef T centrality_type;

  /// Terminate clustering when maximum absolute edge centrality is
  /// below the given threshold.
  explicit bc_clustering_threshold(T threshold)
    : threshold(threshold), dividend(1.0) {}

  /**
   * Terminate clustering when the maximum edge centrality is below
   * the given threshold.
   *
   * @param threshold the threshold value
   *
   * @param g the graph on which the threshold will be calculated
   *
   * @param normalize when true, the threshold is compared against the
   * normalized edge centrality based on the input graph; otherwise,
   * the threshold is compared against the absolute edge centrality.
   */
  template<typename Graph>
  bc_clustering_threshold(T threshold, const Graph& g, bool normalize = true)
    : threshold(threshold), dividend(1.0)
  {
    if (normalize) {
      typename graph_traits<Graph>::vertices_size_type n = num_vertices(g);
      dividend = T((n - 1) * (n - 2)) / T(2);
    }
  }

  /** Returns true when the given maximum edge centrality (potentially
   * normalized) falls below the threshold.
   */
  template<typename Graph, typename Edge>
  bool operator()(T max_centrality, Edge, const Graph&)
  {
    return (max_centrality / dividend) < threshold;
  }

 protected:
  T threshold;
  T dividend;
};

/** Graph clustering based on edge betweenness centrality.
 *
 * This algorithm implements graph clustering based on edge
 * betweenness centrality. It is an iterative algorithm, where in each
 * step it compute the edge betweenness centrality (via @ref
 * brandes_betweenness_centrality) and removes the edge with the
 * maximum betweenness centrality. The @p done function object
 * determines when the algorithm terminates (the edge found when the
 * algorithm terminates will not be removed).
 *
 * @param g The graph on which clustering will be performed. The type
 * of this parameter (@c MutableGraph) must be a model of the
 * VertexListGraph, IncidenceGraph, EdgeListGraph, and Mutable Graph
 * concepts.
 *
 * @param done The function object that indicates termination of the
 * algorithm. It must be a ternary function object thats accepts the
 * maximum centrality, the descriptor of the edge that will be
 * removed, and the graph @p g.
 *
 * @param edge_centrality (UTIL/OUT) The property map that will store
 * the betweenness centrality for each edge. When the algorithm
 * terminates, it will contain the edge centralities for the
 * graph. The type of this property map must model the
 * ReadWritePropertyMap concept. Defaults to an @c
 * iterator_property_map whose value type is
 * @c Done::centrality_type and using @c get(edge_index, g) for the
 * index map.
 *
 * @param vertex_index (IN) The property map that maps vertices to
 * indices in the range @c [0, num_vertices(g)). This type of this
 * property map must model the ReadablePropertyMap concept and its
 * value type must be an integral type. Defaults to
 * @c get(vertex_index, g).
 */
template<typename MutableGraph, typename Done, typename EdgeCentralityMap,
         typename VertexIndexMap>
void
betweenness_centrality_clustering(MutableGraph& g, Done done,
                                  EdgeCentralityMap edge_centrality,
                                  VertexIndexMap vertex_index)
{
  typedef typename property_traits<EdgeCentralityMap>::value_type
    centrality_type;
  typedef typename graph_traits<MutableGraph>::edge_iterator edge_iterator;
  typedef typename graph_traits<MutableGraph>::edge_descriptor edge_descriptor;
  typedef typename graph_traits<MutableGraph>::vertices_size_type
    vertices_size_type;

  if (edges(g).first == edges(g).second) return;

  // Function object that compares the centrality of edges
  indirect_cmp<EdgeCentralityMap, std::less<centrality_type> >
    cmp(edge_centrality);

  bool is_done;
  do {
    brandes_betweenness_centrality(g,
                                   edge_centrality_map(edge_centrality)
                                   .vertex_index_map(vertex_index));
    edge_descriptor e = *max_element(edges(g).first, edges(g).second, cmp);
    centrality_type max_centrality = get(edge_centrality, e);
    is_done = done(get(edge_centrality, e), e, g);
    if (!is_done) remove_edge(e, g);
  } while (!is_done && edges(g).first != edges(g).second);
}

/**
 * \overload
 */
template<typename MutableGraph, typename Done, typename EdgeCentralityMap>
void
betweenness_centrality_clustering(MutableGraph& g, Done done,
                                  EdgeCentralityMap edge_centrality)
{
  betweenness_centrality_clustering(g, done, edge_centrality,
                                    get(vertex_index, g));
}

/**
 * \overload
 */
template<typename MutableGraph, typename Done>
void
betweenness_centrality_clustering(MutableGraph& g, Done done)
{
  typedef typename Done::centrality_type centrality_type;
  std::vector<centrality_type> edge_centrality(num_edges(g));
  betweenness_centrality_clustering(g, done,
    make_iterator_property_map(edge_centrality.begin(), get(edge_index, g)),
    get(vertex_index, g));
}

} // end namespace boost

#endif // BOOST_GRAPH_BETWEENNESS_CENTRALITY_CLUSTERING_HPP