#include <learning/independences/continuous/mutual_information.hpp>
#include <boost/math/special_functions/digamma.hpp>
#include <algorithm>

#include <iomanip>

namespace learning::independences::continuous {

double mi_pair(const DataFrame& df, int k) {
    KDTree kdtree(df);
    auto knn_results = kdtree.query(df, k + 1, std::numeric_limits<double>::infinity());

    VectorXd eps(df->num_rows());
    for (auto i = 0; i < df->num_rows(); ++i) {
        eps(i) = knn_results[i].first(k);
    }

    VectorXi nv1(df->num_rows());
    VectorXi nv2(df->num_rows());

    auto raw_values1 = df.data<arrow::FloatType>(0);
    auto raw_values2 = df.data<arrow::FloatType>(1);
    for (int i = 0, rows = static_cast<int>(df->num_rows()); i < rows; ++i) {
        auto eps_i = static_cast<int>(eps(i));

        auto v1 = static_cast<int>(raw_values1[i]);
        auto v2 = static_cast<int>(raw_values2[i]);

        nv1(i) = std::min(1 + v1, eps_i) + std::min(rows - v1, eps_i) - 1;
        nv2(i) = std::min(1 + v2, eps_i) + std::min(rows - v2, eps_i) - 1;
    }

    double res = 0;
    for (int i = 0; i < df->num_rows(); ++i) {
        res -= boost::math::digamma(nv1(i)) + boost::math::digamma(nv2(i));
    }

    res /= df->num_rows();
    res += boost::math::digamma(k) + boost::math::digamma(df->num_rows());

    return res;
}

double mi_triple(const DataFrame& df, int k) {
    KDTree kdtree(df);
    auto knn_results = kdtree.query(df, k + 1, std::numeric_limits<double>::infinity());

    VectorXd eps(df->num_rows());
    for (auto i = 0; i < df->num_rows(); ++i) {
        eps(i) = knn_results[i].first(k);
    }

    VectorXi n_xz = VectorXi::Zero(df->num_rows());
    VectorXi n_yz = VectorXi::Zero(df->num_rows());
    VectorXi n_z(df->num_rows());

    auto raw_x = df.data<arrow::FloatType>(0);
    auto raw_y = df.data<arrow::FloatType>(1);
    auto raw_z = df.data<arrow::FloatType>(2);

    IndexComparator comp_z(raw_z);
    std::vector<size_t> sort_z(df->num_rows());
    std::iota(sort_z.begin(), sort_z.end(), 0);
    std::sort(sort_z.begin(), sort_z.end(), comp_z);

    for (int i = 0, rows = static_cast<int>(df->num_rows()); i < rows; ++i) {
        auto eps_i = static_cast<int>(eps(i));
        auto x_i = static_cast<int>(raw_x[i]);
        auto y_i = static_cast<int>(raw_y[i]);
        auto z_i = static_cast<int>(raw_z[i]);

        n_z(i) = std::min(1 + z_i, eps_i) + std::min(rows - z_i, eps_i) - 1;

        if (z_i < eps_i) {
            for (int j = 0, end = z_i + eps_i; j < end; ++j) {
                auto index = sort_z[j];
                auto x_value = raw_x[index];
                auto y_value = raw_y[index];
                if (std::abs(x_i - x_value) < eps_i) ++n_xz(i);
                if (std::abs(y_i - y_value) < eps_i) ++n_yz(i);
            }
        } else if (z_i > (rows - eps_i)) {
            for (int j = z_i - eps_i + 1, end = df->num_rows(); j < end; ++j) {
                auto index = sort_z[j];
                auto x_value = raw_x[index];
                auto y_value = raw_y[index];
                if (std::abs(x_i - x_value) < eps_i) ++n_xz(i);
                if (std::abs(y_i - y_value) < eps_i) ++n_yz(i);
            }
        } else {
            for (int j = z_i - eps_i + 1, end = z_i + eps_i; j < end; ++j) {
                auto index = sort_z[j];
                auto x_value = raw_x[index];
                auto y_value = raw_y[index];
                if (std::abs(x_i - x_value) < eps_i) ++n_xz(i);
                if (std::abs(y_i - y_value) < eps_i) ++n_yz(i);
            }
        }
    }

    double res = 0;
    for (int i = 0; i < df->num_rows(); ++i) {
        res += boost::math::digamma(n_z(i)) - boost::math::digamma(n_xz(i)) - boost::math::digamma(n_yz(i));
    }

    res /= df->num_rows();
    res += boost::math::digamma(k);

    return res;
}

double mi_general(const DataFrame& df, int k) {
    KDTree kdtree(df);
    auto knn_results = kdtree.query(df, k + 1, std::numeric_limits<double>::infinity());

    VectorXd eps(df->num_rows());
    for (auto i = 0; i < df->num_rows(); ++i) {
        eps(i) = knn_results[i].first(k);
    }

    std::vector<size_t> indices(df->num_columns() - 2);
    std::iota(indices.begin(), indices.end(), 2);
    auto z_df = df.loc(indices);
    KDTree ztree(z_df);
    auto [n_xz, n_yz, n_z] = ztree.count_ball_subspaces(z_df, df.col(0), df.col(1), eps);

    double res = 0;
    for (int i = 0; i < df->num_rows(); ++i) {
        res += boost::math::digamma(n_z(i)) - boost::math::digamma(n_xz(i)) - boost::math::digamma(n_yz(i));
    }

    res /= df->num_rows();
    res += boost::math::digamma(k);

    return res;
}

double KMutualInformation::mi(const std::string& x, const std::string& y) const {
    auto subset_df = m_ranked_df.loc(x, y);
    return mi_pair(subset_df, m_k);
}

double KMutualInformation::mi(const std::string& x, const std::string& y, const std::string& z) const {
    auto subset_df = m_ranked_df.loc(x, y, z);
    return mi_triple(subset_df, m_k);
}

double KMutualInformation::mi(const std::string& x, const std::string& y, const std::vector<std::string>& z) const {
    auto subset_df = m_ranked_df.loc(x, y, z);
    return mi_general(subset_df, m_k);
}

double KMutualInformation::pvalue(const std::string& x, const std::string& y) const {
    auto value = mi(x, y);

    auto shuffled_df = m_ranked_df.loc(Copy(x), y);

    auto x_begin = shuffled_df.template mutable_data<arrow::FloatType>(0);
    auto x_end = x_begin + shuffled_df->num_rows();
    std::mt19937 rng{m_seed};

    int count_greater = 0;
    for (int i = 0; i < m_samples; ++i) {
        std::shuffle(x_begin, x_end, rng);
        auto shuffled_value = mi_pair(shuffled_df, m_k);

        if (shuffled_value >= value) ++count_greater;
    }

    return static_cast<double>(count_greater) / m_samples;
}

double KMutualInformation::pvalue(const std::string& x, const std::string& y, const std::string& z) const {
    auto original_mi = mi(x, y, z);
    auto z_df = m_df.loc(z);
    auto shuffled_df = m_ranked_df.loc(Copy(x), y, z);
    auto original_rank_x = m_ranked_df.template data<arrow::FloatType>(x);

    return shuffled_pvalue(original_mi, original_rank_x, z_df, shuffled_df, MITriple{});
}

double KMutualInformation::pvalue(const std::string& x, const std::string& y, const std::vector<std::string>& z) const {
    auto original_mi = mi(x, y, z);
    auto z_df = m_df.loc(z);
    auto shuffled_df = m_ranked_df.loc(Copy(x), y, z);
    auto original_rank_x = m_ranked_df.template data<arrow::FloatType>(x);

    return shuffled_pvalue(original_mi, original_rank_x, z_df, shuffled_df, MIGeneral{});
}

}  // namespace learning::independences::continuous


/*
 * Copyright (C) 2015 Hamza Merzić
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
*/
#include "model_parser.h"

#include <cmath>
#include <PQP/PQP.h>
#include <Eigen/Dense>
#include <fstream>
#include <memory>
#include <algorithm>

typedef Eigen::Vector3f EVector3f;

double PointDistanceToAxis(const EVector3f& point, const EVector3f& axis) {
  double l = point.dot(axis);
  return (point - axis * l).norm();
}

double PointDistanceToVector(const EVector3f& point, const EVector3f& vect) {
  double point_a_distance(point.squaredNorm());
  EVector3f d = point - vect;
  double point_b_distance(d.squaredNorm());
  EVector3f u = -vect;
  u.normalize();
  if (point_a_distance < point_b_distance) {
    d = point;
    d.normalize();
    double c = d.dot((vect).normalized());
    return (c > 0.0)
        ? sqrt(point_a_distance * (1.0 - c * c)) :
          sqrt(point_a_distance);
  } else {
    d.normalize();
    double c = d.dot((-vect).normalized());
    return (c > 0.0)
        ? sqrt(point_b_distance * (1.0 - c * c)) :
          sqrt(point_b_distance);
  }
}

PQP_Model* ModelParser::GetTransformModel(const std::string& model_file,
                                          const EMatrix& R,
                                          const EVector3f& T,
                                          const EVector3f& axis,
                                          double* axis_length,
                                          double* radius) {
  std::ifstream input_file (model_file.c_str(), std::ios::binary);
  try {
    std::unique_ptr<PQP_Model> model (new PQP_Model);

    char header[80] = "";         // Reads STL binary header
    input_file.read(header, 80);

    unsigned num_tris = 0;  // Reads number of triangles
    input_file.read(reinterpret_cast<char*>(&num_tris), sizeof(num_tris));

    EVector3f vertex[3];  // Triangle represented as three vertices
    float surf_vec[3];

    model->BeginModel();

    int16_t temp = 0;  // Used for taking two bits of data after a triangle
    uint64_t counter = 0;

    *axis_length = 0.0;
    *radius = 0.0;
    while (counter < num_tris) {
      input_file.read(reinterpret_cast<char*>(surf_vec), sizeof(surf_vec));

      for (unsigned i = 0; i < 3; ++i) {
        input_file.read(reinterpret_cast<char*>(&vertex[i]), sizeof(vertex[i]));
        vertex[i] = R * vertex[i] + T;
        *axis_length = std::max(*axis_length, double(vertex[i].dot(axis)));
        *radius = std::max(*radius,
            PointDistanceToVector(vertex[i], axis * *axis_length));
      }
      model->AddTri(vertex[0].data(), vertex[1].data(), vertex[2].data(),
        counter);

      input_file.read(reinterpret_cast<char*>(&temp), sizeof(temp));
      ++counter;
    }

    model->EndModel();
    return model.release();
  }
  catch(...) {
    throw "File " + model_file + " error!";
  }
}

// TODO(hamza): Try to get rid of code repetition
PQP_Model* ModelParser::GetModel(const std::string& model_file) {
  std::ifstream input_file (model_file.c_str(), std::ios::binary);
  try {
    std::unique_ptr<PQP_Model> model (new PQP_Model);

    char header[80] = "";         // Reads STL binary header
    input_file.read(header, 80);

    unsigned num_tris = 0;  // Reads number of triangles
    input_file.read(reinterpret_cast<char*>(&num_tris), sizeof(num_tris));

    float vertex[3][3];  // Triangle represented as three vertices
    float surf_vec[3];

    model->BeginModel();

    int16_t temp (0);  // Used for taking two bits of data after a triangle
    uint64_t counter = 0;
    while (counter < num_tris) {
      input_file.read(reinterpret_cast<char*>(surf_vec), sizeof(surf_vec));

      input_file.read(reinterpret_cast<char*>(&vertex[0]), sizeof(vertex[0]));
      input_file.read(reinterpret_cast<char*>(&vertex[1]), sizeof(vertex[1]));
      input_file.read(reinterpret_cast<char*>(&vertex[2]), sizeof(vertex[2]));
      model->AddTri(vertex[0], vertex[1], vertex[2], counter);

      input_file.read(reinterpret_cast<char*>(&temp), sizeof(temp));
      ++counter;
    }

    model->EndModel();
    return model.release();
  }
  catch(...) {
    throw "File " + model_file + " error!";
  }
}


// This file is part of the dune-mlmc project:
//   http://users.dune-project.org/projects/dune-mlmc
// Copyright Holders: Rene Milk
// License: BSD 2-Clause License (http://opensource.org/licenses/BSD-2-Clause)

#include <config.h>

#include "msfem.hh"

#include <dune/multiscale/common/main_init.hh>
#include <dune/multiscale/msfem/localsolution_proxy.hh>
#include <dune/multiscale/msfem/localproblems/localgridlist.hh>
#include <dune/multiscale/problems/base.hh>
#include <dune/multiscale/common/grid_creation.hh>
#include <dune/multiscale/msfem/msfem_solver.hh>
#include <dune/multiscale/msfem/fem_solver.hh>
#include <dune/multiscale/msfem/msfem_traits.hh>
#include <dune/multiscale/common/heterogenous.hh>

#if DUNE_MULTISCALE_WITH_DUNE_FEM
#include <dune/fem/mpimanager.hh>
#endif

#include <dune/xt/common/ranges.hh>
#include <dune/stuff/grid/walker/functors.hh>
#include <dune/stuff/grid/walker/apply-on.hh>
#include <dune/xt/common/timings.hh>
#include <dune/xt/common/logging.hh>
#include <dune/xt/common/configuration.hh>
#include <dune/xt/common/signals.hh>
#include <dune/xt/common/memory.hh>

#include <dune/gdt/products/h1.hh>
#include <dune/gdt/products/weightedl2.hh>
#include <dune/gdt/products/boundaryl2.hh>

#include <boost/filesystem.hpp>
#include <tbb/task_scheduler_init.h>

double surface_flow_gdt(const Dune::Multiscale::CommonTraits::GridType &grid,
                    const Dune::Multiscale::CommonTraits::ConstDiscreteFunctionType& solution,
                        const DMP::ProblemContainer& problem) {
  using namespace Dune::Multiscale;
  const auto gv = grid.leafGridView();

  // Constants and types
  constexpr auto dim = CommonTraits::world_dim;
  typedef double REAL; //TODO read from input
  typedef typename Dune::FieldVector<REAL,dim> FV;   // point on cell
  typedef typename Dune::FieldMatrix<REAL,dim,dim> FM;   // point on cell
  typedef typename Dune::FieldMatrix<REAL,1,dim> Grad;   // point on cell
  typedef typename Dune::QuadratureRule<REAL,dim-1> QR;
  typedef typename Dune::QuadratureRules<REAL,dim-1> QRS;

  const auto& diffusion = problem.getDiffusion();

  // Quadrature rule
  auto iCell = gv.template begin< 0,Dune::Interior_Partition >();
  auto iFace = gv.ibegin(*iCell);
  const QR& rule = QRS::rule(iFace->geometry().type(),2); // TODO order as para

  // Loop over cells
  REAL localFlux(0);
  for(iCell = gv.template begin< 0,Dune::Interior_Partition >();
      iCell != gv.template end< 0,Dune::Interior_Partition >(); ++iCell) {
    // Loop over interfaces
    const auto local_solution = solution.local_function(*iCell);
    for(iFace = gv.ibegin(*iCell); iFace != gv.iend(*iCell); ++iFace) {
      if(iFace->boundary() && abs(iFace->geometry().center()[0]) < 1e-10) {
        double area = iFace->geometry().volume();
        // Loop over gauss points
        for(auto iGauss = rule.begin(); iGauss != rule.end(); ++iGauss) {
          FV pos = iFace->geometry().global(iGauss->position());
          Grad grad;
          FM diff;
          diffusion.evaluate(pos, diff);
          local_solution->jacobian(pos, grad);
          localFlux -= iGauss->weight() * area * diff[0][0] * grad[0][0];
        }
      }
    }
  }
  localFlux = grid.comm().sum(localFlux);
  return localFlux;
}

void MultiLevelMonteCarlo::MsCgFemDifference::init(Dune::MPIHelper::MPICommunicator global, Dune::MPIHelper::MPICommunicator local) {
  // inits perm field only, no create()
  local_comm_ = local;
  if(init_called_)
    return;

  problem_ = Dune::XT::Common::make_unique<DMP::ProblemContainer>(global, local, DXTC_CONFIG);
  assert(problem_);
  init_called_ = true;
}

double MultiLevelMonteCarlo::MsCgFemDifference::compute_inflow_difference(const Dune::Multiscale::CommonTraits::GridType& coarse_grid,
                                                                          Dune::Multiscale::LocalsolutionProxy &msfem_solution,
                                                                          const std::shared_ptr<Dune::Multiscale::CommonTraits::GridType> fine_grid,
                                                                          const Dune::Multiscale::CommonTraits::ConstDiscreteFunctionType* fine_function) {
  using namespace Dune;
  typedef Multiscale::CommonTraits::SpaceChooserType::PartViewType
      PartViewType;
  const Multiscale::CommonTraits::SpaceType coarse_space(PartViewType::create(
                                                         coarse_grid, Multiscale::CommonTraits::st_gdt_grid_level));
  Multiscale::CommonTraits::DiscreteFunctionType projected_msfem_solution(
        coarse_space, "MsFEM_Solution");
  Multiscale::MsFEMProjection::project(
        msfem_solution, projected_msfem_solution);
  const auto coarse_flow = surface_flow_gdt(coarse_grid, projected_msfem_solution, *problem_);

  if(fine_function && fine_grid) {
    const auto fine_flow = surface_flow_gdt(*fine_grid, *fine_function, *problem_);

    //fine_function->visualize("fine_sol");
    //projected_msfem_solution.visualize("proj_msfem_sol");

    return fine_flow - coarse_flow;
  }

  return coarse_flow;
}

double MultiLevelMonteCarlo::MsCgFemDifference::eval() {
  using namespace Dune;
  Dune::XT::Common::OutputScopedTiming tm("mlmc.difference_cg-msfem", DXTC_LOG_INFO_0);
  assert(init_called_);
  assert(problem_);
  auto coarse_grid = Multiscale::make_coarse_grid(*problem_, local_comm_);
  // create() new perm field
  problem_->getMutableModelData().prepare_new_evaluation(*problem_);
  auto fine_grid = Multiscale::make_fine_grid(*problem_, coarse_grid, true,local_comm_);

  typedef Multiscale::CommonTraits::SpaceChooserType::PartViewType
      PartViewType;
  const Multiscale::CommonTraits::SpaceType coarse_space(PartViewType::create(
                                                           *coarse_grid, Multiscale::CommonTraits::st_gdt_grid_level));

  std::unique_ptr<Multiscale::LocalsolutionProxy> msfem_solution(nullptr);

  Multiscale::LocalGridList localgrid_list(*problem_, coarse_space);
  Dune::XT::Common::timings().start("mlmc.difference_cg-msfem.msfem-solve");
  Multiscale::Elliptic_MsFEM_Solver().apply(*problem_, coarse_space, msfem_solution,
                                            localgrid_list);
  Dune::XT::Common::timings().stop("mlmc.difference_cg-msfem.msfem-solve");

  Dune::XT::Common::timings().start("mlmc.difference_cg-msfem.cgfem-solve");
  Multiscale::Elliptic_FEM_Solver fem(*problem_, fine_grid);
  const auto &fine_fem_solution = fem.solve();
  Dune::XT::Common::timings().stop("mlmc.difference_cg-msfem.cgfem-solve");

  Dune::XT::Common::OutputScopedTiming tmd("mlmc.difference_cg-msfem.compute_inflow_difference", DXTC_LOG_INFO_0);
  return compute_inflow_difference(*coarse_grid, *msfem_solution,
                                   fine_grid, &fine_fem_solution);
}

double MultiLevelMonteCarlo::MsFemSingleDifference::eval() {
  using namespace Dune;
  Dune::XT::Common::OutputScopedTiming tm("mlmc.single_msfem", DXTC_LOG_INFO_0);
  assert(problem_);
//  assert(init_called_);
  auto coarse_grid = Multiscale::make_coarse_grid(*problem_, local_comm_);
  // create() new perm field
  problem_->getMutableModelData().prepare_new_evaluation(*problem_);
  typedef Multiscale::CommonTraits::SpaceChooserType::PartViewType
      PartViewType;
  const Multiscale::CommonTraits::SpaceType coarse_space(PartViewType::create(
                                                           *coarse_grid, Multiscale::CommonTraits::st_gdt_grid_level));
  std::unique_ptr<Multiscale::LocalsolutionProxy> msfem_solution(nullptr);

  Multiscale::LocalGridList localgrid_list(*problem_, coarse_space);
  Dune::XT::Common::timings().start("mlmc.single_msfem.msfem-solve");
  Multiscale::Elliptic_MsFEM_Solver().apply(*problem_, coarse_space, msfem_solution,
                                            localgrid_list);
  Dune::XT::Common::timings().stop("mlmc.single_msfem.msfem-solve");

  Dune::XT::Common::OutputScopedTiming tmd("mlmc.single_msfem.compute_inflow_difference", DXTC_LOG_INFO_0);
  return compute_inflow_difference(*coarse_grid, *msfem_solution);
}

//! workaround for https://github.com/wwu-numerik/dune-stuff/issues/42
void set_config_values(const std::vector<std::string> &keys,
                       const std::vector<std::string> &values) {
  assert(keys.size() == values.size());
  for (const auto i : Dune::XT::Common::value_range(keys.size()))
    DXTC_CONFIG.set(keys[i], values[i], true);

  // should just be
  // Dune::XT::Common::Config().add(Dune::XT::Common::Configuration(keys, values), "", true);
}

void handle_sigterm(int signal) {
  DXTC_TIMINGS.stop();
  DXTC_TIMINGS.output_per_rank("profiler");
  std::exit(signal);
}

void MultiLevelMonteCarlo::msfem_init(int argc, char **argv) {
  using namespace std;
#if DUNE_MULTISCALE_WITH_DUNE_FEM
  Dune::Fem::MPIManager::initialize(argc, argv);
#endif
  auto &helper = Dune::MPIHelper::instance(argc, argv);
  if (helper.size() > 1 &&
      !(Dune::Capabilities::isParallel<
        Dune::Multiscale::CommonTraits::GridType>::v)) {
    DUNE_THROW(Dune::InvalidStateException,
               "mpi enabled + serial grid = bad idea");
  }

  // config defaults defaults
  const vector<string> keys{"grids.macro_cells_per_dim",
                            "grids.micro_cells_per_macrocell_dim",
                            "msfem.oversampling_layers", "problem.name"};
  const vector<string> values{"8", "12", "1", "Random"};
  set_config_values(keys, values);

  //if (argc > 1 && boost::filesystem::is_regular_file(argv[1]))
  //  Dune::XT::Common::Config().read_command_line(argc, argv);

  if (argc > 1 ) {
      Dune::XT::Common::Config().read_command_line(argc, argv);
      Dune::Stuff::Common::Config().read_command_line(argc, argv);
  }
  Dune::XT::Common::test_create_directory(DXTC_CONFIG_GET("global.datadir", "data/"));

  // LOG_NONE = 1, LOG_ERROR = 2, LOG_INFO = 4,LOG_DEBUG = 8,LOG_CONSOLE =
  // 16,LOG_FILE = 32
  // --> LOG_ERROR | LOG_INFO | LOG_DEBUG | LOG_CONSOLE | LOG_FILE = 62
  Dune::XT::Common::Logger().create(DXTC_CONFIG_GET("logging.level", 62),
                       DXTC_CONFIG_GET("logging.file", std::string(argv[0]) + ".log"),
                       DXTC_CONFIG_GET("global.datadir", "data"),
                       DXTC_CONFIG_GET("logging.dir", "log" /*path below datadir*/));
  DXTC_TIMINGS.set_outputdir(DXTC_CONFIG_GET("global.datadir", "data"));
  Dune::XT::Common::install_signal_handler(SIGTERM, handle_sigterm);
}


#include "include/GeneralizedRfAlgo.h"
#include <boost/math/special_functions/factorials.hpp>
#include <queue>
// boost logging
#include <FastSplitList.h>
#include <SpiAlgo.h>
#include <boost/log/attributes/constant.hpp>
#include <boost/log/sources/record_ostream.hpp>
#include <future>

std::atomic_size_t GeneralizedRfAlgo::tree_idx = 0;

LogDblFact GeneralizedRfAlgo::factorials = LogDblFact();
GeneralizedRfAlgo::GeneralizedRfAlgo() : pairwise_split_scores(0) {
	logger.add_attribute("Tag", boost::log::attributes::constant<std::string>("generalized_RF"));
}

RfAlgorithmInterface::Scalar GeneralizedRfAlgo::h_info_content(size_t a, size_t b) {
	// no trivial splits allowed here (outer log would return infty, because no information present)
	assert(a >= 2);
	assert(b >= 2);
	// precompute (maybe lazy) all double fac results for a and b separatly
	return -factorials.lg_rooted_dbl_fact_fast(static_cast<long>(a)) -
	       factorials.lg_rooted_dbl_fact_fast(static_cast<long>(b)) +
	       factorials.lg_unrooted_dbl_fact_fast(static_cast<long>(a + b));
}

size_t GeneralizedRfAlgo::bits_too_many(size_t taxa) {
	constexpr size_t bit_amount_split = sizeof(pll_split_base_t) * 8;
	auto bits_too_many =
	    taxa % bit_amount_split == 0 ? 0 : bit_amount_split - (taxa % bit_amount_split);
	assert(bits_too_many < sizeof(pll_split_base_t) * 8);
	return bits_too_many;
}

void GeneralizedRfAlgo::calc_thread(GeneralizedRfAlgo &alg,
                                    const std::vector<FastSplitList> &trees,
                                    size_t pairwise_tree_cnt,
                                    SymmetricMatrix<Scalar> &sim) {
	while (true) {
		size_t index = tree_idx++;
		if (index >= pairwise_tree_cnt) {
			break;
		}
		// Calculates the corresponding row from the index using the quadratic formula
		auto row = static_cast<size_t>(std::sqrt(1 + 8 * index) / 2 - .5);
		size_t col = index - (row * row + row) / 2;
		auto score = alg.calc_tree_score(trees[row], trees[col]);
		sim.raw_set_at(index, score);
	}
}

RfMetricInterface::Results GeneralizedRfAlgo::calculate(std::vector<PllTree> &trees,
                                                        const RfMetricInterface::Params &params) {
	assert(trees.size() >= 2);
	// extract splits. Each tree now identifies by its index in all_splits
	std::vector<PllSplitList> all_splits;
	all_splits.reserve(trees.size());
	for (auto &t : trees) {
		t.alignNodeIndices(*trees.begin());
		all_splits.emplace_back(t);
	}
	PllSplit::split_len = all_splits.back().computeSplitLen();
	taxa = all_splits.back().size() + 3;

	BOOST_LOG_SEV(logger, lg::notification) << "Parsed trees. Starting calculations.";
	factorials.reserve(4 * taxa + 8);
	std::vector<FastSplitList> fast_trees = generateFastList(all_splits);
	assert(PllSplit::split_len != std::numeric_limits<size_t>::max());
	setup_temporary_storage(PllSplit::split_len);
	pairwise_split_scores = calcPairwiseSplitScores();

	BOOST_LOG_SEV(logger, lg::notification)
	    << "Calculated pairwise scores; Calculating pairwise tree scores.";
	RfMetricInterface::Results res(trees.size());

	size_t pairwise_tree_cnt = res.pairwise_similarities.get_num_entries();
	int num_threads = params.threads == -1 ? static_cast<int>(std::thread::hardware_concurrency())
	                                       : params.threads;
	std::vector<std::thread> pool;
	match_solver.init(all_splits.back().size());

	pool.reserve(num_threads);
    for (int i = 0; i < num_threads; ++i) {
		pool.emplace_back(calc_thread,
		                  std::ref(*this),
		                  std::ref(fast_trees),
		                  std::ref(pairwise_tree_cnt),
		                  std::ref(res.pairwise_similarities));
	}

	for (auto &thread : pool) {
		thread.join();
	}

	// calc mean distance between trees
	double total_dst = 0.;
	for (size_t idx_a = 0; idx_a < all_splits.size(); ++idx_a) {
		for (size_t idx_b = 0; idx_b <= idx_a; ++idx_b) {
			total_dst += res.pairwise_similarities.at(idx_a, idx_b);
		}
	}
	res.mean_distance = total_dst / static_cast<Scalar>(trees.size());

	// calculate distances
	calc_pairwise_tree_dist(fast_trees, res);

	factorials.printLog();
	return res;
}
RfAlgorithmInterface::Scalar GeneralizedRfAlgo::calc_tree_score(const SplitList &A,
                                                                const SplitList &B) {
	//	auto scores = calc_pairwise_split_scores(A, B);
	SplitScores scores(A.size());
	Scalar max_val = 0;
	for (size_t row = 0; row < A.size(); ++row) {
		size_t row_idx = A[row].getScoreIndex();
		for (size_t col = 0; col < A.size(); ++col) {
			size_t col_idx = B[col].getScoreIndex();
			auto val = pairwise_split_scores.checked_at(row_idx, col_idx);
			scores.scores.set(row, col, val);
			if (val > max_val) {
				max_val = val;
			}
		}
	}
	scores.max_score = max_val;
	auto total_score = match_solver.solve(scores);
	return total_score;
}

[[maybe_unused]] GeneralizedRfAlgo::SplitScores
GeneralizedRfAlgo::calc_pairwise_split_scores(const SplitList &S1, const SplitList &S2) {
	SplitScores scores(S1.size());
	factorials.reserve(taxa + taxa + 4);
	for (size_t row = 0; row < S1.size(); ++row) {
		for (size_t col = 0; col < S1.size(); ++col) {
			Scalar val;
			// when using fast Split list, the pointers to PllSplit define equality
			if (&S1[row] == &S2[col]) {
				assert(S1[row] == S2[col]);
				val = calc_split_score(S1[row]);
			} else {
				val = calc_split_score(S1[row], S2[col]);
			}

			if (scores.max_score < val) {
				scores.max_score = val;
			}
			scores.scores.set(row, col, val);
		}
	}

	return scores;
}

void GeneralizedRfAlgo::compute_split_comparison(const PllSplit &S1, const PllSplit &S2) {
	// B1 -> &split_buffer[0]
	S1.set_not(PllSplit::split_len, &temporary_split_content[0]);
	// B2 -> &split_buffer[split_len]
	S2.set_not(PllSplit::split_len, &temporary_split_content[PllSplit::split_len]);
	// A1 and A2 -> &split_buffer[2 * split_len]
	S1.intersect(S2, PllSplit::split_len, &temporary_split_content[2 * PllSplit::split_len]);
	// B1 and B2 -> &split_buffer[3 * split_len]
	temporary_splits[0].intersect(temporary_splits[1],
	                              PllSplit::split_len,
	                              &temporary_split_content[3 * PllSplit::split_len]);
	// A1 and B2 -> &split_buffer[4 * split_len]
	S1.intersect(temporary_splits[1],
	             PllSplit::split_len,
	             &temporary_split_content[4 * PllSplit::split_len]);
	// A2 and B1 -> &split_buffer[5 * split_len]
	S2.intersect(temporary_splits[0],
	             PllSplit::split_len,
	             &temporary_split_content[5 * PllSplit::split_len]);
}
RfAlgorithmInterface::Scalar GeneralizedRfAlgo::calc_tree_info_content(const SplitList &S) {
	Scalar sum = 0;
	for (size_t i = 0; i < S.size(); ++i) {
		sum += S[i].getHInfoContent();
	}
	return sum;
}
void GeneralizedRfAlgo::calc_pairwise_tree_dist(const std::vector<FastSplitList> &trees,
                                                RfMetricInterface::Results &res) {
	std::vector<GeneralizedRfAlgo::Scalar> tree_info(trees.size());
	for (size_t i = 0; i < trees.size(); ++i) {
		tree_info[i] = calc_tree_info_content(trees[i]);
	}

	GeneralizedRfAlgo::Scalar summed_dist = 0;
	for (size_t row = 0; row < trees.size(); ++row) {
		for (size_t col = 0; col <= row; ++col) {
			auto score = res.pairwise_similarities.at(row, col);
			auto max = tree_info[row] + tree_info[col];
			res.pairwise_distances.set_at(row, col, (max - score - score));
			summed_dist += max / score;
		}
	}

	res.mean_distance = summed_dist / (static_cast<Scalar>(trees.size() + 1) *
	                                   (static_cast<Scalar>(trees.size()) / 2.));
}
void GeneralizedRfAlgo::setup_temporary_storage(size_t split_len) {
	temporary_split_content.assign(split_len * 6, 0);
	temporary_splits.reserve(6);
	for (size_t i = 0; i < 6; ++i) {
		temporary_splits.emplace_back(&temporary_split_content[i * split_len]);
	}
}
GeneralizedRfAlgo::GeneralizedRfAlgo(size_t split_len) : pairwise_split_scores(0) {
	logger.add_attribute("Tag", boost::log::attributes::constant<std::string>("generalized_RF"));
	setup_temporary_storage(split_len);
}
std::vector<FastSplitList>
GeneralizedRfAlgo::generateFastList(const std::vector<PllSplitList> &slow_split_list) {
	// expect that all elements of slow_split_list[i] are already sorted
	// -> Perform k-way merge to only store non-duplicate PllSplits
	PllSplit::split_len = slow_split_list.front().computeSplitLen();
	BOOST_LOG_SEV(logger, lg::normal)
	    << "Start reducing PllSplit Size, current split_len: " << PllSplit::split_len;

	std::vector<FastSplitList> returnList(slow_split_list.size(),
	                                      FastSplitList(slow_split_list.front().size()));

	// data used inside k-way merge
	std::vector<size_t> currently_inPQ(slow_split_list.size(), 0);
	size_t current_split_offset = 0;
	size_t found_duplicates = 0;
	// PQ which stores a PllSplit (underlying data not touched) and the index from which tree it
	// originates from
	typedef std::pair<PllSplit, size_t> pq_type;
	std::priority_queue<pq_type, std::vector<pq_type>, std::greater<>> pq;
	// initialize by inserting first elements of all slow lists
	for (size_t i = 0; i < slow_split_list.size(); ++i) {
		pq.push(std::make_pair(PllSplit(slow_split_list[i][0]), i));
	}
	auto insert_in_pq = [&](size_t active_slow_idx) {
		// takes the current index
		auto &active_slow_list = slow_split_list[active_slow_idx];
		// increment where the active element is
		size_t list_idx = ++currently_inPQ[active_slow_idx];
		if (list_idx < active_slow_list.size()) {
			PllSplit insert_split(active_slow_list[list_idx]());
			pq.push(std::make_pair(insert_split, active_slow_idx));
		}
	};
	while (!pq.empty()) {
		// no further duplications store the next split in unique_pll_splits (either because of
		// start/or handled all duplicates), increment current_split_offset
		unique_pll_splits.emplace_back(pq.top().first);
		{
			// pre-calc
			unique_pll_splits.back().perform_popcount_precalc(PllSplit::split_len);
			const size_t a = unique_pll_splits.back().getPrecalcPopcnt();
			unique_pll_splits.back().setHInfoContent(h_info_content(a, taxa - a));
		}

		// get current state
		PllSplit curr_split = pq.top().first;
		{
			size_t curr_slow_list_idx = pq.top().second;
			// add current unique split to result
			assert(currently_inPQ[curr_slow_list_idx] < slow_split_list.back().size());
			returnList[curr_slow_list_idx].setOffsetAt(currently_inPQ[curr_slow_list_idx],
			                                           current_split_offset);
			// add from the same list a replacement element
			pq.pop();

			insert_in_pq(curr_slow_list_idx);
		}
		// check if further elements are equal
		while (!pq.empty() && curr_split == pq.top().first) {
			++found_duplicates;
			// duplicate found - use the same offset value
			size_t equal_in_tree_idx = pq.top().second;
			// the found value must have un-inserted elements
			assert(currently_inPQ[equal_in_tree_idx] < slow_split_list[equal_in_tree_idx].size());
			returnList[equal_in_tree_idx].setOffsetAt(currently_inPQ[equal_in_tree_idx],
			                                          current_split_offset);
			pq.pop();
			// insertion of replacement for equal_in_tree element
			insert_in_pq(equal_in_tree_idx);
		}
		++current_split_offset;
	}
	{
		size_t total_splits = slow_split_list.size() * slow_split_list.front().size();
		double duplicate_ratio =
		    static_cast<double>(found_duplicates) / static_cast<double>(total_splits);
		BOOST_LOG_SEV(logger, lg::notification)
		    << "Done construction of FastSplitList. " << found_duplicates << " duplicates of "
		    << total_splits << " possible PllSplits. Duplicate ratio: " << duplicate_ratio
		    << " Number of unique splits: " << unique_pll_splits.size();
	}
	// unique_pll_splits will no longer reallocate -> write base-ptr to Static variable
	FastSplitList::setBasePtr(&unique_pll_splits[0]);
	return returnList;
}

SymmetricMatrix<GeneralizedRfAlgo::Scalar> GeneralizedRfAlgo::calcPairwiseSplitScores() {
	assert(!unique_pll_splits.empty());
	factorials.reserve(taxa + taxa + 4);

	size_t split_num = unique_pll_splits.size();
	SymmetricMatrix<Scalar> resMtx(split_num);
	for (size_t row = 0; row < split_num; ++row) {
		auto &rSplit = unique_pll_splits[row];
		// let the split know, which index it is. -> should happen once per PllSplit
		rSplit.setIntersectionIdx(row);
		for (size_t col = 0; col < row; ++col) {
			auto &cSplit = unique_pll_splits[col];
			resMtx.set_at(row, col, calc_split_score(rSplit, cSplit));
		}
		resMtx.set_at(row, row, calc_split_score(rSplit));
	}
	return resMtx;
}


/*****************************************************************************/
/*  Copyright (c) 2016, Alessandro Pieropan                                  */
/*  All rights reserved.                                                     */
/*                                                                           */
/*  Redistribution and use in source and binary forms, with or without       */
/*  modification, are permitted provided that the following conditions       */
/*  are met:                                                                 */
/*                                                                           */
/*  1. Redistributions of source code must retain the above copyright        */
/*  notice, this list of conditions and the following disclaimer.            */
/*                                                                           */
/*  2. Redistributions in binary form must reproduce the above copyright     */
/*  notice, this list of conditions and the following disclaimer in the      */
/*  documentation and/or other materials provided with the distribution.     */
/*                                                                           */
/*  3. Neither the name of the copyright holder nor the names of its         */
/*  contributors may be used to endorse or promote products derived from     */
/*  this software without specific prior written permission.                 */
/*                                                                           */
/*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS      */
/*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT        */
/*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR    */
/*  A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT     */
/*  HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,   */
/*  SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT         */
/*  LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,    */
/*  DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY    */
/*  THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT      */
/*  (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE    */
/*  OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.     */
/*****************************************************************************/

#include "opencv2/video/tracking.hpp"
#include "opencv2/highgui/highgui.hpp"
#include <iostream>
#include <fstream>
#include <iomanip>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <random>
#include <cmath>

using namespace std;
using namespace cv;
using namespace Eigen;

double deg2rad(double deg) {
    return deg * M_PI / 180.0;
}

template <typename T>
string toString(Mat& mat) {
  stringstream ss;
  ss << fixed << setprecision(3);
  for (auto i = 0; i < mat.rows; ++i) {
    for (auto j = 0; j < mat.cols; ++j) {
      ss << mat.at<T>(i, j) << " ";
    }
    ss << "\n";
  }

  return ss.str();
}

void printKalman(KalmanFilter& kf) {
  ofstream file("/home/alessandro/debug/pkf.txt");

  Mat prediction = kf.predict();

  file << "transition matrix: " << toString<float>(kf.transitionMatrix) << "\n"
       << "post " << toString<float>(kf.statePost) << "\n"
       << "predict " << toString<float>(prediction) << endl;

  Mat_<float> measurement(6, 1);
  measurement(0) = 0.01;
  measurement(1) = 0;
  measurement(2) = 0;
  measurement(3) = 0;
  measurement(4) = 0;
  measurement(5) = 0;

  Mat estimated = kf.correct(measurement);

  file << "measurements: " << toString<float>(measurement) << "\n"
       << "post " << toString<float>(kf.statePost) << "\n"
       << "estimated " << toString<float>(estimated) << endl;

  file.close();

}

KalmanFilter initKalmanPose() {
  KalmanFilter kalman_pose_pnp_(18, 6);

  Mat A = Mat::eye(18, 18, CV_32FC1);

  double dt = 1;

  for (auto i = 0; i < 9; ++i) {
    auto id_vel = i + 3;
    auto id_acc = i + 6;
    auto id_vel2 = i + 12;
    auto id_acc2 = i + 15;

    if (id_vel < 9) A.at<float>(i, id_vel) = dt;
    if (id_acc < 9) A.at<float>(i, id_acc) = 0.5 * dt * dt;
    if (id_vel2 < 18) A.at<float>(i + 9, id_vel2) = dt;
    if (id_acc2 < 18) A.at<float>(i + 9, id_acc2) = 0.5 * dt * dt;
  }

  kalman_pose_pnp_.transitionMatrix = A.clone();

  kalman_pose_pnp_.measurementMatrix.at<float>(0, 0) = 1;
  kalman_pose_pnp_.measurementMatrix.at<float>(1, 1) = 1;
  kalman_pose_pnp_.measurementMatrix.at<float>(2, 2) = 1;
  kalman_pose_pnp_.measurementMatrix.at<float>(3, 9) = 1;
  kalman_pose_pnp_.measurementMatrix.at<float>(4, 10) = 1;
  kalman_pose_pnp_.measurementMatrix.at<float>(5, 11) = 1;

  setIdentity(kalman_pose_pnp_.processNoiseCov, Scalar::all(1e-1));
  setIdentity(kalman_pose_pnp_.measurementNoiseCov, Scalar::all(1e-2));
  setIdentity(kalman_pose_pnp_.errorCovPost, Scalar::all(.1));

  kalman_pose_pnp_.measurementNoiseCov.at<float>(0, 0) = 5;
  kalman_pose_pnp_.measurementNoiseCov.at<float>(1, 1) = 1;
  kalman_pose_pnp_.measurementNoiseCov.at<float>(2, 2) = 1;
  kalman_pose_pnp_.measurementNoiseCov.at<float>(3, 3) = 1;
  kalman_pose_pnp_.measurementNoiseCov.at<float>(4, 4) = 1;
  kalman_pose_pnp_.measurementNoiseCov.at<float>(5, 5) = 1;

  kalman_pose_pnp_.statePre.at<float>(0) = 0;
  kalman_pose_pnp_.statePre.at<float>(1) = 0;
  kalman_pose_pnp_.statePre.at<float>(2) = 0;
  kalman_pose_pnp_.statePre.at<float>(3) = 0;
  kalman_pose_pnp_.statePre.at<float>(4) = 0;
  kalman_pose_pnp_.statePre.at<float>(5) = 0;
  kalman_pose_pnp_.statePre.at<float>(6) = 0;
  kalman_pose_pnp_.statePre.at<float>(7) = 0;
  kalman_pose_pnp_.statePre.at<float>(8) = 0;
  kalman_pose_pnp_.statePre.at<float>(9) = 0;
  kalman_pose_pnp_.statePre.at<float>(10) = 0;
  kalman_pose_pnp_.statePre.at<float>(11) = 0;
  kalman_pose_pnp_.statePre.at<float>(12) = 0;
  kalman_pose_pnp_.statePre.at<float>(13) = 0;
  kalman_pose_pnp_.statePre.at<float>(14) = 0;
  kalman_pose_pnp_.statePre.at<float>(15) = 0;
  kalman_pose_pnp_.statePre.at<float>(16) = 0;
  kalman_pose_pnp_.statePre.at<float>(17) = 0;

  return kalman_pose_pnp_;
}

void testKalmanPose()
{

    ofstream file("/home/alessandro/debug/pkf.txt");
    random_device rd;
    default_random_engine dre(rd());

    std::uniform_real_distribution<float> dist(0, 1);

    float tx = 1;
    float tx_acc =0;
    float rx_acc = 0;
    float rx = deg2rad(2);

    uniform_real_distribution<float> deg_dist(0, rx/4);

    KalmanFilter kfp = initKalmanPose();

    file << toString<float>(kfp.measurementNoiseCov) << "\n";

    for(auto i = 0; i < 5; ++i)
    {
        float val = dist(dre);

        tx_acc += val + tx;
        rx_acc += rx + deg_dist(dre);

        Mat_<float> measurement(6, 1);
        measurement(0) = tx_acc;
        measurement(1) = 0;
        measurement(2) = 0;
        measurement(3) = 0;
        measurement(4) = 0;
        measurement(5) = rx_acc;

        Mat predictions = kfp.predict();
        Mat corrections = kfp.correct(measurement);
        Mat pred_t, mea_t, corr_t;
        transpose(predictions, pred_t);
        transpose(corrections, corr_t);
        transpose(measurement, mea_t);

        file << "predictions \n" << toString<float>(pred_t);
        file << "measurements \n" << toString<float>(mea_t);
        file << "corrections \n" << toString<float>(corr_t) << "\n";
    }

    file.close();

}

void test_eigen() {
  Matrix3f m;

  m(0,0) = 1;
  m(0,1) = -0.001;
  m(0,2) = 0.006;

  m(1,0) = 0.001;
  m(1,1) = 1;
  m(1,2) = -0.001;

  m(2,0) = -0.006;
  m(2,1) = 0.001;
  m(2,2) = 1;

  cout << "original rotation:" << endl;
  cout << m << endl << endl;

  Vector3f ea = m.eulerAngles(2, 1, 0);
  cout << "to Euler angles:" << endl;
  cout << ea << endl << endl;

  Matrix3f n;
  n = AngleAxisf(ea[0], Vector3f::UnitZ()) *
      AngleAxisf(ea[1], Vector3f::UnitY()) *
      AngleAxisf(ea[2], Vector3f::UnitX());
  cout << "recalc original rotation:" << endl;
  cout << n << endl;
}

float mouse_x, mouse_y;

void CallBackFunc(int event, int x, int y, int flags, void* userdata) {
  if (event == EVENT_LBUTTONDOWN) {
    //cout << "Left button of the mouse is clicked - position (" << x << ", " << y
    //     << ")" << endl;
  } else if (event == EVENT_RBUTTONDOWN) {
    //cout << "Right button of the mouse is clicked - position (" << x << ", "
    //     << y << ")" << endl;
  } else if (event == EVENT_MBUTTONDOWN) {
    //cout << "Middle button of the mouse is clicked - position (" << x << ", "
    //     << y << ")" << endl;
  } else if (event == EVENT_MOUSEMOVE) {
//    cout << "Mouse move over the window - position (" << x << ", " << y << ")"
//         << endl;
    mouse_x = x;
    mouse_y = y;
  }
}

int main(int argc, char** argv) {

    //testKalmanPose();

    //test_eigen();

  KalmanFilter KF(4, 2, 0);
  KF.transitionMatrix =
      *(Mat_<float>(4, 4) << 1, 0, 1, 0, 0, 1, 0, 1, 0, 0, 1, 0, 0, 0, 0, 1);

  Mat_<float> measurement(2, 1);
  measurement.setTo(Scalar(0));

  // init...
  KF.statePre.at<float>(0) = 320;
  KF.statePre.at<float>(1) = 240;
  KF.statePre.at<float>(2) = 0;
  KF.statePre.at<float>(3) = 0;
  setIdentity(KF.measurementMatrix);
  setIdentity(KF.processNoiseCov, Scalar::all(1e-4));
  setIdentity(KF.measurementNoiseCov, Scalar::all(1e-1));
  setIdentity(KF.errorCovPost, Scalar::all(.1));

  cout << fixed << setprecision(2) << "process noise cov \n"
       << KF.processNoiseCov << " \n"
       << "measurementNoiseCov \n" << KF.measurementNoiseCov << "\n"
       << "errorCovPost \n" << KF.errorCovPost << endl;

  return 0;

  // Create a window
  namedWindow("Kalman tutorial", 1);

  // set the callback function for any mouse event
  setMouseCallback("Kalman tutorial", CallBackFunc, NULL);

  Mat res_image(480, 640, CV_8UC3, Scalar(0, 0, 0));

  mouse_x = 320;
  mouse_y = 240;

  vector<Point> mouse_line;
  vector<Point> estimated_line;

  bool running = true;
  while (running) {
    Mat res_image(480, 640, CV_8UC3, Scalar(0, 0, 0));

    // First predict, to update the internal statePre variable
    Mat prediction = KF.predict();
    Point predictPt(prediction.at<float>(0), prediction.at<float>(1));

    // Get mouse point
    measurement(0) = mouse_x;
    measurement(1) = mouse_y;

    Point measPt(measurement(0), measurement(1));

    // The "correct" phase that is going to use the predicted value and our
    // measurement
    Mat estimated = KF.correct(measurement);
    Point statePt(estimated.at<float>(0), estimated.at<float>(1));

    mouse_line.push_back(measPt);
    estimated_line.push_back(statePt);

    if (mouse_line.size() > 0) {
      polylines(res_image, mouse_line, false, Scalar(255, 0, 0));
      polylines(res_image, estimated_line, false, Scalar(0, 255, 0));
    }

    // circle(res_image, measPt, 1, Scalar(255,0,0), -1);
    // circle(res_image, statePt, 1, Scalar(0,255,0), -1);

    imshow("Kalman tutorial", res_image);

    auto c = waitKey(100);

    if (c == 'q') break;
  }

  return 0;
}


// from ros-control meta packages
#include <controller_interface/controller.h>
#include <hardware_interface/joint_command_interface.h>

#include <pluginlib/class_list_macros.h>
#include <std_msgs/Float64MultiArray.h>

#include <urdf/model.h>

#include <Eigen/LU>
// from kdl packages
#include <kdl/tree.hpp>
#include <kdl/kdl.hpp>
#include <kdl/chain.hpp>
#include <kdl_parser/kdl_parser.hpp>
#include <kdl/chaindynparam.hpp>              // inverse dynamics
#include <kdl/jntarrayacc.hpp>
#include <kdl/jntarray.hpp>
#include <math.h>
#include <numeric>
#include <kdl/chainjnttojacsolver.hpp>
#include <kdl/chainfksolver.hpp>
#include <kdl/chainfksolverpos_recursive.hpp>
#include <kdl/chainfksolvervel_recursive.hpp>
#include <kdl/framevel.hpp>

#include <boost/scoped_ptr.hpp>
#include <boost/lexical_cast.hpp>

#define PI 3.141592
#define D2R PI / 180.0
#define R2D 180.0 / PI
#define SaveDataMax 49

namespace arm_controllers
{
class GravityControllerReactive : public controller_interface::Controller<hardware_interface::EffortJointInterface>
{
  public:
    bool init(hardware_interface::EffortJointInterface *hw, ros::NodeHandle &n)
    {
        // ********* 1. Get joint name / gain from the parameter server *********
        // 1.1 Joint Name
        if (!n.getParam("joints", joint_names_))
        {
            ROS_ERROR("Could not find joint name");
            return false;
        }
        n_joints_ = joint_names_.size();

        if (n_joints_ == 0)
        {
            ROS_ERROR("List of joint names is empty.");
            return false;
        }
        else
        {
            ROS_INFO("Found %d joint names", n_joints_);
            for (int i = 0; i < n_joints_; i++)
            {
                ROS_INFO("%s", joint_names_[i].c_str());
            }
        }

        // 1.2 Gain
        // 1.2.1 Joint Controller
        Kp_.resize(n_joints_);
        Kd_.resize(n_joints_);
        Ki_.resize(n_joints_);
        seg_jac_1.resize(1);
        seg_jac_2.resize(2);
        seg_jac_3.resize(3);
        seg_jac_4.resize(4);
        seg_jac_5.resize(5);
        J_.resize(n_joints_);
        K_kine_.resize(n_joints_);
        K_kine_.data(0) = 1.0;
        K_kine_.data(1) = 1.0;
        K_kine_.data(2) = 1.0;
        K_kine_.data(3) = 1.0;
        K_kine_.data(4) = 1.0;
        K_kine_.data(5) = 1.0;

        std::vector<double> Kp(n_joints_), Ki(n_joints_), Kd(n_joints_);
        for (size_t i = 0; i < n_joints_; i++)
        {
            std::string si = boost::lexical_cast<std::string>(i + 1);
            if (n.getParam("/elfin/gravity_controller_reactive/gains/elfin_joint" + si + "/pid/p", Kp[i]))
            {
                Kp_(i) = Kp[i];
            }
            else
            {
                std::cout << "/elfin/gravity_controller_reactive/gains/elfin_joint" + si + "/pid/p" << std::endl;
                ROS_ERROR("Cannot find pid/p gain");
                return false;
            }

            if (n.getParam("/elfin/gravity_controller_reactive/gains/elfin_joint" + si + "/pid/i", Ki[i]))
            {
                Ki_(i) = Ki[i];
            }
            else
            {
                ROS_ERROR("Cannot find pid/i gain");
                return false;
            }

            if (n.getParam("/elfin/gravity_controller_reactive/gains/elfin_joint" + si + "/pid/d", Kd[i]))
            {
                Kd_(i) = Kd[i];
            }
            else
            {
                ROS_ERROR("Cannot find pid/d gain");
                return false;
            }
        }

        // 2. ********* urdf *********
        urdf::Model urdf;
        if (!urdf.initParam("robot_description"))
        {
            ROS_ERROR("Failed to parse urdf file");
            return false;
        }
        else
        {
            ROS_INFO("Found robot_description");
        }

        // 3. ********* Get the joint object to use in the realtime loop [Joint Handle, URDF] *********
        for (int i = 0; i < n_joints_; i++)
        {
            try
            {
                joints_.push_back(hw->getHandle(joint_names_[i]));
            }
            catch (const hardware_interface::HardwareInterfaceException &e)
            {
                ROS_ERROR_STREAM("Exception thrown: " << e.what());
                return false;
            }

            urdf::JointConstSharedPtr joint_urdf = urdf.getJoint(joint_names_[i]);
            if (!joint_urdf)
            {
                ROS_ERROR("Could not find joint '%s' in urdf", joint_names_[i].c_str());
                return false;
            }
            joint_urdfs_.push_back(joint_urdf);
        }

        // 4. ********* KDL *********
        // 4.1 kdl parser
        if (!kdl_parser::treeFromUrdfModel(urdf, kdl_tree_))
        {
            ROS_ERROR("Failed to construct kdl tree");
            return false;
        }
        else
        {
            ROS_INFO("Constructed kdl tree");
        }

        // 4.2 kdl chain
        std::string root_name, tip_name;
        if (!n.getParam("root_link", root_name))
        {
            ROS_ERROR("Could not find root link name");
            return false;
        }
        if (!n.getParam("tip_link", tip_name))
        {
            ROS_ERROR("Could not find tip link name");
            return false;
        }
        if (!kdl_tree_.getChain(root_name, tip_name, kdl_chain_))
        {
            ROS_ERROR_STREAM("Failed to get KDL chain from tree: ");
            ROS_ERROR_STREAM("  " << root_name << " --> " << tip_name);
            ROS_ERROR_STREAM("  Tree has " << kdl_tree_.getNrOfJoints() << " joints");
            ROS_ERROR_STREAM("  Tree has " << kdl_tree_.getNrOfSegments() << " segments");
            ROS_ERROR_STREAM("  The segments are:");

            KDL::SegmentMap segment_map = kdl_tree_.getSegments();
            KDL::SegmentMap::iterator it;

            for (it = segment_map.begin(); it != segment_map.end(); it++)
                ROS_ERROR_STREAM("    " << (*it).first);

            return false;
        }
        else
        {
            ROS_INFO("Got kdl chain");
        }

        // 4.3 inverse dynamics solver 초기화
        gravity_ = KDL::Vector::Zero(); // ?
        gravity_(2) = -9.81;            // 0: x-axis 1: y-axis 2: z-axis

        id_solver_.reset(new KDL::ChainDynParam(kdl_chain_, gravity_));
        FKSolver_.reset(new KDL::ChainFkSolverPos_recursive(kdl_chain_));
        FKSolver_vel_.reset(new KDL::ChainFkSolverVel_recursive(kdl_chain_));
        jnt_to_jac_solver_.reset(new KDL::ChainJntToJacSolver(kdl_chain_));

        kdl_tree_.getChain("world", "elfin_link1", kdl_chain_1);
        jac_solver_1.reset(new KDL::ChainJntToJacSolver(kdl_chain_1));
        fk_solver_1.reset(new KDL::ChainFkSolverPos_recursive(kdl_chain_1));

        kdl_tree_.getChain("world", "elfin_link2", kdl_chain_2);
        jac_solver_2.reset(new KDL::ChainJntToJacSolver(kdl_chain_2));
        fk_solver_2.reset(new KDL::ChainFkSolverPos_recursive(kdl_chain_2));

        kdl_tree_.getChain("world", "elfin_link3", kdl_chain_3);
        jac_solver_3.reset(new KDL::ChainJntToJacSolver(kdl_chain_3));
        fk_solver_3.reset(new KDL::ChainFkSolverPos_recursive(kdl_chain_3));

        kdl_tree_.getChain("world", "elfin_link4", kdl_chain_4);
        jac_solver_4.reset(new KDL::ChainJntToJacSolver(kdl_chain_4));
        fk_solver_4.reset(new KDL::ChainFkSolverPos_recursive(kdl_chain_4));

        kdl_tree_.getChain("world", "elfin_link5", kdl_chain_5);
        jac_solver_5.reset(new KDL::ChainJntToJacSolver(kdl_chain_5));
        fk_solver_5.reset(new KDL::ChainFkSolverPos_recursive(kdl_chain_5));

        // ********* 5. 각종 변수 초기화 *********

        // 5.1 Vector 초기화 (사이즈 정의 및 값 0)
        tau_d_.data = Eigen::VectorXd::Zero(n_joints_);

        qd_.data = Eigen::VectorXd::Zero(n_joints_);
        qd_dot_.data = Eigen::VectorXd::Zero(n_joints_);
        qd_ddot_.data = Eigen::VectorXd::Zero(n_joints_);
        qd_old_.data = Eigen::VectorXd::Zero(n_joints_);

        q_.data = Eigen::VectorXd::Zero(n_joints_);
        qdot_.data = Eigen::VectorXd::Zero(n_joints_);
        xdot_.data = Eigen::VectorXd::Zero(n_joints_);
        ex_ = Eigen::VectorXd::Zero(n_joints_);
        jnt_limits_lower_.data = Eigen::VectorXd::Zero(n_joints_);
        jnt_limits_upper_.data = Eigen::VectorXd::Zero(n_joints_);
        diff_to_low_.data = Eigen::VectorXd::Zero(n_joints_);
        diff_to_low_prev_.data = Eigen::VectorXd::Zero(n_joints_);
        diff_to_upper_.data = Eigen::VectorXd::Zero(n_joints_);
        diff_to_upper_prev_.data = Eigen::VectorXd::Zero(n_joints_);
        ex_obstacle_prev_ = Eigen::VectorXd::Zero(n_joints_);

        obstacle_.p(0) = -0.1;
        obstacle_.p(1) = -0.1;
        obstacle_.p(2) = 0.75;
        obstacle_.M.RPY(0,0,0);

        jnt_limits_lower_(0) = -0.6;
        jnt_limits_lower_(1) = -0.6;
        jnt_limits_lower_(2) = -0.6;
        jnt_limits_lower_(3) = -0.6;
        jnt_limits_lower_(4) = -0.6;
        jnt_limits_lower_(5) = -0.6;

        jnt_limits_upper_(0) = 0.6;
        jnt_limits_upper_(1) = 0.6;
        jnt_limits_upper_(2) = 0.6;
        jnt_limits_upper_(3) = 0.6;
        jnt_limits_upper_(4) = 0.6;
        jnt_limits_upper_(5) = 0.6;

        table_corner_1 = KDL::Vector(-0.2, -0.25, 0.3);
        table_corner_2 = KDL::Vector(-0.9, -0.25, 0.3);
        table_corner_3 = KDL::Vector(-0.9, 0.25, 0.3);
        table_corner_4 = KDL::Vector(-0.2, 0.25, 0.3);

        interpolated_table_surface_.push_back(table_corner_1);
        interpolated_table_surface_.push_back(table_corner_2);
     //   interpolated_table_surface_.push_back(table_corner_3);
     //   interpolated_table_surface_.push_back(table_corner_4);

        int split_num = 30;
        for(int i = 1; i < split_num; i++){
            double diff12 = (table_corner_2(0) - table_corner_1(0)) * i/split_num;
            interpolated_table_surface_.push_back(KDL::Vector(table_corner_1(0) + diff12, -0.25, 0.3));
         //   double diff23 = (table_corner_3(1) - table_corner_2(1)) * i/split_num;
         //   interpolated_table_surface_.push_back(KDL::Vector(-0.9, diff23, 0.5));
         //   double diff34 = (table_corner_4(0) - table_corner_3(0)) * i/split_num;
         //   interpolated_table_surface_.push_back(KDL::Vector(diff34, 0.25, 0.5));
         //   double diff41 = (table_corner_1(1) - table_corner_4(1)) * i/split_num;
         //   interpolated_table_surface_.push_back(KDL::Vector(-0.2, diff41, 0.5));
        }
        std::vector<KDL::Vector> tmp_copy = interpolated_table_surface_;
        for(int i = 1; i < (int)tmp_copy.size(); i++){
            double curr_x = tmp_copy[i](0);
            for(int j = 0; j <= split_num; j++){
                double next_y = tmp_copy[i](1) + 0.5*j/split_num;
                interpolated_table_surface_.push_back(KDL::Vector(curr_x, next_y, 0.3));
            }
        }
        std::cout << "TABLE HAS: " << interpolated_table_surface_.size() << std::endl;

        for(int i = 1; i <= 5; i++){
            KDL::JntArray arr;
            arr.data = Eigen::VectorXd::Zero(i);
            arm_segment_states_.push_back(arr);
        }
        std::cout << "JntArrays done " << std::endl;

        e_.data = Eigen::VectorXd::Zero(n_joints_);
        e_dot_.data = Eigen::VectorXd::Zero(n_joints_);
        e_int_.data = Eigen::VectorXd::Zero(n_joints_);

        // 5.2 Matrix 초기화 (사이즈 정의 및 값 0)
        M_.resize(kdl_chain_.getNrOfJoints());
        C_.resize(kdl_chain_.getNrOfJoints());
        G_.resize(kdl_chain_.getNrOfJoints());

        // ********* 6. ROS 명령어 *********
        // 6.1 publisher
        pub_qd_ = n.advertise<std_msgs::Float64MultiArray>("qd", 1000);
        pub_q_ = n.advertise<std_msgs::Float64MultiArray>("q", 1000);
        pub_e_ = n.advertise<std_msgs::Float64MultiArray>("e", 1000);

        pub_SaveData_ = n.advertise<std_msgs::Float64MultiArray>("SaveData", 1000); // 뒤에 숫자는?

        // 6.2 subsriber

        return true;
    }

    void commandCB(const std_msgs::Float64MultiArrayConstPtr &msg)
    {
        if (msg->data.size() != n_joints_)
        {
            ROS_ERROR_STREAM("Dimension of command (" << msg->data.size() << ") does not match number of joints (" << n_joints_ << ")! Not executing!");
            return;
        }
    }

    void starting(const ros::Time &time)
    {
        t = 0.0;
        ROS_INFO("Starting Reactive Controller");
    }

    void update(const ros::Time &time, const ros::Duration &period)
    {
        // ********* 0. Get states from gazebo *********
        // 0.1 sampling time
        double dt = period.toSec();
        t = t + 0.001;

        // 0.2 joint state
        for (int i = 0; i < n_joints_; i++)
        {
            q_(i) = joints_[i].getPosition();
            qdot_(i) = joints_[i].getVelocity();
        }

        int segment_length = 1;
        for(int i = 0; i < 5; i++){
            for(int j = 1; j <= segment_length; j++){
                arm_segment_states_[i](j) = q_(j);
            }
            segment_length++;
        }

        std::vector<KDL::Jacobian> jacobians_;
        jac_solver_1->JntToJac(arm_segment_states_[0], seg_jac_1);
        jacobians_.push_back(seg_jac_1);

        jac_solver_2->JntToJac(arm_segment_states_[1], seg_jac_2);
        jacobians_.push_back(seg_jac_2);

        jac_solver_3->JntToJac(arm_segment_states_[2], seg_jac_3);
        jacobians_.push_back(seg_jac_3);

        jac_solver_4->JntToJac(arm_segment_states_[3], seg_jac_4);
        jacobians_.push_back(seg_jac_4);

        jac_solver_5->JntToJac(arm_segment_states_[4], seg_jac_5);
        jacobians_.push_back(seg_jac_5);

        // ********* 1. Desired Trajecoty in Joint Space *********

        for (size_t i = 0; i < n_joints_; i++)
        {
            qd_ddot_(i) = -M_PI * M_PI / 4 * 45 * KDL::deg2rad * sin(M_PI / 2 * t);
            qd_dot_(i) = M_PI / 2 * 45 * KDL::deg2rad * cos(M_PI / 2 * t);
            qd_(i) = 45 * KDL::deg2rad * sin(M_PI / 2* t);
          //  continue;
         //
            if( t < 1 ){
                qd_(i) = 0;
            }
           // qd_(i) = 0;
            qd_ddot_(i) = 0;
            qd_dot_(i) = 0;

            // This is the sequence to bring the arm to starting position.
            if(t < 2){
               // qd_(i) = 0;
                qd_(0) = -1.6;
                qd_(1) = 0;
                qd_(2) = -1.4;
                qd_(3) = 0.01;
                qd_(4) = -1.57;
                qd_(5) = 0;
            }

            // This is the sweeping sequence.
            if(t > 2){
          //     qd_(i) = 1.57;
                //qd_(0) = q_(0) + dt*30;
           //     qd_(0) += dt;
             //   qd_(1) = 0;
               // qd_(2) = -1.4;
            //    qd_(3) = 0.01;
              //  qd_(4) = -1.57;
                //qd_(5) = 0;
              //  qd_(1) = 35 * KDL::deg2rad * sin(M_PI / 2* t);
            }
            if(t < 1){
               // qd_(i) = 0;
                qd_(0) = 0;
                qd_(1) = 0;
                qd_(2) = 0;
                qd_(3) = 0.0;
                qd_(4) = 0;
                qd_(5) = 0;
            }

            // This is the sweeping sequence.
            if(t > 1){
          //     qd_(i) = 1.57;
                //qd_(0) = q_(0) + dt*30;
                qd_(0) = 0;
                //qd_(1) = 0;
                //qd_(2) = 0;
                qd_(1) = -55 * KDL::deg2rad * sin(M_PI * t);
                qd_(2) = -55 * KDL::deg2rad * sin(M_PI * t);
                qd_(3) = 0;
                qd_(4) = 0;
                qd_(5) = 0;

            }

        }

        tau_rep_ = Eigen::VectorXd::Zero(n_joints_);

        // *** 2.2.1 Compute model(M,C,G) ***
        id_solver_->JntToMass(q_, M_);
        id_solver_->JntToCoriolis(q_, qdot_, C_);
        id_solver_->JntToGravity(q_, G_);

        // Apply Joint limits as repulsive forces/torques
        KDL::JntArray diff_q_;
        Subtract(q_, qd_, diff_q_);
        Subtract(q_, jnt_limits_lower_, diff_to_low_);
        Subtract(q_, jnt_limits_upper_, diff_to_upper_);

 //       Eigen::Matrix<double, 6, 1> U_att = (1/2) * diff_q_.data.cwiseProduct(diff_q_.data);
 //       Eigen::Matrix<double, 6, 1> U_rep_low_ = -(1/2) * diff_to_low_.data.cwiseProduct(diff_to_low_.data);
 //       Eigen::Matrix<double, 6, 1> U_rep_upper_ = -(1/2) * diff_to_upper_.data.cwiseProduct(diff_to_upper_.data);

        KDL::Frame xd_;
        KDL::Frame x_up_lim_;
        KDL::Frame x_low_lim_;
        Eigen::Matrix<double, 6, 1> ex_low_;
        Eigen::Matrix<double, 6, 1> ex_upper_;
        Eigen::Matrix<double, 6, 1> ex_obstacle_;

        jnt_to_jac_solver_->JntToJac(q_, J_);
        FKSolver_->JntToCart(qd_, xd_);
        FKSolver_->JntToCart(jnt_limits_lower_, x_low_lim_);
        FKSolver_->JntToCart(jnt_limits_upper_, x_up_lim_);
        FKSolver_->JntToCart(q_, x_);

        // Calculate workspace positions for joints and middle parts of links.
        std::vector<KDL::Frame> arm_segment_states_cart_;
        std::vector<KDL::Frame> arm_segment_midpoints_states_cart_;

        KDL::Frame seg_cart_;
        fk_solver_1->JntToCart(arm_segment_states_[0], seg_cart_);
        arm_segment_states_cart_.push_back(seg_cart_);
        fk_solver_2->JntToCart(arm_segment_states_[1], seg_cart_);
        arm_segment_states_cart_.push_back(seg_cart_);
        fk_solver_3->JntToCart(arm_segment_states_[2], seg_cart_);
        arm_segment_states_cart_.push_back(seg_cart_);
        fk_solver_4->JntToCart(arm_segment_states_[3], seg_cart_);
        arm_segment_states_cart_.push_back(seg_cart_);
        fk_solver_5->JntToCart(arm_segment_states_[4], seg_cart_);
        arm_segment_states_cart_.push_back(seg_cart_);

        for(int i = 0; i < (int)arm_segment_states_cart_.size() - 1; i++){
            KDL::Frame start_ = arm_segment_states_cart_[i];
            KDL::Frame end_ = arm_segment_states_cart_[i+1];
            KDL::Frame mid_point_;
            mid_point_.p(0) = (start_.p(0) + end_.p(0)) / 2;
            mid_point_.p(1) = (start_.p(1) + end_.p(1)) / 2;
            mid_point_.p(2) = (start_.p(2) + end_.p(2)) / 2;
            mid_point_.M.RPY(0,0,0);
            arm_segment_midpoints_states_cart_.push_back(mid_point_);
        }

        ex_temp_ = diff(x_, xd_);

        ex_(0) = ex_temp_(0);
        ex_(1) = ex_temp_(1);
        ex_(2) = ex_temp_(2);
        ex_(3) = ex_temp_(3);
        ex_(4) = ex_temp_(4);
        ex_(5) = ex_temp_(5);

        ex_temp_ = diff(x_, x_up_lim_);

        ex_upper_(0) = ex_temp_(0);
        ex_upper_(1) = ex_temp_(1);
        ex_upper_(2) = ex_temp_(2);
        ex_upper_(3) = ex_temp_(3);
        ex_upper_(4) = ex_temp_(4);
        ex_upper_(5) = ex_temp_(5);

        ex_temp_ = diff(x_, x_low_lim_);

        ex_low_(0) = ex_temp_(0);
        ex_low_(1) = ex_temp_(1);
        ex_low_(2) = ex_temp_(2);
        ex_low_(3) = ex_temp_(3);
        ex_low_(4) = ex_temp_(4);
        ex_low_(5) = ex_temp_(5);

        ex_obstacle_ = ex_obstacle_prev_;

        ex_temp_ = diff(obstacle_, x_);

        ex_obstacle_(0) = ex_temp_(0);
        ex_obstacle_(1) = ex_temp_(1);
        ex_obstacle_(2) = ex_temp_(2);
        ex_obstacle_(3) = ex_temp_(3);
        ex_obstacle_(4) = ex_temp_(4);
        ex_obstacle_(5) = ex_temp_(5);

        Eigen::Matrix<double, 6, 1> grad_D_ = ex_obstacle_ - ex_obstacle_prev_;

     //   Eigen::Matrix<double, 6, 1> U_att = (1/2) * ex_.cwiseProduct(ex_);
        Eigen::Matrix<double, 6, 1> U_rep_lower_ = -(1/2) * ex_low_.cwiseProduct(ex_low_);
        Eigen::Matrix<double, 6, 1> U_rep_upper_ = -(1/2) * ex_upper_.cwiseProduct(ex_upper_);
        Eigen::Matrix<double, 6, 1> U_rep_ = U_rep_lower_ + U_rep_upper_;

       // Eigen::Matrix<double, 6, 1> F_att = -ex_;
        Eigen::Matrix<double, 6, 1> F_rep_lower_; // = (1 / ex_low_) * (1 / ex_low_.cwiseProduct(ex_low_));
        Eigen::Matrix<double, 6, 1> F_rep_upper_; // = (1 / ex_upper_) * (1 / ex_upper_.cwiseProduct(ex_upper_));
        Eigen::Matrix<double, 6, 1> F_obstacle_;

        // This is where the joint limits are applied.
        double Q_star_ = 0.0;

        Eigen::Matrix<double, 6, 1> grad_D_low_ = diff_to_low_.data - diff_to_low_prev_.data;
        Eigen::Matrix<double, 6, 1> grad_D_upper_ = diff_to_upper_.data - diff_to_upper_prev_.data;
        for(int i = 0; i < 6; i++){

            // UNCOMMENT THIS break to include joint limit forces.
             break;
            if( abs(diff_to_low_(i)) < Q_star_ ){
                tau_rep_(i) += 0.001*(1 / diff_to_low_(i) - 1 / Q_star_) * (1 / diff_to_low_(i) - 1 / Q_star_);
            }
            if( abs(diff_to_upper_(i)) < Q_star_){
                tau_rep_(i) += -0.001 * (1 / diff_to_upper_(i) - 1 / Q_star_) * (1 / diff_to_upper_(i) - 1 / Q_star_);
            }
        }
        diff_to_low_prev_ = diff_to_low_;
        diff_to_upper_prev_ = diff_to_upper_;

        double Q_star_table_ = 0.22;
        double closest_dist_ = 999;
        int closest_idx_ = -2;
        Eigen::Matrix<double, 6, 1> ex_table_point_ = Eigen::VectorXd::Zero(6);
        Eigen::Matrix<double, 6, 1> ex_closest_table_point_ = Eigen::VectorXd::Zero(6);
        Eigen::Matrix<double, 6, 1> F_rep_table_total_ = Eigen::VectorXd::Zero(6);

        // Include repulsion field of the table WRT end effector only.
        for(int i = 0; i < (int)interpolated_table_surface_.size(); i++){
           // break;
            KDL::Vector curr_table_point_ = interpolated_table_surface_[i];
            ex_table_point_(0) = curr_table_point_(0) - x_.p(0);
            ex_table_point_(1) = curr_table_point_(1) - x_.p(1);
            ex_table_point_(2) = curr_table_point_(2) - x_.p(2);
            table_point_distances_[i] = -ex_table_point_;

            double dist = sqrt( ex_table_point_(0)*ex_table_point_(0) + ex_table_point_(1)*ex_table_point_(1) + ex_table_point_(2)*ex_table_point_(2));
            if( dist < Q_star_table_ ){
                closest_dist_ = dist;
                closest_idx_ = i;
                ex_closest_table_point_ = -ex_table_point_;

                Eigen::Matrix<double, 6, 1> grad_D_table_ = ex_table_point_ - table_point_distances_prev_[i];

                F_rep_table_total_(0) = -0.1 * (1 / ex_table_point_(0) - 1 / Q_star_table_) * (1 / (ex_table_point_(0) * ex_table_point_(0)) * -grad_D_table_(0));
                F_rep_table_total_(1) = -0.1 * (1 / ex_table_point_(1) - 1 / Q_star_table_) * (1 / (ex_table_point_(1) * ex_table_point_(1)) * -grad_D_table_(1));
                F_rep_table_total_(2) = -10 * (1 / ex_table_point_(2) - 1 / Q_star_table_) * (1 / (ex_table_point_(2) * ex_table_point_(2)) * -grad_D_table_(2));
                for(int f = 0; f < 3; f++){
                    if( F_rep_table_total_(f) > 500 ){
                        F_rep_table_total_(f) = 500;
                    }
                }

                // UNCOMMENT THIS to include the tables repulsion forces in the arm.
               tau_rep_ += J_.data.transpose() * F_rep_table_total_;
            }
        }

        // Include repulsion field of the table WRT all physical points of arm.
        for(int i = 0; i < (int)interpolated_table_surface_.size(); i++){
          // break;
            KDL::Vector curr_table_point_ = interpolated_table_surface_[i];

            for(int a = 1; a < (int)arm_segment_states_cart_.size(); a++ ){
                KDL::Frame arm_point_ = arm_segment_states_cart_[a];
                ex_table_point_(0) = curr_table_point_(0) - arm_point_.p(0);
                ex_table_point_(1) = curr_table_point_(1) - arm_point_.p(1);
                ex_table_point_(2) = curr_table_point_(2) - arm_point_.p(2);
                table_point_distances_arm_[a][i] = -ex_table_point_;

                double dist = sqrt( ex_table_point_(0)*ex_table_point_(0) + ex_table_point_(1)*ex_table_point_(1) + ex_table_point_(2)*ex_table_point_(2));
                if( dist < Q_star_table_ ){

                    closest_dist_ = dist;
                    closest_idx_ = i;
                    ex_closest_table_point_ = -ex_table_point_;

                    std::cout << "ROWS: " << jacobians_[a].rows()  << " COLUMNS: " << jacobians_[a].columns() << std::endl;
                    int c_size = jacobians_[a].columns();
                    //Eigen::VectorXd F_rep_table_total_segs_ = Eigen::VectorXd::Zero(jacobians_[a].columns());
                    Eigen::Matrix<double, 6, 1> grad_D_table_ = ex_table_point_ - table_point_distances_arm_prev_[a][i];

                   // F_rep_table_total_(0) = -0.01 * (1 / ex_table_point_(0) - 1 / Q_star_table_) * (1 / (ex_table_point_(0) * ex_table_point_(0)) * -grad_D_table_(0));
                   // F_rep_table_total_(1) = -0.01 * (1 / ex_table_point_(1) - 1 / Q_star_table_) * (1 / (ex_table_point_(1) * ex_table_point_(1)) * -grad_D_table_(1));
                    F_rep_table_total_(2) = -1 * (1 / ex_table_point_(2) - 1 / Q_star_table_) * (1 / (ex_table_point_(2) * ex_table_point_(2)) * -grad_D_table_(2));
                    for(int f = 0; f < 3; f++){
                        if( F_rep_table_total_(f) > 1000 ){
                            F_rep_table_total_(f) = 1000;
                        }
                    }
                   std::cout << "x: " << F_rep_table_total_(0) << " y: " << F_rep_table_total_(1) << " z: "  << F_rep_table_total_(2) << std::endl;
                // UNCOMMENT THIS to include the tables repulsion forces in the arm.
                   Eigen::VectorXd torques = jacobians_[a].data.transpose() * F_rep_table_total_;
                   for(int t = 0; t < c_size; t++){
                      tau_rep_(t) += torques(t) - (M_.data * qdot_.data)(t);
                   }
                }
            }
        }
        tau_rep_ -= M_.data * qdot_.data;

       // ex_table_prev_ = ex_closest_table_point_;
        table_point_distances_prev_ = table_point_distances_;
        table_point_distances_arm_prev_ = table_point_distances_arm_;

        //Eigen::Matrix<double, 6, 1> F_rep_ = F_obstacle_;
        Eigen::Matrix<double, 6, 1> F_rep_ = F_rep_table_total_;
        F_rep_(3) = 0;
        F_rep_(4) = 0;
        F_rep_(5) = 0;

        // ********* 2. Motion Controller in Joint Space*********
        // *** 2.1 Error Definition in Joint Space ***
        e_.data = qd_.data - q_.data;
        e_dot_.data = qd_dot_.data - qdot_.data;
        e_int_.data = qd_.data - q_.data; // (To do: e_int 업데이트 필요요)

        // *** 2.2.2 gravity + PD as in slides ***

        // This can be considered as u_att
        tau_PD_grav_.data = G_.data + Kp_.data.cwiseProduct(e_.data) - Kd_.data.cwiseProduct(qdot_.data);

        //tau_rep_ = J_.data.transpose() * F_rep_ - M_.data * qdot_.data;
       // tau_rep_ = J_.data.transpose() * F_rep_ - Kd_.data.cwiseProduct(qdot_.data);
        KDL::JntArray tau_total_;
        tau_total_.data = tau_PD_grav_.data + tau_rep_;

        // *** 2.3 Apply Torque Command to Actuator ***

        // Use 'e_dot_.data = qdot_c_.data - qdot_.data' for kinematic controller.
        // Otherwise use 'e_dot_.data = qd_dot_.data - qdot_.data'.
        //e_dot_.data = qdot_c_.data - qdot_.data;
        //aux_d_.data = M_.data * (qd_ddot_.data + Kd_.data.cwiseProduct(e_dot_.data));
        //comp_d_.data = C_.data + G_.data;
        //tau_d_.data = aux_d_.data + comp_d_.data;

        for (int i = 0; i < n_joints_; i++)
        {
            joints_[i].setCommand(tau_total_(i));
            //joints_[i].setCommand(tau_PD_grav_(i));
            //joints_[i].setCommand(tau_d_(i));
        }

        // ********* 3. data 저장 *********
        save_data();

        // ********* 4. state 출력 *********
        print_state();
    }

    void stopping(const ros::Time &time)
    {
    }

    void save_data()
    {
        // 1
        // Simulation time (unit: sec)
        SaveData_[0] = t;

        // Desired position in joint space (unit: rad)
        SaveData_[1] = qd_(0);
        SaveData_[2] = qd_(1);
        SaveData_[3] = qd_(2);
        SaveData_[4] = qd_(3);
        SaveData_[5] = qd_(4);
        SaveData_[6] = qd_(5);

        // Desired velocity in joint space (unit: rad/s)
        SaveData_[7] = qd_dot_(0);
        SaveData_[8] = qd_dot_(1);
        SaveData_[9] = qd_dot_(2);
        SaveData_[10] = qd_dot_(3);
        SaveData_[11] = qd_dot_(4);
        SaveData_[12] = qd_dot_(5);

        // Desired acceleration in joint space (unit: rad/s^2)
        SaveData_[13] = qd_ddot_(0);
        SaveData_[14] = qd_ddot_(1);
        SaveData_[15] = qd_ddot_(2);
        SaveData_[16] = qd_ddot_(3);
        SaveData_[17] = qd_ddot_(4);
        SaveData_[18] = qd_ddot_(5);

        // Actual position in joint space (unit: rad)
        SaveData_[19] = q_(0);
        SaveData_[20] = q_(1);
        SaveData_[21] = q_(2);
        SaveData_[22] = q_(3);
        SaveData_[23] = q_(4);
        SaveData_[24] = q_(5);

        // Actual velocity in joint space (unit: rad/s)
        SaveData_[25] = qdot_(0);
        SaveData_[26] = qdot_(1);
        SaveData_[27] = qdot_(2);
        SaveData_[28] = qdot_(3);
        SaveData_[29] = qdot_(4);
        SaveData_[30] = qdot_(5);

        // Error position in joint space (unit: rad)
        SaveData_[31] = e_(0);
        SaveData_[32] = e_(1);
        SaveData_[33] = e_(2);
        SaveData_[34] = e_(3);
        SaveData_[35] = e_(4);
        SaveData_[36] = e_(5);

        j1_avg.push_back(abs(R2D * e_(0)));
        j2_avg.push_back(abs(R2D * e_(1)));
        j3_avg.push_back(abs(R2D * e_(2)));
        j4_avg.push_back(abs(R2D * e_(3)));
        j5_avg.push_back(abs(R2D * e_(4)));
        j6_avg.push_back(abs(R2D * e_(5)));

        // Error velocity in joint space (unit: rad/s)
        SaveData_[37] = e_dot_(0);
        SaveData_[38] = e_dot_(1);
        SaveData_[39] = e_dot_(3);
        SaveData_[40] = e_dot_(4);
        SaveData_[41] = e_dot_(5);
        SaveData_[42] = e_dot_(6);

        j1_vel_avg.push_back(abs(R2D * e_dot_(0)));
        j2_vel_avg.push_back(abs(R2D * e_dot_(1)));
        j3_vel_avg.push_back(abs(R2D * e_dot_(2)));
        j4_vel_avg.push_back(abs(R2D * e_dot_(3)));
        j5_vel_avg.push_back(abs(R2D * e_dot_(4)));
        j6_vel_avg.push_back(abs(R2D * e_dot_(5)));

        // Error intergal value in joint space (unit: rad*sec)
        SaveData_[43] = e_int_(0);
        SaveData_[44] = e_int_(1);
        SaveData_[45] = e_int_(2);
        SaveData_[46] = e_int_(3);
        SaveData_[47] = e_int_(4);
        SaveData_[48] = e_int_(5);

        // 2
        msg_qd_.data.clear();
        msg_q_.data.clear();
        msg_e_.data.clear();

        msg_SaveData_.data.clear();

        // 3
        for (int i = 0; i < n_joints_; i++)
        {
            msg_qd_.data.push_back(qd_(i));
            msg_q_.data.push_back(q_(i));
            msg_e_.data.push_back(e_(i));
        }

        for (int i = 0; i < SaveDataMax; i++)
        {
            msg_SaveData_.data.push_back(SaveData_[i]);
        }

        // 4
        pub_qd_.publish(msg_qd_);
        pub_q_.publish(msg_q_);
        pub_e_.publish(msg_e_);

        pub_SaveData_.publish(msg_SaveData_);
    }

    void print_state()
    {
        static int count = 0;
        if (count > 99)
        {
            printf("*********************************************************\n\n");
            printf("*** Simulation Time (unit: sec)  ***\n");
            printf("t = %f\n", t);
            printf("\n");

            printf("*** Desired State in Joint Space (unit: deg) ***\n");
            printf("qd_(0): %f, ", qd_(0)*R2D);
            printf("qd_(1): %f, ", qd_(1)*R2D);
            printf("qd_(2): %f, ", qd_(2)*R2D);
            printf("qd_(3): %f, ", qd_(3)*R2D);
            printf("qd_(4): %f, ", qd_(4)*R2D);
            printf("qd_(5): %f\n", qd_(5)*R2D);
            printf("\n");

            printf("*** Actual State in Joint Space (unit: deg) ***\n");
            printf("q_(0): %f, ", q_(0) * R2D);
            printf("q_(1): %f, ", q_(1) * R2D);
            printf("q_(2): %f, ", q_(2) * R2D);
            printf("q_(3): %f, ", q_(3) * R2D);
            printf("q_(4): %f, ", q_(4) * R2D);
            printf("q_(5): %f\n", q_(5) * R2D);
            printf("\n");

            printf("*** Joint Space Vel Error (unit: deg)  ***\n");
            printf("%f, ", R2D * e_dot_(0));
            printf("%f, ", R2D * e_dot_(1));
            printf("%f, ", R2D * e_dot_(2));
            printf("%f, ", R2D * e_dot_(3));
            printf("%f, ", R2D * e_dot_(4));
            printf("%f\n", R2D * e_dot_(5));
            printf("\n");

            int num_vals = j1_vel_avg.size();
            double average = 0.0;
            if ( num_vals != 0) {
                average = std::accumulate( j1_vel_avg.begin(), j1_vel_avg.end(), 0.0) / num_vals;
                std::cout << "JOINT VEL ERROR AVERAGES: " << std::to_string(average) << " - ";
                average = 0.0;
                average = std::accumulate( j2_vel_avg.begin(), j2_vel_avg.end(), 0.0) / num_vals;
                std::cout << std::to_string(average) << " - ";
                average = 0.0;
                average = std::accumulate( j3_vel_avg.begin(), j3_vel_avg.end(), 0.0) / num_vals;
                std::cout << std::to_string(average) << " - ";
                average = 0.0;
                average = std::accumulate( j4_vel_avg.begin(), j4_vel_avg.end(), 0.0) / num_vals;
                std::cout << std::to_string(average) << " - ";
                average = 0.0;
                average = std::accumulate( j5_vel_avg.begin(), j5_vel_avg.end(), 0.0) / num_vals;
                std::cout << std::to_string(average) << " - ";
                average = 0.0;
                average = std::accumulate( j6_vel_avg.begin(), j6_vel_avg.end(), 0.0) / num_vals;
                std::cout << std::to_string(average) << std::endl;
            }

            count = 0;
        }
        count++;
    }

  private:
    // others
    double t;

    //Joint handles
    unsigned int n_joints_;                               // joint 숫자
    std::vector<std::string> joint_names_;                // joint name ??
    std::vector<hardware_interface::JointHandle> joints_; // ??
    std::vector<urdf::JointConstSharedPtr> joint_urdfs_;  // ??

    // kdl
    KDL::Tree kdl_tree_;   // tree?
    KDL::Chain kdl_chain_; // chain?

    // kdl M,C,G
    KDL::JntSpaceInertiaMatrix M_; // intertia matrix
    KDL::JntSpaceInertiaMatrix M_inv_;
    KDL::JntArray C_;              // coriolis
    KDL::JntArray G_;              // gravity torque vector
    KDL::Vector gravity_;

    // kdl solver
    boost::scoped_ptr<KDL::ChainDynParam> id_solver_;                  // Solver To compute the inverse dynamics

    // Joint Space State
    KDL::JntArray qd_, qd_dot_, qd_ddot_;
    KDL::JntArray qd_old_;
    KDL::JntArray q_, qdot_, qddot_;
    KDL::JntArray e_, e_dot_, e_int_, e_ddot_;
    KDL::Jacobian J_;
    KDL::Jacobian seg_jac_1;
    KDL::Jacobian seg_jac_2;
    KDL::Jacobian seg_jac_3;
    KDL::Jacobian seg_jac_4;
    KDL::Jacobian seg_jac_5;
    boost::scoped_ptr<KDL::ChainFkSolverPos>    FKSolver_;
    boost::scoped_ptr<KDL::ChainFkSolverVel>  FKSolver_vel_;
    boost::scoped_ptr<KDL::ChainJntToJacSolver> jnt_to_jac_solver_;
    boost::scoped_ptr<KDL::ChainJntToJacSolver> jac_solver_1;
    boost::scoped_ptr<KDL::ChainJntToJacSolver> jac_solver_2;
    boost::scoped_ptr<KDL::ChainJntToJacSolver> jac_solver_3;
    boost::scoped_ptr<KDL::ChainJntToJacSolver> jac_solver_4;
    boost::scoped_ptr<KDL::ChainJntToJacSolver> jac_solver_5;
    boost::scoped_ptr<KDL::ChainFkSolverPos>    fk_solver_1;
    boost::scoped_ptr<KDL::ChainFkSolverPos>    fk_solver_2;
    boost::scoped_ptr<KDL::ChainFkSolverPos>    fk_solver_3;
    boost::scoped_ptr<KDL::ChainFkSolverPos>    fk_solver_4;
    boost::scoped_ptr<KDL::ChainFkSolverPos>    fk_solver_5;
    KDL::JntArray  xdot_;

    // Input
    KDL::JntArray aux_d_;
    KDL::JntArray aux_d_Kpzero_;
    KDL::JntArray comp_d_;
    KDL::JntArray tau_d_;
    KDL::JntArray tau_PD_grav_;
    KDL::JntArray vel_control_;
    KDL::JntArray kine_control_;
    KDL::JntArray qdot_c_;
    double rotX_pos, rotY_pos, rotZ_pos, rotW_pos, rotX_des, rotY_des, rotZ_des, rotW_des;

    // Joint limits
    KDL::JntArray jnt_limits_lower_;
    KDL::JntArray jnt_limits_upper_;
    KDL::JntArray diff_to_low_;
    KDL::JntArray diff_to_upper_;
    KDL::JntArray diff_to_low_prev_;
    KDL::JntArray diff_to_upper_prev_;
    KDL::Frame obstacle_;
    std::vector<KDL::JntArray> arm_segment_states_;

    Eigen::Matrix<double, 6, 1> ex_obstacle_prev_;
    Eigen::Matrix<double, 3, 1> ex_table_prev_;
    KDL::Vector table_corner_1;
    KDL::Vector table_corner_2;
    KDL::Vector table_corner_3;
    KDL::Vector table_corner_4;
    std::vector<KDL::Vector> interpolated_table_surface_;
    std::map<int, Eigen::Matrix<double, 6, 1>> table_point_distances_;
    std::map<int, Eigen::Matrix<double, 6, 1>> table_point_distances_prev_;
    std::map<int, std::map<int, Eigen::Matrix<double, 6, 1>>> table_point_distances_arm_;
    std::map<int, std::map<int, Eigen::Matrix<double, 6, 1>>> table_point_distances_arm_prev_;
    Eigen::Matrix<double ,6, 1> tau_rep_;
    std::vector<boost::scoped_ptr<KDL::ChainJntToJacSolver>> jac_seg_solvers_;

    // gains
    KDL::JntArray Kp_, Ki_, Kd_, K_kine_;
    KDL::Frame fKine_pos_Frame_;
    KDL::Frame fKine_des_Frame_;
    KDL::Twist ex_temp_;
    Eigen::Matrix<double, 6, 1> ex_;
    KDL::Frame x_;
    KDL::FrameVel fKine_vel_Frame_;
    KDL::JntArrayVel qdot_vel_array_;

    KDL::Chain kdl_chain_1;
    KDL::Chain kdl_chain_2;
    KDL::Chain kdl_chain_3;
    KDL::Chain kdl_chain_4;
    KDL::Chain kdl_chain_5;

    // save the data
    double SaveData_[SaveDataMax];
    std::vector<double> j1_avg;
    std::vector<double> j2_avg;
    std::vector<double> j3_avg;
    std::vector<double> j4_avg;
    std::vector<double> j5_avg;
    std::vector<double> j6_avg;

    std::vector<double> j1_vel_avg;
    std::vector<double> j2_vel_avg;
    std::vector<double> j3_vel_avg;
    std::vector<double> j4_vel_avg;
    std::vector<double> j5_vel_avg;
    std::vector<double> j6_vel_avg;

    // ros publisher
    ros::Publisher pub_qd_, pub_q_, pub_e_;
    ros::Publisher pub_SaveData_;

    // ros message
    std_msgs::Float64MultiArray msg_qd_, msg_q_, msg_e_;
    std_msgs::Float64MultiArray msg_SaveData_;
};
}; // namespace arm_controllers
PLUGINLIB_EXPORT_CLASS(arm_controllers::GravityControllerReactive, controller_interface::ControllerBase)


// -*- mode: c++; indent-tabs-mode: nil; -*-
//
// Paragraph
// Copyright (c) 2016-2019 Illumina, Inc.
// All rights reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// You may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//		http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied
// See the License for the specific language governing permissions and limitations
//
//

#include "genotyping/BreakpointGenotyper.hh"
#include "common/Error.hh"
#include <algorithm>
#include <boost/math/distributions/normal.hpp>
#include <boost/math/distributions/poisson.hpp>
#include <boost/math/special_functions/binomial.hpp>
#include <cmath>
#include <limits>
#include <math.h>
#include <numeric>

using boost::math::normal_distribution;
using boost::math::poisson_distribution;
using std::map;
using std::vector;

namespace genotyping
{

/**
 * @param genotype_parameters GenotypeParameter class
 */
BreakpointGenotyper::BreakpointGenotyper(std::unique_ptr<GenotypingParameters> const& param)
    : n_alleles_(param->numAlleles())
    , ploidy_(param->ploidy())
    , coverage_test_cutoff_(param->coverageTestCutoff())
    , min_pass_gq_(param->minPassGQ())
    , min_overlap_bases_(param->minOverlapBases())
    , possible_genotypes(param->possibleGenotypes())
{
    if (param->alleleErrorRates().empty())
    {
        allele_error_rate_.push_back(param->otherAlleleErrorRate());
    }
    else
    {
        allele_error_rate_ = param->alleleErrorRates();
    }

    if (param->hetHaplotypeFractions().empty())
    {
        haplotype_read_fraction_.push_back(param->otherHetHaplotypeFraction());
    }
    else
    {
        haplotype_read_fraction_ = param->hetHaplotypeFractions();
    }

    if (!param->genotypeFractions().empty())
    {
        genotype_prior_ = param->genotypeFractions();
        for (auto& phi : genotype_prior_)
        {
            if (phi.first.size() < ploidy_)
            {
                error("Error: genotype and ploidy does not match.");
            }
            if (phi.second < 0 || phi.second > 1)
            {
                error("Error: genotype prior should be between 0~1.");
            }
            phi.second = log(phi.second);
        }
    }
};

Genotype BreakpointGenotyper::genotype(
    const BreakpointGenotyperParameter& param, const std::vector<int32_t>& read_counts_per_allele) const
{
    if (read_counts_per_allele.size() != n_alleles_)
    {
        error(
            "Error: number of read counts and alleles mismatches. %i != %i.", (int)read_counts_per_allele.size(),
            n_alleles_);
    }
    Genotype result;

    // compute adjusted depth
    const double multiplier = (param.read_length - min_overlap_bases_) / (double)param.read_length;
    assert(multiplier > 0);
    const double lambda = param.read_depth * multiplier;
    const int32_t total_num_reads = std::accumulate(read_counts_per_allele.begin(), read_counts_per_allele.end(), 0);
    if (total_num_reads == 0)
    {
        result.filters.insert("NO_READS");
        return result;
    }
    result.num_reads = total_num_reads;

    // compute GL and GT
    double best_gl = -std::numeric_limits<double>::max();

    for (const auto& igt : possible_genotypes)
    {
        const double gl = genotypeLikelihood(lambda, igt, read_counts_per_allele);
        result.gl_name.push_back(igt);
        result.gl.push_back(gl);

        // update GT if GL is better
        if (gl > best_gl)
        {
            best_gl = gl;
            result.gt = igt;
        }
    }

    // compute GQ and set filter
    double sum_gl = 0;
    for (auto l : result.gl)
    {
        sum_gl += exp(l);
    }
    double pr_gt_error = (double)1.0 - exp(best_gl) / sum_gl;
    if (pr_gt_error == 0)
    {
        result.gq = 100;
    }
    else
    {
        double gq_log10 = log10(pr_gt_error);
        if (gq_log10 < -10)
        {
            result.gq = 100;
        }
        else
        {
            result.gq = -10 * gq_log10;
        }
    }
    if (result.gq < min_pass_gq_)
    {
        result.filters.insert("GQ");
    }

    // compute allele fractions
    result.allele_fractions.resize(n_alleles_, 0.0);
    for (unsigned int al = 0; al < n_alleles_; ++al)
    {
        result.allele_fractions[al] = ((double)read_counts_per_allele[al]) / total_num_reads;
    }

    // compute coverage test p value
    double coverage_test_pvalue;
    if (param.use_poisson_depth) // use poisson test for depth (more stringent)
    {
        const poisson_distribution<> poisson_coverage_distribution(lambda);
        coverage_test_pvalue = cdf(poisson_coverage_distribution, total_num_reads);
    }
    else // use normal test for depth (default)
    {
        const normal_distribution<> normal_coverage_distribution(lambda, param.depth_sd);
        coverage_test_pvalue = cdf(normal_coverage_distribution, total_num_reads);
    }

    if (coverage_test_pvalue > 0.5)
    {
        coverage_test_pvalue = 1 - coverage_test_pvalue;
        if (coverage_test_pvalue < coverage_test_cutoff_.first)
        {
            result.filters.insert("BP_DEPTH");
        }
    }
    else
    {
        if (coverage_test_pvalue < coverage_test_cutoff_.second)
        {
            result.filters.insert("BP_DEPTH");
        }
    }
    result.coverage_test_pvalue = coverage_test_pvalue;

    return result;
}

/**
 * return genotype likelihood given genotype (using Poisson model with internal parameters)
 * @param lambda Poisson distribution parameter
 * @param gv Genotype vector
 * @param read_counts Read count vector for each allele
 */
double BreakpointGenotyper::genotypeLikelihood(
    double lambda, const GenotypeVector& gv, const vector<int32_t>& read_counts) const
{
    double log_phi;
    auto it = genotype_prior_.find(gv);
    if (it == genotype_prior_.end())
    {
        log_phi = 0;
    }
    else
    {
        log_phi = it->second;
    }

    // compute how many copies of each allele we have in this GT
    vector<int> allele_ploidy;
    allele_ploidy.resize(n_alleles_, 0);
    for (unsigned int al = 0; al < n_alleles_; ++al)
    {
        for (const auto g : gv)
        {
            if (al == g)
            {
                ++allele_ploidy[al];
            }
        }
    }

    // compute GL by summing all allele contributions
    double gl = log_phi;
    for (unsigned int al = 0; al < n_alleles_; ++al)
    {
        if (allele_ploidy[al] == 0)
        {
            // no copies -> all reads supporting this allele will be errors
            const double eps = (allele_error_rate_.size() == 1 ? allele_error_rate_[0] : allele_error_rate_[al]);
            const poisson_distribution<> error_distribution(lambda * eps);
            gl += log(pdf(error_distribution, read_counts[al]));
        }
        else
        {
            const double mu
                = (haplotype_read_fraction_.size() == 1 ? haplotype_read_fraction_[0] : haplotype_read_fraction_[al]);
            const double mu_with_ploidy = allele_ploidy[al] * mu;

            const poisson_distribution<> allele_count_distribution(lambda * mu_with_ploidy);
            gl += log(pdf(allele_count_distribution, read_counts[al]));
        }
        if (std::isinf(gl))
        {
            return -std::numeric_limits<double>::max();
        }
    }

    return gl;
}
}


#include <string>
#include <sstream>
#include <iostream>
#include <vector>
#include <list>
#include <math.h>
#include <vtkSmartPointer.h>
#include <vtkPolyDataReader.h>
#include <vtkPolyDataWriter.h>
#include <vtkPolyData.h>
#include <vtkCellData.h>
#include <vtkPointData.h>
#include <vtkCell.h>
#include <vtkDoubleArray.h>
#include <vtkKdTree.h>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <omp.h>
//#include "HelperFunctions.h"

using namespace std;
//using namespace OPS;

typedef Eigen::Vector3d Vector3d;

//! A struct to store a vtkIdType and an angle
struct neighbors{
    vtkIdType _id;
    double_t _angle;
    neighbors(vtkIdType i, double_t a):_id(i), _angle(a){}
    bool operator <(const neighbors &n) const{
	return _angle < n._angle;
    }
};

int main(int argc, char* argv[])
{
    if( argc != 5 ) {
	cout << "argc = " << argc << endl
	    << "Usage: dualMesh baseName numStart numEnd numOPENMPThreads"
	    << endl;
	return(0);
    }

    ////////////////////////////////////////////////////////////////////
    // Input section
    ////////////////////////////////////////////////////////////////////

    // read in vtk file
    string baseFileName = argv[1];
    size_t numStart = stoi(argv[2]);
    size_t numEnd = stoi(argv[3]);
    size_t numThreads = stoi(argv[4]);

    //Set the number of threads
    omp_set_num_threads(numThreads);

#pragma omp parallel for
    for(size_t bigI=numStart; bigI <= numEnd; bigI++){
	stringstream sstm;
	string inputFileName, outFileName;
	sstm << baseFileName << "-" << bigI <<".vtk";
	inputFileName = sstm.str();
	sstm.str("");
	sstm.clear();

	auto reader = vtkSmartPointer<vtkPolyDataReader>::New();
	reader->SetFileName( inputFileName.c_str() );
	reader->Update();
	vtkSmartPointer<vtkPolyData> inputMesh = reader->GetOutput();
	inputMesh->BuildLinks();
	size_t npts = inputMesh->GetNumberOfPoints();

	//get displacement vectors, if they exist
	string vectorName="displacements";
	vtkSmartPointer<vtkDoubleArray> displacements =
	    vtkDoubleArray::SafeDownCast(inputMesh->GetPointData()->
		    GetVectors(vectorName.c_str()));

	// get vertex positions
	std::vector< Vector3d > points( npts, Vector3d::Zero() );

	// Calculate centroid of each triangle while updating points vector
	auto newPts = vtkSmartPointer<vtkPoints>::New();
	vtkSmartPointer<vtkCellArray> cells = inputMesh->GetPolys();
	auto cellPointIds = vtkSmartPointer<vtkIdList>::New();
	cells->InitTraversal();
	while( cells->GetNextCell( cellPointIds ) ){
	    size_t numCellPoints = cellPointIds->GetNumberOfIds();
	    Vector3d centroid(0.0,0.0,0.0);
	    for(size_t i=0; i < numCellPoints; i++){
		vtkIdType currCellPoint = cellPointIds->GetId(i);
		inputMesh->GetPoint( currCellPoint, &points[currCellPoint][0] );
		if( displacements.GetPointer() != NULL){
		    Vector3d currDisp(0.0,0.0,0.0);
		    displacements->GetTuple( currCellPoint, &currDisp[0] );
		    points[currCellPoint] += currDisp;
		}
		centroid += points[currCellPoint];
	    }
	    centroid /= numCellPoints;
	    newPts->InsertNextPoint( &centroid[0] );
	}

	// Prepare valence Cell Data array
	auto valence = vtkSmartPointer<vtkIntArray>::New();
	valence->SetName("Valence");
	valence->SetNumberOfComponents(1);

	// Prepare new cell array for polygons
	auto newPolys = vtkSmartPointer<vtkCellArray>::New();

	for(size_t a=0; a < npts; a++) {

	    auto currPolyPtIds = vtkSmartPointer<vtkIdList>::New();
	    std::list< neighbors > currPoly;
	    Vector3d vec0, vecj, currCross, axis, centroid(0.0,0.0,0.0);
	    double vec0_norm, vecj_norm, sign, currSin, currCos, currAngle;

	    inputMesh->GetPointCells( a, currPolyPtIds );
	    size_t numCellPoints = currPolyPtIds->GetNumberOfIds();

	    // Get coordinates of first cell's centroid
	    vtkIdType currId = currPolyPtIds->GetId(0);
	    newPts->GetPoint( currId, &centroid[0] );
	    vec0 = (centroid - points[a]).normalized();
	    neighbors pt0(currId, 0.0);
	    currPoly.push_back( pt0 );

	    // For remaining centroids
	    for(auto j=1; j < numCellPoints; j++){
		currId = currPolyPtIds->GetId(j);
		newPts->GetPoint( currId, &centroid[0] );
		vecj = (centroid - points[a]).normalized();
		currSin = (vec0.cross( vecj )).norm();
		axis = (vec0.cross( vecj )).normalized();
		sign = axis.dot( points[a] );
		currSin = (sign > 0.0) ? currSin : -1.0 * currSin;
		currCos = vec0.dot( vecj );
		currAngle = (180 / M_PI) * atan2(currSin, currCos);
		currAngle = (currAngle < 0) ? (360 + currAngle) : currAngle;
		neighbors ptj(currId, currAngle);
		currPoly.push_back(ptj);
	    }

	    //Sort the list of neigbors and make a polygon
	    currPoly.sort();
	    newPolys->InsertNextCell( numCellPoints );
	    for(auto t = currPoly.begin(); t != currPoly.end(); ++t){
		neighbors n = *t;
		newPolys->InsertCellPoint( n._id );
	    }
	    valence->InsertNextTuple1( numCellPoints );
	}

	//Assign points and polygons to a new polydata and write it out
	auto newPolyData = vtkSmartPointer<vtkPolyData>::New();
	auto writer = vtkSmartPointer<vtkPolyDataWriter>::New();

	newPolyData->SetPoints( newPts );
	newPolyData->SetPolys( newPolys );
	newPolyData->GetCellData()->AddArray( valence );
	writer->SetInputData( newPolyData );
	sstm << baseFileName << "-dual-" << bigI <<".vtk";
	outFileName = sstm.str();
	sstm.str("");
	sstm.clear();
	writer->SetFileName(outFileName.c_str());
	writer->Write();
    }
    return 0;
}