// 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_HPP__
#define GRASSMANN_AVERAGES_PCA_HPP__

/*!@file
 * Grassmann PCA functions, following the paper of Soren Hauberg.
 *
 * @note These implementations assume the existence of boost somewhere.
 */

#include <vector>

#include <boost/numeric/ublas/vector_expression.hpp>
#include <boost/numeric/ublas/vector.hpp>

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

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

namespace grassmann_averages_pca
{

  namespace ub = boost::numeric::ublas;

  /*!@brief Grassmann Averages for scalable PCA algorithm
   *
   * This class implements the Grassmann average for computing the PCA in a robust manner.
   * Its purpose is to compute the PCA of a dataset @f$\{X_i\}@f$, where each @f$X_i@f$ is a vector of dimension
   * D.
   *
   * The algorithm is the following:
   * - pick a random or a given @f$\mu_{k, 0}@f$, where @f$k@f$ is the current eigen-vector being computed and @f$0@f$ is the current iteration number (0).
   * - 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]
   *   - compute the update of @f$\mu_{k, .}@f$: @f[\mu_{k, t+1} = \frac{\sum_j s_{j, t} X_j}{\left\|\sum_j s_{j, t} X_j\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 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 eigen-vector with highest eigen-value. This can be configured through the function
   * grassmann_pca::set_nb_steps_pca.
   *
   * @tparam data_t type of vectors used for the computation.
   * @tparam observer_t an observer type following the signature of the class grassmann_trivial_callback.
   * @tparam norm_mu_t norm used to normalize the eigen-vector and project them onto the unit circle.
   *
   * @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
  {
  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;

    //! 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 (default to 3).
    size_t nb_steps_pca;

    //! 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 asynchronous_chunks_processor
    {
    private:
      //! Type of the elements contained in the vectors
      typedef typename data_t::value_type scalar_t;

      //! Number of vectors contained in this chunk.
      size_t nb_elements;

      //! Dimension of the vectors.
      size_t data_dimension;

      //! Internal accumulator.
      //! Should live beyond the scope of update and init, as required by the merger.
      data_t accumulator;

      //! Signs stored for decreasing the number of updates
      std::vector<bool> v_signs;

      // this is to send an update of the value of mu to one listener
      // the connexion should be managed externally
      typedef boost::function<void (data_t const*)> connector_accumulator_t;
      connector_accumulator_t signal_acc;

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

      //! The matrix containing a copy of the data.
      //! The vectors are stored per row in this matrix.
      scalar_t *p_c_matrix;

      //! Padding for one line of the matrix
      size_t data_padding;

      //! "Optimized" inner product.
      //! This one has the particularity to be more cache/memory bandwidth friendly. More efficient
      //! implementations may be used, but it turned out that the memory bandwidth is saturated when
      //! many threads are processing different data.
      scalar_t inner_product(scalar_t const* p_mu, scalar_t const* current_line) const
      {
        scalar_t const * const current_line_end = current_line + data_dimension;

        const int _64_elements = static_cast<int>(data_dimension >> 6);
        scalar_t acc(0);

        for(int j = 0; j < _64_elements; j++, current_line += 64, p_mu += 64)
        {
          for(int i = 0; i < 64; i++)
          {
            acc += current_line[i] * p_mu[i];
          }
        }
        for(; current_line < current_line_end; current_line++, p_mu++)
        {
          acc += (*current_line) * (*p_mu);
        }
        return acc;
      }

      //! "Optimized" inner product
      scalar_t inner_product(scalar_t const* p_mu, size_t element_index) const
      {
        return inner_product(p_mu, p_c_matrix + element_index * data_padding);
      }

    public:
      asynchronous_chunks_processor() : nb_elements(0), data_dimension(0), p_c_matrix(0), data_padding(0)
      {
      }

      ~asynchronous_chunks_processor()
      {
        delete [] p_c_matrix;
      }

      //! Sets the data range
      template <class container_iterator_t>
      void set_data_range(container_iterator_t const &b, container_iterator_t const& e)
      {
        nb_elements = std::distance(b, e);
        assert(nb_elements > 0);
        v_signs.resize(nb_elements);

        // aligning on 32 bytes = 1 << 5
        data_padding = (data_dimension*sizeof(scalar_t) + (1<<5) - 1) & (~((1<<5)-1));
        data_padding /= sizeof(scalar_t);

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

        container_iterator_t bb(b);

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

        signal_counter();
      }

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

      //! Returns the callback object that will be called to signal an updated accumulator.
      connector_accumulator_t& connector_accumulator()
      {
        return signal_acc;
      }

      //! Returns the callback object that will be called to signal the end of the current computation.
      connector_counter_t& connector_counter()
      {
        return signal_counter;
      }

      //! 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;
        accumulator = data_t(data_dimension, 0);
        scalar_t * const p_acc_begin = &accumulator(0);
        scalar_t const * const p_acc_end = p_acc_begin + data_dimension;

        for(size_t current_element = 0;
            current_element < nb_elements;
            current_element++, current_line+= data_padding - data_dimension)
        {
          scalar_t * p_acc = p_acc_begin;
          for(; p_acc < p_acc_end; p_acc++, current_line++)
          {
            *p_acc += *current_line;
          }
        }

        for(scalar_t * p_acc = p_acc_begin; p_acc < p_acc_end; p_acc++)
        {
          *p_acc /= full_dataset_size;
        }

        // posts the new value to the listeners for the current dimension
        signal_acc(&accumulator);
        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_line = p_c_matrix;
        scalar_t const * const p_mean_begin = &mean_value(0);
        scalar_t const * const p_mean_end = p_mean_begin + data_dimension;

        for(size_t current_element = 0;
            current_element < nb_elements;
            current_element++, current_line+= data_padding - data_dimension)
        {
          const scalar_t * p_mean = p_mean_begin;
          for(; p_mean < p_mean_end; p_mean++, current_line++)
          {
            *current_line -= *p_mean;
          }
        }

        // posts the new value to the listeners for the current dimension
        signal_counter();

      }

      //! PCA steps
      void pca_accumulation(data_t const &mu)
      {
        accumulator = data_t(data_dimension, 0);
        scalar_t const * const p_mu = &mu.data()[0];
        scalar_t * const p_acc = &accumulator.data()[0];

        scalar_t const * current_line = p_c_matrix;

        for(size_t s = 0; s < nb_elements; s++, current_line += data_padding)
        {
          const scalar_t inner_prod = inner_product(p_mu, current_line);
          for(size_t d = 0; d < data_dimension; d++)
          {
            p_acc[d] += inner_prod * current_line[d];
          }
        }

        // posts the new value to the listeners
        signal_acc(&accumulator);
        signal_counter();
      }

      //! Initialises the accumulator and the signs vector from the first mu
      void initial_accumulation(data_t const &mu)
      {
        accumulator = data_t(data_dimension, 0);
        std::vector<bool>::iterator itb(v_signs.begin());

        scalar_t const * const p_mu = &mu.data()[0];
        scalar_t * const p_acc = &accumulator.data()[0];

        // first iteration, we store the signs
        for(size_t s = 0; s < nb_elements; ++itb, s++)
        {
          bool sign = inner_product(p_mu, s) >= 0;

          *itb = sign;
          scalar_t *p(p_acc);
          scalar_t const * current_line = p_c_matrix + s * data_padding;
          scalar_t const * const current_line_end = current_line + data_dimension;
          if(sign)
          {
            for(; current_line < current_line_end; ++current_line, ++p)
            {
              *p += *current_line;
            }
          }
          else
          {
            for(; current_line < current_line_end; ++current_line, ++p)
            {
              *p -= *current_line;
            }
          }
        }

        // posts the new value to the listeners
        signal_acc(&accumulator);
        signal_counter();
      }

      //! Update the accumulator and the signs vector from an upated mu
      void update_accumulation(data_t const& mu)
      {
        accumulator = data_t(data_dimension, 0);

        bool update = false;

        scalar_t const * const p_mu = &mu.data()[0];
        scalar_t * const p_acc = &accumulator.data()[0];
        scalar_t const * current_line = p_c_matrix;

        std::vector<bool>::iterator itb(v_signs.begin());
        for(size_t s = 0; s < nb_elements; ++itb, s++, current_line += data_padding)
        {
          bool sign = inner_product(p_mu, current_line) >= 0;
          if(sign != *itb)
          {
            update = true;

            // update the value of the accumulator according to sign change
            *itb = sign;

            scalar_t *p(p_acc);
            scalar_t const * current_line_copy= current_line;
            scalar_t const * const current_line_end = current_line + data_dimension;

            if(sign)
            {
              for(; current_line_copy < current_line_end; ++current_line_copy, ++p)
              {
                *p += *current_line_copy;
              }
            }
            else
            {
              for(; current_line_copy < current_line_end; ++current_line_copy, ++p)
              {
                *p -= *current_line_copy;
              }
            }
          }
        }

        // posts the new value to the listeners
        if(update)
        {
          scalar_t *p(p_acc);
          for(size_t d = 0; d < data_dimension; d++, ++p)
          {
            *p *= 2;
          }
          signal_acc(&accumulator);
        }
        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)
      {
        // update of vectors in the orthogonal space, and update of the norms at the same time.
		    std::vector<typename vector_t::value_type> v(mu.begin(), mu.end()); // some issues with VC2015
        scalar_t const * const p_mu = &v[0];
        scalar_t * current_line = p_c_matrix;

        for(size_t line = 0; line < nb_elements; line ++, current_line += data_padding)
        {
          scalar_t const inner_prod = inner_product(p_mu, current_line);
          for(int column = 0; column < data_dimension; column++)
          {
            current_line[column] -= inner_prod * p_mu[column];
          }
        }

        signal_counter();
      }

    };

    /*!@internal
     * @brief Accumulation gathering the result of all workers.
     *
     * 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::threading::merger_addition<data_t>,
        details::threading::initialisation_vector_specific_dimension<data_t>
        >
    {
    private:
      typedef details::threading::initialisation_vector_specific_dimension<data_t> data_init_type;
      typedef details::threading::merger_addition<data_t> merger_type;
      typedef details::threading::asynchronous_results_merger<data_t, merger_type, data_init_type> parent_type;

    public:

      /*!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_))
      {}

    };

  public:

    /*!@brief Constructor
     *
     * @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() :
      random_init_op(details::fVerySmallButStillComputable, details::fVeryBigButStillComputable),
      nb_processors(1),
      max_chunk_size(std::numeric_limits<size_t>::max()),
      nb_steps_pca(3),
      need_centering(false),
      observer(0)
    {}

    //! 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_)
    {
      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 "regular PCA" 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 eigen-vectors of the provided dataset.
     *
     * @tparam it_t an input random iterator. Each element pointed by the iterator should be convertible to data_t.
     * @tparam it_o_basisvectors_t an output iterator for storing the computed eigenvalues. This iterator should model a forward output iterator.
     *
     * @param[in] max_iterations the maximum number of iterations in order to compute each eigen-vector.
     * @param[in] max_dimension_to_compute the maximum number of eigen-vectors to compute.
     * @param[in] it an (input) iterator pointing on the beginning of the data
     * @param[in] ite an (input) iterator pointing on the end of the data
     * @param[out] it_basisvectors an iterator on the beginning of the area where the computed eigen-vectors will be stored. The space should be at least @c max_dimension_to_compute.
     * @param[in] initial_guess if provided, the initial vectors will be initialized to this value. The size of the pointed container should be at least @c 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).
     * - @c std::next(it_eigenvectors, i) should yield a valid iterator pointing on a valid storage area, for @c i in [0, max_dimension_to_compute[.
     *
     */
    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.
      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();

      // the first element is used for the init guess because for dynamic std::vector like element, the size is needed.
      data_t mu(initial_guess != 0 ? (*initial_guess)[0] : random_init_op(*it));
      mu *= typename data_t::value_type(1./norm_op(mu)); // normalizing
      assert(mu.size() == number_of_dimensions);

      max_dimension_to_compute = std::min(max_dimension_to_compute, number_of_dimensions);

      size_t iterations = 0;

      // 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 asynchronous_chunks_processor async_processor_t;
      std::vector<async_processor_t> v_individual_accumulators(nb_chunks);

      asynchronous_results_merger async_merger(number_of_dimensions);
      async_merger.init_notifications();

      {
        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;
          }

          async_processor_t &current_acc_object = v_individual_accumulators[i];

          // 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);

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

          // pushing the asynchronous copy
          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));

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

          // updating the next
          it_current_begin = it_current_end;
        }

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

      }

      // 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();

        //for (auto i = mean_vector.begin(); i != mean_vector.end(); ++i)
        //{
        //  std::cout << *i << ' ';
        //}

        // 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 dimension
      for(size_t 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);

        // reseting the accumulator and the notifications
        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::initial_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();
        mu *= typename data_t::value_type(1./norm_op(mu));

        // other iterations as usual
        for(iterations = 1; !convergence_op(mu) && iterations < max_iterations; iterations++)
        {

          // reseting the final accumulator
          async_merger.init_notifications();
          //async_merger.init();
          //async_merger.get_merged_result() = mu_no_norm;

          // 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::update_accumulation, boost::ref(v_individual_accumulators[i]), boost::cref(mu)));
          }

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

          // gathering the mus
          //mu_no_norm = async_merger.get_merged_result();
          mu = async_merger.get_merged_result();
          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);
          }
        }

        // mu is the eigenvector 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
          }

          mu = initial_guess != 0 ? (*initial_guess)[current_subspace_index+1] : random_init_op(*it);

          async_merger.wait_notifications(v_individual_accumulators.size());

        }

      }

      // stopping the pool is done in the destruction of worker_lock_guard

      return true;
    }
  };

}

#endif /* GRASSMANN_AVERAGES_PCA_HPP__ */


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_EULER_FUNCTIONS_SCALAR_SIGNGAM_HPP_INCLUDED
#define NT2_EULER_FUNCTIONS_SCALAR_SIGNGAM_HPP_INCLUDED

#include <nt2/euler/functions/signgam.hpp>
#include <nt2/include/functions/scalar/is_nan.hpp>
#include <nt2/include/functions/scalar/is_lez.hpp>
#include <nt2/include/functions/scalar/is_flint.hpp>
#include <nt2/include/functions/scalar/is_odd.hpp>
#include <nt2/include/functions/scalar/floor.hpp>
#include <nt2/include/functions/scalar/is_inf.hpp>
#include <nt2/include/constants/one.hpp>
#include <nt2/include/constants/two.hpp>
#include <nt2/include/constants/nan.hpp>
#include <boost/dispatch/meta/as_floating.hpp>

namespace nt2 { namespace ext
{
  NT2_FUNCTOR_IMPLEMENTATION( nt2::tag::signgam_, tag::cpu_
                            , (A0)
                            , (scalar_< fundamental_<A0> >)
                            )
  {
    typedef typename boost::dispatch::meta::as_floating<A0>::type result_type;
    NT2_FUNCTOR_CALL(1)
    {
      typedef result_type type;
      bool isinfa0 =  nt2::is_inf(a0);
      if (nt2::is_lez(a0))
      {
        if (nt2::is_flint(a0)||isinfa0)
          return nt2::Nan<type>();
        else
          return nt2::One<type>()-bool(nt2::is_odd(nt2::floor(a0)))*nt2::Two<A0>();
      }
      else if (nt2::is_nan(a0)) return a0;
      return nt2::One<type>();
    }
  };
} }

#endif


#ifndef STAN_MATH_PRIM_PROB_HMM_LATENT_RNG_HPP
#define STAN_MATH_PRIM_PROB_HMM_LATENT_RNG_HPP

#include <stan/math/prim/core.hpp>
#include <stan/math/prim/meta.hpp>
#include <stan/math/prim/err/hmm_check.hpp>
#include <stan/math/prim/fun/Eigen.hpp>
#include <boost/random.hpp>
#include <vector>

namespace stan {
namespace math {

/**
 * For a hidden Markov model with observation y, hidden state x,
 * and parameters theta, generate samples from the posterior distribution
 * of the hidden states, x.
 * In this setting, the hidden states are discrete
 * and takes values over the finite space {1, ..., K}.
 * log_omegas is a matrix of observational densities, where
 * the (i, j)th entry corresponds to the density of the ith observation, y_i,
 * given x_i = j.
 * The transition matrix Gamma is such that the (i, j)th entry is the
 * probability that x_n = j given x_{n - 1} = i. The rows of Gamma are
 * simplexes.
 *
 * @tparam T_omega type of the log likelihood matrix
 * @tparam T_Gamma type of the transition matrix
 * @tparam T_rho type of the initial guess vector
 * @param[in] log_omegas log matrix of observational densities.
 * @param[in] Gamma transition density between hidden states.
 * @param[in] rho initial state
 * @param[in] rng random number generator
 * @return sample from the posterior distribution of the hidden states.
 * @throw `std::invalid_argument` if Gamma is not square, when we have
 *         at least one transition, or if the size of rho is not the
 *         number of rows of log_omegas.
 * @throw `std::domain_error` if rho is not a simplex and of the rows
 *         of Gamma are not a simplex (when there is at least one transition).
 */
template <typename T_omega, typename T_Gamma, typename T_rho, class RNG,
          require_all_eigen_t<T_omega, T_Gamma>* = nullptr,
          require_eigen_col_vector_t<T_rho>* = nullptr>
inline std::vector<int> hmm_latent_rng(const T_omega& log_omegas,
                                       const T_Gamma& Gamma, const T_rho& rho,
                                       RNG& rng) {
  int n_states = log_omegas.rows();
  int n_transitions = log_omegas.cols() - 1;

  Eigen::MatrixXd omegas = value_of(log_omegas).array().exp();
  ref_type_t<decltype(value_of(rho))> rho_dbl = value_of(rho);
  ref_type_t<decltype(value_of(Gamma))> Gamma_dbl = value_of(Gamma);
  hmm_check(log_omegas, Gamma_dbl, rho_dbl, "hmm_latent_rng");

  Eigen::MatrixXd alphas(n_states, n_transitions + 1);
  alphas.col(0) = omegas.col(0).cwiseProduct(rho_dbl);
  alphas.col(0) /= alphas.col(0).maxCoeff();

  for (int n = 0; n < n_transitions; ++n) {
    alphas.col(n + 1)
        = omegas.col(n + 1).cwiseProduct(Gamma_dbl.transpose() * alphas.col(n));
    alphas.col(n + 1) /= alphas.col(n + 1).maxCoeff();
  }

  Eigen::VectorXd beta = Eigen::VectorXd::Ones(n_states);

  // sample the last hidden state
  std::vector<int> hidden_states(n_transitions + 1);
  std::vector<double> probs(n_states);
  Eigen::Map<Eigen::VectorXd> probs_vec(probs.data(), n_states);
  probs_vec = alphas.col(n_transitions) / alphas.col(n_transitions).sum();
  boost::random::discrete_distribution<> cat_hidden(probs);
  hidden_states[n_transitions] = cat_hidden(rng) + stan::error_index::value;

  for (int n = n_transitions; n-- > 0;) {
    // Sample the nth hidden state conditional on (n + 1)st hidden state.
    // Subtract error_index in order to use C++ index.
    int last_hs = hidden_states[n + 1] - stan::error_index::value;

    probs_vec = alphas.col(n).cwiseProduct(Gamma_dbl.col(last_hs))
                * beta(last_hs) * omegas(last_hs, n + 1);

    probs_vec /= probs_vec.sum();

    // discrete_distribution produces samples in [0, K), so
    // we need to add 1 to generate over [1, K).
    boost::random::discrete_distribution<> cat_hidden(probs);
    hidden_states[n] = cat_hidden(rng) + stan::error_index::value;

    // update backwards state
    beta = Gamma_dbl * (omegas.col(n + 1).cwiseProduct(beta));
    beta /= beta.maxCoeff();
  }

  return hidden_states;
}

}  // namespace math
}  // namespace stan
#endif


/*
 * Copyright (c) 2013, Christian Gehring, Hannes Sommer, Paul Furgale, Remo Diethelm
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the Autonomous Systems Lab, ETH Zurich 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 Christian Gehring, Hannes Sommer, Paul Furgale,
 * Remo Diethelm 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.
 *
*/
#pragma once

#include <Eigen/Geometry>

#include "kindr/common/common.hpp"
#include "kindr/common/assert_macros_eigen.hpp"
#include "kindr/quaternions/QuaternionBase.hpp"

namespace kindr {

template<typename PrimType_>
class UnitQuaternion;

//! Implementation of a Quaternion based on Eigen::Quaternion
/*!
 * The Hamiltonian convention is used, where
 * Q = w + x*i + y*j + z*k and i*i=j*j=k*k=ijk=-1.
 *
 * The following two typedefs are provided for convenience:
 *   - QuaternionF for float
 *   - QuaternionD for double
 * \ingroup quaternions
 * \see rm::UnitQuaternion for an implementation of a unit quaternion
 * \see rm::rotations::RotationQuaternion for quaternions that represent a rotation
 */
template<typename PrimType_>
class Quaternion : public QuaternionBase<Quaternion<PrimType_>>, private Eigen::Quaternion<PrimType_> {
 private:
  typedef Eigen::Quaternion<PrimType_> Base;
 public:
  //! the implementation type, i.e., Eigen::Quaternion<>
  typedef Base Implementation;
  //! the scalar type, i.e., the type of the coefficients
  typedef PrimType_ Scalar;
  //! the imaginary type, i.e., Eigen::Quaternion<>
  typedef Eigen::Matrix<PrimType_,3,1> Imaginary;
  //! quaternion as 4x1 matrix: [w; x; y; z]
  typedef Eigen::Matrix<PrimType_,4,1> Vector4;

  //! Default constructor creates a quaternion with all coefficients equal to zero
  Quaternion()
    : Base(Implementation(0,0,0,0)) {
  }

  /*! \brief Constructor using four scalars.
   *  \param w     first entry of the quaternion
   *  \param x     second entry of the quaternion
   *  \param y     third entry of the quaternion
   *  \param z     fourth entry of the quaternion
   */
  Quaternion(Scalar w, Scalar x, Scalar y, Scalar z)
    : Base(w,x,y,z) {
  }

  /*! \brief Constructor using real and imaginary part.
   *  \param real   real part (PrimType_)
   *  \param imag   imaginary part (Eigen::Matrix<PrimType_,3,1>)
   */
  Quaternion(Scalar real, const Imaginary& imag)
    : Base(real,imag(0),imag(1),imag(2)) {
  }

  /*! \brief Constructor using Eigen::Matrix<PrimType_,4,1>.
   *  \param other   Eigen::Matrix<PrimType_,4,1>
   */
  Quaternion(const Vector4& vector4)
    : Base(vector4(0),vector4(1),vector4(2),vector4(3)) {
  }

  // create from Eigen::Quaternion
  explicit Quaternion(const Base& other)
    : Base(other) {
  }

  /*! \returns the inverse of the quaternion
    */
  Quaternion inverted() const {
    return Quaternion(Implementation::inverse());
  }

  /*! \inverts the quaternion
    */
  Quaternion& invert() {
    *this = Quaternion(Implementation::inverse());
    return *this;
  }

  /*! \returns the conjugate of the quaternion
    */
  Quaternion conjugated() const {
    return Quaternion(Implementation::conjugate());
  }

  /*! \conjugates the quaternion
    */
  Quaternion& conjugate() {
    *this = Quaternion(Implementation::conjugate());
    return *this;
  }

  Quaternion& operator =(const Quaternion<PrimType_>& other) {
    this->w() = other.w();
    this->x() = other.x();
    this->y() = other.y();
    this->z() = other.z();
    return *this;
  }

  Quaternion& operator =(const UnitQuaternion<PrimType_>& other) {
    *this = Quaternion(other.toImplementation());
    return *this;
  }

//  bool operator ==(const Quaternion<PrimType_>& other) {
//	  return this->isEqual(other);
//  }

  template<typename PrimTypeIn_>
  Quaternion& operator ()(const Quaternion<PrimTypeIn_>& other) {
//	*this = other.template cast<PrimType_>();
	this->w() = static_cast<PrimType_>(other.w());
	this->x() = static_cast<PrimType_>(other.x());
	this->y() = static_cast<PrimType_>(other.y());
	this->z() = static_cast<PrimType_>(other.z());
	return *this;
  }

  template<typename PrimTypeIn_>
  Quaternion& operator ()(const UnitQuaternion<PrimTypeIn_>& other) {
//	*this = other.uq.template cast<PrimType_>(); // uq is private
	this->w() = static_cast<PrimType_>(other.w());
	this->x() = static_cast<PrimType_>(other.x());
	this->y() = static_cast<PrimType_>(other.y());
	this->z() = static_cast<PrimType_>(other.z());
	return *this;
  }

  inline Implementation& toImplementation() {
    return static_cast<Implementation&>(*this);
  }
  inline const Implementation& toImplementation() const {
    return static_cast<const Implementation&>(*this);
  }

  using QuaternionBase<Quaternion<PrimType_>>::operator==;
  using QuaternionBase<Quaternion<PrimType_>>::operator*;

  inline Scalar w() const {
    return Base::w();
  }

  inline Scalar x() const {
    return Base::x();
  }

  inline Scalar y() const {
    return Base::y();
  }

  inline Scalar z() const {
    return Base::z();
  }

  inline Scalar& w() { // todo: attention: no assertion for unitquaternions!
    return Base::w();
  }

  inline Scalar& x() {
    return Base::x();
  }

  inline Scalar& y() {
    return Base::y();
  }

  inline Scalar& z() {
    return Base::z();
  }

  inline Scalar real() const {
    return Base::w();
  }

  inline Imaginary imaginary() const {
    return Imaginary(Base::x(),Base::y(),Base::z());
  }

  inline Vector4 vector() const {
    Vector4 vector4;
    vector4 << w(), x(), y(), z();
    return vector4;
  }

  inline Scalar norm() const {
    return Base::norm();
  }

  Quaternion normalized() const {
    return Quaternion(this->Base::normalized());
  }

  Quaternion& normalize() {
	  this->Base::normalize();
	  return *this;
  }

  Quaternion& setZero() {
    this->w() = Scalar(0.0);
    this->x() = Scalar(0.0);
    this->y() = Scalar(0.0);
    this->z() = Scalar(0.0);
    return *this;
  }

  /*! \brief Get zero element.
   *  \returns zero element
   */
  static Quaternion Zero() {
    return Quaternion();
  }

  UnitQuaternion<PrimType_> toUnitQuaternion() const {
    return UnitQuaternion<PrimType_>(this->Base::normalized());
  }

  /*! \brief Returns the quaternion matrix Qleft: q*p = Qleft(q)*p
   *  This function can be used to get the derivative of the concatenation with respect to the right quaternion.
   *  \returns the quaternion matrix Qleft
   */
  Eigen::Matrix<PrimType_,4,4> getQuaternionMatrix() {
    Eigen::Matrix<PrimType_,4,4> Qleft;
    Qleft(0,0) =  w();      Qleft(0,1) = -x();      Qleft(0,2) = -y();      Qleft(0,3) = -z();
    Qleft(1,0) =  x();      Qleft(1,1) =  w();      Qleft(1,2) = -z();      Qleft(1,3) =  y();
    Qleft(2,0) =  y();      Qleft(2,1) =  z();      Qleft(2,2) =  w();      Qleft(2,3) = -x();
    Qleft(3,0) =  z();      Qleft(3,1) = -y();      Qleft(3,2) =  x();      Qleft(3,3) =  w();
    return Qleft;
  }

  /*! \brief Returns the quaternion matrix Qright: q*p = Qright(p)*q
   *  This function can be used to get the derivative of the concatenation with respect to the left quaternion.
   *  \returns the quaternion matrix Qright
   */
  Eigen::Matrix<PrimType_,4,4> getConjugateQuaternionMatrix() {
    Eigen::Matrix<PrimType_,4,4> Qright;
    Qright(0,0) =  w();      Qright(0,1) = -x();      Qright(0,2) = -y();      Qright(0,3) = -z();
    Qright(1,0) =  x();      Qright(1,1) =  w();      Qright(1,2) =  z();      Qright(1,3) = -y();
    Qright(2,0) =  y();      Qright(2,1) = -z();      Qright(2,2) =  w();      Qright(2,3) =  x();
    Qright(3,0) =  z();      Qright(3,1) =  y();      Qright(3,2) = -x();      Qright(3,3) =  w();
    return Qright;
  }
};

//! Quaternion using double
typedef Quaternion<double> QuaternionD;
//! Quaternion using float
typedef Quaternion<float> QuaternionF;

//! Implementation of a unit quaternion based on Eigen::Quaternion
/*! The Hamiltonian convention is used, where
 * Q = w + x*i + y*j + z*k and i*i=j*j=k*k=ijk=-1.
 *
 * The following two typedefs are provided for convenience:
 *   - UnitQuaternionF for float
 *   - UnitQuaternionD for double
 * \ingroup quaternions
 * \see rm::Quaternion for an implementation of a generic quaternion
 * \see rm::rotations::RotationQuaternion for quaternions that represent a rotation
 */
template<typename PrimType_>
class UnitQuaternion : public UnitQuaternionBase<UnitQuaternion<PrimType_>> {
 private:
  Quaternion<PrimType_> unitQuternion_;
  typedef UnitQuaternionBase<UnitQuaternion<PrimType_>> Base;
 public:
  //! the implementation type, i.e., Eigen::Quaternion<>
  typedef typename Quaternion<PrimType_>::Implementation Implementation;
  //! the scalar type, i.e., the type of the coefficients
  typedef PrimType_ Scalar;
  //! the imaginary type, i.e., Eigen::Quaternion<>
  typedef Eigen::Matrix<PrimType_,3,1> Imaginary;
  //! quaternion as 4x1 matrix: [w; x; y; z]
  typedef Eigen::Matrix<PrimType_,4,1> Vector4;

  //! Default Constructor initializes the unit quaternion to identity
  UnitQuaternion()
    : unitQuternion_(Implementation::Identity()) {
  }

  //! Constructor to create unit quaternion from coefficients
  /*! Q = w + x*i + y*j + z*k
   * \param   w   scalar
   * \param   x   vector index 1
   * \param   y   vector index 2
   * \param   z   vector index 3
   */
  UnitQuaternion(Scalar w, Scalar x, Scalar y, Scalar z)
    : unitQuternion_(w,x,y,z) {
    KINDR_ASSERT_SCALAR_NEAR_DBG(std::runtime_error, norm(), static_cast<Scalar>(1.0), static_cast<Scalar>(1e-2), "Input quaternion has not unit length.");
  }

  /*! \brief Constructor using real and imaginary part.
   *  In debug mode, an assertion is thrown if the quaternion has not unit length.
   *  \param real   real part (PrimType_)
   *  \param imag   imaginary part (Eigen::Matrix<PrimType_,3,1>)
   */
  UnitQuaternion(Scalar real, const Imaginary& imag)
    : unitQuternion_(real,imag) {
    KINDR_ASSERT_SCALAR_NEAR_DBG(std::runtime_error, norm(), static_cast<Scalar>(1.0), static_cast<Scalar>(1e-2), "Input quaternion has not unit length.");
  }

  /*! \brief Constructor using Eigen::Matrix<PrimType_,4,1>.
   *  In debug mode, an assertion is thrown if the quaternion has not unit length.
   *  \param other   Eigen::Matrix<PrimType_,4,1>
   */
  UnitQuaternion(const Vector4& vector4)
    : unitQuternion_(vector4(0),vector4(1),vector4(2),vector4(3)) {
    KINDR_ASSERT_SCALAR_NEAR_DBG(std::runtime_error, norm(), static_cast<Scalar>(1.0), static_cast<Scalar>(1e-2), "Input quaternion has not unit length.");
  }

  //! Constructor to create unit quaternion from Quaternion
  explicit UnitQuaternion(const Quaternion<PrimType_>& other)
    : unitQuternion_(other.toImplementation()) {
    KINDR_ASSERT_SCALAR_NEAR_DBG(std::runtime_error, norm(), static_cast<Scalar>(1.0), static_cast<Scalar>(1e-2), "Input quaternion has not unit length.");
  }

  //! Constructor to create unit quaternion from Eigen::Quaternion
  /*!
   * \param other Eigen::Quaternion
   */
  explicit UnitQuaternion(const Implementation& other)
    : unitQuternion_(other) {
    KINDR_ASSERT_SCALAR_NEAR_DBG(std::runtime_error, norm(), static_cast<Scalar>(1.0), static_cast<Scalar>(1e-2), "Input quaternion has not unit length.");
  }

  UnitQuaternion& operator =(const UnitQuaternion<PrimType_>& other) {
	    this->w() = other.w();
	    this->x() = other.x();
	    this->y() = other.y();
	    this->z() = other.z();
	    return *this;
  }

  template<typename PrimTypeIn_>
  UnitQuaternion& operator ()(const UnitQuaternion<PrimTypeIn_>& other) {
//	uq = other.uq;
	this->w() = static_cast<PrimType_>(other.w());
	this->x() = static_cast<PrimType_>(other.x());
	this->y() = static_cast<PrimType_>(other.y());
	this->z() = static_cast<PrimType_>(other.z());
	return *this;
  }

  template<typename PrimTypeIn_>
  UnitQuaternion& operator ()(const Quaternion<PrimTypeIn_>& other) {
//		*this = (UnitQuaternion)quat;
//	uq = other.template cast<PrimType_>();
	this->w() = static_cast<PrimType_>(other.w());
	this->x() = static_cast<PrimType_>(other.x());
	this->y() = static_cast<PrimType_>(other.y());
	this->z() = static_cast<PrimType_>(other.z());
    KINDR_ASSERT_SCALAR_NEAR_DBG(std::runtime_error, norm(), static_cast<Scalar>(1.0), 1e-2, "Input quaternion has not unit length.");
	return *this;
  }

//  UnitQuaternion<PrimType_> operator *(const UnitQuaternion<PrimType_>& other) {
//	  return UnitQuaternion<PrimType_>(this->uq * other.uq);
//  }
//
//  Quaternion<PrimType_> operator *(const Quaternion<PrimType_>& other) {
//	  return Quaternion<PrimType_>(this->uq * other);
//  }
//  bool operator ==(const UnitQuaternion<PrimType_>& other) {
//	  return this->uq == other.uq;
//  }
//
//  bool operator ==(const Quaternion<PrimType_>& other) {
//	  return this->uq == other.uq;
//  }

  inline Scalar w() const {
    return unitQuternion_.w();
  }

  inline Scalar x() const {
    return unitQuternion_.x();
  }

  inline Scalar y() const {
    return unitQuternion_.y();
  }

  inline Scalar z() const {
    return unitQuternion_.z();
  }

  inline Scalar& w() { // todo: attention: no assertion for unitquaternions!
    return unitQuternion_.w();
  }

  inline Scalar& x() {
    return unitQuternion_.x();
  }

  inline Scalar& y() {
    return unitQuternion_.y();
  }

  inline Scalar& z() {
    return unitQuternion_.z();
  }

  inline Scalar real() const {
    return unitQuternion_.w();
  }

  inline Imaginary imaginary() const {
    return Imaginary(unitQuternion_.x(),unitQuternion_.y(),unitQuternion_.z());
  }

  inline Vector4 vector() const {
    Vector4 vector4;
    vector4 << w(), x(), y(), z();
    return vector4;
  }

//  using Base::operator*;

//  using UnitQuaternionBase<UnitQuaternion<PrimType_>>::conjugate;

  /*! \returns the conjugate of the quaternion
    */
  UnitQuaternion conjugated() const {
    return UnitQuaternion(unitQuternion_.conjugated());
  }

  /*! \conjugates the quaternion
    */
  UnitQuaternion& conjugate() {
    unitQuternion_.conjugate();
    return *this;
  }

//  using Base::inverted;
//  using Base::invert;

//  /*! \returns the inverse of the quaternion which is the conjugate for unit quaternions
//    */
//  UnitQuaternion inverse() const {
//    return UnitQuaternion(Base::conjugate());
//  }

  Scalar norm() const {
    return unitQuternion_.norm();
  }

  /*! \brief Sets unit quaternion to identity.
   *  \returns reference
   */
  UnitQuaternion& setIdentity() {
    this->w() = Scalar(0.0);
    this->x() = Scalar(0.0);
    this->y() = Scalar(0.0);
    this->z() = Scalar(0.0);
    return *this;
  }

  /*! \brief Get identity unit quaternion.
   *  \returns identity unit quaternion
   */
  static UnitQuaternion Identity() {
    return UnitQuaternion(Implementation::Identity());
  }

  const Implementation& toImplementation() const {
    return unitQuternion_.toImplementation();
  }

  Implementation& toImplementation() {
    return unitQuternion_.toImplementation();
  }

  /*! \brief Returns the quaternion matrix Qleft: q*p = Qleft(q)*p
   *  This function can be used to get the derivative of the concatenation with respect to the right quaternion.
   *  \returns the quaternion matrix Qleft
   */
  Eigen::Matrix<PrimType_,4,4> getQuaternionMatrix() {
    Eigen::Matrix<PrimType_,4,4> Qleft;
    Qleft(0,0) =  w();      Qleft(0,1) = -x();      Qleft(0,2) = -y();      Qleft(0,3) = -z();
    Qleft(1,0) =  x();      Qleft(1,1) =  w();      Qleft(1,2) = -z();      Qleft(1,3) =  y();
    Qleft(2,0) =  y();      Qleft(2,1) =  z();      Qleft(2,2) =  w();      Qleft(2,3) = -x();
    Qleft(3,0) =  z();      Qleft(3,1) = -y();      Qleft(3,2) =  x();      Qleft(3,3) =  w();
    return Qleft;
  }

  /*! \brief Returns the quaternion matrix Qright: q*p = Qright(p)*q
   *  This function can be used to get the derivative of the concatenation with respect to the left quaternion.
   *  \returns the quaternion matrix Qright
   */
  Eigen::Matrix<PrimType_,4,4> getConjugateQuaternionMatrix() {
    Eigen::Matrix<PrimType_,4,4> Qright;
    Qright(0,0) =  w();      Qright(0,1) = -x();      Qright(0,2) = -y();      Qright(0,3) = -z();
    Qright(1,0) =  x();      Qright(1,1) =  w();      Qright(1,2) =  z();      Qright(1,3) = -y();
    Qright(2,0) =  y();      Qright(2,1) = -z();      Qright(2,2) =  w();      Qright(2,3) =  x();
    Qright(3,0) =  z();      Qright(3,1) =  y();      Qright(3,2) = -x();      Qright(3,3) =  w();
    return Qright;
  }
};

//! Unit quaternion using double
typedef UnitQuaternion<double> UnitQuaternionD;
//! Unit quaternion using float
typedef UnitQuaternion<float> UnitQuaternionF;

} // namespace kindr


#pragma once

#include <Eigen/Eigen>
#include <cmath>
#include <vector>

#include "root_finder.hpp"

// Polynomial order and trajectory dimension are fixed here
typedef Eigen::Matrix<double, 3, 6> CoefficientMat;
typedef Eigen::Matrix<double, 3, 5> VelCoefficientMat;
typedef Eigen::Matrix<double, 3, 4> AccCoefficientMat;

namespace mpc_utils{
class Piece {
 private:
  double duration;
  CoefficientMat coeffMat;

 public:
  Piece() = default;

  Piece(double dur, const CoefficientMat &cMat)
      : duration(dur), coeffMat(cMat) {}

  inline int getDim() const {
    return 3;
  }

  inline int getOrder() const {
    return 5;
  }

  inline double getDuration() const {
    return duration;
  }

  inline const CoefficientMat &getCoeffMat() const {
    return coeffMat;
  }

  inline VelCoefficientMat getVelCoeffMat() const {
    VelCoefficientMat velCoeffMat;
    int n = 1;
    for (int i = 4; i >= 0; i--) {
      velCoeffMat.col(i) = n * coeffMat.col(i);
      n++;
    }
    return velCoeffMat;
  }

  inline Eigen::Vector3d getPos(const double &t) const {
    Eigen::Vector3d pos(0.0, 0.0, 0.0);
    double tn = 1.0;
    for (int i = 5; i >= 0; i--) {
      pos += tn * coeffMat.col(i);
      tn *= t;
    }
    return pos;
  }

  inline Eigen::Vector3d getVel(const double &t) const {
    Eigen::Vector3d vel(0.0, 0.0, 0.0);
    double tn = 1.0;
    int n = 1;
    for (int i = 4; i >= 0; i--) {
      vel += n * tn * coeffMat.col(i);
      tn *= t;
      n++;
    }
    return vel;
  }

  inline Eigen::Vector3d getAcc(const double &t) const {
    Eigen::Vector3d acc(0.0, 0.0, 0.0);
    double tn = 1.0;
    int m = 1;
    int n = 2;
    for (int i = 3; i >= 0; i--) {
      acc += m * n * tn * coeffMat.col(i);
      tn *= t;
      m++;
      n++;
    }
    return acc;
  }

  inline Eigen::Vector3d getJer(const double &t) const {
    Eigen::Vector3d jer(0.0, 0.0, 0.0);
    double tn = 1.0;
    int l = 1;
    int m = 2;
    int n = 3;
    for (int i = 2; i >= 0; i--) {
      jer += l * m * n * tn * coeffMat.col(i);
      tn *= t;
      l++;
      m++;
      n++;
    }
    return jer;
  }

  inline CoefficientMat normalizePosCoeffMat() const {
    CoefficientMat nPosCoeffsMat;
    double t = 1.0;
    for (int i = 5; i >= 0; i--) {
      nPosCoeffsMat.col(i) = coeffMat.col(i) * t;
      t *= duration;
    }
    return nPosCoeffsMat;
  }

  inline VelCoefficientMat normalizeVelCoeffMat() const {
    VelCoefficientMat nVelCoeffMat;
    int n = 1;
    double t = duration;
    for (int i = 4; i >= 0; i--) {
      nVelCoeffMat.col(i) = n * coeffMat.col(i) * t;
      t *= duration;
      n++;
    }
    return nVelCoeffMat;
  }

  inline AccCoefficientMat normalizeAccCoeffMat() const {
    AccCoefficientMat nAccCoeffMat;
    int n = 2;
    int m = 1;
    double t = duration * duration;
    for (int i = 3; i >= 0; i--) {
      nAccCoeffMat.col(i) = n * m * coeffMat.col(i) * t;
      n++;
      m++;
      t *= duration;
    }
    return nAccCoeffMat;
  }

  inline double getMaxVelRate() const {
    Eigen::MatrixXd nVelCoeffMat = normalizeVelCoeffMat();
    Eigen::VectorXd coeff = RootFinder::polySqr(nVelCoeffMat.row(0)) +
                            RootFinder::polySqr(nVelCoeffMat.row(1)) +
                            RootFinder::polySqr(nVelCoeffMat.row(2));
    int N = coeff.size();
    int n = N - 1;
    for (int i = 0; i < N; i++) {
      coeff(i) *= n;
      n--;
    }
    if (coeff.head(N - 1).squaredNorm() < DBL_EPSILON) {
      return 0.0;
    } else {
      double l = -0.0625;
      double r = 1.0625;
      while (fabs(RootFinder::polyVal(coeff.head(N - 1), l)) < DBL_EPSILON) {
        l = 0.5 * l;
      }
      while (fabs(RootFinder::polyVal(coeff.head(N - 1), r)) < DBL_EPSILON) {
        r = 0.5 * (r + 1.0);
      }
      std::set<double> candidates = RootFinder::solvePolynomial(coeff.head(N - 1), l, r,
                                                                FLT_EPSILON / duration);
      candidates.insert(0.0);
      candidates.insert(1.0);
      double maxVelRateSqr = -INFINITY;
      double tempNormSqr;
      for (std::set<double>::const_iterator it = candidates.begin();
           it != candidates.end();
           it++) {
        if (0.0 <= *it && 1.0 >= *it) {
          tempNormSqr = getVel((*it) * duration).squaredNorm();
          maxVelRateSqr = maxVelRateSqr < tempNormSqr ? tempNormSqr : maxVelRateSqr;
        }
      }
      return sqrt(maxVelRateSqr);
    }
  }

  inline double getMaxAccRate() const {
    Eigen::MatrixXd nAccCoeffMat = normalizeAccCoeffMat();
    Eigen::VectorXd coeff = RootFinder::polySqr(nAccCoeffMat.row(0)) +
                            RootFinder::polySqr(nAccCoeffMat.row(1)) +
                            RootFinder::polySqr(nAccCoeffMat.row(2));
    int N = coeff.size();
    int n = N - 1;
    for (int i = 0; i < N; i++) {
      coeff(i) *= n;
      n--;
    }
    if (coeff.head(N - 1).squaredNorm() < DBL_EPSILON) {
      return 0.0;
    } else {
      double l = -0.0625;
      double r = 1.0625;
      while (fabs(RootFinder::polyVal(coeff.head(N - 1), l)) < DBL_EPSILON) {
        l = 0.5 * l;
      }
      while (fabs(RootFinder::polyVal(coeff.head(N - 1), r)) < DBL_EPSILON) {
        r = 0.5 * (r + 1.0);
      }
      std::set<double> candidates = RootFinder::solvePolynomial(coeff.head(N - 1), l, r,
                                                                FLT_EPSILON / duration);
      candidates.insert(0.0);
      candidates.insert(1.0);
      double maxAccRateSqr = -INFINITY;
      double tempNormSqr;
      for (std::set<double>::const_iterator it = candidates.begin();
           it != candidates.end();
           it++) {
        if (0.0 <= *it && 1.0 >= *it) {
          tempNormSqr = getAcc((*it) * duration).squaredNorm();
          maxAccRateSqr = maxAccRateSqr < tempNormSqr ? tempNormSqr : maxAccRateSqr;
        }
      }
      return sqrt(maxAccRateSqr);
    }
  }

  inline bool checkMaxVelRate(const double &maxVelRate) const {
    double sqrMaxVelRate = maxVelRate * maxVelRate;
    if (getVel(0.0).squaredNorm() >= sqrMaxVelRate ||
        getVel(duration).squaredNorm() >= sqrMaxVelRate) {
      return false;
    } else {
      Eigen::MatrixXd nVelCoeffMat = normalizeVelCoeffMat();
      Eigen::VectorXd coeff = RootFinder::polySqr(nVelCoeffMat.row(0)) +
                              RootFinder::polySqr(nVelCoeffMat.row(1)) +
                              RootFinder::polySqr(nVelCoeffMat.row(2));
      double t2 = duration * duration;
      coeff.tail<1>()(0) -= sqrMaxVelRate * t2;
      return RootFinder::countRoots(coeff, 0.0, 1.0) == 0;
    }
  }

  inline bool checkMaxAccRate(const double &maxAccRate) const {
    double sqrMaxAccRate = maxAccRate * maxAccRate;
    if (getAcc(0.0).squaredNorm() >= sqrMaxAccRate ||
        getAcc(duration).squaredNorm() >= sqrMaxAccRate) {
      return false;
    } else {
      Eigen::MatrixXd nAccCoeffMat = normalizeAccCoeffMat();
      Eigen::VectorXd coeff = RootFinder::polySqr(nAccCoeffMat.row(0)) +
                              RootFinder::polySqr(nAccCoeffMat.row(1)) +
                              RootFinder::polySqr(nAccCoeffMat.row(2));
      double t2 = duration * duration;
      double t4 = t2 * t2;
      coeff.tail<1>()(0) -= sqrMaxAccRate * t4;
      return RootFinder::countRoots(coeff, 0.0, 1.0) == 0;
    }
  }

  // GaaiLam
  inline double project_pt(const Eigen::Vector3d &pt,
                           double &tt, Eigen::Vector3d &pro_pt) {
    // 2*(p-p0)^T * \dot{p} = 0
    auto l_coeff = getCoeffMat();
    l_coeff.col(5) = l_coeff.col(5) - pt;
    auto r_coeff = getVelCoeffMat();
    Eigen::VectorXd eq = Eigen::VectorXd::Zero(2 * 5);
    for (int j = 0; j < l_coeff.rows(); ++j) {
      eq = eq + RootFinder::polyConv(l_coeff.row(j), r_coeff.row(j));
    }
    double l = -0.0625;
    double r = duration + 0.0625;
    while (fabs(RootFinder::polyVal(eq, l)) < DBL_EPSILON) {
      l = 0.5 * l;
    }
    while (fabs(RootFinder::polyVal(eq, r)) < DBL_EPSILON) {
      r = 0.5 * (duration + r);
    }
    std::set<double> roots =
        RootFinder::solvePolynomial(eq, l, r, 1e-6);
    // std::cout << "# roots: " << roots.size() << std::endl;
    double min_dist = -1;
    for (const auto &root : roots) {
      // std::cout << "root: " << root << std::endl;
      if (root < 0 || root > duration) {
        continue;
      }
      if (getVel(root).norm() < 1e-6) {  // velocity == 0, ignore it
        continue;
      }
      // std::cout << "find min!" << std::endl;
      Eigen::Vector3d p = getPos(root);
      // std::cout << "p: " << p.transpose() << std::endl;
      double distance = (p - pt).norm();
      if (distance < min_dist || min_dist < 0) {
        min_dist = distance;
        tt = root;
        pro_pt = p;
      }
    }
    return min_dist;
  }

  inline bool intersection_plane(const Eigen::Vector3d p,
                                 const Eigen::Vector3d v,
                                 double &tt, Eigen::Vector3d &pt) const {
    // (pt - p)^T * v = 0
    auto coeff = getCoeffMat();
    coeff.col(5) = coeff.col(5) - p;
    Eigen::VectorXd eq = coeff.transpose() * v;
    double l = -0.0625;
    double r = duration + 0.0625;
    while (fabs(RootFinder::polyVal(eq, l)) < DBL_EPSILON) {
      l = 0.5 * l;
    }
    while (fabs(RootFinder::polyVal(eq, r)) < DBL_EPSILON) {
      r = 0.5 * (duration + r);
    }
    std::set<double> roots =
        RootFinder::solvePolynomial(eq, l, r, 1e-6);
    for (const auto &root : roots) {
      tt = root;
      pt = getPos(root);
      return true;
    }
    return false;
  }
};

class Trajectory {
 private:
  typedef std::vector<Piece> Pieces;
  Pieces pieces;

 public:
  Trajectory() = default;

  Trajectory(const std::vector<double> &durs,
             const std::vector<CoefficientMat> &cMats) {
    int N = std::min(durs.size(), cMats.size());
    pieces.reserve(N);
    for (int i = 0; i < N; i++) {
      pieces.emplace_back(durs[i], cMats[i]);
    }
  }

  inline int getPieceNum() const {
    return pieces.size();
  }

  inline Eigen::VectorXd getDurations() const {
    int N = getPieceNum();
    Eigen::VectorXd durations(N);
    for (int i = 0; i < N; i++) {
      durations(i) = pieces[i].getDuration();
    }
    return durations;
  }

  inline double getTotalDuration() const {
    int N = getPieceNum();
    double totalDuration = 0.0;
    for (int i = 0; i < N; i++) {
      totalDuration += pieces[i].getDuration();
    }
    return totalDuration;
  }

  inline Eigen::MatrixXd getPositions() const {
    int N = getPieceNum();
    Eigen::MatrixXd positions(3, N + 1);
    for (int i = 0; i < N; i++) {
      positions.col(i) = pieces[i].getCoeffMat().col(5);
    }
    positions.col(N) = pieces[N - 1].getPos(pieces[N - 1].getDuration());
    return positions;
  }

  inline const Piece &operator[](int i) const {
    return pieces[i];
  }

  inline Piece &operator[](int i) {
    return pieces[i];
  }

  inline void clear(void) {
    pieces.clear();
    return;
  }

  inline Pieces::const_iterator begin() const {
    return pieces.begin();
  }

  inline Pieces::const_iterator end() const {
    return pieces.end();
  }

  inline Pieces::iterator begin() {
    return pieces.begin();
  }

  inline Pieces::iterator end() {
    return pieces.end();
  }

  inline void reserve(const int &n) {
    pieces.reserve(n);
    return;
  }

  inline void emplace_back(const Piece &piece) {
    pieces.emplace_back(piece);
    return;
  }

  inline void emplace_back(const double &dur,
                           const CoefficientMat &cMat) {
    pieces.emplace_back(dur, cMat);
    return;
  }

  inline void append(const Trajectory &traj) {
    pieces.insert(pieces.end(), traj.begin(), traj.end());
    return;
  }

  inline int locatePieceIdx(double &t) const {
    int N = getPieceNum();
    int idx;
    double dur;
    for (idx = 0;
         idx < N &&
         t > (dur = pieces[idx].getDuration());
         idx++) {
      t -= dur;
    }
    if (idx == N) {
      idx--;
      t += pieces[idx].getDuration();
    }
    return idx;
  }

  inline Eigen::Vector3d getPos(double t) const {
    int pieceIdx = locatePieceIdx(t);
    return pieces[pieceIdx].getPos(t);
  }

  inline Eigen::Vector3d getVel(double t) const {
    int pieceIdx = locatePieceIdx(t);
    return pieces[pieceIdx].getVel(t);
  }

  inline Eigen::Vector3d getAcc(double t) const {
    int pieceIdx = locatePieceIdx(t);
    return pieces[pieceIdx].getAcc(t);
  }

  inline Eigen::Vector3d getJer(double t) const {
    int pieceIdx = locatePieceIdx(t);
    return pieces[pieceIdx].getJer(t);
  }

  inline Eigen::Vector3d getJuncPos(int juncIdx) const {
    if (juncIdx != getPieceNum()) {
      return pieces[juncIdx].getCoeffMat().col(5);
    } else {
      return pieces[juncIdx - 1].getPos(pieces[juncIdx - 1].getDuration());
    }
  }

  inline Eigen::Vector3d getJuncVel(int juncIdx) const {
    if (juncIdx != getPieceNum()) {
      return pieces[juncIdx].getCoeffMat().col(4);
    } else {
      return pieces[juncIdx - 1].getVel(pieces[juncIdx - 1].getDuration());
    }
  }

  inline Eigen::Vector3d getJuncAcc(int juncIdx) const {
    if (juncIdx != getPieceNum()) {
      return pieces[juncIdx].getCoeffMat().col(3) * 2.0;
    } else {
      return pieces[juncIdx - 1].getAcc(pieces[juncIdx - 1].getDuration());
    }
  }

  inline double getMaxVelRate() const {
    int N = getPieceNum();
    double maxVelRate = -INFINITY;
    double tempNorm;
    for (int i = 0; i < N; i++) {
      tempNorm = pieces[i].getMaxVelRate();
      maxVelRate = maxVelRate < tempNorm ? tempNorm : maxVelRate;
    }
    return maxVelRate;
  }

  inline double getMaxAccRate() const {
    int N = getPieceNum();
    double maxAccRate = -INFINITY;
    double tempNorm;
    for (int i = 0; i < N; i++) {
      tempNorm = pieces[i].getMaxAccRate();
      maxAccRate = maxAccRate < tempNorm ? tempNorm : maxAccRate;
    }
    return maxAccRate;
  }

  inline bool checkMaxVelRate(const double &maxVelRate) const {
    int N = getPieceNum();
    bool feasible = true;
    for (int i = 0; i < N && feasible; i++) {
      feasible = feasible && pieces[i].checkMaxVelRate(maxVelRate);
    }
    return feasible;
  }

  inline bool checkMaxAccRate(const double &maxAccRate) const {
    int N = getPieceNum();
    bool feasible = true;
    for (int i = 0; i < N && feasible; i++) {
      feasible = feasible && pieces[i].checkMaxAccRate(maxAccRate);
    }
    return feasible;
  }

  // GaaiLam
  inline double project_pt(const Eigen::Vector3d &pt,
                           int &ii, double &tt, Eigen::Vector3d &pro_pt) {
    double min_dist = -1;
    for (int i = 0; i < getPieceNum(); ++i) {
      auto piece = pieces[i];
      double t = 0;
      double dist = piece.project_pt(pt, t, pro_pt);
      if (dist < 0) {
        continue;
      }
      if (min_dist < 0 || dist < min_dist) {
        min_dist = dist;
        ii = i;
        tt = t;
      }
    }
    return min_dist;
  }
  inline bool intersection_plane(const Eigen::Vector3d p,
                                 const Eigen::Vector3d v,
                                 int &ii, double &tt, Eigen::Vector3d &pt) {
    for (int i = 0; i < getPieceNum(); ++i) {
      const auto &piece = pieces[i];
      if (piece.intersection_plane(p, v, tt, pt)) {
        ii = i;
        return true;
      }
    }
    return false;
  }
  inline std::vector<Eigen::Vector3d> way_points() {
    std::vector<Eigen::Vector3d> pts;
    for (int i = 0; i < getPieceNum(); ++i) {
      pts.push_back(pieces[i].getPos(0));
    }
    return pts;
  }
};
}


#include <boost/units/base_units/metric/hour.hpp>
#include <boost/units/base_units/us/mile.hpp>
#include <boost/units/io.hpp>
#include <boost/units/make_scaled_unit.hpp>
#include <boost/units/quantity.hpp>
#include <boost/units/systems/si/length.hpp>
#include <boost/units/systems/si/prefixes.hpp>
#include <boost/units/systems/si/time.hpp>
#include <boost/units/systems/si/velocity.hpp>
#include <cassert>
#include <iostream>

namespace {

namespace bu = boost::units;

// h
using hour = bu::metric::hour_base_unit::unit_type;
BOOST_UNITS_STATIC_CONSTANT(hours, hour);

// km
using kilometer = bu::make_scaled_unit<bu::si::length, bu::scale<10, bu::static_rational<3>>>::type;

// km/h
using kilometer_per_hour = bu::divide_typeof_helper<kilometer, hour>::type;
BOOST_UNITS_STATIC_CONSTANT(kilometers_per_hour, kilometer_per_hour);

// mi
using mile = bu::us::mile_base_unit::unit_type;
BOOST_UNITS_STATIC_CONSTANT(miles, mile);

// mi/h
using mile_per_hour = bu::divide_typeof_helper<mile, hour>::type;
BOOST_UNITS_STATIC_CONSTANT(miles_per_hour, mile_per_hour);

constexpr bu::quantity<bu::si::velocity> avg_speed(bu::quantity<bu::si::length> d, bu::quantity<bu::si::time> t)
{
  return d / t;
  // return d * t;
}

template<typename T>
void km_per_h(const T& v)
{
  const bu::quantity<kilometer_per_hour> kmph(v);

  std::cout << "v = " << v << '\n';       // prints "v = 30.5556 m s^-1"
  std::cout << "kmph = " << kmph << '\n'; // prints "kmph = 110 k(m h^-1)"

  assert(v.value() != 110);                                                 // <=== !!!!
  assert(v == bu::quantity<bu::si::velocity>(110 * kilometers_per_hour));
  assert(bu::quantity<kilometer_per_hour>(v) != 110 * kilometers_per_hour); // <=== !!!!
  assert(kmph.value() != 110);                                              // <=== !!!!
  assert(kmph != 110 * kilometers_per_hour);                                // <=== !!!!
}

void km_per_h()
{
  constexpr auto v = avg_speed(bu::quantity<bu::si::length>(220 * bu::si::kilo * bu::si::meters), bu::quantity<bu::si::time>(2 * hours));
  km_per_h(v);
}

void km_per_h(double a, double b)
{
  const auto v = avg_speed(bu::quantity<bu::si::length>(a * bu::si::kilo * bu::si::meters), bu::quantity<bu::si::time>(b * hours));
  km_per_h(v);
}

template<typename T>
void mi_per_h(const T& v)
{
  const bu::quantity<mile_per_hour> miph(v);

  std::cout << "v = " << v << '\n';       // prints "v = 31.2928 m s^-1"
  std::cout << "miph = " << miph << '\n'; // prints "miph = 70 mi h^-1"

  assert(v.value() != 70);                                          // <=== !!!!
  assert(v == bu::quantity<bu::si::velocity>(70 * miles_per_hour));
  assert(bu::quantity<mile_per_hour>(v) != 70 * miles_per_hour);    // <=== !!!!
  assert(miph.value() != 70);                                       // <=== !!!!
  assert(miph != 70 * miles_per_hour);                              // <=== !!!!
}

void mi_per_h()
{
  const auto v2 = avg_speed(bu::quantity<bu::si::length>(140 * miles), bu::quantity<bu::si::time>(2 * hours));
  mi_per_h(v2);
}

void mi_per_h(double a, double b)
{
  const auto v2 = avg_speed(bu::quantity<bu::si::length>(a * miles), bu::quantity<bu::si::time>(b * hours));
  mi_per_h(v2);
}

}  // namespace

int main()
{
  km_per_h();
  km_per_h(220, 2);
  mi_per_h();
  mi_per_h(140, 2);
}


#include "boundary.hpp"
#include "eigen_ext.hpp"
#include "parameters.hpp"
#include <cassert>
#include <iostream>

#include <Eigen/Core>
#include <Eigen/SparseCore>

namespace pear {
void boundary_vector(Vec &xp, Vec &yp, MatI &boundary, Vec &Bu, Vec &Bv,
                     Mat &Kbu, Mat &Kbv) {
  /*
  INPUTS
  xp : vector with points x coordinates
  yp : vector with points y coordinates
  g1 : vector of indices of first points of the gamma1 boundary segment
  g2 : vector of indices of second ponts of the gamma1 boundary segment

  OUTPUTS
  Bu, Bv : boundary vector
  Kbu, Kbv : boundary vector term dependent on Cu and Cv
   */
  int np = xp.rows();
  int nb = boundary.rows();
  int ng = 2 * nb - boundary.col(2).sum();

  VecI g1(ng);
  VecI g2(ng);
  compute_skin(boundary, g1, g2);

  // COMPUTE DISTANCES
  Vec hpx =
      (pear::extract<Vec>(xp, g1) - pear::extract<Vec>(xp, g2)).array().pow(2);
  Vec hpy =
      (pear::extract<Vec>(yp, g1) - pear::extract<Vec>(yp, g2)).array().pow(2);

  Vec h = (hpy + hpx).array().pow(.5);

  // COMPUTE BOUNDARY FUNC
  Vec bu(np);
  Vec bv(np);
  Vec kbu(np);
  Vec kbv(np);
  boundary_func(xp, bu, bv, kbu, kbv);

  // BB
  Vec bbu1 =
      (h.array() *
       (2 * pear::extract<Vec>(bu, g1) + pear::extract<Vec>(bu, g2)).array() /
       6);

  Vec bbu2 =
      (h.array() *
       (pear::extract<Vec>(bu, g1) + 2 * pear::extract<Vec>(bu, g2)).array() /
       6);

  Vec bbv1 =
      (h.array() *
       (2 * pear::extract<Vec>(bv, g1) + pear::extract<Vec>(bv, g2)).array() /
       6);

  Vec bbv2 =
      (h.array() *
       (pear::extract<Vec>(bu, g1) + 2 * pear::extract<Vec>(bu, g2)).array() /
       6);

  // KBB
  Vec kbbu1 =
      (h.array() *
       (3 * pear::extract<Vec>(kbu, g1) + pear::extract<Vec>(kbu, g2)).array() /
       12);

  Vec kbbu2 =
      (h.array() *
       (pear::extract<Vec>(kbu, g1) + 3 * pear::extract<Vec>(kbu, g2)).array() /
       12);

  Vec kbbum =
      (h.array() *
       (pear::extract<Vec>(kbu, g1) + pear::extract<Vec>(kbu, g2)).array() /
       12);

  Vec kbbv1 =
      (h.array() *
       (3 * pear::extract<Vec>(kbv, g1) + pear::extract<Vec>(kbv, g2)).array() /
       12);

  Vec kbbv2 =
      (h.array() *
       (pear::extract<Vec>(kbv, g1) + 3 * pear::extract<Vec>(kbv, g2)).array() /
       12);

  Vec kbbvm =
      (h.array() *
       (pear::extract<Vec>(kbv, g1) + pear::extract<Vec>(kbv, g2)).array() /
       12);

  // FILL BOUNDARY VECTOR
  for (int idxg = 0; idxg < ng; idxg++) {
    Bu(g1(idxg)) += bbu1(idxg);
    Bu(g2(idxg)) += bbu2(idxg);
    Bv(g2(idxg)) += bbv1(idxg);
    Bv(g2(idxg)) += bbv2(idxg);
  }

  // FILL BOUNDARY MATRIX

  for (int idxg = 0; idxg < ng; idxg++) {
    Kbu(g1(idxg), g1(idxg)) += kbbu1(idxg);
    Kbu(g2(idxg), g2(idxg)) += kbbu2(idxg);
    Kbu(g1(idxg), g2(idxg)) += kbbum(idxg);
    Kbu(g2(idxg), g1(idxg)) += kbbum(idxg);

    Kbv(g1(idxg), g1(idxg)) += kbbv1(idxg);
    Kbv(g2(idxg), g2(idxg)) += kbbv2(idxg);
    Kbv(g1(idxg), g2(idxg)) += kbbvm(idxg);
    Kbv(g2(idxg), g1(idxg)) += kbbvm(idxg);
  }
}

void boundary_func(Vec &xp, Vec &bu, Vec &bv, Vec &kbu, Vec &kbv) {
  bu = xp * pear::hu * pear::Cuamb;
  bv = xp * pear::hv * pear::Cvamb;

  kbu = -pear::hu * xp;
  kbv = -pear::hv * xp;
}

void compute_skin(MatI &boundary, VecI &g1, VecI &g2) {
  int nb = boundary.rows();
  int ng = 2 * nb - boundary.col(2).sum();

  int idxg = 0;
  for (int idxb = 0; idxb < nb; idxb++) {
    if (boundary(idxb, 2) == 1) {
      g1(idxg) = boundary(idxb, 0);
      g2(idxg) = boundary(idxb, 1);
      idxg++;
    }
  }
}
} // namespace pear


//
// Created by david on 2019-05-27.
//

#include <Eigen/QR>
#include <general/class_tic_toc.h>
#include <math/svd.h>

std::optional<long long> svd::solver::count = 0;

svd::solver::solver() {
    setLogLevel(2);
    t_wrk = std::make_unique<class_tic_toc>();
    t_adj = std::make_unique<class_tic_toc>();
    t_jac = std::make_unique<class_tic_toc>();
    t_svd = std::make_unique<class_tic_toc>();
    if(not count) count = 0;
}
void svd::solver::copy_settings(const svd::settings &svd_settings) {
    if(svd_settings.threshold) threshold = svd_settings.threshold.value();
    if(svd_settings.switchsize) switchsize = svd_settings.switchsize.value();
    if(svd_settings.loglevel) setLogLevel(svd_settings.loglevel.value());
    if(svd_settings.use_bdc) use_bdc = svd_settings.use_bdc.value();
    if(svd_settings.use_lapacke) use_lapacke = svd_settings.use_lapacke.value();
    if(svd_settings.profile and svd_settings.profile.value()) enableProfiling();
}

svd::solver::solver(const svd::settings &svd_settings) : solver() { copy_settings(svd_settings); }

svd::solver::solver(std::optional<svd::settings> svd_settings) : solver() {
    if(svd_settings) copy_settings(svd_settings.value());
}

void svd::solver::enableProfiling() {
    t_wrk->set_properties(true, 5, "work");
    t_adj->set_properties(true, 5, "adjoint");
    t_jac->set_properties(true, 5, "jacobi");
    t_svd->set_properties(true, 5, "bdcsvd");
}
void svd::solver::disableProfiling() {
    t_wrk->set_properties(false, 0, "");
    t_adj->set_properties(false, 0, "");
    t_jac->set_properties(false, 0, "");
    t_svd->set_properties(false, 0, "");
}

void svd::solver::setLogLevel(size_t logLevel) {
    if(not svd::log)
        tools::Logger::setLogger(svd::log, "svd", logLevel, true);
    else
        tools::Logger::setLogLevel(svd::log, logLevel);
    svd::log->set_pattern("[%Y-%m-%d %H:%M:%S.%e][%n]%^[%=8l]%$ %v");
}

/*! \brief Performs SVD on a matrix
 *  This function is defined in cpp to avoid long compilation times when having Eigen::BDCSVD included everywhere in headers.
 *  Performs rigorous checks to ensure stability of DMRG.
 *  In some cases Eigen::BCDSVD/JacobiSVD will fail with segfault. Here we use a patched version of Eigen that throws an error
 *  instead so we get a chance to catch it and use lapack svd instead.
 *   \param mat_ptr Pointer to the matrix. Supported are double * and std::complex<double> *
 *   \param rows Rows of the matrix
 *   \param cols Columns of the matrix
 *   \param rank_max Maximum number of singular values
 *   \return The U, S, and V matrices (with S as a vector) extracted from the Eigen::BCDSVD SVD object.
 */
template<typename Scalar>
std::tuple<svd::solver::MatrixType<Scalar>, svd::solver::VectorType<Scalar>, svd::solver::MatrixType<Scalar>, long>
    svd::solver::do_svd(const Scalar *mat_ptr, long rows, long cols, std::optional<long> rank_max) {
    if(use_lapacke) {
        try {
            return do_svd_lapacke(mat_ptr, rows, cols, rank_max);
        } catch(const std::exception &ex) {
            svd::log->warn(FMT_COMPILE("Lapacke failed to perform SVD: {} | Trying Eigen"), ex.what());
            return do_svd_eigen(mat_ptr, rows, cols, rank_max);
        }
    }else {
        try {
            return do_svd_eigen(mat_ptr, rows, cols, rank_max);
        } catch(const std::exception &ex) {
            svd::log->warn(FMT_COMPILE("Eigen failed to perform SVD: {} | Trying Lapacke"), ex.what());
            return do_svd_lapacke(mat_ptr, rows, cols, rank_max);
        }
    }
}

//! \relates svd::class_SVD
//! \brief force instantiation of do_svd for type 'double'
template std::tuple<svd::solver::MatrixType<double>, svd::solver::VectorType<double>, svd::solver::MatrixType<double>, long>
    svd::solver::do_svd(const double *, long, long, std::optional<long>);

using cplx = std::complex<double>;
//! \relates svd::class_SVD
//! \brief force instantiation of do_svd for type 'std::complex<double>'
template std::tuple<svd::solver::MatrixType<cplx>, svd::solver::VectorType<cplx>, svd::solver::MatrixType<cplx>, long>
    svd::solver::do_svd(const cplx *, long, long, std::optional<long>);

template<typename Scalar>
Eigen::Tensor<Scalar, 2> svd::solver::pseudo_inverse(const Eigen::Tensor<Scalar, 2> &tensor) {
    if(tensor.dimension(0) <= 0) { throw std::runtime_error("pseudo_inverse error: Dimension is zero: tensor.dimension(0)"); }
    if(tensor.dimension(1) <= 0) { throw std::runtime_error("pseudo_inverse error: Dimension is zero: tensor.dimension(1)"); }
    Eigen::Map<const MatrixType<Scalar>> mat(tensor.data(), tensor.dimension(0), tensor.dimension(1));
    return Textra::TensorCast(mat.completeOrthogonalDecomposition().pseudoInverse());
}

//! \relates svd::class_SVD
//! \brief force instantiation of pseudo_inverse for type 'double'
template Eigen::Tensor<double, 2> svd::solver::pseudo_inverse(const Eigen::Tensor<double, 2> &tensor);
//! \relates svd::class_SVD
//! \brief force instantiation of pseudo_inverse for type 'std::complex<double>'
template Eigen::Tensor<cplx, 2> svd::solver::pseudo_inverse(const Eigen::Tensor<cplx, 2> &tensor);


#include "SimulationCore_pcp.h"

#include <Eigen/SparseLU>

#define Keep_Newmark_Coefficients
#include "Step_S2D_CHM_s_FEM_uUp.h"

namespace
{
	typedef Model_S2D_CHM_s_FEM_uUp::ShapeFuncValue ShapeFuncValue;
	typedef Model_S2D_CHM_s_FEM_uUp::GaussPoint GaussPoint_fem;
	typedef Model_S2D_CHM_s_FEM_uUp::Element Element_fem;
	typedef Model_S2D_CHM_s_FEM_uUp::Node Node_fem;
	typedef Model_S2D_CHM_s_FEM_uUp::DOF DOF;

	void print_sparse_mat(Eigen::SparseMatrix<double> &mat,
		std::fstream &out_file, const char *mat_name = nullptr)
	{
		if (mat_name)
			out_file << mat_name << "\n";
		size_t row_num = mat.rows();
		size_t col_num = mat.cols();
		double value;
		for (size_t i = 0; i < row_num; ++i)
		{
			for (size_t j = 0; j < col_num; ++j)
			{
				value = mat.coeff(i, j);
				out_file << value << ", ";
			}
			out_file << "\n";
		}
	}
};

Step_S2D_CHM_s_FEM_uUp::Step_S2D_CHM_s_FEM_uUp() :
	Step(&solve_substep_S2D_CHM_s_FEM_uUp),
	model(nullptr), kmat_col(nullptr) {}

Step_S2D_CHM_s_FEM_uUp::~Step_S2D_CHM_s_FEM_uUp() {}

int Step_S2D_CHM_s_FEM_uUp::init_calculation(void)
{
	if (is_first_step) {}

	kmat_col = new double[model->dof_num];

	// for debug
	out_file.open("debug_mat_out_CHM_s_FEM_uUp.csv", std::ios::binary | std::ios::out);

	return 0;
}

int Step_S2D_CHM_s_FEM_uUp::finalize_calculation(void)
{
	if (kmat_col)
	{
		delete[] kmat_col;
		kmat_col = nullptr;
	}

	// for debug
	out_file.close();

	return 0;
}

int solve_substep_S2D_CHM_s_FEM_uUp(void *_self)
{
	// for debug
	//static std::fstream out_f2("out_f2.txt", std::ios::binary | std::ios::out);

	Step_S2D_CHM_s_FEM_uUp &self = *(Step_S2D_CHM_s_FEM_uUp *)(_self);
	Model_S2D_CHM_s_FEM_uUp &model = *self.model;
	double dt = self.dtime;
	double dt2 = dt * dt;

	// list of non-zeros coefficients
	MatrixCoefficientSet<> &g_kmat_coefs = self.g_kmat_coefs;
	g_kmat_coefs.init(model.dof_num);
	Eigen::VectorXd g_fvec(model.dof_num);
	g_fvec.setZero();

	size_t l2g_dof_id_map[20];
	double e_kmat[20][20], e_fvec[20];
	for (size_t e_id = 0; e_id < model.elem_num; ++e_id)
	{
		Element_fem &e = model.elems[e_id];
		// form elemental stiffness matrix and force vector
		self.form_elem_stiffness_mat_and_force_vec(e, e_kmat, e_fvec);

		// map from local dof id to global dof id
		// solid phase ux
		l2g_dof_id_map[0] = model.n_id_to_dof_id(e.n1_id, DOF::usx);
		l2g_dof_id_map[1] = model.n_id_to_dof_id(e.n2_id, DOF::usx);
		l2g_dof_id_map[2] = model.n_id_to_dof_id(e.n3_id, DOF::usx);
		l2g_dof_id_map[3] = model.n_id_to_dof_id(e.n4_id, DOF::usx);
		// solid phase uy
		l2g_dof_id_map[4] = model.n_id_to_dof_id(e.n1_id, DOF::usy);
		l2g_dof_id_map[5] = model.n_id_to_dof_id(e.n2_id, DOF::usy);
		l2g_dof_id_map[6] = model.n_id_to_dof_id(e.n3_id, DOF::usy);
		l2g_dof_id_map[7] = model.n_id_to_dof_id(e.n4_id, DOF::usy);
		// fluid phase ux
		l2g_dof_id_map[8]  = model.n_id_to_dof_id(e.n1_id, DOF::ufx);
		l2g_dof_id_map[9]  = model.n_id_to_dof_id(e.n2_id, DOF::ufx);
		l2g_dof_id_map[10] = model.n_id_to_dof_id(e.n3_id, DOF::ufx);
		l2g_dof_id_map[11] = model.n_id_to_dof_id(e.n4_id, DOF::ufx);
		// fluid phase uy
		l2g_dof_id_map[12] = model.n_id_to_dof_id(e.n1_id, DOF::ufy);
		l2g_dof_id_map[13] = model.n_id_to_dof_id(e.n2_id, DOF::ufy);
		l2g_dof_id_map[14] = model.n_id_to_dof_id(e.n3_id, DOF::ufy);
		l2g_dof_id_map[15] = model.n_id_to_dof_id(e.n4_id, DOF::ufy);
		// p
		l2g_dof_id_map[16] = model.n_id_to_dof_id(e.n1_id, DOF::p);
		l2g_dof_id_map[17] = model.n_id_to_dof_id(e.n2_id, DOF::p);
		l2g_dof_id_map[18] = model.n_id_to_dof_id(e.n3_id, DOF::p);
		l2g_dof_id_map[19] = model.n_id_to_dof_id(e.n4_id, DOF::p);
		//print_vec(l2g_dof_id_map);
		// add to global matrix and vector
		size_t g_dof_id1, g_dof_id2;
		for (size_t l_id1 = 0; l_id1 < 20; ++l_id1)
		{
			// add to global matrix
			g_dof_id1 = l2g_dof_id_map[l_id1];
			for (size_t l_id2 = 0; l_id2 < 20; ++l_id2)
			{
				g_dof_id2 = l2g_dof_id_map[l_id2];
				g_kmat_coefs.add_coefficient(g_dof_id1, g_dof_id2, e_kmat[l_id1][l_id2]);
			}
			// add to global force vector
			g_fvec[g_dof_id1] += e_fvec[l_id1];
		}
	}

	// apply external force body force and traction
	size_t dof_id;
	// traction
	double tfs[4];
	for (size_t t_id = 0; t_id < model.tx_num; ++t_id)
	{
		TractionBC_2DFEM &tx = model.txs[t_id];
		model.cal_traction_bc(tx, tfs);
		Element_fem &e = model.elems[tx.elem_id];
		dof_id = model.n_id_to_dof_id(e.n1_id, DOF::usx);
		g_fvec[dof_id] += tfs[0];
		dof_id = model.n_id_to_dof_id(e.n2_id, DOF::usx);
		g_fvec[dof_id] += tfs[1];
		dof_id = model.n_id_to_dof_id(e.n3_id, DOF::usx);
		g_fvec[dof_id] += tfs[2];
		dof_id = model.n_id_to_dof_id(e.n4_id, DOF::usx);
		g_fvec[dof_id] += tfs[3];
	}
	for (size_t t_id = 0; t_id < model.ty_num; ++t_id)
	{
		TractionBC_2DFEM &ty = model.tys[t_id];
		model.cal_traction_bc(ty, tfs);
		Element_fem &e = model.elems[ty.elem_id];
		dof_id = model.n_id_to_dof_id(e.n1_id, DOF::usy);
		g_fvec[dof_id] += tfs[0];
		dof_id = model.n_id_to_dof_id(e.n2_id, DOF::usy);
		g_fvec[dof_id] += tfs[1];
		dof_id = model.n_id_to_dof_id(e.n3_id, DOF::usy);
		g_fvec[dof_id] += tfs[2];
		dof_id = model.n_id_to_dof_id(e.n4_id, DOF::usy);
		g_fvec[dof_id] += tfs[3];
	}
	// body force ...  to be finished

	// apply displacement boundary condition
	double dig_term;
	for (size_t bc_id = 0; bc_id < model.usx_num; ++bc_id)
	{
		DisplacementBC &dbc = model.usxs[bc_id];
		dof_id = model.n_id_to_dof_id(dbc.node_id, DOF::usx);
		dig_term = g_kmat_coefs.del_col_and_row(dof_id, self.kmat_col);
		for (size_t row_id = 0; row_id < model.dof_num; ++row_id)
			g_fvec[row_id] -= self.kmat_col[row_id] * dbc.u;
		g_fvec[dof_id] = dig_term * dbc.u;
	}
	for (size_t bc_id = 0; bc_id < model.usy_num; ++bc_id)
	{
		DisplacementBC &dbc = model.usys[bc_id];
		dof_id = model.n_id_to_dof_id(dbc.node_id, DOF::usy);
		dig_term = g_kmat_coefs.del_col_and_row(dof_id, self.kmat_col);
		for (size_t row_id = 0; row_id < model.dof_num; ++row_id)
			g_fvec[row_id] -= self.kmat_col[row_id] * dbc.u;
		g_fvec[dof_id] = dig_term * dbc.u;
	}
	for (size_t bc_id = 0; bc_id < model.ufx_num; ++bc_id)
	{
		DisplacementBC &dbc = model.ufxs[bc_id];
		dof_id = model.n_id_to_dof_id(dbc.node_id, DOF::ufx);
		dig_term = g_kmat_coefs.del_col_and_row(dof_id, self.kmat_col);
		for (size_t row_id = 0; row_id < model.dof_num; ++row_id)
			g_fvec[row_id] -= self.kmat_col[row_id] * dbc.u;
		g_fvec[dof_id] = dig_term * dbc.u;
	}
	for (size_t bc_id = 0; bc_id < model.ufy_num; ++bc_id)
	{
		DisplacementBC &dbc = model.ufys[bc_id];
		dof_id = model.n_id_to_dof_id(dbc.node_id, DOF::ufy);
		dig_term = g_kmat_coefs.del_col_and_row(dof_id, self.kmat_col);
		for (size_t row_id = 0; row_id < model.dof_num; ++row_id)
			g_fvec[row_id] -= self.kmat_col[row_id] * dbc.u;
		g_fvec[dof_id] = dig_term * dbc.u;
	}
	for (size_t bc_id = 0; bc_id < model.pbc_num; ++bc_id)
	{
		PressureBC &pbc = model.pbcs[bc_id];
		dof_id = model.n_id_to_dof_id(pbc.node_id, DOF::p);
		dig_term = g_kmat_coefs.del_col_and_row(dof_id, self.kmat_col);
		for (size_t row_id = 0; row_id < model.dof_num; ++row_id)
			g_fvec[row_id] -= self.kmat_col[row_id] * pbc.p;
		g_fvec[dof_id] = dig_term * pbc.p;
	}

	// solve
	//g_kmat_coefs.print_with_iter();
	Eigen::SparseMatrix<double> g_kmat(model.dof_num, model.dof_num);
	g_kmat.setFromTriplets(g_kmat_coefs.begin(), g_kmat_coefs.end());
	//print_sparse_mat(g_kmat, self.out_file,nullptr);
	Eigen::SparseLU<Eigen::SparseMatrix<double> > solver(g_kmat);
	Eigen::VectorXd g_du_vec = solver.solve(g_fvec);
	//std::cout << g_fvec << "\n";
	//std::cout << g_du_vec << "\n";

	// reapply disp bc
	for (size_t bc_id = 0; bc_id < model.usx_num; ++bc_id)
	{
		DisplacementBC &dbc = model.usxs[bc_id];
		dof_id = model.n_id_to_dof_id(dbc.node_id, DOF::usx);
		g_du_vec[dof_id] = dbc.u;
	}
	for (size_t bc_id = 0; bc_id < model.usy_num; ++bc_id)
	{
		DisplacementBC &dbc = model.usys[bc_id];
		dof_id = model.n_id_to_dof_id(dbc.node_id, DOF::usy);
		g_du_vec[dof_id] = dbc.u;
	}
	for (size_t bc_id = 0; bc_id < model.ufx_num; ++bc_id)
	{
		DisplacementBC &dbc = model.ufxs[bc_id];
		dof_id = model.n_id_to_dof_id(dbc.node_id, DOF::ufx);
		g_du_vec[dof_id] = dbc.u;
	}
	for (size_t bc_id = 0; bc_id < model.ufy_num; ++bc_id)
	{
		DisplacementBC &dbc = model.ufys[bc_id];
		dof_id = model.n_id_to_dof_id(dbc.node_id, DOF::ufy);
		g_du_vec[dof_id] = dbc.u;
	}
	for (size_t bc_id = 0; bc_id < model.pbc_num; ++bc_id)
	{
		PressureBC &pbc = model.pbcs[bc_id];
		dof_id = model.n_id_to_dof_id(pbc.node_id, DOF::p);
		g_du_vec[dof_id] = pbc.p;
	}

	// update nodal variables
	double da, dv, du;
	for (size_t n_id = 0; n_id < model.node_num; ++n_id)
	{
		Node_fem &n = model.nodes[n_id];
		// sx
		dof_id = model.n_id_to_dof_id(n_id, DOF::usx);
		du = g_du_vec[dof_id];
		n.ux_s += du;
		dv = gamma / (beta * dt) * du - gamma / beta * n.vx_s + dt * (1.0 - gamma / (2.0 * beta)) * n.ax_s;
		n.vx_s += dv;
		da = du / (gamma * dt2) - n.vx_s / (gamma * dt) - n.ax_s / (2.0 * gamma);
		n.ax_s += da;
		// sy
		dof_id = model.n_id_to_dof_id(n_id, DOF::usy);
		du = g_du_vec[dof_id];
		n.uy_s += du;
		dv = gamma / (beta * dt) * du - gamma / beta * n.vy_s + dt * (1.0 - gamma / (2.0 * beta)) * n.ay_s;
		n.vy_s += dv;
		da = du / (gamma * dt2) -  n.vy_s / (gamma * dt) - n.ay_s / (2.0 * gamma);
		n.ay_s += da;
		// fx
		dof_id = model.n_id_to_dof_id(n_id, DOF::ufx);
		du = g_du_vec[dof_id];
		n.ux_f += du;
		dv = gamma / (beta * dt) * du - gamma / beta * n.vx_f + dt * (1.0 - gamma / (2.0 * beta)) * n.ax_f;
		n.vx_f += dv;
		da = 1.0 / (gamma * dt2) * du - 1.0 / (gamma * dt) * n.vx_f - 1.0 / (2.0 * gamma) * n.ax_f;
		n.ax_f += da;
		// fy
		dof_id = model.n_id_to_dof_id(n_id, DOF::ufy);
		du = g_du_vec[dof_id];
		n.uy_f += du;
		dv = gamma / (beta * dt) * du - gamma / beta * n.vy_f + dt * (1.0 - gamma / (2.0 * beta)) * n.ay_s;
		n.vy_f += dv;
		da = 1.0 / (gamma * dt2) * du - 1.0 / (gamma * dt) * n.vy_f - 1.0 / (2.0 * gamma) * n.ay_s;
		n.ay_f += da;
		// p
		dof_id = model.n_id_to_dof_id(n_id, DOF::p);
		n.p += g_du_vec[dof_id];
	}

	// Update element variables
	ShapeFuncValue &sf1 = model.gp1_sf;
	ShapeFuncValue &sf2 = model.gp2_sf;
	ShapeFuncValue &sf3 = model.gp3_sf;
	ShapeFuncValue &sf4 = model.gp4_sf;
	double dux_s1, duy_s1, dux_f1, duy_f1, dp1;
	double dux_s2, duy_s2, dux_f2, duy_f2, dp2;
	double dux_s3, duy_s3, dux_f3, duy_f3, dp3;
	double dux_s4, duy_s4, dux_f4, duy_f4, dp4;
	for (size_t e_id = 0; e_id < model.elem_num; ++e_id)
	{
		Element_fem &e = model.elems[e_id];
		dux_s1 = g_du_vec[model.n_id_to_dof_id(e.n1_id, DOF::usx)];
		duy_s1 = g_du_vec[model.n_id_to_dof_id(e.n1_id, DOF::usy)];
		dux_f1 = g_du_vec[model.n_id_to_dof_id(e.n1_id, DOF::ufx)];
		duy_f1 = g_du_vec[model.n_id_to_dof_id(e.n1_id, DOF::ufy)];
		dp1    = g_du_vec[model.n_id_to_dof_id(e.n1_id, DOF::p)];
		dux_s2 = g_du_vec[model.n_id_to_dof_id(e.n2_id, DOF::usx)];
		duy_s2 = g_du_vec[model.n_id_to_dof_id(e.n2_id, DOF::usy)];
		dux_f2 = g_du_vec[model.n_id_to_dof_id(e.n2_id, DOF::ufx)];
		duy_f2 = g_du_vec[model.n_id_to_dof_id(e.n2_id, DOF::ufy)];
		dp2    = g_du_vec[model.n_id_to_dof_id(e.n2_id, DOF::p)];
		dux_s3 = g_du_vec[model.n_id_to_dof_id(e.n3_id, DOF::usx)];
		duy_s3 = g_du_vec[model.n_id_to_dof_id(e.n3_id, DOF::usy)];
		dux_f3 = g_du_vec[model.n_id_to_dof_id(e.n3_id, DOF::ufx)];
		duy_f3 = g_du_vec[model.n_id_to_dof_id(e.n3_id, DOF::ufy)];
		dp3    = g_du_vec[model.n_id_to_dof_id(e.n3_id, DOF::p)];
		dux_s4 = g_du_vec[model.n_id_to_dof_id(e.n4_id, DOF::usx)];
		duy_s4 = g_du_vec[model.n_id_to_dof_id(e.n4_id, DOF::usy)];
		dux_f4 = g_du_vec[model.n_id_to_dof_id(e.n4_id, DOF::ufx)];
		duy_f4 = g_du_vec[model.n_id_to_dof_id(e.n4_id, DOF::ufy)];
		dp4    = g_du_vec[model.n_id_to_dof_id(e.n4_id, DOF::p)];

		self.update_gauss_point(e.gp1, sf1, dt, dt2,
			dux_s1, dux_s2, dux_s3, dux_s4,
			duy_s1, duy_s2, duy_s3, duy_s4,
			dux_f1, dux_f2, dux_f3, dux_f4,
			duy_f1, duy_f2, duy_f3, duy_f4,
			dp1, dp2, dp3, dp4);

		self.update_gauss_point(e.gp2, sf2, dt, dt2,
			dux_s1, dux_s2, dux_s3, dux_s4,
			duy_s1, duy_s2, duy_s3, duy_s4,
			dux_f1, dux_f2, dux_f3, dux_f4,
			duy_f1, duy_f2, duy_f3, duy_f4,
			dp1, dp2, dp3, dp4);

		self.update_gauss_point(e.gp3, sf3, dt, dt2,
			dux_s1, dux_s2, dux_s3, dux_s4,
			duy_s1, duy_s2, duy_s3, duy_s4,
			dux_f1, dux_f2, dux_f3, dux_f4,
			duy_f1, duy_f2, duy_f3, duy_f4,
			dp1, dp2, dp3, dp4);

		self.update_gauss_point(e.gp4, sf4, dt, dt2,
			dux_s1, dux_s2, dux_s3, dux_s4,
			duy_s1, duy_s2, duy_s3, duy_s4,
			dux_f1, dux_f2, dux_f3, dux_f4,
			duy_f1, duy_f2, duy_f3, duy_f4,
			dp1, dp2, dp3, dp4);
	}

	return 0;
}

void Step_S2D_CHM_s_FEM_uUp::update_gauss_point(
	Model_S2D_CHM_s_FEM_uUp::GaussPoint &gp,
	Model_S2D_CHM_s_FEM_uUp::ShapeFuncValue &sf,
	double dt, double dt2,
	double dux_s1, double dux_s2, double dux_s3, double dux_s4,
	double duy_s1, double duy_s2, double duy_s3, double duy_s4,
	double dux_f1, double dux_f2, double dux_f3, double dux_f4,
	double duy_f1, double duy_f2, double duy_f3, double duy_f4,
	double dp1, double dp2, double dp3, double dp4)
{
	double dux_s, duy_s, dux_f, duy_f, dp;
	double dvx_s, dvy_s, dvx_f, dvy_f;
	double dax_s, day_s, dax_f, day_f;
	double de11, de22, de12, ds11, ds22, ds12;
	double de_vol_s, de_vol_f;

	// displacement
	dux_s = dux_s1 * sf.N1 + dux_s2 * sf.N2 + dux_s3 * sf.N3 + dux_s4 * sf.N4;
	duy_s = duy_s1 * sf.N1 + duy_s2 * sf.N2 + duy_s3 * sf.N3 + duy_s4 * sf.N4;
	dux_f = dux_f1 * sf.N1 + dux_f2 * sf.N2 + dux_f3 * sf.N3 + dux_f4 * sf.N4;
	duy_f = duy_f1 * sf.N1 + duy_f2 * sf.N2 + duy_f3 * sf.N3 + duy_f4 * sf.N4;
	// velocity
	dvx_s = gamma / (beta * dt) * dux_s - gamma / beta * gp.vx_s
		  + dt * (1.0 - gamma / (2.0 * beta)) * gp.ax_s;
	dvy_s = gamma / (beta * dt) * duy_s - gamma / beta * gp.vy_s
		  + dt * (1.0 - gamma / (2.0 * beta)) * gp.ay_s;
	dvx_f = gamma / (beta * dt) * dux_f - gamma / beta * gp.vx_f
		  + dt * (1.0 - gamma / (2.0 * beta)) * gp.ax_f;
	dvy_f = gamma / (beta * dt) * duy_f - gamma / beta * gp.vy_f
		  + dt * (1.0 - gamma / (2.0 * beta)) * gp.ay_f;
	// acceleration
	dax_s = dux_s / (beta * dt2) - gp.vx_s / (beta * dt) - gp.ax_s / (2.0 * beta);
	day_s = duy_s / (beta * dt2) - gp.vy_s / (beta * dt) - gp.ay_s / (2.0 * beta);
	dax_f = dux_f / (beta * dt2) - gp.vx_f / (beta * dt) - gp.ax_f / (2.0 * beta);
	day_f = duy_f / (beta * dt2) - gp.vy_f / (beta * dt) - gp.ay_f / (2.0 * beta);
	gp.ux_s += dux_s;
	gp.uy_s += duy_s;
	gp.ux_f += dux_f;
	gp.uy_f += duy_f;
	gp.vx_s += dvx_s;
	gp.vy_s += dvy_s;
	gp.vx_f += dvx_f;
	gp.vy_f += dvy_f;
	gp.ax_s += dax_s;
	gp.ay_s += day_s;
	gp.ax_f += dax_f;
	gp.ay_f += day_f;

	// pore pressure
	gp.p += dp1 * sf.N1 + dp2 * sf.N2 + dp3 * sf.N3 + dp4 * sf.N4;

	// strain increment
	de11 = dux_s1 * sf.dN1_dx + dux_s2 * sf.dN2_dx
		 + dux_s3 * sf.dN3_dx + dux_s4 * sf.dN4_dx;
	de22 = duy_s1 * sf.dN1_dy + duy_s2 * sf.dN2_dy
		 + duy_s3 * sf.dN3_dy + duy_s4 * sf.dN4_dy;
	de12 = (dux_s1 * sf.dN1_dy + dux_s2 * sf.dN2_dy
		  + dux_s3 * sf.dN3_dy + dux_s4 * sf.dN4_dy
		  + duy_s1 * sf.dN1_dx + duy_s2 * sf.dN2_dx
		  + duy_s3 * sf.dN3_dx + duy_s4 * sf.dN4_dx) * 0.5;
	gp.e11 += de11;
	gp.e22 += de22;
	gp.e12 += de12;

	// update stress
	double E_tmp = gp.E / (1.0 + gp.niu) / (1.0 - 2.0 * gp.niu);
	ds11 = E_tmp * ((1.0 - gp.niu) * de11 + gp.niu * de22);
	ds22 = E_tmp * (gp.niu * de11 + (1.0 - gp.niu) * de22);
	ds12 = 2.0 * gp.E / (2.0 * (1.0 + gp.niu)) * de12;
	gp.s11 += ds11;
	gp.s22 += ds22;
	gp.s12 += ds12;

	// volumetric strain of solid phase
	de_vol_s = de11 + de22;
	// "volumetric strain" of fluid phase
	de_vol_f = -(1.0 - gp.n) / gp.n * de_vol_s
		- (dux_f1 * sf.dN1_dx + dux_f2 * sf.dN2_dx + dux_f3 * sf.dN3_dx + dux_f4 * sf.dN4_dx)
		- (duy_f1 * sf.dN1_dy + duy_f2 * sf.dN2_dy + duy_f3 * sf.dN3_dy + duy_f4 * sf.dN4_dy);

	// porosity
	//gp.n = (de_vol_s + gp.n) / (1.0 + de_vol_s);
	// fluid density
	//gp.density_f += gp.density_f * de_vol_f;
}