/******************************************************************************
 * Author:   Laurent Kneip                                                    *
 * Contact:  kneip.laurent@gmail.com                                          *
 * License:  Copyright (c) 2013 Laurent Kneip, ANU. 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 ANU 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 ANU OR THE 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 <opengv/triangulation/methods.hpp>
#include <Eigen/Eigenvalues>

opengv::point_t
opengv::triangulation::triangulate(
    const relative_pose::RelativeAdapterBase & adapter,
    size_t index )
{
  translation_t t12 = adapter.gett12();
  rotation_t R12 = adapter.getR12();
  Eigen::Matrix<double,3,4> P1 = Eigen::Matrix<double,3,4>::Zero();
  P1.block<3,3>(0,0) = Eigen::Matrix3d::Identity();
  Eigen::Matrix<double,3,4> P2 = Eigen::Matrix<double,3,4>::Zero();
  P2.block<3,3>(0,0) = R12.transpose();
  P2.block<3,1>(0,3) = -R12.transpose()*t12;
  bearingVector_t f1 = adapter.getBearingVector1(index);
  bearingVector_t f2 = adapter.getBearingVector2(index);

  Eigen::MatrixXd A(4,4);
  A.row(0) = f1[0] * P1.row(2) - f1[2] * P1.row(0);
  A.row(1) = f1[1] * P1.row(2) - f1[2] * P1.row(1);
  A.row(2) = f2[0] * P2.row(2) - f2[2] * P2.row(0);
  A.row(3) = f2[1] * P2.row(2) - f2[2] * P2.row(1);

  Eigen::JacobiSVD< Eigen::MatrixXd > mySVD(A, Eigen::ComputeFullV );
  point_t worldPoint;
  worldPoint[0] = mySVD.matrixV()(0,3);
  worldPoint[1] = mySVD.matrixV()(1,3);
  worldPoint[2] = mySVD.matrixV()(2,3);
  worldPoint = worldPoint / mySVD.matrixV()(3,3);

  return worldPoint;
};

opengv::point_t
opengv::triangulation::triangulate(bearingVector_t const & f1, bearingVector_t const &  f2,
    translation_t const &  t12, rotation_t const &  R12)
{
  Eigen::Matrix<double,3,4> P1 = Eigen::Matrix<double,3,4>::Zero();
  P1.block<3,3>(0,0) = Eigen::Matrix3d::Identity();
  Eigen::Matrix<double,3,4> P2 = Eigen::Matrix<double,3,4>::Zero();
  P2.block<3,3>(0,0) = R12.transpose();
  P2.block<3,1>(0,3) = -R12.transpose()*t12;

  Eigen::MatrixXd A(4,4);
  A.row(0) = f1[0] * P1.row(2) - f1[2] * P1.row(0);
  A.row(1) = f1[1] * P1.row(2) - f1[2] * P1.row(1);
  A.row(2) = f2[0] * P2.row(2) - f2[2] * P2.row(0);
  A.row(3) = f2[1] * P2.row(2) - f2[2] * P2.row(1);

  Eigen::JacobiSVD< Eigen::MatrixXd > mySVD(A, Eigen::ComputeFullV );
  point_t worldPoint;
  worldPoint[0] = mySVD.matrixV()(0,3);
  worldPoint[1] = mySVD.matrixV()(1,3);
  worldPoint[2] = mySVD.matrixV()(2,3);
  worldPoint = worldPoint / mySVD.matrixV()(3,3);

  return worldPoint;
};

opengv::point_t
opengv::triangulation::triangulate2(
    const relative_pose::RelativeAdapterBase & adapter,
    size_t index )
{
  translation_t t12 = adapter.gett12();
  rotation_t R12 = adapter.getR12();
  bearingVector_t f1 = adapter.getBearingVector1(index);
  bearingVector_t f2 = adapter.getBearingVector2(index);

  bearingVector_t f2_unrotated = R12 * f2;
  Eigen::Vector2d b;
  b[0] = t12.dot(f1);
  b[1] = t12.dot(f2_unrotated);
  Eigen::Matrix2d A;
  A(0,0) = f1.dot(f1);
  A(1,0) = f1.dot(f2_unrotated);
  A(0,1) = -A(1,0);
  A(1,1) = -f2_unrotated.dot(f2_unrotated);
  Eigen::Vector2d lambda = A.inverse() * b;
  Eigen::Vector3d xm = lambda[0] * f1;
  Eigen::Vector3d xn = t12 + lambda[1] * f2_unrotated;
  point_t point = ( xm + xn )/2;
  return point;
};


// Copyright 2014, Max Planck Society.
// Distributed under the BSD 3-Clause license.
// (See accompanying file LICENSE.txt or copy at
// http://opensource.org/licenses/BSD-3-Clause)

#ifndef GRASSMANN_AVERAGES_PCA_TRIMMED_HPP__
#define GRASSMANN_AVERAGES_PCA_TRIMMED_HPP__

/*!@file
 * Grassmann averages for robust PCA functions, following the paper of Soren Hauberg.
 *
 * This file contains the implementation of the trimmed version.
 */

// for the thread pools
#include <boost/asio/io_service.hpp>
#include <boost/bind.hpp>
#include <boost/thread/thread.hpp>

#include <boost/scoped_array.hpp>
#include <boost/function.hpp>

// utilities
#include <include/private/utilities.hpp>

namespace grassmann_averages_pca
{

  namespace details
  {

    //!@internal
    //!Helper object for the updates
    template <class scalar_t>
    struct s_dimension_update
    {
      size_t dimension;
      scalar_t value;
    };

    /*!@internal
     * @brief Adaptation of merger_addition concept for accumulator and count at the same time.
     *
     * The trimmed version of the grassmann pca algorithm may strip some element along each dimension.
     * In order to compute the @f$\mu@f$ properly, the count should also be transfered.
     */
    template <class data_t>
    struct merger_update_specific_dimension
    {
      typedef data_t input_t;
      bool operator()(input_t &current_state, s_dimension_update<typename data_t::value_type> const& update_value) const
      {
        current_state(update_value.dimension) += update_value.value;
        return true;
      }
    };

  }

  /*!@brief Grassmann Average algorithm for robust PCA computation, with trimming of outliers.
   *
   * This class implements the Grassmann average for computing the robust PCA, which also includes the trimming of "outliers".
   * Its purpose is to compute the PCA of a dataset @f$\mathbf{X} = \{X_i\}@f$, where each @f$X_i@f$ is a vector of dimension
   * D, and also by being less sensitive to the outliers of the original data.
   *
   * The algorithm is the following:
   * - pick a random or a given @f$\mu_{k, 0}@f$, where @f$k@f$ is the current basis vector being computed and @f$0@f$ is the current iteration number (0).
   * - ensure this @f$\mu_{k, 0}@f$ is orthogonal to the previous detected @f$\mu_{k', 0},\, \forall k' \in [0, k)@f$
   * - until the sequence @f$(\mu_{k, t})_t@f$ converges, do:
   *   - computes the sign @f$s_{j, t}@f$ of the projection of the input vectors @f$X_j@f$ onto @f$\mu_{i, t}@f$. We have @f[s_{j, t} = X_j \cdot \mu_{k, t} \geq 0@f]
   *   - for each dimension @f$0 \lt d \leq D@f$, do:
   *     - For all @f$j@f$, consider the multiplied data set projected onto dimension @f$d@f$:
   *       @f[\mathbf{X_\mu^{(d)}} = \left\{proj_d \left(s_{j, t} \cdot X_j\right) \right\} = \left\{s_{j, t} \cdot X_j^{(d)}\right\}@f]
   *       which is a 1-D sequence
   *     - compute the indexes @f$J_d@f$ of the @f$\frac{K}{2}@f$ lowest and biggest points of this 1-D sequence @f$\mathbf{X_\mu^{(d)}}@f$
   *     - compute the update of @f$\mu_{k, .}@f$ for dimension @f$d@f$ :
   *       @f[\mu_{k, t+1}^{(d)} = \frac{\sum_{j \notin J_d} proj_d \left(s_{j, t} \cdot X_j \right)}{\# J - \# J_d} @f]
   *   - normalize @f[\mu_{k, t+1} = \frac{\left(\mu_{k, t+1}^{(1)}, \ldots \mu_{k, t+1}^{(D)}\right)^t}{\left\|\left(\mu_{k, t+1}^{(1)}, \ldots \mu_{k, t+1}^{(D)}\right)^t\right\|}@f]
   * - project the @f$X_j@f$'s onto the orthogonal subspace of @f$\mu_{k} = \lim_{t \rightarrow +\infty} \mu_{k, t}@f$: @f[\forall j, X_{j} = X_{j} - X_{j}\cdot\mu_{k} @f]
   *
   * The range taken by @f$k@f$ is a parameter of the algorithm: @c max_dimension_to_compute (see grassmann_pca::batch_process).
   * The range taken by @f$t@f$ is also a parameter of the algorithm: @c max_iterations (see grassmann_pca::batch_process).
   * The test for convergence is delegated to the class details::convergence_check.
   *
   * The computation is distributed among several threads. The multithreading strategy is
   * - to split the computation of @f$\sum_j s_{j, t} X_j@f$ among several independant chunks. This computation involves the inner product and the sign. Each chunk addresses
   *   a subset of the data @f$\{X_j\}@f$ without any overlap with other chunks. The maximal size of a chunk can be configured through the function grassmann_pca::set_max_chunk_size.
   *   By default, the size of the chunk would be the size of the data divided by the number of threads.
   * - to compute the @f$\frac{K}{2}@f$ extremal points along each dimension in parallel
   * - to split the computation of the projection onto the orthogonal subspace of @f$\mu_{k}@f$.
   * - to split the computation of the regular PCA algorithm (if any) into several independant chunks.
   *
   * The number of threads can be configured through the function grassmann_pca::set_nb_processors.
   *
   * @note
   * The algorithm may also perform a few "regular PCA" steps, which is the computation of the basis vector with highest "eigen-value". This can be configured through the function
   * grassmann_pca_with_trimming::set_nb_steps_pca. Also, the data can be centered before applying any computation (see grassmann_pca_with_trimming::set_centering). This is
   * convenient especially when the input data iterator is just a stub that provides data read from the disk for instance (see the application of Grassmann applied on videos).
   *
   * @tparam data_t type of vectors used for the computation.
   * @tparam norm_mu_t norm used to normalize the basis vectors and project them onto the unit circle.
   * @tparam observer_t an observer type following the signature of the class grassmann_trivial_callback
   *
   * @author Soren Hauberg, Raffi Enficiaud
   */
  template <class data_t,
            class observer_t = grassmann_trivial_callback<data_t>,
            class norm_mu_t = details::norm2 >
  struct grassmann_pca_with_trimming
  {
  private:
    //! Random generator for initialising @f$\mu@f$ at each dimension.
    details::random_data_generator<data_t> random_init_op;

    //! Norm used for normalizing @f$\mu@f$.
    norm_mu_t norm_op;

    //! The percentage of the data that should be trimmed.
    //! The trimming is performed symmetrically in the upper and lower distribution of the data, hence
    //! each side is trimmed by trimming_percentage/2.
    double trimming_percentage;

    //! Number of parallel tasks that will be used for computing.
    size_t nb_processors;

    //! Maximal size of a chunk (infinity by default).
    size_t max_chunk_size;

    //! Number of steps for the initial PCA like algorithm (defaults to 3).
    size_t nb_steps_pca;

    //! Type of the element of data_t.
    typedef typename data_t::value_type scalar_t;

    //! Type of the vector used for counting the element falling into the non-trimmed range.
    typedef boost::numeric::ublas::vector<size_t> count_vector_t;

    //! Indicates that the incoming data is not centered and a centering should be performed prior
    //! to the computation of the PCA or the trimmed grassmann average.
    bool need_centering;

    //! An instance observing the steps of the algorithm
    observer_t *observer;

    //!@internal
    //!@brief Contains the logic for processing part of the accumulator
    struct s_grassmann_averages_trimmed_processor_inner_products
    {
    private:
      //! Scalar type of the data
      typedef typename data_t::value_type scalar_t;

      typedef details::s_dimension_update<scalar_t> accumulator_element_t;

      size_t nb_elements;                 //!< The size of the current dataset
      size_t data_dimension;              //!< The dimension of the data
      size_t k_first_last;                //!< The number of elements to remove from the lower and upper distributions.

      typedef boost::function<void ()> connector_counter_t;
      connector_counter_t signal_counter;

      typedef boost::function<void (accumulator_element_t const*)> connector_accumulator_dimension_t;
      connector_accumulator_dimension_t signal_acc_dimension;

      std::vector<accumulator_element_t> v_accumulated_per_dimension;

      //! The matrix containing a copy of the data
      scalar_t *p_c_matrix;

      //! The result of the inner products
      std::vector<scalar_t> inner_prod_results;

      void compute_inner_products(data_t const &mu)
      {
        scalar_t *out = &inner_prod_results[0];
        scalar_t mu_element = mu(0);
        scalar_t const * current_line = p_c_matrix;

        for(int column = 0; column < nb_elements; column++)
        {
          out[column] = mu_element * current_line[column];
        }
        current_line += nb_elements;

        for(int line = 1; line < data_dimension; line ++, current_line += nb_elements)
        {
          mu_element = mu(line);
          for(int column = 0; column < nb_elements; column++)
          {
            out[column] += mu_element * current_line[column];
          }
        }

      }

    public:
      s_grassmann_averages_trimmed_processor_inner_products() :
        nb_elements(0),
        data_dimension(0),
        k_first_last(0),
        p_c_matrix(0)
      {
      }

      ~s_grassmann_averages_trimmed_processor_inner_products()
      {
        delete [] p_c_matrix;
      }

      //! Returns the connected object that will receive the notification of the end of the current process.
      connector_counter_t& connector_counter()
      {
        return signal_counter;
      }

      connector_accumulator_dimension_t& connector_accumulator()
      {
        return signal_acc_dimension;
      }

      //! Sets the data range
      template <class container_iterator_t>
      bool set_data_range(container_iterator_t const &b, container_iterator_t const& e)
      {
        if(data_dimension <= 0)
        {
          return false;
        }

        nb_elements = std::distance(b, e);

        assert(data_dimension > 0);
        assert(nb_elements > 0);

        delete [] p_c_matrix;
        p_c_matrix = new scalar_t[nb_elements*data_dimension];

        container_iterator_t bb(b);

        for(int column = 0; column < nb_elements; column++, ++bb)
        {
          scalar_t* current_line = p_c_matrix + column;
          for(int line = 0; line < data_dimension; line ++, current_line += nb_elements)
          {
            *current_line = (*bb)(line);
          }

        }

        inner_prod_results.resize(nb_elements);

        signal_counter();
        return true;
      }

      //! Sets the dimension of the data vectors
      //! @pre data_dimensions_ is strictly positive
      void set_data_dimensions(size_t data_dimensions_)
      {
        assert(data_dimensions_ > 0);
        data_dimension = data_dimensions_;
        v_accumulated_per_dimension.resize(data_dimension);
      }

      //! Sets the number of element to remove in the upper and lower distributions.
      void set_nb_elements_to_remove(int k_first_last_)
      {
        assert(k_first_last_ >= 0);
        k_first_last = k_first_last_;
      }

      //! Centering the data in case it was not possible to do it beforehand
      void data_centering_first_phase(size_t full_dataset_size)
      {
        scalar_t const * current_line = p_c_matrix;
        for(size_t dimension = 0; dimension < data_dimension; dimension++)
        {
          scalar_t acc = 0;
          for(size_t s = 0; s < nb_elements; s++)
          {
            acc += *current_line++;
          }

          // posts the new value to the listeners for the current dimension
          accumulator_element_t &result = v_accumulated_per_dimension[dimension];
          result.dimension = dimension;
          result.value = acc / full_dataset_size;
          signal_acc_dimension(&result);
        }

        signal_counter();

      }

      //! Project the data onto the orthogonal subspace of the provided vector
      void data_centering_second_phase(data_t const &mean_value)
      {
        scalar_t *current_element_ptr = p_c_matrix;

        for(int line = 0; line < data_dimension; line++)
        {
          const scalar_t scalar = mean_value(line);
          scalar_t * const current_line_end = current_element_ptr + nb_elements;
          for(; current_element_ptr < current_line_end; current_element_ptr++)
          {
            *current_element_ptr -= scalar;
          }
        }

        signal_counter();
      }

      //! PCA steps
      void pca_accumulation(data_t const &mu)
      {
        compute_inner_products(mu);

        scalar_t const * const p_inner_product = &inner_prod_results[0];

        for(size_t dimension = 0; dimension < data_dimension; dimension++)
        {
          scalar_t const * const current_line = p_c_matrix + dimension*nb_elements;
          scalar_t acc = 0;
          for(size_t s = 0; s < nb_elements; s++)
          {
            acc += p_inner_product[s] * current_line[s];
          }

          // posts the new value to the listeners for the current dimension
          accumulator_element_t &result = v_accumulated_per_dimension[dimension];
          result.dimension = dimension;
          result.value = acc;
          signal_acc_dimension(&result);
        }

        signal_counter();
      }

      //! Computes the inner products and stores the signed result where appropriate.
      void compute_data_matrix(data_t const &mu, scalar_t* p_out, size_t padding)
      {
        // updates the internal inner products
        compute_inner_products(mu);

        // the current line is spans a particular dimension
        scalar_t *current_line = p_c_matrix;

        // this spans the inner product results for all dimensions

        std::vector<int> v_mult(nb_elements);
        for(size_t element(0); element < nb_elements; element++)
        {
          v_mult[element] = inner_prod_results[element] >= 0 ? 1 : -1;
        }
        int const * const out = &v_mult[0];

        for(size_t current_dimension = 0;
            current_dimension < data_dimension;
            current_dimension++, current_line += nb_elements, p_out+= padding)
        {
          for(size_t element(0); element < nb_elements; element++)
          {
            p_out[element] = out[element] * current_line[element];
          }
        }

        // signals the main merger
        signal_counter();
      }

      //! Computes the mean on the subset of the data where the k first and last elements are removed.
      void compute_bounded_accumulation(size_t dimension, size_t nb_total_elements, scalar_t* p_data)
      {
        accumulator_element_t &result = v_accumulated_per_dimension[dimension];
        result.dimension = dimension;
        result.value = details::compute_mean_within_bounds(p_data, nb_total_elements, k_first_last);

        // signals the update
        signal_acc_dimension(&result);
        // signals the main merger
        signal_counter();
      }

      //! Project the data onto the orthogonal subspace of the provided vector
	    template <class vector_t>
      void project_onto_orthogonal_subspace(vector_t const &mu)
      {
        compute_inner_products(mu);
        scalar_t *current_line = p_c_matrix;

        for(int line = 0; line < data_dimension; line ++, current_line += nb_elements)
        {
          scalar_t mu_element = mu(line);
          for(int column = 0; column < nb_elements; column++)
          {
            current_line[column] -= mu_element * inner_prod_results[column];
          }
        }

        signal_counter();
      }

    };

    /*!@internal
     * @brief Merges the result of all workers and signals the results to the main thread.
     *
     * The purpose of this class is to add the computed accumulator of each thread to the final result
     * which contains the sum of all accumulators.
     *
     */
    struct asynchronous_results_merger :
      details::threading::asynchronous_results_merger<
        data_t,
        details::merger_update_specific_dimension<data_t>,
        details::threading::initialisation_vector_specific_dimension<data_t>,
        details::s_dimension_update<typename data_t::value_type>
      >
    {
    public:
      typedef data_t result_t;

    private:
      typedef details::threading::initialisation_vector_specific_dimension<data_t> data_init_type;
      typedef details::merger_update_specific_dimension<data_t> merger_type;

      const size_t data_dimension;

    public:
      typedef details::threading::asynchronous_results_merger<
        result_t,
        merger_type,
        data_init_type,
        details::s_dimension_update<typename data_t::value_type>
      > parent_type;
      typedef typename parent_type::lock_t lock_t;

      /*!Constructor
       *
       * @param dimension_ the number of dimensions of the vector to accumulate
       */
      asynchronous_results_merger(size_t data_dimension_) :
        parent_type(data_init_type(data_dimension_)),
        data_dimension(data_dimension_)
      {}

    };

  public:
    /*!@brief Constructor
     *
     * Constructs an instance of the RobustPCA with trimming with the provided percentage of trimming.
     *
     * @param[in] trimming_percentage_ the percentage of data that should be trimmed from the lower and upper distributions.
     * @note By default the number of processors used for computation is set to 1.
     * The maximum size of the chunks is "infinite": each chunk will receive in that case the size of the data
     * divided by the number of running threads.
     */
    grassmann_pca_with_trimming(double trimming_percentage_ = 0) :
      random_init_op(details::fVerySmallButStillComputable, details::fVeryBigButStillComputable),
      trimming_percentage(trimming_percentage_),
      nb_processors(1),
      max_chunk_size(std::numeric_limits<size_t>::max()),
      nb_steps_pca(3),
      need_centering(false),
      observer(0)
    {
      assert(trimming_percentage_ >= 0 && trimming_percentage_ <= 1);
    }

    //! Sets the observer of the algorithm.
    //!
    //! The lifetime of the observer is not managed by this class. Set to 0 to disable
    //! observation.
    bool set_observer(observer_t* observer_)
    {
      observer = observer_;
      return true;
    }

    //! Sets the number of parallel tasks used for computing.
    bool set_nb_processors(size_t nb_processors_)
    {
      assert(nb_processors_ >= 1);
      nb_processors = nb_processors_;
      return true;
    }

    /*!@brief Sets the maximum chunk size.
     *
     * By default, the chunk size is the size of the data divided by the number of processing threads.
     * Lowering the chunk size should provid better granularity in the overall processing time at the end
     * of the processing.
     */
    bool set_max_chunk_size(size_t chunk_size)
    {
      if(chunk_size == 0)
      {
        return false;
      }
      max_chunk_size = chunk_size;
      return true;
    }

    //! Sets the number of iterations for the initial PCA like algorithm.
    bool set_nb_steps_pca(size_t nb_steps)
    {
      nb_steps_pca = nb_steps;
      return true;
    }

    //! Sets the centering flags.
    //!
    //! If set to true, a centering will be performed before applying any computation (PCA and Grassmann averages).
    bool set_centering(bool need_centering_)
    {
      need_centering = need_centering_;
      return true;
    }

    /*!@brief Performs the computation of the current subspace on the elements given by the two iterators.
     *
     * @tparam it_t an input forward iterator to input vectors points. Each element pointed by the underlying iterator should be iterable and
     *   should provide a vector point.
     * @tparam it_o_basisvectors_t an output iterator for storing the computed basis vectors. This iterator should model a forward output iterator.
     *
     * @param[in] max_iterations the maximum number of iterations at each dimension.
     * @param[in] max_dimension_to_compute the maximum number of data_dimension to compute in the PCA (only the first max_dimension_to_compute will be
     *            computed).
     * @param[in] it input iterator at the beginning of the data
     * @param[in] ite input iterator at the end of the data
     * @param[in] initial_guess if provided, the initial vectors will be initialized to this value.
     * @param[out] it_basisvectors an iterator on the beginning of the area where the detected basis vectors will be stored. The space should be at least max_dimension_to_compute.
     *
     * @returns true on success, false otherwise
     * @pre
     * - @c !(it >= ite)
     * - all the vectors given by the iterators pair should be of the same size (no check is performed).
     */
    template <class it_t, class it_o_basisvectors_t>
    bool batch_process(
      const size_t max_iterations,
      size_t max_dimension_to_compute,
      it_t const it,
      it_t const ite,
      it_o_basisvectors_t it_basisvectors,
      std::vector<data_t> const * initial_guess = 0)
    {
      // add some log information
      if(it >= ite)
      {
        return false;
      }

      // preparing the thread pool, to avoid individual thread creation/deletion at each step.
      // we perform the init here because it might take some time for the thread to really start.
      boost::asio::io_service ioService;
      boost::thread_group threadpool;

      // in case of non clean exit (or even in case of clean one).
      details::threading::safe_stop worker_lock_guard(ioService, threadpool);

      // this is exactly the number of processors
      boost::asio::io_service::work work(ioService);
      for(int i = 0; i < nb_processors; i++)
      {
        threadpool.create_thread(boost::bind(&boost::asio::io_service::run, &ioService));
      }

      // contains the number of elements. In case the iterator is random access, could be deduced simply
      // by a call to distance.
      const size_t size_data(std::distance(it, ite));

      // size of the chunks.
      const size_t chunks_size = std::min(max_chunk_size, static_cast<size_t>(ceil(double(size_data)/nb_processors)));
      const size_t nb_chunks = (size_data + chunks_size - 1) / chunks_size;

      // number of dimensions of the data vectors
      const size_t number_of_dimensions = it->size();
      max_dimension_to_compute = std::min(max_dimension_to_compute, number_of_dimensions);

      // initial iterator on the output basis vectors
      it_o_basisvectors_t const it_output_basis_vector_beginning(it_basisvectors);
      it_o_basisvectors_t it_output_basis_vector_end(it_output_basis_vector_beginning);
      std::advance(it_output_basis_vector_end, max_dimension_to_compute);

      // the initialisation of mus
      {
        it_o_basisvectors_t it_basis(it_output_basis_vector_beginning);
        for(int i = 0; it_basis != it_output_basis_vector_end; ++it_basis, ++i)
        {
          *it_basis = initial_guess != 0 ? (*initial_guess)[i] : random_init_op(*it);
        }
      }
      if(!details::gram_schmidt_orthonormalisation(it_output_basis_vector_beginning, it_output_basis_vector_end, it_output_basis_vector_beginning, norm_op))
      {
        return false;
      }

      // preparing mu
      data_t mu(*it_basisvectors);
      assert(mu.size() == number_of_dimensions);

      // number of elements to remove, lower bound
      const int K_elements = static_cast<int>(trimming_percentage*size_data/2);

      // preparing the ranges on which each processing thread will run.
      // the number of objects can be much more than the current number of processors, in order to
      // avoid waiting too long for a thread (better granularity) but involving a slight overhead in memory and
      // processing at the synchronization point.
      typedef s_grassmann_averages_trimmed_processor_inner_products async_processor_t;
      std::vector<async_processor_t> v_individual_accumulators(nb_chunks);

      asynchronous_results_merger async_merger(number_of_dimensions);

      {
        it_t it_current_begin(it);
        for(int i = 0; i < nb_chunks; i++)
        {
          // setting the range
          it_t it_current_end;
          if(i == nb_chunks - 1)
          {
            // just in case the division giving the chunk has some rounding (the parenthesis are important
            // otherwise it is a + followed by a -, which can be out of range after the first +)
            it_current_end = it_current_begin + (size_data - chunks_size*(nb_chunks - 1));
          }
          else
          {
            it_current_end = it_current_begin + chunks_size;
          }

          // the processor object for this new range
          async_processor_t &current_acc_object = v_individual_accumulators[i];

          // updating the dimension of the problem
          current_acc_object.set_data_dimensions(number_of_dimensions);

          // setting the number of elements to remove
          current_acc_object.set_nb_elements_to_remove(K_elements);

          // attaching the update object callbacks
          current_acc_object.connector_accumulator() = boost::bind(
              &asynchronous_results_merger::update,
              &async_merger,
              _1);

          current_acc_object.connector_counter() = boost::bind(&asynchronous_results_merger::notify, &async_merger);

          //bool b_result = current_acc_object.set_data_range(it_current_begin, it_current_end);
          //if(!b_result)
          //{
          //  return b_result;
          //}

          // pushing the asynchronous copy, which saves a lot of time when loading
          // data lazily from disk
          ioService.post(
            boost::bind(
              &async_processor_t::template set_data_range<it_t>,
              boost::ref(v_individual_accumulators[i]),
              it_current_begin, it_current_end));

          // updating the next
          it_current_begin = it_current_end;
        }

        // waiting for completion (barrier)
        async_merger.wait_notifications(v_individual_accumulators.size());
      }

      // this matrix is a copy of the data. It is a convenient structure for storing the
      // flipped vectors that will then be trimmed and accumulated. But this is a copy of the initial data:
      // if the memory pressure is too big, then something else should be found (like a vector of vector, being
      // potentially scattered in memory).
      boost::scoped_array<scalar_t> matrix_temp(new scalar_t[number_of_dimensions*size_data]);

      // Centering the data if needed:
      // - first run the accumulation and gather all results in a multithreaded manner
      // - second center the data with the collected mean
      if(need_centering)
      {
        // Computing the accumulation
        async_merger.init();

        for(int i = 0; i < v_individual_accumulators.size(); i++)
        {
          ioService.post(
            boost::bind(
              &async_processor_t::data_centering_first_phase,
              boost::ref(v_individual_accumulators[i]),
              size_data)); // size of the dataset to perform division and avoid doing accumulation over big numerical values
        }

        // waiting for completion (barrier)
        async_merger.wait_notifications(v_individual_accumulators.size());

        // gathering the accumulated, already divided by the size
        data_t mean_vector = async_merger.get_merged_result();

        // sending result to observer
        if(observer)
        {
          observer->signal_mean(mean_vector);
        }

        // centering the data
        async_merger.init();

        for(int i = 0; i < v_individual_accumulators.size(); i++)
        {
          ioService.post(
            boost::bind(
              &async_processor_t::data_centering_second_phase,
              boost::ref(v_individual_accumulators[i]),
              boost::cref(mean_vector)
              ));
        }

        // waiting for completion (barrier)
        async_merger.wait_notifications(v_individual_accumulators.size());

      }

      // for each requested subspace
      for(int current_subspace_index = 0; current_subspace_index < max_dimension_to_compute; current_subspace_index++, ++it_basisvectors)
      {

        // PCA like initial steps
        if(nb_steps_pca)
        {
          for(size_t pca_it = 0; pca_it < nb_steps_pca; pca_it++)
          {
            // reseting the final accumulator
            async_merger.init();

            // pushing the initialisation of the mu and sign vectors to the pool
            for(int i = 0; i < v_individual_accumulators.size(); i++)
            {
              ioService.post(
                boost::bind(
                  &async_processor_t::pca_accumulation,
                  boost::ref(v_individual_accumulators[i]),
                  boost::cref(mu)));
            }

            // waiting for completion (barrier)
            async_merger.wait_notifications(v_individual_accumulators.size());

            // gathering the first mu
            mu = async_merger.get_merged_result();
            double norm_mu = norm_op(mu);
            if(norm_mu < 1E-12)
            {
              if(observer)
              {
                std::ostringstream o;
                o << "The result of the PCA is null for subspace " << current_subspace_index;
                observer->log_error_message(o.str().c_str());
              }
              return false;
            }
            mu *= typename data_t::value_type(1./norm_mu);
          }

          // sending result to observer
          if(observer)
          {
            observer->signal_pca(mu, current_subspace_index);
          }
        }

        details::convergence_check<data_t> convergence_op(mu);

        int iterations = 0;
        for(; (!convergence_op(mu) && iterations < max_iterations) || iterations == 0; iterations++)
        {

          // reseting the merger object
          async_merger.init();

          // pushing the computation of the bounds
          for(int i = 0; i < v_individual_accumulators.size(); i++)
          {
            ioService.post(
              boost::bind(
                &async_processor_t::compute_data_matrix,
                boost::ref(v_individual_accumulators[i]),
                boost::cref(mu),
                matrix_temp.get() + i*chunks_size,
                size_data));
          }

          // waiting for completion (barrier)
          async_merger.wait_notifications(v_individual_accumulators.size());

          // clearing the notifications
          async_merger.init_notifications();

          // pushing the computation of the trimmed accumulation on each dimension
          for(int dim_to_compute = 0; dim_to_compute < number_of_dimensions; dim_to_compute++)
          {
            ioService.post(
              boost::bind(
                &async_processor_t::compute_bounded_accumulation,
                boost::ref(v_individual_accumulators[0]),
                dim_to_compute,
                size_data,
                matrix_temp.get() + dim_to_compute*size_data));
          }

          // waiting for completion (barrier)
          async_merger.wait_notifications(number_of_dimensions);

          // gathering the mus
          mu = async_merger.get_merged_result();

          // normalize mu on the sphere
          mu *= typename data_t::value_type(1./norm_op(mu));

          // sending result to observer
          if(observer)
          {
            observer->signal_intermediate_result(mu, current_subspace_index, iterations);
          }

        }

        // orthogonalisation against previous basis vectors
        bool renormalise(false);
        for(it_o_basisvectors_t it_mus(it_output_basis_vector_beginning); it_mus < it_basisvectors; ++it_mus)
        {
          mu -= boost::numeric::ublas::inner_prod(mu, *it_mus) * (*it_mus);
          renormalise = true;
        }
        if(renormalise)
        {
          double norm_mu = norm_op(mu);
          if(norm_mu < 1E-12)
          {
            if(observer)
            {
              std::ostringstream o;
              o << "The result of the subspace computation is null (subspace " << current_subspace_index << ")";
              observer->log_error_message(o.str().c_str());
            }
            return false;
          }

          mu *= typename data_t::value_type(1./norm_mu);
        }

        // mu is the basis vector of the current dimension, we store it in the output vector
        *it_basisvectors = mu;

        // sending result to observer
        if(observer)
        {
          observer->signal_eigenvector(*it_basisvectors, current_subspace_index);
        }

        // projection onto the orthogonal subspace
        if(current_subspace_index < max_dimension_to_compute - 1)
        {
          async_merger.init_notifications();

          // pushing the update of the mu (and signs)
          for(int i = 0; i < v_individual_accumulators.size(); i++)
          {

			      ioService.post(
				      boost::bind(
					      &async_processor_t::template project_onto_orthogonal_subspace<typename it_o_basisvectors_t::value_type>,
					      boost::ref(v_individual_accumulators[i]),
					      *it_basisvectors)); // this is not mu, since we are changing it before the process ends here
          }

          // this is to follow the matlab implementation, but the idea is the following:
          // each time we pick a new candidate vector, we project it to the orthogonal subspace of the previously computed
          // basis vectors. This can be done in two ways:
          // 1. compute the projection on the orthogonal subspace of the current (or next) candidate
          // 2. compute the projection on the orthogonal subspace of the remainder elements
          //
          // in order to be consistent with the matlab implementation, the second choice is implemented here
          if(current_subspace_index+1 < max_dimension_to_compute)
          {
            it_o_basisvectors_t remainder(it_basisvectors);
            ++remainder;

            if(!details::gram_schmidt_orthonormalisation(it_output_basis_vector_beginning, it_output_basis_vector_end, remainder, norm_op))
            {
              return false;
            }

            mu = *remainder;
          }

          // wait for the workers
          async_merger.wait_notifications(v_individual_accumulators.size());

        }

      }

      return true;
    }

  };

}

#endif /* GRASSMANN_AVERAGES_PCA_TRIMMED_HPP__ */


#pragma once
#include <Eigen/Core>
#include <Eigen/Dense>

#include "gradient.hpp"
#include "hllc.hpp"
#include "mesh.hpp"
#include "slope_limiter.hpp"

// Note: this class will compute the rate of change due to the fluxes.
// Note: the reason we made this a class is that it allows you to allocate
//       buffers, once at the beginning of the simulation. Add these buffers
//       as needed.
class FluxRateOfChange {
  public:
    explicit FluxRateOfChange(int n_cells) : n_cells(n_cells) {}

    void operator()(Eigen::MatrixXd &dudt,
                    const Eigen::MatrixXd &u,
                    const Mesh &mesh) const
    {
        // Compute the rate of change of u.
        // Note: Please use the method `computeFlux` to abstract
        // away the details of computing the flux through a
        // given interface.
        // Note: You can use `assert_valid_flux` to check
        // if what `computeFlux` returns makes any sense.
        // Note: Do not assume `dudt` is filled with zeros.

        // sanity check:
        assert ((mesh.getNumberOfTriangles() == n_cells));

        // reset dudt to zero:
        dudt*= 0.0;

        // iterate over cells then over the 3 edges within each cell:
        for (int i= 0; i < n_cells; ++i)
            for (int k= 0; k < 3; ++k)
            {
                EulerState edge_flux= computeFlux(u, i, k, mesh);
                assert_valid_flux(mesh, i, k, edge_flux);
                dudt.row(i)+= edge_flux;
            }
    }

    void assert_valid_flux(const Mesh &mesh,
                           int i,
                           int k,
                           const EulerState &nF) const {
        // This is mostly for debugging (but also important to check in
        // real simulations!): Make sure our flux contribution is not
        // nan (ie. it is not not a number, ie it is a number)
        if (!euler::isValidFlux(nF)) {
            // clang-format off
            throw std::runtime_error(
                "invalid value detected in numerical flux, " + euler::to_string(nF)
                + "\nat triangle: " + std::to_string(i)
                + "\nedge:        " + std::to_string(k)
                + "\nis_boundary: " + std::to_string(!mesh.isValidNeighbour(i, k)));
            // clang-format on
        }
    }

    /// Compute the flux through the k-th interface of cell i.
    EulerState computeFlux(const Eigen::MatrixXd &U,
                           int i,
                           int k,
                           const Mesh &mesh) const
    {
        auto boundary_type = mesh.getBoundaryType(i, k);

        if (boundary_type == Mesh::BoundaryType::INTERIOR_EDGE)
        {
            return computeInteriorFlux(U, i, k, mesh);
        }
        else
        {
            if (boundary_type == Mesh::BoundaryType::OUTFLOW_EDGE)
            {
                return computeOutflowFlux(U, i, k, mesh);
            }
            else /* boundary_type == Mesh::BoundaryType::WING_EDGE */
            {
                return computeReflectiveFlux(U, i, k, mesh);
            }
        }
    }

    /// Compute the outflow flux through the k-th interface of cell i.
    /** Note: you know that edge k is an outflow edge.
     */
    EulerState computeOutflowFlux(const Eigen::MatrixXd &U,
                                  int i,
                                  int k,
                                  const Mesh &mesh) const
    {
        // Implement the outflow flux boundary condition.
        auto u_aligned= rotate_align(U.row(i), i, k, mesh);

        auto f_aligned= euler::flux(u_aligned);

        auto f= rotate_dealign(f_aligned, i, k, mesh);

        return mesh.getEdgeLength(i, k) * f; // eqn (35)
    }

    /// Compute the reflective boundary flux through the k-th edge of cell i.
    /** Note: you know that edge k is a reflective/wall boundary edge.
     */
    EulerState computeReflectiveFlux(const Eigen::MatrixXd &U,
                                     int i,
                                     int k,
                                     const Mesh &mesh) const
    {
        // Implement the reflective flux boundary condition.
        auto u= U.row(i);

        // get unit outward normal and transverse t:
        auto n= mesh.getUnitNormal(i, k).normalized();
        auto t= Eigen::Vector2d(-n(1), n(0));

        // assemble u_star:
        double rho= u[0];
        Eigen::Vector2d v= u.segment(1, 2);
        double E= u[3];

        EulerState u_star;
        u_star[0]=  rho;
        u_star.segment(1, 2)= -rho * v.dot(n) * n + rho * v.dot(t) * t;
        u_star[3]=  E;

        // rotate:
        auto u_aligned= rotate_align(u, i, k, mesh);
        auto u_star_aligned= - rotate_align(u_star, i, k, mesh); // (-) for reflection

        // flux:
        auto f_aligned= hllc(u_aligned, u_star_aligned); // equation (36)

        // derotate:
        auto f= rotate_dealign(f_aligned, i, k, mesh);

        return mesh.getEdgeLength(i, k) * f;
    }

    /// Compute the flux through the k-th interface of cell i.
    /** Note: This edge is an interior edge, therefore approximate the flux
     * through this edge with the appropriate FVM formulas.
     */
    EulerState computeInteriorFlux(const Eigen::MatrixXd &U,
                                   int i,
                                   int k,
                                   const Mesh &mesh) const
    {
        // Reconstruct the trace values of U and compute
        // the numerical flux through the k-th interface of
        // cell i.

        // figure out neighbour of i at edge k and check validity:
        int j= mesh.getNeighbour(i, k);
        assert(j >= 0);

        // figure out neighbour j's edge l collocated with i's edge k:
        int l= mesh.getNeighbourEdge(i, k);

        // reconstruction:
        auto uL= reconstruction(U.row(i), i, k, mesh);
        auto uR= reconstruction(U.row(j), j, l, mesh);

        // rotate:
        auto uL_aligned=   rotate_align(uL, i, k, mesh);
        auto uR_aligned= - rotate_align(uR, j, l, mesh);

        // flux:
        auto f_aligned= hllc(uL_aligned, uR_aligned); // equation (32)

        // derotate:
        auto f= rotate_dealign(f_aligned, i, k, mesh);

        return mesh.getEdgeLength(i, k) * f;
    }

    private:
        // generate rotation matrix to align edge k of triangle i with cartesian y-axis:
        Eigen::Matrix2d rot(int i, int k, const Mesh& mesh) const
        {
            auto n= mesh.getUnitNormal(i, k).normalized(); // why is this called UnitNormal if it's not unit normalized?

            Eigen::Matrix2d rot;
            rot << n(0),  n(1),
                  -n(1),  n(0);

            return rot;
        }

        // generate inverse rotation matrix to DEalign edge k of triangle i from cartesian y-axis back to original position:
        Eigen::Matrix2d rot_inv(int i, int k, const Mesh& mesh) const
        {
            // our rotation matrix is orthogonal: RR'=I:
            return rot(i, k, mesh).transpose();
        }

        // rotate u to align velocity component [u(1), u(2)] with cartesian x-axis:
        EulerState rotate_align(EulerState u, int i, int k, const Mesh& mesh) const
        {
            u.segment(1, 2)= rot(i, k, mesh) * u.segment(1, 2);
            return u;
        }

        // rotate calculated f back to point in original grid position before rotation:
        EulerState rotate_dealign(EulerState f, int i, int k, const Mesh& mesh) const
        {
            f.segment(1, 2)= rot_inv(i, k, mesh) * f.segment(1, 2);
        }

        // arggh there's probably no time for doing piecewise linear construction in task 2h)
        EulerState reconstruction(const Eigen::MatrixXd& U, int i, int k, const Mesh& mesh) const
        {
            // pass:
            return U.row(i);
        }

        int n_cells;
};


// Copyright(c) 2019-present, Alexander Silva Barbosa & bflib contributors.
// Distributed under the MIT License (http://opensource.org/licenses/MIT)

/**
 * @author Alexander Silva Barbosa <alexander.ti.ufv@gmail.com>
 * @date 2019
 * Extended Kalman Filter
 */

#pragma once

#include <Eigen/Dense>
#include <random>
#include <chrono>
#include <thread>
#include <vector>

using namespace Eigen;

template <typename dataType, int states, int inputs, int outputs, int dataConverter = -1>
class EKF
{
    private:
        typedef Matrix<dataType, states, 1> MatNx1;
        typedef Matrix<dataType, states, states> MatNxN;
        typedef Matrix<dataType, states, inputs> MatNxM;
        typedef Matrix<dataType, states, outputs> MatNxP;
        typedef Matrix<dataType, outputs, states> MatPxN;
        typedef Matrix<dataType, outputs, outputs> MatPxP;
        typedef Matrix<dataType, inputs, 1> MatMx1;
        typedef Matrix<dataType, outputs, 1> MatPx1;
        typedef Matrix<dataType, dataConverter, 1> MatDx1;
        typedef Matrix<dataType, 3, 1> Mat3x1;

    public:
        typedef MatNx1 State;
        typedef MatMx1 Input;
        typedef MatPx1 Output;
        typedef Input Control;
        typedef Output Sensor;
        typedef MatNxN ModelCovariance;
        typedef MatPxP SensorCovariance;
        typedef MatNxN StateMatrix;
        typedef MatNxM InputMatrix;
        typedef MatPxN OutputMatrix;
        typedef MatNxN ModelJacobian;
        typedef MatPxN SensorJacobian;
        typedef MatDx1 Data;
        typedef Data Landmark;
        typedef Mat3x1 Uncertainty;

    private:
        typedef void (*ModelFunction)(State &x, Input &u, double dt);
        typedef void (*SensorFunction)(Output &y, State &x, Data &d, double dt);
        typedef void (*ModelJacobianFunction)(ModelJacobian &F, State &x, Input &u, double dt);
        typedef void (*SensorJacobianFunction)(SensorJacobian &H, State &x, Data &d, double dt);

        std::default_random_engine gen;
        std::normal_distribution<double> distr{0.0, 1.0};
        std::chrono::time_point<std::chrono::high_resolution_clock> start;

        State x, x_1;
        ModelCovariance Q;
        SensorCovariance R;
        Output z;

        ModelJacobian F;
        SensorJacobian H;

        std::vector<Data> dataPoints;

        MatNx1 randX;
        MatPx1 randY;

        MatNxN P;
        MatNxN Qsqrt;
        MatPxP Rsqrt;

        MatPxP S;
        MatNxP K;
        MatNxN I;

        ModelFunction modelFn;
        SensorFunction sensorFn;
        ModelJacobianFunction modelJFn;
        SensorJacobianFunction sensorJFn;

        void init()
        {
            modelFn = NULL;
            sensorFn = NULL;
            modelJFn = NULL;
            sensorJFn = NULL;

            P = Q;
            I.setIdentity();

            Qsqrt = Q.cwiseSqrt();
            Rsqrt = R.cwiseSqrt();

            start = std::chrono::high_resolution_clock::now();
        }
    public:

        EKF()
        {
            Q.setIdentity();
            R.setIdentity();
            x.setZero();
            init();
        }

        EKF(State X) : x(X)
        {
            Q.setIdentity();
            R.setIdentity();
            init();
        }

        EKF(ModelCovariance Q, SensorCovariance R) : Q(Q), R(R)
        {
            x.setZero();
            init();
        }

        EKF(State X, ModelCovariance Q, SensorCovariance R) : x(X), Q(Q), R(R)
        {
            init();
        }

        virtual ~EKF()
        {

        }

        long long seed()
        {
            long long s = std::chrono::system_clock::now().time_since_epoch().count();
            return seed(s);
        }

        long long seed(long long s)
        {
            gen = std::default_random_engine(s);
            return s;
        }

        State state()
        {
            MatNx1 x;
            x.setZero();
            return x;
        }

        Input input()
        {
            MatMx1 u;
            u.setZero();
            return u;
        }

        Output output()
        {
            MatPx1 y;
            y.setZero();
            return y;
        }

        ModelCovariance createQ()
        {
            ModelCovariance Q;
            Q.setZero();
            return Q;
        }

        SensorCovariance createR()
        {
            SensorCovariance R;
            R.setZero();
            return R;
        }

        Data createData()
        {
            Data D;
            D.setZero();
            return D;
        }

        ModelCovariance getP()
        {
            return P;
        }

        Uncertainty getUncertainty(unsigned int x1, unsigned int x2)
        {
            Uncertainty C;
            C.setZero();
            if(x1 >= states || x2 >= states)
                return C;

            Matrix<dataType, 2, 2> p;
            p(0, 0) = P(x1, x1);
            p(0, 1) = P(x1, x2);
            p(1, 0) = P(x2, x1);
            p(1, 1) = P(x2, x2);

            EigenSolver< Matrix<dataType, 2, 2> > es(p);
            Matrix<dataType, 2, 2> eValue = es.pseudoEigenvalueMatrix();
            Matrix<dataType, 2, 2> eVector = es.pseudoEigenvectors();

            C[0] = eValue(0,0);
            C[1] = eValue(1,1);
            C[2] = std::atan2(eVector(0, 1), eVector(0, 0));

            return C;
        }

        void setQ(ModelCovariance Q)
        {
            this->Q = Q;
            P = Q;
            Qsqrt = Q.cwiseSqrt();
        }

        void setR(SensorCovariance R)
        {
            this->R = R;
            Rsqrt = R.cwiseSqrt();
        }

        double time()
        {
            auto end = std::chrono::high_resolution_clock::now();
            std::chrono::duration<double> diff = end - start;
            start = std::chrono::high_resolution_clock::now();
            return diff.count();
        }

        double delay(double s)
        {
            double ellapsed = time();
            double remain = s - ellapsed;
            if(remain < 0)
                return ellapsed;
            std::this_thread::sleep_for(std::chrono::nanoseconds((long long)(remain * 1e9)));
            ellapsed += time();
            return ellapsed;
        }

        void addData(Data &data)
        {
            dataPoints.push_back(data);
        }

        void fillData(std::vector<Data> &data)
        {
            dataPoints = data;
        }

        std::vector<Data>& data()
        {
            return dataPoints;
        }

        void setModel(ModelFunction fn)
        {
            modelFn = fn;
        }

        void setSensor(SensorFunction fn)
        {
            sensorFn = fn;
        }

        void setModelJacobian(ModelJacobianFunction fn)
        {
            modelJFn = fn;
        }

        void setSensorJacobian(SensorJacobianFunction fn)
        {
            sensorJFn = fn;
        }

        virtual void model(State &x, Input &u, double dt)
        {

        }

        virtual void sensor(Output &z, State &x, Data &d, double dt)
        {

        }

        virtual void modelJacobian(ModelJacobian &F, State &x, Input &u, double dt)
        {

        }

        virtual void sensorJacobian(SensorJacobian &H, State &x, Data &d, double dt)
        {

        }

        void simulate(State &x, Output &y, Input &u, double dt)
        {
            Data data;
            randn(data);

            if(dataPoints.size() > 0)
                data = dataPoints[0];

            doModel(x, u, dt);
            randn(randX);
            x = x + Qsqrt * randX;

            doSensor(y, x, data, dt);
            randn(randY);
            y = y + Rsqrt * randY;
        }

        void simulate(State &x, std::vector<Output> &y, Input &u, double dt)
        {
            doModel(x, u, dt);
            randn(randX);
            x = x + Qsqrt * randX;

            int j = 0, index = 0;
            if(dataPoints.size() == 0)
                j = -1;

            MatDx1 data;
            for(int i = 0; i < y.size(); i++)
            {
                if(j == -1)
                {
                    randn(data);
                    doSensor(y[i], x, data, dt);
                }
                else
                {
                    doSensor(y[i], x, dataPoints[j], dt);
                    j = rand() % dataPoints.size();
                }
                randn(randY);
                y[i] = y[i] + Rsqrt * randY;
            }
        }

        void run(State &xK, Output &y, Input &u, double dt)
        {
            predict(u, dt);

            std::vector<MatDx1> data(1);
            std::vector<MatPx1> ys(1);
            ys[0] = y;
            dataAssoc(ys, data, dt);

            update(y, data[0], dt);
            xK = x;
        }

        void run(State &xK, std::vector<Output> &y, Input &u, double dt)
        {
            predict(u, dt);

            if(y.size() > 0)
            {
                std::vector<MatDx1> data(y.size());
                dataAssoc(y, data, dt);

                for(int i = 0; i < y.size(); i++)
                    update(y[i], data[i], dt);
            }
            xK = x;
        }

    private:
        void predict(Input &u, double dt)
        {
            x_1 = x;
            doModel(x, u, dt);
            doModelJ(F, x_1, u, dt);
            P = F * P * F.transpose() + Q;
        }

        void update(Output &y, Data &d, double dt)
        {
            doSensorJ(H, x, d, dt);

            doSensor(z, x, d, dt);
            S = ( H * P * H.transpose() ) + R;
            K = P * H.transpose() * S.inverse();
            x = x + K * (y - z);
            P = (I - K * H) * P;
        }

        void dataAssoc(std::vector<Output> &y, std::vector<Data> &data, double dt)
        {
            if(dataPoints.size() == 0)
                return;
            MatPx1 v;
            dataType minX, X;
            int minJ;
            Matrix<dataType, 1, 1> Xsq;
            Data d;

            for(int i = 0; i < y.size(); i++)
            {
                minX = 0;
                minJ = -1;
                for (int j = 0; j < dataPoints.size(); j++)
                {
                    d = dataPoints[j];
                    doSensor(z, x, d, dt);
                    v = z - y[i];
                    doSensorJ(H, x, d, dt);
                    S = H * P * H.transpose() + R;
                    Xsq = v.transpose() * S.inverse() * v;
                    X = sqrt(Xsq(0));
                    if(minJ < 0)
                    {
                        minJ = j;
                        minX = X;
                    }
                    else if(X < minX)
                    {
                        minJ = j;
                        minX = X;
                    }
                }
                data[i] = dataPoints[minJ];
            }
        }

        void doModel(State &x, Input &u, double dt)
        {
            if(modelFn != NULL)
                modelFn(x, u, dt);
            else
                model(x, u, dt);
        }

        void doSensor(Output &z, State &x, Data &d, double dt)
        {
            if(sensorFn != NULL)
                sensorFn(z, x, d, dt);
            else
                sensor(z, x, d, dt);
        }

        void doModelJ(ModelJacobian &F, State &x, Input &u, double dt)
        {
            if(modelJFn != NULL)
                modelJFn(F, x, u, dt);
            else
                modelJacobian(F, x, u, dt);
        }

        void doSensorJ(SensorJacobian &H, State &x, Data &d, double dt)
        {
            if(sensorJFn != NULL)
                sensorJFn(H, x, d, dt);
            else
                sensorJacobian(H, x, d, dt);
        }

        template<class T>
        void randn(T &mat)
        {
            for (size_t i = 0; i < mat.rows(); i++)
            {
                for (size_t j = 0; j < mat.cols(); j++)
                {
                    mat(i, j) = distr(gen);
                }
            }
        }

};


//
// Expansion Hunter
// Copyright 2016-2019 Illumina, Inc.
// All rights reserved.
//
// Author: Egor Dolzhenko <edolzhenko@illumina.com>,
//         Mitch Bekritsky <mbekritsky@illumina.com>, Richard Shaw
// Concept: Michael Eberle <meberle@illumina.com>
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//      http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
//
//

#include "genotyping/RegionLengthEstimation.hh"

#include <boost/lexical_cast.hpp>
#include <boost/math/distributions.hpp>
#include <boost/math/distributions/binomial.hpp>

#include <cmath>
#include <iostream>
#include <random>
#include <vector>

using boost::lexical_cast;
using boost::math::cdf;
using boost::math::poisson_distribution;
using std::round;
using std::vector;

namespace ehunter
{

// Uses the standard Lander-Waterman model to estimate length of a workflow. The confidence interval is computed using a
// generic parametric bootstrap procedure (note that a simpler implementation using Poisson mean CI is possible).
void estimateRegionLength(
    int readCount, int readLength, double depth, int& regionLength, int& lowerBound, int& upperBound)
{
    const double proportionOfReadsStartAtPosition = depth / readLength;
    // The length of sub-workflow where reads can start and still be fully within the workflow
    const int extensionLength = static_cast<int>(round(readCount / proportionOfReadsStartAtPosition));

    const int kSeed = 42;
    std::mt19937 numberGenerator(kSeed);

    // Model for the number of reads that fall within the workflow
    std::poisson_distribution<> poisson(readCount);

    vector<int> bootstrapSamples;
    const int kNumSamples = 10000;
    for (int sampleIndex = 0; sampleIndex < kNumSamples; ++sampleIndex)
    {
        const int sampledReadCount = poisson(numberGenerator);
        const int sampledExtensionLength = static_cast<int>(round(sampledReadCount / proportionOfReadsStartAtPosition));
        const int bootstrapSample = sampledExtensionLength - extensionLength;

        bootstrapSamples.push_back(bootstrapSample);
    }

    // Compute 2.5% and 97.5% quantiles
    std::sort(bootstrapSamples.begin(), bootstrapSamples.end());
    const int lowerQuantile = *(bootstrapSamples.begin() + static_cast<int>(bootstrapSamples.size() * 0.025));
    const int upperQuantile = *(bootstrapSamples.begin() + static_cast<int>(bootstrapSamples.size() * 0.975));

    regionLength = extensionLength + readLength;

    lowerBound = readLength;
    lowerBound += extensionLength - upperQuantile > 0 ? extensionLength - upperQuantile : 0;

    upperBound = readLength;
    upperBound += extensionLength - lowerQuantile > 0 ? extensionLength - lowerQuantile : 0;
}

}


/** \defgroup main Главный модуль программы
    @{
*/

#include <iostream>
#include <cmath>
#include <vector>
#include <algorithm>
#include <chrono>
#include <thread>
#include <sstream>
#include <fstream>
#include <list>
#include <mutex>

#include <boost/asio/thread_pool.hpp>
#include <boost/asio/post.hpp>

//#define debug

using namespace std;
using namespace chrono;

/// Миллисекунды в формате дробного числа.
using d_milliseconds = duration<double, milliseconds::period>;

/// Массив с базовыми простыми числами (результат первого этапа алгоритма).
using FirstStepResult = vector<size_t>;

/// Массив с найденными простыми числами в ходе выполнения второго этапа алгоритма.
using SecondStepResult = vector<uint64_t>;

/// Вывести содержимое контейнера (где есть begin и end).
template <typename C>
string displayContainer(const C& container)
{
    typename C::const_iterator it;

    stringstream ss;

    for (it = container.begin(); it != container.end(); ++it)
    {
        ss << *it << " ";
    }

    ss << endl;

    return ss.str();
}

/**
 * @brief Бенчмарк.
 * @param func Выполняемая функция.
 * @param args Аргументы выполняемой функции.
 * @return Результат вычисления и время выполнения функции.
 */
template <typename Result, typename Function, typename... Args>
pair<double, Result> timeBenchmark(const Function& func, const Args&... args)
{
    Result calc;

    // начинаем считать время
    auto start = steady_clock::now();

    // что-то вычисляем
    calc = func(args...);

    // заканчиваем считать время
    auto end = steady_clock::now();

    auto time = duration_cast<d_milliseconds>(end-start).count();

    return make_pair(time, calc);
}

/// Запись данных в файл.
template <typename T>
void writeToFile(const string& filename, const T& data)
{
    ofstream f(filename);

    if (f)
    {
        for (auto c: data)
        {
            f << c << " ";
        }
    }
}

/**
 * @brief Первый этап модицифированного последовательного поиска простых чисел.
 * @details
 * Классический метод - решето Эратосфена.
 * Поиск в интервале [2; n]
 * @param n Верхняя граница поиска простых чисел.
 * @return Массив с базовыми простыми числами.
 */
FirstStepResult getBasePrimeBySieveOfEratosthenes(const size_t n)
{
    FirstStepResult basePrime;

    vector<uint8_t> range(n+1, 0);
    for (size_t i = 2; i <= n; ++i)
    {
        // если непомеченное простое число
        if (!range[i])
        {
            basePrime.push_back(i);
            // пометим другие числа на расстоянии шага i как составные
            for (size_t j = i + i; j <= n; j += i)
            {
                range[j] = 1;
            }
        }
    }

    return basePrime;
}

/**
 * @brief Второй этап модифицированного последовательного поиска простых чисел.
 * @param i_begin Начальный индекс поиска
 * @param i_end Конечный индекс поиска
 * @param basePrime Массив с базовыми простыми числами
 * @return Массив с простыми числами, найденные в диапазоне от (i_begin; i_end]
 */
SecondStepResult seqModSearch(const uint64_t i_begin,
                              const uint64_t i_end,
                              const FirstStepResult &basePrime
                              )
{
    SecondStepResult primeNumbers;

    for (uint64_t number = i_begin + 1; number <= i_end; ++number)
    {
        auto isDivide = [&](uint64_t basePrimeNumber)
        {
            return number % basePrimeNumber == 0;
        };

        const auto res = find_if(basePrime.begin(), basePrime.end(), isDivide);

        if (res == basePrime.end())
        {
            primeNumbers.push_back(number);
        }
    }

    return primeNumbers;
}

/**
 * @brief Второй этап модифицированного последовательного поиска простых чисел в полном диапазоне.
 * @param[in] i_begin Начальный индекс поиска
 * @param[in] i_end Конечный индекс поиска
 * @param[in] basePrime Массив с базовыми простыми числами
 * @param[in] basePrimeBegin Начальный индекс массива с базовыми простыми числами
 * @param[in] basePrimeEnd Конечный индекс массива с базовыми простыми числами
 * @param[out] fullRange Полный диапазон чисел, где 0 - простое, 1 - составное число.
 */
void seqModSearchInFullRange(const uint64_t i_begin,
                             const uint64_t i_end,
                             const FirstStepResult &basePrime,
                             const ptrdiff_t basePrimeBegin,
                             const ptrdiff_t basePrimeEnd,
                             vector<uint8_t> &fullRange
                             )
{
    uint64_t firstNumber = i_begin + 1;
    for (uint64_t number = firstNumber; number <= i_end; ++number)
    {
        auto isDivide = [&](uint64_t basePrimeNumber)
        {
            return number % basePrimeNumber == 0;
        };

        const auto res = find_if(basePrime.begin() + basePrimeBegin, basePrime.begin() + basePrimeEnd, isDivide);

        if (res != basePrime.begin() + basePrimeEnd)
        {
            fullRange[number - firstNumber] = 1;
        }
    }
}

/**
 * @brief Второй этап модифицированного последовательного поиска простых чисел в полном диапазоне по одному простому числу.
 * @param[in] i_begin Начальный индекс поиска
 * @param[in] i_end Конечный индекс поиска
 * @param[in] prime Простое число
 * @param[out] fullRange Полный диапазон чисел, где 0 - простое, 1 - составное число.
 */
void seqModSearchInFullRangeByOnePrime(const uint64_t i_begin,
                                       const uint64_t i_end,
                                       const size_t prime,
                                       vector<uint8_t> &fullRange
                                       )
{
    uint64_t firstNumber = i_begin + 1;
    for (uint64_t number = firstNumber; number <= i_end; ++number)
    {
        if (number % prime == 0)
        {
            fullRange[number - firstNumber] = 1;
        }
    }
}

/// Параллельная декомпозиция по данным.
SecondStepResult parDecompositionByData(uint64_t begin,
                                        uint64_t n,
                                        const FirstStepResult &basePrime,
                                        uint8_t threadCount
                                        )
{
#ifdef debug
    cout << "> Par decomposition by data [" << int(threadCount) << " threads] start..." << endl;
#endif

    // Результат - массив с простыми числами из диапазона от (begin, n].
    SecondStepResult prime;

    list<SecondStepResult> results;

    double range = double(n - begin) / double(threadCount);

#ifdef debug
    cout << "data range: " << range << endl;
#endif

    list<thread> threads;

    for (size_t i = 0; i < threadCount; ++i)
    {
        const auto i_begin = uint64_t(llround(double(i) * range)) + begin;
        const auto i_end = uint64_t(llround(double(i) * range + range)) + begin;

#ifdef debug
        cout << i << " thread - " << "begin: " << i_begin << " " << "end: " << i_end << endl;
#endif

        const auto threadHandler = [i_begin, i_end, &basePrime, &results]()
        {
            results.push_back(seqModSearch(i_begin, i_end, ref(basePrime)));
        };

        thread t { threadHandler };
        threads.push_back(move(t));
    }

    for (auto &t : threads)
    {
        t.join();
    }

    results.sort([](const SecondStepResult& v1, const SecondStepResult& v2)
    {
        return v1.front() < v2.front();
    });

    for (const auto &res: results)
    {
        prime.insert(prime.end(), res.begin(), res.end());
    }

    return prime;
}

/// Последовательная декомпозиция набора простых чисел.
SecondStepResult seqDecompositionByBasePrime(const uint64_t begin,
                                             const uint64_t n,
                                             const FirstStepResult &basePrime
                                             )
{
    const auto diff = n - begin;
    // диапазон чисел (begin, n]
    // 1  - если составное
    // 0 - если простое
    vector<uint8_t> fullRange(diff, 0);

    // Результат - массив с простыми числами из диапазона от (begin, n].
    SecondStepResult prime;

    seqModSearchInFullRange(begin, n, basePrime, ptrdiff_t(0), ptrdiff_t(basePrime.size()-1), fullRange);

    for (size_t i = 0; i < diff; ++i)
    {
        if (!fullRange[i])
        {
            prime.push_back(i + (begin + 1));
        }
    }

    return prime;
}

/// Параллельная декомпозиция набора простых чисел.
SecondStepResult parDecompositionByBasePrime(const uint64_t begin,
                                             const uint64_t n,
                                             const FirstStepResult &basePrime,
                                             const uint8_t threadCount
                                             )
{

#ifdef debug
    cout << "> Par decomposition by base prime [" << int(threadCount) << " threads] start..." << endl;
#endif

    const auto diff = n - begin;
    // диапазон чисел (begin, n]
    // 1  - если составное
    // 0 - если простое
    vector<uint8_t> fullRange(diff, 0);

    // Результат - массив с простыми числами из диапазона от (begin, n].
    SecondStepResult prime;

    double rangeForThread = double(basePrime.size()) / double(threadCount);

#ifdef debug
    cout << "base prime range for thread: " << rangeForThread << endl;
#endif

    list<thread> threads;

    for (size_t i = 0; i < threadCount; ++i)
    {
        const auto i_begin = ptrdiff_t(llround(double(i) * rangeForThread));
        const auto i_end = ptrdiff_t(llround(double(i) * rangeForThread + rangeForThread));

#ifdef debug
        cout << i << " thread - " << "begin: " << i_begin << " " << "end: " << i_end << endl;
#endif

        if (i_begin == i_end)
        {

#ifdef debug
            cout << "skip thread, because i_begin == i_end" << endl;
#endif
            continue;
        }

        thread t { seqModSearchInFullRange, begin, n, ref(basePrime), i_begin, i_end, ref(fullRange) };
        threads.push_back(move(t));
    }

    for (auto &t : threads)
    {
        t.join();
    }

    for (size_t i = 0; i < diff; ++i)
    {
        if (!fullRange[i])
        {
            prime.push_back(i + (begin + 1));
        }
    }

    return prime;
}

/// Параллельный алгоритм с использованием Thread Pool
SecondStepResult parThreadPool(uint64_t begin,
                               uint64_t n,
                               const FirstStepResult &basePrime,
                               const uint8_t threadCount
                               )
{

#ifdef debug
    cout << "> Par thread pool start..." << endl;
#endif

    const auto diff = n - begin;
    // диапазон чисел (begin, n]
    // 1  - если составное
    // 0 - если простое
    vector<uint8_t> fullRange(diff, 0);

    // Результат - массив с простыми числами из диапазона от (begin, n].
    SecondStepResult prime;

    const auto basePrimeSize = basePrime.size();

    boost::asio::thread_pool pool(threadCount);

    for (size_t i = 0; i < basePrimeSize; ++i)
    {
        const auto prime = basePrime[i];

        boost::asio::post(pool, [prime, begin, n, &fullRange]()
        {
            seqModSearchInFullRangeByOnePrime(begin, n, prime, fullRange);
        });

    }

    // ожидаем все потоки в пуле
    pool.join();

    for (size_t i = 0; i < diff; ++i)
    {
        if (!fullRange[i])
        {
            prime.push_back(i + (begin + 1));
        }
    }

    return prime;
}

/// Параллельный алгоритм с использованием Thread Pool
SecondStepResult parPrimeEnumeration(uint64_t begin,
                                     uint64_t n,
                                     const FirstStepResult &basePrime,
                                     const uint8_t threadCount
                                     )
{

#ifdef debug
    cout << "> Par prime enumeration [" << int(threadCount) << " threads] start..." << endl;
#endif

    const auto diff = n - begin;
    // диапазон чисел (begin, n]
    // 1  - если составное
    // 0 - если простое
    vector<uint8_t> fullRange(diff, 0);

    const auto basePrimeSize = basePrime.size();

    list<thread> threads;

    size_t currentPrimeIndex = 0;
    mutex m;

    for (size_t i = 0; i < threadCount; ++i)
    {
        auto handler = [begin, n, basePrimeSize, &basePrime, &m, &currentPrimeIndex, &fullRange]()
        {
            while (true)
            {
                unique_lock lck {m};

                if (currentPrimeIndex >= basePrimeSize)
                {
                    break;
                }

                auto prime = basePrime[currentPrimeIndex++];

                // разблокируем мьютекс, так как перестали работать
                // с разделяемым ресурсом (индекс)
                lck.unlock();

                seqModSearchInFullRangeByOnePrime(begin, n, prime, fullRange);
            }
        };

        thread t { handler };
        threads.push_back(move(t));
    }

    for (auto &t : threads)
    {
        t.join();
    }

    // Результат - массив с простыми числами из диапазона от (begin, n].
    SecondStepResult prime;

    for (size_t i = 0; i < diff; ++i)
    {
        if (!fullRange[i])
        {
            prime.push_back(i + (begin + 1));
        }
    }

    return prime;
}

int main(int argc, char *argv[])
{
    cout << thread::hardware_concurrency() << endl;

    constexpr size_t default_n = 100;

    auto n = default_n;

    if (argc >= 2)
    {
        n = stoull(argv[1]);
    }

    const auto sqrtN = static_cast<size_t>(sqrt(n));

    // Первый шаг.
    const auto firstStepRes = timeBenchmark<FirstStepResult>(getBasePrimeBySieveOfEratosthenes, sqrtN);
    cout << "> First step: " << firstStepRes.first << " ms" << endl;
    writeToFile("firstStep.txt", firstStepRes.second);

    // Последовательная декомпозиция по данным
    const auto seqDecompositionByDataRes = timeBenchmark<SecondStepResult>(seqModSearch, sqrtN, n, ref(firstStepRes.second));
    cout << "> Seq decomposition by data: " << seqDecompositionByDataRes.first << " ms" << endl;
    writeToFile("SeqDecompositionByData.txt", seqDecompositionByDataRes.second);

    // Параллельная декомпозиция по данным: threadCount потоков
    const auto doParDecompositionByData = [&](uint8_t threadCount)
    {
        const auto parDecompositionByDataRes = timeBenchmark<SecondStepResult>(parDecompositionByData, sqrtN, n, ref(firstStepRes.second), threadCount);
        cout << "> Par decomposition by data [" + to_string(threadCount) + " threads]: " << parDecompositionByDataRes.first << " ms" << endl;
        writeToFile("ParDecompositionByData_" + to_string(threadCount) + "t.txt", parDecompositionByDataRes.second);
    };

    doParDecompositionByData(2);
    doParDecompositionByData(4);
    doParDecompositionByData(8);

    // Последовательная декомпозиция набора простых чисел
    const auto seqDecompositionByBasePrimeRes = timeBenchmark<SecondStepResult>(seqDecompositionByBasePrime, sqrtN, n, ref(firstStepRes.second));
    cout << "> Seq decomposition by base prime set: " << seqDecompositionByBasePrimeRes.first << " ms" << endl;
    writeToFile("SeqDecompositionByBasePrime.txt", seqDecompositionByBasePrimeRes.second);

    // Параллельная декомпозиция набора простых чисел: threadCount потоков
    const auto doParDecompositionByBasePrime = [n, sqrtN, &firstStepRes](uint8_t threadCount)
    {
        const auto parDecompositionByBasePrimeRes = timeBenchmark<SecondStepResult>(parDecompositionByBasePrime, sqrtN, n, ref(firstStepRes.second), threadCount);
        cout << "> Par decomposition by base prime set [" + to_string(threadCount) + " threads]: " << parDecompositionByBasePrimeRes.first << " ms" << endl;
        writeToFile("ParDecompositionByBasePrime_" + to_string(threadCount) + "t.txt", parDecompositionByBasePrimeRes.second);
    };

    doParDecompositionByBasePrime(2);
    doParDecompositionByBasePrime(4);
    doParDecompositionByBasePrime(8);

    // С применением Thread Pool
    const auto doParThreadPool = [n, sqrtN, &firstStepRes]()
    {
        const auto parThreadPoolRes = timeBenchmark<SecondStepResult>(parThreadPool, sqrtN, n, ref(firstStepRes.second), uint8_t(thread::hardware_concurrency()));
        cout << "> Par thread pool [" << thread::hardware_concurrency() << " threads] [" + to_string(firstStepRes.second.size()) + " tasks]: " << parThreadPoolRes.first << " ms" << endl;
        writeToFile("ParThreadPool.txt", parThreadPoolRes.second);
    };

    // Пул потоков
    doParThreadPool();

    // С последовательным перебором простых чисел
    const auto doParPrimeEnumeration = [n, sqrtN, &firstStepRes](uint8_t threadCount)
    {
        const auto parPrimeEnumerationRes = timeBenchmark<SecondStepResult>(parPrimeEnumeration, sqrtN, n, ref(firstStepRes.second), threadCount);
        cout << "> Par prime enumeration [" << int(threadCount) << " threads]: " << parPrimeEnumerationRes.first << " ms" << endl;
        writeToFile("ParPrimeEnumeration_" + to_string(threadCount) + "t.txt", parPrimeEnumerationRes.second);
    };

    doParPrimeEnumeration(2);
    doParPrimeEnumeration(4);
    doParPrimeEnumeration(8);

    system("pause");
    return 0;
}
/** @} */


#include <iostream>
#include <cassert>
#include <iomanip>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/graph/push_relabel_max_flow.hpp>

const int debug_level = 0;

#define DEBUG(min_level, x)      \
  if (debug_level >= min_level)  \
  {                              \
    std::cerr << x << std::endl; \
  }

typedef boost::adjacency_list_traits<boost::vecS, boost::vecS, boost::directedS> GraphTraits;
typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::directedS, boost::no_property,
                              boost::property<boost::edge_capacity_t, int,
                                              boost::property<boost::edge_residual_capacity_t, int,
                                                              boost::property<boost::edge_reverse_t, GraphTraits::edge_descriptor,
                                                                              boost::property<boost::edge_weight_t, int>>>>>
    Graph;

const int unreachable_distance = std::numeric_limits<int>::max();

bool check_shelter_reachable_in_t(const std::vector<std::vector<int>> &distance_matrix, const int t)
{
  const int a = distance_matrix.size();
  const int s = distance_matrix.at(0).size(); // HACK This might be double the s in testcase()

  int next_free_node = 0;
  const int node_source = next_free_node++;
  const int node_sink = next_free_node++;
  const auto get_agent_node = [next_free_node, a](int i) {
    assert(i >= 0 && i < a);
    return next_free_node + i;
  };
  next_free_node += a;
  const auto get_shelter_node = [next_free_node, s](int i) {
    assert(i >= 0 && i < s);
    return next_free_node + i;
  };
  next_free_node += s;
  const int num_nodes = next_free_node;
  Graph G(num_nodes);

  const auto add_edge = [&G](int from, int to) {
    DEBUG(5, "add_edge(" << from << ", " << from << ")");
    auto c_map = boost::get(boost::edge_capacity, G);
    auto r_map = boost::get(boost::edge_reverse, G);
    const Graph::edge_descriptor e = boost::add_edge(from, to, G).first;
    const Graph::edge_descriptor rev_e = boost::add_edge(to, from, G).first;
    c_map[e] = 1;
    c_map[rev_e] = 0;
    r_map[e] = rev_e;
    r_map[rev_e] = e;
  };

  for (int i_a = 0; i_a < a; i_a++)
  {
    add_edge(node_source, get_agent_node(i_a));
  }
  for (int i_s = 0; i_s < s; i_s++)
  {
    add_edge(get_shelter_node(i_s), node_sink);
  }
  for (int i_a = 0; i_a < a; i_a++)
  {
    for (int i_s = 0; i_s < s; i_s++)
    {
      const int dist = distance_matrix.at(i_a).at(i_s);
      if (dist != unreachable_distance && dist <= t)
      {
        add_edge(get_agent_node(i_a), get_shelter_node(i_s));
      }
    }
  }

  const int flow = boost::push_relabel_max_flow(G, node_source, node_sink);
  DEBUG(3, "t " << t << " flow " << flow);
  assert(flow >= 0 && flow <= a && flow <= s);
  return flow == a;
}

int find_lower_bound(std::function<bool(int)> is_match)
{
  if (is_match(0))
  {
    return 0;
  }

  int base = 0;
  while (!is_match(1 << base))
  {
    base++;
  }

  int low = base == 0 ? 0 : 1 << (base - 1);
  int high = 1 << base;
  assert(!is_match(low) && is_match(high));
  while (low < high)
  {
    const int mid = (low + high) / 2;
    if (is_match(mid))
    {
      high = mid;
    }
    else
    {
      low = mid + 1;
    }
  }
  assert(!is_match(low - 1) && is_match(low));
  return low;
}

void testcase()
{
  int n, m, a, s, c, d;
  std::cin >> n >> m >> a >> s >> c >> d;
  assert(n >= 1 && n <= 1e3 && m >= 0 && m <= 5e3);
  assert(a >= 1 && a <= 1e2 && s >= 1 && s <= 1e2);
  assert(c >= 1 && c <= 2);
  assert(d >= 1 && d <= 1e3);

  Graph G(n);
  const auto add_edge = [&G](int from, int to, int weight) {
    auto w_map = boost::get(boost::edge_weight, G);
    const auto e_res = boost::add_edge(from, to, G);
    const Graph::edge_descriptor e = e_res.first;
    const bool is_new = e_res.second;
    w_map[e] = is_new ? weight : std::min(w_map[e], weight);
  };
  for (int i = 0; i < m; i++)
  {
    std::string w_string;
    int x, y, z;
    std::cin >> w_string >> x >> y >> z;
    assert(w_string == "S" || w_string == "L");
    assert(x >= 0 && x < n && y >= 0 && y < n && z >= 1 && z <= 1e4);
    const bool is_lift = w_string == "L";

    add_edge(x, y, z);
    if (is_lift)
    {
      add_edge(y, x, z);
    }
  }

  std::vector<int> nodes_by_agent(a);
  for (int &node : nodes_by_agent)
  {
    std::cin >> node;
    assert(node >= 0 && node < n);
  }

  std::vector<int> nodes_by_shelter(s);
  for (int &node : nodes_by_shelter)
  {
    std::cin >> node;
    assert(node >= 0 && node < n);
  }

  std::vector<std::vector<int>> distance_matrix(a, std::vector<int>(c * s, unreachable_distance));
  std::vector<int> temp_distances(n);
  auto temp_distance_map = boost::make_iterator_property_map(temp_distances.begin(), boost::get(boost::vertex_index, G));
  for (int i_a = 0; i_a < a; i_a++)
  {
    boost::dijkstra_shortest_paths(G, nodes_by_agent.at(i_a), boost::distance_map(temp_distance_map).distance_inf(unreachable_distance));
    for (int i_s = 0; i_s < s; i_s++)
    {
      const int dist = temp_distances.at(nodes_by_shelter.at(i_s));
      if (dist != unreachable_distance)
      {
        for (int i_c = 0; i_c < c; i_c++)
        {
          const int virtual_i_s = i_c * s + i_s;
          distance_matrix.at(i_a).at(virtual_i_s) = dist + i_c * d;
          DEBUG(4, "distance_matrix.at(" << i_a << ").at(" << virtual_i_s << ") = " << distance_matrix.at(i_a).at(virtual_i_s));
        }
      }
    }
  }

  int t = find_lower_bound([&distance_matrix](const int test_t) {
    return check_shelter_reachable_in_t(distance_matrix, test_t);
  });
  std::cout << t + d << "\n";
}

int main()
{
  std::ios_base::sync_with_stdio(false);
  std::cout << std::fixed << std::setprecision(0);

  int t;
  std::cin >> t;
  for (int i = 0; i < t; i++)
  {
    testcase();
    DEBUG(1, "");
  }

  return 0;
}


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

#include "micro_graph_optimizer.h"

#include <Eigen/Geometry>
#include <string>
#include <iostream>
#include <map>
#include <fstream>

using namespace mgo;

// This example demonstrates optimzing a non-linear 2D SLAM problem read from a g2o file.

double normalize_angle(double theta_rad)
{
  // Normalize the angle to the range [-pi, pi).
  constexpr double kPI = 3.14159265358979323846;
  constexpr double k2PI = 2.0 * kPI;
  return (theta_rad - k2PI * std::floor((theta_rad + kPI) / k2PI));
}

class Pose2d : public Variable
{
public:
  Pose2d(double x, double y, double yaw_rad) :
    m_x(x), m_y(y), m_yaw_rad(yaw_rad)
  {
  }

  virtual int dim()const override { return 3; }
  virtual void plus(const Eigen::VectorXd& delta) override
  {
    m_x += delta[0];
    m_y += delta[1];
    m_yaw_rad = normalize_angle(m_yaw_rad + delta[2]);
  }

  double x()const { return m_x; }
  double y()const { return m_y; }
  double yaw_rad()const { return m_yaw_rad; }

private:
  double m_x, m_y, m_yaw_rad;
};

class Constraint2d : public Factor
{
public:
  Constraint2d(Pose2d* v_a, Pose2d* v_b, double x_ab, double y_ab,
    double yaw_ab_rad, const Eigen::Matrix3d& sqrt_info) :
    m_pos_ab(x_ab, y_ab), m_yaw_ab_rad(yaw_ab_rad), m_sqrt_info(sqrt_info)
  {
    add_variable(v_a);
    add_variable(v_b);
  }

  virtual int dim()const { return 3; }

  virtual Eigen::VectorXd error()const override
  {
    MGO_ASSERT(this->num_variables() == 2);
    const Pose2d* v_a = static_cast<Pose2d*>(this->variable_at(0));
    const Pose2d* v_b = static_cast<Pose2d*>(this->variable_at(1));
    Eigen::Vector3d r;
    Eigen::Vector2d pos_ab_pred = { v_b->x() - v_a->x(), v_b->y() - v_a->y() };
    r.head<2>() = Eigen::Rotation2Dd(v_a->yaw_rad()).toRotationMatrix().transpose() * pos_ab_pred - m_pos_ab;
    r(2) = normalize_angle((v_b->yaw_rad() - v_a->yaw_rad()) - m_yaw_ab_rad);
    return r;
  }

  virtual Eigen::VectorXd subtract_error(const Eigen::VectorXd& e1, const Eigen::VectorXd& e2)const override
  {
    Eigen::Vector3d diff;
    diff << (e1(0) - e2(0)), (e1(1) - e2(1)), normalize_angle(e1(2) - e2(2));
    return diff;
  }

  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

private:
  Eigen::Vector2d m_pos_ab;
  double m_yaw_ab_rad;
  Eigen::Matrix3d m_sqrt_info;
};

bool read_g2o(const std::string& filename, mgo::FactorGraph* graph)
{
  std::ifstream file(filename);
  if (!file.is_open())
  {
    MGO_LOG("Failed to open file: %s", filename.c_str());
    return false;
  }

  std::string line;
  std::map<int, Pose2d*> id_to_pose;
  while (std::getline(file, line))
  {
    std::stringstream ss(line);
    std::string data_type;
    ss >> data_type;
    if (data_type == "VERTEX_SE2")
    {
      int id;
      double x, y, th;
      ss >> id >> x >> y >> th;
      Pose2d* p = new Pose2d(x, y, normalize_angle(th));
      graph->add_variable(p);
      id_to_pose[id] = p;
    }
    else if (data_type == "EDGE_SE2")
    {
      int id_a, id_b;
      double dx, dy, d_yaw, i11, i12, i13, i22, i23, i33;
      ss >> id_a >> id_b >> dx >> dy >> d_yaw >> i11 >> i12 >> i13 >> i22 >> i23 >> i33;
      Eigen::Matrix3d info_mtrx = (Eigen::Matrix3d() <<
        i11, i12, i13,
        i12, i22, i23,
        i13, i23, i33).finished();
      MGO_ASSERT(id_to_pose.count(id_a) != 0);
      MGO_ASSERT(id_to_pose.count(id_b) != 0);
      graph->add_factor(new Constraint2d(id_to_pose[id_a], id_to_pose[id_b], dx, dy,
        d_yaw, info_mtrx.llt().matrixL()));
    }
    else
    {
      MGO_LOG("Unhandled type: %s", data_type.c_str());
      return false;
    }
  }
  return true;
}

void dump_poses(const std::string& filename, const FactorGraph& graph)
{
  std::ofstream file(filename);
  if (!file.is_open())
  {
    MGO_LOG("Failed to open file: %s", filename.c_str());
    return;
  }

  const std::vector<Variable*>& variables = graph.get_variables();
  for (int i = 0, count = variables.size(); i < count; ++i)
  {
    Pose2d* p = static_cast<Pose2d*>(variables[i]);
    file << i << " " << p->x() << " " << p->y() << " " << p->yaw_rad() << std::endl;
  }
}

int main()
{
  FactorGraph graph;
  // You can get this dataset from: https://lucacarlone.mit.edu/datasets/
  if (!read_g2o("./input_M3500_g2o.g2o", &graph))
  {
    return -1;
  }

  // Fix the first variable.
  graph.get_variables()[0]->fixed = true;

  dump_poses("./original.txt", graph);
  mgo::optimize_gn(&graph);
  dump_poses("./optimized.txt", graph);

  return 0;
}


#include <bits/stdc++.h>
#include <iostream>
#include <vector>
#include <Eigen/Dense>
#define EIGEN_DONT_PARALLELIZE

const double PI	=	3.1415926535897932384;
#include <map>
struct pts2D {
	double x,y;
};

class kernel {
public:
  double a;
  std::vector<pts2D> particles_X;
	std::vector<pts2D> particles_Y;

	kernel(std::vector<pts2D>& particles_X, std::vector<pts2D>& particles_Y) {
			this->particles_X = particles_X;
			this->particles_Y = particles_Y;
	}

	virtual double getMatrixEntry(const unsigned i, const unsigned j) {
		std::cout << "virtual getInteraction" << std::endl;
		return 0.0;
	}

	Eigen::VectorXd getRow(const int j, std::vector<int> col_indices) {
		int n_cols = col_indices.size();
		Eigen::VectorXd row(n_cols);
    #pragma omp parallel for
    for(int k = 0; k < n_cols; k++) {
        row(k) = this->getMatrixEntry(j, col_indices[k]);
    }
    return row;
  }

  Eigen::VectorXd getCol(const int k, std::vector<int> row_indices) {
		int n_rows = row_indices.size();
    Eigen::VectorXd col(n_rows);
    #pragma omp parallel for
    for (int j=0; j<n_rows; ++j) {
			col(j) = this->getMatrixEntry(row_indices[j], k);
    }
    return col;
  }

  Eigen::MatrixXd getMatrix(std::vector<int> row_indices, std::vector<int> col_indices) {
		int n_rows = row_indices.size();
		int n_cols = col_indices.size();
    Eigen::MatrixXd mat(n_rows, n_cols);
    #pragma omp parallel for
    for (int j=0; j < n_rows; ++j) {
        #pragma omp parallel for
        for (int k=0; k < n_cols; ++k) {
            mat(j,k) = this->getMatrixEntry(row_indices[j], col_indices[k]);
        }
    }
    return mat;
  }
  ~kernel() {};
};

class userkernel: public kernel {
public:
	double chargesFunction(const pts2D r) {
		double q = r.x; //user defined
		return q;
	};
	// #ifdef ONEOVERR
	// userkernel(std::vector<pts2D>& particles_X, std::vector<pts2D>& particles_Y): kernel(particles_X, particles_Y) {
	// };
	// double getMatrixEntry(const unsigned i, const unsigned j) {
	// 	pts2D r1 = particles_X[i];
	// 	pts2D r2 = particles_X[j];
	// 	double R2	=	(r1.x-r2.x)*(r1.x-r2.x) + (r1.y-r2.y)*(r1.y-r2.y);
	// 	double R	=	sqrt(R2);
	// 	if (R < a) {
	// 		return R/a;
	// 	}
	// 	else {
	// 		return a/R;
	// 	}
	// }
	// #elif LOGR
	userkernel(std::vector<pts2D> particles_X, std::vector<pts2D> particles_Y): kernel(particles_X, particles_Y) {
	};
	double getMatrixEntry(const unsigned i, const unsigned j) {
		pts2D r1 = particles_X[i];
		pts2D r2 = particles_X[j];
		double R2	=	(r1.x-r2.x)*(r1.x-r2.x) + (r1.y-r2.y)*(r1.y-r2.y);
		if (R2 < 1e-10) {
			return 0.0;
		}
		else if (R2 < a*a) {
			return 0.5*R2*log(R2)/a/a;
		}
		else {
			return 0.5*log(R2);
		}
	}
	// #endif
	~userkernel() {};
};