/*
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2019, Locus Robotics
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * 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.
 *   * 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 <fuse_constraints/marginal_constraint.h>
#include <fuse_constraints/marginalize_variables.h>
#include <fuse_constraints/uuid_ordering.h>
#include <fuse_constraints/variable_constraints.h>
#include <fuse_core/uuid.h>

#include <boost/iterator/transform_iterator.hpp>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <suitesparse/ccolamd.h>

#include <algorithm>
#include <iterator>
#include <numeric>
#include <string>
#include <unordered_map>
#include <unordered_set>
#include <vector>

namespace fuse_constraints
{

UuidOrdering computeEliminationOrder(
  const std::vector<fuse_core::UUID>& marginalized_variables,
  const fuse_core::Graph& graph)
{
  // COLAMD wants a somewhat weird structure
  // Variables are numbered sequentially in some arbitrary order. We call this the "variable index" order.
  // Similarly, the Constraints are numbered sequentially. We call this the "constraint index" order.
  // 'A' contains the constraint index for each connected constraint to a specific variable. The connected
  //     constraints are added to 'A' in variable index order.
  // 'p' contains the boundary indices for each variable in 'A'. So variable #1 spans entries
  //     from A[p[0]] to A[p[1] - 1], and variable #2 is the range A[p[1]] to A[p[2] - 1], etc.
  // In order to compute A and p efficiently, we first construct a VariableConstraints object
  // We will construct sequential indices for the variables and constraints while we populate the VariableConstraints
  // object.
  auto variable_order = UuidOrdering();
  auto constraint_order = UuidOrdering();
  auto variable_constraints = VariableConstraints();
  for (const auto& variable_uuid : marginalized_variables)
  {
    // Get all connected constraints to this variable
    auto constraints = graph.getConnectedConstraints(variable_uuid);

    // Add each constraint to the VariableConstraints object
    // New constraint and variable indices are automatically generated
    for (const auto& constraint : constraints)
    {
      unsigned int constraint_index = constraint_order[constraint.uuid()];
      for (const auto& variable_uuid : constraint.variables())
      {
        variable_constraints.insert(constraint_index, variable_order[variable_uuid]);
      }
    }
  }

  // Construct the CCOLAMD input structures
  auto recommended_size = ccolamd_recommended(
    variable_constraints.size(),
    constraint_order.size(),
    variable_order.size());
  auto A = std::vector<int>(recommended_size);
  auto p = std::vector<int>(variable_order.size() + 1);

  // Use the VariableConstraints table to construct the A and p structures
  auto A_iter = A.begin();
  auto p_iter = p.begin();
  *p_iter = 0;
  ++p_iter;
  for (unsigned int variable_index = 0u; variable_index < variable_order.size(); ++variable_index)
  {
    variable_constraints.getConstraints(variable_index, A_iter);
    *p_iter = std::distance(A.begin(), A_iter);
    ++p_iter;
  }

  // Define the variable groups used by CCOLAMD. All of the marginalized variables should be group0, all the
  // rest should be group1.
  std::vector<int> variable_groups(variable_order.size(), 1);  // Default all variables to group1
  for (const auto& variable_uuid : marginalized_variables)
  {
    // Reassign the marginalized variables to group0
    variable_groups[variable_order[variable_uuid]] = 0;
  }

  // Create some additional CCOLAMD required structures
  double knobs[CCOLAMD_KNOBS];
  ccolamd_set_defaults(knobs);
  int stats[CCOLAMD_STATS];

  // Finally call CCOLAMD
  auto success = ccolamd(
    constraint_order.size(),
    variable_order.size(),
    recommended_size,
    A.data(),
    p.data(),
    knobs,
    stats,
    variable_groups.data());
  if (!success)
  {
    throw std::runtime_error("Failed to call CCOLAMD to generate the elimination order.");
  }

  // Extract the elimination order from CCOLAMD.
  // CCOLAMD returns the elimination order by updating the values stored in p with the variable index
  // Remember that p is larger than variable_order.size()
  auto elimination_order = UuidOrdering();
  for (size_t i = 0ul; i < variable_order.size(); ++i)
  {
    elimination_order.push_back(variable_order[p[i]]);
  }

  return elimination_order;
}

fuse_core::Transaction marginalizeVariables(
  const std::string& source,
  const std::vector<fuse_core::UUID>& marginalized_variables,
  const fuse_core::Graph& graph)
{
  return marginalizeVariables(
    source,
    marginalized_variables,
    graph,
    computeEliminationOrder(marginalized_variables, graph));
}

fuse_core::Transaction marginalizeVariables(
  const std::string& source,
  const std::vector<fuse_core::UUID>& marginalized_variables,
  const fuse_core::Graph& graph,
  const fuse_constraints::UuidOrdering& elimination_order)
{
  assert(std::all_of(marginalized_variables.begin(),
                     marginalized_variables.end(),
                     [&elimination_order, &marginalized_variables](const fuse_core::UUID& variable_uuid)
                     {
                       return elimination_order.exists(variable_uuid) &&
                              elimination_order.at(variable_uuid) < marginalized_variables.size();
                     }));  // NOLINT

  fuse_core::Transaction transaction;

  // Mark all of the marginalized variables for removal
  for (const auto& variable_uuid : marginalized_variables)
  {
    transaction.removeVariable(variable_uuid);
  }

  // Copy the elimination order so we can add additional variables if needed
  auto variable_order = elimination_order;

  // Linearize all involved constraints, and store them with the variable where they will be used
  auto used_constraints = std::unordered_set<fuse_core::UUID, fuse_core::uuid::hash>();
  std::vector<std::vector<detail::LinearTerm>> linear_terms(variable_order.size());
  for (size_t i = 0ul; i < marginalized_variables.size(); ++i)
  {
    auto constraints = graph.getConnectedConstraints(variable_order[i]);
    for (const auto& constraint : constraints)
    {
      if (used_constraints.find(constraint.uuid()) == used_constraints.end())
      {
        used_constraints.insert(constraint.uuid());
        // Ensure all connected variables are added to the ordering
        for (const auto& variable_uuid : constraint.variables())
        {
          variable_order.push_back(variable_uuid);
        }
        // Add the linearized constraint to the lowest-ordered connected variable
        linear_terms[i].push_back(detail::linearize(constraint, graph, variable_order));
        // And mark the constraint for removal from the graph
        transaction.removeConstraint(constraint.uuid());
      }
    }
  }

  // Expand the linear_terms to include all the connected variables as well
  // During the marginalize process, marginal variables may be associated with these higher-ordered variables
  linear_terms.resize(variable_order.size());

  // Use the linearized constraints to marginalize each variable in order
  // Place the resulting marginal in the linear constraint bucket associated with the lowest-ordered remaining variable
  for (size_t i = 0ul; i < marginalized_variables.size(); ++i)
  {
    auto linear_marginal = detail::marginalizeNext(linear_terms[i]);
    if (!linear_marginal.variables.empty())
    {
      auto lowest_ordered_variable = linear_marginal.variables.front();
      linear_terms[lowest_ordered_variable].push_back(std::move(linear_marginal));
    }
  }

  // Convert all remaining linear marginals into marginal constraints
  for (size_t i = marginalized_variables.size(); i < linear_terms.size(); ++i)
  {
    for (const auto& linear_term : linear_terms[i])
    {
      auto marginal_constraint = detail::createMarginalConstraint(source, linear_term, graph, variable_order);
      transaction.addConstraint(std::move(marginal_constraint));
    }
  }

  return transaction;
}

namespace detail
{
// TODO(swilliams) There are more graph lookups of each Variable than needed. Refactor so that each Variable is only
//                 accessed once. This will mean storing the current variable value and local parameterization in
//                 the LinearTerm.

/**
 * In order for the linearize function to work correctly, it must perform the same operations as Google Ceres-Solver.
 * Unfortunately those functions are not callable from the public API, so we must replicate them here. The following
 * function is not identical to the Ceres-Solver code, but it was referenced heavily during the creation of this
 * function. As such, I'd like to acknowledge the original authors.
 *
 * Ceres Solver - A fast non-linear least squares minimizer
 * http://ceres-solver.org/
 * Author: keir@google.com (Keir Mierle)
 *         sameeragarwal@google.com (Sameer Agarwal)
 *
 *  - https://github.com/ceres-solver/ceres-solver/blob/master/internal/ceres/residual_block.cc
 *  - https://github.com/ceres-solver/ceres-solver/blob/master/internal/ceres/corrector.cc
 */
LinearTerm linearize(
  const fuse_core::Constraint& constraint,
  const fuse_core::Graph& graph,
  const UuidOrdering& elimination_order)
{
  LinearTerm result;

  // Generate the cost function from the input constraint
  auto cost_function = constraint.costFunction();
  size_t row_count = cost_function->num_residuals();

  // Loop over the constraint's variables and do several things:
  // * Generate a vector of variable value pointers. This is needed for the Ceres API.
  // * Allocate a matrix for each jacobian block. We will have Ceres populate the matrix.
  // * Generate a vector of jacobian pointers. This is needed for the Ceres API.
  const auto& variable_uuids = constraint.variables();
  const size_t variable_count = variable_uuids.size();
  std::vector<const double*> variable_values;
  variable_values.reserve(variable_count);
  std::vector<double*> jacobians;
  jacobians.reserve(variable_count);
  result.variables.reserve(variable_count);
  result.A.reserve(variable_count);
  for (const auto& variable_uuid : variable_uuids)
  {
    const auto& variable = graph.getVariable(variable_uuid);
    variable_values.push_back(variable.data());
    result.variables.push_back(elimination_order.at(variable_uuid));
    result.A.push_back(fuse_core::MatrixXd(row_count, variable.size()));
    jacobians.push_back(result.A.back().data());
  }
  result.b = fuse_core::VectorXd(row_count);

  // Evaluate the cost function, populating the A matrices and b vector
  bool success = cost_function->Evaluate(variable_values.data(), result.b.data(), jacobians.data());
  delete cost_function;
  success = success && result.b.array().isFinite().all();
  for (const auto& A : result.A)
  {
    success = success && A.array().isFinite().all();
  }
  if (!success)
  {
    throw std::runtime_error("Error in evaluating the cost function. There are two possible reasons. "
                             "Either the CostFunction did not evaluate and fill all residual and jacobians "
                             "that were requested or there was a non-finite value (nan/infinite) generated "
                             "during the jacobian computation.");
  }

  // Update the jacobians with the local parameterizations.
  for (size_t index = 0ul; index < variable_count; ++index)
  {
    const auto& variable_uuid = variable_uuids[index];
    const auto& variable = graph.getVariable(variable_uuid);
    auto local_parameterization = variable.localParameterization();
    if (local_parameterization)
    {
      auto& jacobian = result.A[index];
      fuse_core::MatrixXd J(local_parameterization->GlobalSize(), local_parameterization->LocalSize());
      local_parameterization->ComputeJacobian(variable_values[index], J.data());
      delete local_parameterization;
      jacobian *= J;
    }
  }

  // Correct A and b for the effects of the loss function
  auto loss_function = constraint.lossFunction();
  if (loss_function)
  {
    double squared_norm = result.b.squaredNorm();
    double rho[3];
    loss_function->Evaluate(squared_norm, rho);
    delete loss_function;
    double sqrt_rho1 = std::sqrt(rho[1]);
    double alpha = 0.0;
    if ((squared_norm > 0.0) && (rho[2] > 0.0))
    {
      const double D = 1.0 + 2.0 * squared_norm * rho[2] / rho[1];
      alpha = 1.0 - std::sqrt(D);
    }

    // Correct the Jacobians
    for (auto& jacobian : result.A)
    {
      if (alpha == 0.0)
      {
        jacobian *= sqrt_rho1;
      }
      else
      {
        // TODO(swilliams) This may be inefficient, at least according to notes in the Ceres codebase.
        jacobian = sqrt_rho1 * (jacobian - (alpha / squared_norm) * result.b * (result.b.transpose() * jacobian));
      }
    }

    // Correct the residuals
    result.b *= sqrt_rho1 / (1 - alpha);
  }

  return result;
}

LinearTerm marginalizeNext(const std::vector<LinearTerm>& linear_terms)
{
  // We need to create a dense matrix from all of the provided linear terms, and that matrix must order the variables
  // in the proper elimination order. The LinearTerms have the elimination order baked into the variable indices, but
  // since not all variables are necessarily present, we need to remove any gaps from the variable indices.
  // We use vector operations instead of a std::set because the number of variables is assumed to be small. You need
  // 1000s of variables before the std::set outperforms the std::vector.
  auto dense_to_index = std::vector<unsigned int>();
  for (const auto& linear_term : linear_terms)
  {
    std::copy(linear_term.variables.begin(), linear_term.variables.end(), std::back_inserter(dense_to_index));
  }
  std::sort(dense_to_index.begin(), dense_to_index.end());
  dense_to_index.erase(std::unique(dense_to_index.begin(), dense_to_index.end()), dense_to_index.end());

  // Construct the inverse mapping
  auto index_to_dense = std::vector<unsigned int>(dense_to_index.back() + 1, 0);
  for (size_t dense = 0ul; dense < dense_to_index.size(); ++dense)
  {
    index_to_dense[dense_to_index[dense]] = dense;
  }

  // Compute the row offsets
  auto row_offsets = std::vector<unsigned int>();
  row_offsets.reserve(linear_terms.size() + 1ul);
  row_offsets.push_back(0u);
  for (const auto& linear_term : linear_terms)
  {
    row_offsets.push_back(row_offsets.back() + linear_term.b.rows());
  }

  // Compute the column offsets
  auto index_to_cols = std::vector<unsigned int>(dense_to_index.back() + 1u, 0u);
  for (const auto& linear_term : linear_terms)
  {
    for (size_t i = 0ul; i < linear_term.variables.size(); ++i)
    {
      auto index = linear_term.variables[i];
      index_to_cols[index] = linear_term.A[i].cols();
    }
  }

  auto column_offsets = std::vector<unsigned int>();
  column_offsets.reserve(dense_to_index.size() + 1ul);
  column_offsets.push_back(0u);
  for (size_t dense = 0; dense < dense_to_index.size(); ++dense)
  {
    column_offsets.push_back(column_offsets.back() + index_to_cols[dense_to_index[dense]]);
  }

  // Construct the Ab matrix
  fuse_core::MatrixXd Ab = fuse_core::MatrixXd::Zero(row_offsets.back(), column_offsets.back() + 1u);
  for (size_t term_index = 0ul; term_index < linear_terms.size(); ++term_index)
  {
    const auto& linear_term = linear_terms[term_index];
    auto row_offset = row_offsets[term_index];
    for (size_t i = 0ul; i < linear_term.variables.size(); ++i)
    {
      const auto& A = linear_term.A[i];
      auto dense = index_to_dense[linear_term.variables[i]];
      auto column_offset = column_offsets[dense];
      for (int row = 0; row < A.rows(); ++row)
      {
        for (int col = 0; col < A.cols(); ++col)
        {
          Ab(row_offset + row, column_offset + col) = A(row, col);
        }
      }
    }
    const auto& b = linear_term.b;
    int column_offset = column_offsets.back();
    for (int row = 0; row < b.rows(); ++row)
    {
      Ab(row_offset + row, column_offset) = b(row);
    }
  }

  // Compute the QR decomposition
  // I really want to do this "in place" instead of making a copy into the Eigen QR object and a second copy back out,
  // but Eigen does not make it easy.
  // https://eigen.tuxfamily.org/dox/HouseholderQR_8h_source.html Line 379 HouseholderQR<MatrixType>::computeInPlace()
  {
    using MatrixType = fuse_core::MatrixXd;
    using HCoeffsType = Eigen::internal::plain_diag_type<MatrixType>::type;
    using RowVectorType = Eigen::internal::plain_row_type<MatrixType>::type;
    auto rows = Ab.rows();
    auto cols = Ab.cols();
    auto size = std::min(rows, cols);
    auto hCoeffs = HCoeffsType(size);
    auto temp = RowVectorType(cols);
    Eigen::internal::householder_qr_inplace_blocked<MatrixType, HCoeffsType>::run(Ab, hCoeffs, 48, temp.data());
    Ab.triangularView<Eigen::StrictlyLower>().setZero();  // Zero out the below-diagonal elements
  }

  // Extract the marginal term from R (new stored in Ab)
  // The first row block is the conditional term for the marginalized variable: P(x | y, z, ...)
  // The remaining rows are the marginal on the remaining variables: P(y, z, ...)
  auto min_row = column_offsets[1];
  // However, depending on the input, not all rows may be usable.
  auto max_row = std::min(Ab.rows(), Ab.cols() - 1);  // -1 for the included b vector
  auto marginal_rows = max_row - min_row;
  auto marginal_term = LinearTerm();
  if (marginal_rows > 0)
  {
    auto variable_count = dense_to_index.size() - 1;
    marginal_term.variables.reserve(variable_count);
    marginal_term.A.reserve(variable_count);
    for (size_t dense = 1ul; dense < dense_to_index.size(); ++dense)  // Skipping the marginalized variable
    {
      auto index = dense_to_index[dense];
      marginal_term.variables.push_back(index);
      fuse_core::MatrixXd A = fuse_core::MatrixXd::Zero(marginal_rows, index_to_cols[index]);
      auto column_offset = column_offsets[dense];
      for (int row = 0; row < A.rows(); ++row)
      {
        for (int col = 0; col < A.cols(); ++col)
        {
          A(row, col) = Ab(min_row + row, column_offset + col);
        }
      }
      marginal_term.A.push_back(std::move(A));
    }
    marginal_term.b = fuse_core::VectorXd::Zero(marginal_rows);
    auto column_offset = column_offsets.back();
    for (int row = 0; row < marginal_term.b.rows(); ++row)
    {
      marginal_term.b(row) = Ab(min_row + row, column_offset);
    }
  }
  return marginal_term;
}

MarginalConstraint::SharedPtr createMarginalConstraint(
  const std::string& source,
  const LinearTerm& linear_term,
  const fuse_core::Graph& graph,
  const UuidOrdering& elimination_order)
{
  auto index_to_variable = [&graph, &elimination_order](const unsigned int index) -> const fuse_core::Variable&
  {
    return graph.getVariable(elimination_order.at(index));
  };

  return MarginalConstraint::make_shared(
    source,
    boost::make_transform_iterator(linear_term.variables.begin(), index_to_variable),
    boost::make_transform_iterator(linear_term.variables.end(), index_to_variable),
    linear_term.A.begin(),
    linear_term.A.end(),
    linear_term.b);
}

}  // namespace detail

}  // namespace fuse_constraints


#define BOOST_TEST_MAIN

#include <boost/test/unit_test.hpp>

#include <crocore/BuddyPool.hpp>
#include <unordered_map>

static inline bool is_pow_2(size_t v)
{
    return !(v & (v - 1));
}

static inline size_t next_pow_2(size_t v)
{
    if(is_pow_2(v)){ return v; }
    v--;
    v |= v >> 1U;
    v |= v >> 2U;
    v |= v >> 4U;
    v |= v >> 8U;
    v |= v >> 16U;
    v |= v >> 32U;
    v++;
    return v;
}

BOOST_AUTO_TEST_CASE(Constructors)
{
    constexpr size_t numBytes_128Mb = 1U << 27U;

    // no pre-allocation
    {
        crocore::BuddyPool::create_info_t createInfo;

        // 128MB - something
        createInfo.block_size = numBytes_128Mb - 12345;
        createInfo.min_block_size = 512;

        // check for existence of default allocator/de-allocator (malloc/free)
        BOOST_CHECK(createInfo.alloc_fn);
        BOOST_CHECK(createInfo.dealloc_fn);

        auto pool = crocore::BuddyPool::create(createInfo);

        auto pool_state = pool->pool_state();

        // nothing was pre-allocated
        BOOST_CHECK_EQUAL(pool_state.num_blocks, 0);

        // we have passed a non pow2 -> check for correct rounding to next pow2
        BOOST_CHECK_EQUAL(pool_state.block_size, numBytes_128Mb);

        // maximum height of binary tree
        BOOST_CHECK_EQUAL(pool_state.max_level, 18);
    }

    // with pre-allocation
    {
        constexpr size_t minNumBlocks = 2;

        crocore::BuddyPool::create_info_t createInfo = {};

        // 128MB - something
        createInfo.block_size = numBytes_128Mb - 54321;
        createInfo.min_block_size = 2048;

        // request a minimum of blocks to be around -> pre-allocation
        createInfo.min_num_blocks = minNumBlocks;

        auto pool = crocore::BuddyPool::create(createInfo);

        auto pool_state = pool->pool_state();

        // check for pre-allocated blocks
        BOOST_CHECK_EQUAL(pool_state.num_blocks, minNumBlocks);

        // we have passed a non pow2 -> check for correct rounding to next pow2
        BOOST_CHECK_EQUAL(pool_state.block_size, numBytes_128Mb);

        // maximum height of binary tree
        BOOST_CHECK_EQUAL(pool_state.max_level, 16);
    }
}

BOOST_AUTO_TEST_CASE(Allocations)
{
    constexpr size_t numBytes_128Mb = 1U << 27U;
    constexpr size_t numBytes_1Mb = 1U << 20U;

    crocore::BuddyPool::create_info_t fmt;

    // 128MB
    fmt.block_size = numBytes_128Mb;
    fmt.min_block_size = 512;

    // preallocate one block
    fmt.min_num_blocks = 0;

    // no automatic deallocation
    fmt.dealloc_unused_blocks = false;

    auto pool = crocore::BuddyPool::create(fmt);

    // allocate entire toplevel block
    void *ptr1 = pool->allocate(numBytes_128Mb);
    BOOST_CHECK_NE(ptr1, nullptr);

    // free pointer
    pool->free(ptr1);

    // 1MB allocation
    void *ptr2 = pool->allocate(numBytes_1Mb);

    // allocation succeeded
    BOOST_CHECK_NE(ptr2, nullptr);

    // should have been assigned the same offset like before
    BOOST_CHECK_EQUAL(ptr1, ptr2);

    auto pool_state = pool->pool_state();

    // expect exactly one 1MB-block here
    BOOST_CHECK_EQUAL(pool_state.allocations.size(), 1);
    BOOST_CHECK_EQUAL(pool_state.allocations[numBytes_1Mb], 1);

    // free pointer
    pool->free(ptr2);

    // expect an empty pool
    pool_state = pool->pool_state();
    BOOST_CHECK_EQUAL(pool_state.allocations.size(), 0);

    // test zero byte allocation fails
    BOOST_CHECK_EQUAL(pool->allocate(0), nullptr);

    // test too large allocation fails
    BOOST_CHECK_EQUAL(pool->allocate(fmt.block_size + 1), nullptr);

    struct allocation_test_data_t
    {
        size_t num_bytes = 0;
        std::unique_ptr<uint8_t[]> data = nullptr;
    };
    std::unordered_map<void *, allocation_test_data_t> pointer_map;

    constexpr size_t num_iterations = 50;
    constexpr size_t num_allocations = 12;

    // loop and create more involved allocations and checks
    for(uint32_t i = 0; i < num_iterations; ++i)
    {
        for(uint32_t j = 0; j < num_allocations; ++j)
        {
            // pow2 from 1 kB ... 4 MB
            size_t numBytes = (1024U << j);

            // check for proper upscaling to pow2
            numBytes -= j + 3;

            // actual size will be next power of two
            size_t allocatedBytes = next_pow_2(numBytes);

            uint8_t *ptr = static_cast<uint8_t *>(pool->allocate(numBytes));
            BOOST_CHECK_NE(ptr, nullptr);

            // expect we haven't seen this pointer already
            BOOST_CHECK_EQUAL(pointer_map.count(ptr), 0);

            // fill memory with dummy values
            auto data = std::unique_ptr<uint8_t[]>(new uint8_t[numBytes]);
            for(uint32_t k = 0; k < numBytes; ++k){ ptr[k] = data[k] = static_cast<uint8_t>(rand() % 256); }
            pointer_map[ptr] = {numBytes, std::move(data)};

            pool_state = pool->pool_state();
            BOOST_CHECK_EQUAL(pool_state.allocations[allocatedBytes], i + 1);
        }
    }

    pool_state = pool->pool_state();
    BOOST_CHECK_EQUAL(pool_state.allocations.size(), num_allocations);
    size_t num_blocks_before_shrink = pool_state.num_blocks;

    // should do nothing (all toplevel-blocks still in use)
    pool->shrink();

    pool_state = pool->pool_state();
    BOOST_CHECK_EQUAL(pool_state.num_blocks, num_blocks_before_shrink);

    // sanity-check for correct content, free everything again
    for(auto &pair : pointer_map)
    {
        BOOST_CHECK_EQUAL(memcmp(pair.first, pair.second.data.get(), pair.second.num_bytes), 0);
        pool->free(pair.first);
    }

    // no allocations left, but still 2 unused toplevel-blocks
    pool_state = pool->pool_state();
    BOOST_CHECK_EQUAL(pool_state.allocations.size(), 0);
    BOOST_CHECK_EQUAL(pool_state.num_blocks, 2);

    // shrink to deallocate unused blocks
    pool->shrink();

    // expect an empty pool again
    pool_state = pool->pool_state();
    BOOST_CHECK_EQUAL(pool_state.allocations.size(), 0);
    BOOST_CHECK_EQUAL(pool_state.num_blocks, fmt.min_num_blocks);

    // double free -> violates assert|Expects
//    pool->free(ptr2);
}

BOOST_AUTO_TEST_CASE(BaseclassInterface)
{
    constexpr size_t numBytes_128Mb = 1U << 27U;

    crocore::BuddyPool::create_info_t fmt;

    // 128MB
    fmt.block_size = numBytes_128Mb;
    fmt.min_block_size = 512;

    crocore::AllocatorPtr allocator = crocore::BuddyPool::create(fmt);

    struct poo_dackel_t
    {
        int foo[512] = {};
    };

    auto* dackel = new (allocator) poo_dackel_t();
    dackel->~poo_dackel_t();
    allocator->free(dackel);
}


/* boost random/detail/seed.hpp header file
 *
 * Copyright Steven Watanabe 2009
 * Distributed under the Boost Software License, Version 1.0. (See
 * accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 *
 * See http://www.boost.org for most recent version including documentation.
 *
 * $Id$
 */

#ifndef BOOST_RANDOM_DETAIL_SEED_IMPL_HPP
#define BOOST_RANDOM_DETAIL_SEED_IMPL_HPP

#include <stdexcept>
#include <boost/cstdint.hpp>
#include <boost/throw_exception.hpp>
#include <boost/config/no_tr1/cmath.hpp>
#include <boost/integer/integer_mask.hpp>
#include <boost/integer/static_log2.hpp>
#include <boost/random/traits.hpp>
#include <boost/mpl/bool.hpp>
#include <boost/mpl/if.hpp>
#include <boost/mpl/int.hpp>
#include <boost/random/detail/const_mod.hpp>
#include <boost/random/detail/integer_log2.hpp>
#include <boost/random/detail/signed_unsigned_tools.hpp>
#include <boost/random/detail/generator_bits.hpp>

#include <boost/random/detail/disable_warnings.hpp>

namespace boost {
namespace random {
namespace detail {

// finds the seed type of an engine, given its
// result_type.  If the result_type is integral
// the seed type is the same.  If the result_type
// is floating point, the seed type is uint32_t
template<class T>
struct seed_type
{
    typedef typename boost::mpl::if_<boost::is_integral<T>,
        T,
        boost::uint32_t
    >::type type;
};

template<int N>
struct const_pow_impl
{
    template<class T>
    static T call(T arg, int n, T result)
    {
        return const_pow_impl<N / 2>::call(T(arg * arg), n / 2,
            n%2 == 0? result : T(result * arg));
    }
};

template<>
struct const_pow_impl<0>
{
    template<class T>
    static T call(T, int, T result)
    {
        return result;
    }
};

// requires N is an upper bound on n
template<int N, class T>
inline T const_pow(T arg, int n) { return const_pow_impl<N>::call(arg, n, T(1)); }

template<class T>
inline T pow2(int n)
{
    typedef unsigned int_type;
    const int max_bits = std::numeric_limits<int_type>::digits;
    T multiplier = T(int_type(1) << (max_bits - 1)) * 2;
    return (int_type(1) << (n % max_bits)) *
        const_pow<std::numeric_limits<T>::digits / max_bits>(multiplier, n / max_bits);
}

template<class Engine, class Iter>
void generate_from_real(Engine& eng, Iter begin, Iter end)
{
    using std::fmod;
    typedef typename Engine::result_type RealType;
    const int Bits = detail::generator_bits<Engine>::value();
    int remaining_bits = 0;
    boost::uint_least32_t saved_bits = 0;
    RealType multiplier = pow2<RealType>( Bits);
    RealType mult32 = RealType(4294967296.0); // 2^32
    while(true) {
        RealType val = eng() * multiplier;
        int available_bits = Bits;
        // Make sure the compiler can optimize this out
        // if it isn't possible.
        if(Bits < 32 && available_bits < 32 - remaining_bits) {
            saved_bits |= boost::uint_least32_t(val) << remaining_bits;
            remaining_bits += Bits;
        } else {
            // If Bits < 32, then remaining_bits != 0, since
            // if remaining_bits == 0, available_bits < 32 - 0,
            // and we won't get here to begin with.
            if(Bits < 32 || remaining_bits != 0) {
                boost::uint_least32_t divisor =
                    (boost::uint_least32_t(1) << (32 - remaining_bits));
                boost::uint_least32_t extra_bits = boost::uint_least32_t(fmod(val, mult32)) & (divisor - 1);
                val = val / divisor;
                *begin++ = saved_bits | (extra_bits << remaining_bits);
                if(begin == end) return;
                available_bits -= 32 - remaining_bits;
                remaining_bits = 0;
            }
            // If Bits < 32 we should never enter this loop
            if(Bits >= 32) {
                for(; available_bits >= 32; available_bits -= 32) {
                    boost::uint_least32_t word = boost::uint_least32_t(fmod(val, mult32));
                    val /= mult32;
                    *begin++ = word;
                    if(begin == end) return;
                }
            }
            remaining_bits = available_bits;
            saved_bits = static_cast<boost::uint_least32_t>(val);
        }
    }
}

template<class Engine, class Iter>
void generate_from_int(Engine& eng, Iter begin, Iter end)
{
    typedef typename Engine::result_type IntType;
    typedef typename boost::random::traits::make_unsigned<IntType>::type unsigned_type;
    int remaining_bits = 0;
    boost::uint_least32_t saved_bits = 0;
    unsigned_type range = boost::random::detail::subtract<IntType>()((eng.max)(), (eng.min)());

    int bits =
        (range == (std::numeric_limits<unsigned_type>::max)()) ?
            std::numeric_limits<unsigned_type>::digits :
            detail::integer_log2(range + 1);

    {
        int discarded_bits = detail::integer_log2(bits);
        unsigned_type excess = (range + 1) >> (bits - discarded_bits);
        if(excess != 0) {
            int extra_bits = detail::integer_log2((excess - 1) ^ excess);
            bits = bits - discarded_bits + extra_bits;
        }
    }

    unsigned_type mask = (static_cast<unsigned_type>(2) << (bits - 1)) - 1;
    unsigned_type limit = ((range + 1) & ~mask) - 1;

    while(true) {
        unsigned_type val;
        do {
            val = boost::random::detail::subtract<IntType>()(eng(), (eng.min)());
        } while(limit != range && val > limit);
        val &= mask;
        int available_bits = bits;
        if(available_bits == 32) {
            *begin++ = static_cast<boost::uint_least32_t>(val) & 0xFFFFFFFFu;
            if(begin == end) return;
        } else if(available_bits % 32 == 0) {
            for(int i = 0; i < available_bits / 32; ++i) {
                boost::uint_least32_t word = boost::uint_least32_t(val) & 0xFFFFFFFFu;
                int suppress_warning = (bits >= 32);
                BOOST_ASSERT(suppress_warning == 1);
                val >>= (32 * suppress_warning);
                *begin++ = word;
                if(begin == end) return;
            }
        } else if(bits < 32 && available_bits < 32 - remaining_bits) {
            saved_bits |= boost::uint_least32_t(val) << remaining_bits;
            remaining_bits += bits;
        } else {
            if(bits < 32 || remaining_bits != 0) {
                boost::uint_least32_t extra_bits = boost::uint_least32_t(val) & ((boost::uint_least32_t(1) << (32 - remaining_bits)) - 1);
                val >>= 32 - remaining_bits;
                *begin++ = saved_bits | (extra_bits << remaining_bits);
                if(begin == end) return;
                available_bits -= 32 - remaining_bits;
                remaining_bits = 0;
            }
            if(bits >= 32) {
                for(; available_bits >= 32; available_bits -= 32) {
                    boost::uint_least32_t word = boost::uint_least32_t(val) & 0xFFFFFFFFu;
                    int suppress_warning = (bits >= 32);
                    BOOST_ASSERT(suppress_warning == 1);
                    val >>= (32 * suppress_warning);
                    *begin++ = word;
                    if(begin == end) return;
                }
            }
            remaining_bits = available_bits;
            saved_bits = static_cast<boost::uint_least32_t>(val);
        }
    }
}

template<class Engine, class Iter>
void generate_impl(Engine& eng, Iter first, Iter last, boost::mpl::true_)
{
    return detail::generate_from_int(eng, first, last);
}

template<class Engine, class Iter>
void generate_impl(Engine& eng, Iter first, Iter last, boost::mpl::false_)
{
    return detail::generate_from_real(eng, first, last);
}

template<class Engine, class Iter>
void generate(Engine& eng, Iter first, Iter last)
{
    return detail::generate_impl(eng, first, last, boost::random::traits::is_integral<typename Engine::result_type>());
}

template<class IntType, IntType m, class SeedSeq>
IntType seed_one_int(SeedSeq& seq)
{
    static const int log = ::boost::mpl::if_c<(m == 0),
        ::boost::mpl::int_<(::std::numeric_limits<IntType>::digits)>,
        ::boost::static_log2<m> >::type::value;
    static const int k =
        (log + ((~(static_cast<IntType>(2) << (log - 1)) & m)? 32 : 31)) / 32;
    ::boost::uint_least32_t array[log / 32 + 4];
    seq.generate(&array[0], &array[0] + k + 3);
    IntType s = 0;
    for(int j = 0; j < k; ++j) {
        IntType digit = const_mod<IntType, m>::apply(IntType(array[j+3]));
        IntType mult = IntType(1) << 32*j;
        s = const_mod<IntType, m>::mult_add(mult, digit, s);
    }
    return s;
}

template<class IntType, IntType m, class Iter>
IntType get_one_int(Iter& first, Iter last)
{
    static const int log = ::boost::mpl::if_c<(m == 0),
        ::boost::mpl::int_<(::std::numeric_limits<IntType>::digits)>,
        ::boost::static_log2<m> >::type::value;
    static const int k =
        (log + ((~(static_cast<IntType>(2) << (log - 1)) & m)? 32 : 31)) / 32;
    IntType s = 0;
    for(int j = 0; j < k; ++j) {
        if(first == last) {
            boost::throw_exception(::std::invalid_argument("Not enough elements in call to seed."));
        }
        IntType digit = const_mod<IntType, m>::apply(IntType(*first++));
        IntType mult = IntType(1) << 32*j;
        s = const_mod<IntType, m>::mult_add(mult, digit, s);
    }
    return s;
}

// TODO: work in-place whenever possible
template<int w, std::size_t n, class SeedSeq, class UIntType>
void seed_array_int_impl(SeedSeq& seq, UIntType (&x)[n])
{
    boost::uint_least32_t storage[((w+31)/32) * n];
    seq.generate(&storage[0], &storage[0] + ((w+31)/32) * n);
    for(std::size_t j = 0; j < n; j++) {
        UIntType val = 0;
        for(std::size_t k = 0; k < (w+31)/32; ++k) {
            val += static_cast<UIntType>(storage[(w+31)/32*j + k]) << 32*k;
        }
        x[j] = val & ::boost::low_bits_mask_t<w>::sig_bits;
    }
}

template<int w, std::size_t n, class SeedSeq, class IntType>
inline void seed_array_int_impl(SeedSeq& seq, IntType (&x)[n], boost::mpl::true_)
{
    BOOST_STATIC_ASSERT_MSG(boost::is_integral<IntType>::value, "Sorry but this routine has not been ported to non built-in integers as it relies on a reinterpret_cast.");
    typedef typename boost::make_unsigned<IntType>::type unsigned_array[n];
    seed_array_int_impl<w>(seq, reinterpret_cast<unsigned_array&>(x));
}

template<int w, std::size_t n, class SeedSeq, class IntType>
inline void seed_array_int_impl(SeedSeq& seq, IntType (&x)[n], boost::mpl::false_)
{
    seed_array_int_impl<w>(seq, x);
}

template<int w, std::size_t n, class SeedSeq, class IntType>
inline void seed_array_int(SeedSeq& seq, IntType (&x)[n])
{
    seed_array_int_impl<w>(seq, x, boost::random::traits::is_signed<IntType>());
}

template<int w, std::size_t n, class Iter, class UIntType>
void fill_array_int_impl(Iter& first, Iter last, UIntType (&x)[n])
{
    for(std::size_t j = 0; j < n; j++) {
        UIntType val = 0;
        for(std::size_t k = 0; k < (w+31)/32; ++k) {
            if(first == last) {
                boost::throw_exception(std::invalid_argument("Not enough elements in call to seed."));
            }
            val += static_cast<UIntType>(*first++) << 32*k;
        }
        x[j] = val & ::boost::low_bits_mask_t<w>::sig_bits;
    }
}

template<int w, std::size_t n, class Iter, class IntType>
inline void fill_array_int_impl(Iter& first, Iter last, IntType (&x)[n], boost::mpl::true_)
{
    BOOST_STATIC_ASSERT_MSG(boost::is_integral<IntType>::value, "Sorry but this routine has not been ported to non built-in integers as it relies on a reinterpret_cast.");
    typedef typename boost::make_unsigned<IntType>::type unsigned_array[n];
    fill_array_int_impl<w>(first, last, reinterpret_cast<unsigned_array&>(x));
}

template<int w, std::size_t n, class Iter, class IntType>
inline void fill_array_int_impl(Iter& first, Iter last, IntType (&x)[n], boost::mpl::false_)
{
    fill_array_int_impl<w>(first, last, x);
}

template<int w, std::size_t n, class Iter, class IntType>
inline void fill_array_int(Iter& first, Iter last, IntType (&x)[n])
{
    fill_array_int_impl<w>(first, last, x, boost::random::traits::is_signed<IntType>());
}

template<int w, std::size_t n, class RealType>
void seed_array_real_impl(const boost::uint_least32_t* storage, RealType (&x)[n])
{
    boost::uint_least32_t mask = ~((~boost::uint_least32_t(0)) << (w%32));
    RealType two32 = 4294967296.0;
    const RealType divisor = RealType(1)/detail::pow2<RealType>(w);
    unsigned int j;
    for(j = 0; j < n; ++j) {
        RealType val = RealType(0);
        RealType mult = divisor;
        for(int k = 0; k < w/32; ++k) {
            val += *storage++ * mult;
            mult *= two32;
        }
        if(mask != 0) {
            val += (*storage++ & mask) * mult;
        }
        BOOST_ASSERT(val >= 0);
        BOOST_ASSERT(val < 1);
        x[j] = val;
    }
}

template<int w, std::size_t n, class SeedSeq, class RealType>
void seed_array_real(SeedSeq& seq, RealType (&x)[n])
{
    using std::pow;
    boost::uint_least32_t storage[((w+31)/32) * n];
    seq.generate(&storage[0], &storage[0] + ((w+31)/32) * n);
    seed_array_real_impl<w>(storage, x);
}

template<int w, std::size_t n, class Iter, class RealType>
void fill_array_real(Iter& first, Iter last, RealType (&x)[n])
{
    boost::uint_least32_t mask = ~((~boost::uint_least32_t(0)) << (w%32));
    RealType two32 = 4294967296.0;
    const RealType divisor = RealType(1)/detail::pow2<RealType>(w);
    unsigned int j;
    for(j = 0; j < n; ++j) {
        RealType val = RealType(0);
        RealType mult = divisor;
        for(int k = 0; k < w/32; ++k, ++first) {
            if(first == last) boost::throw_exception(std::invalid_argument("Not enough elements in call to seed."));
            val += *first * mult;
            mult *= two32;
        }
        if(mask != 0) {
            if(first == last) boost::throw_exception(std::invalid_argument("Not enough elements in call to seed."));
            val += (*first & mask) * mult;
            ++first;
        }
        BOOST_ASSERT(val >= 0);
        BOOST_ASSERT(val < 1);
        x[j] = val;
    }
}

}
}
}

#include <boost/random/detail/enable_warnings.hpp>

#endif


// Copyright (c) 2013 Vasili Baranau
// Distributed under the MIT software license
// See the accompanying file License.txt or http://opensource.org/licenses/MIT

#include <stdio.h>
#include <boost/numeric/odeint.hpp>
#include "../Headers/ClosestJammingStep.h"
#include "Core/Headers/Exceptions.h"
#include "Core/Headers/Constants.h"
#include "Generation/PackingServices/DistanceServices/Headers/INeighborProvider.h"
#include "Generation/PackingServices/DistanceServices/Headers/IClosestPairProvider.h"
#include "Generation/Model/Headers/Config.h"
#include "Generation/Geometries/Headers/IGeometry.h"

using namespace PackingServices;
using namespace Core;
using namespace Model;
using namespace std;
using namespace boost::numeric::odeint;

namespace PackingGenerators
{
    ClosestJammingStep::ClosestJammingStep(GeometryService* geometryService,
            INeighborProvider* neighborProvider,
            PackingServices::IClosestPairProvider* closestPairProvider,
            MathService* mathService) :
            BasePackingStep(geometryService, neighborProvider, closestPairProvider, mathService),
            closestJammingVelocityProvider(mathService)
    {
        isOuterDiameterChanging = false;
        canOvercomeTheoreticalDensity = true;

        maxTimeStep = -1.0;
        integrationTimeStep = 1e-9;
    }

    ClosestJammingStep::~ClosestJammingStep()
    {

    }

    const FLOAT_TYPE ClosestJammingStep::GetBondThreshold() const
    {
        return bondsProvider.GetBondThreshold();
    }

    void ClosestJammingStep::SetBondThreshold(Core::FLOAT_TYPE value)
    {
        bondsProvider.SetBondThreshold(value);
    }

    void ClosestJammingStep::SetParticles(Packing* particles)
    {
        BasePackingStep::SetParticles(particles);
        ParticlePair closestPair = closestPairProvider->FindClosestPair();
        innerDiameterRatio = sqrt(closestPair.normalizedDistanceSquare);

        bondsProvider.Reset(config->particlesCount);

        particleVelocities.clear();
        particleVelocities.resize(config->particlesCount);

        bondsProvider.UpdateBonds(*neighborProvider, *mathService, *particles, innerDiameterRatio, false);
        startBondsCountForIntegrationTimeStep = bondsProvider.GetBonds().size();
    }

    void ClosestJammingStep::DisplaceParticles()
    {
        initialInnerDiameterRatio = innerDiameterRatio;
        clock_t linearSystemSolutionTime = FillVelocities();
        FLOAT_TYPE timeStep = FindBestMovementTime();

        int odeCyclesCount = 0;
        if (timeStep <= integrationTimeStep)
        {
            MoveParticles(timeStep);
            innerDiameterRatio += timeStep;
        }
        else
        {
            odeCyclesCount = DisplaceParticlesForLongTime();
        }

        // Remove intersections at each iteration
        innerDiameterRatio = sqrt(closestPairProvider->FindClosestPair().normalizedDistanceSquare);

        // Search for new contacts, update bonds and bonds per particle
        BondsProvider::Statistics statistics = bondsProvider.UpdateBonds(*neighborProvider, *mathService, *particles, innerDiameterRatio, false);
        if (statistics.gapsCount > 0)
        {
            printf("BROKEN: broken bonds count is %d, mean gap length is %g\n", statistics.gapsCount, statistics.meanGapLength);
        }
        if (statistics.intersectionsCount > 0)
        {
            printf("INTERSECTIONS: intersections count is %d, mean intersection length is %g\n", statistics.intersectionsCount, statistics.meanIntersectionLength);
        }

        printf("Bonds count: %d; bond pairs count: %d; time step: %g; slae solution time: %g (s); ode cycles count: %d; integration time step: %g\n",
                bondsProvider.GetBonds().size(), bondsProvider.GetBondPairsCount(), timeStep, linearSystemSolutionTime / static_cast<FLOAT_TYPE>(CLOCKS_PER_SEC), odeCyclesCount, integrationTimeStep);

        FixIntersections(statistics);
    }

    // TODO: merge somehow with DoBinarySearchForCollision
    int ClosestJammingStep::DisplaceParticlesForLongTime()
    {
        // OdeObserver is passed by value, that's why need to store reference to odeCyclesCount.
        int odeCyclesCount = 0;
        OdeObserver observer(&odeCyclesCount, this);

        ParticleSystemForODE odeSystem(this);
        vector<FLOAT_TYPE> combinedParticleCoordinates(DIMENSIONS * config->particlesCount);
        UpdateCombinedCoordinates(&combinedParticleCoordinates);

        FLOAT_TYPE maxTime = maxTimeStep > 0 ? innerDiameterRatio + maxTimeStep : MAX_FLOAT_VALUE;

        try
        {
            // Solve ODE to reach the nextCollisionTime
//            typedef runge_kutta_dopri5< vector<FLOAT_TYPE> > TStepper;
//            result_of::make_controlled<TStepper>::type stepper = make_controlled(bondsProvider.GetBondThreshold(), bondsProvider.GetBondThreshold(), TStepper());
//            integrate_adaptive(stepper, odeSystem, combinedParticleCoordinates, innerDiameterRatio, maxTime, integrationTimeStep, observer);

//            bulirsch_stoer< vector<FLOAT_TYPE> > stepper(bondsProvider.GetBondThreshold(), bondsProvider.GetBondThreshold());
//            integrate_adaptive(stepper, odeSystem, combinedParticleCoordinates, innerDiameterRatio, maxTime, integrationTimeStep, observer);

            typedef runge_kutta_dopri5< vector<FLOAT_TYPE> > TStepper;
            integrate_const(TStepper(), odeSystem, combinedParticleCoordinates, innerDiameterRatio, maxTime, integrationTimeStep, observer);

            // Integration may end normally only if maxTimeStep is specified and reached.
            // Observer is called even for the very last time, therefore coordinates and velocities are correctly updated.
            // In case observer is not called for the last time.
            if (maxTime != MAX_FLOAT_VALUE && maxTime > innerDiameterRatio)
            {
                observer(combinedParticleCoordinates, maxTime);
            }
        }
        catch (const CollisionMissedException& e)
        {
            printf((e.GetMessage() + "\n").c_str());
            DoBinarySearchForCollision(e.nextCollisionTimeEstimate, &observer);
        }
        catch (const CollisionIsNearException& e)
        {
            printf((e.GetMessage() + "\n").c_str());
            // All the velocities are set in the observer and are correct.
            MoveParticles(e.nextCollisionTime - innerDiameterRatio);
            innerDiameterRatio = e.nextCollisionTime;
        }
        return odeCyclesCount;
    }

    void ClosestJammingStep::DoBinarySearchForCollision(FLOAT_TYPE nextCollisionTimeEstimate, OdeObserver* observer)
    {
        printf("DoBinarySearchForCollision; innerDiameterRatio: %17.15g, nextCollisionTimeEstimate: %17.15g\n", innerDiameterRatio, nextCollisionTimeEstimate);
        if ((nextCollisionTimeEstimate - innerDiameterRatio) < integrationTimeStep)
        {
            printf("(nextCollisionTimeEstimate - innerDiameterRatio) < integrationTimeStep\n");
            MoveParticles(nextCollisionTimeEstimate - innerDiameterRatio);
            innerDiameterRatio = nextCollisionTimeEstimate;
            return;
        }

        // Do binary search for collision with support for missing collision detection in the course of integration as well.
        ParticleSystemForODE odeSystem(this);
        vector<FLOAT_TYPE> combinedParticleCoordinates(DIMENSIONS * config->particlesCount);
        UpdateCombinedCoordinates(&combinedParticleCoordinates);

        // Integrate from last valid time (always innerDiameterRatio) to half between valid and invalid times
        FLOAT_TYPE middleTime = (innerDiameterRatio + nextCollisionTimeEstimate) * 0.5;
        try
        {
            // Solve ODE to reach the middleTime
//            typedef runge_kutta_dopri5< vector<FLOAT_TYPE> > TStepper;
//            result_of::make_controlled<TStepper>::type stepper = make_controlled(bondsProvider.GetBondThreshold(), bondsProvider.GetBondThreshold(), TStepper());
//            integrate_adaptive(stepper, odeSystem, combinedParticleCoordinates, innerDiameterRatio, middleTime, integrationTimeStep, observer);

//            bulirsch_stoer< vector<FLOAT_TYPE> > stepper(bondsProvider.GetBondThreshold(), bondsProvider.GetBondThreshold());
//            integrate_adaptive(stepper, odeSystem, combinedParticleCoordinates, innerDiameterRatio, middleTime, integrationTimeStep, observer);

            typedef runge_kutta_dopri5< vector<FLOAT_TYPE> > TStepper;
            integrate_const(TStepper(), odeSystem, combinedParticleCoordinates, innerDiameterRatio, middleTime, integrationTimeStep, *observer);

            // Observer was not called for the last time
            if (middleTime > innerDiameterRatio)
            {
                (*observer)(combinedParticleCoordinates, middleTime);
            }

            printf("innerDiameterRatio after successful integration: %17.15g\n", innerDiameterRatio);
            // Integrated successfully. Observer was also called for the last step,
            // so innerDiameterRatio, particle coordinates, bonds, and particle velocities have correct values (innerDiameterRatio = middleTime),
            // collision has not happened in the middle time.
            // Last valid time is the current time, nextCollisionTimeEstimate is the same.
            DoBinarySearchForCollision(nextCollisionTimeEstimate, observer);
        }

        // If integrated not successfully:
        // a. if collision missed, it can not be too close to the last valid time (otherwise the near exception would be fired).
        catch (const CollisionMissedException& e)
        {
            // innerDiameterRatio, particle coordinates, bonds, and particle velocities have correct values (innerDiameterRatio = last time before collision)
            printf((e.GetMessage() + "\n").c_str());
            DoBinarySearchForCollision(e.nextCollisionTimeEstimate, observer);
        }
        // b. if collision is near, move particles
        catch (const CollisionIsNearException& e)
        {
            printf((e.GetMessage() + "\n").c_str());
            // All the velocities are set in the observer and are correct.
            MoveParticles(e.nextCollisionTime - innerDiameterRatio);
            innerDiameterRatio = e.nextCollisionTime;
        }
    }

    void ClosestJammingStep::FixIntersections(BondsProvider::Statistics statistics)
    {
        ParticlePair closestPair = closestPairProvider->FindClosestPair();
        const FLOAT_TYPE tolerance = 1.0 - 5.0 * bondsProvider.GetBondThreshold();
        bool intersectionsExist = closestPair.normalizedDistanceSquare < innerDiameterRatio * innerDiameterRatio * tolerance * tolerance;
        if (intersectionsExist)
        {
            printf("WARNING: min normalized distance %f is lower than inner diameter ratio %f. Particles pair: %d and %d.\n",
                    std::sqrt(closestPair.normalizedDistanceSquare),
                    innerDiameterRatio,
                    closestPair.firstParticleIndex,
                    closestPair.secondParticleIndex);
        }

        bool errorIsLarge =
                statistics.meanGapLength > bondsProvider.GetBondThreshold() * 5.0 ||
                statistics.meanIntersectionLength > bondsProvider.GetBondThreshold() * 5.0 ||
                intersectionsExist;
        if (!errorIsLarge)
        {
            return;
        }

        printf("WARNING: meanGapLength or meanIntersectionLength are too large. Updating innerDiameterRatio and bonds.\n");
        // The last bond is added with errors. The last but one is not, because otherwise bonds would be recalculated.
        int endBondsCountForIntegrationTimeStep = bondsProvider.GetBonds().size() - 1;
        innerDiameterRatio = sqrt(closestPair.normalizedDistanceSquare);
        bondsProvider.UpdateBonds(*neighborProvider, *mathService, *particles, innerDiameterRatio, true);

        UpdateIntegrationTimeStep(endBondsCountForIntegrationTimeStep);
    }

    void ClosestJammingStep::UpdateIntegrationTimeStep(int endBondsCountForIntegrationTimeStep)
    {
        FLOAT_TYPE addedBondsCount = endBondsCountForIntegrationTimeStep - startBondsCountForIntegrationTimeStep;
        bool errorGrowsTooQuickly = addedBondsCount < 10;

        startBondsCountForIntegrationTimeStep = endBondsCountForIntegrationTimeStep;

        // After division by 2 integrationTimeStep should be at least 1e-14, as final particle diameter is about 1
        const FLOAT_TYPE minIntegrationTimeStep = 2e-14;
        bool shouldUpdateIntegrationTimeStep = errorGrowsTooQuickly && integrationTimeStep > minIntegrationTimeStep;
        if (shouldUpdateIntegrationTimeStep)
        {
            integrationTimeStep *= 0.5;
            printf("WARNING: error grows too quickly. Updated integrationTimeStep to %g.\n", integrationTimeStep);
        }

        if (addedBondsCount <= 0 && integrationTimeStep <= minIntegrationTimeStep)
        {
            throw InvalidOperationException("Particles do not grow during integration, integrationTimeStep is too low to be decreased further.");
        }
    }

    clock_t ClosestJammingStep::FillVelocities()
    {
        clock_t solutionTime = closestJammingVelocityProvider.FillVelocities(*neighborProvider, bondsProvider, *particles, innerDiameterRatio, &particleVelocities);
        return solutionTime;
    }

    void ClosestJammingStep::UpdateCombinedCoordinates(vector<FLOAT_TYPE>* combinedParticleCoordinates)
    {
        vector<FLOAT_TYPE>& combinedParticleCoordinatesRef = *combinedParticleCoordinates;
        const Packing& particlesRef = *particles;
        for (ParticleIndex particleIndex = 0; particleIndex < config->particlesCount; ++particleIndex)
        {
            const DomainParticle& particle = particlesRef[particleIndex];
            for (int i = 0; i < DIMENSIONS; ++i)
            {
                combinedParticleCoordinatesRef[particleIndex * DIMENSIONS + i] = particle.coordinates[i];
            }
        }
    }

    void ClosestJammingStep::UpdateCombinedVelocities(vector<FLOAT_TYPE>* combinedVelocities)
    {
        vector<FLOAT_TYPE>& combinedVelocitiesRef = *combinedVelocities;
        for (ParticleIndex particleIndex = 0; particleIndex < config->particlesCount; ++particleIndex)
        {
            const Core::SpatialVector& velocity = particleVelocities[particleIndex];
            for (int i = 0; i < DIMENSIONS; ++i)
            {
                combinedVelocitiesRef[particleIndex * DIMENSIONS + i] = velocity[i];
            }
        }
    }

    void ClosestJammingStep::UpdateParticleCoordinates(const vector<FLOAT_TYPE>& combinedParticleCoordinates)
    {
        Model::Packing& particlesRef = *particles;
        for (ParticleIndex particleIndex = 0; particleIndex < config->particlesCount; ++particleIndex)
        {
            DomainParticle& particle = particlesRef[particleIndex];

            closestPairProvider->StartMove(particleIndex);
            neighborProvider->StartMove(particleIndex);

            for (int i = 0; i < DIMENSIONS; ++i)
            {
                particle.coordinates[i] = combinedParticleCoordinates[particleIndex * DIMENSIONS + i];
            }
            geometry->EnsureBoundaries(particle, &particle, innerDiameterRatio);

            neighborProvider->EndMove();
            closestPairProvider->EndMove();
        }
    }

    void ClosestJammingStep::ProvideODEDerivative(const vector<FLOAT_TYPE>& x, vector<FLOAT_TYPE>* dxdt, const FLOAT_TYPE t)
    {
        // copy x into particles, update innerDiameterRatio
        UpdateParticleCoordinates(x);
//        FLOAT_TYPE previousTime = innerDiameterRatio;
        innerDiameterRatio = t;

        FillVelocities();
        UpdateCombinedVelocities(dxdt);

//        ////////////////
//        // Search for new contacts, update bonds and bonds per particle
//        Nullable<FLOAT_TYPE> meanGapLength = UpdateContacts();
//
//        ParticlePair closestPair = closestPairProvider->FindClosestPair();
//        const FLOAT_TYPE tolerance = 1.0 - bondThreshold;
//        if (closestPair.normalizedDistanceSquare < innerDiameterRatio * innerDiameterRatio * tolerance * tolerance)
//        {
//            printf("ODE: WARNING: min normalized distance %f is lower than inner diameter ratio %f. Particles pair: %d and %d; bond existed: %d\n",
//                    std::sqrt(closestPair.normalizedDistanceSquare),
//                    innerDiameterRatio,
//                    closestPair.firstParticleIndex,
//                    closestPair.secondParticleIndex,
//                    ParticlesShareBond(closestPair.firstParticleIndex, closestPair.secondParticleIndex));
//        }
//
//        printf("ODE: Bonds count: %d; bond pairs count: %d; time step: %g; slae solution time: %g (s); ode delta t: %g\n",
//                bonds.size(), bondPairs.size(), timeStep, linearSystemSolutionTime / static_cast<FLOAT_TYPE>(CLOCKS_PER_SEC), t - previousTime);
//        ////////////////
    }

    FLOAT_TYPE ClosestJammingStep::FindBestMovementTime()
    {
        FLOAT_TYPE movementTime = closestJammingVelocityProvider.FindBestMovementTime();
        if (maxTimeStep > 0 && (movementTime + innerDiameterRatio - initialInnerDiameterRatio > maxTimeStep))
        {
            movementTime = initialInnerDiameterRatio + maxTimeStep - innerDiameterRatio;
        }
        return movementTime;
    }

    void ClosestJammingStep::MoveParticles(FLOAT_TYPE timeStep)
    {
        Model::Packing& particlesRef = *particles;
        for (ParticleIndex particleIndex = 0; particleIndex < config->particlesCount; ++particleIndex)
        {
            DomainParticle& particle = particlesRef[particleIndex];
            if (bondsProvider.GetBondIndexesPerParticle()[particleIndex].size() == 0)
            {
                continue;
            }

            closestPairProvider->StartMove(particleIndex);
            neighborProvider->StartMove(particleIndex);

            SpatialVector displacement;
            VectorUtilities::MultiplyByValue(particleVelocities[particleIndex], timeStep, &displacement);
            VectorUtilities::Add(particle.coordinates, displacement, &particle.coordinates);
            geometry->EnsureBoundaries(particle, &particle, innerDiameterRatio);

            neighborProvider->EndMove();
            closestPairProvider->EndMove();
        }
    }

    void ClosestJammingStep::ResetGeneration()
    {
        throw InvalidOperationException("ResetGeneration does nothing for ClosestJammingStep. Always set maxRunsCount = 1 for LS.");
    }

    bool ClosestJammingStep::ShouldContinue() const
    {
         // 3 degrees of freedom are diminished as one particle can always be considered fixed.
         return bondsProvider.GetBonds().size() < static_cast<size_t>(DIMENSIONS * (config->particlesCount - 1));

//        // Expected bonds count n = DIMENSIONS * (particlesCount - 1) = 30 000 - 3 = 29997
//        // Expected coordination number c = 2 * n / particlesCount = 2 * n - 1 / particlesCount = 5.9994 ~ 6 = 2 * n / particlesCount
//        // Acceptable coordination number = 5.98
//        // Acceptable bonds count n = 5.98 / 2 * particlesCount = 29900. This difference will matter a lot (each iteration may consume > 1 minute)
//        size_t acceptableBondsCount = DIMENSIONS * (5.98 / 6.0) * config->particlesCount;
//        return bondsProvider.GetBonds().size() < acceptableBondsCount;
    }
}


// Copyright 2021 The Autoware Foundation
//
// 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.
//
// Co-developed by Tier IV, Inc. and Apex.AI, Inc.

#include "tracking/tracked_object.hpp"

#include <measurement_conversion/measurement_conversion.hpp>
#include <measurement_conversion/measurement_typedefs.hpp>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>

#include <Eigen/Core>

#include <algorithm>
#include <stdexcept>

namespace autoware
{
namespace perception
{
namespace tracking
{

namespace
{

using autoware::common::state_vector::variable::X;
using autoware::common::state_vector::variable::Y;
using autoware::common::state_vector::variable::X_VELOCITY;
using autoware::common::state_vector::variable::Y_VELOCITY;
using autoware::common::state_vector::variable::X_ACCELERATION;
using autoware::common::state_vector::variable::Y_ACCELERATION;

using autoware::common::state_estimation::PoseMeasurementXYZ64;
using autoware::common::state_estimation::Stamped;
using autoware::common::state_estimation::convert_to;

using common::types::float64_t;

using CA = autoware::common::state_vector::ConstAccelerationXY64;
using MotionModel = autoware::common::motion_model::LinearMotionModel<CA>;
using NoiseModel = autoware::common::state_estimation::WienerNoise<CA>;
using EKF = autoware::common::state_estimation::KalmanFilter<MotionModel, NoiseModel>;
using TrackedObjectMsg = autoware_auto_perception_msgs::msg::TrackedObject;
using DetectedObjectMsg = autoware_auto_perception_msgs::msg::DetectedObject;

EKF init_ekf(
  const DetectedObjectMsg & detection, float64_t default_variance,
  float64_t noise_variance)
{
  auto state = MotionModel::State {};
  state.at<X>() = detection.kinematics.pose_with_covariance.pose.position.x;
  state.at<Y>() = detection.kinematics.pose_with_covariance.pose.position.y;
  // When there is no twist available, velocity will be initialized to 0
  if (detection.kinematics.has_twist) {
    state.at<X_VELOCITY>() = detection.kinematics.twist.twist.linear.x;
    state.at<Y_VELOCITY>() = detection.kinematics.twist.twist.linear.y;
  }
  using CovarianceMatrix = Eigen::Matrix<float64_t, state.size(), state.size()>;
  CovarianceMatrix cov = default_variance * CovarianceMatrix::Identity();
  if (detection.kinematics.has_position_covariance) {
    cov(
      state.index_of<X>(),
      state.index_of<X>()) = detection.kinematics.pose_with_covariance.covariance[0];
    cov(
      state.index_of<X>(),
      state.index_of<Y>()) = detection.kinematics.pose_with_covariance.covariance[1];
    cov(
      state.index_of<Y>(),
      state.index_of<X>()) = detection.kinematics.pose_with_covariance.covariance[6];
    cov(
      state.index_of<Y>(),
      state.index_of<Y>()) = detection.kinematics.pose_with_covariance.covariance[7];
  }
  if (detection.kinematics.has_twist_covariance) {
    cov(
      state.index_of<X_VELOCITY>(),
      state.index_of<X_VELOCITY>()) =
      detection.kinematics.twist.covariance[0];
    cov(
      state.index_of<X_VELOCITY>(),
      state.index_of<Y_VELOCITY>()) =
      detection.kinematics.twist.covariance[1];
    cov(
      state.index_of<Y_VELOCITY>(),
      state.index_of<X_VELOCITY>()) =
      detection.kinematics.twist.covariance[6];
    cov(
      state.index_of<Y_VELOCITY>(),
      state.index_of<Y_VELOCITY>()) =
      detection.kinematics.twist.covariance[7];
  }
  return make_kalman_filter(
    MotionModel {}, NoiseModel {{noise_variance, noise_variance}},
    state, cov);
}

}  // anonymous namespace

TrackedObject::TrackedObject(
  const DetectedObjectMsg & detection,
  const DetectedObjectMsg::_classification_type & override_classification,
  common::types::float64_t default_variance,
  common::types::float64_t noise_variance)
: m_msg{},
  m_ekf{init_ekf(detection, default_variance, noise_variance)},
  m_default_variance{default_variance}
{
  static uint64_t object_id = 0;
  m_msg.object_id = ++object_id;
  m_msg.existence_probability = detection.existence_probability;
  m_msg.classification = override_classification;
  m_msg.shape.push_back(detection.shape);
  // z is not filtered through ekf. Just pass it through
  m_msg.kinematics.centroid_position.z = detection.kinematics.pose_with_covariance.pose.position.z;
  // Orientation is not filtered though ekf. Just pass it through from the input
  m_msg.kinematics.orientation_availability = detection.kinematics.orientation_availability;
  m_msg.kinematics.orientation = detection.kinematics.pose_with_covariance.pose.orientation;
  // Kinematics are owned by the EKF and only filled in in the msg() getter
  m_classifier.update(override_classification);
}

/// \relates autoware::perception::tracking::TrackedObject
TrackedObject::TrackedObject(
  const DetectedObjectMsg & detection,
  float64_t default_variance,
  float64_t noise_variance)
: TrackedObject{detection, detection.classification, default_variance, noise_variance} {}

void TrackedObject::predict(std::chrono::nanoseconds dt)
{
  m_ekf.predict(dt);
  m_time_since_last_seen += dt;
}

void TrackedObject::update(const DetectedObjectMsg & detection)
{
  m_time_since_last_seen = std::chrono::nanoseconds::zero();
  m_ticks_alive++;
  m_ticks_since_last_seen = 0;
  // Update the shape
  m_msg.shape = {detection.shape};
  m_msg.kinematics.centroid_position.z = detection.kinematics.pose_with_covariance.pose.position.z;
  m_msg.kinematics.orientation = detection.kinematics.pose_with_covariance.pose.orientation;

  // It needs to be determined which parts of the DetectedObject message are set, and can be used
  // to update the state. Also, even if a variable is set, its covariance might not be set.
  autoware_auto_geometry_msgs::msg::RelativePositionWithCovarianceStamped position;
  position.position.x = detection.kinematics.pose_with_covariance.pose.position.x;
  position.position.y = detection.kinematics.pose_with_covariance.pose.position.y;
  position.position.z = detection.kinematics.pose_with_covariance.pose.position.z;
  const auto & cov = detection.kinematics.pose_with_covariance.covariance;
  position.covariance = {cov[0], cov[1], cov[2], cov[6], cov[7], cov[8], cov[12], cov[13], cov[14]};
  auto pose_measurement =
    convert_to<Stamped<PoseMeasurementXYZ64>>::from(position).measurement;
  if (!detection.kinematics.has_position_covariance) {
    pose_measurement.covariance() = m_default_variance *
      PoseMeasurementXYZ64::State::Matrix::Identity();
  }
  m_ekf.correct(pose_measurement);
}

void TrackedObject::update(
  const ObjectClassifications & classification,
  const common::types::float32_t covariance)
{
  m_classifier.update(classification, covariance);
}

void TrackedObject::no_update()
{
  m_ticks_alive++;
  m_ticks_since_last_seen++;
}

const TrackedObject::TrackedObjectMsg & TrackedObject::msg()
{
  // Fill the message fields from the filter state
  m_msg.kinematics.centroid_position.x = m_ekf.state().at<X>();
  m_msg.kinematics.centroid_position.y = m_ekf.state().at<Y>();
  m_msg.kinematics.twist.twist.linear.x = m_ekf.state().at<X_VELOCITY>();
  m_msg.kinematics.twist.twist.linear.y = m_ekf.state().at<Y_VELOCITY>();
  m_msg.kinematics.acceleration.accel.linear.x =
    m_ekf.state().at<X_ACCELERATION>();
  m_msg.kinematics.acceleration.accel.linear.y =
    m_ekf.state().at<Y_ACCELERATION>();

  // Set covariances
  m_msg.kinematics.position_covariance[0] =
    m_ekf.covariance()(
    m_ekf.state().index_of<X>(),
    m_ekf.state().index_of<X>());
  m_msg.kinematics.position_covariance[1] =
    m_ekf.covariance()(
    m_ekf.state().index_of<X>(),
    m_ekf.state().index_of<Y>());
  m_msg.kinematics.position_covariance[3] =
    m_ekf.covariance()(
    m_ekf.state().index_of<Y>(),
    m_ekf.state().index_of<X>());
  m_msg.kinematics.position_covariance[4] =
    m_ekf.covariance()(
    m_ekf.state().index_of<Y>(),
    m_ekf.state().index_of<Y>());
  m_msg.kinematics.twist.covariance[0] =
    m_ekf.covariance()(
    m_ekf.state().index_of<X_VELOCITY>(),
    m_ekf.state().index_of<X_VELOCITY>());
  m_msg.kinematics.twist.covariance[1] =
    m_ekf.covariance()(
    m_ekf.state().index_of<X_VELOCITY>(),
    m_ekf.state().index_of<Y_VELOCITY>());
  m_msg.kinematics.twist.covariance[6] =
    m_ekf.covariance()(
    m_ekf.state().index_of<Y_VELOCITY>(),
    m_ekf.state().index_of<X_VELOCITY>());
  m_msg.kinematics.twist.covariance[7] =
    m_ekf.covariance()(
    m_ekf.state().index_of<Y_VELOCITY>(),
    m_ekf.state().index_of<Y_VELOCITY>());
  m_msg.classification = m_classifier.object_classification_vector();
  // TODO(nikolai.morin): Set is_stationary etc.
  return m_msg;
}

bool TrackedObject::should_be_removed(
  const std::chrono::nanoseconds time_threshold,
  const std::size_t ticks_threshold) const
{
  return m_time_since_last_seen >= time_threshold || m_ticks_since_last_seen >= ticks_threshold;
}

}  // namespace tracking
}  // namespace perception
}  // namespace autoware


#include <iostream>
#include <opencv2/imgcodecs.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/calib3d.hpp>
#include <Eigen/Geometry>
#include "camera/Camera.hpp"
#include "camera/RealCamera.hpp"
#include "calibration/CameraPoseCalculator.h"
#include "Common.h"
#include "optical_flow/ReportOpticalFlowCalculator.h"
#include "Triangulator.h"
#include "Visualization.h"
#include "object/SingleObjectSelector.hpp"
#include "mask/OFForegroundMaskCalculator.h"
#include "mask/MOG2ForegroundMaskCalculator.h"
#include "optical_flow/OpticalFlowCalculator.h"
#include "object/MultiObjectSelector.h"

using namespace cv;
using namespace std;

//int main_mask(int argc, char **argv) {
//
//    Mat img = imread("/media/balint/Data/cucc/mask343.png");
//    Mat mask;
//    cvtColor(img, mask, COLOR_RGB2GRAY);
//
//    Mat maskWithoutShadows = mask.clone();
//    for (int y = 0; y < maskWithoutShadows.rows; y++) {
//        for (int x = 0; x < maskWithoutShadows.cols; x++) {
//            if (maskWithoutShadows.at<uchar>(y, x) != 255) {
//                maskWithoutShadows.at<uchar>(y, x) = 0;
//            }
//        }
//    }
//
//    Mat result = maskWithoutShadows.clone();
//
//    int niters = 3;
//
//    Mat small = getStructuringElement(MORPH_RECT, Size(2, 2));
//    Mat bigger = getStructuringElement(MORPH_RECT, Size(5, 5));
//
//    erode(result, result, small, Point(-1, -1), niters);
//    dilate(result, result, bigger, Point(-1, -1), niters * 2);
//    erode(result, result, bigger, Point(-1, -1), niters * 2);
//
////    erode(result, result, Mat(), Point(-1, -1), niters * 2);
////    dilate(result, result, Mat(), Point(-1, -1), niters);
////    erode(result, result, Mat(), Point(-1, -1), niters);
//
//    imwrite("/media/balint/Data/cucc/mask343_fixed.png", result);
//
//    Mat original = imread("/media/balint/Data/cucc/image343.png");
//    Mat applied;
//    original.copyTo(applied, result);
//    imwrite("/media/balint/Data/cucc/mask343_applied.png", applied);
//
//
//    vector<vector<Point>> contours;
//
//    findContours(result, contours, RETR_EXTERNAL, CHAIN_APPROX_SIMPLE);
//
//    Mat contourResult(480, 640, CV_8U, Scalar(0));
//
//    if (contours.size() != 0) {
//        // iterate through all the top-level contours,
//        // draw each connected component with its own random color
//        int largestComp = 0;
//        double maxArea = 0;
//
//        for (int idx = 0; idx < contours.size(); idx++) {
//            const vector<Point> &c = contours[idx];
//            double area = fabs(contourArea(Mat(c)));
//            if (area > maxArea) {
//                maxArea = area;
//                largestComp = idx;
//            }
//        }
//
//        for (int idx = 0; idx < contours.size(); idx++) {
//            if (idx == largestComp) {
//                drawContours(contourResult, contours, idx, Scalar(255), FILLED, LINE_8);
//            } else {
//                drawContours(contourResult, contours, idx, Scalar(255), 1, LINE_AA);
//            }
//        }
//    }
//
//    imwrite("/media/balint/Data/cucc/mask343_contours.png", contourResult);
//
//    while (true) {
//        imshow("original", mask);
//        imshow("maskWithoutShadows", maskWithoutShadows);
//        imshow("result", result);
//        imshow("contourResult", contourResult);
//
//        char ch = waitKey(50);
//        if (ch == 27) {
//            break;
//        }
//    }
//
//    return 0;
//}
//
//int main___maskcalc(int argc, char **argv) {
//
//    Ptr<Camera> camera1(new RealCamera(Camera::LEFT, "/media/balint/Data/Linux/diploma/src/final/intrinsics_left.yml"));
//    Ptr<Camera> camera2(new RealCamera(Camera::RIGHT, "/media/balint/Data/Linux/diploma/src/final/intrinsics_right.yml"));
//
//    OFForegroundMaskCalculator maskCalculator1, maskCalculator2;
//
//    int frameId = 0;
//    while (true) {
//
//        frameId++;
//
//        Mat frame1, frame2;
//        camera1->read(frame1);
//        Mat mask1 = maskCalculator1.calculate(frame1);
//
//        camera2->read(frame2);
//        Mat mask2 = maskCalculator2.calculate(frame2);
//
//        imshow("frame1", frame1);
//        imshow("mask1", mask1);
//        imshow("frame2", frame2);
//        imshow("mask2", mask2);
//
//        char ch = (char) waitKey(33);
//        if (ch == 27) {
//            break;
//        } else if (frameId > 200) {
//            imwrite("/media/balint/Data/Linux/frames/frame_left_" + std::to_string(frameId) + ".png", frame1);
//            imwrite("/media/balint/Data/Linux/frames/frame_right_" + std::to_string(frameId) + ".png", frame2);
//            imwrite("/media/balint/Data/Linux/frames/mask_left_" + std::to_string(frameId) + ".png", mask1);
//            imwrite("/media/balint/Data/Linux/frames/mask_right_" + std::to_string(frameId) + ".png", mask2);
//        }
//    }
//
//    return 0;
//}
//
//int mainXXXX(int argc, char **argv) {
//
//    Mat left = imread("/media/balint/Data/Linux/diploma/of_img_left.png");
//    Mat right = imread("/media/balint/Data/Linux/diploma/of_img_right.png");
//
//    Mat maskLeft = imread("/media/balint/Data/Linux/diploma/of_mask_left.png", IMREAD_GRAYSCALE);
//    Mat maskRight = imread("/media/balint/Data/Linux/diploma/of_mask_right.png", IMREAD_GRAYSCALE);
//
//    Rect leftBr = boundingRect(maskLeft);
//    Rect rightBr = boundingRect(maskRight);
//    Rect uni = leftBr | rightBr;
//
//    rectangle(left, leftBr.tl(), leftBr.br(), Scalar(0, 255, 255), 2, LINE_AA);
//    rectangle(right, rightBr.tl(), rightBr.br(), Scalar(0, 255, 255), 2, LINE_AA);
//
//    std::cout << "left: " << (double)leftBr.x / 640 << " " << (double)leftBr.y / 480 << " " << (double)(leftBr.x + leftBr.width) / 640 << " " << (double)(leftBr.y + leftBr.height) / 480 <<std::endl;
//    std::cout << "right: " << (double)rightBr.x / 640 << " " << (double)rightBr.y / 480 << " " << (double)(rightBr.x + rightBr.width) / 640 << " " << (double)(rightBr.y + rightBr.height) / 480 <<std::endl;
//    std::cout << "uni: " << (double)uni.x / 640 << " " << (double)uni.y / 480 << " " << (double)(uni.x + uni.width) / 640 << " " << (double)(uni.y + uni.height) / 480 <<std::endl;
//
//    rectangle(left, uni.tl(), uni.br(), Scalar(0, 0, 255), 2, LINE_AA);
//    rectangle(right, uni.tl(), uni.br(), Scalar(0, 0, 255), 2, LINE_AA);
//
//    imshow("left", left);
//    imshow("right", right);
//
//    imwrite("/media/balint/Data/Linux/diploma/magic1.png", left);
//    imwrite("/media/balint/Data/Linux/diploma/magic2.png", right);
//
//    waitKey();
//
//    return 0;
//}
//
//int main_SELECT(int argc, char **argv) {
//
//    Ptr<Camera> camera1(new RealCamera(Camera::LEFT, "/media/balint/Data/Linux/diploma/src/final/intrinsics_left.yml"));
//    Ptr<Camera> camera2(new RealCamera(Camera::RIGHT, "/media/balint/Data/Linux/diploma/src/final/intrinsics_right.yml"));
//
//    FileStorage fs;
//    fs.open("/media/balint/Data/Linux/diploma/F.yml", FileStorage::READ);
//    Mat F;
//    fs["myF"] >> F;
//
//    Matcher matcher(camera1, camera2, F);
//    SingleObjectSelector objectSelector(matcher);
//
//    std::vector<Mat> frames(2), masks(2);
//
//    frames[0] = imread("/media/balint/Data/Linux/mask/frame_ofmask_104.png");
//    frames[1] = imread("/media/balint/Data/Linux/mask/frame_ofmask_107.png");
//    masks[0] = imread("/media/balint/Data/Linux/mask/mask_ofmask_104.png", IMREAD_GRAYSCALE);
//    masks[1] = imread("/media/balint/Data/Linux/mask/mask_ofmask_107.png", IMREAD_GRAYSCALE);
//
//    std::vector<Object> objects = objectSelector.selectObjects(frames, masks);
//
//    imwrite("/media/balint/Data/Linux/mask/frame_ofmask_104_selected.png", objects[0].masks[0]);
//
//    int frameId = 0;
//    while (true) {
//
//        imshow("masks", objects[0].masks[0]);
//
//        char ch = (char) waitKey(33);
//        if (ch == 27) {
//            break;
//        }
//    }
//
//    return 0;
//}
//
//int main_pose(int argc, char **argv) {
//
//    int camId = Camera::RIGHT;
//    Ptr<Camera> camera(new RealCamera(camId, "/media/balint/Data/Linux/diploma/src/final/intrinsics_right.yml"));
//    CameraPoseCalculator calculator(camera);
//
//    int count = 0;
//    while (true) {
//        Mat image;
//        camera->readUndistorted(image);
//        if (calculator.poseCalculated()) {
//            drawGridXY(image, camera, calculator.cameraPose);
//            // drawBoxOnChessboard(image, camera, calculator.cameraPose);
//        }
//        imshow("image", image);
//
//        char ch = (char) waitKey(33);
//        count++;
//
//        if (ch == 27) {
//            break;
//        } else if (ch == 'p') {
//            if (calculator.calculate()) {
//                std::cout << "Calculated." << std::endl;
//            } else {
//                std::cout << "Not calculated!" << std::endl;
//            }
//        } else if (calculator.poseCalculated() && (count % 90 == 0)) {
//            imwrite("/media/balint/Data/Linux/diploma/pose" + to_string(camId) + "_" + to_string(count) + ".png",
//                    image);
//        }
//    }
//
//    return 0;
//}

Mat getFundamentalMat(const Ptr<Camera> &camera1, const CameraPose &pose1,
                      const Ptr<Camera> &camera2, const CameraPose &pose2) {
    Mat R1, R2;
    Mat T1 = pose1.tvec;
    Mat T2 = pose2.tvec;
    Rodrigues(pose1.rvec, R1);
    Rodrigues(pose2.rvec, R2);

    Mat R = R2 * R1.inv();
    Mat T = T2 - R * T1;

    Matx33d Tx(0, -T.at<double>(2), T.at<double>(1),
               T.at<double>(2), 0, -T.at<double>(0),
               -T.at<double>(1), T.at<double>(0), 0);

    Mat E = Mat(Tx) * R;

    return Mat(camera2->cameraMatrix.t().inv()) * E * Mat(camera1->cameraMatrix.inv());
}

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

    Ptr<Camera> camera1(new RealCamera(1, "intrinsics_left.yml"));
    CameraPoseCalculator calculator1(camera1);
    Ptr<Camera> camera2(
            new RealCamera(2, "intrinsics_right.yml"));
    CameraPoseCalculator calculator2(camera2);

    int count = 0;
    bool undistorted = false;
    while (true) {
        Mat image1, image2;

        if (undistorted) {
            camera1->readUndistorted(image1);
            camera2->readUndistorted(image2);
        } else {
            camera1->read(image1);
            camera2->read(image2);
        }

        if (calculator1.poseCalculated()) {
//            drawGridXY(image1, camera1, calculator1.cameraPose);
            // drawBoxOnChessboard(image, camera, calculator.cameraPose);

//            Size boardSize(9, 6);
//            float squareSize = 1.0f;
//
//            std::vector<Point3f> objectPoints;
//            for (int k = 0; k < boardSize.height; k++)
//                for (int j = 0; j < boardSize.width; j++)
//                    objectPoints.push_back(Point3f(squareSize * j, squareSize * k, 0));
//
//            Mat imagePoints;
//            projectPoints(objectPoints, calculator1.cameraPose->rvec, calculator1.cameraPose->tvec, camera1->cameraMatrix, camera1->distCoeffs, imagePoints);
//
//            for(int i=0; i<9*6; i++) {
//                circle(image1, imagePoints.at<Point2f>(i), 1, Scalar(255, 255, 255), 1);
//            }
        }
        if (calculator2.poseCalculated()) {
//            drawGridXY(image2, camera2, calculator2.cameraPose);
            // drawBoxOnChessboard(image, camera, calculator.cameraPose);
        }

        imshow("image1", image1);
        imshow("image2", image2);

        char ch = (char) waitKey(33);
        count++;

        if (ch == 27) {
            break;
        } else if (ch == 'p') {
            if (calculator1.calculate() && calculator2.calculate()) {

                imwrite("polc_left.png", image1);
                imwrite("polc_right.png", image2);

                calculator1.cameraPose->save("polc_pose_left.yml");
                calculator2.cameraPose->save("polc_pose_right.yml");

                FileStorage fs;
                fs.open("F.yml", FileStorage::WRITE);

                Mat points1, points2;
                undistortPoints(calculator1.imagePoints, points1, camera1->cameraMatrix, camera1->distCoeffs, Mat(),
                                camera1->cameraMatrix);
                undistortPoints(calculator2.imagePoints, points2, camera2->cameraMatrix, camera2->distCoeffs, Mat(),
                                camera2->cameraMatrix);

                Point2f x1 = points1.at<Point2f>(0);
                Point2f x2 = points2.at<Point2f>(0);

                // try#1
                Mat F = findFundamentalMat(points1, points2);

                // try#2

                // CALCULATING ESSENTIAL MATRIX
                Mat F2 = getFundamentalMat(camera1, *calculator1.cameraPose, camera2, *calculator2.cameraPose);

                fs << "cvF" << F << "myF" << F2;

//                std::cout << F2 << endl;
//                std::cout << Mat(Matx13d(x2.x, x2.y, 1)) * F2 * Mat(Matx31d(x1.x, x1.y, 1)) << endl;
//
//
//                // trying to use
//
//                Mat newPoints1, newPoints2;
//                correctMatches(F, points1, points2, newPoints1, newPoints2);
//
//                Point2f newx1 = newPoints1.at<Point2f>(0);
//                Point2f newx2 = newPoints2.at<Point2f>(0);
//
//                std::cout << x1 << " " << newx1 << endl;
//
//                std::cout << Mat(Matx13d(newx2.x, newx2.y, 1)) * F * Mat(Matx31d(newx1.x, newx1.y, 1)) << endl;
//
//                // F2
//
//                Mat newPoints1_, newPoints2_;
//                correctMatches(F2, points1, points2, newPoints1_, newPoints2_);
//
//                Point2f newx1_ = newPoints1_.at<Point2f>(0);
//                Point2f newx2_ = newPoints2_.at<Point2f>(0);
//
//                std::cout << newx1 << " " << newx1_ << endl;
//
//                std::cout << Mat(Matx13d(newx2_.x, newx2_.y, 1)) * F2 * Mat(Matx31d(newx1_.x, newx1_.y, 1)) << endl;

                std::cout << "Calculated." << std::endl;
            } else {
                std::cout << "Not calculated!" << std::endl;
            }
        } else if (ch == 'u') {
            undistorted = !undistorted;
        }
    }

    return 0;
}

//int main_SAVE(int argc, char **argv) {
//    vector<Ptr<Camera>> camera(2);
//    camera[Camera::LEFT] = Ptr<Camera>(
//            new RealCamera(Camera::LEFT, "/media/balint/Data/Linux/diploma/src/final/intrinsics_left.yml"));
//    camera[Camera::RIGHT] = Ptr<Camera>(
//            new RealCamera(Camera::RIGHT, "/media/balint/Data/Linux/diploma/src/final/intrinsics_right.yml"));
//
//    while (true) {
//        Mat left, right;
//
//        camera[0]->read(left);
//        camera[1]->read(right);
//
//        imshow("left", left);
//        imshow("right", right);
//
//        char ch = (char) waitKey(33);
//        if (ch == 27) {
//            break;
//        } else if (ch == ' ') {
//            imwrite("left.png", left);
//            imwrite("right.png", right);
//        }
//    }
//
//    return 0;
//}
//
//
//int main______(int argc, char **argv) {
//    vector<Ptr<Camera>> camera(2);
//    camera[Camera::LEFT] = Ptr<Camera>(
//            new RealCamera(Camera::LEFT, "/media/balint/Data/Linux/diploma/src/final/intrinsics_left.yml"));
//    camera[Camera::RIGHT] = Ptr<Camera>(
//            new RealCamera(Camera::RIGHT, "/media/balint/Data/Linux/diploma/src/final/intrinsics_right.yml"));
//
//    vector<CameraPose> cameraPose(2);
//    cameraPose[Camera::LEFT].load("/media/balint/Data/Linux/diploma/src/final/pose_left.yml");
//    cameraPose[Camera::RIGHT].load("/media/balint/Data/Linux/diploma/src/final/pose_right.yml");
//
//    FileStorage fs;
//    fs.open("/media/balint/Data/Linux/diploma/F.yml", FileStorage::READ);
//    Mat F;
//    fs["myF"] >> F;
//
//    OFForegroundMaskCalculator maskCalculator1, maskCalculator2;
//    Matcher matcher(camera[0], camera[1], F);
//    MultiObjectSelector objSelector(matcher);
//    OpticalFlowCalculator ofCalculator;
//
//    int frame = 0;
//
//    while (true) {
//        frame++;
//        std::vector<Mat> frames(2), masks(2);
//
//        camera[0]->read(frames[0]);
//        masks[0] = maskCalculator1.calculate(frames[0]);
//
//        camera[1]->read(frames[1]);
//        masks[1] = maskCalculator2.calculate(frames[1]);
//
//        vector<Object> objects = objSelector.selectObjects(frames, masks);
//
////        if (boundingRect(objects[0].masks[0]).area() < 100 || boundingRect(objects[0].masks[1]).area() < 100) {
////            continue;
////        }
////
////        ofCalculator.feed(frames, objects[0]);
////
////        Triangulator triangulator(camera[Camera::LEFT], camera[Camera::RIGHT],
////                                  cameraPose[Camera::LEFT], cameraPose[Camera::RIGHT]);
////        std::vector<CloudPoint> cvPointcloud;
////        triangulator.triangulateCv(ofCalculator.points1, ofCalculator.points2, cvPointcloud);
////
////        Visualization matVis2(cameraPose[Camera::LEFT], camera[Camera::LEFT]->cameraMatrix);
////        matVis2.renderWithDepth(cvPointcloud);
////        imshow("Result", matVis2.getResult());
//
//        imshow("left", frames[0]);
//        imshow("right", frames[1]);
//
//
//        // tiny display.
//        Mat leftResult(480, 640, CV_8UC3, Scalar(0, 0, 0));
//        Mat rightResult(480, 640, CV_8UC3, Scalar(0, 0, 0));
//
//        RNG rng;
//        for (int i = 0; i < objects.size(); i++) {
//            const Object &obj = objects[i];
//
//            int icolor = (unsigned) rng;
//            Scalar color(icolor & 255, (icolor >> 8) & 255, (icolor >> 16) & 255);
//            Mat objMat(480, 640, CV_8UC3, color);
//
//            objMat.copyTo(leftResult, obj.masks[0]);
//            objMat.copyTo(rightResult, obj.masks[1]);
//        }
//
//        Mat merged = mergeImages(leftResult, rightResult);
//
//        for (int i = 0; i < objects.size(); i++) {
//            const Object &obj = objects[i];
//
//            for (int j = 0; j < obj.matches.size(); j++) {
//                int icolor = (unsigned) rng;
//                Scalar color(icolor & 255, (icolor >> 8) & 255, (icolor >> 16) & 255);
//
//                circle(merged, obj.matches[j].first, 1, color, 1);
//                circle(merged, obj.matches[j].second + Point2f(640, 0), 2, color, 1);
//                line(merged, obj.matches[j].first, obj.matches[j].second + Point2f(640, 0), color, 1);
//            }
//        }
//
//        imshow("objects", merged);
//
//        char ch = (char) waitKey(33);
//        if (ch == 27) {
//            break;
//        } else if (frame % 5 == 0) {
//            imwrite("/media/balint/Data/Linux/for_multi/" + std::to_string(frame) + "_1left_img.png", frames[0]);
//            imwrite("/media/balint/Data/Linux/for_multi/" + std::to_string(frame) + "_2left_mask.png", masks[0]);
//            imwrite("/media/balint/Data/Linux/for_multi/" + std::to_string(frame) + "_3right_img.png", frames[1]);
//            imwrite("/media/balint/Data/Linux/for_multi/" + std::to_string(frame) + "_4right_mask.png", masks[1]);
//            imwrite("/media/balint/Data/Linux/for_multi/" + std::to_string(frame) + "_5objects.png", merged);
////            imwrite("/media/balint/Data/Linux/for_multi/" + std::to_string(frame) + "_5vis.png", matVis2.getResult());
//        }
//    }
//}
//
//enum VIS_TYPE {
//    PIXELS, DEPTH, CONTOURS
//};
//
//int mainSDFSDF(int argc, char **argv) {
//
//    vector<Ptr<Camera>> camera(2);
//    camera[Camera::LEFT] = Ptr<Camera>(
//            new RealCamera(Camera::LEFT, "/media/balint/Data/Linux/diploma/src/final/intrinsics_left.yml"));
//    camera[Camera::RIGHT] = Ptr<Camera>(
//            new RealCamera(Camera::RIGHT, "/media/balint/Data/Linux/diploma/src/final/intrinsics_right.yml"));
//
//    vector<CameraPose> cameraPose(2);
//    cameraPose[Camera::LEFT].load("/media/balint/Data/Linux/diploma/src/final/pose_left.yml");
//    cameraPose[Camera::RIGHT].load("/media/balint/Data/Linux/diploma/src/final/pose_right.yml");
//
//    std::vector<Ptr<BackgroundSubtractorMOG2>> bgSub(2);
//    bgSub[Camera::LEFT] = createBackgroundSubtractorMOG2(300, 25.0, true);
//    bgSub[Camera::RIGHT] = createBackgroundSubtractorMOG2(300, 25.0, true);
//
//    int focus = 80;
//    static_cast<RealCamera *>(camera[Camera::LEFT].get())->focus(focus);
//    static_cast<RealCamera *>(camera[Camera::RIGHT].get())->focus(focus);
//
//    FileStorage fs;
//    fs.open("/media/balint/Data/Linux/diploma/F.yml", FileStorage::READ);
//    Mat F;
//    fs["myF"] >> F;
//
//    Matcher matcher(camera[0], camera[1], F);
//    SingleObjectSelector objSelector(matcher);
//
//
//    OpticalFlowCalculator ofCalculator;
//
//    Mat originalImage = imread("/media/balint/Data/Linux/diploma/of_img_left.png");
//
////    Mat left = imread("/media/balint/Data/Linux/for_report/390_1left_img.png", IMREAD_GRAYSCALE); // of_img_left
////    Mat right = imread("/media/balint/Data/Linux/for_report/390_3right_img.png", IMREAD_GRAYSCALE); // of_img_right
////    Mat maskLeft = imread("/media/balint/Data/Linux/for_report/390_2left_mask.png", IMREAD_GRAYSCALE);
////    Mat maskRight = imread("/media/balint/Data/Linux/for_report/390_4right_mask.png", IMREAD_GRAYSCALE);
//
//    Mat left = imread("/media/balint/Data/Linux/diploma/of_img_left.png", IMREAD_GRAYSCALE); // of_img_left
//    Mat right = imread("/media/balint/Data/Linux/diploma/of_img_right.png", IMREAD_GRAYSCALE); // of_img_right
//    Mat maskLeft = imread("/media/balint/Data/Linux/diploma/of_mask_left.png", IMREAD_GRAYSCALE);
//    Mat maskRight = imread("/media/balint/Data/Linux/diploma/of_mask_right.png", IMREAD_GRAYSCALE);
//
//    Mat eqLeft, eqRight;
//    equalizeHist(left, eqLeft);
//    equalizeHist(right, eqRight);
//
//    // SURF
////    vector<Mat> images(2);
////    images[0] = left;
////    images[1] = right;
////
////    SURFFeatureExtractor extractor(images);
////
////    std::vector<DMatch> matches;
////    MyMatcher matcher(camera[0], camera[1]);
////    std::vector<std::pair<Point2f, Point2f>> points = matcher.match(extractor, matches, F);
////
////    //-- Draw matches
////    Mat img_matches;
////    drawMatches(images[0], extractor.keypoints[0], images[1], extractor.keypoints[1], matches, img_matches,
////                Scalar::all(-1), Scalar::all(-1), std::vector<char>(), DrawMatchesFlags::NOT_DRAW_SINGLE_POINTS);
////
////    //-- Show detected matches
////    imshow("Matches", img_matches);
////    // -----------
////
////    // MAGIC
////    std::vector<Point2f> vectors;
////    for (int i = 0; i < points.size(); i++) {
////        vectors.push_back(points[i].second - points[i].first);
////    }
////    Point2f v = magicVector(vectors);
////
////
////
////    Rect bRect(boundingRect(maskLeft));
////    Mat shiftedLeft(480, 640, CV_8UC3);
////    shiftImage(left, bRect, Point2i(v.x, v.y), shiftedLeft);
////    imwrite("/media/balint/Data/Linux/diploma/__left_shifted.png", shiftedLeft);
//
//
//    vector<Mat> frames(2);
//    frames[0] = eqLeft;
//    frames[1] = eqRight;
//
//    vector<Mat> masks(2);
//    masks[0] = maskLeft;
//    masks[1] = maskRight;
//
//    vector<Object> objects = objSelector.selectObjects(frames, masks);
//
//    pair<vector<Point2f>, vector<Point2f>> matches = ofCalculator.calcDenseMatches(frames, objects[0]);
//
//    Triangulator triangulator(camera[Camera::LEFT], camera[Camera::RIGHT],
//                              cameraPose[Camera::LEFT], cameraPose[Camera::RIGHT]);
//
//    std::vector<CloudPoint> pointcloud;
//    triangulator.triangulateIteratively(matches.first, matches.second, pointcloud);
//
//    std::vector<CloudPoint> cvPointcloud;
//    triangulator.triangulateCv(matches.first, matches.second, cvPointcloud);
//
////    Visualization matVis(cameraPose[Camera::LEFT], camera[Camera::LEFT]->cameraMatrix);
//
////    matVis.renderWithDepth(pointcloud);
//
//    namedWindow("Vizualizáció", 1);
//
//    //imshow("magic", matVis.getResult());
//
//    CameraPose virtualPose;
//    Mat virtualCameraMatrix = (Mat_<double>(3, 3) << 540, 0, 320, 0, 540, 240, 0, 0, 1);
//    cameraPose[Camera::LEFT].copyTo(virtualPose);
//
//    int pos = 0;
//    char const *xTrackbar = "Váltás kamerák között";
//    createTrackbar(xTrackbar, "Vizualizáció", &pos, 100);
//
//    VIS_TYPE type = VIS_TYPE::DEPTH;
//
//    while (true) {
//        // calculate the relative position.
//        double ratio = ((double) pos) / 100;
//        virtualPose.tvec = (1 - ratio) * cameraPose[Camera::LEFT].tvec + ratio * cameraPose[Camera::RIGHT].tvec;
//        virtualPose.rvec = slerp(cameraPose[Camera::LEFT].rvec, cameraPose[Camera::RIGHT].rvec, ratio);
//
//        Visualization matVis2(virtualPose, virtualCameraMatrix);
//        if (type == VIS_TYPE::DEPTH) {
//            matVis2.renderWithDepth(cvPointcloud);
//        } else if (type == VIS_TYPE::PIXELS) {
//            matVis2.renderWithColors(cvPointcloud, matches.first, originalImage);
//        } else {
//            matVis2.renderWithContours(cvPointcloud);
//        }
//        imshow("Vizualizáció", matVis2.getResult());
//
//        char ch = (char) waitKey(33);
//        if (ch == 27) {
//            break;
//        } else if (ch == 'd') {
//            cameraPose[Camera::RIGHT].copyTo(virtualPose);
//            pos = 100;
//            setTrackbarPos(xTrackbar, "Result", pos);
//        } else if (ch == 'a') {
//            cameraPose[Camera::LEFT].copyTo(virtualPose);
//            pos = 0;
//            setTrackbarPos(xTrackbar, "Result", pos);
//        } else if (ch == '1') {
//            type = VIS_TYPE::DEPTH;
//        } else if (ch == '2') {
//            type = VIS_TYPE::PIXELS;
//        } else if (ch == '3') {
//            type = VIS_TYPE::CONTOURS;
//        } else if (ch == 'f') {
//            imwrite("/media/balint/Data/Linux/diploma/visualization.png",
//                    matVis2.getResult()(Rect(165, 145, 315, 315)));
//        }
//    }
//
//    return 0;
//}