#include <boost/program_options.hpp>
#include <iostream>
#include <vector>
#include "core/Value.hpp"
#include "decpomdp/discrete/DecPOMDPFlat.hpp"
#include "decpomdp/discrete/JointActionSpaceFlat.hpp"
#include "decpomdp/discrete/JointBeliefFlat.hpp"
#include "decpomdp/discrete/JointObservationSpaceFlat.hpp"
#include "madp_wrapper/MADPWrapper.h"
#include "madp_wrapper/MADPWrapperUtilities.h"
#include "observationmodel/ObservationModelFlat.hpp"
#include "policy/graph/FSC.hpp"
#include "policy/graph/FSCPolicy.hpp"
#include "rewardmodel/RewardModelFlat.hpp"
#include "statetransitionmodel/StateTransitionModelFlat.hpp"

int main(int argc, char** argv) {
  int tstart;
  int tlast;
  unsigned int seed;

  namespace po = boost::program_options;
  po::options_description config(
      "Compute the exact value of a joint policy in a Dec-rhoPOMDP"
      "\nUsage: " +
      std::string(argv[0]) + " [OPTION]... [DPOMDP-FILE]\nOptions");
  config.add_options()("help", "produce help message")(
      "start,s", po::value<int>(&tstart)->default_value(0),
      "starting time step")("last,l", po::value<int>(&tlast)->default_value(1),
                            "last time step")(
      "files", po::value<std::vector<std::string>>()->multitoken(),
      "[AGENT0_POLICY] ... [AGENTN_POLICY] individual policy files")(
      "use-entropy-reward,e", po::bool_switch()->default_value(false),
      "toggle to use expected posterior entropy as final step reward")(
      "seed,g", po::value<unsigned int>(&seed)->default_value(1234567890),
      "RNG seed");

  po::positional_options_description pp;
  pp.add("dpomdp-file", -1);

  po::options_description hidden("Hidden options");
  hidden.add_options()("dpomdp-file", po::value<std::string>(), "input file");

  po::options_description cmdline_options;
  cmdline_options.add(config).add(hidden);

  po::variables_map vm;
  po::store(po::command_line_parser(argc, argv)
                .options(cmdline_options)
                .positional(pp)
                .run(),
            vm);
  po::notify(vm);
  if (vm.count("help") || argc == 1) {
    std::cout << config << std::endl;
    return 1;
  }
  if (!vm.count("dpomdp-file")) {
    std::cout << "error: You must specify an input dpomdp file" << std::endl;
    return 1;
  }

  if (tlast < tstart) {
    std::cout << "error: last time step must be less than or equal to starting "
                 "time step\n";
    return 1;
  }

  npgi::madpwrapper::MADPDecPOMDPDiscrete madp(
      vm["dpomdp-file"].as<std::string>());
  std::cout << "Loaded problem file " << vm["dpomdp-file"].as<std::string>()
            << std::endl;
  npgi::DecPOMDPFlat d = npgi::madpwrapper::to_flat_decpomdp(madp);
  npgi::JointBeliefFlat initial_state_distribution(
      npgi::madpwrapper::initial_state_distribution(madp));

  const std::size_t num_files =
      vm["files"].as<std::vector<std::string>>().size();
  if (num_files != madp.num_agents()) {
    std::cout << "error: number of policy files provided (" << num_files
              << ") does not match number of agents (" << madp.num_agents()
              << ")\n";
    return 1;
  }

  using pg_type = npgi::fsc_graph_t<npgi::DiscreteLocalAction<>, npgi::DiscreteLocalObservation<>>;
  std::map<npgi::Agent<>, pg_type> fscs;
  for (std::size_t i = 0; i < num_files; ++i) {
    const std::string filename =
        vm["files"].as<std::vector<std::string>>().at(i);
    std::ifstream is(filename);
    if (!is.is_open()) {
      std::cout << "error: failed to open file: " << filename << '\n';
      return 1;
    } else {
      is >> fscs[i];
    }
    std::cout << "agent " << i << ", read policy\n" << fscs[i] << std::endl;
  }
  auto policy = npgi::policy::FSCPolicy(tstart, tlast, fscs, d.A(), d.Z());

  using decpomdp_type = npgi::DecPOMDPFlat<std::size_t, std::size_t, std::size_t, double, int>;
  auto final_reward =
      (vm["use-entropy-reward"].as<bool>()
           ? [](const typename decpomdp_type::joint_belief_type&
                    b) { return -b.entropy(); }
           : [](const typename decpomdp_type::joint_belief_type& b) {
               return 0.0;
             });

  std::cout << "value = " << npgi::value(d, initial_state_distribution, policy, final_reward)
            << "\n";
  return 0;
}


//==================================================================================================
/**
  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_DEFINITION_REC_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_DEFINITION_REC_HPP_INCLUDED

#include <boost/simd/config.hpp>
#include <boost/simd/detail/dispatch/function/make_callable.hpp>
#include <boost/simd/detail/dispatch/hierarchy/functions.hpp>
#include <boost/simd/detail/dispatch.hpp>

namespace boost { namespace simd
{
  namespace tag
  {
    BOOST_DISPATCH_MAKE_TAG(ext, rec_, boost::dispatch::elementwise_<rec_>);
  }

  namespace ext
  {
    BOOST_DISPATCH_FUNCTION_DECLARATION(tag, rec_)
  }

  BOOST_DISPATCH_CALLABLE_DEFINITION(tag::rec_,rec);
} }

#endif


#include <stdio.h>
#include <cfloat>
#include <math.h>
#include <vector>
#include <ctime>
#include <cstdlib>
#include <unistd.h>
#include <iostream>
#include <fstream>
#include <string>
#include <cstddef>

#include "opencv2/opencv.hpp"

#define GL_GLEXT_PROTOTYPES
#include "GL/gl.h"
#include "GL/glext.h"
#include "GL/osmesa.h"

#include <cuda_runtime.h>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <sophus/se3.hpp>

#include <assimp/cimport.h>
#include <assimp/scene.h>

template <typename Derived>
inline void operator >>(std::istream & stream, Eigen::MatrixBase<Derived> & M)
{

    for (int r = 0; r < M.rows(); ++r) {
        for (int c = 0; c < M.cols(); ++c) {
            stream >> M(r,c);
        }
    }

}

template<typename P> inline
void OpenGlMatrix(const Eigen::Matrix<P,4,4>& mat, float* m)
{
    for(int r=0; r<4; ++r ) {
        for(int c=0; c<4; ++c ) {
            m[c*4+r] = mat(r,c);
        }
    }
}

unsigned char class_colors[22][3] = {{255, 255, 255}, {255, 0, 0}, {0, 255, 0}, {0, 0, 255}, {255, 255, 0}, {255, 0, 255}, {0, 255, 255},
                              {128, 0, 0}, {0, 128, 0}, {0, 0, 128}, {128, 128, 0}, {128, 0, 128}, {0, 128, 128},
                              {64, 0, 0}, {0, 64, 0}, {0, 0, 64}, {64, 64, 0}, {64, 0, 64}, {0, 64, 64},
                              {192, 0, 0}, {0, 192, 0}, {0, 0, 192}};

typedef struct
{
  int num_vertices;
  int num_faces;
  void* vertices;
  void* faces;
}MyModel;

class Render
{
 public:

  Render() {};
  Render(std::string model_file);
  ~Render();

  void setup(std::string model_file);
  float render(const float* data, const int* labels, const float* rois, int num_rois, int num_gt, int num_classes, int width, int height,
               const float* poses_gt, const float* poses_pred, const float* poses_init, float* bottom_diff, const float* meta_data, int num_meta_data);
  void loadModels(const std::string filename);
  void loadTexturedMesh(const std::string filename, std::string & texture_name, MyModel* model);
  void initializeBuffers(MyModel* model, std::string textureName, GLuint vertexbuffer, GLuint indexbuffer);
  void ProjectionMatrixRDF_TopLeft(float* m, int w, int h, float fu, float fv, float u0, float v0, float zNear, float zFar );
  void write_ppm(const char *filename, const GLubyte *buffer, int width, int height);
  void print_matrix(float *m);

 private:
  int counter_;

  // 3D models
  std::vector<MyModel*> models_;
  std::vector<std::string> texture_names_;
};


//
// Tracker Pattern Recognition based on Robust Helix Fit
//
//
// Original author D. Brown and G. Tassielli
//

#include "art/Framework/Principal/Event.h"
#include "Offline/GeometryService/inc/GeomHandle.hh"
#include "fhiclcpp/types/Atom.h"
#include "fhiclcpp/types/Sequence.h"
#include "art/Framework/Principal/Handle.h"
#include "art/Framework/Core/EDProducer.h"
#include "art/Framework/Core/ModuleMacros.h"
#include "art_root_io/TFileService.h"
#include "Offline/GeneralUtilities/inc/Angles.hh"
#include "Offline/Mu2eUtilities/inc/MVATools.hh"

#include "Offline/ProditionsService/inc/ProditionsHandle.hh"

#include "Offline/DataProducts/inc/Helicity.hh"
#include "Offline/RecoDataProducts/inc/StrawHit.hh"
#include "Offline/RecoDataProducts/inc/StrawHitPosition.hh"
#include "Offline/RecoDataProducts/inc/StrawHitFlag.hh"
#include "Offline/RecoDataProducts/inc/TimeCluster.hh"
#include "Offline/RecoDataProducts/inc/HelixSeed.hh"
#include "Offline/RecoDataProducts/inc/TrkFitFlag.hh"

#include "Offline/TrkReco/inc/TrkTimeCalculator.hh"
#include "Offline/TrackerGeom/inc/Tracker.hh"
#include "Offline/CalorimeterGeom/inc/DiskCalorimeter.hh"

#include "BTrk/BaBar/BaBar.hh"
#include "Offline/TrkReco/inc/RobustHelixFit.hh"

#include "Offline/ConfigTools/inc/ConfigFileLookupPolicy.hh"
#include "Offline/Mu2eUtilities/inc/ModuleHistToolBase.hh"
#include "Offline/Mu2eUtilities/inc/polyAtan2.hh"
#include "Offline/Mu2eUtilities/inc/HelixTool.hh"
#include "art/Utilities/make_tool.h"

#include "Offline/TrkPatRec/inc/RobustHelixFinder_types.hh"
#include "Offline/TrkReco/inc/RobustHelixFinderData.hh"
#include "Offline/TrkReco/inc/TrkFaceData.hh"

#include "CLHEP/Units/PhysicalConstants.h"
#include "CLHEP/Matrix/Vector.h"
#include "CLHEP/Matrix/SymMatrix.h"

#include <boost/accumulators/accumulators.hpp>
#include "Offline/boost_fix/accumulators/statistics/stats.hpp"
#include "Offline/boost_fix/accumulators/statistics.hpp"
#include <boost/accumulators/statistics/median.hpp>

#include "TH1F.h"
#include "Math/VectorUtil.h"
#include "TVector2.h"

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

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

namespace {
  // comparison functor for sorting by z
  struct zcomp : public std::binary_function<mu2e::ComboHit,mu2e::ComboHit,bool> {
    bool operator()(mu2e::ComboHit const& p1, mu2e::ComboHit const& p2) { return p1._pos.z() < p2._pos.z(); }
  };

  // comparison functor for sorting byuniquePanel ID
  struct panelcomp : public std::binary_function<mu2e::ComboHit,mu2e::ComboHit,bool> {
    bool operator()(mu2e::ComboHit const& p1, mu2e::ComboHit const& p2) { return p1.strawId().uniquePanel() < p2.strawId().uniquePanel(); }
  };
  struct HelixHitMVA
  {
    std::vector <float> _pars,_pars2;
    float& _dtrans; // distance from hit to helix perp to the wrire
    float& _dwire;  // distance from hit to helix along the wrire
    float& _chisq;  // chisq of spatial information, using average errors
    float& _dt;     // time difference of hit WRT average
    float& _drho;   // hit transverse radius minus helix radius
    float& _dphi;   // hit azimuth minus helix azimuth (at the hit z)
    float& _rwdot;  // dot product between circle radial direction and wire direction
    float& _hrho;   // helix transverse radius (at the hit z)
    float& _hhrho;  // hit transverse radius
    //HelixHitMVA() : _pars(9,0.0),_dtrans(_pars[0]),_dwire(_pars[1]),_chisq(_pars[2]),_dt(_pars[3]),
    // _drho(_pars[4]),_dphi(_pars[5]),_rwdot(_pars[6]),_hrho(_pars[7]),_hhrho(_pars[8]){}
    HelixHitMVA() : _pars(7,0.0),_pars2(2,0.0),_dtrans(_pars[0]),_dwire(_pars[1]),_chisq(_pars[2]),_dt(_pars[3]),
		    _drho(_pars[4]),_dphi(_pars[5]),_rwdot(_pars[6]),_hrho(_pars[0]),_hhrho(_pars2[1]) {}
  };

}

namespace mu2e {

  class RobustHelixFinder : public art::EDProducer {
  public:

    struct Config
    {
      using Name = fhicl::Name;
      using Comment = fhicl::Comment;
      fhicl::Atom<int>                      diagLevel{            Name("DiagLevel"),            Comment("Diag"),0 };
      fhicl::Atom<int>                      debugLevel{           Name("DebugLevel"),           Comment("Debug"),0 };
      fhicl::Atom<int>                      printFrequency{       Name("PrintFrequency"),       Comment("Print Frequency") };
      fhicl::Atom<bool>                     PrefilterHits{        Name("PrefilterHits"),        Comment("Produce prefiltered hit collections")  };
      fhicl::Atom<bool>                     UpdateStereoHits{     Name("UpdateStereoHits"),     Comment("Update stereo hits") };
      fhicl::Atom<int>                      minNStrawHits{        Name("MinNStrawHits"),        Comment("Minimum number of straw hits") };
      fhicl::Atom<float>                    AveragePitch{         Name("AveragePitch"),         Comment("Average helix pitch") };
      fhicl::Atom<float>                    MaxChi2dXY{           Name("MaxChi2dXY"),           Comment("Maximum Chi2 dXY") };
      fhicl::Atom<float>                    MaxChi2dZPhi{         Name("MaxChi2dZPhi"),         Comment("Maximum Chi2 dZPhi") };
      fhicl::Atom<float>                    MaxHitPhiChi2{        Name("MaxHitPhiChi2"),        Comment("Maximum phi hit Chi2") };
      fhicl::Atom<float>                    MaxRadiusDiff{        Name("MaxRadiusDiff"),        Comment("Maximum radius difference, in mm") };
      fhicl::Atom<float>                    MaxRPull{             Name("MaxRPull"),             Comment("Maximum radius pull, unitless") };
      fhicl::Atom<bool>                     targetconsistent_init{Name("targetconsistent_init"),Comment("Initial target consistency condition") };
      fhicl::Atom<bool>                     targetconsistent{     Name("targetconsistent"),     Comment("Target consistency condition") };
      fhicl::Atom<float>                    RPullScaleF{          Name("RPullScaleF"),          Comment("Radius pull scale factor, unitless") };
      fhicl::Atom<float>                    MaxPhiHitSeparation{  Name("MaxPhiHitSeparation"),  Comment("Maximum phi hit separation") };
      fhicl::Sequence<std::string>          SaveHelixFlag{        Name("SaveHelixFlag"),        Comment("Save Helix Flag, 'HelixOK'") };
      fhicl::Sequence<int>                  Helicities{           Name("Helicities"),           Comment("Helicity values") };
      fhicl::Atom<unsigned>                 MaxIterations{        Name("MaxIterations"),        Comment("Maximum iterations over outlier removal") };
      fhicl::Atom<float>                    CentralRadialResolution{Name("CentralRadialResolution"),Comment("Central radial resolution") };
      fhicl::Atom<float>                    CentralPerpResolution{Name("CentralPerpResolution"),Comment("Central perpendicular resolution") };
      fhicl::Atom<float>                    MaxWireDistance{      Name("MaxWireDistance"),      Comment("Maximum distance along wire") };
      fhicl::Atom<float>                    MaxTransDistance{     Name("MaxTransDistance"),     Comment("Maximum distance perp to wire (and z)") };
      fhicl::Atom<float>                    MaxChisquared{        Name("MaxChisquared"),        Comment("Maximum chi squared") };
      fhicl::Atom<float>                    MaxRWDot{             Name("MaxRWDot"),             Comment("Maximum RW Dot") };
      fhicl::Atom<float>                    MinRadiusErr{         Name("MinRadiusErr"),         Comment("Minimum radius error, in mm") };
      fhicl::Atom<bool>                     UseHitMVA{            Name("UseHitMVA"),            Comment("Use hit MVA flag") };
      fhicl::Atom<float>                    MinMVA{               Name("MinMVA"),               Comment("Minimum MVA output to define an outlier") };
      fhicl::Atom<bool>                     UseTripletArea{       Name("UseTripletArea"),       Comment("Use triplet area flag") };
      fhicl::Atom<art::InputTag>            ComboHitCollection{   Name("ComboHitCollection"),   Comment("ComboHit collection name") };
      fhicl::Atom<art::InputTag>            TimeClusterCollection{Name("TimeClusterCollection"),Comment("TimeCluster collection name") };
      fhicl::Sequence<std::string>          HitSelectionBits{     Name("HitSelectionBits"),     Comment("Hit selection bits") };
      fhicl::Sequence<std::string>          HitBackgroundBits{    Name("HitBackgroundBits"),    Comment("Hit background bits") };
      fhicl::Table<MVATools::Config>        HelixStereoHitMVA{    Name("HelixStereoHitMVA"),    Comment("Helix Stereo Hit MVA Configuration") };
      fhicl::Table<MVATools::Config>        HelixNonStereoHitMVA{ Name("HelixNonStereoHitMVA"), Comment("Helix Non Stereo Hit MVA Configuration") };
      fhicl::Table<RobustHelixFit::Config>  HelixFitter{          Name("HelixFitter"),          Comment("Robust Helix Fit config") };
      fhicl::Table<RobustHelixFinderTypes::Config>DiagPlugin{     Name("DiagPlugin"),           Comment("Diag plugin") };
      fhicl::Table<TrkTimeCalculator::Config>T0Calculator{        Name("T0Calculator"),         Comment("Track Time Calculator config") };
      fhicl::Atom<bool>                     UpdateStereo{         Name("UpdateStereo"),         Comment("Update Stereo") };
    };

    explicit RobustHelixFinder(const art::EDProducer::Table<Config>& config);
    virtual ~RobustHelixFinder();
    virtual void beginJob();
    virtual void beginRun(art::Run&   run   );
    virtual void produce(art::Event& event );

  private:
    int                                 _diag,_debug;
    int                                 _printfreq;
    bool				_prefilter; // prefilter hits based on sector
    bool				_updatestereo; // update the stereo hit positions each iteration
    int 				_minnsh; // minimum # of strawHits to work with
    float			        _pitch; // average pitch to assume in time calculations
    float                               _maxchi2dxy;
    float                               _maxchi2dzphi;
    float                               _maxphihitchi2;
    float				_maxdr; // maximum hit-helix radius difference
    float				_maxrpull; // maximum hit-helix radius difference pull
    bool                                _targetconInit;//require the firs circle fit to intersect the Al stopping Target
    bool                                _targetcon;//require the circle fit to intersect the Al stopping Target
    float                               _rpullScaleF;//need to scale the radial pull in filterCircleHits
    float				_maxphisep; // maximum separation in global azimuth of hits
    TrkFitFlag				_saveflag; // write out all helices that satisfy these flags
    unsigned				_maxniter;  // maximum # of iterations over outlier filtering + fitting
    float				_cradres; // average center resolution along center position (mm)
    float				_cperpres; // average center resolution perp to center position (mm)
    float				_maxdwire; // outlier cut on distance between hit and helix along wire
    float				_maxdtrans; // outlier cut on distance between hit and helix perp to wire
    float				_maxchisq; // outlier cut on chisquared
    float				_maxrwdot; // outlier cut on angle between radial direction and wire: smaller is better
    float				_minrerr; // minimum radius error

    bool				_usemva; // use MVA to cut outliers
    float                               _minmva; // outlier cut on MVA
    bool                                _useTripletAreaWt;

    art::ProductToken<ComboHitCollection> const _chToken;
    art::ProductToken<TimeClusterCollection> const _tcToken;

    StrawHitFlag  _hsel, _hbkg;

    MVATools _stmva, _nsmva;
    HelixHitMVA _vmva; // input variables to TMVA for filtering hits

    TH1F* _niter, *_niterxy, *_niterfz, *_nitermva;

    RobustHelixFit   _hfit;

    std::vector<Helicity> _hels; // helicity values to fit
    TrkTimeCalculator _ttcalc;
    StrawHitFlag      _outlier;
    bool              _updateStereo;

    std::unique_ptr<ModuleHistToolBase>   _hmanager;
    RobustHelixFinderTypes::Data_t        _data;
    RobustHelixFinderData                 _hfResult;

    ProditionsHandle<Tracker> _alignedTracker_h;
    const Tracker* _tracker;

    void     findHelices(ComboHitCollection& chcol, const TimeClusterCollection& tccol);
    void     prefilterHits(RobustHelixFinderData& helixData, int& nFilteredStrawHits);
    unsigned filterCircleHits(RobustHelixFinderData& helixData);
    bool     filterHits(RobustHelixFinderData& helixData);
    void     fillMVA(RobustHelixFinderData& helixData);
    bool     filterHitsMVA(RobustHelixFinderData& helixData);
    void     updateT0(RobustHelixFinderData& helixData);
    bool     updateStereo(RobustHelixFinderData& helixData);
    unsigned hitCount(RobustHelixFinderData& helixData);
    void     pickBestHelix      (std::vector<HelixSeed>& HelVec, int &Index_best);
    void     fillFaceOrderedHits(RobustHelixFinderData& helixData);
    void     fillGoodHits       (RobustHelixFinderData& helixData);
    void     fitHelix           (RobustHelixFinderData& helixData);
    void     refitHelix         (RobustHelixFinderData& helixData);
    void     fillPluginDiag     (RobustHelixFinderData& helixData, int helCounter);
    void     updateHelixXYInfo  (RobustHelixFinderData& helixData);
    void     updateHelixZPhiInfo(RobustHelixFinderData& helixData);
  };

  RobustHelixFinder::RobustHelixFinder(const art::EDProducer::Table<Config>& config):
     art::EDProducer{config},
    _diag        (config().diagLevel()),
    _debug       (config().debugLevel()),
    _printfreq   (config().printFrequency()),
    _prefilter   (config().PrefilterHits()),
    _updatestereo(config().UpdateStereoHits()),
    _minnsh      (config().minNStrawHits()),
    _pitch       (config().AveragePitch()),
    _maxchi2dxy  (config().MaxChi2dXY()),
    _maxchi2dzphi(config().MaxChi2dZPhi()),
    _maxphihitchi2(config().MaxHitPhiChi2()),
    _maxdr	 (config().MaxRadiusDiff()), // mm
    _maxrpull	 (config().MaxRPull()), // unitless
    _targetconInit(config().targetconsistent_init()),
    _targetcon   (config().targetconsistent()),
    _rpullScaleF (config().RPullScaleF()), // unitless
    _maxphisep	 (config().MaxPhiHitSeparation()),
    _saveflag    (config().SaveHelixFlag()),
    _maxniter    (config().MaxIterations()), // iterations over outlier removal
    _cradres     (config().CentralRadialResolution()),
    _cperpres    (config().CentralPerpResolution()),
    _maxdwire    (config().MaxWireDistance()), // max distance along wire
    _maxdtrans   (config().MaxTransDistance()), // max distance perp to wire (and z)
    _maxchisq    (config().MaxChisquared()), //100.0)), // max chisquared
    _maxrwdot	 (config().MaxRWDot()),
    _minrerr     (config().MinRadiusErr()), // mm
    _usemva      (config().UseHitMVA()),
    _minmva      (config().MinMVA()), // min MVA output to define an outlier
    _useTripletAreaWt(config().UseTripletArea()),
    _chToken     {consumes<ComboHitCollection>(config().ComboHitCollection()) },
    _tcToken     {consumes<TimeClusterCollection>(config().TimeClusterCollection()) },
    _hsel        (config().HitSelectionBits()),
    _hbkg        (config().HitBackgroundBits()),
    _stmva       (config().HelixStereoHitMVA()),
    _nsmva       (config().HelixNonStereoHitMVA()),
    _hfit        (config().HelixFitter()),
    _ttcalc      (config().T0Calculator()),
    _outlier     (StrawHitFlag::outlier),
    _updateStereo(config().UpdateStereo())
    {
      std::vector<int> helvals = config().Helicities();
      for(auto hv : helvals) {
	Helicity hel(hv);
	_hels.push_back(hel);
	produces<HelixSeedCollection>(Helicity::name(hel));
      }

      if (_diag != 0) _hmanager = art::make_tool<ModuleHistToolBase>(config().DiagPlugin," ");
      else            _hmanager = std::make_unique<ModuleHistToolBase>();
    }

  RobustHelixFinder::~RobustHelixFinder(){}

  //-----------------------------------------------------------------------------
  void RobustHelixFinder::beginRun(art::Run& ) {
    mu2e::GeomHandle<mu2e::Calorimeter> ch;

    _hfit.setCalorimeter(ch.get());
  }
  //--------------------------------------------------------------------------------

  void RobustHelixFinder::beginJob() {

    _stmva.initMVA();
    _nsmva.initMVA();
    if (_debug > 0)
      {
	std::cout << "RobustHeilxFinder Stereo Hit MVA parameters: " << std::endl;
	_stmva.showMVA();
	std::cout << "RobustHeilxFinder Non-Stereo Hit MVA parameters: " << std::endl;
	_nsmva.showMVA();
      }

    if (_diag > 0){
      art::ServiceHandle<art::TFileService> tfs;
      _niter = tfs->make<TH1F>( "niter" , "Number of Fit Iteraions",201,-0.5,200.5);
      _niterxy = tfs->make<TH1F>( "niterxy" , "Number of XY Fit Iteraions",201,-0.5,200.5);
      _niterfz = tfs->make<TH1F>( "niterfz" , "Number of FZ Fit Iteraions",201,-0.5,200.5);
      _nitermva = tfs->make<TH1F>( "nitermva" , "Number of MVA Fit Iteraions",201,-0.5,200.5);
      _hmanager->bookHistograms(tfs);
    }
  }

  void RobustHelixFinder::produce(art::Event& event ) {

    _tracker = _alignedTracker_h.getPtr(event.id()).get();
    _hfit.setTracker    (_tracker);

    // find input
    auto const& tcH = event.getValidHandle(_tcToken);
    const TimeClusterCollection& tccol(*tcH);

    auto const& chH = event.getValidHandle(_chToken);
    const ComboHitCollection& chcol(*chH);

    // create output: seperate by helicity
    std::map<Helicity,unique_ptr<HelixSeedCollection>> helcols;
    int counter(0);
    for( auto const& hel : _hels) {
      helcols[hel] = unique_ptr<HelixSeedCollection>(new HelixSeedCollection());
      _data.nseeds [counter] = 0;
      ++counter;
    }

    _data.event       = &event;
    //    _data.result      = &_hfit;
    _data.nTimePeaks  = tccol.size();

    _hfResult._chcol  = &chcol;

    // create initial helicies from time clusters: to begin, don't specificy helicity
    for (size_t index=0;index< tccol.size();++index) {
      const auto& tclust = tccol[index];
      HelixSeed hseed;
      hseed._status.merge(TrkFitFlag::TPRHelix);
      //clear the variables in hfResult
      _hfResult.clearTempVariables();

      //set variables used for searching the helix candidate
      _hfResult._hseed              = hseed;
      _hfResult._timeCluster        = &tclust;
      _hfResult._hseed._hhits.setParent(chcol.parent());
      _hfResult._hseed._t0          = tclust._t0;
      _hfResult._hseed._timeCluster = art::Ptr<TimeCluster>(tcH,index);
      // copy combo hits
      fillFaceOrderedHits(_hfResult);

      //skip the reconstruction if there are few strawHits
      if (_hfResult._nFiltStrawHits < _minnsh)                  continue;

      // filter hits and test
      int nFilteredSh(0);
      if (_prefilter) prefilterHits(_hfResult,nFilteredSh);

      if ((_hfResult._nFiltStrawHits - nFilteredSh) < _minnsh)  continue;

      _hfResult._hseed._status.merge(TrkFitFlag::hitsOK);
      if (_diag) _hfResult._diag.circleFitCounter = 0;

      // initial circle fit

	_hfit.fitCircle(_hfResult, _targetconInit, _useTripletAreaWt);//require consistency for the trajectory of being produced in the Al stopping target

      if (_diag) {
	_hfResult._diag.nShFitCircle = _hfResult._nXYSh;
	_hfResult._diag.nChFitCircle = _hfResult._sxy.qn()-1;//take into account one hit form the stopping target center
      }
      //check the number of points associated with the result of the circle fit
      // if (_hfResult._nXYSh < _minnsh)                           continue;

      if (_hfResult._hseed._status.hasAnyProperty(TrkFitFlag::circleOK)) {
	// loop over helicities.
	unsigned    helCounter(0);
	HelixSeed   helixSeed_from_fitCircle = _hfResult._hseed;

	std::vector<HelixSeed>          helix_seed_vec;

	for(auto const& hel : _hels ) {
	  // tentatively put a copy with the specified helicity in the appropriate output vector
	  RobustHelixFinderData tmpResult(_hfResult);
	  tmpResult._hseed._helix._helicity = hel;

	  //fit the helix: refine the XY-circle fit + performs the ZPhi fit
	  // it also performs a clean-up of the hits with large residuals
	  fitHelix(tmpResult);

	  if (tmpResult._hseed.status().hasAnyProperty(_saveflag)){
	    //fill the hits in the HelixSeedCollection
	    fillGoodHits(tmpResult);

	    helix_seed_vec.push_back(tmpResult._hseed);

	    // HelixSeedCollection* hcol = helcols[hel].get();
	    // hcol->push_back(tmpResult._hseed);

	    if (_diag > 0) {
	      fillPluginDiag(tmpResult, helCounter);
	    }
	  }
	  ++helCounter;
	}//end loop over the helicity

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

	int    index_best(-1);
	pickBestHelix(helix_seed_vec, index_best);

	if ( (index_best>=0) && (index_best < 2) ){
	  Helicity              hel_best = helix_seed_vec[index_best]._helix._helicity;
	  HelixSeedCollection*  hcol     = helcols[hel_best].get();
	  hcol->push_back(helix_seed_vec[index_best]);
	} else if (index_best == 2){//both helices need to be saved

	  for (unsigned k=0; k<_hels.size(); ++k){
	    Helicity              hel   = helix_seed_vec[k]._helix._helicity;
	    HelixSeedCollection*  hcol  = helcols[hel].get();
	    hcol->push_back(helix_seed_vec[k]);
	  }
	}

      }

    }
    // put final collections into event
    if (_diag > 0) _hmanager->fillHistograms(&_data);

    for(auto const& hel : _hels ) {
      event.put(std::move(helcols[hel]),Helicity::name(hel));
    }
  }
//--------------------------------------------------------------------------------
// function to select the best Helix among the results of the two helicity hypo
//--------------------------------------------------------------------------------
  void  RobustHelixFinder::pickBestHelix(std::vector<HelixSeed>& HelVec, int &Index_best){
    if (HelVec.size() == 1) {
      Index_best = 0;
      return;
    }

    const HelixSeed           *h1, *h2;
    const ComboHitCollection  *tlist, *clist;
    int                        nh1, nh2;

    h1     = &HelVec[0];
//------------------------------------------------------------------------------
// check if an AlgorithmID collection has been created by the process
//-----------------------------------------------------------------------------
    tlist  = &h1->hits();
    nh1    = tlist->size();

    h2     = &HelVec[1];
//-----------------------------------------------------------------------------
// at Mu2e, 2 helices with different helicity could be duplicates of each other
//-----------------------------------------------------------------------------
    clist  = &h2->hits();
    nh2    = clist->size();
//-----------------------------------------------------------------------------
// pick the helix with the largest number of hits
//-----------------------------------------------------------------------------
    if (nh2 > nh1) {
//-----------------------------------------------------------------------------
// h2 is a winner, no need to save h1
//-----------------------------------------------------------------------------
      Index_best = 1;
      return;
    }
    else if (nh1 > nh2){
//-----------------------------------------------------------------------------
// h1 is a winner, mark h2 in hope that it will be OK, continue looping
//-----------------------------------------------------------------------------
      Index_best = 0;
      return;
    }

//-----------------------------------------------------------------------------
// in case they have the exact amount of hits, pick the one with better chi2dZphi
//-----------------------------------------------------------------------------
    if (nh1 == nh2) {
      float   chi2dZphi_h1 = h1->helix().chi2dZPhi();
      float   chi2dZphi_h2 = h2->helix().chi2dZPhi();
      if (chi2dZphi_h1 < chi2dZphi_h2){
	Index_best = 0;
	return;
      }else {
	Index_best = 1;
	return;
      }
    }

  }

//--------------------------------------------------------------------------------
//
//--------------------------------------------------------------------------------
  void RobustHelixFinder::fillGoodHits(RobustHelixFinderData& helixData){

    ComboHit*     hit(0);
    unsigned      nhits = helixData._chHitsToProcess.size();

    for (unsigned f=0; f<nhits; ++f){
      hit = &helixData._chHitsToProcess[f];
      if (hit->_flag.hasAnyProperty(_outlier))     continue;

      ComboHit                hhit(*hit);
      helixData._hseed._hhits.push_back(hhit);
    }

    if (_diag){
      HelixTool helTool(&helixData._hseed, _tracker);//_trackerRIn, _trackerROut, _trackerLength);
      helixData._diag.nLoops            = helTool.nLoops();
      helixData._diag.meanHitRadialDist = helTool.meanHitRadialDist();
    }
  }

  void RobustHelixFinder::fillMVA(RobustHelixFinderData& helixData)
  {
    RobustHelix& helix = helixData._hseed._helix;

    static XYZVectorF  zaxis(0.0,0.0,1.0); // unit in z direction
    ComboHit*      hhit(0);

    for (unsigned f=0; f<helixData._chHitsToProcess.size(); ++f){
      hhit = &helixData._chHitsToProcess[f];

      if (hhit->_flag.hasAnyProperty(_outlier))   continue;

      const XYZVectorF& wdir = hhit->wdir();
      XYZVectorF wtdir = zaxis.Cross(wdir); // transverse direction to the wire
      XYZVectorF cvec = PerpVector(hhit->pos() - helix.center(),GenVector::ZDir());// direction from the circle center to the hit
      XYZVectorF cdir = cvec.Unit();        // direction from the circle center to the hit
      XYZVectorF cperp = zaxis.Cross(cdir); // direction perp to the radius

      XYZVectorF hpos = hhit->pos();      // this sets the z position to the hit z
      helix.position(hpos);                     // this computes the helix expectation at that z
      XYZVectorF dh = hhit->pos() - hpos; // this is the vector between them

      _vmva._dtrans = fabs(dh.Dot(wtdir));              // transverse projection
      _vmva._dwire = fabs(dh.Dot(wdir));               // projection along wire direction
      _vmva._drho = fabs(sqrtf(cvec.mag2()) - helix.radius()); // radius difference
      _vmva._dphi = fabs(hhit->helixPhi() - helix.circleAzimuth(hhit->pos().z())); // azimuth difference WRT circle center
      _vmva._hhrho = sqrtf(cvec.mag2());            // hit transverse radius WRT circle center
      _vmva._hrho = sqrtf(hpos.Perp2());            // hit detector transverse radius
      _vmva._rwdot = fabs(wdir.Dot(cdir));  // compare directions of radius and wire

      // compute the total resolution including hit and helix parameters first along the wire
      float wres2 = std::pow(hhit->posRes(StrawHitPosition::wire),(int)2) +
	std::pow(_cradres*cdir.Dot(wdir),(int)2) +
	std::pow(_cperpres*cperp.Dot(wdir),(int)2);

      // transverse to the wires
      float wtres2 = std::pow(hhit->posRes(StrawHitPosition::trans),(int)2) +
	std::pow(_cradres*cdir.Dot(wtdir),(int)2) +
	std::pow(_cperpres*cperp.Dot(wtdir),(int)2);

      _vmva._chisq = sqrtf( _vmva._dwire*_vmva._dwire/wres2 + _vmva._dtrans*_vmva._dtrans/wtres2 );
      _vmva._dt = hhit->time() - helixData._hseed._t0.t0();

      if (hhit->_flag.hasAnyProperty(StrawHitFlag::stereo))
	{
	  hhit->_qual = _stmva.evalMVA(_vmva._pars);
	} else {
	hhit->_qual = _nsmva.evalMVA(_vmva._pars);
      }
    }
  }

  bool RobustHelixFinder::filterHitsMVA(RobustHelixFinderData& helixData)
  {
    bool           changed(false);
    ComboHit*      hhit(0);

    for (unsigned f=0; f<helixData._chHitsToProcess.size(); ++f){

      hhit = &helixData._chHitsToProcess[f];

      bool oldout = hhit->_flag.hasAnyProperty(_outlier);

      if (hhit->_qual < _minmva ) hhit->_flag.merge(_outlier);
      else                        hhit->_flag.clear(_outlier);

      changed |= oldout != hhit->_flag.hasAnyProperty(_outlier);
    }

    return changed;
  }

  // 3d selection on top of radial selection
  bool RobustHelixFinder::filterHits(RobustHelixFinderData& helixData)
  {
    RobustHelix& helix = helixData._hseed._helix;
    bool changed(false);
    static XYZVectorF zaxis(0.0,0.0,1.0); // unit in z direction
    int      nGoodSH(0);

    // loop over hits
    ComboHit*     hit(0);
    FaceZ_t*      facez;

    int           nhitsFace(0);
    float         chCounter(1e-10), chi2dZPhi(0);

    for (int f=0; f<StrawId::_ntotalfaces; ++f){
      facez     = &helixData._oTracker[f];

      float      minChi2(_maxchisq);
      HitInfo_t  indexBestComboHit;

      nhitsFace = facez->nChHits();
      if (nhitsFace == 0)                        continue;
      int        idFirstFaceCh(facez->idChBegin);
      for (int ip=0; ip<nhitsFace; ++ip){
	hit = &helixData._chHitsToProcess[idFirstFaceCh + ip];
	bool trash=hit->_flag.hasAnyProperty(_outlier);
	if (trash)                               continue;

	float hphi = polyAtan2(hit->pos().y(),hit->pos().x());//phi();
	float dphi = fabs(Angles::deltaPhi(hphi,helix.fcent()));

	const XYZVectorF& wdir = hit->wdir();
	XYZVectorF wtdir = zaxis.Cross(wdir);   // transverse direction to the wire
	XYZVectorF cvec = PerpVector(hit->pos() - helix.center(),GenVector::ZDir()); // direction from the circle center to the hit
	XYZVectorF cdir = cvec.Unit();          // direction from the circle center to the hit
	XYZVectorF cperp = zaxis.Cross(cdir);   // direction perp to the radius

	XYZVectorF hpos = hit->pos(); // this sets the z position to the hit z
	helix.position(hpos);                // this computes the helix expectation at that z
	XYZVectorF dh = hit->pos() - hpos;   // this is the vector between them
	float dtrans = fabs(dh.Dot(wtdir)); // transverse projection
	float dwire = fabs(dh.Dot(wdir));   // projection along wire direction

	// compute the total resolution including hit and helix parameters first along the wire
	float wres2 = std::pow(hit->posRes(StrawHitPosition::wire),(int)2) +
	  std::pow(_cradres*cdir.Dot(wdir),(int)2) +
	  std::pow(_cperpres*cperp.Dot(wdir),(int)2);
	// transverse to the wires
	float wtres2 = std::pow(hit->posRes(StrawHitPosition::trans),(int)2) +
	  std::pow(_cradres*cdir.Dot(wtdir),(int)2) +
	  std::pow(_cperpres*cperp.Dot(wtdir),(int)2);

	float chisq = dwire*dwire/wres2 + dtrans*dtrans/wtres2;

	if( dphi > _maxphisep || fabs(dwire) > _maxdwire || fabs(dtrans) > _maxdtrans || chisq > _maxchisq)
	  {
	    changed = true;
	  }

	if ( chisq <= minChi2)
	  {
	    minChi2 = chisq;

	    indexBestComboHit.face          = f;
	    indexBestComboHit.panel         = hit->strawId().uniquePanel();
	    indexBestComboHit.panelHitIndex = facez->idChBegin + ip;
	  }

	//flagg all hits within the face as outlier. Only the best found will be "cleared"
	hit->_flag.merge(_outlier);

      }//end loop over the panels

      //remove the outlier flag
      if (indexBestComboHit.face >=0 ) {
	hit     = &helixData._chHitsToProcess[indexBestComboHit.panelHitIndex];

	//remove the outlier flag
	hit->_flag.clear(StrawHitFlag::outlier);
	nGoodSH += hit->nStrawHits();

	chi2dZPhi += minChi2;
	chCounter += 1.;
      }
    }//end loop over the faces

    helixData._nZPhiSh = nGoodSH;

    //update the value of the chi2ZPhi
    helix._chi2dZPhi   = chi2dZPhi/chCounter;

    if (_diag) {
      helixData._diag.chi2dZPhi = chi2dZPhi/chCounter;
    }

    return changed;
  }

  void RobustHelixFinder::prefilterHits(RobustHelixFinderData& HelixData, int& NRemovedStrawHits)
  {
    // ComboHitCollection& hhits = HelixData._hseed._hhits;

    bool changed(true);
    // size_t nhit = hhits.size();
    int nhit = HelixData._nFiltComboHits;

    ComboHit*  hit(0);
    ComboHit*  worsthit(0);

    NRemovedStrawHits = 0;

    while (changed && nhit > 0)
      {
	nhit = 0;
	changed = false;
	accumulator_set<float, stats<tag::median(with_p_square_quantile) > > accx;
	accumulator_set<float, stats<tag::median(with_p_square_quantile) > > accy;

	for (unsigned f=0; f<HelixData._chHitsToProcess.size(); ++f){
	  hit =  &HelixData._chHitsToProcess[f];
	  bool trashHit=hit->_flag.hasAnyProperty(_outlier);
	  if (trashHit)                              continue;
	  accx(hit->_pos.x());
	  accy(hit->_pos.y());
	  ++nhit;
	}

	float mx = extract_result<tag::median>(accx);
	float my = extract_result<tag::median>(accy);
	float mphi = polyAtan2(my,mx);//atan2f(my,mx);

	float maxdphi{0.0};
	// auto worsthit = hhits.end();
	for (unsigned f=0; f<HelixData._chHitsToProcess.size(); ++f){
	  hit =  &HelixData._chHitsToProcess[f];
	  bool trashHit = hit->_flag.hasAnyProperty(_outlier);
	  if (trashHit)                              continue;
	  float phi  = polyAtan2(hit->pos().y(), hit->pos().x());//ihit->pos().phi();
	  float dphi = fabs(Angles::deltaPhi(phi,mphi));
	  if(dphi > maxdphi)
	    {
	      maxdphi = dphi;
	      worsthit = hit;
	    }
	}//end loop over the faces

	if (maxdphi > _maxphisep)
	  {
	    worsthit->_flag.merge(_outlier);
	    NRemovedStrawHits += worsthit->nStrawHits();
	    changed = true;
	  }
      }
  }

  void RobustHelixFinder::updateT0(RobustHelixFinderData& helixData)
  {
  // compute the pitch
    float pitch = helixData._hseed.helix().pitch();
    accumulator_set<float, stats<tag::weighted_variance(lazy)>, float > terr;
  // update t0 from calo cluster according to current pitch
    if (helixData._hseed.caloCluster().isNonnull()){
      float cwt = std::pow(1.0/_ttcalc.caloClusterTimeErr(),2);
      terr(_ttcalc.caloClusterTime(*helixData._hseed.caloCluster(),pitch),weight=cwt);
    }
    ComboHit*      hit(0);
    float hwt = std::pow(1.0/_ttcalc.strawHitTimeErr(),2);
    for (unsigned f=0; f<helixData._chHitsToProcess.size(); ++f){
      hit = &helixData._chHitsToProcess[f];
      if (hit->_flag.hasAnyProperty(_outlier))   continue;
      terr(_ttcalc.comboHitTime(*hit,pitch),weight=hwt);
    }//end faces loop
    if (sum_of_weights(terr) > 0.0)
      {
	helixData._hseed._t0._t0 = extract_result<tag::weighted_mean>(terr);
	helixData._hseed._t0._t0err = sqrtf(std::max(float(0.0),extract_result<tag::weighted_variance(lazy)>(terr))/extract_result<tag::count>(terr));
      }
  }

  //------------------------------------------------------------------------------------------
  void     RobustHelixFinder::fillFaceOrderedHits(RobustHelixFinderData& HelixData){

    const vector<StrawHitIndex>& shIndices = HelixData._timeCluster->hits();
    mu2e::RobustHelixFinderData::ChannelID cx, co;

    int     size  = shIndices.size();
    int     nFiltComboHits(0), nFiltStrawHits(0);
    // int nTotalStations = _tracker->nStations();
    //--------------------------------------------------------------------------------
    int loc;
    StrawHitFlag flag;

    //sort the hits by z coordinate
    ComboHitCollection ordChCol;
    ordChCol.reserve(size);

    for (int i=0; i<size; ++i) {
      loc = shIndices[i];
      const ComboHit& ch  = (*_hfResult._chcol)[loc];
      if(ch.flag().hasAnyProperty(_hsel) && !ch.flag().hasAnyProperty(_hbkg) ) {
	ordChCol.push_back(ComboHit(ch));
      }
    }
    std::sort(ordChCol.begin(), ordChCol.end(),panelcomp());//zcomp());

    if (_debug>0){
      printf("[RobustHelixFinder::FillHits]-----------------------------------------------------------\n");
      printf("[RobustHelixFinder::FillHits]     i     Face     Panel      X         Y         Z        \n");
      printf("[RobustHelixFinder::FillHits]-----------------------------------------------------------\n");
    }

    for (unsigned i=0; i<ordChCol.size(); ++i) {
      // loc = shIndices[i];
      // const ComboHit& ch  = _hfResult._chcol->at(loc);
      ComboHit& ch = ordChCol[i];

      //    if(ch.flag().hasAnyProperty(_hsel) && !ch.flag().hasAnyProperty(_hbkg) ) {
      ComboHit hhit(ch);
      hhit._flag.clear(StrawHitFlag::resolvedphi);

      _hfResult._chHitsToProcess.push_back(hhit);

      cx.Station                 = ch.strawId().station();//straw.id().getStation();
      cx.Plane                   = ch.strawId().plane() % 2;//straw.id().getPlane() % 2;
      cx.Face                    = ch.strawId().face();
      cx.Panel                   = ch.strawId().panel();//straw.id().getPanel();

      // get Z-ordered location
      HelixData.orderID(&cx, &co);

      int os       = co.Station;
      int of       = co.Face;
      int op       = co.Panel;

      _hfResult._chHitsWPos.push_back(XYWVec(hhit.pos(),  of, hhit.nStrawHits()));

      int       stationId = os;
      int       faceId    = of + stationId*StrawId::_nfaces*FaceZ_t::kNPlanesPerStation;//RobustHelixFinderData::kNFaces;
      // int       panelId   = op + faceId*RobustHelixDataFinderData::kNPanelsPerFace;
      FaceZ_t* fz        = &HelixData._oTracker[faceId];
      PanelZ_t*pz        = &fz->panelZs[op];

      //	pz->_chHitsToProcess.push_back(hhit);//[fz->fNHits] = hhit;
      //	pz->fNHits  = pz->fNHits + 1;
      if (pz->idChBegin < 0 ){
	pz->idChBegin = _hfResult._chHitsToProcess.size() - 1;
	pz->idChEnd   = _hfResult._chHitsToProcess.size();
      } else {
	pz->idChEnd   = _hfResult._chHitsToProcess.size();
      }

      if (fz->idChBegin < 0 ){
	fz->idChBegin = _hfResult._chHitsToProcess.size() - 1;
	fz->idChEnd   = _hfResult._chHitsToProcess.size();
      } else {
	fz->idChEnd   = _hfResult._chHitsToProcess.size();
      }

      if (_debug>0){
	printf("[RobustHelixFinder::FillHits] %4i %6i %10i %10.3f %10.3f %10.3f\n", nFiltComboHits, faceId, op, ch.pos().x(), ch.pos().y(), ch.pos().z() );
      }

      // if (pz->nChHits() > PanelZ_t::kNMaxPanelHits) printf("[RobustHelixDataFinderAlg::fillFaceOrderedHits] number of hits with the panel exceed the limit: NHits =  %i MaxNHits = %i\n", pz->fNHits, PanelZ_t::kNMaxPanelHits);
      ++nFiltComboHits;
      nFiltStrawHits += ch.nStrawHits();
      //      }
    }
    // }

    HelixData._nFiltComboHits = nFiltComboHits;  //ComboHit counter
    HelixData._nFiltStrawHits = nFiltStrawHits;  //StrawHit counter

    if (_diag) {
      HelixData._diag.nChPPanel = 0;
      HelixData._diag.nChHits   = HelixData._chHitsToProcess.size();

      FaceZ_t*      facez;
      PanelZ_t*     panelz;

      int           nhitsFace(0);

      if (_debug>0){
	printf("[RobustHelixFinder::ReadHits]-----------------------------------------------------------\n");
	printf("[RobustHelixFinder::ReadHits]    i     Face     Panel      X         Y         Z      \n");
	printf("[RobustHelixFinder::ReadHits]-----------------------------------------------------------\n");

	for (unsigned i=0; i<HelixData._chHitsToProcess.size(); ++i){
	  ComboHit* ch = &HelixData._chHitsToProcess[i];
	  printf("[RobustHelixFinder::ReadHits] %4i %6i %10i %10.3f %10.3f %10.3f\n", i, ch->strawId().uniqueFace(), ch->strawId().panel(), ch->pos().x(), ch->pos().y(), ch->pos().z() );
	}

	printf("[RobustHelixFinder::ReadIndeces]----------------------------------------------------------------------\n");
	printf("[RobustHelixFinder::ReadIndeces]    Face        fBg       fEnd       Panel       pBg        pEnd      \n");
	printf("[RobustHelixFinder::ReadIndeces]----------------------------------------------------------------------\n");

	for (int f=0; f<StrawId::_ntotalfaces; ++f){
	  facez     = &HelixData._oTracker[f];

	  for (int p=0; p<FaceZ_t::kNPanels; ++p){
	    panelz = &facez->panelZs[p];
	    if (panelz->nChHits() != 0) printf("[RobustHelixFinder::ReadIndeces] %6i %10i %10i %10i %10i %10i\n", f, facez->idChBegin, facez->idChEnd, p, panelz->idChBegin, panelz->idChEnd);
	  }
	}
      }
      for (int f=0; f<StrawId::_ntotalfaces; ++f){
	facez     = &HelixData._oTracker[f];

	for (int p=0; p<FaceZ_t::kNPanels; ++p){
	  panelz = &facez->panelZs[p];
	  nhitsFace = panelz->nChHits();
	  if ( nhitsFace > HelixData._diag.nChPPanel) HelixData._diag.nChPPanel = nhitsFace;
	}//end loop over the panel
      }//end loop over the faces
    }
  }

  unsigned  RobustHelixFinder::filterCircleHits(RobustHelixFinderData& helixData)
  {
    unsigned changed(0);
    int      nGoodSH(0);
    static XYZVectorF zaxis(0.0,0.0,1.0); // unit in z direction
    RobustHelix& helix = helixData._hseed._helix;

    // loop over hits
    ComboHit*     hit(0);
    FaceZ_t*      facez;

    float         chi2dXY(0), chCounter(1e-10);
    int           nhitsFace(0);

    //for diagnostic purposes
    float         drBestVec   [RobustHelixFinderData::kMaxResidIndex]={-9999.};
    float         rwdotBestVec[RobustHelixFinderData::kMaxResidIndex]={-9999.};

    for (int f=0; f<StrawId::_ntotalfaces; ++f){
      facez     = &helixData._oTracker[f];

      float      minChi2(_maxrpull);
      float      drBest(-1.), rwdotBest(-1.);
      HitInfo_t  indexBestComboHit;
      bool       oldoutBest(false);

      nhitsFace = facez->nChHits();
      if (nhitsFace == 0)                        continue;

      for (int ip=0; ip<nhitsFace; ++ip){
	hit = &helixData._chHitsToProcess[facez->idChBegin + ip];

	bool oldout = hit->_flag.hasAnyProperty(_outlier);
	hit->_flag.clear(_outlier);

	const XYZVectorF& wdir = hit->wdir();
	XYZVectorF cvec = PerpVector(hit->pos() - helix.center(),GenVector::ZDir()); // direction from the circle center to the hit
	XYZVectorF cdir = cvec.Unit(); // direction from the circle center to the hit
	float rwdot = wdir.Dot(cdir); // compare directions of radius and wire
	if(rwdot > _maxrwdot){
	  hit->_flag.merge(_outlier);
	  //	  if(!oldout) ++changed;
	  continue;
	}
	float dr = sqrtf(cvec.mag2())-helix.radius();
	if ( fabs(dr) > _maxdr ) {
	  hit->_flag.merge(_outlier);
	  //	  if(!oldout) ++changed;
	  continue;
	}

	float rwdot2 = rwdot*rwdot;
	// compute radial difference and pull
	float werr = hit->posRes(StrawHitPosition::wire);
	float terr = hit->posRes(StrawHitPosition::trans);
	// the resolution is dominated the resolution along the wire
	//	float rres = std::max(sqrtf(werr*werr*rwdot2 + terr*terr*(1.0-rwdot2)),_minrerr);
	float rres = sqrtf(werr*werr*rwdot2 + terr*terr*(1.0-rwdot2));
	float rpull = fabs(dr/rres)*_rpullScaleF;
	if ( rpull > _maxrpull ) {
	  hit->_flag.merge(_outlier);
	  //	  if(!oldout) ++changed;
	  continue;
	}

	//	if (oldout) ++changed;

	if ( rpull < minChi2){
	  minChi2    = rpull;
	  drBest     = dr;
	  rwdotBest  = rwdot;
	  oldoutBest = oldout;
	  indexBestComboHit.face          = f;
	  indexBestComboHit.panel         = hit->strawId().uniquePanel();
	  indexBestComboHit.panelHitIndex = facez->idChBegin + ip;
	}

	//set all the hits as outlier. Only the best within the face will be cleared
	hit->_flag.merge(_outlier);

      }//end loop over the panels

      if (indexBestComboHit.face >=0 ) {
	hit     = &helixData._chHitsToProcess[indexBestComboHit.panelHitIndex];

	//remove the outlier flag
	hit->_flag.clear(StrawHitFlag::outlier);
	nGoodSH += hit->nStrawHits();
	chi2dXY += minChi2*minChi2;

	if(oldoutBest) ++changed;

	if (chCounter < RobustHelixFinderData::kMaxResidIndex) {
	  drBestVec   [int(chCounter)] = drBest;
	  rwdotBestVec[int(chCounter)] = rwdotBest;
	}
	chCounter += 1.;
      }

    }//end loop over the faces

    helixData._nXYSh = nGoodSH;
    helixData._nXYCh = int(chCounter);

    helix._chi2dXY   = chi2dXY/chCounter;

    if (_diag) {
      helixData._diag.chi2dXY = chi2dXY/chCounter;
      helixData._diag.nXYCh   = helixData._nXYCh;

      for (int i=0; i<int(chCounter); ++i){
	if (drBestVec   [i]>-999.) helixData._diag.resid[i] = drBestVec   [i];
	if (rwdotBestVec[i]>-999.) helixData._diag.rwdot[i] = rwdotBestVec[i];
      }
      //      helixData._diag. = chi2dXY/chCounter;
    }

    return changed;
  }

  void RobustHelixFinder::fitHelix(RobustHelixFinderData& helixData){
    // iteratively fit the helix including filtering
    unsigned niter(0);
    unsigned nitermva(0);
    bool     changed(true), xychanged(true), fzchanged(true);
    unsigned niterxy(0), niterfz(0);

    do {
      niterxy = 0;
      do {
	_hfit.fitCircle(helixData, _targetcon, _useTripletAreaWt);
	xychanged = filterCircleHits(helixData) > 0;
	++niterxy;
      } while (helixData._hseed._status.hasAllProperties(TrkFitFlag::circleOK) && niterxy < _maxniter && xychanged);

      if (_diag) {
	helixData._diag.xyniter  = niterxy;
	helixData._diag.nShFitXY = helixData._nXYSh;
	helixData._diag.nChFitXY = helixData._nXYCh;
      }

      if (helixData._nXYSh < _minnsh) {
	helixData._hseed._status.clear(TrkFitFlag::circleOK);
	niter = _maxniter;//exit from this while()
      }

      // then fit phi-Z
      if (helixData._hseed._status.hasAnyProperty(TrkFitFlag::circleOK)) {
	if (niterxy < _maxniter)
	  helixData._hseed._status.merge(TrkFitFlag::circleConverged);
	else
	  helixData._hseed._status.clear(TrkFitFlag::circleConverged);

	// solve for the longitudinal parameters
	niterfz = 0;
	fzchanged = false;
	do {
	  _hfit.fitFZ(helixData);
	  fzchanged = filterHits(helixData);
	  ++niterfz;
	} while (helixData._hseed._status.hasAllProperties(TrkFitFlag::phizOK)  && niterfz < _maxniter && fzchanged);

	if (helixData._nZPhiSh < _minnsh) {
	  helixData._hseed._status.clear(TrkFitFlag::phizOK);
	  niter = _maxniter;//exit from this while()
	}

	if (helixData._hseed._status.hasAnyProperty(TrkFitFlag::phizOK)) {
	  if (niterfz < _maxniter)
	    helixData._hseed._status.merge(TrkFitFlag::phizConverged);
	  else
	    helixData._hseed._status.clear(TrkFitFlag::phizConverged);
	}
      }
      //here is where we should check for the hits within the face to searchfor missing/best ones
      ++niter;
      changed = fzchanged || xychanged;

      // update the stereo hit positions; this checks how much the positions changed
      // do this only in non trigger mode

      if (_updateStereo && _hfit.goodHelix(helixData._hseed.helix()))
	changed |= updateStereo(helixData);
    } while (_hfit.goodHelix(helixData._hseed.helix()) && niter < _maxniter && changed);

    if (_diag) helixData._diag.niter = niter;

    if (_hfit.goodHelix(helixData._hseed.helix())  &&
	helixData._hseed._status.hasAnyProperty(TrkFitFlag::circleOK) &&
	helixData._hseed._status.hasAnyProperty(TrkFitFlag::phizOK) ) {

      helixData._hseed._status.merge(TrkFitFlag::helixOK);
      updateT0(helixData);
      if (niter < _maxniter) helixData._hseed._status.merge(TrkFitFlag::helixConverged);

      if (_usemva) {
	bool changed = true;
	while (helixData._hseed._status.hasAllProperties(TrkFitFlag::helixOK)  && nitermva < _maxniter && changed) {
	  fillMVA(helixData);
	  changed = filterHitsMVA(helixData);
	  if (!changed) break;
	  refitHelix(helixData);
	  // update t0 each iteration as that's used in the MVA
	  updateT0(helixData);
	  ++nitermva;
	}
	if (nitermva < _maxniter)
	  helixData._hseed._status.merge(TrkFitFlag::helixConverged);
	else
	  helixData._hseed._status.clear(TrkFitFlag::helixConverged);
      }
    }
    if (_diag > 0){
      _niter->Fill(niter);
      _niterfz->Fill(niterfz);
      _niterxy->Fill(niterxy);
      _nitermva->Fill(nitermva);
      if (!_usemva) fillMVA(helixData);
    }
  }

  //------------------------------------------------------------------------------------------

  void RobustHelixFinder::refitHelix(RobustHelixFinderData& helixData) {
    // reset the fit status flags, in case this is called iteratively

    helixData._hseed._status.clear(TrkFitFlag::helixOK);
    _hfit.fitCircle(helixData, _targetcon, _useTripletAreaWt);
    if (helixData._hseed._status.hasAnyProperty(TrkFitFlag::circleOK)) {
      _hfit.fitFZ(helixData);
      if (_hfit.goodHelix(helixData._hseed._helix)) helixData._hseed._status.merge(TrkFitFlag::helixOK);
    }
  }

  unsigned RobustHelixFinder::hitCount(RobustHelixFinderData& helixData) {
    unsigned nHits(0);

    ComboHit*      hit(0);

    for (unsigned f=0; f<helixData._chHitsToProcess.size(); ++f){
      hit = &helixData._chHitsToProcess[f];
      if (hit->_flag.hasAnyProperty(_outlier))   continue;
      ++nHits;
    }//end faces loop

    return nHits;
  }

  bool RobustHelixFinder::updateStereo(RobustHelixFinderData& helixData) {
    static StrawHitFlag stereo(StrawHitFlag::stereo);
    bool retval(false);
    // loop over the stereo hits in the helix and update their positions given the local helix direction
    for(auto& ch : helixData._hseed._hhits){
      if(ch.flag().hasAllProperties(stereo) && ch.nCombo() >=2) {
	// local helix direction at the average z of this hit
	XYZVectorF hdir;
	helixData._hseed.helix().direction(ch.pos().z(),hdir);
	// needs re-implementing with ComboHits FIXME!
	//	XYZVectorF pos1, pos2;
	//	sthit.position(shcol,tracker,pos1,pos2,hdir);
      }
    }
    return retval;
  }

  void RobustHelixFinder::fillPluginDiag(RobustHelixFinderData& helixData, int helCounter) {
    //--------------------------------------------------------------------------------
    // fill diagnostic information
    //--------------------------------------------------------------------------------
    float          mm2MeV = 3./10.;  // approximately , at B=1T

    int loc = _data.nseeds[helCounter];
    if (loc < _data.maxSeeds()) {
      _data.nChPPanel   [helCounter][loc] = helixData._diag.nChPPanel;
      _data.nChHits     [helCounter][loc] = helixData._diag.nChHits;

      int nhits          = helixData._hseed._hhits.size();
      _data.ntclhits    [helCounter][loc] = helixData._timeCluster->hits().size();
      _data.nhits       [helCounter][loc] = nhits;

      _data.ntriplet0   [helCounter][loc] = helixData._diag.ntriple_0;
      _data.ntriplet1   [helCounter][loc] = helixData._diag.ntriple_1;
      _data.ntriplet2   [helCounter][loc] = helixData._diag.ntriple_2;

      _data.xyniter     [helCounter][loc] = helixData._diag.xyniter;
      _data.fzniter     [helCounter][loc] = helixData._diag.fzniter;
      _data.niter       [helCounter][loc] = helixData._diag.niter;
      _data.nrescuedhits[helCounter][loc] = helixData._diag.nrescuedhits;

      _data.nShFitCircle[helCounter][loc] = helixData._diag.nShFitCircle;
      _data.nChFitCircle[helCounter][loc] = helixData._diag.nChFitCircle;
      _data.nShFitXY    [helCounter][loc] = helixData._diag.nShFitXY;
      _data.nChFitXY    [helCounter][loc] = helixData._diag.nChFitXY;

      _data.nfz0counter [helCounter][loc] = helixData._diag.nfz0counter;

      _data.nshsxy_0    [helCounter][loc] = helixData._diag.nshsxy_0;
      _data.rsxy_0      [helCounter][loc] = helixData._diag.rsxy_0;
      _data.chi2dsxy_0  [helCounter][loc] = helixData._diag.chi2dsxy_0;

      _data.nshsxy_1    [helCounter][loc] = helixData._diag.nshsxy_1;
      _data.rsxy_1      [helCounter][loc] = helixData._diag.rsxy_1;
      _data.chi2dsxy_1  [helCounter][loc] = helixData._diag.chi2dsxy_1;

      _data.nshszphi_0  [helCounter][loc] = helixData._diag.nshszphi_0;
      _data.lambdaszphi_0    [helCounter][loc] = helixData._diag.lambdaszphi_0;
      _data.chi2dszphi_0[helCounter][loc] = helixData._diag.chi2dszphi_0;

      _data.nshszphi_1  [helCounter][loc] = helixData._diag.nshszphi_1;
      _data.lambdaszphi_1    [helCounter][loc] = helixData._diag.lambdaszphi_1;
      _data.chi2dszphi_1[helCounter][loc] = helixData._diag.chi2dszphi_1;

      _data.nXYSh       [helCounter][loc] = helixData._nXYSh;
      _data.nZPhiSh     [helCounter][loc] = helixData._nZPhiSh;

      _data.rinit       [helCounter][loc] = helixData._diag.radius_0;
      _data.lambda0     [helCounter][loc] = helixData._diag.lambda_0;
      _data.lambda1     [helCounter][loc] = helixData._diag.lambda_1;
      _data.radius      [helCounter][loc] = helixData._hseed.helix().radius();
      _data.pT          [helCounter][loc] = mm2MeV*_data.radius[helCounter][loc];
      _data.p           [helCounter][loc] = _data.pT[helCounter][loc]/std::cos( std::atan(helixData._hseed.helix().lambda()/_data.radius[helCounter][loc]));

      _data.chi2XY      [helCounter][loc] = helixData._diag.chi2dXY;
      _data.chi2ZPhi    [helCounter][loc] = helixData._diag.chi2dZPhi;

      _data.nseeds[helCounter]++;

      _data.dr          [helCounter][loc] = helixData._diag.radius_2 - helixData._diag.radius_1;
      _data.chi2d_helix [helCounter][loc] = helixData._diag.chi2d_helix;

      _data.nXYCh       [helCounter][loc] = helixData._diag.nXYCh;

      _data.nLoops      [helCounter][loc] = helixData._diag.nLoops           ;

      _data.nHitsLoopFailed[helCounter][loc] = helixData._diag.nHitsLoopFailed;

      _data.meanHitRadialDist [helCounter][loc] = helixData._diag.meanHitRadialDist;

      for (int i=0; i<helixData._diag.nXYCh; ++i) {
	if (helixData._diag.rwdot[i]>-999.) _data.hitRWDot[helCounter][loc][i] = helixData._diag.rwdot[i];

	if (helixData._diag.resid[i]>-999.) _data.hitDr   [helCounter][loc][i] = helixData._diag.resid[i];
	else break;
      }
    }   else {
      printf(" N(seeds) > %i, IGNORE SEED\n",_data.maxSeeds());
    }
  }

  void     RobustHelixFinder::updateHelixXYInfo(RobustHelixFinderData& helixData){
    RobustHelix&  helix        = helixData._hseed._helix;
    XYVec         center       = XYVec(helix.center().x(), helix.center().y());

    center.SetX(helixData._sxy.x0());
    center.SetY(helixData._sxy.y0());

    helix._rcent  = sqrtf(center.Mag2());
    helix._fcent  = polyAtan2(center.y(), center.x());
    helix._radius = helixData._sxy.radius();
  }

  void     RobustHelixFinder::updateHelixZPhiInfo(RobustHelixFinderData& helixData){
    RobustHelix&  helix        = helixData._hseed._helix;

    helix._lambda = 1./(helixData._szphi.dfdz());
    helix._fz0    = helixData._szphi.phi0();
  }
}
using mu2e::RobustHelixFinder;
DEFINE_ART_MODULE(RobustHelixFinder);


// Boost.Geometry - gis-projections (based on PROJ4)

// Copyright (c) 2008-2015 Barend Gehrels, Amsterdam, the Netherlands.

// This file was modified by Oracle on 2017, 2018.
// Modifications copyright (c) 2017-2018, Oracle and/or its affiliates.
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle.

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

// This file is converted from PROJ4, http://trac.osgeo.org/proj
// PROJ4 is originally written by Gerald Evenden (then of the USGS)
// PROJ4 is maintained by Frank Warmerdam
// PROJ4 is converted to Boost.Geometry by Barend Gehrels

// Last updated version of proj: 5.0.0

// Original copyright notice:

// Permission is hereby granted, free of charge, to any person obtaining a
// copy of this software and associated documentation files (the "Software"),
// to deal in the Software without restriction, including without limitation
// the rights to use, copy, modify, merge, publish, distribute, sublicense,
// and/or sell copies of the Software, and to permit persons to whom the
// Software is furnished to do so, subject to the following conditions:

// The above copyright notice and this permission notice shall be included
// in all copies or substantial portions of the Software.

// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
// OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
// DEALINGS IN THE SOFTWARE.

#ifndef BOOST_GEOMETRY_PROJECTIONS_LAGRNG_HPP
#define BOOST_GEOMETRY_PROJECTIONS_LAGRNG_HPP

#include <boost/geometry/srs/projections/impl/base_static.hpp>
#include <boost/geometry/srs/projections/impl/base_dynamic.hpp>
#include <boost/geometry/srs/projections/impl/factory_entry.hpp>
#include <boost/geometry/srs/projections/impl/pj_param.hpp>
#include <boost/geometry/srs/projections/impl/projects.hpp>

#include <boost/geometry/util/math.hpp>

namespace boost { namespace geometry
{

namespace projections
{
    #ifndef DOXYGEN_NO_DETAIL
    namespace detail { namespace lagrng
    {

            static const double tolerance = 1e-10;

            template <typename T>
            struct par_lagrng
            {
                T    a1;
                T    rw;
                T    hrw;
            };

            // template class, using CRTP to implement forward/inverse
            template <typename T, typename Parameters>
            struct base_lagrng_spheroid
                : public base_t_f<base_lagrng_spheroid<T, Parameters>, T, Parameters>
            {
                par_lagrng<T> m_proj_parm;

                inline base_lagrng_spheroid(const Parameters& par)
                    : base_t_f<base_lagrng_spheroid<T, Parameters>, T, Parameters>(*this, par)
                {}

                // FORWARD(s_forward)  spheroid
                // Project coordinates from geographic (lon, lat) to cartesian (x, y)
                inline void fwd(T lp_lon, T lp_lat, T& xy_x, T& xy_y) const
                {
                    static const T half_pi = detail::half_pi<T>();

                    T v, c;

                    if (fabs(fabs(lp_lat) - half_pi) < tolerance) {
                        xy_x = 0;
                        xy_y = lp_lat < 0 ? -2. : 2.;
                    } else {
                        lp_lat = sin(lp_lat);
                        v = this->m_proj_parm.a1 * math::pow((T(1) + lp_lat)/(T(1) - lp_lat), this->m_proj_parm.hrw);
                        if ((c = 0.5 * (v + 1./v) + cos(lp_lon *= this->m_proj_parm.rw)) < tolerance) {
                            BOOST_THROW_EXCEPTION( projection_exception(error_tolerance_condition) );
                        }
                        xy_x = 2. * sin(lp_lon) / c;
                        xy_y = (v - 1./v) / c;
                    }
                }

                static inline std::string get_name()
                {
                    return "lagrng_spheroid";
                }

            };

            // Lagrange
            template <typename Params, typename Parameters, typename T>
            inline void setup_lagrng(Params const& params, Parameters& par, par_lagrng<T>& proj_parm)
            {
                T phi1;

                proj_parm.rw = 0.0;
                bool is_w_set = pj_param_f<srs::spar::w>(params, "W", srs::dpar::w, proj_parm.rw);

                // Boost.Geometry specific, set default parameters manually
                if (! is_w_set) {
                    bool const use_defaults = ! pj_get_param_b<srs::spar::no_defs>(params, "no_defs", srs::dpar::no_defs);
                    if (use_defaults) {
                        proj_parm.rw = 2;
                    }
                }

                if (proj_parm.rw <= 0)
                    BOOST_THROW_EXCEPTION( projection_exception(error_w_or_m_zero_or_less) );

                proj_parm.rw = 1. / proj_parm.rw;
                proj_parm.hrw = 0.5 * proj_parm.rw;
                phi1 = pj_get_param_r<T, srs::spar::lat_1>(params, "lat_1", srs::dpar::lat_1);
                if (fabs(fabs(phi1 = sin(phi1)) - 1.) < tolerance)
                    BOOST_THROW_EXCEPTION( projection_exception(error_lat_larger_than_90) );

                proj_parm.a1 = math::pow((T(1) - phi1)/(T(1) + phi1), proj_parm.hrw);

                par.es = 0.;
            }

    }} // namespace detail::lagrng
    #endif // doxygen

    /*!
        \brief Lagrange projection
        \ingroup projections
        \tparam Geographic latlong point type
        \tparam Cartesian xy point type
        \tparam Parameters parameter type
        \par Projection characteristics
         - Miscellaneous
         - Spheroid
         - no inverse
        \par Projection parameters
         - W (real)
         - lat_1: Latitude of first standard parallel (degrees)
        \par Example
        \image html ex_lagrng.gif
    */
    template <typename T, typename Parameters>
    struct lagrng_spheroid : public detail::lagrng::base_lagrng_spheroid<T, Parameters>
    {
        template <typename Params>
        inline lagrng_spheroid(Params const& params, Parameters const& par)
            : detail::lagrng::base_lagrng_spheroid<T, Parameters>(par)
        {
            detail::lagrng::setup_lagrng(params, this->m_par, this->m_proj_parm);
        }
    };

    #ifndef DOXYGEN_NO_DETAIL
    namespace detail
    {

        // Static projection
        BOOST_GEOMETRY_PROJECTIONS_DETAIL_STATIC_PROJECTION(srs::spar::proj_lagrng, lagrng_spheroid, lagrng_spheroid)

        // Factory entry(s)
        BOOST_GEOMETRY_PROJECTIONS_DETAIL_FACTORY_ENTRY_F(lagrng_entry, lagrng_spheroid)

        BOOST_GEOMETRY_PROJECTIONS_DETAIL_FACTORY_INIT_BEGIN(lagrng_init)
        {
            BOOST_GEOMETRY_PROJECTIONS_DETAIL_FACTORY_INIT_ENTRY(lagrng, lagrng_entry);
        }

    } // namespace detail
    #endif // doxygen

} // namespace projections

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_PROJECTIONS_LAGRNG_HPP


//==================================================================================================
/*!
  @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_BITWISE_NOTAND_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_BITWISE_NOTAND_HPP_INCLUDED

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

  /*!
    @ingroup group-bitwise
    This function object computes the bitwise not and of its parameters.

    The result type is the one of the first operand.

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

    @pre  The operands must share the same bit size.

    @par Example:

      @snippet bitwise_notand.cpp bitwise_notand

    @par Possible output:

      @snippet bitwise_notand.txt bitwise_notand

  **/
  Value0 bitwise_notand(Value0 const& x, Value1 const& y);
} }
#endif

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

#endif


/**
 * @file
 * @license BSD 3-clause
 * @copyright Copyright (c) 2021, New York University and Max Planck
 * Gesellschaft
 *
 * @brief Teststand biped low level drivers.
 */

#pragma once

#include <math.h>

#include <Eigen/Eigen>

#include "AtiFTSensor.h"
#include "blmc_drivers/serial_reader.hpp"
#include "odri_control_interface/calibration.hpp"
#include "odri_control_interface/robot.hpp"
#include "teststand/teststand_abstract_interface.hpp"

namespace teststand
{
#define TESTSTAND_NB_MOTOR_BOARD 1
#define TESTSTAND_NB_MOTOR 2
#define TESTSTAND_NB_SLIDER 0

/** @brief Control state of the robot. */
enum ControlState
{
    initial,
    ready,
    calibrate
};

/**
 * @brief Driver for the Teststand biped robot.
 */
class Teststand : public TeststandAbstractInterface
{
public:
    /**
     * @brief Teststand is the constructor of the class.
     */
    Teststand();

    virtual void initialize()
    {
        throw std::runtime_error(
            "Teststand::initialize: Missing the network id argument");
    }

    /**
     * @copydoc TeststandAbstractInterface::initialize
     */
    virtual void initialize(const std::string& network_id);

    /**
     * @copydoc TeststandAbstractInterface::send_target_joint_torque
     */
    virtual void send_target_joint_torque(
        const Eigen::Ref<const Eigen::Vector2d> target_joint_torque);

    /**
     * @copydoc TeststandAbstractInterface::acquire_sensors
     */
    virtual bool acquire_sensors();

    /**
     * @brief Set max current.
     */
    virtual void set_max_current(double max_current);

    /**
     * @copydoc TeststandAbstractInterface::calibrate
     */
    virtual void calibrate(const Eigen::Vector2d& home_offset_rad);

    /**
     * @copydoc TeststandAbstractInterface::calibrate
     */
    void calibrate();

    /**
     * @brief Wait until the hardware is ready to be controlled.
     */
    void wait_until_ready();

    /**
     * @brief Request calibration of the joints by moving to the next joint
     * index position. The control is made inside the send_target_joint_torque.
     *
     * @param home_offset_rad This is the angle between the index and the zero
     * pose.
     * @return true
     * @return false
     */
    void request_calibration(
        const Eigen::Ref<const Eigen::VectorXd> home_offset_rad);

    /**
     * @brief Request calibration of the joints by moving to the next joint
     * index position. The control is made inside the send_target_joint_torque.
     * Use the yaml information in order to get the offset from the nearest
     * motor index.
     *
     * @return true
     * @return false
     */
    void request_calibration();

    /**
     * @brief get_base_accelerometer
     * @return the base_accelerometer
     * WARNING !!!!
     * The method <acquire_sensors>"()" has to be called
     * prior to any getter to have up to date data.
     */
    const Eigen::Ref<const Eigen::Vector3d> get_base_accelerometer()
    {
        return base_accelerometer_;
    }

    /**
     * @brief get_base_accelerometer
     * @return the base_accelerometer
     * WARNING !!!!
     * The method <acquire_sensors>"()" has to be called
     * prior to any getter to have up to date data.
     */
    const Eigen::Ref<const Eigen::Vector3d> get_base_gyroscope()
    {
        return base_gyroscope_;
    }

    /**
     * @brief get_base_accelerometer
     * @return the base_accelerometer
     * WARNING !!!!
     * The method <acquire_sensors>"()" has to be called
     * prior to any getter to have up to date data.
     */
    const Eigen::Ref<const Eigen::Vector3d> get_base_attitude()
    {
        return base_attitude_;
    }

    /**
     * @brief get_base_accelerometer
     * @return the base_accelerometer
     * WARNING !!!!
     * The method <acquire_sensors>"()" has to be called
     * prior to any getter to have up to date data.
     */
    const Eigen::Ref<const Eigen::Vector3d> get_base_linear_acceleration()
    {
        return base_linear_acceleration_;
    }

    /**
     * @brief get_base_accelerometer
     * @return the base_accelerometer
     * WARNING !!!!
     * The method <acquire_sensors>"()" has to be called
     * prior to any getter to have up to date data.
     */
    const Eigen::Ref<const Eigen::Vector4d> get_base_attitude_quaternion()
    {
        return base_attitude_quaternion_;
    }

    /**
     * @brief is_calibrating()
     * @return Returns true if the calibration procedure is running right now.
     */
    bool is_calibrating()
    {
        return (control_state_ == ControlState::calibrate) ||
               calibrate_request_;
    }

protected:
    /**
     * @brief Fill attitude quaternion.
     */
    void fill_base_attitude_quaternion();

private:
    /** @brief base accelerometer. */
    Eigen::Vector3d base_accelerometer_;

    /** @brief base accelerometer. */
    Eigen::Vector3d base_gyroscope_;

    /** @brief base accelerometer. */
    Eigen::Vector3d base_attitude_;

    /** @brief base accelerometer. */
    Eigen::Vector3d base_linear_acceleration_;

    /** @brief base attitude quaternion. */
    Eigen::Vector4d base_attitude_quaternion_;

    /*
     * Controllers
     */

    /** @brief Controller to run the calibration procedure */
    std::shared_ptr<odri_control_interface::JointCalibrator> calib_ctrl_;

    /*
     * Finite state machine of the controller.
     */

    /** @brief Control state Initial, Ready, Calibration */
    ControlState control_state_;

    /** @brief Check if the user called for the joint calibration. */
    bool calibrate_request_;

    /** @brief Number of time we acquire the sensor readings, this is used
     *  to prevent spamming prints */
    long int nb_time_we_acquired_sensors_;

    /*
     * Drivers communication objects
     */

    /**
     * @brief Robot complete interface.
     * Wrapper around the MasterBoardInterface.
     *
     * PC <------------------> main board <-------> Motor Board
     *       Ethernet/Wifi                   SPI
     */
    std::shared_ptr<odri_control_interface::Robot> robot_;

    /**
     * @brief ATI sensor.
     */
    ati_ft_sensor::AtiFTSensor ati_sensor_;
};

}  // namespace teststand


/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~\
|  Phycas: Python software for phylogenetic analysis                          |
|  Copyright (C) 2006 Mark T. Holder, Paul O. Lewis and David L. Swofford     |
|                                                                             |
|  This program is free software; you can redistribute it and/or modify       |
|  it under the terms of the GNU General Public License as published by       |
|  the Free Software Foundation; either version 2 of the License, or          |
|  (at your option) any later version.                                        |
|                                                                             |
|  This program is distributed in the hope that it will be useful,            |
|  but WITHOUT ANY WARRANTY; without even the implied warranty of             |
|  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the              |
|  GNU General Public License for more details.                               |
|                                                                             |
|  You should have received a copy of the GNU General Public License along    |
|  with this program; if not, write to the Free Software Foundation, Inc.,    |
|  51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.                |
\~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
#include <boost/cast.hpp>       //temporary!
#include <sstream>
#include <boost/math/special_functions/fpclassify.hpp>
#include "probability_distribution.hpp"
#include "relative_rate_distribution.hpp"
#include "model.hpp"
#include "basic_tree_node.hpp"
#include "tree_likelihood.hpp"
#include "xlikelihood.hpp"
#include "mcmc_chain_manager.hpp"
#include "dirichlet_move.hpp"
#include "subset_relrates_move.hpp"
#include "basic_tree.hpp"
#include "tree_manip.hpp"
#include "gtr.hpp"
#include "hky.hpp"

using namespace phycas;

/*----------------------------------------------------------------------------------------------------------------------
|	The constructor simply calls the base class (DirichletMove) constructor.
*/
SubsetRelRatesMove::SubsetRelRatesMove()
  : DirichletMove()
	{
	}

/*----------------------------------------------------------------------------------------------------------------------
|	Sets the relative rates of the associated partition model to those in the supplied vector `v'. No transformation of
|	the relative rates is done by this function.
*/
void SubsetRelRatesMove::sendCurrValuesToModel(const double_vect_t & v)
	{
	PHYCAS_ASSERT(dim == v.size());
	partition_model->setSubsetRelRatesVect(v);
	}

/*----------------------------------------------------------------------------------------------------------------------
|	Obtains the current relative rates from the model, storing them in the supplied vector `v'. No transformation of
|	the relative rates is done by this function.
*/
void SubsetRelRatesMove::getCurrValuesFromModel(double_vect_t & v) const
	{
	const std::vector<double> & rrates = partition_model->getSubsetRelRatesVect();

    //std::cerr << "@#@#@#@#@# SubsetRelRatesMove::getCurrValuesFromModel" << std::endl;
    //std::copy(rrates.begin(), rrates.end(), std::ostream_iterator<double>(std::cerr, " "));
    //std::cerr << std::endl;
    //std::cerr << "@#@#@#@#@# SubsetRelRatesMove::getCurrValuesFromModel" << std::endl;

	PHYCAS_ASSERT(dim == rrates.size());
	v.resize(rrates.size());
	std::copy(rrates.begin(), rrates.end(), v.begin());
	}

/*----------------------------------------------------------------------------------------------------------------------
|	Obtains the current relative rates from the model, returning them as an anonymous vector. No transformation of
|	the relative rates is done by this function.
*/
double_vect_t SubsetRelRatesMove::listCurrValuesFromModel()
	{
	double_vect_t v(dim);
	const double_vect_t & rrates = partition_model->getSubsetRelRatesVect();
	PHYCAS_ASSERT(dim == rrates.size());
	std::copy(rrates.begin(), rrates.end(), v.begin());
	return v;
	}

/*----------------------------------------------------------------------------------------------------------------------
|	Sets `partition_model' to the supplied `m'.
*/
void SubsetRelRatesMove::setPartitionModel(
  PartitionModelShPtr m)    /*< is a shared pointer to the new partition model */
	{
	partition_model = m;
	}

/*----------------------------------------------------------------------------------------------------------------------
|	Obtains the current relative rates from the model by calling SubsetRelRatesMove::getCurrValuesFromModel.
*/
void SubsetRelRatesMove::getParams()
	{
	getCurrValuesFromModel(orig_relrates);
	}

/*----------------------------------------------------------------------------------------------------------------------
|	Replaces the relative rates in the model with those supplied in the vector `v'.
*/
void SubsetRelRatesMove::setParams(
  const double_vect_t & v)    /*< is the vector of parameter values to send to the model */
	{
	PHYCAS_ASSERT(v.size() == dim);
    sendCurrValuesToModel(v);
	}

/*--------------------------------------------------------------------------------------------------------------------------
|   Returns current subset relative rates, proportions and transformed relative rates as well as the weighted average of the
|   relative rates, which should always be 1.0.
*/
std::string SubsetRelRatesMove::debugShowTransformedRelativeRates(const std::string msg) const
	{
    std::stringstream ss;
	ss << ">>>>>>>>>> " << msg << " <<<<<<<<<<" << std::endl;
	ss << "Relative rates: ";
	std::copy(orig_relrates.begin(), orig_relrates.end(), std::ostream_iterator<double>(ss, " "));
	ss << boost::str(boost::format(" sum = %.15f") % std::accumulate(orig_relrates.begin(), orig_relrates.end(), 0.0)) << std::endl;

	ss << "Subset proportions: ";
    const std::vector<unsigned> & nsites = partition_model->getNumSitesVect();
    double total_sites = (double)partition_model->getTotalNumSites();
    double_vect_t p;
    p.resize(nsites.size());
    std::transform(nsites.begin(), nsites.end(), p.begin(), boost::lambda::_1/total_sites);
	std::copy(p.begin(), p.end(), std::ostream_iterator<double>(ss, " "));
	ss << boost::str(boost::format(" sum = %.15f") % std::accumulate(p.begin(), p.end(), 0.0)) << std::endl;

	// transform relative rates using coefficients so that elements of orig_params sum to 1
	ss << "Weighted relative rates: ";
    std::vector<double> tmp(orig_relrates.size(), 0.0);
	std::transform(orig_relrates.begin(), orig_relrates.end(), p.begin(), tmp.begin(), boost::lambda::_1*boost::lambda::_2);
	std::copy(tmp.begin(), tmp.end(), std::ostream_iterator<double>(ss, " "));
	ss << boost::str(boost::format(" sum = %.15f") %  std::accumulate(tmp.begin(), tmp.end(), 0.0)) << std::endl;
    return ss.str();
    }

/*--------------------------------------------------------------------------------------------------------------------------
|   Chooses a new vector of state frequencies using a sharp Dirichlet distribution centered at the original frequencies.
*/
void SubsetRelRatesMove::proposeNewState()
	{
	// transform relative rates using coefficients so that elements of orig_params sum to 1
	orig_params.resize(orig_relrates.size());
    SubsetProportionsShPtr ptr = partition_model->getSubsetProportions();
    std::vector<double> const & p = ptr->getSubsetProportions();
    //double total_sites = (double)partition_model->getTotalNumSites();
	std::transform(orig_relrates.begin(), orig_relrates.end(), p.begin(), orig_params.begin(), boost::lambda::_1*boost::lambda::_2);

    // create vector of Dirichlet parameters for selecting new relative rate values
	// The parameter vector for this temporary distribution is obtained by multiplying
    // each of the current relative rates by the value `psi', which is usually very large (e.g. 1000)
    c_forward.resize(orig_params.size());
	std::transform(orig_params.begin(), orig_params.end(), c_forward.begin(), 1.0 + boost::lambda::_1*psi);

	// create Dirichlet distribution and sample from it
    dir_forward = DirichletShPtr(new DirichletDistribution(c_forward));
    dir_forward->SetLot(getLot().get());
	new_params.resize(orig_params.size());
    new_params = dir_forward->Sample();

    // create vector of Dirichlet parameters for selecting old frequencies (needed for Hastings ratio calculation)
    c_reverse.resize(new_params.size());
	std::transform(new_params.begin(), new_params.end(), c_reverse.begin(), 1.0 + boost::lambda::_1*psi);
    dir_reverse = DirichletShPtr(new DirichletDistribution(c_reverse));

	// transform new_params using coefficients so that elements of new_relrates are equal to proposed new relative rates
	new_relrates.resize(new_params.size());
	std::transform(new_params.begin(), new_params.end(), p.begin(), new_relrates.begin(), boost::lambda::_1/boost::lambda::_2);

#if 0
    std::cerr << "\n^^^^^ orig_relrates ^^^^^" << std::endl;
    std::copy(orig_relrates.begin(), orig_relrates.end(), std::ostream_iterator<double>(std::cerr, "|"));
    std::cerr << "\n^^^^^ orig_params ^^^^^" << std::endl;
    std::copy(orig_params.begin(), orig_params.end(), std::ostream_iterator<double>(std::cerr, "|"));
    std::cerr << "\n^^^^^ c_forward ^^^^^" << std::endl;
    std::copy(c_forward.begin(), c_forward.end(), std::ostream_iterator<double>(std::cerr, "|"));
    std::cerr << "\n^^^^^ c_reverse ^^^^^" << std::endl;
    std::copy(c_reverse.begin(), c_reverse.end(), std::ostream_iterator<double>(std::cerr, "|"));
    std::cerr << "\n^^^^^ new_params ^^^^^" << std::endl;
    std::copy(new_params.begin(), new_params.end(), std::ostream_iterator<double>(std::cerr, "|"));
    std::cerr << "\n^^^^^ new_relrates ^^^^^" << std::endl;
    std::copy(new_relrates.begin(), new_relrates.end(), std::ostream_iterator<double>(std::cerr, "|"));
    std::cerr << std::endl;
#endif
	}

/*--------------------------------------------------------------------------------------------------------------------------
|	Called if the proposed move is accepted. Simply calls the reset() function.
*/
void SubsetRelRatesMove::accept()
	{
	MCMCUpdater::accept();
	reset();
	}

/*--------------------------------------------------------------------------------------------------------------------------
|	Called if the proposed move is rejected. Calls the reset() function after reinstating the original relative rates
|   and ensuring that all conditional likelihood arrays will be recalculated when the likelihood is next calculated.
*/
void SubsetRelRatesMove::revert()
	{
	MCMCUpdater::revert();
    setParams(orig_relrates);

	ChainManagerShPtr p = chain_mgr.lock();
	PHYCAS_ASSERT(p);
    JointPriorManagerShPtr jpm = p->getJointPriorManager();
    jpm->multivariateModified(name, orig_relrates);
    curr_ln_prior = jpm->getLogJointPrior();

    // invalidate all CLAs
    likelihood->useAsLikelihoodRoot(NULL);
    likelihood->storeAllCLAs(tree);

	reset();
	}

/*----------------------------------------------------------------------------------------------------------------------
|	Calls proposeNewState(), then decides whether to accept or reject the proposed new state, calling accept() or
|	revert(), whichever is appropriate.
*/
bool SubsetRelRatesMove::update()
	{
	if (is_fixed)
		return false;

	ChainManagerShPtr p = chain_mgr.lock();
	PHYCAS_ASSERT(p);
    JointPriorManagerShPtr jpm = p->getJointPriorManager();

    // copy the current subset relative rates from the model to the data member orig_params
    getParams();

	proposeNewState();

    double prev_ln_prior		= jpm->getLogJointPrior();

	double prev_ln_like			= p->getLastLnLike();
	PHYCAS_ASSERT(!use_ref_dist || mv_ref_dist);
	double prev_ln_ref_dist = (use_ref_dist ? mv_ref_dist->GetLnPDF(orig_relrates) : 0.0);

    // replace current parameter values with new ones
    setParams(new_relrates);

    jpm->multivariateModified(name, new_relrates);
    double curr_ln_prior    = jpm->getLogJointPrior();

    likelihood->useAsLikelihoodRoot(NULL);	// invalidates all CLAs
	double curr_ln_like			= (heating_power > 0.0 ? likelihood->calcLnL(tree) : 0.0);
	double curr_ln_ref_dist = (use_ref_dist ? mv_ref_dist->GetLnPDF(new_relrates) : 0.0);

    double prev_posterior = 0.0;
	double curr_posterior = 0.0;

	if (is_standard_heating)
		{
		prev_posterior = heating_power*(prev_ln_like + prev_ln_prior);
		curr_posterior = heating_power*(curr_ln_like + curr_ln_prior);
		if (use_ref_dist)
			{
			prev_posterior += (1.0 - heating_power)*prev_ln_ref_dist;
			curr_posterior += (1.0 - heating_power)*curr_ln_ref_dist;
			}
		}
	else
		{
		prev_posterior = heating_power*prev_ln_like + prev_ln_prior;
		curr_posterior = heating_power*curr_ln_like + curr_ln_prior;
		}

	double ln_hastings			= getLnHastingsRatio();
	double ln_accept_ratio		= curr_posterior - prev_posterior + ln_hastings;

    double lnu = std::log(rng->Uniform());

    bool accepted = false;

	if (ln_accept_ratio >= 0.0 || lnu <= ln_accept_ratio)
		{
	    if (save_debug_info)
    	    {
			debug_info = boost::str(boost::format("ACCEPT, prev_ln_like = %.5f, getLastLnLike() = %.5f, curr_ln_like = %.5f, prev_ln_ref_dist = %.5f, curr_ln_ref_dist = %.5f, prev_ln_prior = %.5f, curr_ln_prior = %.5f, lnu = %.5f, ln_accept_ratio = %.5f\n") % prev_ln_like % p->getLastLnLike() % curr_ln_like % prev_ln_ref_dist % curr_ln_ref_dist % prev_ln_prior % curr_ln_prior % lnu % ln_accept_ratio);
            debug_info += debugShowTransformedRelativeRates("Details");
			//for (std::vector<double>::const_iterator it = orig_params.begin(); it != orig_params.end(); ++it)
			//	debug_info += boost::str(boost::format("%15.8f ") % (*it));
			//debug_info += "\nnew_params:  ";
			//for (std::vector<double>::const_iterator it = new_params.begin(); it != new_params.end(); ++it)
			//	debug_info += boost::str(boost::format("%15.8f ") % (*it));
			//debug_info += "\nc_forward:  ";
			//for (std::vector<double>::const_iterator it = c_forward.begin(); it != c_forward.end(); ++it)
			//	debug_info += boost::str(boost::format("%15.8f ") % (*it));
			//debug_info += "\ndir_forward:  ";
			//debug_info += dir_forward->GetDistributionDescription();
			}
		p->setLastLnLike(curr_ln_like);

		accept();
		accepted = true;
		}
	else
		{
	    if (save_debug_info)
    	    {
			debug_info = boost::str(boost::format("REJECT, prev_ln_like = %.5f, getLastLnLike() = %.5f, curr_ln_like = %.5f, prev_ln_ref_dist = %.5f, curr_ln_ref_dist = %.5f, prev_ln_prior = %.5f, curr_ln_prior = %.5f, lnu = %.5f, ln_accept_ratio = %.5f\n") % prev_ln_like % p->getLastLnLike() % curr_ln_like % prev_ln_ref_dist % curr_ln_ref_dist % prev_ln_prior % curr_ln_prior % lnu % ln_accept_ratio);
            debug_info += debugShowTransformedRelativeRates("Details");
            //for (std::vector<double>::const_iterator it = orig_params.begin(); it != orig_params.end(); ++it)
            //    debug_info += boost::str(boost::format("%15.8f ") % (*it));
            //debug_info += "\nnew_params:  ";
            //for (std::vector<double>::const_iterator it = new_params.begin(); it != new_params.end(); ++it)
            //    debug_info += boost::str(boost::format("%15.8f ") % (*it));
            //debug_info += "\nc_forward:  ";
            //for (std::vector<double>::const_iterator it = c_forward.begin(); it != c_forward.end(); ++it)
            //    debug_info += boost::str(boost::format("%15.8f ") % (*it));
            //debug_info += "\ndir_forward:  ";
            //debug_info += dir_forward->GetDistributionDescription();
			}
		curr_ln_like	= p->getLastLnLike();
		revert();
		accepted = false;
		}

    double inverse_psi = 1.0/psi;
    inverse_psi = p->adaptUpdater(inverse_psi, nattempts, accepted);
    psi = 1.0/inverse_psi;

    return accepted;
	}


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

#include <nt2/core/functions/inner_fold.hpp>
#include <nt2/core/functions/details/fold_step.hpp>
#include <boost/fusion/include/pop_front.hpp>
#include <nt2/include/functions/scalar/numel.hpp>
#include <nt2/include/functor.hpp>

namespace nt2 { namespace ext
{

  //============================================================================
  // General inner_fold
  //============================================================================
  BOOST_DISPATCH_IMPLEMENT  ( inner_fold_, tag::cpu_
                            , (Out)(In)(Neutral)(Bop)(Uop)
                            , ((ast_< Out, nt2::container::domain>))
                              ((ast_< In, nt2::container::domain>))
                              (unspecified_<Neutral>)
                              (unspecified_<Bop>)
                              (unspecified_<Uop>)
                            )
  {
    typedef void                                                              result_type;
    typedef typename In::extent_type                                          extent_type;
    typedef typename Out::value_type                                          value_type;

    BOOST_FORCEINLINE result_type
    operator()(Out& out, In& in, Neutral const& neutral, Bop const& bop, Uop const&) const
    {
      extent_type ext = in.extent();
      std::size_t obound = nt2::numel(boost::fusion::pop_front(ext));
      std::size_t ibound  = boost::fusion::at_c<0>(ext);

      for(std::size_t j = 0, k = 0; j < obound; ++j, k+=ibound)
      {
          value_type s_out = details::fold_step(
             neutral(nt2::meta::as_<value_type>()), in, bop, k, ibound, 1
             );

          nt2::run(out, j, s_out);
      }
    }
  };

  } }

#endif


/*
 *  APPLICATION ENTRY POINT
 *      Define ros node
 *      Init memory, perhaps robot controllers/drivers (modifiable by args? or done in program?)
 *
 *  Single node? How will it interface with other, not defined ROS nodes?
 *
 */

#ifndef _ROS_SPINNER_THREADS_
#define _ROS_SPINNER_THREADS_ 2
#endif

#define _USE_MATH_DEFINES
#include <math.h>

#define _RDF_LOG_NAME_ "rdf"

#include <ros/ros.h>

#include "rdf_ur/rdf_robot.h"
#include "rdf_ur/rdf_plan.h"
#include "rdf_ur/rdf_state.h"

#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Geometry>

#include <string>
#include <vector>
#include <stdio.h>

int main (int argc, char** argv) {
    std::string node_name = "rdf_ur_node";
    ROS_INFO_STREAM_NAMED(_RDF_LOG_NAME_, "Initializing ROS node" << node_name << "...");
    ros::init(argc, argv, node_name);
    ros::NodeHandle main_nh;

    ROS_INFO_STREAM_NAMED(_RDF_LOG_NAME_, "Creating ROS async spinner with " << _ROS_SPINNER_THREADS_ << " threads...");
    ros::AsyncSpinner spinner(_ROS_SPINNER_THREADS_);
    ROS_INFO_STREAM_NAMED(_RDF_LOG_NAME_, "Starting ROS spinner...");
    spinner.start();

    ROS_INFO_STREAM_NAMED(_RDF_LOG_NAME_, "Starting application...");
    rdf::state state(main_nh);
    rdf::plan_interface plan("manipulator");

    for (int i = 0; i <= 2; i++) {
        plan.move_j(0.4, 0.6, 0.2, M_PI, 0, M_PI);
        plan.move_j(-0.4, 0.6, 0.2, M_PI, 0, M_PI);
    }

    rdf::joint_tf_map tfs = state.get_robot_joint_tfs();
    for(const auto& tf: tfs) {
        const Eigen::Vector3d pos = tf.second.position;
        const Eigen::Vector3d rot = Eigen::Matrix3d(tf.second.orientation).eulerAngles(0,1,2);
        Eigen::IOFormat CleanFmt(2, 0, ", ", ", ", "[", "]");
        std::cout   << tf.first << "\n"
                    << "XYZ: " << pos.format(CleanFmt) << "\n"
                    << "RPY: " << rot.format(CleanFmt)
                    << std::endl;
    }

    ROS_INFO_STREAM_NAMED(_RDF_LOG_NAME_, "Awaiting shutdown...");
    ros::waitForShutdown();

    return 0;
}


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

  @copyright 2016 J.T.Lapreste

  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_SCALAR_IDIV_INCLUDED
#define BOOST_SIMD_FUNCTION_SCALAR_IDIV_INCLUDED

#include <boost/simd/function/definition/idiv.hpp>

#endif