// --------------------------------------------------------------------------
//                   OpenMS -- Open-Source Mass Spectrometry
// --------------------------------------------------------------------------
// Copyright The OpenMS Team -- Eberhard Karls University Tuebingen,
// ETH Zurich, and Freie Universitaet Berlin 2002-2015.
//
// This software is released under a three-clause BSD license:
//  * Redistributions of source code must retain the above copyright
//    notice, this list of conditions and the following disclaimer.
//  * Redistributions in binary form must reproduce the above copyright
//    notice, this list of conditions and the following disclaimer in the
//    documentation and/or other materials provided with the distribution.
//  * Neither the name of any author or any participating institution
//    may be used to endorse or promote products derived from this software
//    without specific prior written permission.
// For a full list of authors, refer to the file AUTHORS.
// --------------------------------------------------------------------------
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL ANY OF THE AUTHORS OR THE CONTRIBUTING
// INSTITUTIONS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
// OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
// WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
// OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
// ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//
// --------------------------------------------------------------------------
// $Maintainer: David Wojnar $
// $Authors: David Wojnar $
// --------------------------------------------------------------------------

#include <OpenMS/APPLICATIONS/TOPPBase.h>
#include <OpenMS/MATH/STATISTICS/PosteriorErrorProbabilityModel.h>
#include <OpenMS/FORMAT/IdXMLFile.h>
#include <OpenMS/CONCEPT/Exception.h>
#include <boost/math/special_functions/fpclassify.hpp> // for "isnan"
#include <vector>

using namespace OpenMS;
using namespace Math; //PosteriorErrorProbabilityModel
using namespace std;

//-------------------------------------------------------------
//Doxygen docu
//-------------------------------------------------------------

/**
    @page TOPP_IDPosteriorErrorProbability IDPosteriorErrorProbability

    @brief  Tool to estimate the probability of peptide hits to be incorrectly assigned.

             potential predecessor tools
             \f$ \longrightarrow \f$ IDPosteriorErrorProbability \f$ \longrightarrow \f$
             potential successor tools

             @ref TOPP_MascotAdapter (or other ID engines)
             @ref TOPP_ConsensusID

    @experimental This tool has not been tested thoroughly and might behave not as expected!

    By default an estimation is performed using the (inverse) Gumbel distribution for incorrectly assigned sequences
    and a Gaussian distribution for correctly assigned sequences. The probabilities are calculated by using Bayes' law, similar to PeptideProphet.
    Alternatively, a second Gaussian distribution can be used for incorrectly assigned sequences.
    At the moment, IDPosteriorErrorProbability is able to handle X! Tandem, Mascot, MyriMatch and OMSSA scores.

    No target/decoy information needs to be provided, since the model fits are done on the mixed distribution.

    In order to validate the computed probabilities an optional plot output can be generated.
    There are two parameters for the plot:
    The scores are plotted in the form of bins. Each bin represents a set of scores in a range of '(highest_score - smallest_score) / number_of_bins' (if all scores have positive values).
    The midpoint of the bin is the mean of the scores it represents.
    The parameter 'out_plot' should be used to give the plot a unique name. Two files are created. One with the binned scores and one with all steps of the estimation.
    If parameter @p top_hits_only is set, only the top hits of each peptide identification are used for the estimation process.
    Additionally, if 'top_hits_only' is set, target/decoy information is available and a @ref TOPP_FalseDiscoveryRate run was performed previously, an additional plot will be generated with target and decoy bins ('out_plot' must not be empty).
    A peptide hit is assumed to be a target if its q-value is smaller than @p fdr_for_targets_smaller.
    The plots are saved as a Gnuplot file. An attempt is made to call Gnuplot, which will create a PDF file containing all steps of the estimation. If this fails, the user has to run Gnuplot manually - or adjust the PATH environment such that Gnuplot can be found and retry.

    @note Currently mzIdentML (mzid) is not directly supported as an input/output format of this tool. Convert mzid files to/from idXML using @ref TOPP_IDFileConverter if necessary.

    The command line parameters of this tool are:
    @verbinclude TOPP_IDPosteriorErrorProbability.cli
    INI file documentation of this tool:
    @htmlinclude TOPP_IDPosteriorErrorProbability.html

    For the parameters of the algorithm section see the algorithms documentation: @n
        @ref OpenMS::Math::PosteriorErrorProbabilityModel "fit_algorithm" @n

*/

// We do not want this class to show up in the docu:
/// @cond TOPPCLASSES

class TOPPIDPosteriorErrorProbability :
  public TOPPBase
{
public:
  TOPPIDPosteriorErrorProbability() :
    TOPPBase("IDPosteriorErrorProbability", "Estimates probabilities for incorrectly assigned peptide sequences and a set of search engine scores using a mixture model.")
  {

  }

protected:
  void registerOptionsAndFlags_()
  {
    registerInputFile_("in", "<file>", "", "input file ");
    setValidFormats_("in", ListUtils::create<String>("idXML"));
    registerOutputFile_("out", "<file>", "", "output file ");
    setValidFormats_("out", ListUtils::create<String>("idXML"));
    registerOutputFile_("out_plot", "<file>", "", "txt file (if gnuplot is available, a corresponding PDF will be created as well.)", false);
    setValidFormats_("out_plot", ListUtils::create<String>("txt"));

    registerFlag_("split_charge", "The search engine scores are split by charge if this flag is set. Thus, for each charge state a new model will be computed.");
    registerFlag_("top_hits_only", "If set only the top hits of every PeptideIdentification will be used");
    registerDoubleOption_("fdr_for_targets_smaller", "<value>", 0.05, "Only used, when top_hits_only set. Additionally, target/decoy information should be available. The score_type must be q-value from an previous False Discovery Rate run.", false, true);
    registerFlag_("ignore_bad_data", "If set errors will be written but ignored. Useful for pipelines with many datasets where only a few are bad, but the pipeline should run through.");
    registerFlag_("prob_correct", "If set scores will be calculated as '1 - ErrorProbabilities' and can be interpreted as probabilities for correct identifications.");
    registerSubsection_("fit_algorithm", "Algorithm parameter subsection");
    addEmptyLine_();
  }

  //there is only one parameter at the moment
  Param getSubsectionDefaults_(const String& /*section*/) const
  {
    Param p = PosteriorErrorProbabilityModel().getParameters();
    if (p.exists("out_plot"))
    { // hide from user -- we have a top-level param for that
      p.remove("out_plot");
    }
    else
    {
      throw Exception::Precondition(__FILE__, __LINE__, __PRETTY_FUNCTION__, "INTERNAL ERROR: Param 'out_plot' was removed from fit-algorithm. Please update param handling internally!");
    }
    return p;
  }

  double getScore_(String& engine, const PeptideHit& hit)
  {
    if (engine == "OMSSA")
    {
      return (-1) * log10(max(hit.getScore(), smallest_e_value_));
    }
    else if (engine == "MyriMatch")
    {
      //double e_val = exp(-hit.getScore());
      //double score_val = ((-1)* log10(max(e_val,smallest_e_value_)));
      //printf("myri score: %e ; e_val: %e ; score_val: %e\n",hit.getScore(),e_val,score_val);
      //return score_val;
      return hit.getScore();
    }
    else if (engine.compare("XTandem") == 0)
    {
      return (-1) * log10(max((double)hit.getMetaValue("E-Value"), smallest_e_value_));
    }
    else if (engine == "MASCOT")
    {
      // issue #740: unable to fit data with score 0
      if (hit.getScore() == 0.0)
      {
        return numeric_limits<double>::quiet_NaN();
      }
      // end issue #740
      if (hit.metaValueExists("EValue"))
      {
        return (-1) * log10(max((double)hit.getMetaValue("EValue"), smallest_e_value_));
      }
      if (hit.metaValueExists("expect"))
      {
        return (-1) * log10(max((double)hit.getMetaValue("expect"), smallest_e_value_));
      }
    }
    else if (engine == "SpectraST")
    {
      return 100 * hit.getScore(); // SpectraST f-val
    }
    else if (engine == "SimTandem")
    {
      if (hit.metaValueExists("E-Value"))
      {
        return (-1) * log10(max((double)hit.getMetaValue("E-Value"), smallest_e_value_));
      }
    }
    else if (engine == "MSGFPlus")
    {
      return (-1) * log10(max(hit.getScore(), smallest_e_value_));
    }
    else
    {
      throw Exception::UnableToFit(__FILE__, __LINE__, __PRETTY_FUNCTION__, "No parameters for chosen search engine", "The chosen search engine is currently not supported");
    }

    // avoid compiler warning (every code path must return a value, even if there is a throw() somewhere)
    return std::numeric_limits<double>::max();
  }

  ExitCodes main_(int, const char**)
  {
    //-------------------------------------------------------------
    // parsing parameters
    //-------------------------------------------------------------

    String inputfile_name = getStringOption_("in");
    String outputfile_name = getStringOption_("out");
    Param fit_algorithm = getParam_().copy("fit_algorithm:", true);
    fit_algorithm.setValue("out_plot", getStringOption_("out_plot")); // re-assemble full param (was moved to top-level)
    bool split_charge = getFlag_("split_charge");
    bool top_hits_only = getFlag_("top_hits_only");
    double fdr_for_targets_smaller = getDoubleOption_("fdr_for_targets_smaller");
    bool target_decoy_available = false;
    bool ignore_bad_data = getFlag_("ignore_bad_data");
    bool prob_correct = getFlag_("prob_correct");

    // Set fixed e-value threshold
    smallest_e_value_ = numeric_limits<double>::denorm_min();

    //-------------------------------------------------------------
    // reading input
    //-------------------------------------------------------------
    IdXMLFile file;
    vector<ProteinIdentification> protein_ids;
    vector<PeptideIdentification> peptide_ids;
    file.load(inputfile_name, protein_ids, peptide_ids);
    vector<double> scores;
    vector<double> decoy;
    vector<double> target;
    set<Int> charges;
    PosteriorErrorProbabilityModel PEP_model;
    PEP_model.setParameters(fit_algorithm);
    StringList search_engines = ListUtils::create<String>("XTandem,OMSSA,MASCOT,SpectraST,MyriMatch,SimTandem,MSGFPlus");
    //-------------------------------------------------------------
    // calculations
    //-------------------------------------------------------------
    if (split_charge)
    {
      for (vector<PeptideIdentification>::iterator pep_it = peptide_ids.begin(); pep_it != peptide_ids.end(); ++pep_it)
      {
        vector<PeptideHit>& hits = pep_it->getHits();
        for (std::vector<PeptideHit>::iterator hit_it = hits.begin(); hit_it != hits.end(); ++hit_it)
        {
          charges.insert(hit_it->getCharge());
        }
      }
      if (charges.empty())
      {
        throw Exception::ElementNotFound(__FILE__, __LINE__, __PRETTY_FUNCTION__, "no charges found!");
      }
    }
    for (vector<PeptideIdentification>::iterator pep_it = peptide_ids.begin(); pep_it != peptide_ids.end(); ++pep_it)
    {
      if (!pep_it->getHits().empty())
      {
        target_decoy_available = ((pep_it->getScoreType() == "q-value") && pep_it->getHits()[0].metaValueExists("target_decoy"));
        break;
      }
    }

    set<Int>::iterator charge_it = charges.begin(); // charges can be empty, no problem if split_charge is not set
    if (split_charge && charges.empty())
    {
      throw Exception::Precondition(__FILE__, __LINE__, __PRETTY_FUNCTION__, "'split_charge' is set, but the list of charge states is empty");
    }
    map<String, vector<vector<double> > > all_scores;
    char splitter = ','; // to split the engine from the charge state later on
    do
    {
      for (StringList::iterator engine_it = search_engines.begin(); engine_it != search_engines.end(); ++engine_it)
      {
        for (vector<ProteinIdentification>::iterator prot_it = protein_ids.begin(); prot_it != protein_ids.end(); ++prot_it)
        {
          String searchengine = prot_it->getSearchEngine();
          if ((*engine_it == searchengine) || (*engine_it == searchengine.toUpper()))
          {
            for (vector<PeptideIdentification>::iterator pep_it = peptide_ids.begin(); pep_it != peptide_ids.end(); ++pep_it)
            {
              if (prot_it->getIdentifier() == pep_it->getIdentifier())
              {
                vector<PeptideHit>& hits = pep_it->getHits();
                if (top_hits_only)
                {
                  pep_it->sort();
                  if (!hits.empty() && (!split_charge || hits[0].getCharge() == *charge_it))
                  {
                    double score = getScore_(*engine_it, hits[0]);
                    if (!boost::math::isnan(score)) // issue #740: ignore scores with 0 values, otherwise you will get the error "unable to fit data"
                    {
                      scores.push_back(score);

                      if (target_decoy_available)
                      {
                        if (hits[0].getScore() < fdr_for_targets_smaller)
                        {
                          target.push_back(score);
                        }
                        else
                        {
                          decoy.push_back(score);
                        }
                      }
                    }
                  }
                }
                else
                {
                  for (std::vector<PeptideHit>::iterator hit_it = hits.begin(); hit_it != hits.end(); ++hit_it)
                  {
                    if (!split_charge || (hit_it->getCharge() == *charge_it))
                    {
                      double score = getScore_(*engine_it, *hit_it);
                      if (!boost::math::isnan(score)) // issue #740: ignore scores with 0 values, otherwise you will get the error "unable to fit data"
                      {
                        scores.push_back(score);
                      }
                    }
                  }
                }
              }
            }
          }
        }
        if (scores.size() > 2)
        {
          vector<vector<double> > tmp;
          tmp.push_back(scores);
          tmp.push_back(target);
          tmp.push_back(decoy);
          if (split_charge)
          {
            String engine_with_charge_state = *engine_it + String(splitter) + String(*charge_it);
            all_scores.insert(make_pair(engine_with_charge_state, tmp));
          }
          else
          {
            all_scores.insert(make_pair(*engine_it, tmp));
          }
        }

        scores.clear();
        target.clear();
        decoy.clear();
      }

      if (split_charge) ++charge_it;
    }
    while (charge_it != charges.end());

    if (all_scores.empty())
    {
      writeLog_("No data collected. Check whether search engine is supported.");
      if (!ignore_bad_data) return INPUT_FILE_EMPTY;
    }

    String out_plot = fit_algorithm.getValue("out_plot").toString().trim();
    for (map<String, vector<vector<double> > >::iterator score_it = all_scores.begin(); score_it != all_scores.end(); ++score_it)
    {
      vector<String> engine_info;
      score_it->first.split(splitter, engine_info);
      String engine = engine_info[0];
      Int charge = -1;
      if (engine_info.size() == 2)
      {
        charge = engine_info[1].toInt();
      }
      if (split_charge)
      {
        // only adapt plot output if plot is requested (this badly violates the output rules and needs to change!)
        // one way to fix this: plot charges into a single file (no renaming of output file needed) - but this requires major code restructuring
        if (!out_plot.empty()) fit_algorithm.setValue("out_plot", out_plot + "_charge_" + String(charge));
        PEP_model.setParameters(fit_algorithm);
      }

      const bool return_value = PEP_model.fit(score_it->second[0]);
      if (!return_value) writeLog_("Unable to fit data. Algorithm did not run through for the following search engine: " + engine);
      if (!return_value && !ignore_bad_data) return UNEXPECTED_RESULT;

      if (return_value)
      {
        // plot target_decoy
        if (!out_plot.empty() && top_hits_only && target_decoy_available && (score_it->second[0].size() > 0))
        {
          PEP_model.plotTargetDecoyEstimation(score_it->second[1], score_it->second[2]); //target, decoy
        }

        bool unable_to_fit_data = true;
        bool data_might_not_be_well_fit = true;
        for (vector<ProteinIdentification>::iterator prot_it = protein_ids.begin(); prot_it != protein_ids.end(); ++prot_it)
        {
          String searchengine = prot_it->getSearchEngine();
          if ((engine == searchengine) || (engine == searchengine.toUpper()))
          {
            for (vector<PeptideIdentification>::iterator pep_it = peptide_ids.begin(); pep_it != peptide_ids.end(); ++pep_it)
            {
              if (prot_it->getIdentifier() == pep_it->getIdentifier())
              {
                String score_type = pep_it->getScoreType() + "_score";
                vector<PeptideHit> hits = pep_it->getHits();
                for (std::vector<PeptideHit>::iterator hit_it = hits.begin(); hit_it != hits.end(); ++hit_it)
                {
                  if (!split_charge || (hit_it->getCharge() == charge))
                  {
                    double score;
                    hit_it->setMetaValue(score_type, hit_it->getScore());

                    score = getScore_(engine, *hit_it);
                    if (boost::math::isnan(score)) // issue #740: ignore scores with 0 values, otherwise you will get the error "unable to fit data"
                    {
                      score = 1.0;
                    }
                    else
                    {
                      score = PEP_model.computeProbability(score);
                      if ((score > 0.0) && (score < 1.0)) unable_to_fit_data = false;  // only if all it->second[0] are 0 or 1 unable_to_fit_data stays true
                      if ((score > 0.2) && (score < 0.8)) data_might_not_be_well_fit = false;  //same as above
                    }
                    hit_it->setScore(score);
                    if (prob_correct)
                    {
                      hit_it->setScore(1.0 - score);
                    }
                    else
                    {
                      hit_it->setScore(score);
                    }
                  }
                }
                pep_it->setHits(hits);
              }
              if (prob_correct)
              {
                pep_it->setScoreType("Posterior Probability");
                pep_it->setHigherScoreBetter(true);
              }
              else
              {
                pep_it->setScoreType("Posterior Error Probability");
                pep_it->setHigherScoreBetter(false);
              }
            }
          }
        }
        if (unable_to_fit_data) writeLog_(String("Unable to fit data for search engine: ") + engine);
        if (unable_to_fit_data && !ignore_bad_data) return UNEXPECTED_RESULT;

        if (data_might_not_be_well_fit) writeLog_(String("Data might not be well fitted for search engine: ") + engine);
      }
    }
    //-------------------------------------------------------------
    // writing output
    //-------------------------------------------------------------
    file.store(outputfile_name, protein_ids, peptide_ids);
    return EXECUTION_OK;
  }

  //Used in several functions
  double smallest_e_value_;
};

int main(int argc, const char** argv)
{
  TOPPIDPosteriorErrorProbability tool;

  return tool.main(argc, argv);
}

/// @endcond


/*********************************************************************
*
* Author: Fernando Caballero Benítez
* Author: Noé Pérez Higueras
*********************************************************************/

#include <simple_local_planner/pure_planner.h>
#include <costmap_2d/footprint.h>
#include <string>
#include <sstream>
#include <math.h>
#include <angles/angles.h>

#include <boost/algorithm/string.hpp>

#include <ros/console.h>

//for computing path distance
#include <queue>

using namespace std;
using namespace costmap_2d;

namespace simple_local_planner{

  void PurePlanner::reconfigure(SimpleLocalPlannerConfig &cfg)
  {
      SimpleLocalPlannerConfig config(cfg);

      boost::mutex::scoped_lock l(configuration_mutex_);

		acc_lim_trans_ = config.max_trans_acc;
		acc_lim_rot_ = config.max_rot_acc;
		max_vel_x_ = config.max_trans_vel;
		min_vel_x_ = config.min_trans_vel;
		max_vel_th_ = config.max_rot_vel;
		min_vel_th_ = config.min_rot_vel;
		min_in_place_vel_th_ = config.min_in_place_rot_vel;
		goal_lin_tolerance_ = config.xy_goal_tolerance;
		goal_ang_tolerance_ = config.yaw_goal_tolerance;
		wp_tolerance_ = config.wp_tolerance;
		sim_time_ = config.sim_time;
		sim_granularity_ = config.sim_granularity;
		angular_sim_granularity_ = config.angular_sim_granularity;
		dwa_ = config.sample_angular_vels;
		//printf("\nPure Planner Reconfigure. new wp_tolerance: %.2f\n", wp_tolerance_);
  }

  PurePlanner::PurePlanner(WorldModel& world_model,
          	const costmap_2d::Costmap2D& costmap,
			std::vector<geometry_msgs::Point> footprint_spec,
			double controller_freq,
			double max_trans_vel, double min_trans_vel,
			double max_rot_vel, double min_rot_vel,
			double min_in_place_rot_vel,
			double max_trans_acc, double max_rot_acc,
			double yaw_goal_tolerance, double xy_goal_tolerance,
			double wp_tolerance, double sim_time,
			double sim_granularity, double angular_sim_granularity, bool dwa)
				: world_model_(world_model), costmap_(costmap), footprint_spec_(footprint_spec)
  {

		costmap_2d::calculateMinAndMaxDistances(footprint_spec_, inscribed_radius_, circumscribed_radius_);

		printf("\n\n\n---FOOTPRINT----\n");
		printf("inscribed_radius: %.3f, circumscribed_radius: %.3f\n", inscribed_radius_, circumscribed_radius_);
		printf("Footprint_specs:\n");
		for(unsigned int i = 0; i<footprint_spec_.size(); i++)
		{
			printf("point %u: x=%.3f, y=%.3f\n", (i+1), footprint_spec_[i].x, footprint_spec_[i].y);
		}
		printf("\n\n");

		controller_freq_ = controller_freq;
		goal_reached_ = false;

    	//For pure-pursuit
    	running_ = false;
		new_plan_ = false;
		wp_index_ = -1;

    	acc_lim_trans_ = max_trans_acc;
		acc_lim_rot_ = max_rot_acc;
		max_vel_x_ = max_trans_vel;
		min_vel_x_ = min_trans_vel;
		max_vel_th_ = max_rot_vel;
		min_vel_th_ = min_rot_vel;
		min_in_place_vel_th_ = min_in_place_rot_vel;
		goal_lin_tolerance_ = xy_goal_tolerance;
		goal_ang_tolerance_ = yaw_goal_tolerance;
		wp_tolerance_ = wp_tolerance;
		sim_time_ = sim_time;
		sim_granularity_ = sim_granularity;
		angular_sim_granularity_ = angular_sim_granularity;

		dwa_ = dwa;

  }

  PurePlanner::~PurePlanner(){}

  /**
   * create and score a trajectory given the current pose of the robot and selected velocities
   */
  void PurePlanner::generateTrajectory(
      double x, double y, double theta, double vx, double vy,
          double vtheta, double vx_samp, double vy_samp, double vtheta_samp, double acc_x,
          double acc_y, double acc_theta, Trajectory& traj) {

    // make sure the configuration doesn't change mid run
    //boost::mutex::scoped_lock l(configuration_mutex_);

    double x_i = x;
    double y_i = y;
    double theta_i = theta;

    double vx_i , vy_i, vtheta_i;

    vx_i = vx;
    vy_i = vy;
	vtheta_i = vtheta;

    //compute the magnitude of the velocities
    double vmag = hypot(vx_samp, vy_samp);

    //compute the number of steps we must take along this trajectory to be "safe"
    int num_steps;

    //num_steps = int(max((vmag * sim_time_) / sim_granularity_, fabs(vtheta_samp) / angular_sim_granularity_) + 0.5);

    num_steps = int(sim_time_ / sim_granularity_ + 0.5);

    //we at least want to take one step... even if we won't move, we want to score our current position
    if(num_steps == 0) {
      num_steps = 1;
    }

    double dt = sim_time_ / num_steps;
    double time = 0.0;

    //create a potential trajectory
    traj.resetPoints();
    traj.xv_ = vx_samp;
    traj.yv_ = vy_samp;
    traj.thetav_ = vtheta_samp;
    traj.cost_ = -1.0;

    for(int i = 0; i < num_steps; ++i)
	{
      //get map coordinates of a point
      unsigned int cell_x, cell_y;

      //we don't want a path that goes off the know map
      if(!costmap_.worldToMap(x_i, y_i, cell_x, cell_y)){
        traj.cost_ = -1.0;
        return;
      }

      //check the point on the trajectory for legality
      double footprint_cost = footprintCost(x_i, y_i, theta_i);

	  //Added by Noé
	  if(footprint_cost >= 254.0){
		printf("\n\nfootprint cost invalid: %.2f!!!\n\n", footprint_cost);
        traj.cost_ = -1.0;
        return;
      }

      //if the footprint hits an obstacle this trajectory is invalid
      if(footprint_cost < 0){
        traj.cost_ = -1.0;
        return;
      }

      //the point is legal... add it to the trajectory
      traj.addPoint(x_i, y_i, theta_i);

      //calculate velocities
      vx_i = computeNewVelocity(vx_samp, vx_i, acc_x, dt);
      vy_i = computeNewVelocity(vy_samp, vy_i, acc_y, dt);
      vtheta_i = computeNewVelocity(vtheta_samp, vtheta_i, acc_theta, dt);

      //calculate positions
      x_i = computeNewXPosition(x_i, vx_i, vy_i, theta_i, dt);
      y_i = computeNewYPosition(y_i, vx_i, vy_i, theta_i, dt);
      theta_i = computeNewThetaPosition(theta_i, vtheta_i, dt);

      //increment time
      time += dt;
    } // end for i < numsteps

    traj.cost_ = 0;

  }

  /*double PurePlanner::headingDiff(int cell_x, int cell_y, double x, double y, double heading){
    double heading_diff = DBL_MAX;
    unsigned int goal_cell_x, goal_cell_y;
    const double v2_x = cos(heading);
    const double v2_y = sin(heading);

    //find a clear line of sight from the robot's cell to a point on the path
    for (int i = global_plan_.size() - 1; i >=0; --i) {
      if (costmap_.worldToMap(global_plan_[i].pose.position.x, global_plan_[i].pose.position.y, goal_cell_x, goal_cell_y)) {
        if (lineCost(cell_x, goal_cell_x, cell_y, goal_cell_y) >= 0) {
          double gx, gy;
          costmap_.mapToWorld(goal_cell_x, goal_cell_y, gx, gy);
          double v1_x = gx - x;
          double v1_y = gy - y;

          double perp_dot = v1_x * v2_y - v1_y * v2_x;
          double dot = v1_x * v2_x + v1_y * v2_y;

          //get the signed angle
          double vector_angle = atan2(perp_dot, dot);

          heading_diff = fabs(vector_angle);
          return heading_diff;
        }
      }
    }
    return heading_diff;
  }*/

  //calculate the cost of a ray-traced line
  double PurePlanner::lineCost(int x0, int x1,
      int y0, int y1){
    //Bresenham Ray-Tracing
    int deltax = abs(x1 - x0);        // The difference between the x's
    int deltay = abs(y1 - y0);        // The difference between the y's
    int x = x0;                       // Start x off at the first pixel
    int y = y0;                       // Start y off at the first pixel

    int xinc1, xinc2, yinc1, yinc2;
    int den, num, numadd, numpixels;

    double line_cost = 0.0;
    double point_cost = -1.0;

    if (x1 >= x0)                 // The x-values are increasing
    {
      xinc1 = 1;
      xinc2 = 1;
    }
    else                          // The x-values are decreasing
    {
      xinc1 = -1;
      xinc2 = -1;
    }

    if (y1 >= y0)                 // The y-values are increasing
    {
      yinc1 = 1;
      yinc2 = 1;
    }
    else                          // The y-values are decreasing
    {
      yinc1 = -1;
      yinc2 = -1;
    }

    if (deltax >= deltay)         // There is at least one x-value for every y-value
    {
      xinc1 = 0;                  // Don't change the x when numerator >= denominator
      yinc2 = 0;                  // Don't change the y for every iteration
      den = deltax;
      num = deltax / 2;
      numadd = deltay;
      numpixels = deltax;         // There are more x-values than y-values
    } else {                      // There is at least one y-value for every x-value
      xinc2 = 0;                  // Don't change the x for every iteration
      yinc1 = 0;                  // Don't change the y when numerator >= denominator
      den = deltay;
      num = deltay / 2;
      numadd = deltax;
      numpixels = deltay;         // There are more y-values than x-values
    }

    for (int curpixel = 0; curpixel <= numpixels; curpixel++) {
      point_cost = pointCost(x, y); //Score the current point

      if (point_cost < 0) {
        return -1;
      }

      if (line_cost < point_cost) {
        line_cost = point_cost;
      }

      num += numadd;              // Increase the numerator by the top of the fraction
      if (num >= den) {           // Check if numerator >= denominator
        num -= den;               // Calculate the new numerator value
        x += xinc1;               // Change the x as appropriate
        y += yinc1;               // Change the y as appropriate
      }
      x += xinc2;                 // Change the x as appropriate
      y += yinc2;                 // Change the y as appropriate
    }

    return line_cost;
  }

  double PurePlanner::pointCost(int x, int y){
    unsigned char cost = costmap_.getCost(x, y);
    //if the cell is in an obstacle the path is invalid
    if(cost == LETHAL_OBSTACLE || cost == INSCRIBED_INFLATED_OBSTACLE || cost == NO_INFORMATION){
      return -1;
    }

    return cost;
  }

  bool PurePlanner::updatePlan(const vector<geometry_msgs::PoseStamped>& new_plan)
  {
		goal_reached_ = false;

		// Copy new plan
		global_plan_.clear();
		global_plan_.resize(new_plan.size());
		for(unsigned int i = 0; i < new_plan.size(); ++i)
		{
			global_plan_[i] = new_plan[i];
		}

		// Check plan size
		if(global_plan_.size() == 0)
		{
			running_ = false;
			wp_index_ = -1;
			ROS_WARN("New local plan size = 0!");
			return true;
		}

		// Set the way-point index to the first point of the path
		wp_index_ = 0;
		running_ = true;
		new_plan_ = true;

		// Set plan goal point
		geometry_msgs::PoseStamped& goal_pose = global_plan_[global_plan_.size()-1];
		goal_x_ = goal_pose.pose.position.x;
		goal_y_ = goal_pose.pose.position.y;
		goal_t_ = tf::getYaw(goal_pose.pose.orientation);

		// Set the plan starting point
		geometry_msgs::PoseStamped& start_pose = global_plan_[0];
		start_x_ = start_pose.pose.position.x;
		start_y_ = start_pose.pose.position.y;
		start_t_ = tf::getYaw(start_pose.pose.orientation);

		/*if(compute_dists)
		{
			//reset the map for new operations
			path_map_.resetPathDist();
			goal_map_.resetPathDist();

			//make sure that we update our path based on the global plan and compute costs
			path_map_.setTargetCells(costmap_, global_plan_);
			goal_map_.setLocalGoal(costmap_, global_plan_);
			ROS_DEBUG("Path/Goal distance computed");
		}*/
		return true;
  }

  bool PurePlanner::checkTrajectory(double x, double y, double theta, double vx, double vy,
      double vtheta, double vx_samp, double vy_samp, double vtheta_samp, double& px, double& py, double& pth){

	Trajectory t;

	generateTrajectory(x, y, theta,
                       vx, vy, vtheta,
                       vx_samp, vy_samp, vtheta_samp,
                       acc_lim_trans_, 0.0, acc_lim_rot_, t);

	if(t.cost_ < 0.0) {
		//ROS_WARN("Invalid Trajectory vx:%f, vy:%f, vth:%f, cost: %f", vx, vy, vtheta, t.cost_);
		return false;
    }
    if(isnan(t.cost_)) {
		ROS_WARN("Trajectory cost is not a number!!! Invalid Trajectory vx:%f, vy:%f, vth:%f, cost: %f", vx, vy, vtheta, t.cost_);
		//cost = 0.0;
		return false;
	}
	//ROS_WARN("VALID Trajectory vx:%f, vy:%f, vth:%f, cost: %f", vx, vy, vtheta, t.cost_);

    //otherwise the trajectory is valid
	double pointx, pointy, pointth;
	t.getEndpoint(pointx, pointy, pointth);
	px = pointx;
	py = pointy;
	pth = pointth;

    return true;
  }

  bool PurePlanner::isGoalReached()
  {
		if(goal_reached_) {
			goal_reached_ = false; //we reset the flag
			return true;
		}
		return goal_reached_;
  }

  void PurePlanner::resetGoal() {
		goal_reached_ = false;
  }

  //given the current state of the robot, find a good control command
  bool PurePlanner::findBestAction(tf::Stamped<tf::Pose> global_pose, tf::Stamped<tf::Pose> global_vel,
      geometry_msgs::Twist& cmd_vel)
  {

	boost::mutex::scoped_lock l(configuration_mutex_);

	goal_reached_ = false;
	double vx, vy = 0.0, vt;

	// Check we have a path and we are running
	if(!running_)
	{
		vx = 0.0;
		vt = 0.0;
		cmd_vel.linear.x = vx;
		cmd_vel.linear.y = 0.0;
		cmd_vel.linear.z = 0.0;
		cmd_vel.angular.x = 0.0;
		cmd_vel.angular.y = 0.0;
		cmd_vel.angular.z = vt;
		//printf("FindBestAction. PurePursuit waiting for a path\n");
		return true;
	}

	// Get current robot position and velocity in X, Y and Theta
	float rx, ry, rt, rvx, rvy, rvt;
	rx = global_pose.getOrigin().getX();
	ry = global_pose.getOrigin().getY();
	rt = tf::getYaw(global_pose.getRotation());
	rvx = global_vel.getOrigin().getX();
	rvy = global_vel.getOrigin().getY();
	rvt = tf::getYaw(global_vel.getRotation());

    //reset the map for new operations
    /*path_map_.resetPathDist();
    goal_map_.resetPathDist();

    //mark cells within the initial footprint of the robot
    Eigen::Vector3f pos(global_pose.getOrigin().getX(), global_pose.getOrigin().getY(), tf::getYaw(global_pose.getRotation()));
    std::vector<upo_navigation::Position2DInt> footprint_list =
        footprint_helper_.getFootprintCells(
            pos,
            footprint_spec_,
            costmap_,
            true);
    for (unsigned int i = 0; i < footprint_list.size(); ++i) {
      path_map_(footprint_list[i].x, footprint_list[i].y).within_robot = true;
    }*/

    //make sure that we update our path based on the global plan and compute costs
    //path_map_.setTargetCells(costmap_, global_plan_);
    //goal_map_.setLocalGoal(costmap_, global_plan_);
    //ROS_DEBUG("Path/Goal distance computed");

	// Check if we are close enough to the goal
	double dist_goal = sqrt((rx-goal_x_)*(rx-goal_x_)+(ry-goal_y_)*(ry-goal_y_));
	double dist_start = sqrt((rx-start_x_)*(rx-start_x_)+(ry-start_y_)*(ry-start_y_));

	if(dist_goal < goal_lin_tolerance_)
	{
		//printf("FindBestAction. dist_goal: %.2f. rx:%.2f, ry:%.2f, goalx:%.2f, goaly:%.2f\n", dist_goal, rx, ry, goal_x_, goal_y_);
		// Stop the robot
		vx = 0.0;
		vy = 0.0;

		// Rotate at minumin velocity until reaching the goal angle
		if(fabs(goal_t_-rt) < goal_ang_tolerance_)
		{
			vt = 0.0;
			running_ = false;
			goal_reached_ = true;
		}
		/*else if(goal_t_ > rt)
			vt = min_in_place_vel_th_;
		else
			vt = -min_in_place_vel_th_;
		*/
		// Modified by Noé
		else {
			float ang_diff = goal_t_ - rt;
			ang_diff = normalizeAngle(ang_diff, -M_PI, M_PI);
			if(ang_diff > 0.0)
				vt = min_in_place_vel_th_;
			else
				vt = -min_in_place_vel_th_;
		}

		//Added by Noé
		cmd_vel.linear.x = vx;
		cmd_vel.linear.y = vy;
		cmd_vel.linear.z = 0.0;
		cmd_vel.angular.x = 0.0;
		cmd_vel.angular.y = 0.0;
		cmd_vel.angular.z = vt;
		return true;

	}

	// Do we have a new plan? get the closest point into the plan
	if(new_plan_)
	{
		new_plan_ = false;
		double dist;
		wp_index_ = 0;
		for(int i=global_plan_.size()-1; i>=0; i--)
		{
			double wpx = global_plan_[i].pose.position.x;
			double wpy = global_plan_[i].pose.position.y;
			dist = sqrt((rx-wpx)*(rx-wpx)+(ry-wpy)*(ry-wpy));
			if(dist < wp_tolerance_)
			{
				wp_index_ = i;
				break;
			}
		}
	}

	// Get current way-point in the path
	double wpx = global_plan_[wp_index_].pose.position.x;
	double wpy = global_plan_[wp_index_].pose.position.y;

	// Is this way-point still valid?
	double dist_swp = sqrt((rx-wpx)*(rx-wpx)+(ry-wpy)*(ry-wpy));
	while(dist_swp < wp_tolerance_ && wp_index_ < (int)global_plan_.size()-1)
	{
		wp_index_++;
		wpx = global_plan_[wp_index_].pose.position.x;
		wpy = global_plan_[wp_index_].pose.position.y;
		dist_swp = sqrt((rx-wpx)*(rx-wpx)+(ry-wpy)*(ry-wpy));
	}

	// Transform way-point into local robot frame and get desired x,y,theta
	double dx = (wpx-rx)*cos(rt) + (wpy-ry)*sin(rt);
	double dy =-(wpx-rx)*sin(rt) + (wpy-ry)*cos(rt);
	double dt = atan2(dy, dx);

	double incr = 1/controller_freq_;

	// Check if we need rotation in place before moving the robot to reach the way-point
	if(fabs(dt) > 1.3) // 0.87 0.7~41º  0.79~45º
	{
		vx = rvx-incr;
		if(vx < 0.0)
			vx = 0.0;
		vy = 0.0;
		vt = min_in_place_vel_th_;
		if(dt < 0.0)
			vt = -min_in_place_vel_th_;

		//printf("place_vt: %.2f\n", vt);
	}
	else // Select the linear and angular velocities to reach the way-point
	{
		// Compute actions depending to the distance to the goal and the starting points
		double dist_th = 1.5;
		if(dist_goal < dist_th)
		{
			vx = min_vel_x_ + (max_vel_x_ - min_vel_x_)*dist_goal/dist_th;
			vy = 0.0;
			vt = min_vel_th_ + (max_vel_th_ - min_vel_th_)*dist_goal/dist_th;
			//--added by noe
			if(dt < 0.0)
				vt *= -1;
		}
		/*else if(dist_start < dist_th)
		{
			vx = min_vel_x_ + (max_vel_x_ - min_vel_x_)*dist_start/dist_th;
			vy = 0.0;
			vt = min_vel_th_ + (max_vel_th_ - min_vel_th_)*dist_start/dist_th;
		}*/
		else
		{

			vx = max_vel_x_*(0.1 + exp(-fabs(dt))); // * tanh(4*dist_swp); //max_vel_x_;
			if(vx > max_vel_x_)
				vx = max_vel_x_;
			vy = 0.0;
			vt = max_vel_th_* dt; //max_vel_th_;
		}

		//if(vx > rvx+incr)
		//	vx = rvx+incr;

		// Set the sign of the commanded angular velocity and reset in case of small variations
		//if(dt < 0.0) //commented by Noé
		//	vt *= -1;
		if(fabs(dt) < 0.1)
			vt = 0.0;
	}

	// Check if the action collide with an obstacle
	double px, py, pth;
	//bool valid = checkTrajectory(rx, ry, rt, vx, vy, vt, 1.0, 0.0, 1.0, px, py, pth);
	bool valid = checkTrajectory(rx, ry, rt, rvx, rvy, rvt, vx, vy, vt, px, py, pth);

	if(valid || fabs(vx) < 0.0001)
	{
		cmd_vel.linear.x = vx;
		cmd_vel.linear.y = vy;
		cmd_vel.linear.z = 0.0;
		cmd_vel.angular.x = 0.0;
		cmd_vel.angular.y = 0.0;
		cmd_vel.angular.z = vt;
		return true;
	}

	// Try to find a valid command by sampling vels
	else if(dwa_)
	{

		float vt_orig = vt;
		float vx_orig = vx;

		float ang_vel_inc = 0.1;
		float lin_vel_dec = 0.1;

		vels_ best_vels;
		best_vels.vel_x = -1.0;
		best_vels.vel_y = vy;

		//Linear vel
		for(unsigned int l=0; l <= 3; l++)
		{

			vx = vx_orig - (lin_vel_dec*l);
			if(vx < 0.1)
				continue;

			best_vels.vel_x = vx;

			//Angular vel
			for(unsigned int v=1; v <= 4; v++)
			{
				//To the right
				vt = vt_orig + (ang_vel_inc * v);
				if(fabs(vt) > max_vel_th_)
					vt = max_vel_th_;

				//bool valid1 = checkTrajectory(rx, ry, rt, vx, vy, vt, 1.0, 0.0, 1.0, px, py, pth);
				bool valid1 = checkTrajectory(rx, ry, rt, rvx, rvy, rvt, vx, vy, vt, px, py, pth);
				double d1 = 0.0;
				if(valid1) {
					d1 = sqrt((dx-px)*(dx-px) + (dy-py)*(dy-py));
					best_vels.vel_th = vt;
				}

				//to the left
				vt = vt_orig - (ang_vel_inc * v);
				if(fabs(vt) > max_vel_th_)
					vt = -max_vel_th_;
				//bool valid2 = checkTrajectory(rx, ry, rt, vx, vy, vt, 1.0, 0.0, 1.0, px, py, pth);
				bool valid2 = checkTrajectory(rx, ry, rt, rvx, rvy, rvt, vx, vy, vt, px, py, pth);
				if(valid2) {

					//If both commands are valid,
					//chose the closest to the path.
					if(valid1)
					{
						double d2 = sqrt((dx-px)*(dx-px) + (dy-py)*(dy-py));
						if(d2 < d1)
						{
							best_vels.vel_th = vt;
						}
					}
				}
				if(valid1 || valid2)
				{
					cmd_vel.linear.x = best_vels.vel_x;
					cmd_vel.linear.y = best_vels.vel_x;
					cmd_vel.linear.z = 0.0;
					cmd_vel.angular.x = 0.0;
					cmd_vel.angular.y = 0.0;
					cmd_vel.angular.z = best_vels.vel_th;
					//printf("\nValid cmd found! vx:%.2f, vth:%.2f\n", vx, vt);
					return true;
				}

			}
		}
	}

	//If still a valid command is not found, try to rotate in the spot
	if(dt > 0.09 || dt < -0.09) //0.09rad~5º
	{
		//printf("The robot should rotate on the spot\n");
		vx = 0.0;
		vy = 0.0;
		vt = min_in_place_vel_th_;
		if(dt < 0.0)
			vt = -min_in_place_vel_th_;

		cmd_vel.linear.x = vx;
		cmd_vel.linear.y = vy;
		cmd_vel.linear.z = 0.0;
		cmd_vel.angular.x = 0.0;
		cmd_vel.angular.y = 0.0;
		cmd_vel.angular.z = vt;
		return true;
	} else {
		// Stop the robot
		//printf("The robot should stop\n");
		cmd_vel.linear.x = 0.0;
		cmd_vel.linear.y = 0.0;
		cmd_vel.linear.z = 0.0;
		cmd_vel.angular.x = 0.0;
		cmd_vel.angular.y = 0.0;
		cmd_vel.angular.z = 0.0;
		return false;
		//return true;
	}

  }

  //we need to take the footprint of the robot into account when we calculate cost to obstacles
  double PurePlanner::footprintCost(double x_i, double y_i, double theta_i){
    //check if the footprint is legal
    return world_model_.footprintCost(x_i, y_i, theta_i, footprint_spec_, inscribed_radius_, circumscribed_radius_);
  }

  /*void TrajectoryPlannerPP::getLocalGoal(double& x, double& y){
    x = path_map_.goal_x_;
    y = path_map_.goal_y_;
  }*/

};


//==================================================================================================
/*!
  @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_EXPONENTBITS_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_EXPONENTBITS_HPP_INCLUDED

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

 /*!

    @ingroup group-ieee
    Function object implementing exponentbits capabilities

    Returns the exponent bits of the floating input as an integer value,
    the other bits (sign and mantissa) being just masked.

    This function is not defined on integral types.

    @par Semantic:

    @code
    as_integer_t<T> r = exponentbits(x);
    @endcode

  **/
  as_integer_t<Value> exponentbits(Value const & v0);
} }
#endif

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

#endif


//

#include "art/Framework/Principal/Event.h"
#include "fhiclcpp/ParameterSet.h"
#include "art/Framework/Principal/Handle.h"
#include "GeometryService/inc/GeomHandle.hh"
#include "art/Framework/Core/EDProducer.h"
#include "art/Framework/Core/ModuleMacros.h"
#include "art_root_io/TFileService.h"

#include "ConditionsService/inc/ConditionsHandle.hh"
#include "ConfigTools/inc/ConfigFileLookupPolicy.hh"

#include "RecoDataProducts/inc/StrawHit.hh"
#include "RecoDataProducts/inc/StrawHitFlag.hh"
#include "RecoDataProducts/inc/ComboHit.hh"
#include "RecoDataProducts/inc/BkgCluster.hh"
#include "RecoDataProducts/inc/BkgQual.hh"

#include "TrkReco/inc/TLTClusterer.hh"
#include "TrkReco/inc/TNTClusterer.hh"
#include "Mu2eUtilities/inc/MVATools.hh"

#include "CLHEP/Units/PhysicalConstants.h"
#include "TMath.h"
#include "TH1F.h"
#include "TMVA/Reader.h"
#include "Math/VectorUtil.h"
#include "MCDataProducts/inc/StrawDigiMCCollection.hh"
#include "TMVA/Config.h"

#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/accumulators/statistics/median.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/moment.hpp>
#include <boost/accumulators/statistics/variance.hpp>
#include <boost/accumulators/statistics/min.hpp>
#include <boost/accumulators/statistics/max.hpp>
#include <boost/accumulators/statistics/weighted_variance.hpp>

#include <iostream>
#include <fstream>
#include <string>
#include <memory>
#include <functional>
#include <float.h>
#include <vector>
#include <set>
#include <map>
#include <numeric>

using namespace boost::accumulators;
using namespace ROOT::Math::VectorUtil;

namespace mu2e
{

  class FlagBkgHits : public art::EDProducer
  {
  public:
    enum clusterer { TwoLevelThreshold=1, TwoNiveauThreshold=2};
    explicit FlagBkgHits(fhicl::ParameterSet const&);
    void beginJob() override;
    void produce(art::Event& event) override;

  private:

    int const _debug;
    int const _printfreq;
    art::ProductToken<ComboHitCollection> const _chtoken;
    art::ProductToken<StrawHitCollection> const _shtoken;
    bool  _filter, _flagch, _flagsh;
    unsigned _minnhits;
    unsigned _minnstereo;
    unsigned _minnp;
    unsigned _maxisolated;
    bool _savebkg;
    StrawHitFlag _bkgmsk, _stereo;
    const ComboHitCollection* _chcol;
    BkgClusterer* _clusterer;
    float        _cperr2;
    bool         _useMVA;
    float        _bkgMVAcut;
    MVATools     _bkgMVA;

    void classifyClusters(BkgClusterCollection& clusters,BkgQualCollection& cquals) const;
    void fillBkgQual(const BkgCluster& cluster,BkgQual& cqual) const;
    void countHits(const BkgCluster& cluster, unsigned& nactive, unsigned& nstereo) const;
    void countPlanes(const BkgCluster& cluster,BkgQual& cqual) const;
  };

  FlagBkgHits::FlagBkgHits(const fhicl::ParameterSet& pset) :
    art::EDProducer{pset},
    _debug(pset.get<int>(                       "debugLevel",0)),
    _printfreq(pset.get<int>(                   "printFrequency",101)),
    _chtoken{consumes<ComboHitCollection>(pset.get<art::InputTag>("ComboHitCollection"))},
    _shtoken{consumes<StrawHitCollection>(pset.get<art::InputTag>("StrawHitCollection"))},
    _filter(pset.get<bool>(                     "FilterOutput")),
    _flagch(pset.get<bool>(                     "FlagComboHits")),
    _flagsh(pset.get<bool>(                     "FlagStrawHits")),
    _minnhits(pset.get<unsigned>(               "MinActiveHits",3)),
    _minnstereo(pset.get<unsigned>(             "MinStereoHits",0)),
    _minnp(pset.get<unsigned>(                  "MinNPlanes",2)),
    _maxisolated(pset.get<unsigned>(            "MaxIsolated",0)),
    _savebkg(pset.get<bool>(                    "SaveBkgClusters",false)),
    _bkgmsk(pset.get<std::vector<std::string> >("BackgroundMask",std::vector<std::string>{"Background"})),
    _stereo(pset.get<std::vector<std::string> >("StereoSelection",std::vector<std::string>{"Stereo","PanelCombo"})),
    _useMVA(pset.get<bool>(                     "UseBkgMVA",true)),
    _bkgMVAcut(pset.get<float>(                 "BkgMVACut",0.5)),
    _bkgMVA(pset.get<fhicl::ParameterSet>(      "BkgMVA", {}))
  {
    // Must call consumesMany because fillStrawHitIndices calls getManyByType.
    consumesMany<ComboHitCollection>();

    if (_flagch) produces<StrawHitFlagCollection>("ComboHits");
    if (_flagsh) produces<StrawHitFlagCollection>("StrawHits");
    if (_filter) produces<ComboHitCollection>();
    if (_savebkg)
      {
        produces<BkgClusterCollection>();
        produces<BkgQualCollection>();
      }

    clusterer ctype = static_cast<clusterer>(pset.get<int>("Clusterer",TwoLevelThreshold));
    switch ( ctype )
      {
      case TwoLevelThreshold:
        _clusterer = new TLTClusterer(pset.get<fhicl::ParameterSet>("TLTClusterer", {}));
        break;
      case TwoNiveauThreshold:
        _clusterer = new TNTClusterer(pset.get<fhicl::ParameterSet>("TNTClusterer", {}));
        break;
      default:
        throw cet::exception("RECO")<< "Unknown clusterer" << ctype << std::endl;
      }

    float cperr = pset.get<float>("ClusterPositionError",10.0);
    _cperr2 = cperr*cperr;
    // stop the default TMVA::Reader print
    TMVA::gConfig().SetSilent(true);
  }

  void FlagBkgHits::beginJob()
  {
    _clusterer->init();
    if (_useMVA) _bkgMVA.initMVA();
  }

  void FlagBkgHits::produce(art::Event& event )
  {
    unsigned iev=event.id().event();
    if (_debug > 0 && iev%_printfreq==0) std::cout<<"FlagBkgHits: event="<<iev<<std::endl;

    auto chH = event.getValidHandle(_chtoken);
    _chcol = chH.product();
    unsigned nch = _chcol->size();
    // the primary output is either a deep copy of selected inputs or a flag collection on those
    // intermediate results: keep these on the heap unless requested for diagnostics later
    BkgClusterCollection bkgccol;
    bkgccol.reserve(nch/2);
    BkgQualCollection bkgqcol;
    if (_savebkg) bkgqcol.reserve(bkgccol.size());
    StrawHitFlagCollection chfcol(nch);

    // find clusters
    _clusterer->findClusters(bkgccol,*_chcol);

    // evaluate results and put in the data products
    for (auto& cluster : bkgccol) {
      BkgQual cqual;
      fillBkgQual(cluster,cqual);
      StrawHitFlag flag(StrawHitFlag::bkgclust);
      if(cluster.hits().size() <= _maxisolated){
        flag.merge(StrawHitFlag::isolated);
        if(_savebkg)cluster._flag.merge(BkgClusterFlag::iso);
      }
      if(cqual.MVAOutput() > _bkgMVAcut){
        flag.merge(StrawHitFlag::bkg);
        if(_savebkg)cluster._flag.merge(BkgClusterFlag::bkg);
      }
      for (auto const& chit : cluster.hits())
        chfcol[chit.index()] = flag;
      if(_savebkg)bkgqcol.push_back(std::move(cqual));
    }

    if (_filter){
      auto chcol = std::make_unique<ComboHitCollection>();
      chcol->reserve(nch);
      // same parent as the original collection
      chcol->setParent(_chcol->parent());
      for(size_t ich=0;ich < nch; ++ich){
        StrawHitFlag const& flag = chfcol[ich];
        if(!flag.hasAnyProperty(_bkgmsk)) {
          chcol->push_back((*_chcol)[ich]);
          chcol->back()._flag.merge(flag);
        }
      }
      event.put(std::move(chcol));
    }

    if(_flagsh){
      auto shH = event.getValidHandle(_shtoken);
      const StrawHitCollection* shcol  = shH.product();
      // first, copy over the original flags
      unsigned nsh = shcol->size();
      auto shfcol = std::make_unique<StrawHitFlagCollection>(nsh);
      std::vector<std::vector<StrawHitIndex> > shids;
      _chcol->fillStrawHitIndices(event,shids);
      for(size_t ich = 0;ich < _chcol->size();++ich) {
        StrawHitFlag flag = chfcol[ich];
        flag.merge((*_chcol)[ich].flag());
        for(auto ish : shids[ich])
          (*shfcol)[ish] = flag;
      }

      event.put(std::move(shfcol),"StrawHits");
    }

    if (_flagch){
      for(size_t ich=0;ich < nch; ++ich) chfcol[ich].merge((*_chcol)[ich].flag());
      event.put(std::make_unique<StrawHitFlagCollection>(std::move(chfcol)),"ComboHits");
    }

    if (_savebkg){
      event.put(std::make_unique<BkgClusterCollection>(bkgccol));
      event.put(std::make_unique<BkgQualCollection>(bkgqcol));
    }

    return;
  }

  void FlagBkgHits::fillBkgQual(const BkgCluster& cluster, BkgQual& cqual) const
  {
    unsigned nactive, nstereo;
    countHits(cluster,nactive,nstereo);
    cqual.setMVAStatus(MVAStatus::unset);

    if (nactive >= _minnhits && nstereo >= _minnstereo)
      {
        cqual[BkgQual::nhits] = nactive;
        cqual[BkgQual::sfrac] = static_cast<float>(nstereo)/nactive;
        cqual[BkgQual::crho] = sqrtf(cluster.pos().perp2());

        countPlanes(cluster,cqual);
        if (cqual[BkgQual::np] >= _minnp)
          {
            accumulator_set<float, stats<tag::weighted_variance>, float> racc;
            accumulator_set<float, stats<tag::variance(lazy)> > tacc;
            std::vector<float> hz;
            for (const auto& chit : cluster.hits())
              {
                if (chit.flag().hasAllProperties(StrawHitFlag::active))
                  {
                    const ComboHit& ch = (*_chcol)[chit.index()];
                    hz.push_back(ch.pos().z());
                    float dt = ch.time() - cluster.time();
                    tacc(dt);
                    // compute the transverse radius of this hit WRT the cluster center
                    XYZVec psep = PerpVector(ch.pos()-cluster.pos(),Geom::ZDir());
                    float rho = sqrtf(psep.mag2());
                    // project the hit position error along the radial direction to compute the weight
                    XYZVec pdir = psep.unit();
                    XYZVec tdir(-ch.wdir().y(),ch.wdir().x(),0.0);
                    float rwerr = ch.posRes(ComboHit::wire)*pdir.Dot(ch.wdir());
                    float rterr = ch.posRes(ComboHit::trans)*pdir.Dot(tdir);
                    // include the cluster center position error when computing the weight
                    float rwt = 1.0/sqrtf(rwerr*rwerr + rterr*rterr + _cperr2/nactive);
                    racc(rho,weight=rwt);
                  }
              }
            cqual[BkgQual::hrho]  = extract_result<tag::weighted_mean>(racc);
            cqual[BkgQual::shrho] = sqrtf(std::max(extract_result<tag::weighted_variance>(racc),float(0.0)));
            cqual[BkgQual::sdt]  = sqrtf(std::max(extract_result<tag::variance>(tacc),float(0.0)));

            // find the min, max and gap from the sorted Z positions
            std::sort(hz.begin(),hz.end());
            cqual[BkgQual::zmin] = hz.front();
            cqual[BkgQual::zmax] = hz.back();

            // find biggest Z gap
            float zgap = 0.0;
            for (unsigned iz=1;iz<hz.size();++iz) zgap=std::max(zgap,hz[iz]-hz[iz-1]);
            cqual[BkgQual::zgap] = zgap;

            // compute MVA
            cqual.setMVAStatus(MVAStatus::filled);
            if (_useMVA)
              {
                // reduce values down to what's actually used in the MVA.  This functionality
                // should be in MVATool, FIXME!
                std::vector<float> mvavars(9,0.0);
                mvavars[0] = cqual.varValue(BkgQual::hrho);
                mvavars[1] = cqual.varValue(BkgQual::shrho);
                mvavars[2] = cqual.varValue(BkgQual::crho);
                mvavars[3] = cqual.varValue(BkgQual::zmin);
                mvavars[4] = cqual.varValue(BkgQual::zmax);
                mvavars[5] = cqual.varValue(BkgQual::zgap);
                mvavars[6] = cqual.varValue(BkgQual::np);
                mvavars[7] = cqual.varValue(BkgQual::npfrac);
                mvavars[8] = cqual.varValue(BkgQual::nhits);
                float mvaout = _bkgMVA.evalMVA(mvavars);

                cqual.setMVAValue(mvaout);
                cqual.setMVAStatus(MVAStatus::calculated);
              }
          } else {
          cqual[BkgQual::hrho] = -1.0;
          cqual[BkgQual::shrho] = -1.0;
          cqual[BkgQual::sdt] = -1.0;
          cqual[BkgQual::zmin] = -1.0;
          cqual[BkgQual::zmax] = -1.0;
          cqual[BkgQual::zgap] = -1.0;
        }
      }
  }

  void FlagBkgHits::countPlanes(const BkgCluster& cluster, BkgQual& cqual ) const
  {
    std::array<int,StrawId::_nplanes> hitplanes{0};
    for (const auto& chit : cluster.hits())
      {
        if (!chit.flag().hasAllProperties(StrawHitFlag::active)) continue;
        const ComboHit& ch = (*_chcol)[chit.index()];
        hitplanes[ch.strawId().plane()] += ch.nStrawHits();
      }

    unsigned ipmin(0),ipmax(StrawId::_nplanes-1);
    while (hitplanes[ipmin]==0) ++ipmin;
    while (hitplanes[ipmax]==0) --ipmax;

    unsigned npexp(0),np(0),nphits(0);
    for(unsigned ip = ipmin; ip <= ipmax; ++ip)
      {
        npexp++; // should use Tracker to see if plane is physically present FIXME!
        if (hitplanes[ip]> 0)++np;
        nphits += hitplanes[ip];
      }

    cqual[BkgQual::np] = np;
    cqual[BkgQual::npexp] = npexp;
    cqual[BkgQual::npfrac] = static_cast<float>(np)/static_cast<float>(npexp);
    cqual[BkgQual::nphits] = static_cast<float>(nphits)/static_cast<float>(np);
  }

  void FlagBkgHits::countHits(const BkgCluster& cluster, unsigned& nactive, unsigned& nstereo) const
  {
    nactive = nstereo = 0;
    for (const auto& chit : cluster.hits()) {
      const ComboHit& ch = (*_chcol)[chit.index()];
      if (chit.flag().hasAllProperties(StrawHitFlag::active))
        {
          nactive += ch.nStrawHits();
          if (chit.flag().hasAnyProperty(_stereo) ) nstereo += ch.nStrawHits();
        }
    }
  }

}

DEFINE_ART_MODULE(mu2e::FlagBkgHits);


#include <cstdint>
#include <functional>
#include <limits>
#include <queue>
#include <stdexcept>
#include <unordered_map>
#include <vector>
#include <Eigen/Geometry>
#include <arc_utilities/arc_helpers.hpp>

#ifndef DIJKSTRAS_HPP
#define DIJKSTRAS_HPP

namespace arc_dijkstras
{
    class GraphEdge
    {
        protected:

            int64_t from_index_;
            int64_t to_index_;
            double weight_;

        public:

            static uint64_t Serialize(const GraphEdge& edge, std::vector<uint8_t>& buffer)
            {
                return edge.SerializeSelf(buffer);
            }

            static std::pair<GraphEdge, uint64_t> Deserialize(const std::vector<uint8_t>& buffer, const uint64_t current)
            {
                GraphEdge temp_edge;
                const uint64_t bytes_read = temp_edge.DeserializeSelf(buffer, current);
                return std::make_pair(temp_edge, bytes_read);
            }

            GraphEdge(const int64_t from_index, const int64_t to_index, const double weight)
                : from_index_(from_index), to_index_(to_index), weight_(weight)
            {}

            GraphEdge()
                : from_index_(-1), to_index_(-1), weight_(0.0)
            {}

            uint64_t SerializeSelf(std::vector<uint8_t>& buffer) const
            {
                const uint64_t start_buffer_size = buffer.size();
                arc_helpers::SerializeFixedSizePOD<int64_t>(from_index_, buffer);
                arc_helpers::SerializeFixedSizePOD<int64_t>(to_index_, buffer);
                arc_helpers::SerializeFixedSizePOD<double>(weight_, buffer);
                // Figure out how many bytes were written
                const uint64_t end_buffer_size = buffer.size();
                const uint64_t bytes_written = end_buffer_size - start_buffer_size;
                return bytes_written;
            }

            uint64_t DeserializeSelf(const std::vector<uint8_t>& buffer, const uint64_t current)
            {
                uint64_t current_position = current;
                const std::pair<int64_t, uint64_t> deserialized_from_index = arc_helpers::DeserializeFixedSizePOD<int64_t>(buffer, current_position);
                from_index_ = deserialized_from_index.first;
                current_position += deserialized_from_index.second;
                const std::pair<int64_t, uint64_t> deserialized_to_index = arc_helpers::DeserializeFixedSizePOD<int64_t>(buffer, current_position);
                to_index_ = deserialized_to_index.first;
                current_position += deserialized_to_index.second;
                const std::pair<double, uint64_t> deserialized_weight = arc_helpers::DeserializeFixedSizePOD<double>(buffer, current_position);
                weight_ = deserialized_weight.first;
                current_position += deserialized_weight.second;
                // Figure out how many bytes were read
                const uint64_t bytes_read = current_position - current;
                return bytes_read;
            }

            bool operator==(const GraphEdge& other) const
            {
                return (from_index_ == other.GetFromIndex() && to_index_ == other.GetToIndex() && weight_ == other.GetWeight());
            }

            std::string Print() const
            {
                return std::string("(" + std::to_string(from_index_) + "->" + std::to_string(to_index_) + ") : " + std::to_string(weight_));
            }

            int64_t GetFromIndex() const
            {
                return from_index_;
            }

            int64_t GetToIndex() const
            {
                return to_index_;
            }

            double GetWeight() const
            {
                return weight_;
            }

            void SetFromIndex(const int64_t new_from_index)
            {
                from_index_ = new_from_index;
            }

            void SetToIndex(const int64_t new_to_index)
            {
                to_index_ = new_to_index;
            }

            void SetWeight(const double new_weight)
            {
                weight_ = new_weight;
            }
    };

    inline std::ostream& operator<< (std::ostream& stream, const GraphEdge& edge)
    {
        stream << edge.Print();
        return stream;
    }

    template<typename NodeValueType, typename Allocator=std::allocator<NodeValueType>>
    class GraphNode
    {
        protected:

            NodeValueType value_;
            double distance_;
            std::vector<GraphEdge> in_edges_;
            std::vector<GraphEdge> out_edges_;

        public:

            EIGEN_MAKE_ALIGNED_OPERATOR_NEW

            static uint64_t Serialize(const GraphNode<NodeValueType, Allocator>& node, std::vector<uint8_t>& buffer, const std::function<uint64_t(const NodeValueType&, std::vector<uint8_t>&)>& value_serializer)
            {
                return node.SerializeSelf(buffer, value_serializer);
            }

            static std::pair<GraphNode<NodeValueType, Allocator>, uint64_t> Deserialize(const std::vector<uint8_t>& buffer, const uint64_t current, const std::function<std::pair<NodeValueType, uint64_t>(const std::vector<uint8_t>&, const uint64_t)>& value_deserializer)
            {
                GraphNode<NodeValueType, Allocator> temp_node;
                const uint64_t bytes_read = temp_node.DeserializeSelf(buffer, current, value_deserializer);
                return std::make_pair(temp_node, bytes_read);
            }

            GraphNode(const NodeValueType& value, const double distance, const std::vector<GraphEdge>& new_in_edges, const std::vector<GraphEdge>& new_out_edges)
                : value_(value), distance_(distance), in_edges_(new_in_edges), out_edges_(new_out_edges)
            {}

            explicit GraphNode(const NodeValueType& value)
                : value_(value), distance_(std::numeric_limits<double>::infinity())
            {}

            GraphNode()
                : distance_(std::numeric_limits<double>::infinity())
            {}

            uint64_t SerializeSelf(std::vector<uint8_t>& buffer, const std::function<uint64_t(const NodeValueType&, std::vector<uint8_t>&)>& value_serializer) const
            {
                const uint64_t start_buffer_size = buffer.size();
                // Serialize the value
                value_serializer(value_, buffer);
                // Serialize the distance
                arc_helpers::SerializeFixedSizePOD<double>(distance_, buffer);
                // Serialize the in edges
                arc_helpers::SerializeVector<GraphEdge>(in_edges_, buffer, GraphEdge::Serialize);
                // Serialize the in edges
                arc_helpers::SerializeVector<GraphEdge>(out_edges_, buffer, GraphEdge::Serialize);
                // Figure out how many bytes were written
                const uint64_t end_buffer_size = buffer.size();
                const uint64_t bytes_written = end_buffer_size - start_buffer_size;
                return bytes_written;
            }

            uint64_t DeserializeSelf(const std::vector<uint8_t>& buffer, const uint64_t current, const std::function<std::pair<NodeValueType, uint64_t>(const std::vector<uint8_t>&, const uint64_t)>& value_deserializer)
            {
                uint64_t current_position = current;
                // Deserialize the value
                const std::pair<NodeValueType, uint64_t> value_deserialized = value_deserializer(buffer, current_position);
                value_ = value_deserialized.first;
                current_position += value_deserialized.second;
                // Deserialize the distace
                const std::pair<double, uint64_t> distance_deserialized = arc_helpers::DeserializeFixedSizePOD<double>(buffer, current_position);
                distance_ = distance_deserialized.first;
                current_position += distance_deserialized.second;
                // Deserialize the in edges
                const std::pair<std::vector<GraphEdge>, uint64_t> in_edges_deserialized = arc_helpers::DeserializeVector<GraphEdge>(buffer, current_position, GraphEdge::Deserialize);
                in_edges_ = in_edges_deserialized.first;
                current_position += in_edges_deserialized.second;
                // Deserialize the out edges
                const std::pair<std::vector<GraphEdge>, uint64_t> out_edges_deserialized = arc_helpers::DeserializeVector<GraphEdge>(buffer, current_position, GraphEdge::Deserialize);
                out_edges_ = out_edges_deserialized.first;
                current_position += out_edges_deserialized.second;
                // Figure out how many bytes were read
                const uint64_t bytes_read = current_position - current;
                return bytes_read;
            }

            std::string Print() const
            {
                std::ostringstream strm;
                strm << "Node : " << distance_ << " In Edges : ";
                if (in_edges_.size() > 0)
                {
                    strm << in_edges_[0].Print();
                    for (size_t idx = 1; idx < in_edges_.size(); idx++)
                    {
                        strm << ", " << in_edges_[idx].Print();
                    }
                }
                strm << " Out Edges : ";
                if (out_edges_.size() > 0)
                {
                    strm << out_edges_[0].Print();
                    for (size_t idx = 1; idx < out_edges_.size(); idx++)
                    {
                        strm << ", " << out_edges_[idx].Print();
                    }
                }
                return strm.str();
            }

            const NodeValueType& GetValueImmutable() const
            {
                return value_;
            }

            NodeValueType& GetValueMutable()
            {
                return value_;
            }

            void AddInEdge(const GraphEdge& new_in_edge)
            {
                in_edges_.push_back(new_in_edge);
            }

            void AddOutEdge(const GraphEdge& new_out_edge)
            {
                out_edges_.push_back(new_out_edge);
            }

            void AddEdgePair(const GraphEdge& new_in_edge, const GraphEdge& new_out_edge)
            {
                AddInEdge(new_in_edge);
                AddOutEdge(new_out_edge);
            }

            double GetDistance() const
            {
                return distance_;
            }

            void SetDistance(const double distance)
            {
                distance_ = distance;
            }

            const std::vector<GraphEdge>& GetInEdgesImmutable() const
            {
                return in_edges_;
            }

            std::vector<GraphEdge>& GetInEdgesMutable()
            {
                return in_edges_;
            }

            const std::vector<GraphEdge>& GetOutEdgesImmutable() const
            {
                return out_edges_;
            }

            std::vector<GraphEdge>& GetOutEdgesMutable()
            {
                return out_edges_;
            }

            void SetInEdges(const std::vector<GraphEdge>& new_in_edges)
            {
                in_edges_ = new_in_edges;
            }

            void SetOutEdges(const std::vector<GraphEdge>& new_out_edges)
            {
                out_edges_ = new_out_edges;
            }
    };

    template<typename NodeValueType, typename Allocator=std::allocator<NodeValueType>>
    class Graph
    {
        protected:

            std::vector<GraphNode<NodeValueType, Allocator>> nodes_;

        public:

            static uint64_t Serialize(const Graph<NodeValueType, Allocator>& graph, std::vector<uint8_t>& buffer, const std::function<uint64_t(const NodeValueType&, std::vector<uint8_t>&)>& value_serializer)
            {
                return graph.SerializeSelf(buffer, value_serializer);
            }

            static std::pair<Graph<NodeValueType, Allocator>, uint64_t> Deserialize(const std::vector<uint8_t>& buffer, const uint64_t current, const std::function<std::pair<NodeValueType, uint64_t>(const std::vector<uint8_t>&, const uint64_t)>& value_deserializer)
            {
                Graph<NodeValueType, Allocator> temp_graph;
                const uint64_t bytes_read = temp_graph.DeserializeSelf(buffer, current, value_deserializer);
                return std::make_pair(temp_graph, bytes_read);
            }

            Graph(const std::vector<GraphNode<NodeValueType, Allocator>>& nodes)
            {
                if (CheckGraphLinkage(nodes))
                {
                    nodes_ = nodes;
                }
                else
                {
                    throw std::invalid_argument("Invalid graph linkage");
                }
            }

            Graph(const size_t expected_size)
            {
                nodes_.reserve(expected_size);
            }

            Graph()
            {}

            uint64_t SerializeSelf(std::vector<uint8_t>& buffer, const std::function<uint64_t(const NodeValueType&, std::vector<uint8_t>&)>& value_serializer) const
            {
                const uint64_t start_buffer_size = buffer.size();
                std::function<uint64_t(const GraphNode<NodeValueType, Allocator>&, std::vector<uint8_t>&)> graph_state_serializer = std::bind(GraphNode<NodeValueType, Allocator>::Serialize, std::placeholders::_1, std::placeholders::_2, value_serializer);
                arc_helpers::SerializeVector<GraphNode<NodeValueType, Allocator>>(nodes_, buffer, graph_state_serializer);
                // Figure out how many bytes were written
                const uint64_t end_buffer_size = buffer.size();
                const uint64_t bytes_written = end_buffer_size - start_buffer_size;
                return bytes_written;
            }

            uint64_t DeserializeSelf(const std::vector<uint8_t>& buffer, const uint64_t current, const std::function<std::pair<NodeValueType, uint64_t>(const std::vector<uint8_t>&, const uint64_t)>& value_deserializer)
            {
                const std::function<std::pair<GraphNode<NodeValueType, Allocator>, uint64_t>(const std::vector<uint8_t>&, const uint64_t)> graph_state_deserializer = std::bind(GraphNode<NodeValueType, Allocator>::Deserialize, std::placeholders::_1, std::placeholders::_2, value_deserializer);
                const std::pair<std::vector<GraphNode<NodeValueType, Allocator>>, uint64_t> deserialized_nodes = arc_helpers::DeserializeVector<GraphNode<NodeValueType, Allocator>>(buffer, current, graph_state_deserializer);
                nodes_ = deserialized_nodes.first;
                return deserialized_nodes.second;
            }

            std::string Print() const
            {
                std::ostringstream strm;
                strm << "Graph - Nodes : ";
                if (nodes_.size() > 0)
                {
                    strm << nodes_[0].Print();
                    for (size_t idx = 1; idx < nodes_.size(); idx++)
                    {
                        strm << "\n" << nodes_[idx].Print();
                    }
                }
                return strm.str();
            }

            void ShrinkToFit()
            {
                nodes_.shrink_to_fit();
            }

            bool IndexInRange(const int64_t index) const
            {
                if (index >= 0)
                {
                    if (index < (int64_t)(nodes_.size()))
                    {
                        return true;
                    }
                    else
                    {
                        return false;
                    }
                }
                else
                {
                    return false;
                }
            }

            bool CheckGraphLinkage() const
            {
                return CheckGraphLinkage(GetNodesImmutable());
            }

            static bool CheckGraphLinkage(const Graph<NodeValueType, Allocator>& graph)
            {
                return CheckGraphLinkage(graph.GetNodesImmutable());
            }

            static bool CheckGraphLinkage(const std::vector<GraphNode<NodeValueType, Allocator>>& nodes)
            {
                // Go through every node and make sure the edges are valid
                for (size_t idx = 0; idx < nodes.size(); idx++)
                {
                    const GraphNode<NodeValueType, Allocator>& current_node = nodes[idx];
                    // Check the in edges first
                    const std::vector<GraphEdge>& in_edges = current_node.GetInEdgesImmutable();
                    for (size_t in_edge_idx = 0; in_edge_idx < in_edges.size(); in_edge_idx++)
                    {
                        const GraphEdge& current_edge = in_edges[in_edge_idx];
                        // Check from index to make sure it's in bounds
                        const int64_t from_index = current_edge.GetFromIndex();
                        if (from_index < 0 || from_index >= (int64_t)nodes.size())
                        {
                            return false;
                        }
                        // Check to index to make sure it matches our own index
                        const int64_t to_index = current_edge.GetToIndex();
                        if (to_index != (int64_t)idx)
                        {
                            return false;
                        }
                        // Check edge validity (edges to ourself are not allowed)
                        if (from_index == to_index)
                        {
                            return false;
                        }
                        // Check to make sure that the from index node is linked to us
                        const GraphNode<NodeValueType, Allocator>& from_node = nodes[(size_t)from_index];
                        const std::vector<GraphEdge>& from_node_out_edges = from_node.GetOutEdgesImmutable();
                        bool from_node_connection_valid = false;
                        // Make sure at least one out edge of the from index node corresponds to the current node
                        for (size_t from_node_out_edge_idx = 0; from_node_out_edge_idx < from_node_out_edges.size(); from_node_out_edge_idx++)
                        {
                            const GraphEdge& current_from_node_out_edge = from_node_out_edges[from_node_out_edge_idx];
                            if (current_from_node_out_edge.GetToIndex() == (int64_t)idx)
                            {
                                from_node_connection_valid = true;
                            }
                        }
                        if (from_node_connection_valid == false)
                        {
                            return false;
                        }
                    }
                    // Check the out edges second
                    const std::vector<GraphEdge>& out_edges = current_node.GetOutEdgesImmutable();
                    for (size_t out_edge_idx = 0; out_edge_idx < out_edges.size(); out_edge_idx++)
                    {
                        const GraphEdge& current_edge = out_edges[out_edge_idx];
                        // Check from index to make sure it matches our own index
                        const int64_t from_index = current_edge.GetFromIndex();
                        if (from_index != (int64_t)idx)
                        {
                            return false;
                        }
                        // Check to index to make sure it's in bounds
                        const int64_t to_index = current_edge.GetToIndex();
                        if (to_index < 0 || to_index >= (int64_t)nodes.size())
                        {
                            return false;
                        }
                        // Check edge validity (edges to ourself are not allowed)
                        if (from_index == to_index)
                        {
                            return false;
                        }
                        // Check to make sure that the to index node is linked to us
                        const GraphNode<NodeValueType, Allocator>& to_node = nodes[(size_t)to_index];
                        const std::vector<GraphEdge>& to_node_in_edges = to_node.GetInEdgesImmutable();
                        bool to_node_connection_valid = false;
                        // Make sure at least one in edge of the to index node corresponds to the current node
                        for (size_t to_node_in_edge_idx = 0; to_node_in_edge_idx < to_node_in_edges.size(); to_node_in_edge_idx++)
                        {
                            const GraphEdge& current_to_node_in_edge = to_node_in_edges[to_node_in_edge_idx];
                            if (current_to_node_in_edge.GetFromIndex() == (int64_t)idx)
                            {
                                to_node_connection_valid = true;
                            }
                        }
                        if (to_node_connection_valid == false)
                        {
                            return false;
                        }
                    }
                }
                return true;
            }

            const std::vector<GraphNode<NodeValueType, Allocator>>& GetNodesImmutable() const
            {
                return nodes_;
            }

            std::vector<GraphNode<NodeValueType, Allocator>>& GetNodesMutable()
            {
                return nodes_;
            }

            const GraphNode<NodeValueType, Allocator>& GetNodeImmutable(const int64_t index) const
            {
                return nodes_.at((size_t)index);
            }

            GraphNode<NodeValueType, Allocator>& GetNodeMutable(const int64_t index)
            {
                return nodes_.at((size_t)index);
            }

            int64_t AddNode(const GraphNode<NodeValueType, Allocator>& new_node)
            {
                nodes_.push_back(new_node);
                return (int64_t)(nodes_.size() - 1);
            }

            int64_t AddNode(const NodeValueType& new_value)
            {
                nodes_.push_back(GraphNode<NodeValueType, Allocator>(new_value));
                return (int64_t)(nodes_.size() - 1);
            }

            void AddEdgeBetweenNodes(const int64_t from_index, const int64_t to_index, const double edge_weight)
            {
                // We retrieve the nodes first, since retrieval performs bounds checks first
                GraphNode<NodeValueType, Allocator>& from_node = GetNodeMutable(from_index);
                GraphNode<NodeValueType, Allocator>& to_node = GetNodeMutable(to_index);
                if (from_index == to_index)
                {
                    throw std::invalid_argument("Invalid circular edge from==to not allowed");
                }
                const GraphEdge new_edge(from_index, to_index, edge_weight);
                from_node.AddOutEdge(new_edge);
                to_node.AddInEdge(new_edge);
            }

            void AddEdgesBetweenNodes(const int64_t first_index, const int64_t second_index, const double edge_weight)
            {
                // We retrieve the nodes first, since retrieval performs bounds checks first
                GraphNode<NodeValueType, Allocator>& first_node = GetNodeMutable(first_index);
                GraphNode<NodeValueType, Allocator>& second_node = GetNodeMutable(second_index);
                if (first_index == second_index)
                {
                    throw std::invalid_argument("Invalid circular edge first==second not allowed");
                }
                const GraphEdge first_edge(first_index, second_index, edge_weight);
                first_node.AddOutEdge(first_edge);
                second_node.AddInEdge(first_edge);
                const GraphEdge second_edge(second_index, first_index, edge_weight);
                second_node.AddOutEdge(second_edge);
                first_node.AddInEdge(second_edge);
            }
    };

    template<typename NodeValueType, typename Allocator=std::allocator<NodeValueType>>
    class SimpleDijkstrasAlgorithm
    {
        protected:

            class CompareIndexFn
            {
                public:

                    constexpr bool operator()(const std::pair<int64_t, double>& lhs, const std::pair<int64_t, double>& rhs) const
                    {
                        return lhs.second > rhs.second;
                    }
            };

            SimpleDijkstrasAlgorithm() {}

        public:

            typedef std::pair<Graph<NodeValueType, Allocator>, std::pair<std::vector<int64_t>, std::vector<double>>> DijkstrasResult;

            static DijkstrasResult PerformDijkstrasAlgorithm(const Graph<NodeValueType, Allocator>& graph, const int64_t start_index)
            {
                if ((start_index < 0) && (start_index >= (int64_t)graph.GetNodesImmutable().size()))
                {
                    throw std::invalid_argument("Start index out of range");
                }
                Graph<NodeValueType, Allocator> working_copy = graph;
                // Setup
                std::vector<int64_t> previous_index_map(working_copy.GetNodesImmutable().size(), -1);
                std::vector<double> distances(working_copy.GetNodesImmutable().size(), std::numeric_limits<double>::infinity());
                std::priority_queue<std::pair<int64_t, double>, std::vector<std::pair<int64_t, double>>, CompareIndexFn> queue;
                std::unordered_map<int64_t, uint32_t> explored(graph.GetNodesImmutable().size());
                for (size_t idx = 0; idx < working_copy.GetNodesImmutable().size(); idx++)
                {
                    working_copy.GetNodeMutable((int64_t)idx).SetDistance(std::numeric_limits<double>::infinity());
                    queue.push(std::make_pair((int64_t)idx, std::numeric_limits<double>::infinity()));
                }
                working_copy.GetNodeMutable(start_index).SetDistance(0.0);
                previous_index_map[(size_t)start_index] = start_index;
                distances[(size_t)start_index] = 0.0;
                queue.push(std::make_pair(start_index, 0.0));
                while (queue.size() > 0)
                {
                    const std::pair<int64_t, double> top_node = queue.top();
                    const int64_t& top_node_index = top_node.first;
                    const double& top_node_distance = top_node.second;
                    queue.pop();
                    if (explored[top_node.first] > 0)
                    {
                        // We've already been here
                        continue;
                    }
                    else
                    {
                        // Note that we've been here
                        explored[top_node.first] = 1;
                        // Get our neighbors
                        const std::vector<GraphEdge>& neighbor_edges = working_copy.GetNodeImmutable(top_node_index).GetInEdgesImmutable();
                        // Go through our neighbors
                        for (size_t neighbor_idx = 0; neighbor_idx < neighbor_edges.size(); neighbor_idx++)
                        {
                            const int64_t neighbor_index = neighbor_edges[neighbor_idx].GetFromIndex();
                            const double neighbor_edge_weight = neighbor_edges[neighbor_idx].GetWeight();
                            const double new_neighbor_distance = top_node_distance + neighbor_edge_weight;
                            // Check against the neighbor
                            const double stored_neighbor_distance = working_copy.GetNodeImmutable(neighbor_index).GetDistance();
                            if (new_neighbor_distance < stored_neighbor_distance)
                            {
                                // We've found a better way to get to this node
                                // Check if it's already been explored
                                if (explored[neighbor_index] > 0)
                                {
                                    // If it's already been explored, we just update it in place
                                    working_copy.GetNodeMutable(neighbor_index).SetDistance(new_neighbor_distance);
                                }
                                else
                                {
                                    // If it hasn't been explored, we need to update it and add it to the queue
                                    working_copy.GetNodeMutable(neighbor_index).SetDistance(new_neighbor_distance);
                                    queue.push(std::make_pair(neighbor_index, new_neighbor_distance));
                                }
                                // Update that we're the best previous node
                                previous_index_map[(size_t)neighbor_index] = top_node_index;
                                distances[(size_t)neighbor_index] = new_neighbor_distance;
                            }
                            else
                            {
                                // Do nothing
                                continue;
                            }
                        }
                    }
                }
                return std::make_pair(working_copy, std::make_pair(previous_index_map, distances));
            }

            static uint64_t SerializeDijstrasResult(const DijkstrasResult& result, std::vector<uint8_t>& buffer, const std::function<uint64_t(const NodeValueType&, std::vector<uint8_t>&)>& value_serializer)
            {
                const uint64_t start_buffer_size = buffer.size();
                // Serialize the graph
                result.first.SerializeSelf(buffer, value_serializer);
                // Serialize the previous index
                SerializeVector(result.second.first, std::bind(arc_helpers::SerializeFixedSizePOD<uint64_t>, std::placeholders::_1, std::placeholders::_2));
                // Serialze the distances
                SerializeVector(result.second.second, std::bind(arc_helpers::SerializeFixedSizePOD<uint64_t>, std::placeholders::_1, std::placeholders::_2));
                // Figure out how many bytes were written
                const uint64_t end_buffer_size = buffer.size();
                const uint64_t bytes_written = end_buffer_size - start_buffer_size;
                return bytes_written;
            }

            static std::pair<DijkstrasResult, uint64_t> DijstrasResult(const std::vector<uint8_t>& buffer, const uint64_t current, const std::function<std::pair<NodeValueType, uint64_t>(const std::vector<uint8_t>&, const uint64_t)>& value_deserializer)
            {
                uint64_t current_position = current;
                // Deserialize the graph itself
                std::pair<DijkstrasResult, uint64_t> deserialized;
                const std::pair<Graph<NodeValueType, Allocator>, uint64_t> graph_deserialized = Graph<NodeValueType, Allocator>::Deserialize(buffer, current_position, value_deserializer);
                deserialized.first.first = graph_deserialized.first;
                current_position += graph_deserialized.second;
                // Deserialize the previous index
                const std::pair<std::vector<int64_t>, uint64_t> prev_index_deserialized = arc_helpers::DeserializeVector<int64_t>(buffer, current_position, std::bind(arc_helpers::DeserializeFixedSizePOD<uint64_t>, std::placeholders::_1, std::placeholders::_2));
                deserialized.first.second.first = prev_index_deserialized.first;
                current_position += prev_index_deserialized.second;
                // Deserialize the distances
                const std::pair<std::vector<double>, uint64_t> distance_deserialized = arc_helpers::DeserializeVector<double>(buffer, current_position, std::bind(arc_helpers::DeserializeFixedSizePOD<double>, std::placeholders::_1, std::placeholders::_2));
                deserialized.first.second.second = distance_deserialized.first;
                current_position += distance_deserialized.second;
                // Figure out how many bytes were read
                deserialized.second = current_position - current;
                return deserialized;
            }
    };

    template<typename NodeValueType, typename Allocator=std::allocator<NodeValueType>>
    class SimpleGraphAstar
    {
    protected:

        SimpleGraphAstar() {}

    public:

        static arc_helpers::AstarResult PerformLazyAstar(const Graph<NodeValueType, Allocator>& graph, const int64_t start_index, const int64_t goal_index, const std::function<bool(const Graph<NodeValueType, Allocator>&, const GraphEdge&)>& edge_validity_check_fn, const std::function<double(const Graph<NodeValueType, Allocator>&, const GraphEdge&)>& distance_fn, const std::function<double(const NodeValueType&, const NodeValueType&)>& heuristic_fn, const bool limit_pqueue_duplicates)
        {
            // Enforced sanity checks
            if ((start_index < 0) && (start_index >= (int64_t)graph.GetNodesImmutable().size()))
            {
                throw std::invalid_argument("Start index out of range");
            }
            if ((goal_index < 0) && (goal_index >= (int64_t)graph.GetNodesImmutable().size()))
            {
                throw std::invalid_argument("Goal index out of range");
            }
            if (start_index == goal_index)
            {
                throw std::invalid_argument("Start and goal indices must be different");
            }
            // Make helper function
            const auto heuristic_function = [&] (const int64_t node_index) { return heuristic_fn(graph.GetNodeImmutable(node_index).GetValueImmutable(), graph.GetNodeImmutable(goal_index).GetValueImmutable()); };
            // Setup
            std::priority_queue<arc_helpers::AstarPQueueElement, std::vector<arc_helpers::AstarPQueueElement>, arc_helpers::CompareAstarPQueueElementFn> queue;
            // Optional map to reduce the number of duplicate items added to the pqueue
            // Key is the node index in the provided graph
            // Value is cost-to-come
            std::unordered_map<int64_t, double> queue_members_map;
            // Key is the node index in the provided graph
            // Value is a pair<backpointer, cost-to-come>
            // backpointer is the parent index in the provided graph
            std::unordered_map<int64_t, std::pair<int64_t, double>> explored;
            // Initialize
            queue.push(arc_helpers::AstarPQueueElement(start_index, -1, 0.0, heuristic_function(start_index)));
            if (limit_pqueue_duplicates)
            {
                queue_members_map[start_index] = 0.0;
            }
            // Search
            while (queue.size() > 0)
            {
                // Get the top of the priority queue
                const arc_helpers::AstarPQueueElement top_node = queue.top();
                queue.pop();
                // Remove from queue map if necessary
                if (limit_pqueue_duplicates)
                {
                    queue_members_map.erase(top_node.NodeID());
                }
                // Check if the node has already been discovered
                const auto node_explored_find_itr = explored.find(top_node.NodeID());
                // We have not been here before, or it is cheaper now
                const bool node_in_explored = (node_explored_find_itr != explored.end());
                const bool explored_node_is_better = (node_in_explored) ? (top_node.CostToCome() >= node_explored_find_itr->second.second) : false;
                if (!explored_node_is_better)
                {
                    // Add to the explored list
                    explored[top_node.NodeID()] = std::make_pair(top_node.Backpointer(), top_node.CostToCome());
                    // Check if we have reached the goal
                    if (top_node.NodeID() == goal_index)
                    {
                        break;
                    }
                    // Explore and add the children
                    const std::vector<GraphEdge>& out_edges = graph.GetNodeImmutable(top_node.NodeID()).GetOutEdgesImmutable();
                    for (size_t out_edge_idx = 0; out_edge_idx < out_edges.size(); out_edge_idx++)
                    {
                        // Get the next potential child node
                        const GraphEdge& current_out_edge = out_edges[out_edge_idx];
                        const int64_t child_node_index = current_out_edge.GetToIndex();
                        // Check if the top node->child edge is valid
                        if (edge_validity_check_fn(graph, current_out_edge))
                        {
                            // Compute the cost-to-come for the new child
                            const double parent_cost_to_come = top_node.CostToCome();
                            const double parent_to_child_cost = distance_fn(graph, current_out_edge);
                            const double child_cost_to_come = parent_cost_to_come + parent_to_child_cost;
                            // Check if the child state has already been explored
                            const auto child_explored_find_itr = explored.find(child_node_index);
                            // It is not in the explored list, or is there with a higher cost-to-come
                            const bool child_in_explored = (child_explored_find_itr != explored.end());
                            const bool explored_child_is_better = (child_in_explored) ? (child_cost_to_come >= child_explored_find_itr->second.second) : false;
                            // Check if the child state is already in the queue
                            bool queue_is_better = false;
                            if (limit_pqueue_duplicates)
                            {
                                const auto queue_members_map_itr = queue_members_map.find(child_node_index);
                                const bool in_queue = (queue_members_map_itr != queue_members_map.end());
                                queue_is_better = (in_queue) ? (child_cost_to_come >= queue_members_map_itr->second) : false;
                            }
                            // Only add the new state if we need to
                            if (!explored_child_is_better && !queue_is_better)
                            {
                                // Compute the heuristic for the child
                                const double child_heuristic = heuristic_function(child_node_index);
                                // Compute the child value
                                const double child_value = child_cost_to_come + child_heuristic;
                                queue.push(arc_helpers::AstarPQueueElement(child_node_index, top_node.NodeID(), child_cost_to_come, child_value));
                            }
                        }
                    }
                }
            }
            return arc_helpers::ExtractAstarResult(explored, start_index, goal_index);
        }

        static arc_helpers::AstarResult PerformLazyAstar(const Graph<NodeValueType, Allocator>& graph, const int64_t start_index, const int64_t goal_index, const std::function<bool(const NodeValueType&, const NodeValueType&)>& edge_validity_check_fn, const std::function<double(const NodeValueType&, const NodeValueType&)>& distance_fn, const std::function<double(const NodeValueType&, const NodeValueType&)>& heuristic_fn, const bool limit_pqueue_duplicates)
        {
            const auto edge_validity_check_function = [&] (const Graph<NodeValueType, Allocator>& search_graph, const GraphEdge& edge) { return edge_validity_check_fn(search_graph.GetNodeImmutable(edge.GetFromIndex()).GetValueImmutable(), search_graph.GetNodeImmutable(edge.GetToIndex()).GetValueImmutable()); };
            const auto distance_function = [&] (const Graph<NodeValueType, Allocator>& search_graph, const GraphEdge& edge) { return distance_fn(search_graph.GetNodeImmutable(edge.GetFromIndex()).GetValueImmutable(), search_graph.GetNodeImmutable(edge.GetToIndex()).GetValueImmutable()); };
            return PerformLazyAstar(graph, start_index, goal_index, edge_validity_check_function, distance_function, heuristic_fn, limit_pqueue_duplicates);
        }

        static arc_helpers::AstarResult PerformAstar(const Graph<NodeValueType, Allocator>& graph, const int64_t start_index, const int64_t goal_index, const std::function<double(const NodeValueType&, const NodeValueType&)>& heuristic_fn, const bool limit_pqueue_duplicates)
        {
            const auto edge_validity_check_function = [&] (const Graph<NodeValueType, Allocator>& search_graph, const GraphEdge& edge) { UNUSED(search_graph); if (edge.GetWeight() < std::numeric_limits<double>::infinity()) { return true; } else { return false; } };
            const auto distance_function = [&] (const Graph<NodeValueType, Allocator>& search_graph, const GraphEdge& edge) { UNUSED(search_graph); return edge.GetWeight(); };
            return PerformLazyAstar(graph, start_index, goal_index, edge_validity_check_function, distance_function, heuristic_fn, limit_pqueue_duplicates);
        }
    };

}

#endif // DIJKSTRAS_HPP