/* $Id: step-48.cc 27687 2012-11-26 14:37:31Z kronbichler $ */
/* Author: Katharina Kormann, Martin Kronbichler, Uppsala University, 2011-2012 */

/*    $Id: step-48.cc 27687 2012-11-26 14:37:31Z kronbichler $       */
/*                                                                */
/*    Copyright (C) 2011-2012 by the deal.II authors             */
/*                                                                */
/*    This file is subject to QPL and may not be  distributed     */
/*    without copyright and license information. Please refer     */
/*    to the file deal.II/doc/license.html for the  text  and     */
/*    further information on this license.                        */

// The necessary files from the deal.II library.
#include <deal.II/base/logstream.h>
#include <deal.II/base/utilities.h>
#include <deal.II/base/function.h>
#include <deal.II/base/conditional_ostream.h>
#include <deal.II/base/timer.h>
#include <deal.II/lac/vector.h>
#include <deal.II/grid/tria.h>
#include <deal.II/grid/grid_generator.h>
#include <deal.II/grid/tria_boundary_lib.h>
#include <deal.II/dofs/dof_tools.h>
#include <deal.II/dofs/dof_handler.h>
#include <deal.II/lac/constraint_matrix.h>
#include <deal.II/lac/sparse_matrix.h>
#include <deal.II/lac/compressed_simple_sparsity_pattern.h>
#include <deal.II/lac/trilinos_vector.h>
#include <deal.II/fe/fe_q.h>
#include <deal.II/fe/fe_values.h>
#include <deal.II/numerics/vector_tools.h>
#include <deal.II/numerics/data_out.h>
#include <deal.II/distributed/tria.h>

// This includes the data structures for the efficient implementation of
// matrix-free methods.
#include <deal.II/lac/parallel_vector.h>
#include <deal.II/matrix_free/matrix_free.h>
#include <deal.II/matrix_free/fe_evaluation.h>

#include <fstream>
#include <iostream>
#include <iomanip>

namespace Step48
{
  using namespace dealii;

  // We start by defining two global variables to collect all parameters
  // subject to changes at one place: One for the dimension and one for the
  // finite element degree. The dimension is used in the main function as a
  // template argument for the actual classes (like in all other deal.II
  // programs), whereas the degree of the finite element is more crucial, as
  // it is passed as a template argument to the implementation of the
  // Sine-Gordon operator. Therefore, it needs to be a compile-time constant.
  const unsigned int dimension = 2;
  const unsigned int fe_degree = 4;

  // @sect3{SineGordonOperation}

  // The SineGordonOperation class implements the cell-based
  // operation that is needed in each time step. This nonlinear operation can
  // be implemented straight-forwardly based on the MatrixFree
  // class, in the same way as a linear operation would be treated by this
  // implementation of the finite element operator application. We apply two
  // template arguments to the class, one for the dimension and one for the
  // degree of the finite element. This is a difference to other functions in
  // deal.II where only the dimension is a template argument. This is
  // necessary to provide the inner loops in @p FEEvaluation with information
  // about loop lengths etc., which is essential for efficiency. On the other
  // hand, it makes it more challenging to implement the degree as a run-time
  // parameter.
  template <int dim, int fe_degree>
  class SineGordonOperation
  {
  public:
    SineGordonOperation(const MatrixFree<dim,double> &data_in,
                        const double                  time_step);

    void apply (parallel::distributed::Vector<double>                     &dst,
                const std::vector<parallel::distributed::Vector<double>*> &src) const;

  private:
    const MatrixFree<dim,double>         &data;
    const VectorizedArray<double>         delta_t_sqr;
    parallel::distributed::Vector<double> inv_mass_matrix;

    void local_apply (const MatrixFree<dim,double>               &data,
                      parallel::distributed::Vector<double>      &dst,
                      const std::vector<parallel::distributed::Vector<double>*> &src,
                      const std::pair<unsigned int,unsigned int> &cell_range) const;
  };

  // @sect4{SineGordonOperation::SineGordonOperation}

  // This is the constructor of the SineGordonOperation class. It receives a
  // reference to the MatrixFree holding the problem information and the time
  // step size as input parameters. The initialization routine sets up the
  // mass matrix. Since we use Gauss-Lobatto elements, the mass matrix is a
  // diagonal matrix and can be stored as a vector. The computation of the
  // mass matrix diagonal is simple to achieve with the data structures
  // provided by FEEvaluation: Just loop over all (macro-) cells and integrate
  // over the function that is constant one on all quadrature points by using
  // the integrate function with @p true argument at the slot for
  // values. Finally, we invert the diagonal entries since we have to multiply
  // by the inverse mass matrix in each time step.
  template <int dim, int fe_degree>
  SineGordonOperation<dim,fe_degree>::
  SineGordonOperation(const MatrixFree<dim,double> &data_in,
                      const double                  time_step)
    :
    data(data_in),
    delta_t_sqr(make_vectorized_array(time_step *time_step))
  {
    VectorizedArray<double> one = make_vectorized_array (1.);

    data.initialize_dof_vector (inv_mass_matrix);

    FEEvaluationGL<dim,fe_degree> fe_eval(data);
    const unsigned int            n_q_points = fe_eval.n_q_points;

    for (unsigned int cell=0; cell<data.n_macro_cells(); ++cell)
      {
        fe_eval.reinit(cell);
        for (unsigned int q=0; q<n_q_points; ++q)
          fe_eval.submit_value(one,q);
        fe_eval.integrate (true,false);
        fe_eval.distribute_local_to_global (inv_mass_matrix);
      }

    inv_mass_matrix.compress();
    for (unsigned int k=0; k<inv_mass_matrix.local_size(); ++k)
      if (inv_mass_matrix.local_element(k)>1e-15)
        inv_mass_matrix.local_element(k) = 1./inv_mass_matrix.local_element(k);
      else
        inv_mass_matrix.local_element(k) = 0;
  }

  // @sect4{SineGordonOperation::local_apply}

  // This operator implements the core operation of the program, the
  // integration over a range of cells for the nonlinear operator of the
  // Sine-Gordon problem. The implementation is based on the FEEvaluationGL
  // class since we are using the cell-based implementation for Gauss-Lobatto
  // elements.

  // The nonlinear function that we have to evaluate for the time stepping
  // routine includes the value of the function at the present time @p current
  // as well as the value at the previous time step @p old. Both values are
  // passed to the operator in the collection of source vectors @p src, which
  // is simply an STL vector of pointers to the actual solution vectors. This
  // construct of collecting several source vectors into one is necessary as
  // the cell loop in @p MatrixFree takes exactly one source and one
  // destination vector, even if we happen to use many vectors like the two in
  // this case. Note that the cell loop accepts any valid class for input and
  // output, which does not only include vectors but general data
  // types. However, only in case it encounters a
  // parallel::distributed::Vector<Number> or an STL vector collecting these
  // vectors, it calls functions that exchange data at the beginning and the
  // end of the loop. In the loop over the cells, we first have to read in the
  // values in the vectors related to the local values. Then, we evaluate the
  // value and the gradient of the current solution vector and the values of
  // the old vector at the quadrature points. Then, we combine the terms in
  // the scheme in the loop over the quadrature points. Finally, we integrate
  // the result against the test function and accumulate the result to the
  // global solution vector @p dst.
  template <int dim, int fe_degree>
  void SineGordonOperation<dim, fe_degree>::
  local_apply (const MatrixFree<dim>                      &data,
               parallel::distributed::Vector<double>      &dst,
               const std::vector<parallel::distributed::Vector<double>*> &src,
               const std::pair<unsigned int,unsigned int> &cell_range) const
  {
    AssertDimension (src.size(), 2);
    FEEvaluationGL<dim,fe_degree> current (data), old (data);
    for (unsigned int cell=cell_range.first; cell<cell_range.second; ++cell)
      {
        current.reinit (cell);
        old.reinit (cell);

        current.read_dof_values (*src[0]);
        old.read_dof_values     (*src[1]);

        current.evaluate (true, true, false);
        old.evaluate (true, false, false);

        for (unsigned int q=0; q<current.n_q_points; ++q)
          {
            const VectorizedArray<double> current_value = current.get_value(q);
            const VectorizedArray<double> old_value     = old.get_value(q);

            current.submit_value (2.*current_value - old_value -
                                  delta_t_sqr * std::sin(current_value),q);
            current.submit_gradient (- delta_t_sqr *
                                     current.get_gradient(q), q);
          }

        current.integrate (true,true);
        current.distribute_local_to_global (dst);
      }
  }

  //@sect4{SineGordonOperation::apply}

  // This function performs the time stepping routine based on the cell-local
  // strategy. First the destination vector is set to zero, then the cell-loop
  // is called, and finally the solution is multiplied by the inverse mass
  // matrix. The structure of the cell loop is implemented in the cell finite
  // element operator class. On each cell it applies the routine defined as
  // the local_apply() method of the class
  // SineGordonOperation, i.e., this. One could also
  // provide a function with the same signature that is not part of a class.
  template <int dim, int fe_degree>
  void SineGordonOperation<dim, fe_degree>::
  apply (parallel::distributed::Vector<double>                     &dst,
         const std::vector<parallel::distributed::Vector<double>*> &src) const
  {
    dst = 0;
    data.cell_loop (&SineGordonOperation<dim,fe_degree>::local_apply,
                    this, dst, src);
    dst.scale(inv_mass_matrix);
  }

  //@sect3{Equation data}

  // We define a time-dependent function that is used as initial
  // value. Different solutions can be obtained by varying the starting
  // time. This function has already been explained in step-25.
  template <int dim>
  class ExactSolution : public Function<dim>
  {
  public:
    ExactSolution (const unsigned int n_components = 1,
                   const double time = 0.) : Function<dim>(n_components, time) {}
    virtual double value (const Point<dim> &p,
                          const unsigned int component = 0) const;
  };

  template <int dim>
  double ExactSolution<dim>::value (const Point<dim> &p,
                                    const unsigned int /* component */) const
  {
    double t = this->get_time ();

    const double m = 0.5;
    const double c1 = 0.;
    const double c2 = 0.;
    const double factor = (m / std::sqrt(1.-m*m) *
                           std::sin(std::sqrt(1.-m*m)*t+c2));
    double result = 1.;
    for (unsigned int d=0; d<dim; ++d)
      result *= -4. * std::atan (factor / std::cosh(m*p[d]+c1));
    return result;
  }

  // @sect3{SineGordonProblem class}

  // This is the main class that builds on the class in step-25.  However, we
  // replaced the SparseMatrix<double> class by the MatrixFree class to store
  // the geometry data. Also, we use a distributed triangulation in this
  // example.
  template <int dim>
  class SineGordonProblem
  {
  public:
    SineGordonProblem ();
    void run ();

  private:
    ConditionalOStream pcout;

    void make_grid_and_dofs ();
    void oldstyle_operation ();
    void assemble_system ();
    void output_results (const unsigned int timestep_number) const;

#ifdef DEAL_II_USE_P4EST
    parallel::distributed::Triangulation<dim>   triangulation;
#else
    Triangulation<dim>   triangulation;
#endif
    FE_Q<dim>            fe;
    DoFHandler<dim>      dof_handler;
    ConstraintMatrix     constraints;
    IndexSet             locally_relevant_dofs;

    MatrixFree<dim,double> matrix_free_data;

    parallel::distributed::Vector<double> solution, old_solution, old_old_solution;

    const unsigned int n_global_refinements;
    double time, time_step;
    const double final_time;
    const double cfl_number;
    const unsigned int output_timestep_skip;
  };

  //@sect4{SineGordonProblem::SineGordonProblem}

  // This is the constructor of the SineGordonProblem class. The time interval
  // and time step size are defined here. Moreover, we use the degree of the
  // finite element that we defined at the top of the program to initialize a
  // FE_Q finite element based on Gauss-Lobatto support points. These points
  // are convenient because in conjunction with a QGaussLobatto quadrature
  // rule of the same order they give a diagonal mass matrix without
  // compromising accuracy too much (note that the integration is inexact,
  // though), see also the discussion in the introduction.
  template <int dim>
  SineGordonProblem<dim>::SineGordonProblem ()
    :
    pcout (std::cout,
           Utilities::System::get_this_mpi_process(MPI_COMM_WORLD)==0),
#ifdef DEAL_II_USE_P4EST
    triangulation (MPI_COMM_WORLD),
#endif
    fe (QGaussLobatto<1>(fe_degree+1)),
    dof_handler (triangulation),
    n_global_refinements (10-2*dim),
    time (-10),
    final_time (10),
    cfl_number (.1/fe_degree),
    output_timestep_skip (200)
  {}

  //@sect4{SineGordonProblem::make_grid_and_dofs}

  // As in step-25 this functions sets up a cube grid in dim
  // dimensions of extent $[-15,15]$. We refine the mesh more in the center of
  // the domain since the solution is concentrated there. We first refine all
  // cells whose center is within a radius of 11, and then refine once more
  // for a radius 6.  This is simple ad-hoc refinement could be done better by
  // adapting the mesh to the solution using error estimators during the time
  // stepping as done in other example programs, and using
  // parallel::distributed::SolutionTransfer to transfer the solution to the
  // new mesh.
  template <int dim>
  void SineGordonProblem<dim>::make_grid_and_dofs ()
  {
    GridGenerator::hyper_cube (triangulation, -15, 15);
    triangulation.refine_global (n_global_refinements);
    {
      typename Triangulation<dim>::active_cell_iterator
      cell = triangulation.begin_active(),
      end_cell = triangulation.end();
      for ( ; cell != end_cell; ++cell)
        if (cell->is_locally_owned())
          if (cell->center().norm() < 11)
            cell->set_refine_flag();
      triangulation.execute_coarsening_and_refinement();

      cell = triangulation.begin_active();
      end_cell = triangulation.end();
      for ( ; cell != end_cell; ++cell)
        if (cell->is_locally_owned())
          if (cell->center().norm() < 6)
            cell->set_refine_flag();
      triangulation.execute_coarsening_and_refinement();
    }

    pcout << "   Number of global active cells: "
#ifdef DEAL_II_USE_P4EST
          << triangulation.n_global_active_cells()
#else
          << triangulation.n_active_cells()
#endif
          << std::endl;

    dof_handler.distribute_dofs (fe);

    pcout << "   Number of degrees of freedom: "
          << dof_handler.n_dofs()
          << std::endl;

    // We generate hanging node constraints for ensuring continuity of the
    // solution. As in step-40, we need to equip the constraint matrix with
    // the IndexSet of locally relevant degrees of freedom to avoid it to
    // consume too much memory for big problems. Next, the  MatrixFree
    //  for the problem is set up. Note that we specify the MPI
    // communicator which we are going to use, and that we also want to use
    // shared-memory parallelization (hence one would use multithreading for
    // intra-node parallelism and not MPI; note that we here choose the
    // standard option — if we wanted to disable shared memory
    // parallelization, we would choose @p none). Finally, three solution
    // vectors are initialized. MatrixFree stores the layout that is to be
    // used by distributed vectors, so we just ask it to initialize the
    // vectors.
    DoFTools::extract_locally_relevant_dofs (dof_handler,
                                             locally_relevant_dofs);
    constraints.clear();
    constraints.reinit (locally_relevant_dofs);
    DoFTools::make_hanging_node_constraints (dof_handler, constraints);
    constraints.close();

    QGaussLobatto<1> quadrature (fe_degree+1);
    typename MatrixFree<dim>::AdditionalData additional_data;
    additional_data.mpi_communicator = MPI_COMM_WORLD;
    additional_data.tasks_parallel_scheme =
      MatrixFree<dim>::AdditionalData::partition_partition;

    matrix_free_data.reinit (dof_handler, constraints,
                             quadrature, additional_data);

    matrix_free_data.initialize_dof_vector (solution);
    old_solution.reinit (solution);
    old_old_solution.reinit (solution);
  }

  //@sect4{SineGordonProblem::output_results}

  // This function prints the norm of the solution and writes the solution
  // vector to a file. The norm is standard (except for the fact that we need
  // to be sure to only count norms on locally owned cells), and the second is
  // similar to what we did in step-40. However, we first need to generate an
  // appropriate vector for output: The ones we used during time stepping
  // contained information about ghosts dofs that one needs write access to
  // during the loops over cell. However, that is not the same as needed when
  // outputting. So we first initialize a vector with locally relevant degrees
  // of freedom by copying the solution (note how we use the function @p
  // copy_from to transfer data between vectors with the same local range, but
  // different layouts of ghosts). Then, we import the values on the ghost
  // DoFs and then distribute the constraints (as constraints are zero in the
  // vectors during loop over all cells).
  template <int dim>
  void
  SineGordonProblem<dim>::output_results (const unsigned int timestep_number) const
  {
    parallel::distributed::Vector<double> locally_relevant_solution;
    locally_relevant_solution.reinit (dof_handler.locally_owned_dofs(),
                                      locally_relevant_dofs,
                                      MPI_COMM_WORLD);
    locally_relevant_solution.copy_from (solution);
    locally_relevant_solution.update_ghost_values ();
    constraints.distribute (locally_relevant_solution);

    Vector<float> norm_per_cell (triangulation.n_active_cells());
    VectorTools::integrate_difference (dof_handler,
                                       locally_relevant_solution,
                                       ZeroFunction<dim>(),
                                       norm_per_cell,
                                       QGauss<dim>(fe_degree+1),
                                       VectorTools::L2_norm);
    const double solution_norm =
      std::sqrt(Utilities::MPI::sum (norm_per_cell.norm_sqr(), MPI_COMM_WORLD));

    pcout << "   Time:"
          << std::setw(8) << std::setprecision(3) << time
          << ", solution norm: "
          << std::setprecision(5) << std::setw(7) << solution_norm
          << std::endl;

    DataOut<dim> data_out;

    data_out.attach_dof_handler (dof_handler);
    data_out.add_data_vector (locally_relevant_solution, "solution");
    data_out.build_patches ();

    const std::string filename =
      "solution-" + Utilities::int_to_string (timestep_number, 3) +
      "." + Utilities::int_to_string (Utilities::MPI::
                                      this_mpi_process(MPI_COMM_WORLD),4);

    std::ofstream output ((filename + ".vtu").c_str());
    data_out.write_vtu (output);

    if (Utilities::MPI::this_mpi_process(MPI_COMM_WORLD) == 0)
      {
        std::vector<std::string> filenames;
        for (unsigned int i=0;
             i<Utilities::MPI::n_mpi_processes (MPI_COMM_WORLD); ++i)
          filenames.push_back ("solution-" +
                               Utilities::int_to_string (timestep_number, 3) +
                               "." +
                               Utilities::int_to_string (i, 4) +
                               ".vtu");

        std::ofstream master_output ((filename + ".pvtu").c_str());
        data_out.write_pvtu_record (master_output, filenames);
      }
  }

  // @sect4{SineGordonProblem::run}

  // This function is called by the main function and calls the subroutines of
  // the class.
  //
  // The first step is to set up the grid and the cell operator. Then, the
  // time step is computed from the CFL number given in the constructor and
  // the finest mesh size. The finest mesh size is computed as the diameter of
  // the last cell in the triangulation, which is the last cell on the finest
  // level of the mesh. This is only possible for Cartesian meshes, otherwise,
  // one needs to loop over all cells). Note that we need to query all the
  // processors for their finest cell since the not all processors might hold
  // a region where the mesh is at the finest level. Then, we readjust the
  // time step a little to hit the final time exactly if necessary.
  template <int dim>
  void
  SineGordonProblem<dim>::run ()
  {
    make_grid_and_dofs();

    const double local_min_cell_diameter =
      triangulation.last()->diameter()/std::sqrt(dim);
    const double global_min_cell_diameter
      = -Utilities::MPI::max(-local_min_cell_diameter, MPI_COMM_WORLD);
    time_step = cfl_number * global_min_cell_diameter;
    time_step = (final_time-time)/(int((final_time-time)/time_step));
    pcout << "   Time step size: " << time_step << ", finest cell: "
          << global_min_cell_diameter << std::endl << std::endl;

    // Next the initial value is set. Since we have a two-step time stepping
    // method, we also need a value of the solution at time-time_step. For
    // accurate results, one would need to compute this from the time
    // derivative of the solution at initial time, but here we ignore this
    // difficulty and just set it to the initial value function at that
    // artificial time.

    // We create an output of the initial value. Then we also need to collect
    // the two starting solutions in an STL vector of pointers field and to
    // set up an instance of the  SineGordonOperation class
    // based on the finite element degree specified at the top of this file.
    VectorTools::interpolate (dof_handler,
                              ExactSolution<dim> (1, time),
                              solution);
    VectorTools::interpolate (dof_handler,
                              ExactSolution<dim> (1, time-time_step),
                              old_solution);
    output_results (0);

    std::vector<parallel::distributed::Vector<double>*> previous_solutions;
    previous_solutions.push_back(&old_solution);
    previous_solutions.push_back(&old_old_solution);

    SineGordonOperation<dim,fe_degree> sine_gordon_op (matrix_free_data,
                                                       time_step);

    // Now loop over the time steps. In each iteration, we shift the solution
    // vectors by one and call the  apply  function of the
    // SineGordonOperator . Then, we write the solution to a file. We
    // clock the wall times for the computational time needed as wall as the
    // time needed to create the output and report the numbers when the time
    // stepping is finished.
    //
    // Note how this shift is implemented: We simply call the swap method on
    // the two vectors which swaps only some pointers without the need to copy
    // data around. Obviously, this is a more efficient way to move data
    // around. Let us see what happens in more detail: First, we exchange
    // old_solution with old_old_solution, which
    // means that old_old_solution gets
    // old_solution, which is what we expect. Similarly,
    // old_solution gets the content from solution
    // in the next step. Afterward, solution holds
    // old_old_solution, but that will be overwritten during this
    // step.
    unsigned int timestep_number = 1;

    Timer timer;
    double wtime = 0;
    double output_time = 0;
    for (time+=time_step; time<=final_time; time+=time_step, ++timestep_number)
      {
        timer.restart();
        old_old_solution.swap (old_solution);
        old_solution.swap (solution);
        sine_gordon_op.apply (solution, previous_solutions);
        wtime += timer.wall_time();

        timer.restart();
        if (timestep_number % output_timestep_skip == 0)
          output_results(timestep_number / output_timestep_skip);

        output_time += timer.wall_time();
      }
    timer.restart();
    output_results(timestep_number / output_timestep_skip + 1);
    output_time += timer.wall_time();

    pcout << std::endl
          << "   Performed " << timestep_number << " time steps."
          << std::endl;

    pcout << "   Average wallclock time per time step: "
          << wtime / timestep_number << "s" << std::endl;

    pcout << "   Spent " << output_time << "s on output and "
          << wtime << "s on computations." << std::endl;
  }
}

// @sect3{The main function}

// This is as in all other programs:
int main (int argc, char **argv)
{
  using namespace Step48;
  using namespace dealii;

  Utilities::System::MPI_InitFinalize mpi_initialization(argc, argv);

  try
    {
      deallog.depth_console (0);

      SineGordonProblem<dimension> sg_problem;
      sg_problem.run ();
    }
  catch (std::exception &exc)
    {
      std::cerr << std::endl << std::endl
                << "----------------------------------------------------"
                << std::endl;
      std::cerr << "Exception on processing: " << std::endl
                << exc.what() << std::endl
                << "Aborting!" << std::endl
                << "----------------------------------------------------"
                << std::endl;

      return 1;
    }
  catch (...)
    {
      std::cerr << std::endl << std::endl
                << "----------------------------------------------------"
                << std::endl;
      std::cerr << "Unknown exception!" << std::endl
                << "Aborting!" << std::endl
                << "----------------------------------------------------"
                << std::endl;
      return 1;
    }

  return 0;
}


/*
 * Copyright (c) 2016, Markus Achtelik, ASL, ETH Zurich, Switzerland
 * Copyright (c) 2016, Michael Burri, ASL, ETH Zurich, Switzerland
 * Copyright (c) 2016, Helen Oleynikova, ASL, ETH Zurich, Switzerland
 * Copyright (c) 2016, Rik Bähnemann, ASL, ETH Zurich, Switzerland
 * Copyright (c) 2016, Marija Popovic, ASL, ETH Zurich, Switzerland
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include "mav_trajectory_generation_ros/feasibility_recursive.h"

#include <algorithm>
#include <cmath>

#include <Eigen/Core>

#include <mav_trajectory_generation/motion_defines.h>

namespace mav_trajectory_generation {
FeasibilityRecursive::Settings::Settings() : min_section_time_s_(0.05) {}

FeasibilityRecursive::FeasibilityRecursive(const Settings& settings)
    : FeasibilityBase(), settings_(settings) {}
FeasibilityRecursive::FeasibilityRecursive(
    const InputConstraints& input_constraints)
    : FeasibilityBase(input_constraints) {}
FeasibilityRecursive::FeasibilityRecursive(
    const Settings& settings, const InputConstraints& input_constraints)
    : FeasibilityBase(input_constraints), settings_(settings) {}

InputFeasibilityResult FeasibilityRecursive::checkInputFeasibility(
    const Segment& segment) const {
  // Check user input.
  if (!(segment.D() == 3 || segment.D() == 4)) {
    return InputFeasibilityResult::kInputIndeterminable;
  }

  // Find roots to determine single axis minima / maxima:
  Roots roots_acc, roots_jerk, roots_snap;
  if (input_constraints_.hasConstraint(InputConstraintType::kVMax)) {
    roots_acc.resize(3);
    for (size_t i = 0; i < 3; i++) {
      if (!segment[i].getRoots(derivative_order::ACCELERATION,
              &roots_acc[i])) {
        return InputFeasibilityResult::kInputIndeterminable;
      }
    }
  }

  if (input_constraints_.hasConstraint(InputConstraintType::kFMin) ||
      input_constraints_.hasConstraint(InputConstraintType::kFMax) ||
      input_constraints_.hasConstraint(InputConstraintType::kOmegaXYMax)) {
    roots_jerk.resize(3);
    for (size_t i = 0; i < 3; i++) {
      if (!segment[i].getRoots(derivative_order::JERK,
              &roots_jerk[i])) {
        return InputFeasibilityResult::kInputIndeterminable;
      }
    }
  }

  if (input_constraints_.hasConstraint(InputConstraintType::kOmegaXYMax)) {
    roots_snap.resize(3);
    for (size_t i = 0; i < 3; i++) {
      if (!segment[i].getRoots(derivative_order::SNAP,
              &roots_snap[i])) {
        return InputFeasibilityResult::kInputIndeterminable;
      }
    }
  }

  // Recursive test for velocity, acceleration, and roll and pitch rate
  // feasibility.
  double t_1 = 0.0;
  double t_2 = segment.getTime();
  InputFeasibilityResult result = recursiveFeasibility(
      segment, roots_acc, roots_jerk, roots_snap, t_1, t_2);
  if (result != InputFeasibilityResult::kInputFeasible) {
    return result;
  }

  if (segment.D() == 4) {
    // Yaw feasibility (assumed independent of translation in the rigid body
    // model).
    // Check the single axis minimum / maximum yaw rate:
    double yaw_rate_limit;
    if (input_constraints_.getConstraint(InputConstraintType::kOmegaZMax,
                                         &yaw_rate_limit)) {
      std::pair<double, double> yaw_rate_min, yaw_rate_max;
      if (!segment[3].computeMinMax(t_1, t_2,
                                    derivative_order::ANGULAR_VELOCITY,
                                    &yaw_rate_min, &yaw_rate_max)) {
        return InputFeasibilityResult::kInputIndeterminable;
      }
      if (std::max(std::abs(yaw_rate_min.second),
                   std::abs(yaw_rate_max.second)) > yaw_rate_limit) {
        return InputFeasibilityResult::kInputInfeasibleYawRates;
      }
    }

    // Check the single axis minimum / maximum yaw acceleration:
    double yaw_acc_limit;
    if (input_constraints_.getConstraint(InputConstraintType::kOmegaZDotMax,
                                         &yaw_acc_limit)) {
      std::pair<double, double> yaw_acc_min, yaw_acc_max;
      if (!segment[3].computeMinMax(t_1, t_2,
                                    derivative_order::ANGULAR_ACCELERATION,
                                    &yaw_acc_min, &yaw_acc_max)) {
        return InputFeasibilityResult::kInputIndeterminable;
      }
      if (std::max(std::abs(yaw_acc_min.second), std::abs(yaw_acc_max.second)) >
          yaw_acc_limit) {
        return InputFeasibilityResult::kInputInfeasibleYawAcc;
      }
    }
  }

  // Segment definitely feasible.
  return InputFeasibilityResult::kInputFeasible;
}

InputFeasibilityResult FeasibilityRecursive::recursiveFeasibility(
    const Segment& segment, const Roots& roots_acc, const Roots& roots_jerk,
    const Roots& roots_snap, double t_1, double t_2) const {
  if (t_2 - t_1 < settings_.getMinSectionTimeS()) {
    return InputFeasibilityResult::kInputIndeterminable;
  }
  // Evaluate the thrust at the boundaries of the section:
  if (input_constraints_.hasConstraint(InputConstraintType::kFMin) ||
      input_constraints_.hasConstraint(InputConstraintType::kFMax)) {
    const double f_t_1 = evaluateThrust(segment, t_1);
    const double f_t_2 = evaluateThrust(segment, t_2);
    double f_min_limit, f_max_limit;
    if (input_constraints_.getConstraint(InputConstraintType::kFMin,
                                         &f_min_limit) &&
        std::min(f_t_1, f_t_2) < f_min_limit) {
      return InputFeasibilityResult::kInputInfeasibleThrustLow;
    }
    if (input_constraints_.getConstraint(InputConstraintType::kFMax,
                                         &f_max_limit) &&
        std::max(f_t_1, f_t_2) > f_max_limit) {
      return InputFeasibilityResult::kInputInfeasibleThrustHigh;
    }
  }

  // Evaluate the velocity at the boundaries of the section:
  double v_max_limit;
  if (input_constraints_.getConstraint(InputConstraintType::kVMax,
                                       &v_max_limit)) {
    const double v_t_1 =
        segment.evaluate(t_1, derivative_order::VELOCITY).head<3>().norm();
    const double v_t_2 =
        segment.evaluate(t_2, derivative_order::VELOCITY).head<3>().norm();
    if (std::max(v_t_1, v_t_2) > v_max_limit) {
      return InputFeasibilityResult::kInputInfeasibleVelocity;
    }
  }

  // Upper and lower bounds on thrust, velocity and jerk for this section.
  double f_min_sqr = 0.0;
  double f_max_sqr = 0.0;
  double v_max_sqr = 0.0;
  double j_max_sqr = 0.0;

  // Compute upper bound for velocity.
  if (input_constraints_.hasConstraint(InputConstraintType::kVMax)) {
    for (size_t i = 0; i < 3; i++) {
      std::pair<double, double> v_min, v_max;
      segment[i].selectMinMaxFromRoots(t_1, t_2, derivative_order::VELOCITY,
                                       roots_acc[i], &v_min, &v_max);
      // Definitly infeasible:
      // The velocity on a single axis is higher than the allowed total
      // velocity.
      if (std::max(std::pow(v_min.second, 2), std::pow(v_max.second, 2)) >
          std::pow(v_max_limit, 2)) {
        return InputFeasibilityResult::kInputInfeasibleVelocity;
      }
      // Add single axis extrema to upper bound on squared velocity.
      v_max_sqr +=
          std::pow(std::max(std::abs(v_min.second), std::abs(v_max.second)), 2);
    }
  }

  // Compute upper and lower bound on thrust.
  if (input_constraints_.hasConstraint(InputConstraintType::kFMin) ||
      input_constraints_.hasConstraint(InputConstraintType::kFMax) ||
      input_constraints_.hasConstraint(InputConstraintType::kOmegaXYMax)) {
    for (size_t i = 0; i < 3; i++) {
      std::pair<double, double> a_min, a_max;
      segment[i].selectMinMaxFromRoots(t_1, t_2, derivative_order::ACCELERATION,
                                       roots_jerk[i], &a_min, &a_max);
      // Distance from zero thrust point in this axis.
      const double f_i_min = a_min.second + gravity_[i];
      const double f_i_max = a_max.second + gravity_[i];

      // Definitly infeasible:
      // The thrust on a single axis is higher than the allowed total thrust.
      double f_max_limit;
      if (input_constraints_.getConstraint(InputConstraintType::kFMax,
                                           &f_max_limit) &&
          std::max(std::abs(f_i_min), std::abs(f_i_max)) > f_max_limit) {
        return InputFeasibilityResult::kInputInfeasibleThrustHigh;
      }

      // Add single axis extrema to lower bound on squared acceleration.
      if (f_i_min * f_i_max < 0.0) {
        // Sign of acceleration changes. The minimum squared value is zero.
        f_min_sqr += 0.0;
      } else {
        f_min_sqr +=
            std::pow(std::min(std::abs(f_i_min), std::abs(f_i_max)), 2);
      }

      // Add single axis extrema to upper bound on squared acceleration.
      f_max_sqr += std::pow(std::max(std::abs(f_i_min), std::abs(f_i_max)), 2);
    }
  }

  // Compute upper limit on jerk.
  if (input_constraints_.hasConstraint(InputConstraintType::kOmegaXYMax)) {
    for (size_t i = 0; i < 3; i++) {
      // Find the minimum / maximum of each axis.
      std::pair<double, double> j_min, j_max;
      segment[i].selectMinMaxFromRoots(t_1, t_2, derivative_order::JERK,
                                       roots_snap[i], &j_min, &j_max);
      // Add single axis extrema to upper bound on squared velocity,
      // acceleration, and jerk.
      j_max_sqr +=
          std::pow(std::max(std::abs(j_min.second), std::abs(j_max.second)), 2);
    }
  }

  double f_lower_bound = std::sqrt(f_min_sqr);
  double f_upper_bound = std::sqrt(f_max_sqr);
  double v_upper_bound = std::sqrt(v_max_sqr);
  double omega_xy_upper_bound;
  // Divide-by-zero protection.
  if (f_min_sqr > 1.0e-6) {
    omega_xy_upper_bound = std::sqrt(j_max_sqr / f_min_sqr);
  } else {
    omega_xy_upper_bound = std::numeric_limits<double>::max();
  }

  // Definitely infeasible:
  double f_min_limit;
  if (input_constraints_.getConstraint(InputConstraintType::kFMin,
                                       &f_min_limit) &&
      f_upper_bound < f_min_limit) {
    return InputFeasibilityResult::kInputInfeasibleThrustLow;
  }

  double f_max_limit;
  if (input_constraints_.getConstraint(InputConstraintType::kFMax,
                                       &f_max_limit) &&
      f_lower_bound > f_max_limit) {
    return InputFeasibilityResult::kInputInfeasibleThrustHigh;
  }

  // Possible infeasible (one of the bounds is exceeding the limits):
  double omega_xy_limit;
  if ((input_constraints_.hasConstraint(InputConstraintType::kFMin) &&
       f_lower_bound < f_min_limit) ||
      (input_constraints_.hasConstraint(InputConstraintType::kFMax) &&
       f_upper_bound > f_max_limit) ||
      (input_constraints_.hasConstraint(InputConstraintType::kVMax) &&
       v_upper_bound > v_max_limit) ||
      (input_constraints_.getConstraint(InputConstraintType::kOmegaXYMax,
                                        &omega_xy_limit) &&
       omega_xy_upper_bound > omega_xy_limit)) {
    // Indeterminate. Must check more closely:
    double t_half = (t_1 + t_2) / 2;
    InputFeasibilityResult result_1 = recursiveFeasibility(
        segment, roots_acc, roots_jerk, roots_snap, t_1, t_half);

    if (result_1 == InputFeasibilityResult::kInputFeasible) {
      // Continue with second half.
      return recursiveFeasibility(segment, roots_acc, roots_jerk, roots_snap,
                                  t_half, t_2);
    } else {
      // First half is already infeasible or inderterminate:
      return result_1;
    }
  }
  // Definitely feasible:
  return InputFeasibilityResult::kInputFeasible;
}

double FeasibilityRecursive::evaluateThrust(const Segment& segment,
                                            double time) const {
  return (segment.evaluate(time, derivative_order::ACCELERATION).head<3>() +
          gravity_)
      .norm();
}
}  // namespace mav_tube_trajectory_generation


#include "biggles/kalman_filter.hpp"
#include "biggles/observation.hpp"
#include "biggles/partition.hpp"
#include "biggles/samplers.hpp"
#include "biggles/simulate.hpp"
#include "biggles/track.hpp"
#include "biggles/server/engine.hpp"
#include "biggles/server/stepper.hpp"
#include <boost/foreach.hpp>
#include <boost/random.hpp>
#include <boost/timer.hpp>
#include <boost/tuple/tuple_io.hpp>
#include <cmath>
#include <ctime>
#include <inttypes.h>
#include <iomanip>
#include <iostream>
#include <fstream>
#include <stdlib.h>
#include <utility>
#include <vector>

// include this last to stop pre-processor macros breaking things
extern "C" {
#include <ccan/tap/tap.h>
}

static boost::mt19937 random_generator(std::time(0));

using namespace biggles;

void test_engine(const model::parameters& params)
{
    partition_ptr_t partition_ptr;

    // simulate full data
    simulate::generate_partition(partition_ptr, params, 0, 128 + 128, 128.f, 128.f);

    diag("Generated %zu new tracks.", partition_ptr->tracks().size());
    diag("Generated %zu clutter observations.", partition_ptr->clutter().size());

    partition_ptr_t dataset_part_ptr;
    simulate::demote_all_tracks_from_partition(dataset_part_ptr, partition_ptr);

    server::engine worker;
    worker.set_initial_conditions(params, dataset_part_ptr);
    worker.start();
    diag("Started");
    boost::timer timer;
    while (timer.elapsed()<1) {}
    diag("Time over");
    worker.stop();
    diag("Stopped");

    ok1(true);
}

void test_stepper(const model::parameters& params) {
    /*
    partition_ptr_t partition_ptr;

    // simulate full data
    simulate::generate_partition(partition_ptr, params, 0, 128 + 128, 128.f, 128.f);

    diag("Generated %zu new tracks.", partition_ptr->tracks().size());
    diag("Generated %zu clutter observations.", partition_ptr->clutter().size());
    */

    boost::shared_ptr<track_collection> tracks(new track_collection());
    clutter_ptr clutter(new clutter_t());
    partition_ptr_t partition_ptr(new partition(tracks, clutter));

    /*
    partition_ptr_t dataset_part_ptr;
    simulate::demote_all_tracks_from_partition(dataset_part_ptr, partition_ptr);

    server::stepper dancer(params, dataset_part_ptr);
    */
    diag("OK");
    server::stepper dancer(params, partition_ptr);
    /*
    boost::shared_ptr<server::tracking_state> state = dancer.tracking_state_ptr();
    diag("current_partition %zu", state->current_partition.use_count());
    diag("best_partition %zu", state->best_partition.use_count());
    diag("last_proposed_partition %zu", state->last_proposed_partition.use_count());
    */
    /*
    dancer.step(1);
    state = dancer.tracking_state_ptr();
    diag("current_partition %zu", state->current_partition.use_count());
    diag("best_partition %zu", state->best_partition.use_count());
    diag("last_proposed_partition %zu", state->last_proposed_partition.use_count());
    */
}

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

    float sigma_R = 0.1f;

    // initialise model parameters
    model::parameters params;
    model::mean_new_tracks_per_frame(params)           = 0.1f;
    model::mean_false_observations_per_frame(params)   = 0.05f;
    model::frame_to_frame_survival_probability(params) = 0.95f;
    model::generate_observation_probability(params)    = 0.8f;
    model::observation_error_covariance(params)       << sigma_R * sigma_R, 0, 0, sigma_R * sigma_R;

    //test_engine(params);
    test_stepper(params);
    return exit_status();
}


//==============================================================================
//         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 BOOST_SIMD_TOOLBOX_SWAR_FUNCTIONS_SIMD_SSE_SSE2_SPLATTED_MINIMUM_HPP_INCLUDED
#define BOOST_SIMD_TOOLBOX_SWAR_FUNCTIONS_SIMD_SSE_SSE2_SPLATTED_MINIMUM_HPP_INCLUDED
#ifdef BOOST_SIMD_HAS_SSE2_SUPPORT

#include <boost/simd/toolbox/swar/functions/splatted_minimum.hpp>
#include <boost/simd/include/functions/simd/min.hpp>
#include <boost/simd/include/functions/simd/minimum.hpp>
#include <boost/simd/include/functions/simd/splat.hpp>
#include <boost/simd/sdk/meta/make_dependent.hpp>
#include <boost/dispatch/meta/as_floating.hpp>

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is int16_
/////////////////////////////////////////////////////////////////////////////
namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::splatted_minimum_
				   , boost::simd::tag::sse2_
                                   , (A0)
                                   , ((simd_<ints16_<A0>,boost::simd::tag::sse_>))
                                   )
  {
    typedef A0 result_type;

    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      typedef typename meta::make_dependent<float, A0>::type float_t;
      typedef simd::native<float_t, boost::simd::tag::sse_> ftype;
      A0 min1 = _mm_shufflehi_epi16(a0  , _MM_SHUFFLE(1, 0, 3, 2));
         min1 = _mm_shufflelo_epi16(min1, _MM_SHUFFLE(1, 0, 3, 2));
         min1 = min(a0, min1);
      A0 min2 = _mm_shuffle_epi32  (min1, _MM_SHUFFLE(1, 0, 3, 2));
         min2 = _mm_shufflelo_epi16(min2, _MM_SHUFFLE(1, 0, 3, 2));
         min2 = min(min1, min2);
      A0 min3 = _mm_shuffle_epi32(min2, _MM_SHUFFLE(3, 2, 1, 0));
      A0 min4 = _mm_shufflelo_epi16(min3, _MM_SHUFFLE(0, 1, 2, 3));
      ftype min5 = simd::bitwise_cast<ftype>(boost::simd::min(min3, min4));
      return simd::bitwise_cast<A0>(_mm_unpacklo_ps(min5, min5));
    }
  };

  /////////////////////////////////////////////////////////////////////////////
  // Implementation when type A0 is double
  /////////////////////////////////////////////////////////////////////////////
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::splatted_minimum_
				   , boost::simd::tag::sse2_
				   , (A0)
				   , ((simd_<double_<A0>,boost::simd::tag::sse_>))
				   )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL(1)
      {
        A0 that = _mm_min_sd(a0, _mm_unpackhi_pd(a0,a0));
        return simd::bitwise_cast<A0>(_mm_unpacklo_pd(that, that));
      }
  };

  /////////////////////////////////////////////////////////////////////////////
  // Implementation when type A0 is int64_
  /////////////////////////////////////////////////////////////////////////////
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::splatted_minimum_
				   , boost::simd::tag::sse2_
				   , (A0)
				   , ((simd_<ints64_<A0>,boost::simd::tag::sse_>))
				   )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL(1)
      {
        typedef typename dispatch::meta::as_floating<A0>::type ftype;
        A0 a00  =  simd::bitwise_cast<A0>(_mm_shuffle_pd(simd::bitwise_cast<ftype>(a0),
                                                        simd::bitwise_cast<ftype>(a0),0x01));
        return  min(a0, a00);
      }
  };

  /////////////////////////////////////////////////////////////////////////////
  // Implementation when type A0 is float
  /////////////////////////////////////////////////////////////////////////////
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::splatted_minimum_
                                   , boost::simd::tag::sse2_
				   , (A0)
				   , ((simd_<single_<A0>,boost::simd::tag::sse_>))
                                   )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL(1)
      {
        return boost::simd::splat<A0>(minimum(a0));
        //       A0 min1 = {min(a0,simd::bitwise_cast<A0>(_mm_shuffle_ps(a0, a0, _MM_SHUFFLE(1, 0, 3, 2))))};
        //       A0 that = {min(min1, simd::bitwise_cast<A0>(_mm_shuffle_ps(min1, min1, _MM_SHUFFLE(2, 3, 0, 1))))};
        //       return that;
      }
  };

  /////////////////////////////////////////////////////////////////////////////
  // Implementation when type A0 is int8_
  /////////////////////////////////////////////////////////////////////////////
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::splatted_minimum_
                                   , boost::simd::tag::sse2_
				   , (A0)
				   , ((simd_<ints8_<A0>,boost::simd::tag::sse_>))
                                   )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL(1)
      {
        return boost::simd::splat<A0>(minimum(a0));
      }
  };

  /////////////////////////////////////////////////////////////////////////////
  // Implementation when type A0 is int32_
  /////////////////////////////////////////////////////////////////////////////
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::splatted_minimum_
				   , boost::simd::tag::sse2_
				   , (A0)
				   , ((simd_<ints32_<A0>,boost::simd::tag::sse_>))
                                   )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL(1)
      {
        A0 min1 = min(a0,simd::bitwise_cast<A0>(_mm_shuffle_epi32(a0, _MM_SHUFFLE(1, 0, 3, 2))));
        return min(min1, simd::bitwise_cast<A0>(_mm_shuffle_epi32(min1, _MM_SHUFFLE(2, 3, 0, 1))));
      }
  };
} } }

#endif
#endif


#ifndef COMPUTED_TORQUE_CONTROLLERS_TASK_SPACE_CONTROLLER_CORE_HPP
#define COMPUTED_TORQUE_CONTROLLERS_TASK_SPACE_CONTROLLER_CORE_HPP

#include <map>
#include <string>

#include <computed_torque_controllers/common_namespaces.hpp>
#include <computed_torque_controllers/joint_controller_core.hpp>
#include <control_toolbox/pid.h>
#include <hardware_interface/robot_hw.h>
#include <ros/console.h>
#include <ros/duration.h>
#include <ros/names.h>
#include <ros/node_handle.h>
#include <ros/param.h>
#include <ros/time.h>

#include <Eigen/Core>

#include <boost/foreach.hpp>

namespace computed_torque_controllers {

// ===========================================================================================
// core control implementation without command subscription
//   [ input] position and velocity setpoints of each controlled joint & states of all joints
//   [output] effort commands to controlled joints
class TaskSpaceControllerCore : protected JointControllerCore {
protected:
  typedef std::map< std::string, ct::Pid > PidMap;

public:
  TaskSpaceControllerCore() {}

  virtual ~TaskSpaceControllerCore() {}

  bool init(hi::RobotHW *const hw, ros::NodeHandle &param_nh) {
    return JointControllerCore::init(hw, param_nh) && initDofs(param_nh);
  }

  void starting() {
    JointControllerCore::starting();

    // reset PIDs
    BOOST_FOREACH (PidMap::value_type &pid, pids_) { pid.second.reset(); }
  }

  void update(const ros::Duration &period, const std::map< std::string, double > &pos_setpoints,
              const std::map< std::string, double > &vel_setpoints) {
    const std::map< std::string, double > joint_setpoints(
        updateDofs(period, pos_setpoints, vel_setpoints));

    JointControllerCore::update(period,
                                /* pos_joint_setpoints = */ std::map< std::string, double >(),
                                /* vel_joint_setpoints = */ joint_setpoints);
  }

  void stopping() { JointControllerCore::stopping(); }

protected:
  bool initDofs(ros::NodeHandle &param_nh) {
    namespace rn = ros::names;
    namespace rp = ros::param;

    // TODO: load the end link name & offset from params
    model_end_link_ = model_->getBodyNode(model_->getNumBodyNodes() - 1);
    end_link_offset_ = Eigen::Vector3d::Zero();

    // load PIDs if gains are specified. this is an optional step
    // because PIDs are not required unless position setpoints are given.
    const std::string linear_ns(param_nh.resolveName(rn::append("task_space", "linear")));
    if (rp::has(rn::append(linear_ns, "p"))) {
      if (!pids_["linear_x"].initParam(linear_ns) || !pids_["linear_y"].initParam(linear_ns) ||
          !pids_["linear_z"].initParam(linear_ns)) {
        ROS_ERROR_STREAM("TaskSpaceControllerCore::initDofs(): Failed to init a PID by the param '"
                         << linear_ns << "'");
        return false;
      }
    }
    const std::string angular_ns(param_nh.resolveName(rn::append("task_space", "angular")));
    if (rp::has(rn::append(angular_ns, "p"))) {
      if (!pids_["angular_x"].initParam(angular_ns) || !pids_["angular_y"].initParam(angular_ns) ||
          !pids_["angular_z"].initParam(angular_ns)) {
        ROS_ERROR_STREAM("TaskSpaceControllerCore::initDofs(): Failed to init a PID by the param '"
                         << angular_ns << "'");
        return false;
      }
    }

    return true;
  }

  std::map< std::string, double > updateDofs(const ros::Duration &period,
                                             const std::map< std::string, double > &pos_setpoints,
                                             const std::map< std::string, double > &vel_setpoints) {
    // get present position of the end effector
    Eigen::Vector6d p;
    {
      const Eigen::Isometry3d T(model_end_link_->getWorldTransform());
      const Eigen::AngleAxisd aa(T.linear());
      p.head< 3 >() = aa.angle() * aa.axis();
      p.tail< 3 >() = T * end_link_offset_;
    }

    // set reference velocity in task space by integrating position & velocity setpoints
    // according to 'v_r = v_sp + PID(p_sp - p)'
    Eigen::Vector6d v_r;
    v_r[0] = integrateSetpoints(period, "angular_x", vel_setpoints, pos_setpoints, p[0]);
    v_r[1] = integrateSetpoints(period, "angular_y", vel_setpoints, pos_setpoints, p[1]);
    v_r[2] = integrateSetpoints(period, "angular_z", vel_setpoints, pos_setpoints, p[2]);
    v_r[3] = integrateSetpoints(period, "linear_x", vel_setpoints, pos_setpoints, p[3]);
    v_r[4] = integrateSetpoints(period, "linear_y", vel_setpoints, pos_setpoints, p[4]);
    v_r[5] = integrateSetpoints(period, "linear_z", vel_setpoints, pos_setpoints, p[5]);

    // compute Jacobian and its pseudo-inverse
    const dm::Jacobian J(model_end_link_->getWorldJacobian(end_link_offset_));
    const Eigen::MatrixXd pinv_J(
        J.transpose() * (J * J.transpose() + 0.0025 * Eigen::Matrix6d::Identity()).inverse());

    // convert reference velocity from task to joint spaces
    std::map< std::string, double > ctl_joint_setpoints;
    BOOST_FOREACH (const ControlledHardwareJointMap::value_type &joint_val, ctl_hw_joints_) {
      const std::string &name(joint_val.first);
      const ControlledHardwareJoint &joint(*joint_val.second);
      const std::size_t id(joint.id_in_model);

      ctl_joint_setpoints[name] = pinv_J.row(id) * v_r /* + opt function */;
    }

    return ctl_joint_setpoints;
  }

  // make new velocity setpoint by integrating the original velocity & position setpoints.
  // the original setpoints may be missing.
  double integrateSetpoints(const ros::Duration &period, const std::string &dof_name,
                            const std::map< std::string, double > &vel_setpoints,
                            const std::map< std::string, double > &pos_setpoints,
                            const double pos) {
    // find original setpoints
    const double *const pos_sp(findValue(pos_setpoints, dof_name));
    const double *const vel_sp(findValue(vel_setpoints, dof_name));
    ct::Pid *const pid(findValue(pids_, dof_name));

    // print error if position setpoint cannot be integrated
    if (pos_sp && !pid) {
      ROS_ERROR_STREAM(
          "TaskSpaceControllerCore::integrateSetpoints(): No pid controller found for Dof '"
          << dof_name << "'. Will ignore position setpoint in the task space.");
    }

    // return 'vel_sp + PID(pos_sp - pos)'
    return (vel_sp ? *vel_sp : 0.) +
           (pos_sp && pid ? pid->computeCommand(*pos_sp - pos, period) : 0.);
  }

protected:
  dd::BodyNodePtr model_end_link_;
  Eigen::Vector3d end_link_offset_;
  PidMap pids_;
};

} // namespace computed_torque_controllers

#endif


#pragma once

#include <ctime>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_int_distribution.hpp>
#include <limits>

namespace SparCraft
{
	class RandomInt;
}

class SparCraft::RandomInt
{
	boost::random::mt19937 gen;
	boost::random::uniform_int_distribution<> dist;

public:

	RandomInt(int min, int max, int seed)
		: dist(min, max)
	{
		gen.seed(seed);
	}

	int nextInt()
	{
		return dist(gen);
	}

	void seed(int seed)
	{
		gen.seed(seed);
	}

	int reset(int min, int max, int seed)
	{
		dist = boost::random::uniform_int_distribution<>(min, max);
		gen.seed(seed);
	}
};


#include <boost/preprocessor/repetition/repeat.hpp>

#include "StencilFramework.h"
#include "MathFunctions.h"
#include "HorizontalDiffusionFunctions.h"
#include "HorizontalDiffusionSA.h"
#include "Kernel.h"
#include "Options.h"

// define parameter enum
enum
{
    u_in, u_out, lap, hdmaskvel, crlato, crlatu, crlavo, crlavu
};

namespace HorizontalDiffusionSAStages
{
    /**
    * @struct ULapStage
    * Corresponds to numeric_utilities.f90 - first loop of lap_4am
    */
    template<typename TEnv>
    struct ULapStage
    {
        STENCIL_STAGE(TEnv)

        STAGE_PARAMETER(FullDomain, u_out)
        STAGE_PARAMETER(FullDomain, u_in)
        STAGE_PARAMETER(FullDomain, crlato)
        STAGE_PARAMETER(FullDomain, crlatu)

        // flops: 6
        // accesses
        //    no cache: 2 (u, lap) . crlat are J fields not taken into account
        //    cache:    1+1/10
        __ACC__
        static void Do(Context ctx, FullDomain)
        {
            ctx[u_out::Center()] =
                ctx[Call<Laplacian>::With(u_in::Center(), crlato::Center(), crlatu::Center())];
        }
    };
}

HorizontalDiffusionSA::HorizontalDiffusionSA() : stencils_(N_HORIDIFF_VARS), haloUpdates_(N_HORIDIFF_VARS*N_CONCURRENT_HALOS),
    recWBuff_(N_HORIDIFF_VARS*N_CONCURRENT_HALOS), recNBuff_(N_HORIDIFF_VARS*N_CONCURRENT_HALOS), recEBuff_(N_HORIDIFF_VARS*N_CONCURRENT_HALOS), recSBuff_(N_HORIDIFF_VARS*N_CONCURRENT_HALOS),
    sendWBuff_(N_HORIDIFF_VARS*N_CONCURRENT_HALOS), sendNBuff_(N_HORIDIFF_VARS*N_CONCURRENT_HALOS), sendEBuff_(N_HORIDIFF_VARS*N_CONCURRENT_HALOS), sendSBuff_(N_HORIDIFF_VARS*N_CONCURRENT_HALOS)
{
    for(int c=0; c < N_HORIDIFF_VARS; ++c)
    {
        stencils_[c] = new Stencil();
    }

    for(int c=0; c < N_HORIDIFF_VARS*N_CONCURRENT_HALOS; ++c)
    {
        haloUpdates_[c]= new HaloUpdateManager<true, false>();
    }
    cudaStreamCreate(&kernelStream_);

}
HorizontalDiffusionSA::~HorizontalDiffusionSA()
{
    for(int c=0; c < N_CONCURRENT_HALOS; ++c)
    {
        assert(stencils_[c]);
        delete stencils_[c];
    }

    for(int c=0; c < N_HORIDIFF_VARS*N_CONCURRENT_HALOS; ++c)
    {
        assert(haloUpdates_[c]);
        delete haloUpdates_[c];
    }
    cudaStreamDestroy(kernelStream_);

}

void HorizontalDiffusionSA::ResetMeters()
{
    for(int c=0; c < N_HORIDIFF_VARS; ++c)
    {
        assert(stencils_[c]);
        stencils_[c]->ResetMeters();
    }

}
void HorizontalDiffusionSA::Init(
        HoriDiffRepository& horiDiffRepository, CommunicationConfiguration& comm)
{

    MPI_Comm_size(MPI_COMM_WORLD, &numRanks_);
    MPI_Comm_rank(MPI_COMM_WORLD, &rankId_);

    bool found=false;
    for(int i=sqrt(numRanks_); i > 0; --i)
    {
        if(numRanks_ % i == 0) {
            found = true;
            cartSizes_[0] = i;
            cartSizes_[1] = numRanks_ / i;
            break;
        }
    }
    if(!found) std::cout<< "ERROR: Could not apply a domain decomposition" << std::endl;

    if(rankId_ % cartSizes_[0] == 0)
        neighbours_[0] = rankId_ + cartSizes_[0]-1;
    else
        neighbours_[0] = rankId_ -1;

    if(rankId_ < cartSizes_[0])
        neighbours_[1] = rankId_ + (cartSizes_[1]-1)*cartSizes_[0];
    else
        neighbours_[1] = rankId_ - cartSizes_[0];

    if((rankId_+1) % cartSizes_[0] == 0)
        neighbours_[2] = rankId_ - (cartSizes_[0]-1);
    else
        neighbours_[2] = rankId_+1;

    if(rankId_ >= (cartSizes_[1]-1)*cartSizes_[0] )
        neighbours_[3] = (rankId_ + cartSizes_[0] ) - cartSizes_[0]*cartSizes_[1];
    else
        neighbours_[3] =rankId_ + cartSizes_[0];

    commSize_ = (horiDiffRepository.calculationDomain().iSize())*3*horiDiffRepository.calculationDomain().kSize();

    for(int c=0; c < N_HORIDIFF_VARS; ++c)
    {
        for(int h=0; h < N_CONCURRENT_HALOS; ++h)
        {
            cudaMalloc(&(sendWBuff_[c*N_CONCURRENT_HALOS+h]), sizeof(Real)*commSize_);
            cudaMalloc(&(sendNBuff_[c*N_CONCURRENT_HALOS+h]), sizeof(Real)*commSize_);
            cudaMalloc(&(sendEBuff_[c*N_CONCURRENT_HALOS+h]), sizeof(Real)*commSize_);
            cudaMalloc(&(sendSBuff_[c*N_CONCURRENT_HALOS+h]), sizeof(Real)*commSize_);

            cudaMalloc(&(recWBuff_[c*N_CONCURRENT_HALOS+h]), sizeof(Real)*commSize_);
            cudaMalloc(&(recNBuff_[c*N_CONCURRENT_HALOS+h]), sizeof(Real)*commSize_);
            cudaMalloc(&(recEBuff_[c*N_CONCURRENT_HALOS+h]), sizeof(Real)*commSize_);
            cudaMalloc(&(recSBuff_[c*N_CONCURRENT_HALOS+h]), sizeof(Real)*commSize_);
        }
    }

    reqs_ = (MPI_Request*)malloc(N_HORIDIFF_VARS*N_CONCURRENT_HALOS*sizeof(MPI_Request)*4);

    pCommunicationConfiguration_ = &comm;
    // store pointers to the repositories
    pHoriDiffRepository_ = &horiDiffRepository;

    using namespace HorizontalDiffusionSAStages;

    for(int c=0; c < N_HORIDIFF_VARS; ++c)
    {
        assert(stencils_[c]);
        // init the stencil u
        StencilCompiler::Build(
            *(stencils_[c]),
            "HorizontalDiffusion",
            horiDiffRepository.calculationDomain(),
            StencilConfiguration<Real, HorizontalDiffusionSABlockSize>(),
            pack_parameters(
                /* output fields */
                Param<u_out, cInOut, cDataField>(horiDiffRepository.u_out(c)),
                /* input fields */
                Param<u_in, cIn, cDataField>(horiDiffRepository.u_in(c)),
                Param<hdmaskvel, cIn, cDataField>(horiDiffRepository.hdmaskvel()),
                Param<crlato, cIn, cDataField>(horiDiffRepository.crlato()),
                Param<crlatu, cIn, cDataField>(horiDiffRepository.crlatu())
            ),
            define_loops(
                define_sweep<cKIncrement>(
                    define_stages(
                        StencilStage<ULapStage, IJRange<cIndented,0,0,0,0>, KRange<FullDomain,0,0> >()
                    )
                )
            )
        );

        IJBoundary innerBoundary, outerBoundary;
        innerBoundary.Init(0, 0, 0, 0);
        outerBoundary.Init(-3, 3, -3, 3);
        assert(pCommunicationConfiguration_);
        for(int h=0; h < N_CONCURRENT_HALOS; ++h) {
            assert(haloUpdates_[c*N_CONCURRENT_HALOS+h]);
            haloUpdates_[c*N_CONCURRENT_HALOS+h]->Init("HaloUpdate", *pCommunicationConfiguration_);
            haloUpdates_[c*N_CONCURRENT_HALOS+h]->AddJob(horiDiffRepository.u_out(c), innerBoundary, outerBoundary);
        }

   }
}

void HorizontalDiffusionSA::Apply()
{
    for(int c=0; c < N_HORIDIFF_VARS; ++c)
    {
        if(Options::getInstance().nostella_)
        {
            launch_kernel(
                        pHoriDiffRepository_->calculationDomain(),
                        pHoriDiffRepository_->u_in(c).storage().pStorageBase(),
                        pHoriDiffRepository_->u_out(c).storage().pStorageBase(),
                        kernelStream_
            );
        }
        else
        {
            assert(stencils_[c]);
            stencils_[c]->Apply();
        }
    }
}

void HorizontalDiffusionSA::Apply(IJBoundary ijBoundary)
{
    for(int c=0; c < N_HORIDIFF_VARS; ++c)
    {
        assert(stencils_[c]);
        stencils_[c]->Apply(ijBoundary);
    }
}


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   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 BOOST_SIMD_ARITHMETIC_FUNCTIONS_REFINE_REC_HPP_INCLUDED
#define BOOST_SIMD_ARITHMETIC_FUNCTIONS_REFINE_REC_HPP_INCLUDED
#include <boost/simd/include/functor.hpp>
#include <boost/dispatch/include/functor.hpp>

namespace boost { namespace simd
{
  namespace tag
  {
    /*!
      @brief  refine_rec generic tag

      Represents the refine_rec function in generic contexts.

      @par Models:
      Hierarchy
    **/
    struct refine_rec_ : ext::elementwise_<refine_rec_>
    {
      /// @brief Parent hierarchy
      typedef ext::elementwise_<refine_rec_> parent;
      template<class... Args>
      static BOOST_FORCEINLINE BOOST_AUTO_DECLTYPE dispatch(Args&&... args)
      BOOST_AUTO_DECLTYPE_BODY( dispatching_refine_rec_( ext::adl_helper(), static_cast<Args&&>(args)... ) )
    };
  }
  namespace ext
  {
    template<class Site, class... Ts>
    BOOST_FORCEINLINE generic_dispatcher<tag::refine_rec_, Site> dispatching_refine_rec_(adl_helper, boost::dispatch::meta::unknown_<Site>, boost::dispatch::meta::unknown_<Ts>...)
    {
      return generic_dispatcher<tag::refine_rec_, Site>();
    }
    template<class... Args>
    struct impl_refine_rec_;
  }

  /*!
    Performs a Newton-Raphson step to improve precision of reciprocate estimate.
    This function can be used in conjunction with funcref{raw_rec} or
    funcref{fast_rec} to add more precision to the estimate if their default
    precision is not enough.

    @param  a0 Value for which rec is to be computed
    @param  a1 Current estimate of rec(a0)

    @see funcref{rec}

    @return  A value estimating rec(a0) with twice as much precision

  **/
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::refine_rec_, refine_rec, 2)

} }

#endif


#include <stdexcept>
#define BOOST_TEST_MODULE MyTest
#include <boost/test/included/unit_test.hpp>

BOOST_AUTO_TEST_CASE( test_failure_1 )
{
    BOOST_CHECK( 2 * 3 == 5 );
}

BOOST_AUTO_TEST_CASE( test_failure_2 )
{
    BOOST_CHECK( 2 - 1 == 0 );
}


//==================================================================================================
/*!
  @file

    @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_MINNUMMAG_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_MINNUMMAG_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

  /*!
    @ingroup group-ieee
    This function object returns between two floating point arguments,
    the one  having the  smallest absolute value, treating NaNs
    as missing data (between a NaN and a numeric value, the numeric value is chosen).

    @par Header <boost/simd/function/minnummag.hpp>

    @par Example:

      @snippet minnummag.cpp minnummag

    @par Possible output:

      @snippet minnummag.txt minnummag

  **/
  Value minnummag(Value const& x, Value const& y);
} }
#endif

#include <boost/simd/function/scalar/minnummag.hpp>
#include <boost/simd/function/simd/minnummag.hpp>

#endif


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_DETAIL_SCALAR_LOGARITHM_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_DETAIL_SCALAR_LOGARITHM_HPP_INCLUDED

#include <boost/simd/arch/common/detail/scalar/f_log.hpp>
#include <boost/simd/arch/common/detail/scalar/d_log.hpp>

#endif


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

#include "BMSSaliency.h"

#include <BMS.h>
#include <BMS360.h>
#include <UBMS.h>
#include <UBMS360.h>
#include <opencv2/opencv.hpp>
#include <boost/thread.hpp>
#include <boost/bind.hpp>
#include <boost/function.hpp>

#include "ShiftImage.hpp"
#include "EquatorialPrior.h"

BMSSaliency::BMSSaliency(bool loc_bms360, bool umat) {
	m_sampleStep			= 8;
	m_dilatationWidth1		= 3;
	m_dilatationWidth2		= 11;
	m_blurStd				= 20;
	m_normalize				= 1;
	m_handleBorder			= 0;
	m_colorSpace 			= CL_Lab;
	m_whitening				= true;
	m_maxDim				= 400.f;
	m_equatorialPrior 		= false;
	m_nb_projections 		= 4;
	m_bms360				= loc_bms360;
	m_useTAPI				= umat;
}

void BMSSaliency::process(const cv::Mat &inputImage, cv::Mat &sMap, bool normalize) {
	Configuration conf;

    conf.sampleStep = m_sampleStep;
	conf.dilatationWidth1 = m_dilatationWidth1;
	conf.dilatationWidth2 = m_dilatationWidth2;
	conf.blurStd = m_blurStd;
	conf.normalize = m_normalize;
	conf.handleBorder = m_handleBorder;
	conf.colorSpace = m_colorSpace;
	conf.whitening = m_whitening;
	conf.maxDim = static_cast<int>(m_maxDim);
	conf.equatorialPrior = m_equatorialPrior;

	std::vector<cv::Mat> outputs(m_nb_projections);

	// ------------------------------------------------------------------------------------
	// With the new version on GPU, we probably don't want to run that on multiple threads.

    boost::thread_group g;
    for(int i = 0 ; i < m_nb_projections ; ++i) {
    	g.create_thread(boost::bind(&BMSSaliency::processJob, this, i, m_nb_projections, boost::ref(inputImage), boost::ref(outputs), boost::ref(conf)));

    }
    g.join_all();

    //for(int i = 0 ; i < m_nb_projections; ++i) {
    // 	processJob(i, m_nb_projections, inputImage, outputs, conf);
    //}

    sMap = outputs[0];
    for(int i = 1 ; i < m_nb_projections ; ++i) {
    	sMap = sMap + shiftImage<float>(outputs[i], -i * outputs[i].cols / m_nb_projections, 0);
    }

	if(m_normalize) {
		if (m_blurStd > 0) {
			int blur_width = (int)MIN(floor(m_blurStd) * 4 + 1, 51);
			cv::GaussianBlur(sMap, sMap, cv::Size(blur_width, blur_width), m_blurStd, m_blurStd);
		}

		if(m_nb_projections > 1) {
			cv::normalize(sMap, sMap, 0.0, 1.0, cv::NORM_MINMAX);
		}
	}

	cv::resize(sMap, sMap, inputImage.size());

	if(m_equatorialPrior)
		applyEquatorialPrior(sMap, inputImage);

}

void BMSSaliency::processJob(int workerID, int nb_shift, const cv::Mat &input, std::vector<cv::Mat> &outputs, Configuration &conf) {
	cv::Mat inputImage = shiftImage<unsigned char>(input, workerID * input.cols / nb_shift, 0);

	processOneProjection(inputImage, outputs[workerID], conf.maxDim, conf.dilatationWidth1, conf.dilatationWidth2, conf.normalize, conf.handleBorder, conf.colorSpace, conf.whitening, conf.sampleStep, conf.blurStd);
}

// ------------------------------------------------------------------------------------------------------------------------------------------------------
// apply BMS on one frame

void BMSSaliency::processOneProjection(const cv::Mat &input, cv::Mat &output, int maxDim, int dilatationWidth1, int dilatationWidth2, int normalize, int handleBorder, int colorSpace, bool whitening, int sampleStep, float ) {

	cv::Mat src_small;
	float w = (float)input.cols, h = (float)input.rows;
	float maxD = fmax(w, h);

	cv::resize(input, src_small, cv::Size((int)(maxDim*w / maxD), (int)(maxDim*h / maxD)), 0.0, 0.0, cv::INTER_AREA);

	if (!m_useTAPI) {
		boost::shared_ptr<BMS> bms;
		if (m_bms360) {
			bms = boost::shared_ptr<BMS>(new BMS360(src_small, dilatationWidth1, normalize == 1, handleBorder == 1, colorSpace, whitening));
		}
		else {
			bms = boost::shared_ptr<BMS>(new BMS(src_small, dilatationWidth1, normalize == 1, handleBorder == 1, colorSpace, whitening));
		}

		bms->computeSaliency((double)sampleStep);

		cv::Mat result = bms->getSaliencyMap(false);

		if (dilatationWidth2 > 0)
			dilate(result, output, cv::Mat(), cv::Point(-1, -1), dilatationWidth2);
		else
			output = result;

	} else {
		boost::shared_ptr<UBMS> bms;
		if (m_bms360) {
			bms = boost::shared_ptr<UBMS>(new UBMS360(src_small, dilatationWidth1, normalize == 1, handleBorder == 1, colorSpace, whitening));
		}
		else {
			bms = boost::shared_ptr<UBMS>(new UBMS(src_small, dilatationWidth1, normalize == 1, handleBorder == 1, colorSpace, whitening));
		}

		bms->computeSaliency((double)sampleStep);

		output = bms->getSaliencyMap(false, dilatationWidth2);
	}

}


#include "core/plan/runner.hpp"

#include "base/color.hpp"

#include <boost/tokenizer.hpp>

#include <cmath>

DEFINE_string(url, "", "The url for hdfs file");

DEFINE_int32(kNumPartition, 2, "");
DEFINE_int32(kNumItem, 3, "");
DEFINE_int32(kNumUser, 3, "");
// DEFINE_double(eta, 0.1, "");
DEFINE_double(lambda, 0.1, "");
DEFINE_int32(iter, 1, "num of iters");
DEFINE_int32(staleness, 0, "staleness");
DEFINE_int32(num_line_per_part, -1, "num_line_per_part");
DEFINE_int32(backoff_time, 100, "backoff time if there is no item in ms");
DEFINE_int32(max_sample_item_size_each_round, -1, "");
DEFINE_int32(max_retry, 0, "may set to 0 for bsp");

DEFINE_double(alpha, -1, "");
DEFINE_double(beta, -1, "");
// for yahoo music
// alpha = 0.00075;
// beta = 0.01;
// lambda = 1;
//
// for netflix
// alpha = 0.012;
// beta = 0.05;
// lambda = 0.05;

using namespace xyz;

const int kNumLatent = 100;

struct Item {
  using KeyT = int;
  KeyT key;
  float latent[kNumLatent];
  void ClearLatent() {
    for (auto &l : latent) {
      l = 0;
    }
  }
  Item() { ClearLatent(); }
  Item(KeyT _key) : key(_key) { ClearLatent(); }
  KeyT Key() const { return key; }
  void Init() {
    for (auto &i : latent) {
      // i = ((float)rand())/RAND_MAX*2-1;
      i = ((float)rand()) / RAND_MAX * sqrt(5.) / sqrt(kNumLatent * 1.0);
    }
  }
  std::string DebugString() const {
    std::stringstream ss;
    ss << "key: " << key;
    for (auto l : latent) {
      ss << ", " << l;
    }
    return ss.str();
  }
};

using User = Item;
using UserOrItem = Item;

// partitioned by users.
// indexed by items.
struct DataBlock {
  using KeyT = int;
  KeyT key; // the partition id
  // item -> {<u, r, update_count>, ...}
  std::map<int, std::vector<std::tuple<int, float, int>>> points;

  KeyT Key() const { return key; }
  DataBlock() = default;
  DataBlock(KeyT _key) : key(_key) {}
  friend SArrayBinStream &operator<<(xyz::SArrayBinStream &stream,
                                     const DataBlock &d) {
    stream << d.key; // TODO: no need to serialize the points
    // as it is only used in distribute and no data at that time.
    return stream;
  }
  friend SArrayBinStream &operator>>(xyz::SArrayBinStream &stream,
                                     DataBlock &d) {
    stream >> d.key;
    return stream;
  }
  std::string DebugString() const {
    std::stringstream ss;
    ss << "key: " << key << ", ";
    for (auto &kv : points) {
      ss << "item: " << kv.first << ": ";
      for (auto &p : kv.second) {
        ss << "<" << std::get<0>(p) << "," << std::get<1>(p) << ","
           << std::get<2>(p) << ">";
      }
      ss << ", ";
    }
    return ss.str();
  }
};

struct Collector {
  using KeyT = int;
  KeyT key; // the partition id
  // store the user latent factors
  // and the temporary item latent factors
  std::map<int, User> users;
  std::deque<Item> items;
  int version;

  Collector() = default;
  Collector(KeyT _key) : key(_key) {}
  KeyT Key() const { return key; }
  friend SArrayBinStream &operator<<(xyz::SArrayBinStream &stream,
                                     const Collector &c) {
    stream << c.key << c.users << c.items << c.version; // TODO
    return stream;
  }
  friend SArrayBinStream &operator>>(xyz::SArrayBinStream &stream,
                                     Collector &c) {
    stream >> c.key >> c.users >> c.items >> c.version;
    return stream;
  }
};

struct Msg {
  int num_item_processed = -1; // -1 for item migration,
  // others for user updates and a number saying how many items have been
  // processed
  std::map<int, UserOrItem> dict;
  int version; // debug
  friend SArrayBinStream &operator<<(xyz::SArrayBinStream &stream,
                                     const Msg &m) {
    stream << m.num_item_processed << m.dict << m.version;
    return stream;
  }
  friend SArrayBinStream &operator>>(xyz::SArrayBinStream &stream, Msg &m) {
    stream >> m.num_item_processed >> m.dict >> m.version;
    return stream;
  }
  std::string DebugString() const {
    std::stringstream ss;
    ss << "num_item_processed: " << num_item_processed;
    ss << ", dict size: " << dict.size();
    ss << ", version: " << version;
    return ss.str();
  }
};

struct Record {
  int user, item;
  float rating;
};

struct RMSE {
  using KeyT = int;
  RMSE() = default;
  RMSE(KeyT k) : key(k) {}
  KeyT Key() const { return key; }
  KeyT key;
  std::pair<int, float> p; // count, rmse
  int update_count = 0;
  friend SArrayBinStream &operator<<(xyz::SArrayBinStream &stream,
                                     const RMSE &r) {
    stream << r.key << r.p << r.update_count;
    return stream;
  }
  friend SArrayBinStream &operator>>(xyz::SArrayBinStream &stream, RMSE &r) {
    stream >> r.key >> r.p >> r.update_count;
    return stream;
  }
};

int main(int argc, char **argv) {
  Runner::Init(argc, argv);

  CHECK_NE(FLAGS_alpha, double(-1));
  CHECK_NE(FLAGS_beta, double(-1));

  auto load_collection =
      Context::load(FLAGS_url,
                    [](std::string s) {
                      Record r;
                      boost::char_separator<char> sep(" \t");
                      boost::tokenizer<boost::char_separator<char>> tok(s, sep);
                      auto it = tok.begin();
                      r.user = std::stoi(*it++);
                      r.item = std::stoi(*it++);
                      r.rating = std::stof(*it++);
                      return r;
                    },
                    FLAGS_num_line_per_part);

  // distribute the ratings to data_blocks
  auto data_blocks =
      Context::placeholder<DataBlock, RoundRobinKeyToPartMapper<int>>(
          FLAGS_kNumPartition);
  Context::mapupdate(load_collection, data_blocks,
                   [](const Record &r, Output<int, Record> *o) {
                     o->Add(r.user % FLAGS_kNumPartition, r);
                   },
                   [](DataBlock *d, Record r) {
                     d->points[r.item].push_back(
                         std::make_tuple(r.user, r.rating, 0));
                   })
      ->SetName("partition the ratings according to user");

  // distribute the users latent factor and item latent factor (initial
  // distribution)
  auto dummy_distribute_collection =
      Context::distribute(std::vector<int>{0}, 1, "init the collector");
  auto collectors =
      Context::placeholder<Collector, RoundRobinKeyToPartMapper<int>>(
          FLAGS_kNumPartition);
  Context::mapupdate(dummy_distribute_collection, collectors,
                   [](int, Output<int, int> *o) {
                     for (int i = 0; i < FLAGS_kNumPartition; ++i) {
                       o->Add(i, i);
                     }
                   },
                   [](Collector *c, int) {
                     for (int i = 0; i < FLAGS_kNumUser; ++i) {
                       if (i % FLAGS_kNumPartition == c->Key()) {
                         c->users.insert({i, User(i)});
                         c->users[i].Init();
                       }
                     }
                     for (int i = 0; i < FLAGS_kNumItem; ++i) {
                       if (i % FLAGS_kNumPartition == c->Key()) {
                         c->items.push_back(Item(i));
                         c->items.back().Init();
                       }
                     }
                   })
      ->SetName("init the collector");

  // print info
  /*
  Context::foreach(data_blocks, [](const DataBlock& d) {
    LOG(INFO) << d.DebugString();
  });
  */

  // main logic
  Context::mappartwithupdate(
      data_blocks, collectors, collectors,
      [](TypedPartition<DataBlock> *p, TypedCache<Collector> *typed_cache,
         Output<int, Msg> *o) {
        CHECK_EQ(p->GetSize(), 1);
        auto with_part = typed_cache->GetPartition(p->id);
        CHECK_EQ(with_part->GetSize(), 1);
        auto iter = p->begin();
        auto with_iter =
            static_cast<TypedPartition<Collector> *>(with_part.get())->begin();
        int version = typed_cache->GetVersion();

        // FLAGS_max_retry may set to 0 for bsp
        int retry = 0;
        while (with_iter->items.empty() && FLAGS_max_retry) {
          // LOG(INFO) << "retrying " << p->id << " no items, sleep for " <<
          // FLAGS_backoff_time << " ms";
          typed_cache->ReleasePart(p->id);
          std::this_thread::sleep_for(
              std::chrono::milliseconds(FLAGS_backoff_time));
          with_part = typed_cache->GetPartition(p->id);
          with_iter = static_cast<TypedPartition<Collector> *>(with_part.get())
                          ->begin();
          retry += 1;
          if (retry == FLAGS_max_retry) {
            // LOG(INFO) << p->id << " reach max retry";
            break;
          }
        }
        // LOG(INFO) << "item count: " << items.size() << " on " << p->id;
        if (with_iter->items.empty()) {
          LOG(INFO) << p->id << " no items, sleep for " << FLAGS_backoff_time
                    << " ms";
          std::this_thread::sleep_for(
              std::chrono::milliseconds(FLAGS_backoff_time));
        }

        // Msg update_users;
        // Msg migrate_items;
        std::vector<std::pair<int, Msg>> msgs(FLAGS_kNumPartition + 1);
        for (int i = 0; i < FLAGS_kNumPartition; ++i) {
          msgs[i].first = i;
          msgs[i].second.num_item_processed = -1;
        }
        msgs[FLAGS_kNumPartition].first = p->id; // the update_users
        Msg &update_users = msgs[FLAGS_kNumPartition].second;
        update_users.version = version;
        update_users.num_item_processed = 0;

        // migrate_items.num_item_processed = -1;
        auto &items = with_iter->items;
        if (version > 0)
          CHECK_EQ(version, with_iter->version + 1);
        int c = 0;
        int sample = 2;
        int item_count = 0;
        for (auto &item : items) {
          item_count += 1;
          if (item_count == FLAGS_max_sample_item_size_each_round) {
            LOG(INFO) << p->id
                      << " truncated by max_sample_item_size_each_round: "
                      << FLAGS_max_sample_item_size_each_round;
            break;
          }
          // choose a migrate item
          int k = rand() % FLAGS_kNumPartition;
          auto &migrate_items = msgs[k].second;
          // LOG(INFO) << "item: " << item.DebugString();
          update_users.num_item_processed += 1;
          migrate_items.dict[item.key] = item;
          auto &migrate_item = migrate_items.dict[item.key];
          for (auto &u_r_c : iter->points[item.key]) {
            auto &user = with_iter->users[std::get<0>(u_r_c)];
            // int t = std::get<2>(u_r_c);
            // LOG(INFO) << "t: " << t;
            // float lr = FLAGS_alpha / (1 + FLAGS_beta*pow(t, 1.5));
            float lr = FLAGS_alpha;
            float diff =
                -std::inner_product(user.latent, user.latent + kNumLatent,
                                    migrate_item.latent, -std::get<1>(u_r_c));
            /*
            if (c < sample && p->id == 0) {
              // LOG(INFO) << "user: " << user.DebugString();
              LOG(INFO) << "uid, iid: " << user.key << "," << item.key << ",
            estimated, real: "
                << std::inner_product(user.latent, user.latent+kNumLatent,
            migrate_item.latent, 0.0) << ", " << std::get<1>(u_r_c)
                << ", lr: " << lr << ", t: " << t;
            }
            c += 1;
            */
            // std::get<2>(u_r_c) += 1;  // fuck! update map element
            // auto& user_update = update_users.dict[user.key];
            for (int i = 0; i < kNumLatent; ++i) {
              migrate_item.latent[i] +=
                  lr * (user.latent[i] * diff -
                        FLAGS_lambda * migrate_item.latent[i]);
              user.latent[i] += lr * (migrate_item.latent[i] * diff -
                                      FLAGS_lambda * user.latent[i]);
            }
            /*
            diff = - std::inner_product(user.latent, user.latent+kNumLatent,
            migrate_item.latent, - std::get<1>(u_r_c));
            for (int i = 0; i < kNumLatent; ++ i) {
              user_update.latent[i] += lr*(migrate_item.latent[i]*diff -
            FLAGS_lambda*user.latent[i]);
            }
            for (int i = 0; i < kNumLatent; ++ i) {
              user.latent[i] += user_update.latent[i];
              user_update.latent[i] = 0;
            }
            */
          }
        }
        typed_cache->ReleasePart(p->id); // donot forget to call this
        // std::vector<std::pair<int, Msg>> ret;
        // ret.emplace_back(p->id, std::move(update_users));
        // ret.emplace_back((p->id+1)%FLAGS_kNumPartition,
        // std::move(migrate_items));
        // each map send out two Msgs, one to the collector in this part and the
        // other to the collector in next part.
        // e.g. id = 0, send update_users to 0, and send migrate_items to 1.
        // the system should make sure local update will apply before next map
        // (this will be ensured by local_map_mode, as in local_map_mode, the
        // local message will
        // send to local controller queue before the finish map signal).
        for (auto &msg : msgs) {
          o->Add(msg.first, msg.second);
        }
      },
      [](Collector *collector, Msg msg) {
        // LOG(INFO) << msg.DebugString();
        if (msg.num_item_processed == -1) { // migrate item
          for (auto &kv : msg.dict) {
            collector->items.push_back(kv.second);
          }
        } else { // update users
          collector->version = msg.version;
          /*
          for (auto& kv : msg.dict) {
            auto& user = collector->users[kv.first].latent;
            auto& update = kv.second.latent;
            for (int i = 0; i < kNumLatent; ++ i) {
              user[i] += update[i];
            }
          }
          */
          // remove the first num_item_processed.
          for (int i = 0; i < msg.num_item_processed; ++i) {
            collector->items.pop_front();
          }
        }
      })
      ->SetName("main loop")
      ->SetIter(FLAGS_iter)
      ->SetStaleness(FLAGS_staleness);

  // rmse
  // this mse only calculate a part of the training samples!
  auto rmse = Context::placeholder<RMSE>(1);
  Context::mappartwithupdate(
      data_blocks, collectors, rmse,
      [](TypedPartition<DataBlock> *p, TypedCache<Collector> *typed_cache,
         Output<int, std::tuple<int, float, int>> *o) {
        CHECK_EQ(p->GetSize(), 1);

        // LOG(INFO) << "part id, fetch id: " << p->id;
        auto with_part = typed_cache->GetPartition(p->id);
        CHECK_EQ(with_part->GetSize(), 1);
        auto iter = p->begin();
        auto my_with_iter =
            static_cast<TypedPartition<Collector> *>(with_part.get())->begin();

        int count = 0;
        float local_rmse = 0;
        int update_count = 0;
        auto &items = my_with_iter->items;
        for (auto &item : items) {
          for (auto &u_r_c : iter->points[item.key]) {
            auto &user = my_with_iter->users[std::get<0>(u_r_c)];
            float r = std::inner_product(user.latent, user.latent + kNumLatent,
                                         item.latent, 0.0);
            local_rmse += pow(fabs(r - std::get<1>(u_r_c)), 2);
            count += 1;
            update_count += std::get<2>(u_r_c);
          }
        }
        // LOG(INFO) << "[Debug] local item size: " << items.size();

        /*
        for (int i = 0; i < FLAGS_kNumPartition; ++ i) {
          if (i == p->id) {
            continue;
          }
          auto with_part = typed_cache->GetPartition(i);
          // LOG(INFO) << "part id, fetch id: " << p->id << ", " << i
          //   << ", size: " << with_part->GetSize();
          CHECK_EQ(with_part->GetSize(), 1);
          auto iter = p->begin();
          auto with_iter =
        static_cast<TypedPartition<Collector>*>(with_part.get())->begin();

          auto& items = with_iter->items;
          // LOG(INFO) << "[Debug] part id: " << i << ", local item size: " <<
        items.size();
          for (auto& item : items) {
            for (auto& u_r_c: iter->points[item.key]) {
              auto& user = my_with_iter->users[std::get<0>(u_r_c)];
              float r = std::inner_product(user.latent, user.latent+kNumLatent,
        item.latent, 0.0);
              local_rmse += pow(fabs(r-std::get<1>(u_r_c)), 2);
              count += 1;
            }
          }
          typed_cache->ReleasePart(i);  // donot forget to call this
          // LOG(INFO) << "part id, fetch id: " << p->id << ", " << i << ",
        done";
        }
        */

        typed_cache->ReleasePart(p->id); // donot forget to call this
        // LOG(INFO) << "part id, fetch id: " << p->id << ", done";

        // LOG(INFO) << "[Debug] local count, rmse: " << count << ", " <<
        // local_rmse;
        std::vector<std::pair<int, std::tuple<int, float, int>>> ret;
        ret.emplace_back(0, std::make_tuple(count, local_rmse, update_count));
        for (auto &tmp : ret) {
          o->Add(tmp.first, tmp.second);
        }
      },
      [](RMSE *rmse, std::tuple<int, float, int> m) {
        // LOG(INFO) << "update rmse: " << m.first << " " << m.second;
        rmse->p.first += std::get<0>(m);
        rmse->p.second += std::get<1>(m);
        rmse->update_count += std::get<2>(m);
      })
      ->SetName("rmse");
  Context::foreach (rmse, [](RMSE r) {
    CHECK_GT(r.p.first, 0);
    std::stringstream ss;
    ss << "num samples: " << r.p.first
       << " rmse: " << sqrt(r.p.second / r.p.first) << ", "
       << "update_count: " << r.update_count;
    LOG(INFO) << BLUE(ss.str());
  });

  Runner::Run();
  // Runner::PrintDag();
}