/*******************************************************************************
 * 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 "Observable.hpp"
#include "qcor.hpp"

#include "cppmicroservices/BundleActivator.h"
#include "cppmicroservices/BundleContext.h"
#include "cppmicroservices/ServiceProperties.h"
#include "xacc.hpp"
#include "xacc_internal_compiler.hpp"

#include "Instruction.hpp"
#include "InstructionIterator.hpp"
#include "Utils.hpp"

#include <memory>
#include <set>

#include <Eigen/Dense>

using namespace cppmicroservices;

namespace qcor {

class RBMChemistry : public ObjectiveFunction {
public:
  void initialize(std::shared_ptr<xacc::Observable> obs,
                  xacc::CompositeInstruction *qk) override {
    ObjectiveFunction::initialize(obs, qk);
    std::cout << "DW\n" << qk->toString() << "\n";

    c = 0.0;

    ham_mat_elements = obs->to_sparse_matrix();
    for (auto &x : ham_mat_elements) {
      std::cout << x.row() << ", " << x.col() << ", " << x.coeff() << "\n";
    }
  }

protected:
  int nv;
  int nh;
  std::vector<xacc::SparseTriplet> ham_mat_elements;
  Eigen::VectorXd d_vec;
  double c;

  double operator()() override {
    auto tmp_child = qalloc(qreg.size());
    for (auto i : kernel->getInstructions()) {
      std::cout << i->name() << ", " << i->isComposite() << "\n";
      if (i->isComposite() && i->name() == "rbm") {
        nv = i->getParameter(0).as<int>();
        nh = i->getParameter(1).as<int>();
        break;
      }
    }

    d_vec = Eigen::VectorXd::Zero(nv);

    std::cout << "Args; " << kernel->getArguments().size() << "\n";
    std::cout << kernel->getArguments()[0]->name << "\n";

    // Get vis_bias, hid_bias, wij, d, and c
    auto current_parameters =
        kernel->getArguments()[1]->runtimeValue.get<std::vector<double>>(
            xacc::INTERNAL_ARGUMENT_VALUE_KEY);
    std::cout << "Current Params: " << current_parameters.size() << "\n";
    int counter = 0;
    for (int i = nv + nh + nv * nh; i < current_parameters.size() - 1; i++) {
      d_vec(counter) = current_parameters[i];
      counter++;
    }

    c = current_parameters[current_parameters.size() - 1];

    std::cout << "EXECUTING\n";
    auto tmp_kernel =
        xacc::getIRProvider("quantum")->createComposite("tmp_rbm");
    xacc::InstructionIterator iter(xacc::as_shared_ptr(kernel));
    while (iter.hasNext()) {
      auto next = iter.next();
      if (!next->isComposite()) {
        tmp_kernel->addInstruction(next);
      }
    }

    // Here we have an evaluated RBM, execute it, and get its counts back
    xacc::internal_compiler::execute(tmp_child.results(), tmp_kernel.get());
    auto counts = tmp_child.counts();
    std::cout << "EXECUTED\n";
    int shots = 0;
    std::vector<std::vector<int>> states;
    std::vector<int> state_counts_vec;
    std::vector<std::string> states_str;
    for (auto &kv : counts) {
      std::cout << "COUNT: " << kv.first << ", " << kv.second << "\n";
      std::vector<int> tmp;
      std::string tmp_str = "";
      for (int i = 0; i < nv; i++) {
        tmp.push_back(kv.first[i] == '1' ? 1 : -1);
        tmp_str += kv.first[i];
      }
      state_counts_vec.push_back(kv.second);
      states.push_back(tmp);
      states_str.push_back(tmp_str);
      shots += kv.second;
    }

    Eigen::VectorXd state_counts(state_counts_vec.size());
    for (int i = 0; i < state_counts_vec.size(); i++) {
        std::cout << "state ocutns : " << state_counts_vec [i] << "\n";
        state_counts(i) = state_counts_vec[i] / (double) shots;
    }

    Eigen::MatrixXd sMat(states.size(), states[0].size());
    sMat.setZero();

    for (int s = 0; s < states.size(); s++) {
      for (int i = 0; i < states[0].size(); i++) {
        sMat(s, i) = (double)states[s][i];
      }
    }

    std::cout << "smat:\n" << sMat << "\nd_ved\n" << d_vec << "\n";

    // Cmopute sx
    Eigen::MatrixXd d_reshaped = Eigen::Map<Eigen::MatrixXd>(d_vec.data(), nv, 1);
    Eigen::MatrixXd ttmp = sMat * d_reshaped + c*Eigen::MatrixXd::Ones(sMat.rows(), 1) ;
    ttmp = ttmp.array().tanh();
    Eigen::VectorXd sx = Eigen::Map<Eigen::VectorXd>(ttmp.data(), sMat.rows());

    std::cout << "S(X): " << sx.transpose() << "\n";

    //   sx = np.tanh(np.dot(states, d.reshape((nv,1))) + c).reshape((1,
    //   num_states))

    //     # Compute probabilities.
    //     probs = np.multiply((sx**2).flatten(), state_counts**n)
    //     probs = probs / np.sum(probs)

    //     # Compute psi.
    //     psi = np.multiply(np.sign(sx.flatten()), np.sqrt(probs))

    Eigen::VectorXd probs = Eigen::VectorXd::Zero(state_counts.size());
    for (int i = 0; i < sx.rows(); i++) {
        std::cout << "prob: " << state_counts(i) << "\n";
      probs(i) = sx(i) * sx(i) * state_counts(i);
    }

    probs = probs / probs.sum();

    auto sign = [](auto val) { return (0.0 < val) - (val < 0.0); };

    std::cout << "SIGN: " << sign(-1) << ", " << sign (1) << "\n";
    Eigen::VectorXd psi = Eigen::VectorXd::Zero(state_counts.size());
    for (int i = 0; i < psi.rows(); i++) {
      psi(i) = sign(sx(i)) * std::sqrt(probs(i));
    }
    std::cout << "probs:\n" << probs.transpose() << "\n\n";
    std::cout << "psi\n" << psi.transpose() << "\n";

// for i in range(num_states):
//         local_energy = 0
//         x = states_str[i]
//         for j in range(num_states):
//             x1 = states_str[j]
//             if (x, x1) in ham_mat_elements:
//                 local_energy += ham_mat_elements[(x,x1)]*psi[j]
//         local_energy /= psi[i]
//         Eloc.append(local_energy)

    Eigen::VectorXd Eloc = Eigen::VectorXd::Zero(psi.rows());
    for (int i = 0; i < Eloc.rows(); i++) {
      double local_energy = 0.0;
      auto x = states_str[i];
      std::cout << "X IS " << x << "\n";
      auto x_i = std::stoi(x, 0, 2);
      for (int j = 0; j < states_str.size(); j++) {
        auto x1 = states_str[j];
        auto x1_j = std::stoi(x1, 0, 2);
        std::cout << "X1 IS " << x1 << "\n";

        auto found_triplet =
            std::find_if(ham_mat_elements.begin(), ham_mat_elements.end(),
                         [&](xacc::SparseTriplet &val) {
                           if (val.row() == x_i && val.col() == x1_j) {
                             return true;
                           }
                           return false;
                         });

        if (found_triplet != ham_mat_elements.end()) {
          local_energy += found_triplet->coeff().real() * psi[j];
        }
      }
      std::cout << "adding energy : " << local_energy << "\n";
      local_energy /= psi[i];
      Eloc(i) = local_energy;
    }

    double energy = probs.dot(Eloc);

    // dA = n * states.T / 2
    // weights = np.asarray(wij).reshape((nv,nh))
    // theta = np.dot(weights.transpose(), states.T) + b.reshape((nh,1))
    // dB = n * np.tanh(theta) / 2
    // dW = (states.T).reshape((nv,1,num_states)) * dB.reshape((1,nh,num_states))
    // oneoversx = np.clip(1/sx, -10**10, 10**10)
    // dc = oneoversx - sx
    // dd = states.T * dc
    // dP = np.concatenate([dA,dB,dW.reshape(nv*nh, num_states),dd,dc])

    // # Compute S and F.
    // E_dP = np.dot(dP, probs.reshape((num_states, 1)))
    // dP_conj = np.conjugate(dP)
    // E_dP_conj = np.dot(dP_conj, probs.reshape((num_states, 1)))

    // dP_with_probs = np.multiply(dP, probs.reshape((1,num_states)))
    // S = np.dot(dP_conj, dP_with_probs.T) - np.dot(E_dP_conj, E_dP.T)
    // F = np.dot(dP_conj, (Eloc*probs).reshape(num_states,1)) - energy*E_dP_conj

    // # Solve S * \delta p = F
    // update, info = linalg.bicgstab(S + epsilon * np.identity(S.shape[0]), F, x0=update)

    std::cout << "Energy: " << energy << "\n";
    // exit(0);
    qreg.addChild(tmp_child);
    return energy;
  }

public:
  const std::string name() const override { return "rbm-chemistry"; }
  const std::string description() const override { return ""; }
};

} // namespace qcor

namespace {

/**
 */
class US_ABI_LOCAL RBMChemObjectiveActivator : public BundleActivator {

public:
  RBMChemObjectiveActivator() {}

  /**
   */
  void Start(BundleContext context) {
    auto xt = std::make_shared<qcor::RBMChemistry>();
    context.RegisterService<qcor::ObjectiveFunction>(xt);
  }

  /**
   */
  void Stop(BundleContext /*context*/) {}
};

} // namespace

CPPMICROSERVICES_EXPORT_BUNDLE_ACTIVATOR(RBMChemObjectiveActivator)


#include "vtkTensorLines.h"

#include "TensorLines.hh"
#include "utils.hh"

#include <Eigen/Geometry>

#include <vtkCellIterator.h>
#include <vtkDataArray.h>
#include <vtkDataSet.h>
#include <vtkDataSetAttributes.h>
#include <vtkDoubleArray.h>
#include <vtkGenericCell.h>
#include <vtkIdList.h>
#include <vtkIntArray.h>
#include <vtkUnsignedLongLongArray.h>
#include <vtkPointData.h>
#include <vtkCellData.h>
#include <vtkPoints.h>
#include <vtkPolyData.h>
#include <vtkSmartPointer.h>
#include <vtkUnstructuredGrid.h>

#include <vtkCommand.h>
#include <vtkInformation.h>
#include <vtkInformationVector.h>
#include <vtkObjectFactory.h>
#include <vtkStreamingDemandDrivenPipeline.h>

#include <algorithm>
#include <array>
#include <chrono>
#include <iostream>
#include <list>
#include <map>
#include <unordered_map>
#include <unordered_set>
#include <vector>

using namespace cpp_utils;

namespace
{
// vtk Tensors are stored in row major order by convention
using Mat3d = Eigen::Matrix<double, 3, 3, Eigen::RowMajor, 3, 3>;
using Mat3dm = Eigen::Map<Mat3d>;
using Vec3d = tl::Vec3d;
using Vec3dm = Eigen::Map<Vec3d>;

struct TriFace
{
    std::array<vtkIdType, 3> points;
    vtkIdType cellId;
};

std::vector<TriFace> buildFaceList(vtkDataSet* dataset)
{
    auto face_list = std::vector<TriFace>{};
    auto add_face = [&](vtkIdList* point_ids,
                        vtkIdType cell_id,
                        vtkIdType i1,
                        vtkIdType i2,
                        vtkIdType i3) {
        auto tri = TriFace{{point_ids->GetId(i1),
                            point_ids->GetId(i2),
                            point_ids->GetId(i3)},
                           cell_id};
        face_list.push_back(tri);
    };

    // Collect faces and remember cell ID
    auto it = vtkSmartPointer<vtkCellIterator>(dataset->NewCellIterator());
    for(it->InitTraversal(); !it->IsDoneWithTraversal(); it->GoToNextCell())
    {
        if(it->GetCellType() != VTK_TETRA)
        {
            std::cout << "WARNING: Dataset contains non-tet cells, which will "
                         "be ignored.\n";
            continue;
        }

        auto* point_ids = it->GetPointIds();
        auto cid = it->GetCellId();

        add_face(point_ids, cid, 0, 1, 2);
        add_face(point_ids, cid, 1, 3, 2);
        add_face(point_ids, cid, 0, 3, 1);
        add_face(point_ids, cid, 0, 2, 3);
    }
    return face_list;
}

std::array<vtkSmartPointer<vtkDoubleArray>, 3>
computeCellDerivatives(vtkDataSet* dataset,
                       const char* point_data_name)
{
    auto point_data = vtkSmartPointer<vtkDataArray>(
            dataset->GetPointData()->GetArray(point_data_name));

    auto ncomps = point_data->GetNumberOfComponents();

    auto derivs = vtkSmartPointer<vtkDoubleArray>::New();
    derivs->SetNumberOfComponents(ncomps*3);
    derivs->SetNumberOfTuples(dataset->GetNumberOfCells());

    auto it = vtkSmartPointer<vtkCellIterator>(dataset->NewCellIterator());
    for(it->InitTraversal(); !it->IsDoneWithTraversal(); it->GoToNextCell())
    {
        // if(it->GetCellType() != VTK_TETRA)
        // {
        //     std::cout << "WARNING: Dataset contains non-tet cells, which will "
        //                  "be ignored.\n";
        //     continue;
        // }

        auto* point_ids = it->GetPointIds();
        auto npts = point_ids->GetNumberOfIds();
        auto cid = it->GetCellId();

        // copy point data to work array
        auto pt_data =
                std::vector<double>(as_unsigned(npts * ncomps));
        for(auto i: range(npts))
        {
            point_data->GetTuple(point_ids->GetId(i), &pt_data[as_unsigned(i*ncomps)]);
        }

        auto cell = vtkSmartPointer<vtkGenericCell>::New();
        it->GetCell(cell);
        auto cell_center = std::array<double, 3>{};
        cell->GetParametricCenter(cell_center.data());
        auto deriv_data = std::vector<double>(as_unsigned(ncomps*3));
        cell->Derivatives(0,
                          cell_center.data(),
                          pt_data.data(),
                          ncomps,
                          deriv_data.data());

        derivs->SetTuple(cid, deriv_data.data());
    }

    auto dx = vtkSmartPointer<vtkDoubleArray>::New();
    dx->SetNumberOfComponents(ncomps);
    dx->SetNumberOfTuples(dataset->GetNumberOfCells());
    for(auto i: range(ncomps))
    {
        dx->CopyComponent(i, derivs, i*3);
    }

    auto dy = vtkSmartPointer<vtkDoubleArray>::New();
    dy->SetNumberOfComponents(ncomps);
    dy->SetNumberOfTuples(dataset->GetNumberOfCells());
    for(auto i: range(ncomps))
    {
        dy->CopyComponent(i, derivs, i*3+1);
    }

    auto dz = vtkSmartPointer<vtkDoubleArray>::New();
    dz->SetNumberOfComponents(ncomps);
    dz->SetNumberOfTuples(dataset->GetNumberOfCells());
    for(auto i: range(ncomps))
    {
        dz->CopyComponent(i, derivs, i*3+2);
    }
    return {dx, dy, dz};
}

std::vector<tl::TLResult> computePEVPoints(const std::vector<TriFace>& faces,
                                             vtkPoints* points,
                                             vtkDataArray* array1,
                                             vtkDataArray* array2,
                                             vtkAlgorithm* progress_alg,
                                             const tl::TLOptions& opts)
{
    const auto step = 1. / double(faces.size());
    progress_alg->UpdateProgress(0);
    auto results = std::vector<tl::TLResult>(faces.size());
    auto terminate = false;
#pragma omp parallel for schedule(guided, 1)
    for(auto i = std::size_t{0}; i < faces.size(); ++i)
    {
#pragma omp flush(terminate)
        if(terminate) continue;

        auto face = faces[i];

        auto p1 = Vec3d{};
        points->GetPoint(face.points[0], p1.data());
        auto p2 = Vec3d{};
        points->GetPoint(face.points[1], p2.data());
        auto p3 = Vec3d{};
        points->GetPoint(face.points[2], p3.data());

        auto s1 = Mat3d{};
        array1->GetTuple(face.points[0], s1.data());
        auto s2 = Mat3d{};
        array1->GetTuple(face.points[1], s2.data());
        auto s3 = Mat3d{};
        array1->GetTuple(face.points[2], s3.data());

        auto t1 = Mat3d{};
        array2->GetTuple(face.points[0], t1.data());
        auto t2 = Mat3d{};
        array2->GetTuple(face.points[1], t2.data());
        auto t3 = Mat3d{};
        array2->GetTuple(face.points[2], t3.data());

        auto points = tl::findParallelEigenvectors(
                {s1, s2, s3},
                {t1, t2, t3},
                {p1, p2, p3},
                opts);
        results[i] = points;
#pragma omp critical(progress)
        {
            progress_alg->UpdateProgress(progress_alg->GetProgress() + step);
            if(progress_alg->GetAbortExecute())
            {
                std::cout << "Terminate request accepted" << std::endl;
                terminate = true;
            }
        }
    }
    return results;
}

std::vector<tl::TLResult> computeTCLPoints(const std::vector<TriFace>& faces,
                                             vtkPoints* points,
                                             vtkDataArray* tensors,
                                             vtkDataArray* tx,
                                             vtkDataArray* ty,
                                             vtkDataArray* tz,
                                             vtkAlgorithm* progress_alg,
                                             const tl::TLOptions& opts)
{
    const auto step = 1. / double(faces.size());
    progress_alg->UpdateProgress(0);
    auto results = std::vector<tl::TLResult>(faces.size());
    auto terminate = false;
#pragma omp parallel for
    for(auto i = std::size_t{0}; i < faces.size(); ++i)
    {
#pragma omp flush(terminate)
        if(terminate) continue;

        auto face = faces[i];

        auto p1 = Vec3d{};
        points->GetPoint(face.points[0], p1.data());
        auto p2 = Vec3d{};
        points->GetPoint(face.points[1], p2.data());
        auto p3 = Vec3d{};
        points->GetPoint(face.points[2], p3.data());

        auto s1 = Mat3d{};
        tensors->GetTuple(face.points[0], s1.data());
        auto s2 = Mat3d{};
        tensors->GetTuple(face.points[1], s2.data());
        auto s3 = Mat3d{};
        tensors->GetTuple(face.points[2], s3.data());

        auto sx = Mat3d{};
        tx->GetTuple(face.cellId, sx.data());

        auto sy = Mat3d{};
        ty->GetTuple(face.cellId, sy.data());

        auto sz = Mat3d{};
        tz->GetTuple(face.cellId, sz.data());

        auto points = tl::findTensorCoreLines(
                {s1, s2, s3},
                {sx, sy, sz},
                {p1, p2, p3},
                opts);
        results[i] = points;
#pragma omp critical(progress)
        {
            progress_alg->UpdateProgress(progress_alg->GetProgress() + step);
            if(progress_alg->GetAbortExecute())
            {
                std::cout << "Terminate request accepted" << std::endl;
                terminate = true;
            }
        }
    }
    return results;
}

std::vector<tl::TLResult> computeTopoPoints(const std::vector<TriFace>& faces,
                                            vtkPoints* points,
                                            vtkDataArray* tensors,
                                            vtkAlgorithm* progress_alg,
                                            const tl::TLOptions& opts)
{
    const auto step = 1. / double(faces.size());
    progress_alg->UpdateProgress(0);
    auto results = std::vector<tl::TLResult>(faces.size());
    auto terminate = false;
#pragma omp parallel for
    for(auto i = std::size_t{0}; i < faces.size(); ++i)
    {
#pragma omp flush(terminate)
        if(terminate) continue;

        auto face = faces[i];

        auto p1 = Vec3d{};
        points->GetPoint(face.points[0], p1.data());
        auto p2 = Vec3d{};
        points->GetPoint(face.points[1], p2.data());
        auto p3 = Vec3d{};
        points->GetPoint(face.points[2], p3.data());

        auto s1 = Mat3d{};
        tensors->GetTuple(face.points[0], s1.data());
        auto s2 = Mat3d{};
        tensors->GetTuple(face.points[1], s2.data());
        auto s3 = Mat3d{};
        tensors->GetTuple(face.points[2], s3.data());

        auto points = tl::findTensorTopology(
                {s1, s2, s3},
                {p1, p2, p3},
                opts);
        results[i] = points;
#pragma omp critical(progress)
        {
            progress_alg->UpdateProgress(progress_alg->GetProgress() + step);
            if(progress_alg->GetAbortExecute())
            {
                std::cout << "Terminate request accepted" << std::endl;
                terminate = true;
            }
        }
    }
    return results;
}
}

vtkStandardNewMacro(vtkTensorLines)

vtkTensorLines::vtkTensorLines()
{
    this->SetNumberOfInputPorts(1);
    this->SetNumberOfOutputPorts(1);
    this->SetInputArrayToProcess(0,
                                 0,
                                 0,
                                 vtkDataObject::FIELD_ASSOCIATION_POINTS,
                                 vtkDataSetAttributes::TENSORS);
    this->SetInputArrayToProcess(1,
                                 0,
                                 0,
                                 vtkDataObject::FIELD_ASSOCIATION_POINTS,
                                 vtkDataSetAttributes::TENSORS);
}

vtkPolyData* vtkTensorLines::GetOutput()
{
    return this->GetOutput(0);
}

vtkPolyData* vtkTensorLines::GetOutput(int port)
{
    return vtkPolyData::SafeDownCast(this->GetOutputDataObject(port));
}

int vtkTensorLines::ProcessRequest(vtkInformation* request,
                                   vtkInformationVector** inputVector,
                                   vtkInformationVector* outputVector)
{
    // Create an output object of the correct type.
    if(request->Has(vtkDemandDrivenPipeline::REQUEST_DATA_OBJECT()))
    {
        return this->RequestDataObject(request, inputVector, outputVector);
    }
    // generate the data
    if(request->Has(vtkDemandDrivenPipeline::REQUEST_DATA()))
    {
        return this->RequestData(request, inputVector, outputVector);
    }

    if(request->Has(vtkStreamingDemandDrivenPipeline::REQUEST_UPDATE_EXTENT()))
    {
        return this->RequestUpdateExtent(request, inputVector, outputVector);
    }

    // execute information
    if(request->Has(vtkDemandDrivenPipeline::REQUEST_INFORMATION()))
    {
        return this->RequestInformation(request, inputVector, outputVector);
    }

    return this->Superclass::ProcessRequest(request, inputVector, outputVector);
}

int vtkTensorLines::FillOutputPortInformation(int port,
                                              vtkInformation* info)
{
    // now add our info
    if(port == 0)
    {
        info->Set(vtkDataObject::DATA_TYPE_NAME(), "vtkPolyData");
    }
    // else if(port == 1)
    // {
    //     info->Set(vtkDataObject::DATA_TYPE_NAME(), "vtkPolyData");
    // }
    return 1;
}

int vtkTensorLines::FillInputPortInformation(int vtkNotUsed(port),
                                             vtkInformation* info)
{
    info->Set(vtkAlgorithm::INPUT_REQUIRED_DATA_TYPE(), "vtkUnstructuredGrid");
    return 1;
}

int vtkTensorLines::RequestDataObject(
        vtkInformation* vtkNotUsed(request),
        vtkInformationVector** vtkNotUsed(inputVector),
        vtkInformationVector* outputVector)
{
    // RequestDataObject (RDO) is an earlier pipeline pass. During RDO, each
    // filter is supposed to produce an empty data object of the proper type

    auto* outInfo = outputVector->GetInformationObject(0);
    auto* output = vtkPolyData::SafeDownCast(
            outInfo->Get(vtkDataObject::DATA_OBJECT()));
    // auto* outInfo2 = outputVector->GetInformationObject(1);
    // auto* output2 = vtkPolyData::SafeDownCast(
    //         outInfo2->Get(vtkDataObject::DATA_OBJECT()));

    if(!output)
    {
        output = vtkPolyData::New();
        outInfo->Set(vtkDataObject::DATA_OBJECT(), output);
        output->FastDelete();

        this->GetOutputPortInformation(0)->Set(
                vtkDataObject::DATA_EXTENT_TYPE(), output->GetExtentType());
    }
    // if(!output2)
    // {
    //     output2 = vtkPolyData::New();
    //     outInfo2->Set(vtkDataObject::DATA_OBJECT(), output2);
    //     output2->FastDelete();

    //     this->GetOutputPortInformation(1)->Set(
    //             vtkDataObject::DATA_EXTENT_TYPE(), output2->GetExtentType());
    // }

    return 1;
}

int vtkTensorLines::RequestInformation(
        vtkInformation* vtkNotUsed(request),
        vtkInformationVector** vtkNotUsed(inputVector),
        vtkInformationVector* vtkNotUsed(outputVector))
{
    return 1;
}

int vtkTensorLines::RequestUpdateExtent(
        vtkInformation* vtkNotUsed(request),
        vtkInformationVector** inputVector,
        vtkInformationVector* vtkNotUsed(outputVector))
{
    auto numInputPorts = this->GetNumberOfInputPorts();
    for(auto i = 0; i < numInputPorts; i++)
    {
        auto numInputConnections = this->GetNumberOfInputConnections(i);
        for(auto j = 0; j < numInputConnections; j++)
        {
            auto* inputInfo = inputVector[i]->GetInformationObject(j);
            inputInfo->Set(vtkStreamingDemandDrivenPipeline::EXACT_EXTENT(), 1);
        }
    }
    return 1;
}

int vtkTensorLines::RequestData(vtkInformation* vtkNotUsed(request),
                                         vtkInformationVector** inputVector,
                                         vtkInformationVector* outputVector)
{
    using namespace std::chrono;
    using minutes = duration<double, std::chrono::minutes::period>;
    using seconds = duration<double, std::chrono::seconds::period>;
    using milliseconds = duration<double, std::chrono::milliseconds::period>;

    auto* outInfo = outputVector->GetInformationObject(0);
    auto* output = vtkPolyData::SafeDownCast(
            outInfo->Get(vtkDataObject::DATA_OBJECT()));

    // auto* outInfo2 = outputVector->GetInformationObject(1);
    // auto* output2 = vtkPolyData::SafeDownCast(
    //         outInfo2->Get(vtkDataObject::DATA_OBJECT()));

    auto* inInfo = inputVector[0]->GetInformationObject(0);
    auto* input = vtkUnstructuredGrid::SafeDownCast(
            inInfo->Get(vtkDataObject::DATA_OBJECT()));

    // Get the two DataArrays corresponding to the tensor data
    auto* array1 = this->GetInputArrayToProcess(0, inputVector);
    auto* array2 = this->GetInputArrayToProcess(1, inputVector);

    if(!array1 ||
       (_line_type == LineType::ParallelEigenvectors && !array2))
    {
        vtkErrorMacro(<< "Not all input arrays could be found (Maybe you "
                         "specified the wrong name?).");
        return 0;
    }

    // Check if the data arrays have the correct number of components
    if(array1->GetNumberOfComponents() != 9
       || (_line_type == LineType::ParallelEigenvectors
           && array2->GetNumberOfComponents() != 9))
    {
        vtkErrorMacro(<< "All input arrays must be tensors with 9 components.");
        return 0;
    }

    if(this->GetInputArrayInformation(0)->Get(
               vtkDataObject::FIELD_ASSOCIATION())
               != vtkDataObject::FIELD_ASSOCIATION_POINTS
       || (_line_type == LineType::ParallelEigenvectors
           && this->GetInputArrayInformation(1)->Get(
                      vtkDataObject::FIELD_ASSOCIATION())
                      != vtkDataObject::FIELD_ASSOCIATION_POINTS))
    {
        vtkErrorMacro(<< "All input arrays must be point data.");
        return 0;
    }

    // Point and CellArrays for output dataset
    output->SetPoints(vtkPoints::New());
    output->SetVerts(vtkCellArray::New());
    output->SetPolys(vtkCellArray::New());

    // Output arrays for point information
    auto eig_rank1 = vtkSmartPointer<vtkDoubleArray>::New();
    eig_rank1->SetName("Rank1");
    output->GetPointData()->AddArray(eig_rank1);
    auto eig_rank2 = vtkSmartPointer<vtkDoubleArray>::New();
    eig_rank2->SetName("Rank2");
    output->GetPointData()->AddArray(eig_rank2);
    auto eival1 = vtkSmartPointer<vtkDoubleArray>::New();
    eival1->SetName("Eigenvalue 1");
    output->GetPointData()->AddArray(eival1);
    auto eival2 = vtkSmartPointer<vtkDoubleArray>::New();
    eival2->SetName("Eigenvalue 2");
    output->GetPointData()->AddArray(eival2);
    auto eivec = vtkSmartPointer<vtkDoubleArray>::New();
    eivec->SetName("Eigenvector");
    eivec->SetNumberOfComponents(3);
    output->GetPointData()->SetVectors(eivec);
    auto imag1 = vtkSmartPointer<vtkDoubleArray>::New();
    imag1->SetName("Imaginary 1");
    output->GetPointData()->AddArray(imag1);
    auto imag2 = vtkSmartPointer<vtkDoubleArray>::New();
    imag2->SetName("Imaginary 2");
    output->GetPointData()->AddArray(imag2);
    auto csize = vtkSmartPointer<vtkUnsignedLongLongArray>::New();
    csize->SetName("Cluster Size");
    output->GetPointData()->AddArray(csize);
    auto pos_unc = vtkSmartPointer<vtkDoubleArray>::New();
    pos_unc->SetName("Position Uncertainty");
    output->GetPointData()->AddArray(pos_unc);
    auto dir_unc = vtkSmartPointer<vtkDoubleArray>::New();
    dir_unc->SetName("Direction Uncertainty");
    output->GetPointData()->AddArray(dir_unc);
    auto stability = vtkSmartPointer<vtkDoubleArray>::New();
    stability->SetName("Line Stability");
    output->GetPointData()->AddArray(stability);

    // // List of faces that might have non-line structures in separate output
    // output2->SetPoints(vtkPoints::New());
    // output2->GetPoints()->DeepCopy(input->GetPoints());
    // output2->Allocate();

    // auto direction = vtkSmartPointer<vtkDoubleArray>::New();
    // direction->SetNumberOfComponents(3);
    // direction->SetName("Direction");
    // output2->GetCellData()->AddArray(direction);

    // Copy faces to array for parallel looping
    auto faces = buildFaceList(input);
    auto start = high_resolution_clock::now();

    auto opts = tl::TLOptions{this->GetTolerance(),
                                this->GetClusterEpsilon(),
                                this->GetMaxCandidates()};

    auto fresults = std::vector<tl::TLResult>{};

    if(_line_type == LineType::TensorCoreLines)
    {
        auto derivs = computeCellDerivatives(input, array1->GetName());
        fresults = computeTCLPoints(faces,
                                   input->GetPoints(),
                                   array1,
                                   derivs[0],
                                   derivs[1],
                                   derivs[2],
                                   this,
                                   opts);
    }
    else if(_line_type == LineType::ParallelEigenvectors)
    {
        fresults = computePEVPoints(
                faces, input->GetPoints(), array1, array2, this, opts);
    }
    else if(_line_type == LineType::TensorTopology)
    {
        fresults = computeTopoPoints(
                faces, input->GetPoints(), array1, this, opts);
    }

    auto end_pointsearch = high_resolution_clock::now();

    // map cell IDs to parallel eigenvector points found on their faces
    auto cell_map = std::map<vtkIdType, vtkSmartPointer<vtkIdList>>{};

    for(auto i : range(faces.size()))
    {
        auto pev_points = fresults[i];
        auto cid = faces[i].cellId;
        for(const auto& p : pev_points.points)
        {
            auto pid = output->GetPoints()->InsertNextPoint(p.pos.data());
            eig_rank1->InsertValue(pid, double(p.s_rank));
            eig_rank2->InsertValue(pid, double(p.t_rank));
            eival1->InsertValue(pid, p.s_eival);
            eival2->InsertValue(pid, p.t_eival);
            eivec->InsertTuple(pid, p.eivec.data());
            imag1->InsertValue(pid, p.s_has_imaginary ? 1. : 0.);
            imag2->InsertValue(pid, p.t_has_imaginary ? 1. : 0.);
            csize->InsertValue(pid, p.cluster_size);
            pos_unc->InsertValue(pid, p.pos_uncertainty);
            dir_unc->InsertValue(pid, p.dir_uncertainty);
            stability->InsertValue(pid, p.line_stability);

            if(!cell_map[cid].Get())
            {
                cell_map[cid] = vtkSmartPointer<vtkIdList>::New();
            }
            cell_map[cid]->InsertNextId(pid);
        }
    }

    output->SetLines(vtkCellArray::New());

    auto cell_points = vtkSmartPointer<vtkIdList>::New();

    for(const auto& c : cell_map)
    {
        auto point_list = c.second;
        auto npoints = point_list->GetNumberOfIds();
        // For cells with exactly two parallel eigenvector points, connect them
        // with a line
        if(npoints == 2)
        {
            output->InsertNextCell(VTK_LINE, point_list);
        }
        else
        {
            // Match points by eigenvector direction

            using Matrix3X = Eigen::Matrix3Xd;
            using MatrixX = Eigen::MatrixXd;

            // Get eigenvectors of cell points
            auto eigdirs = Matrix3X(3, npoints).eval();
            for(auto i : range(npoints))
            {
                eigdirs.col(i) = Vec3dm{eivec->GetTuple(point_list->GetId(i))};
            }

            // Compute pairwise vector deviations
            auto dist = MatrixX::Ones(npoints, npoints).eval();
            for(auto i : range(npoints))
            {
                for(auto j : range(i + 1, npoints))
                {
                    dist(i, j) =
                            eigdirs.col(i).cross(eigdirs.col(j)).squaredNorm();
                }
            }

            // Greedily find closest two vectors and connect
            auto unlinked = std::unordered_set<vtkIdType>{};
            for(auto i : range(npoints))
            {
                unlinked.insert(i);
            }
            if(npoints < 10)
            {
                while(dist.sum() < double(npoints * npoints))
                {
                    auto row = Matrix3X::Index{};
                    auto col = Matrix3X::Index{};
                    dist.minCoeff(&row, &col);
                    auto line = vtkSmartPointer<vtkIdList>::New();
                    line->SetNumberOfIds(2);
                    line->InsertId(0, point_list->GetId(row));
                    line->InsertId(1, point_list->GetId(col));
                    output->InsertNextCell(VTK_LINE, line);
                    dist.col(col).setOnes();
                    dist.col(row).setOnes();
                    dist.row(col).setOnes();
                    dist.row(row).setOnes();
                    unlinked.erase(row);
                    unlinked.erase(col);
                }
            }

            // Add vertex for last unlinked point if any
            for(auto i : unlinked)
            {
                output->InsertNextCell(
                        VTK_VERTEX, 1, point_list->GetPointer(i));
            }
        }
    }

    // for(auto i : range(faces.size()))
    // {
    //     auto failed_dirs = fresults[i].non_line_dirs;
    //     auto cellpts = vtkSmartPointer<vtkIdList>::New();
    //     cellpts->SetNumberOfIds(3);
    //     cellpts->InsertId(0, faces[i].points[0]);
    //     cellpts->InsertId(1, faces[i].points[1]);
    //     cellpts->InsertId(2, faces[i].points[2]);
    //     for(const auto& f: failed_dirs)
    //     {
    //         auto cid = output2->InsertNextCell(VTK_TRIANGLE, cellpts);
    //         direction->InsertTuple(cid, f.data());
    //     }
    // }

    auto end_all = high_resolution_clock::now();
    auto duration_all = seconds(end_all - start);
    auto duration_pointsearch = seconds(end_pointsearch - start);

    std::cout << "Processed dataset in " << minutes(duration_all).count()
              << " minutes" << std::endl;
    std::cout << "Point search time: " << minutes(duration_pointsearch).count()
              << " minutes" << std::endl;
    std::cout << "Postprocessing time: "
              << seconds(duration_all - duration_pointsearch).count()
              << " seconds" << std::endl;
    std::cout << "Number of faces processed: " << faces.size() << std::endl;
    std::cout << "Average time per face: "
              << (milliseconds(duration_pointsearch) / faces.size()).count()
              << " milliseconds" << std::endl;

    this->UpdateProgress(1.);

    return 1;
}


/**
 * @file dex.cpp
 *
 * This file contains DEx logic.
 */

#include "omnicore/dex.h"

#include "omnicore/convert.h"
#include "omnicore/dbtxlist.h"
#include "omnicore/errors.h"
#include "omnicore/log.h"
#include "omnicore/omnicore.h"
#include "omnicore/rules.h"
#include "omnicore/uint256_extensions.h"

#include "arith_uint256.h"
#include "main.h"
#include "tinyformat.h"
#include "uint256.h"

#include <boost/algorithm/string.hpp>
#include <boost/format.hpp>

#include <openssl/sha.h>

#include <stdint.h>

#include <fstream>
#include <map>
#include <set>
#include <string>
#include <utility>
#include <vector>

namespace mastercore
{
/**
 * Checks, if such a sell offer exists.
 */
bool DEx_offerExists(const std::string& addressSeller, uint32_t propertyId)
{
    std::string key = STR_SELLOFFER_ADDR_PROP_COMBO(addressSeller, propertyId);

    return !(my_offers.find(key) == my_offers.end());
}

/**
 * Retrieves a sell offer.
 *
 * @return The sell offer, or NULL, if no match was found
 */
CMPOffer* DEx_getOffer(const std::string& addressSeller, uint32_t propertyId)
{
    if (msc_debug_dex) PrintToLog("%s(%s, %d)\n", __func__, addressSeller, propertyId);

    std::string key = STR_SELLOFFER_ADDR_PROP_COMBO(addressSeller, propertyId);
    OfferMap::iterator it = my_offers.find(key);

    if (it != my_offers.end()) return &(it->second);

    return NULL;
}

/**
 * Checks, if such an accept order exists.
 */
bool DEx_acceptExists(const std::string& addressSeller, uint32_t propertyId, const std::string& addressBuyer)
{
    std::string key = STR_ACCEPT_ADDR_PROP_ADDR_COMBO(addressSeller, addressBuyer, propertyId);

    return !(my_accepts.find(key) == my_accepts.end());
}

/**
 * Retrieves an accept order.
 *
 * @return The accept order, or NULL, if no match was found
 */
CMPAccept* DEx_getAccept(const std::string& addressSeller, uint32_t propertyId, const std::string& addressBuyer)
{
    if (msc_debug_dex) PrintToLog("%s(%s, %d, %s)\n", __func__, addressSeller, propertyId, addressBuyer);

    std::string key = STR_ACCEPT_ADDR_PROP_ADDR_COMBO(addressSeller, addressBuyer, propertyId);
    AcceptMap::iterator it = my_accepts.find(key);

    if (it != my_accepts.end()) return &(it->second);

    return NULL;
}

namespace legacy
{
/**
 * Legacy calculation of Master Core 0.0.9.
 *
 * @see:
 * https://github.com/mastercoin-MSC/mastercore/blob/mscore-0.0.9/src/mastercore_dex.cpp#L439-L449
 */
static int64_t calculateDesiredBTC(const int64_t amountOffered, const int64_t amountDesired, const int64_t amountAvailable)
{
    uint64_t nValue = static_cast<uint64_t>(amountOffered);
    uint64_t amount_des = static_cast<uint64_t>(amountDesired);
    uint64_t balanceReallyAvailable = static_cast<uint64_t>(amountAvailable);

    double BTC;

    BTC = amount_des * balanceReallyAvailable;
    BTC /= (double) nValue;
    amount_des = rounduint64(BTC);

    return static_cast<int64_t>(amount_des);
}
}

/**
 * Determines the amount of bitcoins desired, in case it needs to be recalculated.
 *
 * TODO: don't expose it!
 * @return The amount of bitcoins desired
 */
int64_t calculateDesiredBTC(const int64_t amountOffered, const int64_t amountDesired, const int64_t amountAvailable)
{
    if (amountOffered == 0) {
        return 0; // divide by null protection
    }

    arith_uint256 amountOffered256 = ConvertTo256(amountOffered);
    arith_uint256 amountDesired256 = ConvertTo256(amountDesired);
    arith_uint256 amountAvailable256 = ConvertTo256(amountAvailable);

    amountDesired256 = DivideAndRoundUp((amountDesired256 * amountAvailable256), amountOffered256);

    return ConvertTo64(amountDesired256);
}

/**
 * Creates a new sell offer.
 *
 * TODO: change nAmended: uint64_t -> int64_t
 * @return 0 if everything is OK
 */
int DEx_offerCreate(const std::string& addressSeller, uint32_t propertyId, int64_t amountOffered, int block, int64_t amountDesired, int64_t minAcceptFee, uint8_t paymentWindow, const uint256& txid, uint64_t* nAmended)
{
    int rc = DEX_ERROR_SELLOFFER;

    // sanity checks
    // shold not be removed, because it may be used when updating/destroying an offer
    if (paymentWindow == 0) {
        return (DEX_ERROR_SELLOFFER -101);
    }
    if (amountDesired == 0) {
        return (DEX_ERROR_SELLOFFER -101);
    }
    if (DEx_getOffer(addressSeller, propertyId)) {
        return (DEX_ERROR_SELLOFFER -10); // offer already exists
    }

    const std::string key = STR_SELLOFFER_ADDR_PROP_COMBO(addressSeller, propertyId);
    if (msc_debug_dex) PrintToLog("%s(%s|%s), nValue=%d)\n", __func__, addressSeller, key, amountOffered);

    const int64_t balanceReallyAvailable = GetTokenBalance(addressSeller, propertyId, BALANCE);

    /**
     * After this feature is enabled, it is no longer valid to create orders, which offer more than
     * the seller has available, and the amounts are no longer adjusted based on the actual balance.
     */
    if (IsFeatureActivated(FEATURE_DEXMATH, block)) {
        if (amountOffered > balanceReallyAvailable) {
            PrintToLog("%s: rejected: sender %s has insufficient balance of property %d [%s < %s]\n", __func__,
                        addressSeller, propertyId, FormatDivisibleMP(balanceReallyAvailable), FormatDivisibleMP(amountOffered));
            return (DEX_ERROR_SELLOFFER -25);
        }
    }

    // -------------------------------------------------------------------------
    // legacy::

    // if offering more than available -- put everything up on sale
    if (amountOffered > balanceReallyAvailable) {
        PrintToLog("%s: adjusting order: %s offers %s %s, but has only %s %s available\n", __func__,
                        addressSeller, FormatDivisibleMP(amountOffered), strMPProperty(propertyId),
                        FormatDivisibleMP(balanceReallyAvailable), strMPProperty(propertyId));

        // AND we must also re-adjust the BTC desired in this case...
        amountDesired = legacy::calculateDesiredBTC(amountOffered, amountDesired, balanceReallyAvailable);
        amountOffered = balanceReallyAvailable;
        if (nAmended) *nAmended = amountOffered;

        PrintToLog("%s: adjusting order: updated amount for sale: %s %s, offered for: %s BTC\n", __func__,
                        FormatDivisibleMP(amountOffered), strMPProperty(propertyId), FormatDivisibleMP(amountDesired));
    }
    // -------------------------------------------------------------------------

    if (amountOffered > 0) {
        assert(update_tally_map(addressSeller, propertyId, -amountOffered, BALANCE));
        assert(update_tally_map(addressSeller, propertyId, amountOffered, SELLOFFER_RESERVE));

        CMPOffer sellOffer(block, amountOffered, propertyId, amountDesired, minAcceptFee, paymentWindow, txid);
        my_offers.insert(std::make_pair(key, sellOffer));

        rc = 0;
    }

    return rc;
}

/**
 * Destorys a sell offer.
 *
 * The remaining amount reserved for the offer is returned to the available balance.
 *
 * @return 0 if the offer was destroyed
 */
int DEx_offerDestroy(const std::string& addressSeller, uint32_t propertyId)
{
    if (!DEx_offerExists(addressSeller, propertyId)) {
        return (DEX_ERROR_SELLOFFER -11); // offer does not exist
    }

    const int64_t amountReserved = GetTokenBalance(addressSeller, propertyId, SELLOFFER_RESERVE);

    // return the remaining reserved amount back to the seller
    if (amountReserved > 0) {
        assert(update_tally_map(addressSeller, propertyId, -amountReserved, SELLOFFER_RESERVE));
        assert(update_tally_map(addressSeller, propertyId, amountReserved, BALANCE));
    }

    // delete the offer
    const std::string key = STR_SELLOFFER_ADDR_PROP_COMBO(addressSeller, propertyId);
    OfferMap::iterator it = my_offers.find(key);
    my_offers.erase(it);

    if (msc_debug_dex) PrintToLog("%s(%s|%s)\n", __func__, addressSeller, key);

    return 0;
}

/**
 * Updates a sell offer.
 *
 * The old offer is destroyed, and replaced by the updated offer.
 *
 * @return 0 if everything is OK
 */
int DEx_offerUpdate(const std::string& addressSeller, uint32_t propertyId, int64_t amountOffered, int block, int64_t amountDesired, int64_t minAcceptFee, uint8_t paymentWindow, const uint256& txid, uint64_t* nAmended)
{
    if (msc_debug_dex) PrintToLog("%s(%s, %d)\n", __func__, addressSeller, propertyId);

    if (!DEx_offerExists(addressSeller, propertyId)) {
        return (DEX_ERROR_SELLOFFER -12); // offer does not exist
    }

    int rc = DEx_offerDestroy(addressSeller, propertyId);

    if (rc == 0) {
        // if the old offer was destroyed successfully, try to create a new one
        rc = DEx_offerCreate(addressSeller, propertyId, amountOffered, block, amountDesired, minAcceptFee, paymentWindow, txid, nAmended);
    }

    return rc;
}

/**
 * Creates a new accept order.
 *
 * TODO: change nAmended: uint64_t -> int64_t
 * @return 0 if everything is OK
 */
int DEx_acceptCreate(const std::string& addressBuyer, const std::string& addressSeller, uint32_t propertyId, int64_t amountAccepted, int block, int64_t feePaid, uint64_t* nAmended)
{
    int rc = DEX_ERROR_ACCEPT -10;
    const std::string keySellOffer = STR_SELLOFFER_ADDR_PROP_COMBO(addressSeller, propertyId);
    const std::string keyAcceptOrder = STR_ACCEPT_ADDR_PROP_ADDR_COMBO(addressSeller, addressBuyer, propertyId);

    OfferMap::const_iterator my_it = my_offers.find(keySellOffer);

    if (my_it == my_offers.end()) {
        PrintToLog("%s: rejected: no matching sell offer for accept order found\n", __func__);
        return (DEX_ERROR_ACCEPT -15);
    }

    const CMPOffer& offer = my_it->second;

    PrintToLog("%s: found a matching sell offer [seller: %s, buyer: %s, property: %d)\n", __func__,
                    addressSeller, addressBuyer, propertyId);

    // the older accept is the valid one: do not accept any new ones!
    if (DEx_acceptExists(addressSeller, propertyId, addressBuyer)) {
        PrintToLog("%s: rejected: an accept order from this same seller for this same offer already exists\n", __func__);
        return DEX_ERROR_ACCEPT -205;
    }

    // ensure the correct BTC fee was paid in this acceptance message
    if (feePaid < offer.getMinFee()) {
        PrintToLog("%s: rejected: transaction fee too small [%d < %d]\n", __func__, feePaid, offer.getMinFee());
        return DEX_ERROR_ACCEPT -105;
    }

    int64_t amountReserved = 0;
    int64_t amountRemainingForSale = GetTokenBalance(addressSeller, propertyId, SELLOFFER_RESERVE);

    // ensure the buyer only can reserve the amount that is still available
    if (amountRemainingForSale >= amountAccepted) {
        amountReserved = amountAccepted;
    } else {
        amountReserved = amountRemainingForSale;
        PrintToLog("%s: buyer wants to reserve %d tokens, but only %d tokens are available\n", __func__, amountAccepted, amountRemainingForSale);
    }

    if (amountReserved > 0) {
        assert(update_tally_map(addressSeller, propertyId, -amountReserved, SELLOFFER_RESERVE));
        assert(update_tally_map(addressSeller, propertyId, amountReserved, ACCEPT_RESERVE));

        CMPAccept acceptOffer(amountReserved, block, offer.getBlockTimeLimit(), offer.getProperty(), offer.getOfferAmountOriginal(), offer.getBTCDesiredOriginal(), offer.getHash());
        my_accepts.insert(std::make_pair(keyAcceptOrder, acceptOffer));

        rc = 0;
    }

    if (nAmended) *nAmended = amountReserved;

    return rc;
}

/**
 * Finalizes an accepted offer.
 *
 * This function is called by handler_block() for each accepted offer that has
 * expired. This function is also called when the purchase has been completed,
 * and the buyer bought everything that was reserved.
 *
 * @return 0 if everything is OK
 */
int DEx_acceptDestroy(const std::string& addressBuyer, const std::string& addressSeller, uint32_t propertyid, bool fForceErase)
{
    int rc = DEX_ERROR_ACCEPT -20;
    CMPOffer* p_offer = DEx_getOffer(addressSeller, propertyid);
    CMPAccept* p_accept = DEx_getAccept(addressSeller, propertyid, addressBuyer);
    bool fReturnToMoney = true;

    if (!p_accept) return rc; // sanity check

    // if the offer is gone, ACCEPT_RESERVE should go back to seller's BALANCE
    // otherwise move the previously accepted amount back to SELLOFFER_RESERVE
    if (!p_offer) {
        fReturnToMoney = true;
    } else {
        PrintToLog("%s: finalize trade [offer=%s, accept=%s]\n", __func__,
                        p_offer->getHash().GetHex(), p_accept->getHash().GetHex());

        // offer exists, determine whether it's the original offer or some random new one
        if (p_offer->getHash() == p_accept->getHash()) {
            // same offer, return to SELLOFFER_RESERVE
            fReturnToMoney = false;
        } else {
            // old offer is gone !
            fReturnToMoney = true;
        }
    }

    const int64_t amountRemaining = p_accept->getAcceptAmountRemaining();

    if (amountRemaining > 0) {
        if (fReturnToMoney) {
            assert(update_tally_map(addressSeller, propertyid, -amountRemaining, ACCEPT_RESERVE));
            assert(update_tally_map(addressSeller, propertyid, amountRemaining, BALANCE));
        } else {
            assert(update_tally_map(addressSeller, propertyid, -amountRemaining, ACCEPT_RESERVE));
            assert(update_tally_map(addressSeller, propertyid, amountRemaining, SELLOFFER_RESERVE));
        }
    }

    // can only erase when is NOT called from an iterator loop
    if (fForceErase) {
        std::string key = STR_ACCEPT_ADDR_PROP_ADDR_COMBO(addressSeller, addressBuyer, propertyid);
        AcceptMap::iterator it = my_accepts.find(key);

        if (my_accepts.end() != it) {
            my_accepts.erase(it);
        }
    }

    rc = 0;
    return rc;
}

namespace legacy
{
/**
 * Legacy calculation of Master Core 0.0.9.
 *
 * @see:
 * https://github.com/mastercoin-MSC/mastercore/blob/mscore-0.0.9/src/mastercore_dex.cpp#L660-L668
 */
static int64_t calculateDExPurchase(const int64_t amountOffered, const int64_t amountDesired, const int64_t amountPaid)
{
    uint64_t acceptOfferAmount = static_cast<uint64_t>(amountOffered);
    uint64_t acceptBTCDesired = static_cast<uint64_t>(amountDesired);
    uint64_t BTC_paid = static_cast<uint64_t>(amountPaid);

    const double BTC_desired_original = acceptBTCDesired;
    const double offer_amount_original = acceptOfferAmount;

    double perc_X = (double) BTC_paid / BTC_desired_original;
    double Purchased = offer_amount_original * perc_X;

    uint64_t units_purchased = rounduint64(Purchased);

    return static_cast<int64_t>(units_purchased);
}
}

/**
 * Determines the purchased amount of tokens.
 *
 * The amount is rounded up, which may be in favor of the buyer, to avoid small
 * leftovers of 1 willet. This is not exploitable due to transaction fees.
 *
 * The desired amount must not be zero, and the purchased amount should be checked
 * against the actual amount that is still left for sale.
 *
 * @return The amount of tokens purchased
 */
int64_t calculateDExPurchase(const int64_t amountOffered, const int64_t amountDesired, const int64_t amountPaid)
{
    // conversion
    arith_uint256 amountOffered256 = ConvertTo256(amountOffered);
    arith_uint256 amountDesired256 = ConvertTo256(amountDesired);
    arith_uint256 amountPaid256 = ConvertTo256(amountPaid);

    // actual calculation; round up
    arith_uint256 amountPurchased256 = DivideAndRoundUp((amountPaid256 * amountOffered256), amountDesired256);

    // convert back to int64_t
    return ConvertTo64(amountPurchased256);
}

/**
 * Handles incoming BTC payment for the offer.
 * TODO: change nAmended: uint64_t -> int64_t
 */
int DEx_payment(const uint256& txid, unsigned int vout, const std::string& addressSeller, const std::string& addressBuyer, int64_t amountPaid, int block, uint64_t* nAmended)
{
    if (msc_debug_dex) PrintToLog("%s(%s, %s)\n", __func__, addressSeller, addressBuyer);

    int rc = DEX_ERROR_PAYMENT;

    uint32_t propertyId = OMNI_PROPERTY_MSC; // test for MSC accept first
    CMPAccept* p_accept = DEx_getAccept(addressSeller, propertyId, addressBuyer);

    if (!p_accept) {
        propertyId = OMNI_PROPERTY_TMSC; // test for TMSC accept second
        p_accept = DEx_getAccept(addressSeller, propertyId, addressBuyer);
    }

    if (!p_accept) {
        // there must be an active accept order for this payment
        return (DEX_ERROR_PAYMENT -1);
    }

    // -------------------------------------------------------------------------

    const int64_t amountDesired = p_accept->getBTCDesiredOriginal();
    const int64_t amountOffered = p_accept->getOfferAmountOriginal();

    // divide by 0 protection
    if (0 == amountDesired) {
        if (msc_debug_dex) PrintToLog("%s: ERROR: desired amount of accept order is zero", __func__);

        return (DEX_ERROR_PAYMENT -2);
    }

    int64_t amountPurchased = 0;

    /**
     * As long as this feature is disabled, floating point math is used to
     * determine the purchased amount.
     *
     * After this feature is enabled, plain integer math is used to determine
     * the purchased amount. The purchased amount is rounded up, which may be
     * in favor of the buyer, to avoid small leftovers of 1 willet.
     *
     * This is not exploitable due to transaction fees.
     */
    if (IsFeatureActivated(FEATURE_DEXMATH, block)) {
        amountPurchased = calculateDExPurchase(amountOffered, amountDesired, amountPaid);
    } else {
        // Fallback to original calculation:
        amountPurchased = legacy::calculateDExPurchase(amountOffered, amountDesired, amountPaid);
    }

    // -------------------------------------------------------------------------

    const int64_t amountRemaining = p_accept->getAcceptAmountRemaining(); // actual amount desired, in the Accept

    if (msc_debug_dex) PrintToLog(
            "%s: BTC desired: %s, offered amount: %s, amount to purchase: %s, amount remaining: %s\n", __func__,
            FormatDivisibleMP(amountDesired), FormatDivisibleMP(amountOffered),
            FormatDivisibleMP(amountPurchased), FormatDivisibleMP(amountRemaining));

    // if units_purchased is greater than what's in the Accept, the buyer gets only what's in the Accept
    if (amountRemaining < amountPurchased) {
        amountPurchased = amountRemaining;

        if (nAmended) *nAmended = amountPurchased;
    }

    if (amountPurchased > 0) {
        PrintToLog("%s: seller %s offered %s %s for %s BTC\n", __func__,
                addressSeller, FormatDivisibleMP(amountOffered), strMPProperty(propertyId), FormatDivisibleMP(amountDesired));
        PrintToLog("%s: buyer %s pays %s BTC to purchase %s %s\n", __func__,
                addressBuyer, FormatDivisibleMP(amountPaid), FormatDivisibleMP(amountPurchased), strMPProperty(propertyId));

        assert(update_tally_map(addressSeller, propertyId, -amountPurchased, ACCEPT_RESERVE));
        assert(update_tally_map(addressBuyer, propertyId, amountPurchased, BALANCE));

        bool valid = true;
        pDbTransactionList->recordPaymentTX(txid, valid, block, vout, propertyId, amountPurchased, addressBuyer, addressSeller);

        rc = 0;
        PrintToLog("#######################################################\n");
    }

    // reduce the amount of units still desired by the buyer and if 0 destroy the Accept order
    if (p_accept->reduceAcceptAmountRemaining_andIsZero(amountPurchased)) {
        const int64_t reserveSell = GetTokenBalance(addressSeller, propertyId, SELLOFFER_RESERVE);
        const int64_t reserveAccept = GetTokenBalance(addressSeller, propertyId, ACCEPT_RESERVE);

        DEx_acceptDestroy(addressBuyer, addressSeller, propertyId, true);

        // delete the Offer object if there is nothing in its Reserves -- everything got puchased and paid for
        if ((0 == reserveSell) && (0 == reserveAccept)) {
            DEx_offerDestroy(addressSeller, propertyId);
        }
    }

    return rc;
}

unsigned int eraseExpiredAccepts(int blockNow)
{
    unsigned int how_many_erased = 0;
    AcceptMap::iterator it = my_accepts.begin();

    while (my_accepts.end() != it) {
        const CMPAccept& acceptOrder = it->second;

        int blocksSinceAccept = blockNow - acceptOrder.getAcceptBlock();
        int blocksPaymentWindow = static_cast<int>(acceptOrder.getBlockTimeLimit());

        if (blocksSinceAccept >= blocksPaymentWindow) {
            PrintToLog("%s: sell offer: %s\n", __func__, acceptOrder.getHash().GetHex());
            PrintToLog("%s: erasing at block: %d, order confirmed at block: %d, payment window: %d\n",
                    __func__, blockNow, acceptOrder.getAcceptBlock(), acceptOrder.getBlockTimeLimit());

            // extract the seller, buyer and property from the key
            std::vector<std::string> vstr;
            boost::split(vstr, it->first, boost::is_any_of("-+"), boost::token_compress_on);
            std::string addressSeller = vstr[0];
            uint32_t propertyId = atoi(vstr[1]);
            std::string addressBuyer = vstr[2];

            DEx_acceptDestroy(addressBuyer, addressSeller, propertyId);

            my_accepts.erase(it++);

            ++how_many_erased;
        } else it++;
    }

    return how_many_erased;
}

} // namespace mastercore


/**
* @file EternalForest.cpp
*
* Copyright (c) 2015 by Gruzdev Alexey
*
* Code covered by the MIT License
* The authors make no representations about the suitability of this software
* for any purpose. It is provided "as is" without express or implied warranty.
*/

#include "EternalForest.h"

#include <boost/multi_array.hpp>

#include <OgreSceneManager.h>
#include <OgreEntity.h>
#include <OgreSceneNode.h>

#include "Ground.h"
#include "World.h"

const float EternalForest::FIELD_BLOCK_SIZE  = 1.0f;
const float EternalForest::FIELD_UPDATE_TICK = 1.0f;
//-------------------------------------------------------
EternalForest::EternalForest(Ogre::SceneManager* sceneManager, const World* world, const Ground* ground, const Ogre::AxisAlignedBox & forestBorders):
    mBorders(forestBorders), mSceneManager(sceneManager), mGround(ground), mWorld(world), mUpdateTickController(FIELD_UPDATE_TICK)
{

}
//-------------------------------------------------------
EternalForest::~EternalForest()
{

}
//-------------------------------------------------------
void EternalForest::InitField(size_t startAmount)
{
    Ogre::Vector3 minBorder = mBorders.getMinimum();
    Ogre::Vector3 maxBorder = mBorders.getMaximum();

    uint32_t fieldSizeX = static_cast<uint32_t>((maxBorder[0] - minBorder[0]) / FIELD_BLOCK_SIZE);
    uint32_t fieldSizeZ = static_cast<uint32_t>((maxBorder[2] - minBorder[2]) / FIELD_BLOCK_SIZE);

    mFieldOffset[0] = 0.5f * std::fmod(maxBorder[0] - minBorder[0], FIELD_BLOCK_SIZE) + minBorder[0];
    mFieldOffset[1] = 0.5f * std::fmod(maxBorder[0] - minBorder[0], FIELD_BLOCK_SIZE) + minBorder[2];

    mLifeField = std::make_unique<LifeField>(boost::extents[fieldSizeZ][fieldSizeX]);
    mLifeFieldNext = std::make_unique<LifeField>(boost::extents[fieldSizeZ][fieldSizeX]);

    LifeField& field = *mLifeField;
    for (uint32_t z = 0; z < fieldSizeZ; ++z)
    {
        for (uint32_t x = 0; x < fieldSizeX; ++x)
        {
            if (z == 0 || z == fieldSizeZ - 1 || x == 0 || x == fieldSizeX - 1)
            {
                field[z][x].flags = BlockInfo::BLOCKED;
                continue;
            }
            float s = mFieldOffset[0] + (x + 0.5f) * FIELD_BLOCK_SIZE;
            float t = mFieldOffset[1] + (z + 0.5f) * FIELD_BLOCK_SIZE;
            float h = mWorld->GetGroundHeightAt(s, t);
            field[z][x].flags = (h >= minBorder[1] && h <= maxBorder[1]) ? BlockInfo::EMPTY : BlockInfo::BLOCKED;
            field[z][x]._height = h;
        }
    }

    //generate random start positions
    size_t amount = std::min(startAmount, fieldSizeX * fieldSizeZ);
    while (amount > 0)
    {
        uint8_t attempts = 0;
        while (attempts < 10)
        {
            uint32_t x = static_cast<uint32_t>(Ogre::Math::UnitRandom() * fieldSizeX);
            uint32_t z = static_cast<uint32_t>(Ogre::Math::UnitRandom() * fieldSizeZ);
            if ((*mLifeField)[z][x].flags & BlockInfo::EMPTY)
            {
                (*mLifeField)[z][x].flags = BlockInfo::TREE;
                auto tree = mSceneManager->createEntity("tree_1.mesh");
                auto node = mSceneManager->getRootSceneNode()->createChildSceneNode();
                node->setScale(0.0005f, 0.0005f, 0.0005f);
                node->setPosition(Ogre::Vector3(mFieldOffset[0] + (x + 0.5f) * FIELD_BLOCK_SIZE, (*mLifeField)[z][x]._height, mFieldOffset[1] + (z + 0.5f) * FIELD_BLOCK_SIZE));
                node->attachObject(tree);
                (*mLifeField)[z][x].treeNode = node;
                break;
            }
            ++attempts;
        }
        --amount;
    }
}
//-------------------------------------------------------
void EternalForest::UpdateField(float time, size_t quota)
{
    LifeField& field = *mLifeField;
    uint32_t width =  mLifeField->shape()[0];
    uint32_t height = mLifeField->shape()[1];

    for (uint32_t z = 1; z < height - 1; ++z)
    {
        for (uint32_t x = 1; x < width - 1; ++x)
        {
            BlockInfo& current = field[z][x];
            BlockInfo& next = (*mLifeFieldNext)[z][x];
            assert(current.flags == BlockInfo::BLOCKED || current.flags == BlockInfo::TREE || current.flags == BlockInfo::EMPTY);

            next = current;
            if (0 == (current.flags & BlockInfo::BLOCKED))
            {
                auto neighbors = {
                    field[z - 1][x - 1].flags,
                    field[z - 1][x].flags,
                    field[z - 1][x + 1].flags,
                    field[z][x - 1].flags,
                    field[z][x + 1].flags,
                    field[z + 1][x - 1].flags,
                    field[z + 1][x].flags,
                    field[z + 1][x + 1].flags
                };
                uint8_t sum = 0;
                std::for_each(std::cbegin(neighbors), std::cend(neighbors), [&sum](const uint8_t& flags)->void { if(flags & BlockInfo::TREE) ++sum; });
                if (current.flags & BlockInfo::TREE)
                {
                    if (sum < 3 || sum > 4)
                    {
                        current.treeNode->detachAllObjects();
                        current.treeNode->removeAndDestroyAllChildren();
                        current.treeNode = nullptr;

                        next.flags = BlockInfo::EMPTY;
                        next.treeNode = nullptr;
                    }
                }
                else if (current.flags & BlockInfo::EMPTY)
                {
                    if (sum >= 3 && sum <= 4)
                    {
                        auto tree = mSceneManager->createEntity("tree_1.mesh");
                        auto node = mSceneManager->getRootSceneNode()->createChildSceneNode();
                        node->setScale(0.0005f, 0.0005f, 0.0005f);
                        node->setPosition(Ogre::Vector3(mFieldOffset[0] + (x + 0.5f) * FIELD_BLOCK_SIZE, (*mLifeField)[z][x]._height, mFieldOffset[1] + (z + 0.5f) * FIELD_BLOCK_SIZE));
                        node->attachObject(tree);

                        next.flags = BlockInfo::TREE;
                        next.treeNode = node;
                    }
                }
            }
        }
    }
    mLifeField.swap(mLifeFieldNext);
}
//-------------------------------------------------------
void EternalForest::Update(float time)
{
    if (nullptr == mLifeField.get())
    {
        InitField(6 * mTreesQuota);
    }
    else
    {
        if (mUpdateTickController.Tick(time))
        {
            UpdateField(time, mTreesQuota);
        }
    }
}


#define BOOST_TEST_MODULE
#include <boost/test/unit_test.hpp>
#include <core/util/test_macros.hpp>
#
#include <Eigen/Core>
#include <Eigen/SparseCore>

#include <core/storage/serialization/serialization_includes.hpp>

using namespace turi;
using namespace Eigen;

template <template <typename, int, int, int, int, int> class EigenContainer,
          typename scalar_type, int rows, int cols>
void check_array_save_load() {

  typedef EigenContainer<scalar_type, rows, cols, Eigen::ColMajor, rows, cols> EigenArray;

  std::vector<long> row_list;

  if(rows == Eigen::Dynamic)
    row_list = {0, 1, 23};
  else
    row_list = {rows};

  std::vector<long> col_list;

  if(cols == Eigen::Dynamic)
    col_list = {0, 1, 17};
  else
    col_list = {cols};

  for(size_t _rows : row_list) {
    for(size_t _cols : col_list) {

      EigenArray X;

      X.resize(_rows, _cols);

      X.setRandom();

      {
        // Save it
        dir_archive archive_write;
        archive_write.open_directory_for_write("eigen_serialize_test");

        turi::oarchive oarc(archive_write);

        size_t check = timer::usec_of_day();
        oarc << X << check;

        archive_write.close();

        // Load it
        dir_archive archive_read;
        archive_read.open_directory_for_read("eigen_serialize_test");

        turi::iarchive iarc(archive_read);

        EigenArray X2;
        size_t check_2;

        iarc >> X2 >> check_2;

        TS_ASSERT_EQUALS(X.rows(), X2.rows());
        TS_ASSERT_EQUALS(X.cols(), X2.cols());
        TS_ASSERT_EQUALS(check, check_2);

        for(long r = 0; r < X.rows(); ++r) {
          for(long c = 0; c < X.cols(); ++c) {
            TS_ASSERT_EQUALS(X(r,c), X2(r,c));
          }
        }
      }
    }
  }
}

  ////////////////////////////////////////////////////////////////////////////////
  //
  //  Array Types

  /********   double   ***********/

BOOST_AUTO_TEST_CASE(test_array_simple_double) {
    check_array_save_load<Eigen::Array, double, -1, -1>();
  }

BOOST_AUTO_TEST_CASE(test_array_fix_row_double) {
    check_array_save_load<Eigen::Array, double, -1, 8>();
  }

BOOST_AUTO_TEST_CASE(test_array_fix_col_double) {
    check_array_save_load<Eigen::Array, double, 8, -1>();
  }

BOOST_AUTO_TEST_CASE(test_array_fix_row_col_double) {
    check_array_save_load<Eigen::Array, double, 8, 8>();
  }

  /********   float   ***********/

BOOST_AUTO_TEST_CASE(test_array_simple_float) {
    check_array_save_load<Eigen::Array, float, -1, -1>();
  }

BOOST_AUTO_TEST_CASE(test_array_fix_row_float) {
    check_array_save_load<Eigen::Array, float, -1, 8>();
  }

BOOST_AUTO_TEST_CASE(test_array_fix_col_float) {
    check_array_save_load<Eigen::Array, float, 8, -1>();
  }

BOOST_AUTO_TEST_CASE(test_array_fix_row_col_float) {
    check_array_save_load<Eigen::Array, float, 8, 8>();
  }

  /********   int   ***********/

BOOST_AUTO_TEST_CASE(test_array_simple_int) {
    check_array_save_load<Eigen::Array, int, -1, -1>();
  }

BOOST_AUTO_TEST_CASE(test_array_fix_row_int) {
    check_array_save_load<Eigen::Array, int, -1, 8>();
  }

BOOST_AUTO_TEST_CASE(test_array_fix_col_int) {
    check_array_save_load<Eigen::Array, int, 8, -1>();
  }

BOOST_AUTO_TEST_CASE(test_array_fix_row_col_int) {
    check_array_save_load<Eigen::Array, int, 8, 8>();
  }

  ////////////////////////////////////////////////////////////////////////////////
  //
  //  Matrix Types

  /********   double   ***********/

BOOST_AUTO_TEST_CASE(test_matrix_simple_double) {
    check_array_save_load<Eigen::Matrix, double, -1, -1>();
  }

BOOST_AUTO_TEST_CASE(test_matrix_fix_row_double) {
    check_array_save_load<Eigen::Matrix, double, -1, 8>();
  }

BOOST_AUTO_TEST_CASE(test_matrix_fix_col_double) {
    check_array_save_load<Eigen::Matrix, double, 8, -1>();
  }

BOOST_AUTO_TEST_CASE(test_matrix_fix_row_col_double) {
    check_array_save_load<Eigen::Matrix, double, 8, 8>();
  }

  /********   float   ***********/

BOOST_AUTO_TEST_CASE(test_matrix_simple_float) {
    check_array_save_load<Eigen::Matrix, float, -1, -1>();
  }

BOOST_AUTO_TEST_CASE(test_matrix_fix_row_float) {
    check_array_save_load<Eigen::Matrix, float, -1, 8>();
  }

BOOST_AUTO_TEST_CASE(test_matrix_fix_col_float) {
    check_array_save_load<Eigen::Matrix, float, 8, -1>();
  }

BOOST_AUTO_TEST_CASE(test_matrix_fix_row_col_float) {
    check_array_save_load<Eigen::Matrix, float, 8, 8>();
  }

  /********   int   ***********/

BOOST_AUTO_TEST_CASE(test_matrix_simple_int) {
    check_array_save_load<Eigen::Matrix, int, -1, -1>();
  }

BOOST_AUTO_TEST_CASE(test_matrix_fix_row_int) {
    check_array_save_load<Eigen::Matrix, int, -1, 8>();
  }

BOOST_AUTO_TEST_CASE(test_matrix_fix_col_int) {
    check_array_save_load<Eigen::Matrix, int, 8, -1>();
  }

BOOST_AUTO_TEST_CASE(test_matrix_fix_row_col_int) {
    check_array_save_load<Eigen::Matrix, int, 8, 8>();
  }


/**
 * @file ray_speed.cc
 *
 * Measure the speed of the model in a realistic scenario.
 * Uses the World Ocean Atlas and the ETOPO1 databases to construct a
 * real world environment near the Malta escarpment south-east of Sicily.
 *
 *      - Area 30N-46N, 8W-37E (entire Mediterranean Sea)
 *      - Month: December
 *      - Source: 36N, 16.0E, 100 meters deep
 *      - Targets: 6x6 grid +/- 2.5 degrees around source
 *      - Frequency: 3000 Hz
 *      - Travel Time: 60 seconds
 *      - Time Step: 50 msec
 *      - D/E: [-90,90] as 181 tangent spaced rays
 *      - AZ: [0,360] in 5.0 deg steps
 *
 */
#include <usml/waveq3d/waveq3d.h>
#include <usml/netcdf/netcdf_files.h>
#include <fstream>
#include <iomanip>
#include <boost/progress.hpp>

using namespace usml::waveq3d ;
using namespace usml::netcdf ;
using namespace usml::ocean ;

/**
 * Command line interface.
 */
int main( int argc, char* argv[] ) {
    cout << "=== ray_speed ===" << endl ;

    int num_targets = 100 ;
    if ( argc > 1 ) {
        num_targets = atoi( argv[1] ) ;
    }

    // define scenario parameters

    int month = 12 ;		// december
    const double lat1 = 30.0 ;  // entire Mediterranean Sea
    const double lat2 = 46.0 ;
    const double lng1 = -8.0 ;
    const double lng2 = 37.0 ;
    wposition::compute_earth_radius( (lat1+lat2)/2.0 ) ;

    wposition1 pos( 36.0, 16.0, -10.0 ) ;
    seq_rayfan de( -90.0, 90.0, 181 ) ;
    seq_linear az( 0.0, 15.0, 360.0 ) ;
    const double time_max = 60.0 ;
    const double time_step = 0.100 ;

    seq_log freq( 3000.0, 1.0, 1 ) ;

    #ifdef USML_DEBUG
        const char* csvname = USML_STUDIES_DIR "/ray_speed/ray_speed_eigenray.csv";
        const char* ncname = USML_STUDIES_DIR "/ray_speed/ray_speed_eigenray.nc";
        const char* ncname_wave = USML_STUDIES_DIR "/ray_speed/ray_speed_eigenray_wave.nc";
    #endif

    // build sound velocity profile from World Ocean Atlas data

    cout << "load temperature & salinity data from World Ocean Atlas" << endl ;
    netcdf_woa* temperature = new netcdf_woa(
	USML_DATA_DIR "/woa09/temperature_seasonal_1deg.nc",
	USML_DATA_DIR "/woa09/temperature_monthly_1deg.nc",
        month, lat1, lat2, lng1, lng2 ) ;
    netcdf_woa* salinity = new netcdf_woa(
	USML_DATA_DIR "/woa09/salinity_seasonal_1deg.nc",
	USML_DATA_DIR "/woa09/salinity_monthly_1deg.nc",
        month, lat1, lat2, lng1, lng2 ) ;

    data_grid<double,3>* ssp = data_grid_mackenzie::construct( temperature, salinity ) ;
    data_grid_svp* fast_ssp = new data_grid_svp(ssp) ;
    profile_model* profile = new profile_grid_fast( fast_ssp ) ;
//    attenuation_model* attn = new attenuation_constant(0.0);
//    profile->attenuation(attn);

    // load bathymetry from ETOPO1 database

    data_grid<double,2>* grid = new netcdf_bathy( USML_DATA_DIR "/bathymetry/ETOPO1_Ice_g_gmt4.grd",
        lat1, lat2, lng1, lng2 );
    data_grid_bathy* fast_grid = new data_grid_bathy(grid) ;
    cout << "load bathymetry from ETOPO1 database" << endl ;
    boundary_model* bottom = new boundary_grid_fast( fast_grid ) ;

    // combine sound speed and bathymetry into ocean model

    boundary_model* surface = new boundary_flat() ;
    ocean_model ocean( surface, bottom, profile ) ;

    // initialize eigenray_collection targets and wavefront

    cout << "initialize " << num_targets << " targets" << endl ;
    randgen::seed(0) ;  // fix the initial seed
    wposition target( num_targets, 1, pos.latitude(), pos.longitude(), pos.altitude() ) ;
    for ( size_t n=0 ; n < target.size1() ; ++n ) {
        target.latitude(  n, 0, pos.latitude() + randgen::uniform() - 0.5 ) ;
        target.longitude( n, 0, pos.longitude() + randgen::uniform() - 0.5 ) ;
    }

    eigenray_collection loss(freq, pos, de, az, time_step, &target);
	wave_queue wave( ocean, freq, pos, de, az, time_step, &target ) ;
	wave.add_eigenray_listener(&loss) ;

    // propagate wavefront

    #ifdef USML_DEBUG
        cout << "writing wavefronts to " << ncname_wave << endl;

        wave.init_netcdf( ncname_wave );
        wave.save_netcdf();
    #endif

    cout << "propagate wavefronts for " << time_max << " secs" << endl ;
    {
        boost::progress_timer timer ;
        while ( wave.time() < time_max ) {
            wave.step() ;
		#ifdef USML_DEBUG
			wave.save_netcdf();
		#endif
		}
    }

    #ifdef USML_DEBUG
        wave.close_netcdf();

        // compute coherent propagation loss and write eigenrays to disk

        loss.sum_eigenrays();
        cout << "writing eigenray_collection to " << ncname << endl;
        loss.write_netcdf(ncname,"WQ3Deigenray test");

        // save results to spreadsheet and compare to analytic results

        cout << "writing tables to " << csvname << endl;
        std::ofstream os(csvname);
        os << "time,intensity,phase,s_de,s_az,t_de,t_az,srf,btm,cst"
        << endl;
        os << std::setprecision(18);
        cout << std::setprecision(18);

        const eigenray_list *raylist = loss.eigenrays(0,0);
        int n=0;
        for ( eigenray_list::const_iterator iter = raylist->begin();
                iter != raylist->end(); ++n, ++iter )
        {
            const eigenray &ray = *iter ;
            os << ray.time
               << "," << ray.intensity(0)
               << "," << ray.phase(0)
               << "," << ray.source_de
               << "," << ray.source_az
               << "," << ray.target_de
               << "," << ray.target_az
               << "," << ray.surface
               << "," << ray.bottom
               << "," << ray.caustic
               << endl;
        }
    #endif
}


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

#include <boost/dynamic_bitset.hpp>

#include <cstddef>
#include <type_traits>
#include <vector>
//===----------------------------------------------------------------------===//
namespace dtl {
//===----------------------------------------------------------------------===//
/// CONCISE -- a variant of WAH that improves compression by dealing with dirty
/// bits. These dirty bits are piggybacked by the fill words.
///
/// The implementation is based on the paper "CONCISE: Compressed 'n' Composable
/// Integer Set" of Colantonio et al.
///
/// Note: The implementation is NOT yet optimized for performance.
class concise {

protected:
  using word_type = $u32;
  static constexpr std::size_t word_bitlength = sizeof(word_type) * 8;
  /// The number of bits required to store the position of a dirty bit.
  static constexpr std::size_t dirty_position_bit_cnt = 5; // log2(32)
  /// The maximum repetition value that can be stored in a fill word.
  static constexpr std::size_t max_fill_repetitions =
      (1ull << (word_bitlength - 2 - dirty_position_bit_cnt)) - 1;
  /// The number of bits that can be stored in a literal word.
  static constexpr std::size_t payload_bit_cnt = word_bitlength - 1;
  static constexpr word_type all_ones = word_type(~word_type(0));

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

  std::size_t remaining_bit_cnt_in_last_literal_word_ = 0;

  //===--------------------------------------------------------------------===//
  // Helper functions to work with the code words.
  //===--------------------------------------------------------------------===//
  /// Returns true if the given word is a literal word.
  static inline constexpr u1
  is_literal_word(const word_type w) noexcept {
    return (w & word_type(1)) == 0;
  }

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

  /// Extract the payload from a literal word.
  static inline constexpr word_type
  extract_payload(const word_type w) noexcept {
    assert(is_literal_word(w));
    return w >> 1;
  }

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

  /// Extract the fill length from a fill word. The actual length is:
  /// (returned value + 1) * (word_bitlength - 1)
  static inline constexpr std::size_t
  extract_fill_repetitions(const word_type w) noexcept {
    assert(is_fill_word(w));
    return w >> (2 + dirty_position_bit_cnt);
  }

  /// Returns true if the given fill word piggybacks a dirty bit position.
  static inline constexpr u1
  has_dirty_bit_position(const word_type w) noexcept {
    assert(is_fill_word(w));
    const word_type mask = (1u << dirty_position_bit_cnt) - 1;
    return ((w >> 2) & mask) != 0;
  }

  /// Extract the position of the dirty bit from a fill word.
  static inline constexpr std::size_t
  extract_dirty_bit_position(const word_type w) noexcept {
    assert(is_fill_word(w));
    const word_type mask = (1u << dirty_position_bit_cnt) - 1;
    const auto raw_val = (w >> 2) & mask;
    assert(raw_val != 0);
    return raw_val - 1;
  }

  /// Increments the counter in the given fill word by the given value.
  static inline constexpr word_type
  increment_fill_repetitions(const word_type w, const std::size_t i) noexcept {
    assert(is_fill_word(w));
    const auto val = extract_fill_repetitions(w) + i;
    assert(val <= max_fill_repetitions);
    const word_type mask = (1u << (2 + dirty_position_bit_cnt)) - 1;
    return (w & mask) | (val << (2 + dirty_position_bit_cnt));
  }

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

  /// Creates a fill word that contains a dirty bit.
  static inline constexpr word_type
  make_fill_word(u1 val, const std::size_t len,
      const std::size_t dirty_bit_pos) noexcept {
    assert(len <= max_fill_repetitions);
    word_type w = val ? word_type(3) : word_type(1);
    w |= (dirty_bit_pos + 1) << 2;
    w |= len << (2 + dirty_position_bit_cnt);
    return w;
  }

  /// Returns a literal word where all bits are 0.
  static inline constexpr word_type
  make_empty_literal_word() noexcept {
    return word_type(0);
  }

  /// Returns a literal word where all bits are set to 1.
  static inline constexpr word_type
  make_all_ones_literal_word() noexcept {
    return ~word_type(0) << 1;
  }

  /// Tests a bit within a literal word.
  static inline constexpr u1
  test_bit_in_literal_word(const word_type w, const std::size_t i) noexcept {
    assert(i <= payload_bit_cnt);
    return ((w >> (1 + i)) & 1) != 0;
  }

  /// Sets a bit within a literal word.
  static inline constexpr word_type
  set_bit_in_literal_word(const word_type w, const std::size_t i, const u1 val)
      noexcept {
    assert(i <= payload_bit_cnt);
    const u1 cur_val = test_bit_in_literal_word(w, i);
    if (cur_val == val) {
      return w;
    }
    // Toggle the bit.
    return w ^ (word_type(1u << (i + 1)));
  }

  /// Returns true, iff the payload of the given literal word consists of a
  /// single 1-bit.
  static inline constexpr u1
  contains_single_bit(const word_type w) {
    assert(is_literal_word(w));
    return dtl::bits::pop_count(w) == 1;
  }
  //===--------------------------------------------------------------------===//

  /// Try to merge the last two words.
  inline void
  compress() {
    if (data_.size() < 2) {
      return;
    }

    const word_type w_last = data_.back();
    const word_type w_prev = data_[data_.size() - 2];

    assert(is_literal_word(w_last));

    u1 last_word_is_all_zeros = (w_last == make_empty_literal_word());
    u1 last_word_is_all_ones = (w_last == make_all_ones_literal_word());

    if (!last_word_is_all_zeros && !last_word_is_all_ones) {
      return;
    }

    u1 prev_word_is_zero_fill =
        is_fill_word(w_prev) && (extract_fill_value(w_prev) == false);
    u1 prev_word_is_one_fill =
        is_fill_word(w_prev) && (extract_fill_value(w_prev) == true);

    if ((last_word_is_all_zeros && prev_word_is_zero_fill)
        || (last_word_is_all_ones && prev_word_is_one_fill)) {
      data_.pop_back();
      data_.back() = increment_fill_repetitions(data_.back(), 1);
      return;
    }

    if ((last_word_is_all_zeros && prev_word_is_one_fill)
        && (last_word_is_all_ones && prev_word_is_zero_fill)) {
      return;
    }

    assert(is_literal_word(w_prev));

    word_type payload_prev = extract_payload(w_prev);
    if (last_word_is_all_ones) {
      payload_prev = (~payload_prev) & ((1u << payload_bit_cnt) - 1);
    }
    if (payload_prev == 0 || dtl::bits::pop_count(payload_prev) == 1) {
      // Merge the last two literal words into a fill word.
      data_.pop_back();
      u1 fill_value = last_word_is_all_ones;
      if (dtl::bits::pop_count(payload_prev) == 1) {
        const auto dirty_bit_pos = dtl::bits::tz_count(payload_prev);
        data_.back() = make_fill_word(fill_value, 1, dirty_bit_pos);
      }
      else {
        data_.back() = make_fill_word(fill_value, 1);
      }
    }
  }

  /// Append a 1-bit to the encoded bitmap at the given position.
  inline void
  append(const std::size_t i) {
    assert(i >= max_);
    if (data_.empty()) {
      const auto zero_block_cnt = i / payload_bit_cnt;
      if (zero_block_cnt == 1) {
        // Append an all-0 literal word.
        data_.push_back(make_empty_literal_word());
      }
      else if (zero_block_cnt > 1) {
        // Append a 0-fill.
        data_.push_back(make_fill_word(0, zero_block_cnt - 1));
      }
      // Append the literal word that contains the (very first) 1-bit.
      auto lit = make_empty_literal_word();
      const auto bit_pos = i % payload_bit_cnt;
      lit = set_bit_in_literal_word(lit, bit_pos, 1);
      data_.push_back(lit);
      max_ = i;
      return;
    }

    // The bit position to set, relative to the current block.
    auto bit_pos = (max_ % payload_bit_cnt) + i - max_;

    // Add 0-bits between the last set bit and the target bit that is to be
    // appended, if necessary.
    if (bit_pos >= payload_bit_cnt) {
      // The target bit is beyond the current block.
      const auto zero_block_cnt = (bit_pos / payload_bit_cnt) - 1;
      if (zero_block_cnt > 0) {
        // Append a 0-fill.
        if (contains_single_bit(data_.back())) {
          // Convert the last word into a fill word that piggybacks that bit.
          const auto dirty_bit_pos = dtl::bits::tz_count(
              extract_payload(data_.back()));
          data_.back() = make_fill_word(0, zero_block_cnt, dirty_bit_pos);
        }
        else {
          if (zero_block_cnt == 1) {
            data_.push_back(make_empty_literal_word()); // all-0 literal
          }
          else {
            data_.push_back(make_fill_word(0, zero_block_cnt - 1)); // 0-fill
          };
        }
      }

      // Append an all-0 literal word (the active word).
      data_.push_back(make_empty_literal_word());

      // Adjust the relative position of the bit to append.
      bit_pos = bit_pos % payload_bit_cnt;
    }

    assert(is_literal_word(data_.back()));
    data_.back() = set_bit_in_literal_word(data_.back(), bit_pos, 1);
    max_ = i;

    compress();
  }

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

  /// Append a 1-run of the given length to the compressed bitmap.
  inline void
  append_one_run(const std::size_t len) {
    for (std::size_t i = 0; i < len; ++i) {
      append(encoded_bitmap_length_ + i);
    }
    encoded_bitmap_length_ += len;
  }

public:
  concise() = default;

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

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

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

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

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

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

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

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

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

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

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

    void __forceinline__
    increment() {
      const auto word_cnt = outer_.data_.size();
      word_type w;
      while (word_idx_ < word_cnt) {
        w = outer_.data_[word_idx_];
        if (is_fill_word(w)) {
          if (!has_dirty_bit_position(w)) {
            if (extract_fill_value(w) == false) {
              pos_ += (extract_fill_repetitions(w) + 1) * payload_bit_cnt;
            }
            else {
              length_ = (extract_fill_repetitions(w) + 1) * payload_bit_cnt;
              in_word_idx_ = 0;
              ++word_idx_;
              break;
            }
          }
          else {
            // Fill with dirty bit.
            const auto dirty_bit_pos = extract_dirty_bit_position(w);
            const auto dirty_bit_val = !extract_fill_value(w);

            if (dirty_bit_val) { // 000001000000
              if (in_word_idx_ == 0) {
                // Skip forward to the dirty bit and produce a 1-run of length 1.
                pos_ += dirty_bit_pos;
                length_ = 1;
                in_word_idx_ = 2; // refers to everything past the dirty bit
                break;
              }
              else {
                assert(in_word_idx_ == 2);
                pos_ += (extract_fill_repetitions(w) + 1) * payload_bit_cnt
                    - (dirty_bit_pos + 1);
                in_word_idx_ = 0;
              }
            }
            else {
              if (dirty_bit_pos == 0) { // Handle special case 011111111
                pos_ += 1; // Skip over the dirty 0-bit.
                length_ = (extract_fill_repetitions(w) + 1) * payload_bit_cnt - 1;
                ++word_idx_;
                in_word_idx_ = 0;
                break;
              }
              else { // 11011111
                if (in_word_idx_ == 0) {
                  length_ += dirty_bit_pos;
                  in_word_idx_ = 1;
                  break;
                }
                else if (in_word_idx_ == 1) {
                  pos_ += 1; // Skip over the dirty 0-bit.
                  length_ = (extract_fill_repetitions(w) + 1) * payload_bit_cnt
                      - (dirty_bit_pos + 1);
                  ++word_idx_;
                  in_word_idx_ = 0;
                  break;
                }
              }
            }
          }
        }
        else { // Literal
          if (in_word_idx_ < payload_bit_cnt) { // TODO decode the entire literal word at once.
            const word_type payload = extract_payload(w) >> in_word_idx_;
            if (payload == 0) {
              pos_ += payload_bit_cnt - in_word_idx_;
            }
            else {
              const std::size_t b = dtl::bits::tz_count(payload);
              std::size_t e = b + 1;
              for (; e < payload_bit_cnt; ++e) {
                u1 is_set = dtl::bits::bit_test(payload, e);
                if (!is_set) break;
              }
              pos_ += b;
              length_ = e - b;
              in_word_idx_ += e;
              break;
            }
          }
        }
        in_word_idx_ = 0;
        ++word_idx_;
      }
    }

  public:
    explicit __forceinline__
    iter(const concise& outer)
        : outer_(outer),
          word_idx_(0),
          in_word_idx_(0),
          pos_(0), length_(0) {
      increment();
    }

    /// Forward the iterator to the next 1-run.
    void __forceinline__
    next() {
      pos_ += length_;
      length_ = 0;
      increment();
    }

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

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

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

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

  using skip_iter_type = iter;
  using scan_iter_type = iter;

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

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

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

  // For debugging purposes.
  void
  print(std::ostream& os) const {
    os << "\nword idx | word type | content" << std::endl;
    for (std::size_t i = 0; i < data_.size(); ++i) {
      os << std::setw(8) << i << " | ";
      auto& w = data_[i];
      if (is_fill_word(w)) {
        os << "fill      | ";
        os << "'" << std::bitset<word_bitlength>(w);
        os << "' => ";
        os << extract_fill_repetitions(w) << " x ";
        os << (extract_fill_value(w) ? "'1'" : "'0'");
        if (has_dirty_bit_position(w)) {
          os << " (dirty bit at " << extract_dirty_bit_position(w) << ")";
        }
        os << std::endl;
      }
      else {
        os << "literal   | ";
        os << "'" << std::bitset<word_bitlength>(w);
        os << "'";
        os << std::endl;
      }
    }
  }
};
//===----------------------------------------------------------------------===//
} // namespace dtl


//
// Created by Hamza El-Kebir on 2/26/22.
//

#ifndef LSEXAMPLE_PANDAARMVELBLOCK_HPP
#define LSEXAMPLE_PANDAARMVELBLOCK_HPP

#include <Lodestar/blocks/Block.hpp>
#include <Eigen/Dense>
#include <string>
#include <array>

#include "arm/Panda.hpp"
#include <thread>

namespace ls {
    namespace blocks {
        class PandaArmVelBlock
                : public Block<
                        ::std::tuple<Eigen::Matrix<double, 3, 1>>,
                        ::std::tuple<PandaState>,
                        ::std::tuple<::std::string>
                > {
        public:
            using Base =
            Block<
                    ::std::tuple<Eigen::Matrix<double, 3, 1>>,
                    ::std::tuple<PandaState>,
                    ::std::tuple<::std::string>
            >;

            using Matrix = Eigen::Matrix<double, 3, 1>;

            PandaArmVelBlock()
            {
                this->template i<0>().object.setZero();
                bindFunction();
            }

            ~PandaArmVelBlock()
            {
                panda_.stop();
            }

            typename ::std::tuple_element<0, typename Base::Params>::type &
            ipAddress()
            {
                return this->template p<0>();
            }

            const typename ::std::tuple_element<0, typename Base::Params>::type &
            ipAddress() const
            {
                return this->template p<0>();
            }

        protected:
            Panda panda_;

            void bindFunction()
            {
                this->equation = ::std::bind(
                        &PandaArmVelBlock::triggerFunction, this,
                        ::std::placeholders::_1);
            }

            void triggerFunction(Base &b)
            {
                static bool init = false;
                static bool ready = false;

                if (!init) {
                    panda_ = Panda{ipAddress(), false};

                    init = panda_.initConnection().ok();

                    if (!init)
                        return;

                    Eigen::AngleAxisd aa{M_PI / 2, Eigen::Vector3d{0, -1, 0}};
                    Eigen::AngleAxisd aa2{M_PI, Eigen::Vector3d{1, 0, 0}};
                    aa = aa * aa2;
                    Eigen::Vector3d translation = {200e-3, 0, 0};

                    panda_.setNominalToEndEffectorTransformation(aa, translation);

                    panda_.setGuidingMode();

                    ready = true;

                    ::std::thread([&] () {
                        panda_.robot_->control([&] (const franka::RobotState &rs, franka::Duration duration) -> franka::CartesianVelocities
                                               {
                                                   // TODO: Add mutex for thread safety

                                                   this->template o<0>().object.robot.parse(rs);
                                                   this->template o<0>().propagate();

                                                   while (!ready) {}

//                                                   auto velocity = this->template o<0>().object.robot.endEffectorPose_Base.block<3, 3>(0, 0) * this->template i<0>().object.block<3,1>(0,0);
//                                                   auto rotation = this->template o<0>().object.robot.endEffectorPose_Base.block<3, 3>(0, 0) * this->template i<0>().object.block<3,1>(3,0);
//
//                                                   ::std::array<double, 6> vel_d_array{velocity.x(),
//                                                                                velocity.y(),
//                                                                                velocity.z(),
//                                                                                rotation.x(),
//                                                                                rotation.y(),
//                                                                                rotation.z()};

                                                   ::std::array<double, 6> vel_d_array{this->template i<0>().object[0],
                                                                                this->template i<0>().object[1],
                                                                                this->template i<0>().object[2],
                                                                                0, 0, 0};
                                                   ready = false;
                                                   return {vel_d_array};
                                               }, franka::ControllerMode::kJointImpedance, true, franka::kMaxCutoffFrequency);
//                        panda_.control(f);
                    }).detach();
                }

                ready = true;

//                b.template o<0>() = panda_.getState().value();
            }
        };

        template<>
        class BlockTraits<PandaArmVelBlock> {
        public:
            static constexpr const BlockType blockType = BlockType::CustomBlock;
            enum {
                directFeedthrough = false
            };

            using type = PandaArmVelBlock;
            using Base = typename type::Base;

            enum {
                kIns = Base::kIns,
                kOuts = Base::kOuts,
                kPars = Base::kPars
            };

            static const ::std::array<::std::string, kIns> inTypes;
            static const ::std::array<::std::string, kOuts> outTypes;
            static const ::std::array<::std::string, kPars> parTypes;

            static const ::std::array<::std::string, 0> templateTypes;
        };
    }
}

#endif //LSEXAMPLE_PANDAARMVELBLOCK_HPP