/* Copyright (c) 2016 - 2020, the adamantine authors.
 *
 * This file is subject to the Modified BSD License and may not be distributed
 * without copyright and license information. Please refer to the file LICENSE
 * for the text and further information on this license.
 */

#ifndef ADAMANTINE_HH
#define ADAMANTINE_HH

#include <Geometry.hh>
#include <PostProcessor.hh>
#include <ThermalPhysics.hh>
#include <Timer.hh>
#include <utils.hh>

#include <deal.II/base/mpi.h>
#include <deal.II/distributed/solution_transfer.h>
#include <deal.II/grid/filtered_iterator.h>
#include <deal.II/grid/grid_refinement.h>
#include <deal.II/numerics/error_estimator.h>

#include <boost/filesystem.hpp>
#include <boost/program_options.hpp>
#include <boost/property_tree/info_parser.hpp>
#include <boost/property_tree/ptree.hpp>

#include <cmath>
#include <iostream>

template <int dim, typename MemorySpaceType,
          std::enable_if_t<
              std::is_same<MemorySpaceType, dealii::MemorySpace::Host>::value,
              int> = 0>
void output_pvtu(
    adamantine::PostProcessor<dim> &post_processor, unsigned int cycle,
    unsigned int n_time_step, double time,
    dealii::AffineConstraints<double> const &affine_constraints,
    dealii::LinearAlgebra::distributed::Vector<double, MemorySpaceType>
        &solution)
{
  affine_constraints.distribute(solution);
  post_processor.output_pvtu(cycle, n_time_step, time, solution);
}

#ifdef ADAMANTINE_HAVE_CUDA
template <int dim, typename MemorySpaceType,
          std::enable_if_t<
              std::is_same<MemorySpaceType, dealii::MemorySpace::CUDA>::value,
              int> = 0>
void output_pvtu(
    adamantine::PostProcessor<dim> &post_processor, unsigned int cycle,
    unsigned int n_time_step, double time,
    dealii::AffineConstraints<double> const &affine_constraints,
    dealii::LinearAlgebra::distributed::Vector<double, MemorySpaceType>
        &solution)
{
  dealii::LinearAlgebra::distributed::Vector<double, dealii::MemorySpace::Host>
      solution_host(solution.get_partitioner());
  solution_host.import(solution, dealii::VectorOperation::insert);
  affine_constraints.distribute(solution_host);
  post_processor.output_pvtu(cycle, n_time_step, time, solution_host);
}
#endif

template <int dim, typename MemorySpaceType,
          std::enable_if_t<
              std::is_same<MemorySpaceType, dealii::MemorySpace::Host>::value,
              int> = 0>
dealii::Vector<float> estimate_error(
    dealii::parallel::distributed::Triangulation<dim> const &triangulation,
    dealii::DoFHandler<dim> const &dof_handler, int fe_degree,
    dealii::LA::distributed::Vector<double, MemorySpaceType> const &solution)
{
  dealii::Vector<float> estimated_error_per_cell(
      triangulation.n_active_cells());
  dealii::KellyErrorEstimator<dim>::estimate(
      dof_handler, dealii::QGauss<dim - 1>(fe_degree + 1),
      std::map<dealii::types::boundary_id,
               const dealii::Function<dim, double> *>(),
      solution, estimated_error_per_cell, dealii::ComponentMask(), nullptr, 0,
      triangulation.locally_owned_subdomain());

  return estimated_error_per_cell;
}

#ifdef ADAMANTINE_HAVE_CUDA
template <int dim, typename MemorySpaceType,
          std::enable_if_t<
              std::is_same<MemorySpaceType, dealii::MemorySpace::CUDA>::value,
              int> = 0>
dealii::Vector<float> estimate_error(
    dealii::parallel::distributed::Triangulation<dim> const &triangulation,
    dealii::DoFHandler<dim> const &dof_handler, int fe_degree,
    dealii::LA::distributed::Vector<double, MemorySpaceType> const &solution)
{
  dealii::LA::distributed::Vector<double, dealii::MemorySpace::Host>
      solution_host(solution.get_partitioner());
  solution_host.import(solution, dealii::VectorOperation::insert);
  dealii::Vector<float> estimated_error_per_cell(
      triangulation.n_active_cells());
  dealii::KellyErrorEstimator<dim>::estimate(
      dof_handler, dealii::QGauss<dim - 1>(fe_degree + 1),
      std::map<dealii::types::boundary_id,
               const dealii::Function<dim, double> *>(),
      solution_host, estimated_error_per_cell, dealii::ComponentMask(), nullptr,
      0, triangulation.locally_owned_subdomain());

  return estimated_error_per_cell;
}
#endif

// inlining this function so we can have in the header
inline void initialize_timers(MPI_Comm const &communicator,
                              std::vector<adamantine::Timer> &timers)
{
  timers.push_back(adamantine::Timer(communicator, "Main"));
  timers.push_back(adamantine::Timer(communicator, "Refinement"));
  timers.push_back(adamantine::Timer(communicator, "Evolve One Time Step"));
  timers.push_back(adamantine::Timer(
      communicator, "Evolve One Time Step: evaluate_thermal_physics"));
  timers.push_back(adamantine::Timer(
      communicator, "Evolve One Time Step: id_minus_tau_J_inverse"));
  timers.push_back(adamantine::Timer(
      communicator, "Evolve One Time Step: evaluate_material_properties"));
}

template <int dim, int fe_degree, typename MemorySpaceType,
          typename QuadratureType>
std::vector<std::unique_ptr<adamantine::ElectronBeam<dim>>> &initialize(
    MPI_Comm const &communicator, boost::property_tree::ptree const &database,
    adamantine::Geometry<dim> &geometry,
    std::unique_ptr<adamantine::Physics<dim, MemorySpaceType>> &thermal_physics)
{
  thermal_physics.reset(
      new adamantine::ThermalPhysics<dim, fe_degree, MemorySpaceType,
                                     QuadratureType>(communicator, database,
                                                     geometry));
  return static_cast<adamantine::ThermalPhysics<dim, fe_degree, MemorySpaceType,
                                                QuadratureType> *>(
             thermal_physics.get())
      ->get_electron_beams();
}

template <int dim, int fe_degree, typename MemorySpaceType>
std::vector<std::unique_ptr<adamantine::ElectronBeam<dim>>> &
initialize_quadrature(
    std::string const &quadrature_type, MPI_Comm const &communicator,
    boost::property_tree::ptree const &database,
    adamantine::Geometry<dim> &geometry,
    std::unique_ptr<adamantine::Physics<dim, MemorySpaceType>> &thermal_physics)
{
  if (quadrature_type.compare("gauss") == 0)
    return initialize<dim, fe_degree, MemorySpaceType, dealii::QGauss<1>>(
        communicator, database, geometry, thermal_physics);
  else
  {
    adamantine::ASSERT_THROW(quadrature_type.compare("lobatto") == 0,
                             "quadrature should be Gauss or Lobatto.");
    return initialize<dim, fe_degree, MemorySpaceType,
                      dealii::QGaussLobatto<1>>(communicator, database,
                                                geometry, thermal_physics);
  }
}

template <int dim, typename MemorySpaceType>
std::vector<std::unique_ptr<adamantine::ElectronBeam<dim>>> &
initialize_thermal_physics(
    unsigned int fe_degree, std::string const &quadrature_type,
    MPI_Comm const &communicator, boost::property_tree::ptree const &database,
    adamantine::Geometry<dim> &geometry,
    std::unique_ptr<adamantine::Physics<dim, MemorySpaceType>> &thermal_physics)
{
  switch (fe_degree)
  {
  case 1:
  {
    return initialize_quadrature<dim, 1, MemorySpaceType>(
        quadrature_type, communicator, database, geometry, thermal_physics);
  }
  case 2:
  {
    return initialize_quadrature<dim, 2, MemorySpaceType>(
        quadrature_type, communicator, database, geometry, thermal_physics);
  }
  case 3:
  {
    return initialize_quadrature<dim, 3, MemorySpaceType>(
        quadrature_type, communicator, database, geometry, thermal_physics);
  }
  case 4:
  {
    return initialize_quadrature<dim, 4, MemorySpaceType>(
        quadrature_type, communicator, database, geometry, thermal_physics);
  }
  case 5:
  {
    return initialize_quadrature<dim, 5, MemorySpaceType>(
        quadrature_type, communicator, database, geometry, thermal_physics);
  }
  case 6:
  {
    return initialize_quadrature<dim, 6, MemorySpaceType>(
        quadrature_type, communicator, database, geometry, thermal_physics);
  }
  case 7:
  {
    return initialize_quadrature<dim, 7, MemorySpaceType>(
        quadrature_type, communicator, database, geometry, thermal_physics);
  }
  case 8:
  {
    return initialize_quadrature<dim, 8, MemorySpaceType>(
        quadrature_type, communicator, database, geometry, thermal_physics);
  }
  case 9:
  {
    return initialize_quadrature<dim, 9, MemorySpaceType>(
        quadrature_type, communicator, database, geometry, thermal_physics);
  }
  default:
  {
    adamantine::ASSERT_THROW(fe_degree == 10,
                             "fe_degree should be between 1 and 10.");
    return initialize_quadrature<dim, 10, MemorySpaceType>(
        quadrature_type, communicator, database, geometry, thermal_physics);
  }
  }
}

template <int dim, typename MemorySpaceType,
          std::enable_if_t<
              std::is_same<MemorySpaceType, dealii::MemorySpace::Host>::value,
              int> = 0>
void refine_and_transfer(
    std::unique_ptr<adamantine::Physics<dim, MemorySpaceType>> &thermal_physics,
    dealii::DoFHandler<dim> &dof_handler,
    dealii::LA::distributed::Vector<double, MemorySpaceType> &solution)
{
  dealii::parallel::distributed::Triangulation<dim> &triangulation =
      dynamic_cast<dealii::parallel::distributed::Triangulation<dim> &>(
          const_cast<dealii::Triangulation<dim> &>(
              dof_handler.get_triangulation()));

  std::shared_ptr<adamantine::MaterialProperty<dim>> material_property =
      thermal_physics->get_material_property();

  dealii::parallel::distributed::SolutionTransfer<
      dim, dealii::LA::distributed::Vector<double, MemorySpaceType>>
      solution_transfer(dof_handler);
  std::vector<dealii::parallel::distributed::SolutionTransfer<
      dim, dealii::LA::distributed::Vector<double, MemorySpaceType>>>
  material_state(
      static_cast<unsigned int>(adamantine::MaterialState::SIZE),
      dealii::parallel::distributed::SolutionTransfer<
          dim, dealii::LA::distributed::Vector<double, MemorySpaceType>>(
          material_property->get_dof_handler()));

  // We need to update the ghost values before we can do the interpolation on
  // the new mesh.
  solution.update_ghost_values();

  // Prepare the Triangulation and the diffent SolutionTransfers for refinement
  triangulation.prepare_coarsening_and_refinement();
  solution_transfer.prepare_for_coarsening_and_refinement(solution);
  for (unsigned int i = 0;
       i < static_cast<unsigned int>(adamantine::MaterialState::SIZE); ++i)
    material_state[i].prepare_for_coarsening_and_refinement(
        material_property->get_state()[i]);

  // Execute the refinement
  triangulation.execute_coarsening_and_refinement();

  // Update the AffineConstraints and resize the solution
  thermal_physics->setup_dofs();
  thermal_physics->initialize_dof_vector(solution);

  // Update MaterialProperty DoFHandler and resize the state vectors
  material_property->reinit_dofs();

  // Interpolate the solution and the state onto the new mesh
  solution_transfer.interpolate(solution);
  for (unsigned int i = 0;
       i < static_cast<unsigned int>(adamantine::MaterialState::SIZE); ++i)
    material_state[i].interpolate(material_property->get_state()[i]);

#if ADAMANTINE_DEBUG
  // Check that we are not losing material
  std::array<dealii::LA::distributed::Vector<double, dealii::MemorySpace::Host>,
             static_cast<unsigned int>(adamantine::MaterialState::SIZE)>
      state = material_property->get_state();
  unsigned int const local_size = state[0].local_size();
  unsigned int constexpr n_material_states =
      static_cast<unsigned int>(adamantine::MaterialState::SIZE);
  for (unsigned int i = 0; i < local_size; ++i)
  {
    double material_ratio = 0.;
    for (unsigned int j = 0; j < n_material_states; ++j)
      material_ratio += state[j].local_element(i);
    adamantine::ASSERT(std::abs(material_ratio - 1.) < 1e-14,
                       "Material is lost.");
  }
#endif
}

#ifdef ADAMANTINE_HAVE_CUDA
template <int dim, typename MemorySpaceType,
          std::enable_if_t<
              std::is_same<MemorySpaceType, dealii::MemorySpace::CUDA>::value,
              int> = 0>
void refine_and_transfer(
    std::unique_ptr<adamantine::Physics<dim, MemorySpaceType>> &thermal_physics,
    dealii::DoFHandler<dim> &dof_handler,
    dealii::LA::distributed::Vector<double, MemorySpaceType> &solution)
{
  dealii::parallel::distributed::Triangulation<dim> &triangulation =
      dynamic_cast<dealii::parallel::distributed::Triangulation<dim> &>(
          const_cast<dealii::Triangulation<dim> &>(
              dof_handler.get_triangulation()));

  std::shared_ptr<adamantine::MaterialProperty<dim>> material_property =
      thermal_physics->get_material_property();

  dealii::parallel::distributed::SolutionTransfer<
      dim, dealii::LA::distributed::Vector<double, dealii::MemorySpace::Host>>
      solution_transfer(dof_handler);
  std::vector<dealii::parallel::distributed::SolutionTransfer<
      dim, dealii::LA::distributed::Vector<double, dealii::MemorySpace::Host>>>
  material_state(
      static_cast<unsigned int>(adamantine::MaterialState::SIZE),
      dealii::parallel::distributed::SolutionTransfer<
          dim,
          dealii::LA::distributed::Vector<double, dealii::MemorySpace::Host>>(
          material_property->get_dof_handler()));

  // We need to update the ghost values before we can do the interpolation on
  // the new mesh.
  solution.update_ghost_values();

  // Prepare the Triangulation and the diffent SolutionTransfers for refinement
  triangulation.prepare_coarsening_and_refinement();
  dealii::LA::distributed::Vector<double, dealii::MemorySpace::Host>
      solution_host(solution.get_partitioner());
  solution_host.import(solution, dealii::VectorOperation::insert);
  solution_transfer.prepare_for_coarsening_and_refinement(solution_host);
  for (unsigned int i = 0;
       i < static_cast<unsigned int>(adamantine::MaterialState::SIZE); ++i)
    material_state[i].prepare_for_coarsening_and_refinement(
        material_property->get_state()[i]);

  // Execute the refinement
  triangulation.execute_coarsening_and_refinement();

  // Update MaterialProperty DoFHandler and resize the state vectors
  material_property->reinit_dofs();

  // Update the AffineConstraints and resize the solution
  thermal_physics->setup_dofs();
  thermal_physics->initialize_dof_vector(solution);
  solution_host.reinit(solution.get_partitioner());

  // Interpolate the solution and the state onto the new mesh
  solution_transfer.interpolate(solution_host);
  solution.import(solution_host, dealii::VectorOperation::insert);
  for (unsigned int i = 0;
       i < static_cast<unsigned int>(adamantine::MaterialState::SIZE); ++i)
    material_state[i].interpolate(material_property->get_state()[i]);

#if ADAMANTINE_DEBUG
  // Check that we are not losing material
  std::array<dealii::LA::distributed::Vector<double, dealii::MemorySpace::Host>,
             static_cast<unsigned int>(adamantine::MaterialState::SIZE)>
      state = material_property->get_state();
  unsigned int const local_size = state[0].local_size();
  unsigned int constexpr n_material_states =
      static_cast<unsigned int>(adamantine::MaterialState::SIZE);
  for (unsigned int i = 0; i < local_size; ++i)
  {
    double material_ratio = 0.;
    for (unsigned int j = 0; j < n_material_states; ++j)
      material_ratio += state[j].local_element(i);
    adamantine::ASSERT(std::abs(material_ratio - 1.) < 1e-14,
                       "Material is lost.");
  }
#endif
}
#endif

template <int dim>
std::vector<typename dealii::parallel::distributed::Triangulation<
    dim>::active_cell_iterator>
compute_cells_to_refine(
    dealii::parallel::distributed::Triangulation<dim> &triangulation,
    double const time, double const next_refinement_time,
    unsigned int const n_time_steps,
    std::vector<std::unique_ptr<adamantine::ElectronBeam<dim>>> &electron_beams)
{
  // The save_time()/rewind_time() functions are used to save the current time
  // and row in the list of beam positions. It does nothing when the position is
  // given by an analytic formula.
  for (auto &beam : electron_beams)
    beam->save_time();

  // Compute the position of the beams between time and next_refinement_time and
  // refine the mesh where the source is greater than 1e-15. This cut-off is due
  // to the fact that the source is gaussian and thus never strictly zero. If
  // the beams intersect, some cells will appear twice in the vector. This is
  // not a problem.
  std::vector<typename dealii::parallel::distributed::Triangulation<
      dim>::active_cell_iterator>
      cells_to_refine;
  for (unsigned int i = 0; i < n_time_steps; ++i)
  {
    double const current_time = time + static_cast<double>(i) /
                                           static_cast<double>(n_time_steps) *
                                           (next_refinement_time - time);
    for (auto &beam : electron_beams)
    {
      beam->set_time(current_time);
      for (auto cell : dealii::filter_iterators(
               triangulation.active_cell_iterators(),
               dealii::IteratorFilters::LocallyOwnedCell()))
        if (beam->value(cell->center()) > 1e-15)
          cells_to_refine.push_back(cell);
    }
  }

  for (auto &beam : electron_beams)
    beam->rewind_time();

  return cells_to_refine;
}

template <int dim, int fe_degree, typename MemorySpaceType>
void refine_mesh(
    std::unique_ptr<adamantine::Physics<dim, MemorySpaceType>> &thermal_physics,
    dealii::LA::distributed::Vector<double, MemorySpaceType> &solution,
    std::vector<std::unique_ptr<adamantine::ElectronBeam<dim>>> &electron_beams,
    double const time, double const next_refinement_time,
    unsigned int const time_steps_refinement,
    boost::property_tree::ptree const &refinement_database)
{
  dealii::DoFHandler<dim> &dof_handler = thermal_physics->get_dof_handler();
  // Use the Kelly error estimator to refine the mesh. This is done so that the
  // part of the domain that were heated stay refined.
  unsigned int const n_kelly_refinements =
      refinement_database.get("n_heat_refinements", 2);
  double coarsening_fraction = 0.3;
  double refining_fraction = 0.6;
  double cells_fraction = refinement_database.get("heat_cell_ratio", 1.);
  dealii::parallel::distributed::Triangulation<dim> &triangulation =
      dynamic_cast<dealii::parallel::distributed::Triangulation<dim> &>(
          const_cast<dealii::Triangulation<dim> &>(
              dof_handler.get_triangulation()));
  // Number of times the mesh on the beam paths will be refined and maximum
  // number of time a cell can be refined.
  unsigned int const n_beam_refinements =
      refinement_database.get("n_beam_refinements", 2);
  int max_level = refinement_database.get<int>("max_level");
  for (unsigned int i = 0; i < n_kelly_refinements; ++i)
  {
    // Estimate the error. For simplicity, always use dealii::QGauss
    dealii::Vector<float> estimated_error_per_cell =
        estimate_error(triangulation, dof_handler, fe_degree, solution);

    // Flag the cells for refinement.
    unsigned int new_n_cells = static_cast<unsigned int>(
        cells_fraction *
        static_cast<double>(triangulation.n_global_active_cells()));
    dealii::GridRefinement::refine_and_coarsen_fixed_fraction(
        triangulation, estimated_error_per_cell, refining_fraction,
        coarsening_fraction, new_n_cells);

    // Don't refine cells that are already as much refined as it is allowed.
    for (auto cell :
         dealii::filter_iterators(triangulation.active_cell_iterators(),
                                  dealii::IteratorFilters::LocallyOwnedCell()))
      if (cell->level() >= max_level)
        cell->clear_refine_flag();

    // Execute the refinement and transfer the solution onto the new mesh.
    refine_and_transfer(thermal_physics, dof_handler, solution);
  }

  // Refine the mesh along the trajectory of the sources.
  for (unsigned int i = 0; i < n_beam_refinements; ++i)
  {
    // Compute the cells to be refined.
    std::vector<typename dealii::parallel::distributed::Triangulation<
        dim>::active_cell_iterator>
        cells_to_refine =
            compute_cells_to_refine(triangulation, time, next_refinement_time,
                                    time_steps_refinement, electron_beams);

    // Flag the cells for refinement.
    for (auto &cell : cells_to_refine)
      if (cell->level() < max_level)
        cell->set_refine_flag();

    // Execute the refinement and transfer the solution onto the new mesh.
    refine_and_transfer(thermal_physics, dof_handler, solution);
  }

  // Recompute the inverse of the mass matrix
  thermal_physics->compute_inverse_mass_matrix();
}

template <int dim, typename MemorySpaceType>
void refine_mesh(
    std::unique_ptr<adamantine::Physics<dim, MemorySpaceType>> &thermal_physics,
    dealii::LA::distributed::Vector<double, MemorySpaceType> &solution,
    std::vector<std::unique_ptr<adamantine::ElectronBeam<dim>>> &electron_beams,
    double const time, double const next_refinement_time,
    unsigned int const time_steps_refinement,
    boost::property_tree::ptree const &refinement_database,
    unsigned int const fe_degree)
{
  switch (fe_degree)
  {
  case 1:
  {
    refine_mesh<dim, 1>(thermal_physics, solution, electron_beams, time,
                        next_refinement_time, time_steps_refinement,
                        refinement_database);
    break;
  }
  case 2:
  {
    refine_mesh<dim, 2>(thermal_physics, solution, electron_beams, time,
                        next_refinement_time, time_steps_refinement,
                        refinement_database);
    break;
  }
  case 3:
  {
    refine_mesh<dim, 3>(thermal_physics, solution, electron_beams, time,
                        next_refinement_time, time_steps_refinement,
                        refinement_database);
    break;
  }
  case 4:
  {
    refine_mesh<dim, 4>(thermal_physics, solution, electron_beams, time,
                        next_refinement_time, time_steps_refinement,
                        refinement_database);
    break;
  }
  case 5:
  {
    refine_mesh<dim, 5>(thermal_physics, solution, electron_beams, time,
                        next_refinement_time, time_steps_refinement,
                        refinement_database);
    break;
  }
  case 6:
  {
    refine_mesh<dim, 6>(thermal_physics, solution, electron_beams, time,
                        next_refinement_time, time_steps_refinement,
                        refinement_database);
    break;
  }
  case 7:
  {
    refine_mesh<dim, 7>(thermal_physics, solution, electron_beams, time,
                        next_refinement_time, time_steps_refinement,
                        refinement_database);
    break;
  }
  case 8:
  {
    refine_mesh<dim, 8>(thermal_physics, solution, electron_beams, time,
                        next_refinement_time, time_steps_refinement,
                        refinement_database);
    break;
  }
  case 9:
  {
    refine_mesh<dim, 9>(thermal_physics, solution, electron_beams, time,
                        next_refinement_time, time_steps_refinement,
                        refinement_database);
    break;
  }
  case 10:
  {
    refine_mesh<dim, 10>(thermal_physics, solution, electron_beams, time,
                         next_refinement_time, time_steps_refinement,
                         refinement_database);
    break;
  }
  default:
  {
    adamantine::ASSERT_THROW(false, "fe_degree should be between 1 and 10.");
  }
  }
}

template <int dim, typename MemorySpaceType>
dealii::LinearAlgebra::distributed::Vector<double, MemorySpaceType>
run(MPI_Comm const &communicator, boost::property_tree::ptree const &database,
    std::vector<adamantine::Timer> &timers)
{
  // Extract property tree children
  boost::property_tree::ptree geometry_database =
      database.get_child("geometry");
  boost::property_tree::ptree discretization_database =
      database.get_child("discretization");
  boost::property_tree::ptree time_stepping_database =
      database.get_child("time_stepping");
  boost::property_tree::ptree post_processor_database =
      database.get_child("post_processor");
  boost::property_tree::ptree refinement_database =
      database.get_child("refinement");

  unsigned int const fe_degree =
      discretization_database.get<unsigned int>("fe_degree");
  std::string quadrature_type =
      discretization_database.get("quadrature", "gauss");
  std::transform(quadrature_type.begin(), quadrature_type.end(),
                 quadrature_type.begin(),
                 [](unsigned char c) { return std::tolower(c); });
  double const initial_temperature =
      database.get("materials.initial_temperature", 300.);

  adamantine::Geometry<dim> geometry(communicator, geometry_database);
  std::unique_ptr<adamantine::Physics<dim, MemorySpaceType>> thermal_physics;
  std::vector<std::unique_ptr<adamantine::ElectronBeam<dim>>> &electron_beams =
      initialize_thermal_physics<dim>(fe_degree, quadrature_type, communicator,
                                      database, geometry, thermal_physics);
  adamantine::PostProcessor<dim> post_processor(
      communicator, post_processor_database, thermal_physics->get_dof_handler(),
      thermal_physics->get_material_property());

  thermal_physics->setup_dofs();
  thermal_physics->compute_inverse_mass_matrix();
  dealii::LA::distributed::Vector<double, MemorySpaceType> solution;
  thermal_physics->initialize_dof_vector(initial_temperature, solution);
  unsigned int progress = 0;
  unsigned int cycle = 0;
  unsigned int n_time_step = 0;
  double time = 0.;
  // Output the initial solution
  dealii::AffineConstraints<double> &affine_constraints =
      thermal_physics->get_affine_constraints();
  output_pvtu(post_processor, cycle, n_time_step, time, affine_constraints,
              solution);
  ++n_time_step;

  bool const verbose_refinement = refinement_database.get("verbose", false);
  unsigned int const time_steps_refinement =
      refinement_database.get("time_steps_between_refinement", 10);
  double next_refinement_time = time;
  double time_step = time_stepping_database.get<double>("time_step");
  double const duration = time_stepping_database.get<double>("duration");
  while (time < duration)
  {
    if ((time + time_step) > duration)
      time_step = duration - time;
    unsigned int rank = dealii::Utilities::MPI::this_mpi_process(communicator);

    // Refine the mesh after time_steps_refinement time steps or when time is
    // greater or equal than the next predicted time for refinement. This is
    // necessary when using an embedded method.
    if (((n_time_step % time_steps_refinement) == 0) ||
        (time >= next_refinement_time))
    {
      next_refinement_time = time + time_steps_refinement * time_step;
      timers[adamantine::refine].start();
      refine_mesh(thermal_physics, solution, electron_beams, time,
                  next_refinement_time, time_steps_refinement,
                  refinement_database, fe_degree);
      timers[adamantine::refine].stop();
      if ((rank == 0) && (verbose_refinement == true))
        std::cout << "n_dofs: " << thermal_physics->get_dof_handler().n_dofs()
                  << std::endl;
    }

    // time can be different than time + time_step if an embedded scheme is
    // used.
    timers[adamantine::evol_time].start();
    time = thermal_physics->evolve_one_time_step(time, time_step, solution,
                                                 timers);
    timers[adamantine::evol_time].stop();

    // Get the new time step
    time_step = thermal_physics->get_delta_t_guess();

    // Output progress on screen
    if (rank == 0)
    {
      double adim_time = time / (duration / 10.);
      double int_part = 0;
      std::modf(adim_time, &int_part);
      if (int_part > progress)
      {
        std::cout << int_part * 10 << '%' << " completed" << std::endl;
        ++progress;
      }
    }

    // Output the solution
    output_pvtu(post_processor, cycle, n_time_step, time, affine_constraints,
                solution);
    ++n_time_step;
  }
  post_processor.output_pvd();

  // This is only used for integration test
  return solution;
}
#endif


/********************************************************************
	created:	2013/01/10
	created:	10:1:2013   9:15
	filename: 	SimpleFinder.cpp
	file path:	GAG\src\GAGPL\SPECTRUM
	file base:	SimpleFinder
	file ext:	cpp
	author:		Han Hu

	purpose:
*********************************************************************/

#include <algorithm>
#include "GAGPL/SPECTRUM/SimpleFinder.h"
#include <boost/foreach.hpp>
#include <boost/lambda/lambda.hpp>

namespace gag
{
	SimpleFinder::SimpleFinder(RichList& spec)
		: spectrum(spec), param(Param::Instance()), pt(PeriodicTable::Instance())
	{
		int precursor_charge = param.getParameter<int>("precursor_charge").first;

		double pre_mz = param.getParameter<double>("precursor_mz").first;
		precursor_mass = calculateMass(pre_mz, precursor_charge);

		/*
		Guess the range of average formula. Usually we assume the difference between composition estimated from the base peak and the one estimated from monoisotopic peak can be safely ignored.
			No Sulfur (100D)		High Sulfur (100D)
			C: 3.7238523        1.9961864
			H: 5.4425534        2.9942796
			O: 2.8645018        3.1938983
			N: 0.2864502        0.1996186
			S: 0                0.5988559
		*/
		no_sulfur_ave.insert(std::make_pair("C", 3.7238523));
		no_sulfur_ave.insert(std::make_pair("H", 5.4425534));
		no_sulfur_ave.insert(std::make_pair("O", 2.8645018));
		no_sulfur_ave.insert(std::make_pair("N", 0.2864502));
		no_sulfur_ave.insert(std::make_pair("S", 0.0));

		max_sulfur_ave.insert(std::make_pair("C", 1.9961864));
		max_sulfur_ave.insert(std::make_pair("H", 2.9942796));
		max_sulfur_ave.insert(std::make_pair("O", 3.1938983));
		max_sulfur_ave.insert(std::make_pair("N", 0.1996186));
		max_sulfur_ave.insert(std::make_pair("S", 0.5988559));
		run();
	}

	void SimpleFinder::run()
	{
		// There is a need to divide the spectrum into separate islands at this moment.
		RichPeakListBySignalOverNoise& pks_sn = spectrum.getPeakListByType<peak_signal_noise>();

		// The S/N threshold for monoisotopic peak.
		double mono_sn_threshold = param.getParameter<double>("signal_noise").first;

		RichPeakListBySignalOverNoise::iterator end_sn = pks_sn.upper_bound(mono_sn_threshold);
		RichPeakListBySignalOverNoise::iterator iter_sn = pks_sn.begin();
		//end_iter--;

		// Start from the peak with highest s/n
		for(; iter_sn != end_sn; iter_sn++) {
			this->findNextEnvelop(spectrum, *iter_sn);
		}

		std::cout << "Before optimizing, output all the envelop information..." << std::endl;
		BOOST_FOREACH(EnvelopPtr env, env_pool)
			this->printEnvelop(env);

		std::cout << std::endl;

		//std::cout << "After optimizing..." << std::endl;
		//Estimate parameters for each envelop and update the peak information.
		this->optimizeEnvelopSet(env_pool);
		this->optimizeSuspiciousPeaks();

	}

	void SimpleFinder::findNextEnvelop(RichList& pk_list, RichPeakPtr base_pk, bool raw_spec /* = true */)
	{
		// Remove the neighbor peak.
		std::cout << "Check if peak " << base_pk->mz << " is in the black list" << std::endl;

		if(!base_pk->pk_status) {
			std::cout << "Peak " << base_pk->mz << " is a noise peak." << std::endl;
			return;
		}

		double mz = base_pk->mz;

		// Process the apodization problem.
		this->cleanNeighborNoise(pk_list, base_pk);

		int precursor_charge = param.getParameter<int>("precursor_charge").first;

		double signal_noise = param.getParameter<double>("signal_noise").first;

		int sign = (precursor_charge > 0 ? 1 : -1);
		bool suspicious_pk = true;

		for(int z = abs(precursor_charge); z>=1; z--) {

			// The actual mass is allowed to be larger than precursor mass.
			//double mass = calculateMass((*iter_intensity)->mz, sign * z);
			//if(mass > precursor_mass) continue;

			// Not a qualified peak due to its poor intensity.
			if(base_pk->signal_noise < signal_noise) continue;

			std::cout << "Identify base peak: " << base_pk->mz << " Charge: " << z;
			std::cout << (precursor_charge > 0 ? "+" : "-") << std::endl;

			bool status = this->extendEnvelop(pk_list, base_pk, z, raw_spec);

			if(status) {
				suspicious_pk = false;
				// If the S/N of the base peak is higher than 200.
				if(base_pk->signal_noise > 200)
					this->detectHarmonicCluster(base_pk, sign * z);
			}

		}

		if(suspicious_pk)
			setPeakType(base_pk, "ISO");
	}

	void SimpleFinder::optimizeEnvelopSet(std::set<EnvelopPtr>& env_set)
	{
		// Start from low m/z to high m/z.
		RichList pk_list = env_ref.getRichPeakList(env_set);
		RichPeakListByMZ& pk_mz = pk_list.getPeakListByType<peak_mz>();

		// Guess the number of sulfate.
		BOOST_FOREACH(EnvelopPtr env, env_set)
			this->updateEnvelopParameter(env);

		for(RichPeakListByMZ::iterator mz_iter = pk_mz.begin();
			mz_iter != pk_mz.end(); mz_iter++)
		{
			RichPeakPtr pk = *mz_iter;
			double mz = pk->mz;
			std::cout << "Check peak " << pk->mz << std::endl;

			// 1. For each peak, check if there is any new envelop occurred at this peak.
			std::vector<EnvEntry> new_env_entries = env_ref.getOccurredEnvelopEntries(pk, NEW);
			if(new_env_entries.size() == 0)
				continue;

			// Update the fitting score for all the new envelop set.
			BOOST_FOREACH(EnvEntry& env_entry, new_env_entries) {
				this->calculateFittingScore(env_entry.env);
			}

			// Check if this is a peak from previously identified envelop.
			std::vector<EnvEntry> old_env_entries = env_ref.getOccurredEnvelopEntries(pk, OLD);

			// 1. Sort the envelop vector by their fitting scores.
			// Since the value of the fitting score will be frequently updated,				// it might not be a good idea to keep it in a multi-index container
			std::sort(new_env_entries.begin(), new_env_entries.end(), EnvEntry::scoreLarger);

			// Check if the new envelops are actually just subset of the old
			// envelops.
			if(old_env_entries.size() != 0) {
				// a. Declared as abnormal by old_env_entry.
				// b. Identified new peak.
				// c. Meet the threshold of the envelop.
				BOOST_FOREACH(EnvEntry& new_entry, new_env_entries)
				{
						bool fake_envelop = false;
						BOOST_FOREACH(EnvEntry old_entry, old_env_entries)
						{
							bool false_positive = this->isRedundantEnvelop(new_entry.env, old_entry.env, old_entry.getShift());

							// If this peak is considered by any of the old envelops as								// a true one, move to the next step.
							if(false_positive) {
								fake_envelop = true;
								break;
							}
						}

						if(fake_envelop) new_entry.info->entry_status = FALSE;
				}
			}

			/* Rule of thumb */
			// 1. If there is only one new envelop, just accept the new envelop.
			if(new_env_entries.size()==1) {
				// No longer new envelop.
				if(new_env_entries[0].info->entry_status != FALSE) {
					// Identify if this is a noise envelop.
					if(!isNoiseEnvelop(new_env_entries[0].env))
						this->acceptEnvelop(new_env_entries[0].env);

				}
				continue;
			}

			// 2. If there is more than one new envelop, which means multiple charge states, select the one with higher fitting score as the basis, sequentially merge the other into it, and see if the overall fitting score will be improved.

			// Upgrade the score of the system.
			// Calculate the previous system score.
			double system_score = 0.0;

			// 2. Sequentially merge one into the basis and decide if the performance is improved.
			std::vector<EnvEntry> added_entries;
			BOOST_FOREACH(EnvEntry& new_entry, new_env_entries)
			{
				if(new_entry.info->entry_status == FALSE)
					continue;

				// Assign the fitting score to the system score.
				if(system_score == 0.0) {
					system_score = new_entry.env->fitting_score;
					this->acceptEnvelop(new_entry.env);

					added_entries.push_back(new_entry);
					continue;
				}

				// 2.a Find the corresponding fitting score. Only considering the situation where there are multiple charge states for the same mono-isotopic peak.
				double new_score = updateEnvelopTree(old_env_entries, added_entries, new_entry, pk);

				// Decide if the envelop is proper to be added into the
				// envelop set.
				if(new_score <= system_score) {
					new_entry.info->entry_status = FALSE;
				}else {
					/* Accept the candidate envelop.*/
					// No longer new envelop.
					this->acceptEnvelop(new_entry.env);
					added_entries.push_back(new_entry);
					system_score = new_score;
				}
			}

		}
	}

	bool SimpleFinder::extendEnvelop( RichList& pk_list, RichPeakPtr base_pk, int charge, bool raw_spec)
	{
		// a. Using A+1 and A+2 peak to identify the most likely composition.
		// b. Try to extend the peak using the estimated composition.
		// c. Notice that sometimes, the A+2 peak might have missing information due to the defective peak picking algorithm.
		// d. This algorithm requires that A peak has to present.

		/* A + 1 peak */
		// Assume it follows the no sulfate model, get the boundary.
		// Guess the number of sulfate. Notice that there might be overlapping.
		// Calculate the no sulfate model. Notice that the information has been corrected by charge.

		AggregatedIsotopicVariants higher_theo = this->estimateDistribution(base_pk->mz, charge);
		//AggregatedIsotopicVariants lower_theo = this->estimateDistribution(base_pk->mz, charge, ave2);

		// Get the isotope information.
		const Isotope s32 = pt.getIsotopeByNominalMass("S", 32);
		const Isotope s34 = pt.getIsotopeByNominalMass("S", 34);
		const Isotope c12 = pt.getIsotopeByNominalMass("C", 12);
		const Isotope c13 = pt.getIsotopeByNominalMass("C", 13);
		const Isotope h = pt.getIsotopeByNominalMass("H",1);
		double electron_mass = param.getParameter<double>("electron_mass").first;
		double low_sn_threshold = param.getParameter<double>("lower_bound_sn").first;

		// The difference between mass of H and peak(A -> A+1). This variable is for re-use convinience.
		double mz_diff = (h.mass + electron_mass)/(double)charge - higher_theo.getMassDifferenceByShift<peak_intensity>(0, 1);

		// Temporary variable for storing the peak information.
		// 1. Shift;
		// 2. Peak;
		// 3. Status.
		std::multimap<int, std::pair<RichPeakPtr, std::string> > pk_map;
		pk_map.insert(std::make_pair(0, std::make_pair(base_pk, "Normal")));

		int shift = 1;

		while(1) {
			// Theoretically, the (A+n)' should always be present.
			double mz0 = base_pk->mz + higher_theo.getMassDifferenceByShift<peak_intensity>(0,shift);

			std::string status = "Normal";
			if(shift == 1) {
				std::set<RichPeakPtr> pks_set = this->getClosestRichPeaks(pk_list, mz0, raw_spec, low_sn_threshold);

				if(pks_set.size() == 0) { // C+H peak for different shift.

					double mz1 = mz0 + mz_diff;

					pks_set = this->getClosestRichPeaks(pk_list, mz1, raw_spec, low_sn_threshold);
					status = "C+H";

					if(pks_set.size() == 0 && shift > 1) { // C+2H
						double mz2 = mz0 + 2.0 * mz_diff;
						pks_set = this->getClosestRichPeaks(pk_list, mz2, raw_spec, low_sn_threshold);
						status = "C+2H";

						if(pks_set.size() == 0)
							break;
					} else if(pks_set.size() == 0 && shift == 1) {
						break;
					}

				}

				BOOST_FOREACH(RichPeakPtr pk, pks_set) {
					pk_map.insert(std::make_pair(shift, std::make_pair(pk, status)));
				}

			} else if(shift == 2) {
				// If 34S peak is present.
				double mz1 = base_pk->mz + (s34.mass - s32.mass)/(double)charge;
				std::set<RichPeakPtr> pks_set1 = this->getClosestRichPeaks(pk_list, mz1, raw_spec, low_sn_threshold);

				BOOST_FOREACH(RichPeakPtr pk, pks_set1) {
					pk_map.insert(std::make_pair(shift, std::make_pair(pk, "Sulfur")));
				}

				// (A+n)' peak.
				std::set<RichPeakPtr> pks_set2 = this->getClosestRichPeaks(pk_list, mz0, raw_spec, low_sn_threshold);

				BOOST_FOREACH(RichPeakPtr pk, pks_set2) {
					pk_map.insert(std::make_pair(shift, std::make_pair(pk, "Normal")));
				}

				if(pks_set1.size() == 0 && pks_set2.size() == 0) {
					// C+H peak.
					double mz2 = mz0 + mz_diff;

					std::set<RichPeakPtr> pks_set3 = this->getClosestRichPeaks(pk_list, mz2, raw_spec, low_sn_threshold);
					BOOST_FOREACH(RichPeakPtr pk, pks_set3) {
						pk_map.insert(std::make_pair(shift, std::make_pair(pk, "C+H")));
					}

					if(pks_set3.size() == 0) {
						// C + 2H
						double mz3 = mz0 + 2.0 * mz_diff;
						std::set<RichPeakPtr> pks_set4 = this->getClosestRichPeaks(pk_list, mz3, raw_spec, low_sn_threshold);
						BOOST_FOREACH(RichPeakPtr pk, pks_set4) {
							pk_map.insert(std::make_pair(shift, std::make_pair(pk, "C+2H")));
						}
						if(pks_set4.size() == 0)
							break;

					}
				}

			} else { // Shift >= 2.
				// Left boundary.
				int sulfur_multiplier = shift / 2;
				size_t count = 0;
				for(int i = sulfur_multiplier; i>=0; i--) {
					int carbon_multiplier = shift - i * 2;
					double mzx = base_pk->mz + i * (s34.mass - s32.mass)/(double)charge + carbon_multiplier * (c13.mass - c12.mass)/(double)charge;
					std::set<RichPeakPtr> pks_set = this->getClosestRichPeaks(pk_list, mzx, raw_spec, low_sn_threshold);
					count += pks_set.size();
					BOOST_FOREACH(RichPeakPtr pk, pks_set) {
						pk_map.insert(std::make_pair(shift, std::make_pair(pk, "C+S")));
					}

          // Also consider the H shift.
          double mz1 = mzx + mz_diff;
          pks_set = this->getClosestRichPeaks(pk_list, mz1, raw_spec, low_sn_threshold);
          if(pks_set.size() != 0) {
            status = "C+H";
            BOOST_FOREACH(RichPeakPtr pk, pks_set) {
              pk_map.insert(std::make_pair(shift, std::make_pair(pk, status)));
            }
            //pk_map.insert(std::make_pair(shift, std::make_pair(pk, "C+S")));
          }
				}
				// No peak has been found in this range.
				if(count == 0) {
          break;
        }

			}

			shift++;
		}

		// No additional peak has been found.
		if(pk_map.size() == 1)
			return false;

		EnvelopPtr env_ptr = createEnvelop(charge);

		double param_confidence = 0.0;

		std::multimap<int, std::pair<RichPeakPtr, std::string> >::iterator map_iter = pk_map.begin();
		for(; map_iter != pk_map.end(); map_iter++)
		{
			int pk_pos = map_iter->first;
			RichPeakPtr pk_ptr = map_iter->second.first;
			this->cleanNeighborNoise(pk_list, pk_ptr);
			std::string pk_status = map_iter->second.second;

			EntryStatus entry_status = (pk_pos == 0 ? NEW : OLD);

			InfoPeakPtr pk_infor = boost::make_shared<InfoPeak>(pk_ptr->intensity, pk_status, entry_status);

			env_ref.addDictionaryReference(pk_ptr, env_ptr, pk_pos, pk_infor);
		}

		int max_shift = shift - 1;
		env_ptr->max_shift = max_shift;

		std::cout << "Found candidate envelop -- mono peak: " << base_pk->mz << " charge: " << env_ptr->charge_state << std::endl;
		env_pool.insert(env_ptr);

		//calculateFittingScore(env_ptr);

		return true;

	}

	AggregatedIsotopicVariants SimpleFinder::estimateDistribution( double mz, int charge, bool sulfur)
	{
		double mass = calculateMass(mz, charge);
		double coef = mass / 100.0;
		Composition compo;

		if(sulfur) {
			// Correct sulfur number.
			AveragineFormulae::iterator sulfur_iter = max_sulfur_ave.find("S");
			int num = (int)ceil(sulfur_iter->second * coef);

			double new_mass = mass - (double)num * Composition("SO3").getMass();

			double new_coef = new_mass / 100.0;

			AveragineFormulae::iterator iter = max_sulfur_ave.begin();
			for(; iter != max_sulfur_ave.end(); iter++) {
				if(iter->first != "S") {
					compo.addElement(iter->first, (int)floor(iter->second * new_coef + 0.5));
				} else {
					compo.addElement(iter->first, num);
				}
			}
			IsotopicDistribution iso(compo);
			return iso.getAggregatedIsotopicVariants(charge);
		} else {
			AveragineFormulae::iterator iter = no_sulfur_ave.begin();
			for(; iter != no_sulfur_ave.end(); iter++)
				compo.addElement(iter->first, (int)floor(iter->second * coef + 0.5));

			IsotopicDistribution iso(compo);
			return iso.getAggregatedIsotopicVariants(charge);
		}

	}

	AggregatedIsotopicVariants SimpleFinder::estimateDistribution( double mz, int charge, int sulfur_num )
	{
		double mass = calculateMass(mz, charge);
		double new_mass = mass - (double)sulfur_num * Composition("SO3").getMass();

		double new_coef = new_mass / 100.0;

		AveragineFormulae::iterator iter = no_sulfur_ave.begin();
		Composition compo;
		for(; iter != no_sulfur_ave.end(); iter++) {
			if(iter->first == "S") {
				compo.addElement(iter->first, sulfur_num);
			} else if(iter->first == "O") {
				compo.addElement(iter->first, sulfur_num * 3);
			} else {
				compo.addElement(iter->first, (int)floor(iter->second * new_coef + 0.5));
			}
		}
		IsotopicDistribution iso(compo);
    //AggregatedIsotopicVariants agr_var = iso.getAggregatedIsotopicVariants(charge);
		return iso.getAggregatedIsotopicVariants(charge);
	}

	std::set<RichPeakPtr> SimpleFinder::getClosestRichPeaks(RichList& pk_list, double expected_mz, bool raw_spec, double sn_threshold, double error)
	{
		RichPeakListByMZ& pks_mz = pk_list.getPeakListByType<peak_mz>();

		RichPeakListByMZ::iterator mz_increase_iter, mz_decrease_iter;
		mz_increase_iter = pks_mz.lower_bound(expected_mz);
		mz_decrease_iter = mz_increase_iter;

		std::set<RichPeakPtr> pk_set;
		std::map<double, RichPeakPtr> pk_map;

		// Examine the peak towards two directions.
		while(1) {
			if(mz_increase_iter == pks_mz.end())
				break;

			RichPeakPtr pk = *mz_increase_iter;

			bool meet_threshold = raw_spec ? true : (pk->signal_noise > sn_threshold);

			double win_error = this->getWindowError(pk, error);

			if(abs(pk->mz - expected_mz) < win_error) {
				if(meet_threshold)
					pk_map.insert(std::make_pair(pk->intensity, pk));
			} else {
				break;
			}

			mz_increase_iter++;
			if(mz_increase_iter == pks_mz.end())
				break;
		}

		mz_decrease_iter--;
		while(1) {
			RichPeakPtr pk = *mz_decrease_iter;

			double win_error = this->getWindowError(pk, error);

			bool meet_threshold = raw_spec ? true : (pk->signal_noise > sn_threshold);

			if(abs(pk->mz - expected_mz) < win_error)	{
				if(meet_threshold)
					pk_map.insert(std::make_pair(pk->intensity, pk));
			} else {
				break;
			}

			if(mz_decrease_iter == pks_mz.begin())
				break;
			else
				mz_decrease_iter--;
		}

		//pk_set.insert(selected_pk);
		if(pk_map.size()>0) {
			RichPeakPtr selected_pk = pk_map.rbegin()->second;
			pk_set.insert(selected_pk);
		}

		return pk_set;
	}

	void SimpleFinder::updateEnvelopParameter( EnvelopPtr env )

	{
		// 0. Check env id.
		unsigned int env_id = env->id;
		std::cout << "Check env #" << env_id << std::endl;

		// 1. Get all the peaks.
		EnvEntry base_entry = env_ref.getEntryByShift(env, 0);

		std::vector<EnvEntry> entry_vec = env_ref.getEntryByEnvelop(env);

		AggregatedIsotopicVariants higher_theo = this->estimateDistribution(base_entry.pk->mz, env->charge_state);
		AggregatedIsotopicVariants lower_theo = this->estimateDistribution(base_entry.pk->mz, env->charge_state, true);

		int max_sulfur = this->guessMaxSulfurNumber(base_entry.pk->mz, env->charge_state);
		int max_num = param.getParameter<int>("max_sulfur_num").first;

		const Isotope s34 = pt.getIsotopeByNominalMass("S", 34);

		std::pair<int, int> sulfur_range = std::make_pair(0,max_sulfur);

		// Gradually estimate the sulfur number. The default value is 0.
		BOOST_FOREACH(EnvEntry entry, entry_vec)
		{
			// For given shift, estimate the range of the intensities.
			int pk_pos = entry.getShift();

			if(pk_pos == 0) continue;

			// Expected abundance.
			double non_sulfur_model_abundance = base_entry.pk->intensity * higher_theo.getPeakByShift<peak_intensity>(pk_pos)->intensity;
			double sulfur_model_abundance = base_entry.pk->intensity * lower_theo.getPeakByShift<peak_intensity>(pk_pos)->intensity;

			double max_abundance = non_sulfur_model_abundance;
			double min_abundance = sulfur_model_abundance;
			if(min_abundance > max_abundance)
				swap(max_abundance, min_abundance);

			// Narrow down the range of sulfur_range.
			// For tandem ms analysis, only use A+1 and A+2 peak.
			// The information from A+1 peak might help increase the lower limit
			if(pk_pos == 1) { // odd peak, increase the lower limit of sulfur number.

				if(entry.info->status != "Normal") continue;

				if((entry.info->adjusted_abundance-max_abundance)/max_abundance >0.2 ) {
					// Significantly higher than upper boundary.
					// Nothing to do with the range.
				} else if((entry.info->adjusted_abundance-min_abundance)/min_abundance < -0.2) {
					// Significantly lower than expected.
					// env->suspicious = true;
				} else {
					// Normal situation. Estimate sulfur_num from the abundance.
					double lambda = (entry.info->adjusted_abundance-min_abundance)/(max_abundance - min_abundance);

					// Convert lambda to sulfur_num.
					if(sulfur_model_abundance == min_abundance)
						lambda = 1.0 - lambda;

					int sulfur_num = (int)floor(lambda * max_sulfur + 0.5);

					//entry.env->sulfur_num = sulfur_num > max_num ? max_num : sulfur_num;
					if(sulfur_num < sulfur_range.second)
						sulfur_range.second = sulfur_num;

				}
			} else if(pk_pos == 2) { // even peak, decrease the upper limit of sulfur number. Notice that one of the peak might be missing.
				if(entry.info->status == "Sulfur") {
					// TBD: decide if the sulfur peak and non-sulfur peak will separate.
					// If present, estimate sulfur using this information.
					int sulfur_num = (int)floor(entry.info->adjusted_abundance/base_entry.info->adjusted_abundance / pow(s34.abundance, pk_pos/2) + 0.5);

					if(sulfur_num > sulfur_range.first && sulfur_num < max_num)
						sulfur_range.first = sulfur_num;
					//entry.env->sulfur_num = sulfur_num > max_num ? max_num : sulfur_num;
					// The information from sulfur peak have enough confidence.
					break;

				} else if(entry.info->status == "Normal") {
					// Assume the peak and 34S peak are able to be resolved.
					// Define the intensity range for current peak.

					// Expected abundance of the non-sulfur-peak.
					double non_sulfur_peak_abundance = sulfur_model_abundance - base_entry.info->adjusted_abundance * pow(s34.abundance, pk_pos);
					// Do the same thing for A+1 peak.
					max_abundance = non_sulfur_model_abundance;
					min_abundance = non_sulfur_peak_abundance;

					if(max_abundance < min_abundance)
						swap(max_abundance, min_abundance);

					if((entry.info->adjusted_abundance - max_abundance)/max_abundance > 0.2) {
						// Significantly higher than expected.
						// Do nothing.
					} else if((entry.info->adjusted_abundance - min_abundance)/min_abundance < -0.2) {
						// Significantly lower than expected.
						// Do nothing.
					} else {

						double lambda = (entry.info->adjusted_abundance-min_abundance)/(max_abundance - min_abundance);

						// Sometimes, the distance between max_abundance and min_abundance is too small, which will cause a lot of troubles.
						if(lambda < 0.0) continue;

						if(non_sulfur_peak_abundance == min_abundance)
							lambda = 1.0 - lambda;

						// Convert lambda to sulfur_num.
						int sulfur_num = (int)floor(lambda * max_sulfur + 0.5);
						if(sulfur_num < sulfur_range.second)
							sulfur_range.second = sulfur_num;
						//entry.env->sulfur_num = sulfur_num > max_num ? max_num : sulfur_num;
					}

				} else {
					// C+H peak. Nothing to do with the sulfur number.
				}
			}
		}

		// Get the sulfur number estimation by averaing all possible
		env->sulfur_num = (sulfur_range.first + sulfur_range.second)/2;

		// Unlikely situation.
		if(sulfur_range.first > sulfur_range.second)
			env->env_status = FP;

		// Update the theoretical distribution for current envelop.
		env->theo_dist = this->estimateDistribution(base_entry.pk->mz, env->charge_state, env->sulfur_num);

		std::map<int, PeakPtr> pk_map = env->getTheoreticalPeaks();

		for(std::map<int, PeakPtr>::iterator iter = pk_map.begin();
			iter != pk_map.end(); iter++)
		{
			// Theo peak information.
			int shift = iter->first; PeakPtr theo_pk = iter->second;
			double theo_abundance = base_entry.info->adjusted_abundance * theo_pk->intensity;

			// No need to estimate base peak.
			if(shift == 0) continue;

			// Be careful of the situation that the shift for the envelop cannot be observed.
			std::vector<EnvEntry> exp_pks = env_ref.getPeaksByShift(env, shift);

			// Depending on the status of the peak, the theoretical information will be recalculated. It might be possible that some of the peaks are missing, it is better to calculate the mean of the shift for each position.
			// If the sulfur number is 0, we assume there is no peak split.
			if(env->sulfur_num == 0) {
				BOOST_FOREACH(EnvEntry& pk_entry, exp_pks) {
					if(pk_entry.info->status == "Normal") {
						pk_entry.info->relative_shift = (pk_entry.info->adjusted_abundance - theo_abundance) / theo_abundance;
					}
				}
			} else {
				if(shift == 1 || shift == 2) {
					BOOST_FOREACH(EnvEntry& pk_entry, exp_pks) {
						if(shift == 1) {
							if(pk_entry.info->status == "Normal") {
								pk_entry.info->relative_shift = (pk_entry.info->adjusted_abundance - theo_abundance)/ theo_abundance;
							}
						} else if(shift == 2){
							// Here we assume the peak will split.
							if(pk_entry.info->status == "Sulfur") {
								// Calculate the theoretical abundance and update the relative shift.
								theo_abundance = base_entry.info->adjusted_abundance * pow(s34.abundance, env->sulfur_num);
								pk_entry.info->relative_shift = (pk_entry.info->adjusted_abundance - theo_abundance) / theo_abundance;
							} else if(pk_entry.info->status == "Normal"){
								// Notice that theo_abundance should be larger than 0.
								theo_abundance = base_entry.info->adjusted_abundance * (theo_pk->intensity - pow(s34.abundance, env->sulfur_num));
								pk_entry.info->relative_shift = (pk_entry.info->adjusted_abundance - theo_abundance) / theo_abundance;
							} else {
								// C+H peak. Nothing to do with the relative shift.
							}
						}
					}
				} else {
					// A + n peak
					double total_shift_abundance = 0.0;
					BOOST_FOREACH(EnvEntry& pk_entry, exp_pks) {
						if(pk_entry.info->status == "Normal"
							|| pk_entry.info->status == "Sulfur") {
								total_shift_abundance += pk_entry.info->adjusted_abundance;
						}
					}
					BOOST_FOREACH(EnvEntry& pk_entry, exp_pks) {
						pk_entry.info->relative_shift = (pk_entry.info->adjusted_abundance - theo_abundance)/ theo_abundance;
					}
				}
			}

		}

	}

	bool SimpleFinder::isRedundantEnvelop( EnvelopPtr new_env, EnvelopPtr old_env, int old_shift)
	{
		bool state = false;

    std::set<RichPeakPtr> old_pks = env_ref.getPeaksByEnvelop(old_env);
    std::set<RichPeakPtr> new_pks = env_ref.getPeaksByEnvelop(new_env);

    if(std::includes(old_pks.begin(), old_pks.end(), new_pks.begin(), new_pks.end())) {
      // Calculate the sub fitting score of the subset from old_pks.
      if(new_env->fitting_score < old_env->fitting_score)
        state = true;

    } else {
      if(new_env->fitting_score < old_env->fitting_score && (old_env->charge_state % new_env->charge_state == 0))
        state = true;

      EnvEntry base_entry = env_ref.getEntryByShift(old_env, 0);
      //  //EnvEntry new_base = env_ref.getEntryByShift(new_env, 0);
      EnvEntry current_entry = env_ref.getEntryByShift(old_env, old_shift);
      //  // Check if it is different from theoretical abundance.
      AggregatedIsotopicVariants higher_theo = this->estimateDistribution(base_entry.pk->mz, old_env->charge_state);
      AggregatedIsotopicVariants lower_theo = this->estimateDistribution(base_entry.pk->mz, old_env->charge_state, true);

      double non_sulfur_abundance = higher_theo.getPeakByShift<peak_intensity>(old_shift)->intensity * base_entry.info->adjusted_abundance;
      double sulfur_abundance = lower_theo.getPeakByShift<peak_intensity>(old_shift)->intensity * base_entry.info->adjusted_abundance;

      double expected_abundance(0.0);
      if(current_entry.info->status == "Normal") {
        expected_abundance = std::max(non_sulfur_abundance,sulfur_abundance);
      } else {
        if(current_entry.info->status == "Sulfur")
          state = true;

        return state;
      }

      double upper_shift = (current_entry.pk->intensity - expected_abundance)/(expected_abundance);

      double max_intensity_shift = param.getParameter<double>("max_intensity_shift").first;

      if(upper_shift < max_intensity_shift)
      	state = true;
    }

		return state;
	}

	void SimpleFinder::printEnvelop( EnvelopPtr env )
	{
		std::cout << "Envelop ID: " << env->id << std::endl;
		std::cout << "Charge: " << env->charge_state << std::endl;
    std::cout << "Fitting score: " << env->fitting_score << std::endl;
		std::cout << std::endl;
		// Peak information.
		std::cout << "Covered peaks:" << std::endl;
		std::cout << "Shift\tMZ\tEXP_ABD\tSTATUS\tSTATUS2" << std::endl;

		// Get all peaks.
		std::vector<EnvEntry> pk_entries = env_ref.getEntryByEnvelop(env);
		std::cout.precision(5);
		BOOST_FOREACH(EnvEntry& pk_entry, pk_entries)
		{
			std::cout << pk_entry.getShift() << "\t" ;
			std::cout << std::fixed << pk_entry.pk->mz << "\t";
			std::cout	<< pk_entry.pk->intensity << "\t" << pk_entry.info->status << std::endl;
		}
		std::cout << std::endl;
	}

	std::multimap<RichPeakPtr, EnvelopPtr> SimpleFinder::getEnvelops(EntryStatus status)
	{
		std::vector<EnvEntry> entry_vec = env_ref.getBaseEntriesByStatus();

		//EnvDictByEnvID& env_by_eid = env_
		std::multimap<RichPeakPtr, EnvelopPtr> env_map;
		BOOST_FOREACH(EnvEntry& env_entry, entry_vec)
		{
			// Get the base peak for the envelop.
			RichPeakPtr base_pk = env_entry.pk;
			env_map.insert(std::make_pair(base_pk, env_entry.env));
		}
		return env_map;
	}

	double SimpleFinder::getWindowError( RichPeakPtr pk, double error )
	{
		// If error is specified.
		if(error != -1.0) return error * pk->mz;

		double internal_accuracy = param.getParameter<double>("internal_accuracy").first;

		//double resolving_power = pk->resolution;
		//double coef = pk->mz / resolving_power;
		double coef = (pk->signal_noise > 15 && pk->mz > 400.0) ? 1.2 : 1.5;
    //double coef = 1.5;
		return 2e-6 * internal_accuracy * pk->mz * coef;
	}

	bool SimpleFinder::isNoiseEnvelop( EnvelopPtr env )
	{
		unsigned int env_id = env->id;
		// Identification of noise envelop.
		// Situation 1. Weired cluster shape. envelop size has to be larger than 3. This is not noise envelop.
		std::set<int> shift_set = env_ref.getShiftSet(env);
		RichPeakPtr base_pk = env_ref.getBasePeakForEnvelop(env);

		if(shift_set.size() > 2) {
			BOOST_FOREACH(int shift, shift_set)
			{
				std::vector<EnvEntry> pk_entry_vec = env_ref.getPeaksByShift(env, shift);
				BOOST_FOREACH(EnvEntry& pk_entry, pk_entry_vec)
				{
					if(pk_entry.info->relative_shift < -0.3) {
						env->env_status = MISC;
						// Misc envelop is not noise envelop.
						return false;
					}

				}
			}
		}

		// Situation 2. Matching to noise peak.
		// 2.a size == 2
		if(shift_set.size() == 2) {
			// Get A+1 peak.
			std::vector<EnvEntry> pk_entry_vec = env_ref.getPeaksByShift(env, 1);
			if(pk_entry_vec.size() == 1 && pk_entry_vec[0].info->status == "C+H") {
				// Get theoretical abundance.
				double theo_abundance = env_ref.getTheoreticalPeak(env, 1)->intensity;
				if(theo_abundance - pk_entry_vec[0].info->adjusted_abundance > 0)
					return true;
			}

			RichPeakPtr base_pk = env_ref.getBasePeakForEnvelop(env);
			std::vector<EnvEntry> old_env_entries = env_ref.getOccurredEnvelopEntries(base_pk, OLD);
			// 2.b A + 1 peak is significantly lower than expected.
			if(pk_entry_vec[0].info->relative_shift < -0.2 || old_env_entries.size() > 0)		{
				return true;
			}
		}

		return false;

	}

	void SimpleFinder::cleanNeighborNoise( RichList& pk_list, RichPeakPtr pk/*, std::set<RichPeakPtr>& blacklist*/ )
	{
		RichPeakListByMZ& pks_mz = pk_list.getPeakListByType<peak_mz>();

		RichPeakListByMZ::iterator mz_increase_iter, mz_decrease_iter;
		mz_increase_iter = pks_mz.lower_bound(pk->mz);
		mz_decrease_iter = mz_increase_iter;

		mz_increase_iter++;

		double win_error = 0.012;

		while(1) {
			if(mz_increase_iter == pks_mz.end())
				break;

			RichPeakPtr neighbor_pk = *mz_increase_iter;

			if(abs(neighbor_pk->mz - pk->mz) < win_error && (neighbor_pk->intensity / pk->intensity < 0.1)) {
				//blacklist.insert(neighbor_pk);
				//neighbor_pk->pk_status = false;
				this->setNoisePeak(neighbor_pk);
			} else if(abs(neighbor_pk->mz - pk->mz) >= win_error)
				break;

			mz_increase_iter++;
			if(mz_increase_iter == pks_mz.end())
				break;
		}

		// This is the begin of the peak list.
		if(mz_decrease_iter == pks_mz.begin())
			return;

			mz_decrease_iter--;
		while(1) {
			RichPeakPtr neighbor_pk = *mz_decrease_iter;

			if(abs(neighbor_pk->mz - pk->mz) < win_error && (neighbor_pk->intensity / pk->intensity < 0.1)) {

				//neighbor_pk->pk_status = false;
				this->setNoisePeak(neighbor_pk);
			} else if(abs(neighbor_pk->mz - pk->mz) >= win_error)
				break;

			if(mz_decrease_iter == pks_mz.begin())
				break;
			else
				mz_decrease_iter--;
		}
	}

	double SimpleFinder::updateEnvelopTree( std::vector<EnvEntry>& old_entries, std::vector<EnvEntry>& added_entries, EnvEntry& test_entry, RichPeakPtr pk )
	{
		double rest_intensity = 0.0;
		BOOST_FOREACH(EnvEntry& entry, old_entries)
			rest_intensity += entry.info->adjusted_abundance;

		rest_intensity = test_entry.pk->intensity - rest_intensity;

		// Construct the vector using theoretical isotopic distributions. Scale the second one and get the best fit.
		double scaling_factor = 0.0;
		double scaling_range[2] = {0.0, 1.0};

		double total_score = this->calculateLinearAssociationScore(added_entries);

		std::vector<EnvEntry> temp_entries = *(&added_entries);
		temp_entries.push_back(test_entry);

		while(1)
		{
			// Based on scaling factor, update the peak abundance.
			std::vector<EnvEntry> pk_entry_vec = env_ref.getEntryByEnvelop(test_entry.env);
			BOOST_FOREACH(EnvEntry& pk_entry, pk_entry_vec)
			{
				pk_entry.info->adjusted_abundance *= scaling_factor;
			}

			BOOST_FOREACH(EnvEntry& env_entry, added_entries)
			{
				std::vector<EnvEntry> pk_entry_vec2 = env_ref.getEntryByEnvelop(env_entry.env);
				BOOST_FOREACH(EnvEntry& pk_entry, pk_entry_vec2)
				{
					pk_entry.info->adjusted_abundance *= (1-scaling_factor);
				}
			}

			// Update scaling factor. It depends on the peaks associated with the second test_entry.
			double new_score = this->calculateLinearAssociationScore(temp_entries);

			double ratio0 = this->calculateScalingRatio(temp_entries);
			double ratio1 = this->calculateScalingRatio(test_entry);

			int scale_index = ratio0 > ratio1 ? 1 : 0;

			// Keep a copy of the current scaling factor.
			double old_scaling_factor = scaling_factor;

			// Update the scaling factor of the new envelop.
			scaling_factor = (scaling_range[scale_index] + scaling_factor)/2.0;

			// Update the boundary of the scale range.
			scaling_range[1-scale_index] = old_scaling_factor;

			if(abs(new_score - total_score) < 1e-6)
				break;
			else
				total_score = new_score;
		}

		double total_fitting_score = 0.0;
		BOOST_FOREACH(EnvEntry& env_entry, temp_entries) {
			// Reset the scaling factor to 1.
			env_entry.env->scaling_factor = 1.0;
			total_fitting_score += this->calculateFittingScore(env_entry.env);
		}
		return total_fitting_score;

	}

	double SimpleFinder::calculateFittingScore( EnvelopPtr env )
	{
		// Get base entry.
		double fitting_score = 0.0;

		std::set<int> shift_set = env_ref.getShiftSet(env);

		// If sulfur peak is present, re-estimate the abundance.

		BOOST_FOREACH(int shift, shift_set)
		{
			double expected_abundance = env_ref.getTheoreticalPeak(env, shift)->intensity;
			double experimental_abundance = env_ref.getExperimentalAbundanceByShift(env, shift);

			// The fitting score comes from 3 parts:
			//1. the shift of intensity: abs(E-T)/T.
			double intensity_shift = (expected_abundance - experimental_abundance)/ expected_abundance;
			if(intensity_shift > 0.0 && intensity_shift <= 1.0)
				intensity_shift = 1.0 - intensity_shift;
			else if(intensity_shift <= 0.0 && intensity_shift >= -1.0)
				intensity_shift = sqrt(1.0 + intensity_shift);
			else if(intensity_shift > 1)
				intensity_shift = 1.0;
			else
				intensity_shift = 0.0;

			// 2. the shift of m/z. Since this is for high resolution data, the shift of m/z should be ignored.

			// 3. the square root of expected_abundance.
			double weight = sqrt(expected_abundance);

			fitting_score += weight * intensity_shift;
		}

		env->fitting_score = fitting_score;
		return fitting_score;

	}

	double SimpleFinder::calculateFittingScore(std::set<EnvelopPtr>& env_set)
	{
		double total_fitting_score = 0.0;
		BOOST_FOREACH(EnvelopPtr env, env_set)
			total_fitting_score += this->calculateFittingScore(env);
		return total_fitting_score;
	}

	double SimpleFinder::calculateLinearAssociationScore( std::vector<EnvEntry>& entries )
	{
		// Calculate the value of cos theta.
		double inner_prod = 0.0; double norm_term[2] = {0.0,0.0};
		BOOST_FOREACH(EnvEntry& env_entry, entries)
		{
			std::set<int> shift_set = env_ref.getShiftSet(env_entry.env);

			BOOST_FOREACH(int shift, shift_set)
			{
				// Calculate the accumulated abundance for multiple peaks mapped into the same shift position.
				std::vector<EnvEntry> pk_entry_vec = env_ref.getPeaksByShift(env_entry.env, shift);
				double accumulated_abundance = 0.0;
				BOOST_FOREACH(EnvEntry& pk_entry, pk_entry_vec)
					accumulated_abundance += pk_entry.info->adjusted_abundance;

				double theo_abundance = env_entry.env->getTheoreticalPeak(shift)->intensity;
				inner_prod += theo_abundance * accumulated_abundance;

				norm_term[0] += pow(theo_abundance, 2);
				norm_term[1] += pow(accumulated_abundance, 2);

			}
		}

		return inner_prod / (sqrt(norm_term[0]) * sqrt(norm_term[1]));

	}

	double SimpleFinder::calculateScalingRatio( std::vector<EnvEntry>& entries )
	{
		double adjusted_total = 0.0; double theo_total = 0.0;

		BOOST_FOREACH(EnvEntry& env_entry, entries)
		{
			std::set<int> shift_set = env_ref.getShiftSet(env_entry.env);
			BOOST_FOREACH(int shift, shift_set) {
				// Get the acutal peaks for given position. There might be more than 1 which can be mapped into one peak.
				std::vector<EnvEntry> pk_entry_vec = env_ref.getPeaksByShift(env_entry.env, shift);

				BOOST_FOREACH(EnvEntry& pk_entry, pk_entry_vec)
					adjusted_total += pk_entry.info->adjusted_abundance;

				// There will be only one theoretical peak for each shift position.
				theo_total += env_entry.env->getTheoreticalPeak(shift)->intensity;
			}
		}

		return adjusted_total / theo_total;
	}

	double SimpleFinder::calculateScalingRatio( EnvEntry& env_entry )
	{
		// Compare the total intensity of the suggested envleop over the theoretical intensity.
		double adjusted_total = 0.0; double theo_total = 0.0;

		std::set<int> shift_set = env_ref.getShiftSet(env_entry.env);
		BOOST_FOREACH(int shift, shift_set) {
			// Get the acutal peaks for given position. There might be more
			// than 1 which can be mapped into one peak.
			std::vector<EnvEntry> pk_entry_vec = env_ref.getPeaksByShift(env_entry.env, shift);

			BOOST_FOREACH(EnvEntry& pk_entry, pk_entry_vec)
				adjusted_total += pk_entry.info->adjusted_abundance;

			// There will be only one theoretical peak for each shift position.
			theo_total += env_entry.env->getTheoreticalPeak(shift)->intensity;
		}

		return adjusted_total/theo_total;

	}

	int SimpleFinder::guessMaxSulfurNumber( double mz, int charge_state )
	{
		double mass = calculateMass(mz, charge_state);
		double coef = mass / 100.0;

		AveragineFormulae::iterator sulfur_iter = max_sulfur_ave.find("S");
		return (int)ceil(sulfur_iter->second * coef);
	}

	void SimpleFinder::acceptEnvelop( EnvelopPtr env )
	{
		// 1. Set the status of the envelop as true positive.
		env->env_status = TP;
		// 2. Set all the peaks associated to the envelop as OLD and the peak type as ENV.
		std::vector<EnvEntry> pk_entry_vec = env_ref.getEntryByEnvelop(env);

		BOOST_FOREACH(EnvEntry& pk_entry, pk_entry_vec) {
			pk_entry.info->entry_status = OLD;
			setPeakType(pk_entry.pk, "ENV");
			//pk_entry.pk->pk_type = "ENV";
		}

	}

	void SimpleFinder::setNoisePeak( RichPeakPtr pk )
	{
		pk->pk_status = false;
		setPeakType(pk, "NOISE");
	}

	void SimpleFinder::optimizeSuspiciousPeaks()
	{
		// 1. For each suspicious peak, iterate over all charge state, try to decide if there is any peak that can follow into the range.
		RichPeakListBySignalOverNoise& pks_sn = spectrum.getPeakListByType<peak_signal_noise>();

		// The S/N threshold for monoisotopic peawk.
		double mono_sn_threshold = param.getParameter<double>("signal_noise").first;

		RichPeakListBySignalOverNoise::iterator end_sn = pks_sn.upper_bound(mono_sn_threshold);
		RichPeakListBySignalOverNoise::iterator iter = pks_sn.begin();
		//RichPeakListBySignalOverNoise::iterator iter;

		/*RichPeakListByType& pks_type = spectrum.getPeakListByType<peak_type>();

		std::pair<RichPeakListByType::iterator, RichPeakListByType::iterator> p = pks_type.equal_range("ISO");*/

		int precursor_z = param.getParameter<int>("precursor_charge").first;
		int charge_sign = precursor_z > 0 ? 1 : -1;

		//for(RichPeakListByType::iterator iter = p.first; iter != p.second; iter++)
		for(; iter != end_sn; iter++ )
		{
			RichPeakPtr base_pk = *iter;
			if(base_pk->pk_type != "ISO")
			{
				continue;
			}

			int count = 0;
			for(int z = 1; z < abs(precursor_z); z++)
			{
				// Consider only A+1 peak.
				std::pair<PeakPtr, PeakPtr> pk_pair = getPeakRangeByShift(base_pk, charge_sign*z, 1, -1);
				double mz0 = pk_pair.second->mz;
				std::set<RichPeakPtr> pk_set = getClosestRichPeaks(spectrum, mz0, true, 0.0);

				if(pk_set.size() == 0)
					continue;

				// Estimate the intensity shift.
				RichPeakPtr pk0 = *(pk_set.begin());

				double intensity_shift = (pk0->intensity - pk_pair.first->intensity) / pk_pair.first->intensity;

				if(intensity_shift > -0.2) {
					count++;
					// A candidate envelop.
					und_pk.insert(std::make_pair(base_pk, z));
				}

			}

			if(count == 0)
				setPeakType(base_pk, "NOISE");

		}
	}

	std::pair<PeakPtr, PeakPtr> SimpleFinder::getPeakRangeByShift( RichPeakPtr base_pk, int charge, int shift, int sulfur /*= -1*/ )
	{
		double pre_mass = calculateMass(base_pk->mz, charge);

		AggregatedIsotopicVariants iso_no_sulfur = this->estimateDistribution(base_pk->mz, charge, 0);
		int max_sulfur = (sulfur == -1 ? guessMaxSulfurNumber(base_pk->mz, charge) : sulfur);
		AggregatedIsotopicVariants iso_max_sulfur = this->estimateDistribution(base_pk->mz, charge, max_sulfur);

		double mz_max_sulfur = base_pk->mz + iso_max_sulfur.getMassDifferenceByShift<peak_intensity>(0, shift)/(float)abs(charge);
		double mz_no_sulfur = base_pk->mz + iso_no_sulfur.getMassDifferenceByShift<peak_intensity>(0, shift)/(float)abs(charge);

		double int_max_sulfur = base_pk->intensity * iso_max_sulfur.getPeakByShift<peak_intensity>(shift)->intensity;
		double int_no_sulfur = base_pk->intensity * iso_no_sulfur.getPeakByShift<peak_intensity>(shift)->intensity;

		PeakPtr pk_max_sulfur = boost::make_shared<Peak>(mz_max_sulfur, int_max_sulfur);
		PeakPtr pk_no_sulfur = boost::make_shared<Peak>(mz_no_sulfur, int_no_sulfur);

		return std::make_pair(pk_max_sulfur, pk_no_sulfur);

	}

	void SimpleFinder::setPeakType( RichPeakPtr pk, const std::string& new_type )
	{
		// 1. Find the iterator of the peak.
		RichPeakListByID& pks_id = spectrum.getPeakListByType<peak_id>();
		RichPeakListByID::iterator id_iter = pks_id.find(pk->id);

		// 2. Project the iterator to type.
		RichPeakListByType::iterator type_iter = spectrum.getPeakContainer().project<peak_type>(id_iter);

		// 2. Modify the type of the element referred by the iterator.
		spectrum.modifyType(type_iter, new_type);
	}

	void SimpleFinder::detectHarmonicCluster(RichPeakPtr base_pk, int charge)
	{
		//std::set<RichPeakPtr> harmonic_cluster;

		// The S/N threshold for monoisotopic peawk.
		double mono_sn_threshold = param.getParameter<double>("signal_noise").first;

		/*double cur_mz = param.getParameter<double>("precursor_mz").first;
		int cur_charge = param.getParameter<int>("precursor_charge").first;*/

		double cur_mass = calculateMass(base_pk->mz, charge);

		int coef = 2;
		// Get the minimum m/z.
		RichPeakListByMZ& pks_mz = spectrum.getPeakListByType<peak_mz>();
		double min_mz = (*(pks_mz.begin()))->mz;

		while(1)
		{
			// Get new m/z based on adjusted charge state.

			int new_charge = coef * charge;
			double new_mz = calculateMZ(cur_mass, new_charge, charge);

			coef++;

			if(new_mz < min_mz)
				break;

			// Find the base peak.
			std::set<RichPeakPtr> pk_set = getClosestRichPeaks(spectrum, new_mz, false, mono_sn_threshold, 3e-5);

			if(pk_set.size() == 0)
				continue;

			RichPeakPtr base_pk = *(pk_set.begin());
			this->cleanNeighborNoise(spectrum, base_pk);

			if(base_pk->signal_noise < mono_sn_threshold)
				continue;

			bool status = extendEnvelop(spectrum, base_pk, abs(new_charge), false);

			if(status) {
				//harmonic_cluster.insert(base_pk);
				std::cout << "Find harmonic cluster: " << base_pk->mz << " " << new_charge << std::endl;
			}
		}

		//return harmonic_cluster;
	}

}


#ifndef SPLX_PIECEWISECURVE_QP_GENERATOR_HPP
#define SPLX_PIECEWISECURVE_QP_GENERATOR_HPP

#include <qp_wrappers/problem.hpp>
#include <splx/opt/QPOperations.hpp>
#include <splx/opt/BezierQPOperations.hpp>
#include <splx/types.hpp>
#include <splx/curve/PiecewiseCurve.hpp>
#include <splx/opt/BezierQPOperations.hpp>
#include <absl/strings/str_cat.h>
#include <Eigen/StdVector>

namespace splx {

template<typename T, unsigned int DIM>
class PiecewiseCurveQPGenerator {
public:
    using _BezierQPOperations = BezierQPOperations<T, DIM>;
    using _Problem = QPWrappers::Problem<T>;
    using Row = splx::Row<T>;
    using VectorDIM = splx::VectorDIM<T, DIM>;
    using Vector = splx::Vector<T>;
    using Hyperplane = splx::Hyperplane<T, DIM>;
    using AlignedBox = splx::AlignedBox<T, DIM>;
    using Index = splx::Index;
    using Matrix = splx::Matrix<T>;
    using _QPOperations = QPOperations<T, DIM>;
    using _PiecewiseCurve = PiecewiseCurve<T, DIM>;
    using StdVectorVectorDIM
        = std::vector<VectorDIM, Eigen::aligned_allocator<VectorDIM>>;
    using Constraint = splx::Constraint<T>;

    PiecewiseCurveQPGenerator() : m_problem(0) {}

    void addPiece(std::shared_ptr<_QPOperations> opt_ptr) {
        m_operations.push_back(opt_ptr);

        if(m_cumulativeMaxParameters.empty()) {
            m_cumulativeMaxParameters.push_back(opt_ptr->maxParameter());
        } else {
            m_cumulativeMaxParameters.push_back(
                m_cumulativeMaxParameters.back() + opt_ptr->maxParameter()
            );
        }

        if(m_cumulativeDecisionVars.empty()) {
            m_cumulativeDecisionVars.push_back(opt_ptr->numDecisionVariables());
        } else {
            m_cumulativeDecisionVars.push_back(
                m_cumulativeDecisionVars.back()
                + opt_ptr->numDecisionVariables()
            );
        }

        m_problem = QPWrappers::Problem<T>(this->numDecisionVariables());
    }

    void setPiece(std::size_t idx, std::shared_ptr<_QPOperations> opt_ptr) {
        m_operations[idx] = opt_ptr;
        this->fixCumulativeStructures(idx);
        m_problem = QPWrappers::Problem<T>(this->numDecisionVariables());
    }

    void removePiece(std::size_t idx) {
        m_operations.erase(m_operations.begin() + idx);
        m_cumulativeDecisionVars.erase(
                m_cumulativeDecisionVars.begin() + idx
        );
        m_cumulativeMaxParameters.erase(
                m_cumulativeMaxParameters.begin() + idx
        );
        this->fixCumulativeStructures(idx);
        m_problem = QPWrappers::Problem<T>(this->numDecisionVariables());
    }

    void removeAllPieces() {
        m_operations.clear();
        m_cumulativeMaxParameters.clear();
        m_cumulativeDecisionVars.clear();
        m_problem = QPWrappers::Problem<T>(0);
    }

    void addBezier(Index ncpts, T a) {
        auto bezptr = std::make_shared<_BezierQPOperations>(ncpts, a);
        auto optptr = std::static_pointer_cast<_QPOperations>(bezptr);
        this->addPiece(optptr);
    }

    void setBezier(std::size_t idx, Index ncpts, T a) {
        auto bezptr = std::make_shared<_BezierQPOperations>(ncpts, a);
        auto optptr = std::static_pointer_cast<_QPOperations>(bezptr);
        this->setPiece(idx, optptr);
    }

    std::size_t numPieces() const {
        return m_cumulativeDecisionVars.size();
    }

    std::vector<T> pieceMaxParameters() const {
        std::vector<T> piece_max_params;
        for(const auto piece_opt : m_operations) {
            piece_max_params.push_back(piece_opt->maxParameter());
        }
        return piece_max_params;
    }

    T maxParameter() const {
        if(m_operations.empty()) {
            throw std::domain_error(
                absl::StrCat(
                    "piecewise curve has no pieces"
                )
            );
        }

        return m_cumulativeMaxParameters.back();
    }

    void setPieceMaxParameters(const std::vector<T>& new_max_params) {
        if(new_max_params.size() != m_operations.size()) {
            throw std::domain_error(
                absl::StrCat(
                    "new max parameters size does not match number of pieces",
                    ", number of pieces: ",
                    m_operations.size(),
                    ", given number of max parameters: ",
                    new_max_params.size()
                )
            );
        }

        for(std::size_t i = 0; i < m_operations.size(); i++) {
            m_operations[i]->maxParameter(new_max_params[i]);
        }

        this->fixCumulativeStructures(0);
        m_problem = QPWrappers::Problem<T>(this->numDecisionVariables());
    }

    Index numDecisionVariables() const {
        if(m_cumulativeDecisionVars.empty()) return 0;

        return m_cumulativeDecisionVars.back();
    }

    void addIntegratedSquaredDerivativeCost(unsigned int k, T lambda) {
        Matrix Q(this->numDecisionVariables(), this->numDecisionVariables());
        Vector c(this->numDecisionVariables());
        Q.setZero();
        c.setZero();

        for(std::size_t i = 0; i < this->numPieces(); i++) {
            Index dvar_start_idx = (i == 0 ? 0 : m_cumulativeDecisionVars[i-1]);
            Index dvar_count = m_operations[i]->numDecisionVariables();

            auto [Qs, cs]
                = m_operations[i]->integratedSquaredDerivativeCost(k, lambda);

            Q.block(dvar_start_idx, dvar_start_idx, dvar_count, dvar_count) = Qs;
            c.block(dvar_start_idx, 0, dvar_count, 1) = cs;
        }

        m_problem.add_Q(Q);
        m_problem.add_c(c);
    }

    void addEvalCost(T u, unsigned int k, const VectorDIM& target, T lambda) {
        Matrix Q(this->numDecisionVariables(), this->numDecisionVariables());
        Vector c(this->numDecisionVariables());
        Q.setZero();
        c.setZero();

        auto [idx, param, first_dvar_index, dvar_count] = pieceInfo(u);
        auto [Qs, cs] = m_operations[idx]->evalCost(param, k, target, lambda);
        Q.block(first_dvar_index, first_dvar_index,
                dvar_count, dvar_count) = Qs;
        c.block(first_dvar_index, 0, dvar_count, 1) = cs;

        m_problem.add_Q(Q);
        m_problem.add_c(c);
    }

    void addEvalConstraint(T u, unsigned int k, const VectorDIM& target,
                           bool soft_convertible = false,
                           T soft_convertible_weight = T(1)) {
        auto [idx, param, first_dvar_index, dvar_count] = pieceInfo(u);
        auto constraints = m_operations[idx]->evalConstraint(param, k, target,
                soft_convertible, soft_convertible_weight);

        this->addConstraints(constraints, first_dvar_index, dvar_count);
    }

    void addHyperplaneConstraintForPiece(std::size_t idx, const Hyperplane& hp,
                                         bool soft_convertible = false,
                                         T soft_convertible_weight = T(1)) {
        Index first_dvar_index =
                (idx == 0 ? 0 : m_cumulativeDecisionVars[idx-1]);
        Index dvar_count = m_operations[idx]->numDecisionVariables();

        auto constraints = m_operations[idx]->hyperplaneConstraintAll(hp,
                                  soft_convertible, soft_convertible_weight);

        this->addConstraints(constraints, first_dvar_index, dvar_count);
    }

    void addHyperplaneConstraintAll(const Hyperplane& hp,
                                    bool soft_convertible = false,
                                    T soft_convertible_weight = T(1)) {
        for(std::size_t i = 0; i < this->numPieces(); i++) {
            this->addHyperplaneConstraintForPiece(i, hp,
                                  soft_convertible, soft_convertible_weight);
        }
    }

    void addHyperplaneConstraintAt(T u, const Hyperplane& hp,
                                   bool soft_convertible = false,
                                   T soft_convertible_weight = T(1)) {
        auto [idx, param, first_dvar_index, dvar_count] = pieceInfo(u);
        auto constraints = m_operations[idx]->hyperplaneConstraintAt(param, hp,
                                 soft_convertible, soft_convertible_weight);
        this->addConstraints(constraints, first_dvar_index, dvar_count);
    }

    void addBoundingBoxConstraint(const AlignedBox& bbox) {
        for(std::size_t i = 0; i < this->numPieces(); i++) {
            auto [lbx, ubx] = m_operations[i]->boundingBoxConstraint(bbox);
            Index first_dvar_index =
                    (i == 0 ? 0 : m_cumulativeDecisionVars[i-1]);
            Index dvar_count = m_operations[i]->numDecisionVariables();

            assert(lbx.rows() == dvar_count);

            for(Index j = 0; j < dvar_count; j++) {
                m_problem.set_var_limits(first_dvar_index + j, lbx(j), ubx(j));
            }
        }
    }

    // adds continueity constraint between piece idx and piece idx + 1
    // in their kth derivatives
    void addContinuityConstraint(std::size_t idx, unsigned int k,
                                 bool soft_convertible = false,
                                 T soft_convertible_weight = T(1)) {
        Index first_piece_numdvars = m_operations[idx]->numDecisionVariables();
        Index second_piece_numdvars =
                m_operations[idx+1]->numDecisionVariables();
        Index first_piece_dvars_start
            = (idx == 0 ? 0 : m_cumulativeDecisionVars[idx-1]);
        Index second_piece_dvars_start = m_cumulativeDecisionVars[idx];

        for(unsigned int d = 0; d < DIM; d++) {
            Row coeff1 = m_operations[idx]->evalBasisRow(
                            d, m_operations[idx]->maxParameter(), k
            );
            Row coeff2 = m_operations[idx+1]->evalBasisRow(
                            d, 0, k
            );

            Row coeff(this->numDecisionVariables());
            coeff.setZero();
            coeff.block(0, first_piece_dvars_start, 1, first_piece_numdvars)
                                = coeff1;
            coeff.block(0, second_piece_dvars_start, 1, second_piece_numdvars)
                                = -coeff2;

            m_problem.add_constraint(coeff, 0, 0, soft_convertible, soft_convertible_weight);
        }
    }

    Vector getDVarsForSegments(const StdVectorVectorDIM& segments) const {
        if(segments.size() != this->numPieces() + 1) {
            throw std::domain_error
            (
                absl::StrCat
                (
                    "number of segments is not equal to the number of pieces",
                    ", segment count: ",
                    segments.size() - 1,
                    ", piece count: ",
                    this->numPieces()
                )
            );
        }

        Vector res(this->numDecisionVariables());

        for(std::size_t i = 0; i < segments.size() - 1; i++) {
            Index piece_dvars_start
                = (i == 0 ? 0: m_cumulativeDecisionVars[i-1]);
            Index piece_numdvars = m_operations[i]->numDecisionVariables();
            res.block(piece_dvars_start, 0, piece_numdvars, 1)
                = m_operations[i]->getDVarsForSegment
            (
                        segments[i],
                        segments[i+1]
            );
        }

        return res;
    }
    _PiecewiseCurve extractCurve(const Vector& soln) {
        if(soln.rows() != this->numDecisionVariables()) {
            throw std::domain_error(
                absl::StrCat(
                    "number of decision variables does not match. given: ",
                    soln.rows(),
                    ", required: ",
                    this->numDecisionVariables()
                )
            );
        }

        _PiecewiseCurve piecewise;
        for(std::size_t i = 0; i < this->numPieces(); i++) {
            Index piece_numdvars = m_operations[i]->numDecisionVariables();
            Index piece_dvars_start =
                    (i==0 ? 0 : m_cumulativeDecisionVars[i-1]);

            Vector dvars = soln.block(piece_dvars_start, 0, piece_numdvars, 1);
            piecewise.addPiece(m_operations[i]->extractCurve(dvars));
        }

        return piecewise;
    }

    void resetProblem() {
        m_problem.reset();
    }

    void resetGenerator() {
        m_operations.clear();
        m_cumulativeMaxParameters.clear();
        m_cumulativeDecisionVars.clear();
        this->resetProblem();
    }

    const QPWrappers::Problem<T>& getProblem() const {
        return this->m_problem;
    }

private:
    std::vector<std::shared_ptr<_QPOperations>> m_operations;
    std::vector<T> m_cumulativeMaxParameters;
    std::vector<Index> m_cumulativeDecisionVars;
    QPWrappers::Problem<T> m_problem;

    /*
    * fix cumulative structures starting from the given index
    */
    void fixCumulativeStructures(std::size_t idx) {
        for(std::size_t i = idx; i < this->numPieces(); i++) {
            if(i == 0) {
                m_cumulativeMaxParameters[i] = m_operations[i]->maxParameter();
                m_cumulativeDecisionVars[i] =
                    m_operations[i]->numDecisionVariables();
            } else {
                m_cumulativeMaxParameters[i] =
                      m_operations[i]->maxParameter()
                    + m_cumulativeMaxParameters[i-1];
                m_cumulativeDecisionVars[i] =
                      m_operations[i]->numDecisionVariables()
                    + m_cumulativeDecisionVars[i-1];
            }
        }
    }

    /*
    * Given a parameter, return the piece index, piece parameter corresponding
    * to the given parameter, first decision variable index and decision
    * variable count for the piece
    */
    std::tuple<std::size_t, T, Index, Index> pieceInfo(T param) const {
        std::size_t idx = std::lower_bound(
                    m_cumulativeMaxParameters.begin(),
                    m_cumulativeMaxParameters.end(),
                    param) - m_cumulativeMaxParameters.begin();

        Index first_dvar_index = 0;
        Index dvar_count = m_operations[idx]->numDecisionVariables();
        if(idx != 0) {
            param -= m_cumulativeMaxParameters[idx-1];
            first_dvar_index = m_cumulativeDecisionVars[idx-1];
        }

        return {
            idx,
            std::min(param, m_operations[idx]->maxParameter()),
            first_dvar_index,
            dvar_count
        };
    }

    void addConstraints(const std::vector<Constraint>& cons,
                        Index first_dvar_index, Index dvar_count) {

        Row coeff(this->numDecisionVariables());

        for(const auto& constraint: cons) {
            coeff.setZero();
            coeff.block(0, first_dvar_index, 1, dvar_count)
                        = constraint.coeff;
            m_problem.add_constraint(coeff,
                                     constraint.lb,
                                     constraint.ub,
                                     constraint.soft_convertible,
                                     constraint.soft_weight
            );
        }
    }

};

}

#endif


// This file is part of OpenMVG, an Open Multiple View Geometry C++ library.

// Copyright (c) 2012, 2013 Pierre MOULON.

// This Source Code Form is subject to the terms of the Mozilla Public
// License, v. 2.0. If a copy of the MPL was not distributed with this
// file, You can obtain one at http://mozilla.org/MPL/2.0/.

#ifndef OPENMVG_NUMERIC_LM_HPP
#define OPENMVG_NUMERIC_LM_HPP

// Levenberg Marquardt Non Linear Optimization
#include <Eigen/Core>

namespace openMVG
{
using namespace Eigen;

/**
 * @brief Generic functor Levenberg-Marquardt minimization
 * @tparam _Scalar Type of internal computation
 * @tparam NX Number of values per sample at compile time
 * @tparam NY Number of samples at compile time
 */
template<typename _Scalar, int NX = Dynamic, int NY = Dynamic>
struct Functor
{
  using Scalar = _Scalar;
  enum
  {
    InputsAtCompileTime = NX,
    ValuesAtCompileTime = NY
  };
  using InputType = Matrix<Scalar, InputsAtCompileTime, 1>;
  using ValueType = Matrix<Scalar, ValuesAtCompileTime, 1>;
  using JacobianType = Matrix<Scalar, ValuesAtCompileTime, InputsAtCompileTime>;

  /// Number of values per sample
  const int m_inputs;

  // Number of sample
  const int m_values;

  /**
  * @brief Default constructor
  */
  Functor()
    : m_inputs( InputsAtCompileTime ),
      m_values( ValuesAtCompileTime )
  {

  }

  /**
  * @brief Constructor
  * @param inputs Number of column per sample
  * @param values Number of sample
  */
  Functor( int inputs, int values ) : m_inputs( inputs ), m_values( values ) {}

  /**
  * @brief Get number of samples
  * @return Number of samples
  */
  int inputs() const
  {
    return m_inputs;
  }

  /**
  * @brief Get number of samples
  * @return Number of samples
  */
  int values() const
  {
    return m_values;
  }

  // you should define that in the subclass :
  //  void operator() (const InputType& x, ValueType* v, JacobianType* _j=0) const;
};

}; // namespace openMVG

#endif // OPENMVG_NUMERIC_LM_HPP