#include "ElPatternEst.h"

#include <pybind11/eigen.h>
#include <pybind11/numpy.h>
#include <pybind11/stl.h>
#include <vector>

#include <Eigen/Dense>

#include <isce3/core/Ellipsoid.h>
#include <isce3/core/Linspace.h>
#include <isce3/core/Orbit.h>
#include <isce3/core/Poly1d.h>
#include <isce3/geometry/DEMInterpolator.h>

// Alias
namespace py = pybind11;
using namespace isce3::core;
using isce3::antenna::ElPatternEst;
using isce3::geometry::DEMInterpolator;

void addbinding(py::class_<ElPatternEst>& pyElPatternEst)
{
    pyElPatternEst
            // constructors
            .def(py::init([](double sr_start, const Orbit& orbit,
                                  int polyfit_deg = 6,
                                  const DEMInterpolator& dem_interp = {},
                                  double win_ped = 0.0,
                                  const Ellipsoid& ellips = {},
                                  bool center_scale_pf = false) {
                return ElPatternEst(sr_start, orbit, polyfit_deg, dem_interp,
                        win_ped, ellips, center_scale_pf);
            }),
                    py::arg("sr_start"), py::arg("orbit"),
                    py::arg("polyfit_deg") = 6,
                    py::arg_v("dem_interp", DEMInterpolator(), "0.0"),
                    py::arg("win_ped") = 0.0,
                    py::arg_v("ellips", Ellipsoid(), "WGS84"),
                    py::arg("center_scale_pf") = false)
            .def(py::init([](double sr_start, const Orbit& orbit,
                                  const DEMInterpolator& dem_interp) {
                return ElPatternEst(sr_start, orbit, 6, dem_interp, 0.0,
                        Ellipsoid(), false);
            }),
                    py::arg("sr_start"), py::arg("orbit"),
                    py::arg("dem_interp"))

            // properties
            .def_property_readonly("win_ped", &ElPatternEst::winPed)
            .def_property_readonly("polyfit_deg", &ElPatternEst::polyfitDeg)
            .def_property_readonly("is_center_scale_polyfit",
                    &ElPatternEst::isCenterScalePolyfit)

            // methods
            .def("power_pattern_2way", &ElPatternEst::powerPattern2way,
                    py::arg("echo_mat"), py::arg("sr_spacing"),
                    py::arg("chirp_rate"), py::arg("chirp_dur"),
                    py::arg("az_time") = std::nullopt, py::arg("size_avg") = 8,
                    py::arg("inc_corr") = true)
            .def("power_pattern_1way", &ElPatternEst::powerPattern1way,
                    py::arg("echo_mat"), py::arg("sr_spacing"),
                    py::arg("chirp_rate"), py::arg("chirp_dur"),
                    py::arg("az_time") = std::nullopt, py::arg("size_avg") = 8,
                    py::arg("inc_corr") = true)
            .doc() = R"(
A class for estimating one-way or two-way elevation (EL) power
pattern from 2-D raw echo data over quasi-homogenous scene
and provide bunch of meta data. The final power in dB scale is
fit into N-order polynomials as a function of look angle in radians.
)";
}


#include <CGAL/Exact_rational.h>
#include <CGAL/Simple_cartesian.h>
#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Exact_predicates_exact_constructions_kernel.h>
#include <CGAL/Convex_hull_traits_3.h>
#include <CGAL/convex_hull_3.h>

#include <CGAL/assertions.h>
#include <boost/type_traits.hpp>

typedef CGAL::Exact_rational Exact_rational;

typedef CGAL::Exact_predicates_exact_constructions_kernel   EPEC;
typedef CGAL::Exact_predicates_inexact_constructions_kernel EPIC;
typedef CGAL::Simple_cartesian<double>                      SCD;
typedef CGAL::Simple_cartesian<Exact_rational>            SCR;

using namespace CGAL::internal::Convex_hull_3;

int main()
{
  CGAL_static_assertion( (boost::is_same<EPEC,Default_traits_for_Chull_3<EPEC::Point_3>::type>::value) );
  CGAL_static_assertion( (boost::is_same<SCD,Default_traits_for_Chull_3<SCD::Point_3>::type>::value) );
  CGAL_static_assertion( (boost::is_same<SCR,Default_traits_for_Chull_3<SCR::Point_3>::type>::value) );
  CGAL_static_assertion( (boost::is_same<EPEC,Default_traits_for_Chull_3<EPEC::Point_3>::type>::value) );
  CGAL_static_assertion( (boost::is_same<CGAL::Convex_hull_traits_3<EPIC, CGAL::Tag_true>,Default_traits_for_Chull_3<EPIC::Point_3>::type>::value) );
  CGAL_static_assertion( (boost::is_same<Is_on_positive_side_of_plane_3<CGAL::Convex_hull_traits_3<EPIC, CGAL::Tag_true> >::Protector,CGAL::Protect_FPU_rounding<true> >::value) );
  return 0;
}


#include <boost/foreach.hpp>

#include "RecoTauTag/RecoTau/interface/TauDiscriminationProducerBase.h"
#include "FWCore/Utilities/interface/InputTag.h"

#include "CommonTools/Utils/interface/StringObjectFunction.h"
#include "DataFormats/Math/interface/deltaR.h"

#include "DataFormats/TauReco/interface/PFRecoTauChargedHadron.h"
#include "DataFormats/TauReco/interface/PFRecoTauChargedHadronFwd.h"

namespace {
  // Apply a hypothesis on the mass of the strips.
  math::XYZTLorentzVector applyMassConstraint(const math::XYZTLorentzVector& vec, double mass)
  {
    double factor = sqrt(vec.energy()*vec.energy() - mass*mass)/vec.P();
    return math::XYZTLorentzVector(vec.px()*factor, vec.py()*factor, vec.pz()*factor, vec.energy());
  }
}

class PFRecoTauDiscriminationByHPSSelection : public PFTauDiscriminationProducerBase
{
 public:
  explicit PFRecoTauDiscriminationByHPSSelection(const edm::ParameterSet&);
  ~PFRecoTauDiscriminationByHPSSelection();
  double discriminate(const reco::PFTauRef&) const override;

 private:
  typedef StringObjectFunction<reco::PFTau> TauFunc;

  struct DecayModeCuts
  {
    DecayModeCuts()
      : maxMass_(0)
    {}
    ~DecayModeCuts() {} // CV: maxMass object gets deleted by PFRecoTauDiscriminationByHPSSelection destructor
    unsigned nTracksMin_;
    unsigned nChargedPFCandsMin_;
    double minMass_;
    TauFunc* maxMass_;
    bool applyBendCorrection_mass_;
    bool applyBendCorrection_eta_;
    bool applyBendCorrection_phi_;
    double minPi0Mass_;
    double maxPi0Mass_;
    double assumeStripMass_;
  };

  typedef std::pair<unsigned int, unsigned int> IntPair;
  typedef std::pair<double, double> DoublePair;
  typedef std::map<IntPair, DecayModeCuts> DecayModeCutMap;

  DecayModeCutMap decayModeCuts_;
  double matchingCone_;
  double minPt_;

  bool requireTauChargedHadronsToBeChargedPFCands_;

  int minPixelHits_;

  int verbosity_;
};

PFRecoTauDiscriminationByHPSSelection::PFRecoTauDiscriminationByHPSSelection(const edm::ParameterSet& pset)
  : PFTauDiscriminationProducerBase(pset)
{
  // Get the matchign cut
  matchingCone_ = pset.getParameter<double>("matchingCone");
  minPt_ = pset.getParameter<double>("minTauPt");
  // Get the mass cuts for each decay mode
  typedef std::vector<edm::ParameterSet> VPSet;
  const VPSet& decayModes = pset.getParameter<VPSet>("decayModes");
  BOOST_FOREACH( const edm::ParameterSet &decayMode, decayModes ) {
    // The mass window(s)
    DecayModeCuts cuts;
    if ( decayMode.exists("nTracksMin") ) {
      cuts.nTracksMin_ = decayMode.getParameter<unsigned>("nTracksMin");
    } else {
      cuts.nTracksMin_ = 0;
    }
    if ( decayMode.exists("nChargedPFCandsMin") ) {
      cuts.nChargedPFCandsMin_ = decayMode.getParameter<unsigned>("nChargedPFCandsMin");
    } else {
      cuts.nChargedPFCandsMin_ = 0;
    }
    cuts.minMass_ = decayMode.getParameter<double>("minMass");
    cuts.maxMass_ = new TauFunc(decayMode.getParameter<std::string>("maxMass"));
    edm::ParameterSet applyBendCorrection = decayMode.getParameter<edm::ParameterSet>("applyBendCorrection");
    cuts.applyBendCorrection_eta_ = applyBendCorrection.getParameter<bool>("eta");
    cuts.applyBendCorrection_phi_ = applyBendCorrection.getParameter<bool>("phi");
    cuts.applyBendCorrection_mass_ = applyBendCorrection.getParameter<bool>("mass");
    if ( decayMode.exists("minPi0Mass") ) {
      cuts.minPi0Mass_ = decayMode.getParameter<double>("minPi0Mass");
      cuts.maxPi0Mass_ = decayMode.getParameter<double>("maxPi0Mass");
    } else {
      cuts.minPi0Mass_ = -1.e3;
      cuts.maxPi0Mass_ = 1.e9;
    }
    if ( decayMode.exists("assumeStripMass") ) {
      cuts.assumeStripMass_ = decayMode.getParameter<double>("assumeStripMass");
    } else {
      cuts.assumeStripMass_ = -1.0;
    }
    decayModeCuts_.insert(std::make_pair(
            // The decay mode as a key
            std::make_pair(
                decayMode.getParameter<uint32_t>("nCharged"),
                decayMode.getParameter<uint32_t>("nPiZeros")),
            cuts
          ));
  }
  requireTauChargedHadronsToBeChargedPFCands_ = pset.getParameter<bool>("requireTauChargedHadronsToBeChargedPFCands");
  minPixelHits_ = pset.getParameter<int>("minPixelHits");
  verbosity_ = pset.exists("verbosity") ?
    pset.getParameter<int>("verbosity") : 0;

}

PFRecoTauDiscriminationByHPSSelection::~PFRecoTauDiscriminationByHPSSelection()
{
  for ( DecayModeCutMap::iterator it = decayModeCuts_.begin();
	it != decayModeCuts_.end(); ++it ) {
    delete it->second.maxMass_;
  }
}

double
PFRecoTauDiscriminationByHPSSelection::discriminate(const reco::PFTauRef& tau) const
{
  if ( verbosity_ ) {
    edm::LogPrint("PFTauByHPSSelect") << "<PFRecoTauDiscriminationByHPSSelection::discriminate>:" ;
    edm::LogPrint("PFTauByHPSSelect") << " nCharged = " << tau->signalTauChargedHadronCandidates().size() ;
    edm::LogPrint("PFTauByHPSSelect") << " nPiZeros = " << tau->signalPiZeroCandidates().size() ;
  }

  // Check if we pass the min pt
  if ( tau->pt() < minPt_ ) {
    if ( verbosity_ ) {
      edm::LogPrint("PFTauByHPSSelect") << " fails minPt cut." ;
    }
    return 0.0;
  }

  // See if we select this decay mode
  DecayModeCutMap::const_iterator massWindowIter =
      decayModeCuts_.find(std::make_pair(tau->signalTauChargedHadronCandidates().size(),
                                         tau->signalPiZeroCandidates().size()));
  // Check if decay mode is supported
  if ( massWindowIter == decayModeCuts_.end() ) {
    if ( verbosity_ ) {
      edm::LogPrint("PFTauByHPSSelect") << " fails mass-window definition requirement." ;
    }
    return 0.0;
  }
  const DecayModeCuts& massWindow = massWindowIter->second;

  if ( massWindow.nTracksMin_ > 0 ) {
    unsigned int nTracks = 0;
    const std::vector<reco::PFRecoTauChargedHadron>& chargedHadrons = tau->signalTauChargedHadronCandidates();
    for ( std::vector<reco::PFRecoTauChargedHadron>::const_iterator chargedHadron = chargedHadrons.begin();
	  chargedHadron != chargedHadrons.end(); ++chargedHadron ) {
      if ( chargedHadron->algoIs(reco::PFRecoTauChargedHadron::kChargedPFCandidate) || chargedHadron->algoIs(reco::PFRecoTauChargedHadron::kTrack) ) {
	++nTracks;
      }
    }
    if ( verbosity_ ) {
      edm::LogPrint("PFTauByHPSSelect") << "nTracks = " << nTracks << " (min = " << massWindow.nTracksMin_ << ")" ;
    }
    if ( nTracks < massWindow.nTracksMin_ ) {
      if ( verbosity_ ) {
	edm::LogPrint("PFTauByHPSSelect") << " fails nTracks requirement for mass-window." ;
      }
      return 0.0;
    }
  }
  if ( massWindow.nChargedPFCandsMin_ > 0 ) {
    unsigned int nChargedPFCands = 0;
    const std::vector<reco::PFRecoTauChargedHadron>& chargedHadrons = tau->signalTauChargedHadronCandidates();
    for ( std::vector<reco::PFRecoTauChargedHadron>::const_iterator chargedHadron = chargedHadrons.begin();
	  chargedHadron != chargedHadrons.end(); ++chargedHadron ) {
      if ( chargedHadron->algoIs(reco::PFRecoTauChargedHadron::kChargedPFCandidate) ) {
	++nChargedPFCands;
      }
    }
    if ( verbosity_ ) {
      edm::LogPrint("PFTauByHPSSelect") << "nChargedPFCands = " << nChargedPFCands << " (min = " << massWindow.nChargedPFCandsMin_ << ")" ;
    }
    if ( nChargedPFCands < massWindow.nChargedPFCandsMin_ ) {
      if ( verbosity_ ) {
	edm::LogPrint("PFTauByHPSSelect") << " fails nChargedPFCands requirement for mass-window." ;
      }
      return 0.0;
    }
  }

  math::XYZTLorentzVector tauP4 = tau->p4();
  if ( verbosity_ ) {
    edm::LogPrint("PFTauByHPSSelect") << "tau: Pt = " << tauP4.pt() << ", eta = " << tauP4.eta() << ", phi = " << tauP4.phi() << ", mass = " << tauP4.mass() ;
  }
  // Find the total pizero p4
  reco::Candidate::LorentzVector stripsP4;
  BOOST_FOREACH(const reco::RecoTauPiZero& cand,
      tau->signalPiZeroCandidates()){
    math::XYZTLorentzVector candP4 = cand.p4();
    stripsP4 += candP4;
  }

  // Apply strip mass assumption corrections
  if (massWindow.assumeStripMass_ >= 0) {
    BOOST_FOREACH(const reco::RecoTauPiZero& cand,
        tau->signalPiZeroCandidates()){
      math::XYZTLorentzVector uncorrected = cand.p4();
      math::XYZTLorentzVector corrected =
        applyMassConstraint(uncorrected, massWindow.assumeStripMass_);
      math::XYZTLorentzVector correction = corrected - uncorrected;
      tauP4 += correction;
      stripsP4 += correction;
    }
  }
  if ( verbosity_ ) {
    edm::LogPrint("PFTauByHPSSelect") << "strips: Pt = " << stripsP4.pt() << ", eta = " << stripsP4.eta() << ", phi = " << stripsP4.phi() << ", mass = " << stripsP4.mass() ;
  }

  // Check if tau fails mass cut
  double maxMass_value = (*massWindow.maxMass_)(*tau);
  double bendCorrection_mass = ( massWindow.applyBendCorrection_mass_ ) ? tau->bendCorrMass() : 0.;
  if ( !((tauP4.M() - bendCorrection_mass) < maxMass_value && (tauP4.M() + bendCorrection_mass) > massWindow.minMass_) ) {
    if ( verbosity_ ) {
      edm::LogPrint("PFTauByHPSSelect") << " fails tau mass-window cut." ;
    }
    return 0.0;
  }
  // CV: require that mass of charged tau decay products is always within specified mass window,
  //     irrespective of bendCorrection_mass
  reco::Candidate::LorentzVector tauP4_charged;
  const std::vector<reco::PFRecoTauChargedHadron>& signalChargedHadrons = tau->signalTauChargedHadronCandidates();
  for ( std::vector<reco::PFRecoTauChargedHadron>::const_iterator signalChargedHadron = signalChargedHadrons.begin();
	signalChargedHadron != signalChargedHadrons.end(); ++signalChargedHadron ) {
    tauP4_charged += signalChargedHadron->p4();
  }
  if ( !(tauP4_charged.mass() < maxMass_value) ) {
    if ( verbosity_ ) {
      edm::LogPrint("PFTauByHPSSelect") << " fails tau mass-window cut." ;
    }
    return 0.0;
  }

  // Check if it fails the pi0 IM cut
  if ( stripsP4.M() > massWindow.maxPi0Mass_ ||
       stripsP4.M() < massWindow.minPi0Mass_ ) {
    if ( verbosity_ ) {
      edm::LogPrint("PFTauByHPSSelect") << " fails strip mass-window cut." ;
    }
    return 0.0;
  }

  // Check if tau passes matching cone cut
  //edm::LogPrint("PFTauByHPSSelect") << "dR(tau, jet) = " << deltaR(tauP4, tau->jetRef()->p4()) ;
  if ( deltaR(tauP4, tau->jetRef()->p4()) > matchingCone_ ) {
    if ( verbosity_ ) {
      edm::LogPrint("PFTauByHPSSelect") << " fails matching-cone cut." ;
    }
    return 0.0;
  }

  // Check if tau passes cone cut
  double cone_size = tau->signalConeSize();
  // Check if any charged objects fail the signal cone cut
  for (auto const& cand : tau->signalTauChargedHadronCandidates()) {
    if ( verbosity_ ) {
      edm::LogPrint("PFTauByHPSSelect") << "dR(tau, signalPFChargedHadr) = " << deltaR(cand.p4(), tauP4) ;
    }
    if ( deltaR(cand.p4(), tauP4) > cone_size ) {
      if ( verbosity_ ) {
	edm::LogPrint("PFTauByHPSSelect") << " fails signal-cone cut for charged hadron(s)." ;
      }
      return 0.0;
    }
  }
  // Now check the pizeros
  for (auto const& cand : tau->signalPiZeroCandidates()) {
    double bendCorrection_eta = ( massWindow.applyBendCorrection_eta_ ) ? cand.bendCorrEta() : 0.;
    double dEta = std::max(0., fabs(cand.eta() - tauP4.eta()) - bendCorrection_eta);
    double bendCorrection_phi = ( massWindow.applyBendCorrection_phi_ ) ? cand.bendCorrPhi() : 0.;
    double dPhi = std::max(0., std::abs(reco::deltaPhi(cand.phi(), tauP4.phi())) - bendCorrection_phi);
    double dR2 = dEta*dEta + dPhi*dPhi;
    if ( verbosity_ ) {
      edm::LogPrint("PFTauByHPSSelect") << "dR2(tau, signalPiZero) = " << dR2 ;
    }
    if ( dR2 > cone_size*cone_size ) {
      if ( verbosity_ ) {
	edm::LogPrint("PFTauByHPSSelect") << " fails signal-cone cut for strip(s)." ;
      }
      return 0.0;
    }
  }

  if ( requireTauChargedHadronsToBeChargedPFCands_ ) {
    BOOST_FOREACH(const reco::PFRecoTauChargedHadron& cand, tau->signalTauChargedHadronCandidates()) {
      if ( verbosity_ ) {
	std::string algo_string;
	if      ( cand.algo() == reco::PFRecoTauChargedHadron::kChargedPFCandidate ) algo_string = "ChargedPFCandidate";
	else if ( cand.algo() == reco::PFRecoTauChargedHadron::kTrack              ) algo_string = "Track";
	else if ( cand.algo() == reco::PFRecoTauChargedHadron::kPFNeutralHadron    ) algo_string = "PFNeutralHadron";
	else                                                                         algo_string = "Undefined";
	edm::LogPrint("PFTauByHPSSelect") << "algo(signalPFChargedHadr) = " << algo_string ;
      }
      if ( !(cand.algo() == reco::PFRecoTauChargedHadron::kChargedPFCandidate) ) {
	if ( verbosity_ ) {
	  edm::LogPrint("PFTauByHPSSelect") << " fails cut on PFRecoTauChargedHadron algo." ;
	}
	return 0.0;
      }
    }
  }

  if ( minPixelHits_ > 0 ) {
    int numPixelHits = 0;
    const std::vector<reco::PFCandidatePtr>& chargedHadrCands = tau->signalPFChargedHadrCands();
    for ( std::vector<reco::PFCandidatePtr>::const_iterator chargedHadrCand = chargedHadrCands.begin();
	  chargedHadrCand != chargedHadrCands.end(); ++chargedHadrCand ) {
      const reco::Track* track = 0;
      if ( (*chargedHadrCand)->trackRef().isNonnull() ) track = (*chargedHadrCand)->trackRef().get();
      else if ( (*chargedHadrCand)->gsfTrackRef().isNonnull() ) track = (*chargedHadrCand)->gsfTrackRef().get();
      if ( track ) {
	numPixelHits += track->hitPattern().numberOfValidPixelHits();
      }
    }
    if ( !(numPixelHits >= minPixelHits_) ) {
      if ( verbosity_ ) {
	edm::LogPrint("PFTauByHPSSelect") << " fails cut on sum of pixel hits." ;
      }
      return 0.0;
    }
  }

  // Otherwise, we pass!
  if ( verbosity_ ) {
    edm::LogPrint("PFTauByHPSSelect") << " passes all cuts." ;
  }
  return 1.0;
}

DEFINE_FWK_MODULE(PFRecoTauDiscriminationByHPSSelection);


#include <iostream>
#include <dlib/dnn.h>
#include <dlib/data_io.h>
#include <dlib/gui_widgets.h>

using namespace std;
using namespace dlib;

template <long num_filters, typename SUBNET> using con5d = con<num_filters,5,5,2,2,SUBNET>;
template <long num_filters, typename SUBNET> using con5  = con<num_filters,3,3,1,1,SUBNET>;

template <typename SUBNET> using downsampler  = relu<affine<con5d<32, relu<affine<con5d<32, relu<affine<con5d<32,SUBNET>>>>>>>>>;
template <typename SUBNET> using rcon5  = relu<affine<con5<45,SUBNET>>>;

using net_type = loss_mmod<con<1,9,9,1,1,rcon5<rcon5<rcon5<downsampler<input_rgb_image_pyramid<pyramid_down<20>>>>>>>>;

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

int main(int argc, char** argv) try
{
    if (argc != 4)
    {
        cout << "Give the path to the examples/faces directory as the argument to this" << endl;
        cout << "program.  For example, if you are in the examples folder then execute " << endl;
        cout << "this program by running: " << endl;
        cout << "   ./dnn_mmod_ex faces_director gpu_stack max_epocs" << endl;
        cout << endl;
        return 0;
    }
    const std::string faces_directory = argv[1];

    const int gpu_stack = atoi(argv[2]);

    const int max_epocs = atoi(argv[3]);

    std::vector<matrix<rgb_pixel>> images_train;

    std::vector<std::vector<mmod_rect>> face_boxes_train;

    load_image_dataset(images_train, face_boxes_train, faces_directory+"/training.xml");

    cout << "gpu stack: " << gpu_stack << endl;
    cout << "num training images: " << images_train.size() << endl;

    mmod_options options(face_boxes_train, 64,64);
    // The detector will automatically decide to use multiple sliding windows if needed.
    // For the face data, only one is needed however.
    cout << "num detector windows: "<< options.detector_windows.size() << endl;
    for (auto& w : options.detector_windows)
        cout << "detector window width by height: " << w.width << " x " << w.height << endl;
    cout << "overlap NMS IOU thresh:             " << options.overlaps_nms.get_iou_thresh() << endl;
    cout << "overlap NMS percent covered thresh: " << options.overlaps_nms.get_percent_covered_thresh() << endl;

    // Now we are ready to create our network and trainer.
    net_type net(options);
    // The MMOD loss requires that the number of filters in the final network layer equal
    // options.detector_windows.size().  So we set that here as well.
    net.subnet().layer_details().set_num_filters(options.detector_windows.size());
    dnn_trainer<net_type> trainer(net);
    trainer.set_learning_rate(0.1);
    trainer.be_verbose();
    trainer.set_synchronization_file("mmod_sync", std::chrono::minutes(1));
    trainer.set_iterations_without_progress_threshold(max_epocs);

    // Now let's train the network.  We are going to use mini-batches of 150
    // images.   The images are random crops from our training set (see
    // random_cropper_ex.cpp for a discussion of the random_cropper).
    std::vector<matrix<rgb_pixel>> mini_batch_samples;
    std::vector<std::vector<mmod_rect>> mini_batch_labels;

    random_cropper cropper;
    //cropper.set_chip_dims(200, 200);
    // Usually you want to give the cropper whatever min sizes you passed to the
    // mmod_options constructor, which is what we do here.
   // cropper.set_min_object_size(80,80);

    dlib::rand rnd;
    // Run the trainer until the learning rate gets small.  This will probably take several
    // hours.
    while(trainer.get_learning_rate() >= 1e-4)
    {
        cropper(gpu_stack, images_train, face_boxes_train, mini_batch_samples, mini_batch_labels);
        // We can also randomly jitter the colors and that often helps a detector
        // generalize better to new images.
        for (auto&& img : mini_batch_samples)
            disturb_colors(img, rnd);

        trainer.train_one_step(mini_batch_samples, mini_batch_labels);
    }
    // wait for training threads to stop
    trainer.get_net();
    cout << "done training" << endl;

    // Save the network to disk
    net.clean();
    serialize("mmod_gun_network.dat") << net;

}
catch(std::exception& e)
{
    cout << e.what() << endl;
}


//==================================================================================================
/*!
  @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_ISINCLUDED_C_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_ISINCLUDED_C_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{
  /*!
    @ingroup group-reduction
     This function object returns @c true is only if all bits set in @c x are
   not set in @c y

     The two arguments must share the same bit size.

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

    @par Example:

      @snippet isincluded_c.cpp isincluded_c

    @par Possible output:

      @snippet isincluded_c.txt isincluded_c

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

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

#endif


#pragma once

#include <vban/lib/config.hpp>
#include <vban/lib/locks.hpp>
#include <vban/lib/numbers.hpp>
#include <vban/lib/utility.hpp>

#include <boost/optional.hpp>
#include <boost/thread/thread.hpp>

#include <atomic>
#include <memory>

namespace vban
{
enum class work_version
{
	unspecified,
	work_1
};
std::string to_string (vban::work_version const version_a);

class block;
class block_details;
enum class block_type : uint8_t;
bool work_validate_entry (vban::block const &);
bool work_validate_entry (vban::work_version const, vban::root const &, uint64_t const);

uint64_t work_difficulty (vban::work_version const, vban::root const &, uint64_t const);

uint64_t work_threshold_base (vban::work_version const);
uint64_t work_threshold_entry (vban::work_version const, vban::block_type const);
// Ledger threshold
uint64_t work_threshold (vban::work_version const, vban::block_details const);

namespace work_v1
{
	uint64_t value (vban::root const & root_a, uint64_t work_a);
	uint64_t threshold_base ();
	uint64_t threshold_entry ();
	uint64_t threshold (vban::block_details const);
}

double normalized_multiplier (double const, uint64_t const);
double denormalized_multiplier (double const, uint64_t const);
class opencl_work;
class work_item final
{
public:
	work_item (vban::work_version const version_a, vban::root const & item_a, uint64_t difficulty_a, std::function<void (boost::optional<uint64_t> const &)> const & callback_a) :
		version (version_a), item (item_a), difficulty (difficulty_a), callback (callback_a)
	{
	}
	vban::work_version const version;
	vban::root const item;
	uint64_t const difficulty;
	std::function<void (boost::optional<uint64_t> const &)> const callback;
};
class work_pool final
{
public:
	work_pool (unsigned, std::chrono::nanoseconds = std::chrono::nanoseconds (0), std::function<boost::optional<uint64_t> (vban::work_version const, vban::root const &, uint64_t, std::atomic<int> &)> = nullptr);
	~work_pool ();
	void loop (uint64_t);
	void stop ();
	void cancel (vban::root const &);
	void generate (vban::work_version const, vban::root const &, uint64_t, std::function<void (boost::optional<uint64_t> const &)>);
	boost::optional<uint64_t> generate (vban::work_version const, vban::root const &, uint64_t);
	// For tests only
	boost::optional<uint64_t> generate (vban::root const &);
	boost::optional<uint64_t> generate (vban::root const &, uint64_t);
	size_t size ();
	vban::network_constants network_constants;
	std::atomic<int> ticket;
	bool done;
	std::vector<boost::thread> threads;
	std::list<vban::work_item> pending;
	vban::mutex mutex{ mutex_identifier (mutexes::work_pool) };
	vban::condition_variable producer_condition;
	std::chrono::nanoseconds pow_rate_limiter;
	std::function<boost::optional<uint64_t> (vban::work_version const, vban::root const &, uint64_t, std::atomic<int> &)> opencl;
	vban::observer_set<bool> work_observers;
};

std::unique_ptr<container_info_component> collect_container_info (work_pool & work_pool, std::string const & name);
}


#include <iostream>
#include <math.h>
#include <ctime>
#include <time.h>
#include <string>
#include <sstream>
#include <cstring>
#include <array>
#include <chrono>
#include <thread>
#include <condition_variable>
#include <dirent.h>

#include<pthread.h>

#include <openssl/evp.h>
#include <openssl/bn.h>
#include <openssl/rand.h>
#include <openssl/sha.h>

#include "src/pir.hpp"
#include "src/adprp.hpp"
#include "src/client.hpp"
#include "src/server.hpp"

#include <boost/math/distributions/hypergeometric.hpp>
#include <random>

using namespace std;

typedef struct {
    int client_no;
    OfflineAddQueryShort qry;
    //OfflineQuery qry;
    //OnlineQuery qry;
} testQuery;

typedef struct {
    int client_no;
    OfflineReply reply;
    //OnlineReply reply;
} testReply;

vector<testQuery> query_pool;
vector<testReply> reply_pool;

int counter = 0;
int task_done = 0;

vector<testQuery> backup_pool;

#define T 1000  // task pool size

vector<time_t> time_stamp_put(T);
vector<time_t> time_stamp_get(T);

int backup_size = 100;

std::time_t getTimeStamp()
{
    std::chrono::time_point<std::chrono::system_clock,std::chrono::microseconds> tp =
            std::chrono::time_point_cast<std::chrono::microseconds>(std::chrono::system_clock::now());//获取当前时间点
    std::time_t timestamp =  tp.time_since_epoch().count(); //计算距离1970-1-1,00:00的时间长度
    return timestamp;
}

void myclient_inc() {

    testQuery test_query;

    while (1) {

        // generate randomized request in other pool,
        // and in this function, just fetch from that pool
        // and put it into request_pool

        if (!backup_pool.empty()) {
            test_query = *(backup_pool.begin());
            backup_pool.erase(backup_pool.begin());
            query_pool.push_back(test_query);

            counter++;

            usleep(10000); // every 1 sec put in a query (offer load)
        }

    }
}

void myclient_online() {

    testQuery test_query;

    while (1) {

        if (!backup_pool.empty()) {
            test_query = *(backup_pool.begin());
            backup_pool.erase(backup_pool.begin());

            int task_no = test_query.client_no;
            query_pool.push_back(test_query);

            time_t timestamp = getTimeStamp();
            time_stamp_put[task_no] = timestamp;

            counter++;

            usleep(1000); // every 1 sec put in a query (offer load)
        }
    }
}

void testfunc1() {
    printf("test func 1\n");
}
void testfunc2() {
    printf("test func 2\n");
}

long mean_rep_time = 0;

void server_process_scratch() {
    PIRServer server;
    uint32_t db_size = 7070;

    std::random_device rd;
    Database my_database(db_size);
    for (int i = 0; i < db_size; i++) {
        auto val = generateRandBlock();
        my_database[i] = val;
    }

    server.set_database(db_size, &my_database);

    testQuery query;
    testReply reply;
    OfflineQuery offline_query;
    OfflineReply offline_reply;

    auto server_start = chrono::high_resolution_clock::now();

}

/*
void server_process_online() {
    PIRServer server;
    uint32_t db_size = 7000;

    std::random_device rd;
    Database my_database(db_size);
    for (int i = 0; i < db_size; i++) {
        auto val = generateRandBlock();
        my_database[i] = val;
    }

    server.set_database(db_size, &my_database);

    OnlineQuery online_qry;
    OnlineReply online_reply;

    cout << "server start processing" << endl;

    auto server_st = chrono::high_resolution_clock::now();

    while(1) {

        if (task_done < counter) {

            int task_no = query_pool[task_done].client_no;
            online_qry = query_pool[task_done].qry;

            auto rep_st = chrono::high_resolution_clock::now();

            online_reply = server.generate_online_reply(online_qry, 1);

            time_t timestamp = getTimeStamp();
            time_stamp_get[task_no] = timestamp;

            auto rep_ed = chrono::high_resolution_clock::now();

            //reply_pool.push_back(reply);

            auto rep_time = chrono::duration_cast<chrono::microseconds>
                    (rep_ed - rep_st).count();
            mean_rep_time += rep_time;

            task_done++;

        }

        auto server_ed = chrono::high_resolution_clock::now();
        auto server_time = chrono::duration_cast<chrono::microseconds>
                (server_ed-server_st).count();

        if (server_time >= 100000) {
            cout << "client put in vector in total = " << counter << endl;
            cout << "#task done by server=" << task_done << endl;
            cout << "mean_rep_time = " << (double(mean_rep_time)/double(task_done)) << endl;

            int sumtime = 0;

            for (int tno = 0; tno < task_done; tno++) {
                long diff = time_stamp_get[tno] - time_stamp_put[tno];
                //cout << time_stamp_get[tno] << endl;
                //cout << time_stamp_put[tno] << endl;
                sumtime += diff;
            }
            cout << "per task = " << double(sumtime)/double(task_done) << "msec" << endl;

            exit(0);
        }
    }
}
 */

void server_process_inc() {

    PIRServer server;
    uint32_t db_size = 7000;

    std::random_device rd;
    Database my_database(db_size);
    for (int i = 0; i < db_size; i++) {
        auto val = generateRandBlock();
        my_database[i] = val;
    }

    server.set_database(db_size, &my_database);

    int nbr_add = 0.01 * db_size;

    std::vector<Block> v(nbr_add);
    for (int i = 0; i < nbr_add; i++) {
        v[i] = rand()%100000;
    }

    server.add_elements(nbr_add, v);

    testQuery query;
    testReply reply;
    OfflineAddQueryShort offline_add_qry;
    //OfflineQuery offline_qry;
    OfflineReply offline_reply;

    auto server_st = chrono::high_resolution_clock::now();

    while(1) {

        if (task_done < counter) {

            auto rep_st = chrono::high_resolution_clock::now();

            query = query_pool[task_done];
            int cno = query.client_no;
            offline_add_qry = query.qry;

            offline_reply = server.batched_addition_reply(offline_add_qry);

            time_t timestamp = getTimeStamp();
            time_stamp_get[cno] = timestamp;

            //offline_reply = server.generate_offline_reply(offline_qry,1);

            //for (int i = 0; i < 10000000; i++) {
            //    int k; k++; k = k/199999;
            //}

            auto rep_ed = chrono::high_resolution_clock::now();

            reply.client_no = cno;
            reply.reply = offline_reply;
            //reply_pool.push_back(reply);

            auto rep_time = chrono::duration_cast<chrono::microseconds>
                    (rep_ed-rep_st).count();
            mean_rep_time += rep_time;

            task_done++;

        }

        auto server_ed = chrono::high_resolution_clock::now();
        auto server_time = chrono::duration_cast<chrono::seconds>
                (server_ed-server_st).count();
        if (server_time >= 10) {
            cout << "client put in vector in total = " << counter << endl;
            cout << "#task done by server=" << task_done << endl;
            cout << "mean_rep_time = " << long(double(mean_rep_time)/double(task_done)) << endl;

            int sumtime = 0;

            for (int tno = 0; tno < task_done; tno++) {
                long diff = time_stamp_get[tno] - time_stamp_put[tno];

                sumtime += diff;
            }
            cout << "per task = " << double(sumtime)/double(task_done) << "msec" << endl;

            exit(0);
        }
    }
}

int small_func(int q) {
    vector<int> s;
    for (int i = 0; i < 10000; i++) {
        q = (q/3)*(q/2)+i;
        s.push_back(q);
    }
    return q;
}

typedef struct {
    int no;
    int q;
} small_query;

vector<small_query> small_query_pool;
vector<int> small_backup_pool(100000);

vector<time_t> small_tp_put(100000);
vector<time_t> small_tp_get(100000);

void small_server() {

    long mean_time = 0;

    auto svr_st = chrono::high_resolution_clock::now();

    while(1) {
        if (small_query_pool.size()!=0) {
            small_query qry = *(small_query_pool.begin());

            small_query_pool.erase(small_query_pool.begin());

            int no = qry.no;
            int q = qry.q;

            auto proc_st = chrono::high_resolution_clock::now();

            int q2 = small_func(q);

            time_t tp = getTimeStamp();
            small_tp_get[no] = tp;

            auto proc_ed = chrono::high_resolution_clock::now();
            auto proc_time = chrono::duration_cast<chrono::microseconds>
                    (proc_ed-proc_st).count();
            mean_time += proc_time;

            task_done++;
        }

        auto svr_ed = chrono::high_resolution_clock::now();
        auto svr_time = chrono::duration_cast<chrono::microseconds>
                (svr_ed-svr_st).count();
        if (svr_time > 100000) {
            cout << "client put req in pool total = " << counter << endl;
            cout << "small server tasks done = " << task_done << endl;
            cout << "per task time = " << mean_time / task_done << " microsec" << endl;

            long sumtime = 0;
            for (int c = 0; c < task_done; c++) {
                long diff = small_tp_get[c] - small_tp_put[c];
                //cout << "diff = " << diff << endl;
                sumtime += diff;
            }
            cout << "how long it took each individual request to complete:" << sumtime/task_done << endl;

            exit(0);
        }
    }

}

void small_client() {

    while(1) {
        if (counter < small_backup_pool.size()) {

            if (small_query_pool.size() > 2000) sleep(1);

            small_query qry;
            qry.no = counter;
            qry.q = small_backup_pool[counter];

            small_query_pool.push_back(qry);

            time_t tp = getTimeStamp();
            small_tp_put[counter] = tp;

            counter++;
            usleep(600);
        }
    }
}

int main() {

    // benchmark small_func in main
    auto func_st = chrono::high_resolution_clock::now();
    small_func(3);
    auto func_ed = chrono::high_resolution_clock::now();
    auto func_time = chrono::duration_cast<chrono::microseconds>
            (func_ed-func_st).count();

    cout << "small_func benchmarked in main: time = " << func_time << " microsec" << endl;

    for (int i = 0; i < small_backup_pool.size(); i++) {
        small_backup_pool[i] = rand()%100;
    }

    thread threadObj(small_client);
    small_server();
    threadObj.join();

    /*
    srand(2);

    int db_size = 7000;
    uint32_t set_size = 80;
    uint32_t nbr_sets = 100;//90*12;

    int nbr_add = 0.1*db_size;

    for (int i = 0; i < backup_size; i++) {

        PIRClient client;
        client.set_parms(db_size, set_size, nbr_sets);
        client.generate_setkeys();

        OfflineAddQueryShort offline_add_qry = client.batched_addition_query(db_size, nbr_add);

        //OfflineQuery offline_qry = client.generate_offline_query();

        testQuery test_qry;
        test_qry.client_no = i;
        test_qry.qry = offline_add_qry;
        //test_qry.qry = offline_qry;

        backup_pool.push_back(test_qry);
    }
     */

    /*
    PIRClient client;
    client.set_parms(db_size, set_size, nbr_sets);

    for (int i = 0; i < backup_size; i++) {
        client.generate_setkeys();
        OnlineQuery online_qry = client.generate_online_query(rand()%db_size);

        testQuery test_qry;
        test_qry.client_no = i;
        test_qry.qry = online_qry;

        backup_pool.push_back(test_qry);
        //if (i%10 ==0) cout << i << " ";
    }
     */

    //cout << "backup_pool size=" << backup_size<< " built" << endl;

    //thread threadObj2(myclient_inc);
    //server_process_inc();
    //server_process_online();
    //threadObj2.join();

    /* Make test database and set related parms */

    /*
     * Database 7000
     * set size = 80
     * nbr sets = 90*12
     *
     * Database 1000000
     * set size = 1000
     * nbr sets = 1000*12
     * */

    return 0;
}


#define BOOST_TEST_MODULE "test_read_3spn2_bond_potential"

#ifdef BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#else
#include <boost/test/included/unit_test.hpp>
#endif
#include <mjolnir/input/read_local_potential.hpp>
#include <tuple>

using test_types = std::tuple<double, float>;

constexpr inline float  tolerance_value(float)  noexcept {return 1e-4;}
constexpr inline double tolerance_value(double) noexcept {return 1e-8;}

template<typename Real>
decltype(boost::test_tools::tolerance(std::declval<Real>()))
tolerance() {return boost::test_tools::tolerance(tolerance_value(Real()));}

BOOST_AUTO_TEST_CASE_TEMPLATE(read_3spn2_bond_noenv, T, test_types)
{
    mjolnir::LoggerManager::set_default_logger("test_read_3spn2_bond.log");

    using real_type = T;
    {
        using namespace toml::literals;
        const toml::value env;
        const auto v = u8R"(
            indices = [1, 2]
            k       = 3.14
            v0      = 2.71
        )"_toml;

        const auto pot = mjolnir::read_3spn2_bond_potential<real_type>(v, env);
        BOOST_TEST(pot.k()  == real_type(3.14), tolerance<real_type>());
        BOOST_TEST(pot.v0() == real_type(2.71), tolerance<real_type>());
    }
}

BOOST_AUTO_TEST_CASE_TEMPLATE(read_3spn2_bond_env, T, test_types)
{
    mjolnir::LoggerManager::set_default_logger("test_read_3spn2_bond.log");

    using real_type = T;
    {
        using namespace toml::literals;
        const auto env = u8R"(
            indices = [1, 2]
            k       = 3.14
            v0      = 2.71
        )"_toml;
        const auto v = u8R"(
            indices = "indices"
            k       = "k"
            v0      = "v0"
        )"_toml;
        const auto pot = mjolnir::read_3spn2_bond_potential<real_type>(v, env);
        BOOST_TEST(pot.k()  == real_type(3.14), tolerance<real_type>());
        BOOST_TEST(pot.v0() == real_type(2.71), tolerance<real_type>());
    }
}

BOOST_AUTO_TEST_CASE_TEMPLATE(read_local_potential_3spn2_bond_noenv, T, test_types)
{
    mjolnir::LoggerManager::set_default_logger("test_read_3spn2_bond.log");

    using real_type = T;
    {
        using namespace toml::literals;
        const auto v = u8R"(
            parameters = [
                {indices = [1,2], k = 3.14, v0 = 2.71}
            ]
        )"_toml;

        const auto pot = mjolnir::read_local_potential<2,
            mjolnir::ThreeSPN2BondPotential<real_type>>(v);

        const std::array<std::size_t, 2> ref_idx{{1, 2}};

        BOOST_TEST(pot.size() == 1u);
        BOOST_TEST(pot.at(0).first == ref_idx);
        BOOST_TEST(pot.at(0).second.k()  == real_type(3.14), tolerance<real_type>());
        BOOST_TEST(pot.at(0).second.v0() == real_type(2.71), tolerance<real_type>());
    }
}

BOOST_AUTO_TEST_CASE_TEMPLATE(read_local_potential_3spn2_bond_env, T, test_types)
{
    mjolnir::LoggerManager::set_default_logger("test_read_3spn2_bond.log");

    using real_type = T;
    {
        using namespace toml::literals;
        const auto v = u8R"(
            env.indices = [1, 2]
            env.pi      = 3.14
            env.e       = 2.71
            parameters = [
                {indices = "indices", k = "pi", v0 = "e"}
            ]
        )"_toml;

        const auto pot = mjolnir::read_local_potential<2,
            mjolnir::ThreeSPN2BondPotential<real_type>>(v);

        const std::array<std::size_t, 2> ref_idx{{1, 2}};

        BOOST_TEST(pot.size() == 1u);
        BOOST_TEST(pot.at(0).first == ref_idx);
        BOOST_TEST(pot.at(0).second.k()  == real_type(3.14), tolerance<real_type>());
        BOOST_TEST(pot.at(0).second.v0() == real_type(2.71), tolerance<real_type>());
    }
}


#include <string>
#include <iostream>
#include <boost/program_options.hpp>
#include <boost/exception/diagnostic_information.hpp>
#include <boost/algorithm/string/join.hpp>
#include "observers/observer_factory.hpp"
#include "dynamics/dynamics_factory.hpp"
#include "initial_conditions/init_factory.hpp"
#include "profiling.hpp"

namespace po = boost::program_options;

namespace targs
{
	int NUM_OF_PARTICLES = 1000;
	double POTENTIAL_STRENGTH = 0.1;
	bool override_strength = false;
	bool trace_all_sides = false;
	std::string potential_source_file;
	std::vector<std::string> incoming_wave = {"planar"};
	std::string result_file = ".";
	std::vector<std::string> observers = {"density"};
	std::vector<std::string> dynamics = {"particle_potential"};
	bool no_norm_energy;
	double abs_err_bound = 1e-6;
	double rel_err_bound = 1e-6;
	double end_time = 1.0;
	double time_step = -1;
	unsigned thread_count = -1;
	std::size_t memory_avail = -1;
	bool periodic = false;
	std::string integrator;

	void parse_parameters(int argc, char* argv[])
	{
		memory_avail = getMaximumMemoryAvailable() / 1024 / 1024;

		po::options_description desc("Allowed options");
		desc.add_options()
			("help,h", "Produce help message. Prints documentation for possible values of incoming, dynamics and observers arguments.")
			("num-particles,n", po::value<int>(&NUM_OF_PARTICLES)->default_value(1000), "Number of particles to trace")
			("potential_strength,s", po::value<double>(&POTENTIAL_STRENGTH), "Override the strength given in the potential file.")
			("periodic", po::bool_switch(&periodic)->default_value(false), "use periodic boundary conditions, causes density observer to crash!")
			("potential", po::value<std::string>(&potential_source_file), "File from which to load the potential")
			("incoming", po::value<std::vector<std::string>>(&incoming_wave)->default_value(incoming_wave, "plane")->multitoken(), "Form of incoming wavefront: plane|sphere|...")
			("observers", po::value<std::vector<std::string>>(&observers)->composing()->multitoken(), "Observers to attach to the tracing. See below for a short description of possible observers.")
			("dynamics", po::value<std::vector<std::string>>(&dynamics)->composing()->multitoken()->default_value(dynamics, dynamics[0]), "The ray dynamics to use for the simulation. See below for a short description of possible dynamics.")
			("rel-err-bound", po::value<double>(&rel_err_bound)->default_value(rel_err_bound), "Maximum relative error for adaptive integration")
			("abs-err-bound", po::value<double>(&abs_err_bound)->default_value(abs_err_bound), "Maximum absolute error for adaptive integration")
			("end-time,e", po::value<double>(&end_time)->default_value(end_time), "Particle time after which the integration is stopped.")
			("result-path,r", po::value<std::string>(&result_file)->default_value("result"), "Target file path")
			("no-norm-energy", po::bool_switch(&no_norm_energy)->default_value(false), "Do not normalize the particles starting energy.")
			("threads,t", po::value<unsigned>(&thread_count)->default_value(-1), "Maximum number of threads to use for computation.")
			("memory", po::value<std::size_t>(&memory_avail)->default_value( memory_avail ), "Maximum memory the programme is allowed to use, in MB.")
			("integrator", po::value<std::string>(&integrator)->default_value( "adaptive" ), "The integrator to use. One of (adaptive, euler)")
			("time-step", po::value<double>(&time_step), "The time step for the integrator.")
		;

		po::positional_options_description p;
		p.add("potential", -1);
		po::variables_map vm;
		try
		{
			po::store(po::command_line_parser(argc, argv).options(desc).positional(p).run(), vm);
			po::notify(vm);
		} catch( std::exception& ex )
		{
			std::cerr << "error parsing command line parameters!\n";
			std::cerr << boost::diagnostic_information(ex, true) << "\n\n";
			exit( EXIT_FAILURE );
		}

		override_strength = vm.count("potential_strength") != 0;

		if (vm.count("help")) {
            std::cout << desc << "\n";
            std::cout << "Observers: \n";
            getObserverFactory().printHelp(  );

            std::cout << "Dynamics: \n";
            getDynamicsFactory().printHelp(  );

            std::cout << "Incomings: \n";
            init_cond::getInitialConditionFactory().printHelp(  );

			exit( EXIT_SUCCESS);
		}
	}
}

void parse_parameters(int argc, char* argv[])
{
	targs::parse_parameters(argc, argv);
}


#include <algorithm>
#include <functional>
#include <limits>
#include <numeric>

#include <boost/format.hpp>

#include "OctopusBoard.hpp"
#include "FixBoard.hpp"
#include "branch.hpp"
#include "Point.hpp"
#include "SharkSolver.hpp"

namespace slide
{

bool SharkSolver::verbose = false;

template<int H, int W>
void SharkSolver::greedy(const PlayBoard<H, W>& board)
{
    using SharkBoard = SharkUrchinBoard<H, W>;
    //using SharkBoard = SharkOctopusBoard<H, W>;

    int offset = 0;
    int bestScore = std::numeric_limits<int>::max();
    SharkBoard returnAnswerBoard;

    reps(i, offset, board.height() - offset) reps(j, offset, board.width() - offset){
        SharkBoard sharkBoard(board);
        sharkBoard.select(sharkBoard.find(Point(i, j).toInt()));

        for(;;){
            sharkBoard.getCandidates(cand);
            verbose && std::cerr << sharkBoard << std::endl;
            verbose && std::cerr << sharkBoard.fixed << std::endl;
            if(cand.empty()){
                break;
            }

            double minScore = std::numeric_limits<double>::max();
            // std::partial_sort(cand.begin(), cand.begin() + 2, [](const Point& a, const Point& b){
            //     const double score_a = sharkBoard.evaluate(a);
            //     const double score_b = sharkBoard.evaluate(b);
            //     return score_a < score_b;
            // });
            Point bestPoint = cand[0];
            Point secondBestPoint = ( static_cast<int>(cand.size()) >= 2 ? cand[1] : cand[0]);

            for(Point p : cand){
                const double score = sharkBoard.evaluate(p);
                if(score < minScore){
                    minScore = score;
                    secondBestPoint = bestPoint;
                    bestPoint = p;
                }
            }

            if(!sharkBoard.align(bestPoint)) {
                if(!sharkBoard.align(secondBestPoint)){
                    verbose && std::cerr << "align failed" << std::endl;
                    verbose && std::cerr << sharkBoard << std::endl;
                    break;
                }
            }
        }

        sharkBoard.answer.optimize();
        if(bestScore > sharkBoard.answer.size()){
            bestScore = sharkBoard.answer.size();
            returnAnswerBoard = sharkBoard;
        }

    }

    verbose && std::cout << "\n";
    verbose && std::cout << returnAnswerBoard << std::endl;
    onCreatedAnswer(returnAnswerBoard.answer);
}

template<int H, int W>
void SharkSolver::solve(const PlayBoard<H, W>& board)
{
    SharkOctopusBoard<H, W> sharkBoard(board);
    //SharkUrchinBoard<H, W> sharkBoard(board);
	Point selectPoint(sharkBoard.height() / 2, sharkBoard.width() / 2);
	sharkBoard.select(sharkBoard.find(sharkBoard.correctId(selectPoint)));

    for(;;){
		sharkBoard.getCandidates(cand);

        if(cand.empty())
            break;

        double minScore = std::numeric_limits<double>::max();
        Point bestPoint;
        for(Point p : cand){
            const double score = sharkBoard.evaluate(p);
            if(score < minScore){
                minScore = score;
                bestPoint = p;
            }
        }

        bool isSuccess = sharkBoard.align(bestPoint);
        if(!isSuccess) std::cerr << "align failed" << std::endl;
	}

	sharkBoard.answer.optimize();
	onCreatedAnswer(sharkBoard.answer);
}

template<>
void SharkSolver::solve<Flexible, Flexible>(const PlayBoard<Flexible>& board)
{
    branch(make_solve_caller(*this), board);
}

void SharkSolver::solve()
{
    greedy(PlayBoard<Flexible>(problem.board));
    //solve(PlayBoard<Flexible>(problem.board));
}

} // end of namespace slide


<<<<<<< HEAD:Tudat/Astrodynamics/OrbitDetermination/LightTimeCorrectionPartials/UnitTests/unitTestLightTimeCorrectionPartials.cpp
/*    Copyright (c) 2010-2018, Delft University of Technology
=======
/*    Copyright (c) 2010-2019, Delft University of Technology
>>>>>>> origin/master:Tudat/Astrodynamics/OrbitDetermination/LightTimeCorrectionPartials/UnitTests/unitTestLightTimePartials.cpp
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 */

#define BOOST_TEST_MAIN

#include <limits>
#include <string>
#include <vector>

#include <boost/test/unit_test.hpp>

#include "Tudat/Astrodynamics/BasicAstrodynamics/timeConversions.h"
#include "Tudat/Astrodynamics/ObservationModels/lightTimeSolution.h"
#include "Tudat/SimulationSetup/EstimationSetup/createObservationModel.h"
#include "Tudat/SimulationSetup/EstimationSetup/createEstimatableParameters.h"
#include "Tudat/SimulationSetup/EstimationSetup/createLightTimeCorrectionPartials.h"
#include "Tudat/Astrodynamics/OrbitDetermination/LightTimeCorrectionPartials/firstOrderRelativisticPartial.h"
#include "Tudat/Astrodynamics/OrbitDetermination/ObservationPartials/UnitTests/observationPartialTestFunctions.h"

namespace tudat
{
namespace unit_tests
{

//Using declarations.
using namespace tudat::gravitation;
using namespace tudat::ephemerides;
using namespace tudat::observation_models;
using namespace tudat::simulation_setup;
using namespace tudat::spice_interface;
using namespace tudat::observation_partials;
using namespace tudat::orbit_determination;
using namespace tudat::observation_models;
using namespace tudat::estimatable_parameters;
using namespace tudat::unit_tests;

BOOST_AUTO_TEST_SUITE( test_light_time_partials )

BOOST_AUTO_TEST_CASE( testOneWayRangePartialsWrtLightTimeParameters )
{
    {
        // Define and create ground stations.
        std::vector< std::pair< std::string, std::string > > groundStations;
        groundStations.resize( 2 );
        groundStations[ 0 ] = std::make_pair( "Earth", "Graz" );
        groundStations[ 1 ] = std::make_pair( "Mars", "MSL" );

        // Create environment
        NamedBodyMap bodyMap = setupEnvironment( groundStations );

        double ephemerisEvaluationTime = basic_astrodynamics::calculateJulianDaySinceEpoch< double >(
                    boost::gregorian::date( 2002, 8, 10 ), 0.0 ) * physical_constants::JULIAN_DAY;

        std::dynamic_pointer_cast< ConstantEphemeris >( bodyMap[ "Earth" ]->getEphemeris( ) )->updateConstantState(
                    getBodyCartesianStateAtEpoch( "Earth", "SSB", "ECLIPJ2000", "NONE", ephemerisEvaluationTime ) );
        std::dynamic_pointer_cast< ConstantEphemeris >( bodyMap[ "Mars" ]->getEphemeris( ) )->updateConstantState(
                    getBodyCartesianStateAtEpoch( "Mars", "SSB", "ECLIPJ2000", "NONE", ephemerisEvaluationTime ) );

        // Set link ends for observation model
        LinkEnds linkEnds;
        linkEnds[ transmitter ] = groundStations[ 1 ];
        linkEnds[ receiver ] = groundStations[ 0 ];

        // Generate one-way range model
        std::vector< std::shared_ptr< LightTimeCorrectionSettings > > lightTimeCorrections;
        std::vector< std::string > relativisticPerturbingBodies = { "Sun" };
        lightTimeCorrections.push_back( std::make_shared< FirstOrderRelativisticLightTimeCorrectionSettings >(
                                            relativisticPerturbingBodies ) );
        std::shared_ptr< ObservationSettings > observationSettings = std::make_shared<
                ObservationSettings >( one_way_range, lightTimeCorrections );
        std::shared_ptr< OneWayRangeObservationModel< double, double > > oneWayRangeModel =
                std::dynamic_pointer_cast< OneWayRangeObservationModel< double, double > >(
                    observation_models::ObservationModelCreator< 1, double, double >::createObservationModel(
                        linkEnds, observationSettings, bodyMap ) );

        // Create parameters for which partials are to be computed
        std::vector< std::shared_ptr< estimatable_parameters::EstimatableParameterSettings > > parameterSettings;
        parameterSettings.push_back( std::make_shared< estimatable_parameters::EstimatableParameterSettings >(
                                         "Sun", gravitational_parameter ) );
        parameterSettings.push_back( std::make_shared< estimatable_parameters::EstimatableParameterSettings >(
                                         "global_metric", ppn_parameter_gamma ) );
        std::shared_ptr< estimatable_parameters::EstimatableParameterSet< double > > parametersToEstimate =
                createParametersToEstimate( parameterSettings, bodyMap );

        // Create partial objects.
        std::pair< SingleLinkObservationPartialList, std::shared_ptr< PositionPartialScaling > > partialList =
                createOneWayRangePartials( linkEnds, bodyMap, parametersToEstimate,
                                           oneWayRangeModel->getLightTimeCalculator( )->getLightTimeCorrection( ) );
        std::shared_ptr< PositionPartialScaling > positionPartialScaler = partialList.second;

        // Compute current observation and link end times/states
        std::vector< double > linkEndTimes;
        std::vector< Eigen::Matrix< double, 6, 1 > > linkEndStates;
        double testTime = 1.1E7;
        Eigen::VectorXd currentPositionObservation =
                oneWayRangeModel->computeObservationsWithLinkEndData( testTime, transmitter, linkEndTimes, linkEndStates );

        // Update position partial scaler for current observation
        positionPartialScaler->update( linkEndStates, linkEndTimes, transmitter, currentPositionObservation );

        // Define numerical partial settings
        std::vector< double > perturbations = { 1.0E16, 1.0E8 };
        std::vector< double > tolerances = { 1.0E-4, 10E-4 };

        // Compute numerical partials for each parameter and compare to analytical result.
        std::function< double( const double ) > observationFunction = std::bind(
                    &ObservationModel< 1, double, double >::computeObservationEntry, oneWayRangeModel, std::placeholders::_1, transmitter, 0 );
        for( SingleLinkObservationPartialList::iterator partialIterator = partialList.first.begin( ); partialIterator != partialList.first.end( );
             partialIterator++ )
        {
            // Compute total analytical partial
            std::vector< std::pair< Eigen::Matrix< double, 1, Eigen::Dynamic >, double > > calculatedPartial =
                    partialIterator->second->calculatePartial( linkEndStates, linkEndTimes, transmitter );
            Eigen::Matrix< double, 1, Eigen::Dynamic > totalPartial = Eigen::Matrix< double, 1, Eigen::Dynamic >::Zero( 1, 1 );
            for( unsigned int j = 0; j < calculatedPartial.size( ); j++ )
            {
                totalPartial += calculatedPartial.at( j ).first;
            }

            // Compute numerical partial and compare to analytical result.
            BOOST_CHECK_CLOSE_FRACTION(
                        calculateNumericalObservationParameterPartial(
                            parametersToEstimate->getDoubleParameters( )[ partialIterator->first.first ],
                        perturbations.at( partialIterator->first.first ), observationFunction, testTime ).x( ), totalPartial.x( ),
                    tolerances.at( partialIterator->first.first ) );
        }
    }

    {

        // Define and create ground stations.
        std::vector< std::pair< std::string, std::string > > groundStations;
        groundStations.resize( 2 );
        groundStations[ 0 ] = std::make_pair( "Earth", "Graz" );
        groundStations[ 1 ] = std::make_pair( "Mars", "MSL" );

        // Create environment
        NamedBodyMap bodyMap = setupEnvironment( groundStations, 1.0E7, 1.2E7, 1.65E7 );

        // Set link ends for observation model
        LinkEnds linkEnds;
        linkEnds[ transmitter ] = groundStations[ 1 ];
        linkEnds[ receiver ] = groundStations[ 0 ];

        // Generate one-way range model
        std::vector< std::shared_ptr< LightTimeCorrectionSettings > > lightTimeCorrections;
        std::vector< std::string > perturbingBodyList = { "Earth", "Sun" };
        lightTimeCorrections.push_back( std::make_shared< FirstOrderRelativisticLightTimeCorrectionSettings >(
                                            perturbingBodyList ) );
        std::shared_ptr< ObservationSettings > observationSettings = std::make_shared<
                ObservationSettings >( one_way_range, lightTimeCorrections );
        std::shared_ptr< ObservationModel< 1 > > oneWayRangeModel =
                observation_models::ObservationModelCreator< 1, double, double >::createObservationModel(
                    linkEnds, observationSettings, bodyMap  );

        std::map< LinkEnds, std::shared_ptr< ObservationModel< 1 > > > oneWayRangeModelMap;
        oneWayRangeModelMap[ linkEnds ] = oneWayRangeModel;

        // Create parameter objects.
        std::vector< std::shared_ptr< EstimatableParameterSettings > > parameterNames;
        parameterNames.push_back( std::make_shared< EstimatableParameterSettings >( "Sun", gravitational_parameter ) );
        parameterNames.push_back( std::make_shared< EstimatableParameterSettings >( "Earth", gravitational_parameter ) );
        parameterNames.push_back( std::make_shared< estimatable_parameters::EstimatableParameterSettings >(
                                      "global_metric", ppn_parameter_gamma ) );
        parameterNames.push_back( std::make_shared< EstimatableParameterSettings >( "Mars", gravitational_parameter ) );
        std::shared_ptr< estimatable_parameters::EstimatableParameterSet< double > > parametersToEstimate =
                createParametersToEstimate< double >( parameterNames, bodyMap );
        std::vector< std::shared_ptr< EstimatableParameter< double > > > doubleParameterVector =
                parametersToEstimate->getEstimatedDoubleParameters( );

        // Create observation partials.
        std::shared_ptr< ObservationPartialCreator< 1, double, double > > observationPartialCreator =
                std::make_shared< ObservationPartialCreator< 1, double, double > >( );
        std::pair< std::map< std::pair< int, int >, std::shared_ptr< ObservationPartial< 1 > > >,
                std::shared_ptr< PositionPartialScaling > > fullAnalyticalPartialSet =
                observationPartialCreator->createObservationPartials(
                    one_way_range, oneWayRangeModelMap, bodyMap, parametersToEstimate ).begin( )->second;

        std::shared_ptr< PositionPartialScaling > positionPartialScaler = fullAnalyticalPartialSet.second;

        // Compute partials for each refernce link end.
        for( LinkEnds::const_iterator linkEndIterator = linkEnds.begin( ); linkEndIterator != linkEnds.end( );
             linkEndIterator++ )
        {
            // Evaluate nominal observation values
            std::vector< Eigen::Vector6d > vectorOfStates;
            std::vector< double > vectorOfTimes;
            double observationTime = 1.1E7;
            Eigen::VectorXd currentRangeObservation = oneWayRangeModel->computeObservationsWithLinkEndData(
                        observationTime, linkEndIterator->first, vectorOfTimes, vectorOfStates );

            // Calculate analytical observation partials.
            positionPartialScaler->update( vectorOfStates, vectorOfTimes, static_cast< LinkEndType >( linkEndIterator->first ),
                                           currentRangeObservation );
            typedef std::vector< std::pair< Eigen::Matrix< double, 1, Eigen::Dynamic >, double > > ObservationPartialReturnType;
            std::vector< ObservationPartialReturnType > analyticalObservationPartials =
                    calculateAnalyticalPartials(
                        fullAnalyticalPartialSet.first, vectorOfStates, vectorOfTimes, linkEndIterator->first );

            // Test evaliuuation time of partials
            for( unsigned int i = 0; i < analyticalObservationPartials.size( ); i++ )
            {
                for( unsigned int j = 0; j < analyticalObservationPartials.at( i ).size( ); j++ )
                {
                    BOOST_CHECK_CLOSE_FRACTION( analyticalObservationPartials.at( i ).at( j ).second,
                                                ( vectorOfTimes.at( 0 ) + vectorOfTimes.at( 1 ) ) / 2.0,
                                                std::numeric_limits< double >::epsilon( ) );
                }

            }

            // Settings for body state partials
            std::function< Eigen::VectorXd( const double ) > observationFunction = std::bind(
                        &ObservationModel< 1, double, double >::computeObservations, oneWayRangeModel, std::placeholders::_1,
                        linkEndIterator->first );

            // Settings for parameter partial functions.
            std::vector< double > parameterPerturbations = { 1.0E19, 1.0E16, 1.0E15, 1.0E8 };
            std::vector< std::function< void( ) > > updateFunctionList;
            updateFunctionList.push_back( emptyVoidFunction );
            updateFunctionList.push_back( emptyVoidFunction );
            updateFunctionList.push_back( emptyVoidFunction );
            updateFunctionList.push_back( emptyVoidFunction );

            // Calculate and test analytical against numerical partials.
            std::vector< Eigen::VectorXd > numericalPartialsWrtDoubleParameters = calculateNumericalPartialsWrtDoubleParameters(
                        doubleParameterVector, updateFunctionList, parameterPerturbations, observationFunction, observationTime );

            // Compare analytical and numerical partials
            for( unsigned int i = 0; i < analyticalObservationPartials.size( ); i++ )
            {

                double currentParameterPartial = 0.0;
                for( unsigned int j = 0; j < analyticalObservationPartials.at( i ).size( ); j++ )
                {
                    currentParameterPartial += analyticalObservationPartials.at( i ).at( j ).first.x( );

                }

                BOOST_CHECK_CLOSE_FRACTION( currentParameterPartial, numericalPartialsWrtDoubleParameters.at( i ).x( ), 1.0E-4 );
            }

            BOOST_CHECK_EQUAL( numericalPartialsWrtDoubleParameters[ 3 ].x( ), 0.0 );

        }
    }
}
BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests

} // namespace tudat


////////////////////////////////////////////////////////////////////////////////
// evaluate_stripe_field.hh
////////////////////////////////////////////////////////////////////////////////
/*! @file
//  Sampling of the stripe pattern on a subdivided mesh.
*/
//  Author:  Julian Panetta (jpanetta), julian.panetta@gmail.com
//  Created:  04/27/2019 15:36:32
////////////////////////////////////////////////////////////////////////////////
#ifndef EVALUATE_STRIPE_FIELD_HH
#define EVALUATE_STRIPE_FIELD_HH

#include <MeshFEM/MeshIO.hh>
#include <Eigen/Dense>

void evaluate_stripe_field(const Eigen::MatrixX3d &vertices,
                           const Eigen::MatrixX3i &elements,
                           const std::vector<double> &stretchAngles,
                           const std::vector<double> &wallWidths,
                           const double frequency,
                           Eigen::MatrixX3d &outVerticesEigen,
                           Eigen::MatrixX3i &outTrianglesEigen,
                           std::vector<double> &stripeField,
                           const size_t nsubdiv = 3,
                           const bool glue = true);

#endif /* end of include guard: EVALUATE_STRIPE_FIELD_HH */


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

  @copyright 2015 NumScale SAS
  @copyright 2015 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_ARCH_COMMON_SCALAR_FUNCTION_CTZ_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SCALAR_FUNCTION_CTZ_HPP_INCLUDED

#ifndef __GNUC__
#include <boost/simd/function/scalar/ffs.hpp>
#endif
#include <boost/simd/function/scalar/bitwise_cast.hpp>
#include <boost/dispatch/function/overload.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/config.hpp>
#ifdef BOOST_MSVC
#include <intrin.h>
#endif
#include <boost/simd/function/scalar/bitwise_cast.hpp>
#include <boost/dispatch/function/overload.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/config.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  BOOST_DISPATCH_OVERLOAD ( ctz_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::type64_<A0> >
                          )
  {
    using result_t = bd::as_integer_t<A0>;
    BOOST_FORCEINLINE result_t operator() ( A0 a0) const
    {
      result_t that = bitwise_cast<result_t>(a0);
      if(!that) return result_t(64);

    #if defined __GNUC__
      return __builtin_ctzll(that);
    #else
      return ffs(that)-1;
    #endif
    }
  };

  BOOST_DISPATCH_OVERLOAD ( ctz_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::type32_<A0> >
                          )
  {
    using result_t = bd::as_integer_t<A0>;
    BOOST_FORCEINLINE result_t operator() ( A0 a0) const
    {
      result_t that = bitwise_cast<result_t>(a0);
      if(!that) return result_t(32);

    #if defined __GNUC__
      return __builtin_ctz(that);
    #else
      return ffs(that)-1;
    #endif
    }
  };

  BOOST_DISPATCH_OVERLOAD ( ctz_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::arithmetic_<A0> >
                          )
  {
    BOOST_FORCEINLINE bd::as_integer_t<A0> operator() ( A0 a0) const
    {
      return ctz(uint32_t(a0));
    }
  };
} } }

#endif


//
// Created by dch on 06/06/16.
//

#include "cuDemonsSolver.h"
#include <boost/program_options.hpp>
#include <string>
#include <iostream>
#include <hoCuNDArray.h>
#include "hoNDArray_fileio.h"
#include <cuLinearResampleOperator.h>
#include <boost/make_shared.hpp>
#include "cuNDArray_fileio.h"
#include "vector_td_io.h"

namespace po = boost::program_options;
using namespace Gadgetron;
using namespace std;

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

    po::options_description desc("Allowed options");

    float sigma_diff,sigma_fluid,sigma_int,sigma_vdiff,alpha;
    bool composite;
    string image_filename, output_filename;
    int iterations,levels;
    vector_td<float,3> phys_dims;
    desc.add_options()
            ("help", "produce help message")
            ("image,f", po::value<string>(&image_filename)->default_value("reconstruction.real"), "4D image")
            ("output", po::value<string>(&output_filename)->default_value("vfield.real"), "Output vfield name")
            ("alpha,a",po::value<float>(&alpha)->default_value(4.0),"Maximum step length per iteration")
            ("sigma_diff",po::value<float>(&sigma_diff)->default_value(1),"Diffusion sigma for regularization")
            ("sigma_fluid",po::value<float>(&sigma_fluid)->default_value(0),"Fluid sigma for regularization")
            ("sigma_int",po::value<float>(&sigma_int)->default_value(0),"Intensity sigma for regularization (bilateral)")
            ("iterations,i",po::value<int>(&iterations)->default_value(30),"Number of iterations to use")
            ("levels",po::value<int>(&levels)->default_value(0),"Number of multiresolution levels to use")
            ("sigma_vdiff",po::value<float>(&sigma_vdiff)->default_value(0),"Vector field difference sigma for regularization (bilateral)")
            ("composite",po::value<bool>(&composite)->default_value(true),"Do proper vector composition when adding vector fields")
            ("physical_dims",po::value<vector_td<float,3>>(&phys_dims)->default_value(vector_td<float,3>(1,1,1)),"Physical dimension in mm")
            ;
    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, desc), vm);
    po::notify(vm);

    if (vm.count("help")) {
        cout << desc << "\n";
        return 1;
    }

    std::cout << "Command line options:" << std::endl;
    for (po::variables_map::iterator it = vm.begin(); it != vm.end(); ++it){
        boost::any a = it->second.value();
        std::cout << it->first << ": ";
        if (a.type() == typeid(std::string)) std::cout << it->second.as<std::string>();
        else if (a.type() == typeid(int)) std::cout << it->second.as<int>();
        else if (a.type() == typeid(unsigned int)) std::cout << it->second.as<unsigned int>();
        else if (a.type() == typeid(float)) std::cout << it->second.as<float>();
        else if (a.type() == typeid(vector_td<float,3>)) std::cout << it->second.as<vector_td<float,3> >();
        else if (a.type() == typeid(vector_td<int,3>)) std::cout << it->second.as<vector_td<int,3> >();
        else if (a.type() == typeid(vector_td<unsigned int,3>)) std::cout << it->second.as<vector_td<unsigned int,3> >();
        else std::cout << "Unknown type" << std::endl;
        std::cout << std::endl;
    }

    floatd3 sigma_diff_v = floatd3(sigma_diff);
    sigma_diff_v /= phys_dims;

    cuDemonsSolver<float,3> demonsSolver;

    demonsSolver.set_sigmaDiff(sigma_diff_v);
    demonsSolver.set_sigmaFluid(sigma_fluid);
    demonsSolver.set_sigmaInt(sigma_int);
    demonsSolver.set_sigmaVDiff(sigma_vdiff);
    demonsSolver.set_compositive(composite);
    demonsSolver.set_exponential(true);
    demonsSolver.set_iterations(iterations);
    demonsSolver.set_alpha(alpha);
    //demonsSolver.use_normalized_gradient_field(0.01);

    auto image = hoCuNDArray<float>(*read_nd_array<float>(image_filename.c_str()));

    if (image.get_number_of_dimensions() != 4)
        throw std::runtime_error("Image must be 4D");

    auto dims = *image.get_dimensions();

    auto dims3D = std::vector<size_t>(dims.begin(),dims.end()-1);
    auto vdims = std::vector<size_t>{dims[0],dims[1],dims[2],3,dims[3]};
    auto vdims3D = std::vector<size_t>(vdims.begin(),vdims.end()-1);

    auto vfield = hoCuNDArray<float>(vdims);

    size_t elements = image.get_number_of_elements()/dims[3];
    size_t ntimes = dims[3];
    for (size_t i = 0; i < vdims[4]; i++){
        std::cout << "Doing registration " << i << std::endl;
        hoCuNDArray<float> movingImage(dims3D,image.get_data_ptr()+(i+1)%ntimes*elements);
        hoCuNDArray<float> staticImage(dims3D,image.get_data_ptr()+i*elements);

        cuNDArray<float> cuMov(movingImage);
        cuNDArray<float> cuStat(staticImage);

        auto cuVfield = demonsSolver.multi_level_reg(cuStat,cuMov,levels);

        auto vfieldView = hoCuNDArray<float>(vdims3D,vfield.get_data_ptr()+i*elements*3);
        vfieldView = cuVfield;
    }

    auto final_vfield = hoCuNDArray<float>(vdims3D,vfield.get_data_ptr()+(vdims[4]-1)*elements*3);
    auto cufinal_vfield = cuNDArray<float>(final_vfield);

    std::cout << "Accumulating " << std::endl;
    for (int i = vdims[4]-2; i >= 0; i--){
        std::cout << "Number " << i << std::endl;
        auto vfieldView = hoCuNDArray<float>(vdims3D,vfield.get_data_ptr()+i*elements*3);
        auto cuVfieldView = cuNDArray<float>(vfieldView);
        deform_vfield(cuVfieldView,cufinal_vfield);
        cufinal_vfield += cuVfieldView;
    }

    std::cout << "Writing output " << std::endl;
    hoCuNDArray<float> tmp(vdims3D);
    tmp = cufinal_vfield;
    write_nd_array(&tmp,"vfield.real");

    std::cout << " Done? " << std::endl;

    return 0;

}


/*
 * parallax_correction.cc
 *
 *  Copyright (C) 2013 Diamond Light Source
 *
 *  Author: James Parkhurst
 *
 *  This code is distributed under the BSD license, a copy of which is
 *  included in the root directory of this package.
 */
#include <boost/python.hpp>
#include <boost/python/def.hpp>
#include <dxtbx/model/parallax_correction.h>
#include <scitbx/vec2.h>
#include <scitbx/vec3.h>

namespace dxtbx { namespace model { namespace boost_python {

  using namespace boost::python;

  // Parallax functions are overloaded so declare the forms for boost
  typedef vec2<double> (*parallax_orig_type)(double d,
                                             double la,
                                             vec2<double> xy0,
                                             vec2<double> xy);
  typedef vec2<double> (*parallax_axis_type)(double mu,
                                             double t0,
                                             vec2<double> xy,
                                             vec3<double> fast_axis,
                                             vec3<double> slow_axis,
                                             vec3<double> origin);

  void export_parallax_correction() {
    def("parallax_correction",
        (parallax_orig_type)&parallax_correction,
        (arg("d"), arg("la"), arg("xy0"), arg("xy")));
    def("parallax_correction",
        (parallax_axis_type)&parallax_correction,
        (arg("mu"), arg("t0"), arg("xy"), arg("fast"), arg("slow"), arg("origin")));
    def("parallax_correction_inv",
        (parallax_orig_type)&parallax_correction_inv,
        (arg("d"), arg("la"), arg("xy0"), arg("xy")));
    def("parallax_correction_inv",
        (parallax_axis_type)&parallax_correction_inv,
        (arg("mu"), arg("t0"), arg("xy"), arg("fast"), arg("slow"), arg("origin")));
  }

}}}  // namespace dxtbx::model::boost_python


//==================================================================================================
/*!
  @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_IF_ZERO_ELSE_ALLBITS_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_IF_ZERO_ELSE_ALLBITS_HPP_INCLUDED

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

  /*!
    @ingroup group-boolean
    This function object conditionally returns  @c 0 (respectively @ref Allbits)
    if @c c is @ref True (respectively  @ref False)

    This is an alias of @ref genmaskc

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

    @par Example:

      @snippet if_zero_else_allbits.cpp if_zero_else_allbits

    @par Possible output:

      @snippet if_zero_else_allbits.txt if_zero_else_allbits

  **/

  ///@{
  as_arithmetic_t<LogicalValue> if_zero_else_allbits(LogicalValue const& c);
  Value if_zero_else_allbits(Value const& c);
  ///@}
} }
#endif

#include <boost/simd/function/scalar/genmaskc.hpp>
#include <boost/simd/function/simd/if_zero_else_allbits.hpp>

#endif


#pragma once

#include <unordered_map>
#include <functional>
#include <variant>
#include <optional>
#include <tuple>
#include <utility>

#include <mplex/tuple/apply.hpp>
#include <mplex/tuple/push_back.hpp>
#include <boost/preprocessor/repetition/repeat.hpp>

namespace EventManagement
{
    namespace Detail
    {
        template <std::size_t Size, typename ElementType>
        struct TupleMaker
        {
            using type = typename mplex::push_back <typename TupleMaker <Size-1, ElementType>::type, ElementType>::type;
        };

        template <typename ElementType>
        struct TupleMaker <0, ElementType>
        {
            using type = std::tuple<>;
        };

        template <typename T>
        struct FunctionTraits : public FunctionTraits<decltype(&T::operator())>
        {};

        template <typename ClassType, typename ReturnType, typename... Args>
        struct FunctionTraits<ReturnType(ClassType::*)(Args...) const>
        // we specialize for pointers to member function
        {
            using result_type = ReturnType;
            using arg_tuple = std::tuple<Args...>;
            static constexpr auto arity = sizeof...(Args);
        };

        template <class F, std::size_t ... Is, class T>
        auto LambdaToFuncImpl(F f, std::index_sequence<Is...>, T) {
            return std::function<typename T::result_type(std::tuple_element_t<Is, typename T::arg_tuple>...)>(f);
        }

        template <class F>
        auto LambdaToFunc(F f) {
            using traits = FunctionTraits<F>;
            return LambdaToFuncImpl(f, std::make_index_sequence<traits::arity>{}, traits{});
        }
    }

    /**
     *  A class that can store Functors with a given maximum amount of parameters and parameter types.
     *  These functors can then be called later as part of an observer or event system.
     *
     *  @param MaxParams The maximum amount of parameters a functor can possess.
     *  @param ParamTypes Possible parameter types.
     */
    template <typename EnumType, std::size_t MaxParams, typename... ParamTypes>
    class EventManager
    {
    public:
        using enum_type = EnumType;

        using parameter_type = std::variant <
            ParamTypes...
        >;

        using parameter_list_type = typename Detail::TupleMaker <MaxParams, parameter_type>::type;

    private:
        template <typename T>
        struct functionFactoryHelper
        {
        };
        template <std::size_t... indice>
        struct functionFactoryHelper <std::index_sequence<indice...>>
        {
            template <typename... T>
            static constexpr std::function <void(parameter_list_type const&)> convert(std::function <void(T const&...)>&& func)
            {
                return [func](parameter_list_type const& list) -> void
                {
                    return func(
                        std::get<T>(std::get<indice>(list))...
                    );
                };
            }
        };
        template <typename T>
        struct functionCallHelper
        {
        };
        template <std::size_t... indice>
        struct functionCallHelper <std::index_sequence<indice...>>
        {
            template <typename... List>
            static constexpr void fillParameters(parameter_list_type& tup, List... params)
            {
                ((std::get <indice>(tup) = params), ...);
            }
        };

        template <typename... T>
        std::function <void(parameter_list_type const&)> makeFunction(std::function <void(T const&...)>&& func)
        {
            return functionFactoryHelper <std::make_index_sequence <sizeof...(T)>>::template convert<T...>(std::move(func));
        }

    public:
        /**
         *  Adds a functor to the list.
         */
        template <typename... ParameterType>
        void add(enum_type type, std::function <void(ParameterType const&...)> const& cb)
        {
            unnamedEvents[type].push_back(makeFunction(std::move(cb)));
        }

        /**
         *  Adds a named functor to the list, useful to avoid accidental duplicates.
         */
        template <typename... ParameterType>
        void add(enum_type type, std::string const& name, std::function <void(ParameterType const&...)> const& cb)
        {
            namedEvents[type][name] = std::move(makeFunction(cb));
        }

        /**
         *  Adds a functor to the list.
         */
        template <typename LambdaT, typename... ParameterType>
        void add(enum_type type, LambdaT const& cb)
        {
            unnamedEvents[type].push_back(makeFunction(Detail::LambdaToFunc(cb)));
        }

        /**
         *  Adds a named functor to the list, useful to avoid accidental duplicates.
         */
        template <typename LambdaT, typename... ParameterType>
        void add(enum_type type, std::string const& name, LambdaT const& cb)
        {
            namedEvents[type][name] = std::move(makeFunction(Detail::LambdaToFunc(cb)));
        }

        void clear()
        {
            namedEvents.clear();
            unnamedEvents.clear();
        }

        template <typename... Params>
        void call(enum_type type, Params&&... params) const
        {
            auto parms = parameter_list_type{};
            functionCallHelper<std::make_index_sequence<sizeof...(params)>>::template fillParameters <Params...>(parms, params...);

            if (auto iter = namedEvents.find(type); iter != std::end(namedEvents))
                for ([[maybe_unused]] auto const& [dummy, func] : iter->second)
                    func(parms);

            if (auto iter = unnamedEvents.find(type); iter != std::end(unnamedEvents))
                for (auto& func : iter->second)
                    func(parms);
        }

        template <typename... Params>
        void callNamed(enum_type type, std::string const& name, Params&&... params) const
        {
            auto parms = parameter_list_type{};
            functionCallHelper<std::make_index_sequence<sizeof...(params)>>::template fillParameters <Params...>(parms, params...);

            namedEvents.at(type).at(name)(std::move(parms));
        }

    private:
        std::unordered_map <enum_type, std::unordered_map <std::string, std::function <void(parameter_list_type const&)>>> namedEvents;
        std::unordered_map <enum_type, std::vector <std::function <void(parameter_list_type const&)>>> unnamedEvents;
    };
}


//!
//! Output samples from the prior in Obsidian format
//!
//! \file prospecter.cpp
//! \author Lachlan McCalman
//! \author Darren Shen
//! \author Nahid Akbar
//! \date February, 2014
//! \license Affero General Public License version 3 or later
//! \copyright (c) 2014, NICTA
//!

// Standard Library
#include <random>
#include <thread>
#include <chrono>
#include <numeric>
// Prerequisites
#include <glog/logging.h>
#include <boost/program_options.hpp>
// Project
#include "app/console.hpp"
#include "app/settings.hpp"
#include "input/input.hpp"
#include "comms/delegator.hpp"
#include "comms/requester.hpp"
#include "serial/serial.hpp"
#include "app/asyncdelegator.hpp"
#include "app/signal.hpp"
#include "infer/mcmc.hpp"
#include "infer/metropolis.hpp"
#include "infer/adaptive.hpp"
#include "fwdmodel/global.hpp"
#include "datatype/sensors.hpp"
#include "io/npy.hpp"
#include "io/dumpnpz.hpp"
#include "likelihood/likelihood.hpp"
#include "detail.hpp"

using namespace obsidian;
using namespace stateline;

namespace ph = std::placeholders;

// Command line options specific to obsidian
po::options_description commandLineOptions()
{
  po::options_description cmdLine("Delegator Command Line Options");
  cmdLine.add_options() //
    ("configfile,c", po::value<std::string>()->default_value("obsidian_config"), "configuration file") //
    ("inputfile,i", po::value<std::string>()->default_value("input.obsidian"), "input file") //
    ("outputfile,o", po::value<std::string>()->default_value("output.npz"), "output file") //
    ("nsamples,n", po::value<uint>()->default_value(100), "The number of samples to output")//
    ("fwdmodel,f", po::bool_switch()->default_value(false), "Also compute forward models");//
  return cmdLine;
}

int main(int ac, char* av[])
{
  init::initialiseSignalHandler();
  // Get the settings from the command line
  auto vm = init::initProgramOptions(ac, av, commandLineOptions());

  // Set up a random generator here
  std::random_device rd;
  std::mt19937 gen(rd());

  readConfigFile(vm["configfile"].as<std::string>(), vm);
  readInputFile(vm["inputfile"].as<std::string>(), vm);
  std::set<ForwardModel> sensorsEnabled = parseSensorsEnabled(vm);
  GlobalPrior prior = parsePrior<GlobalPrior>(vm, sensorsEnabled);

  // Stuff needed for forward modelling
  GlobalSpec globalSpec = parseSpec<GlobalSpec>(vm, sensorsEnabled);
  std::vector<world::InterpolatorSpec> boundaryInterp = world::worldspec2Interp(globalSpec.world);
  GlobalResults realResults = loadResults(globalSpec.world, vm, sensorsEnabled);
  GlobalCache cache = fwd::generateGlobalCache(boundaryInterp, globalSpec, sensorsEnabled);

  // Concatenate all the chains and take samples from them
  uint nsamples = vm["nsamples"].as<uint>();

  std::vector<WorldParams> dumpParams;
  std::vector<double> dumpPrior;
  std::vector<Eigen::VectorXd> dumpTheta;

  // Forward modelling stuff
  bool computeForwardModels = vm["fwdmodel"].as<bool>();
  std::vector<GlobalResults> dumpResults;
  std::vector<std::vector<double>> dumpLikelihood;

  std::string filename = vm["outputfile"].as<std::string>();
  io::NpzWriter writer(filename);

  for (uint i = 0; i < nsamples; i++)
  {
    LOG(INFO) << "Generating sample " << i;
    if(global::interruptedBySignal)
    {
      break;
    }
    // get the state
    Eigen::VectorXd theta = prior.sample(gen);
    GlobalParams params = prior.reconstruct(theta);
    // prior
    double priorLikelihood = prior.evaluate(theta);
    dumpPrior.push_back(priorLikelihood);
    // add the state for writing
    dumpParams.push_back(params.world);
    dumpTheta.push_back(theta);
    if (computeForwardModels)
    {
      GlobalResults results = fwd::forwardModelAll(globalSpec, cache, params, sensorsEnabled);
      dumpResults.push_back(results);
      std::vector<double> likelihoods = lh::likelihoodAll(results, realResults, globalSpec, sensorsEnabled);
      dumpLikelihood.push_back(likelihoods);
    }
  }
  dumpParamsNPZ(writer, dumpParams);
  dumpPriorNPZ(writer, dumpPrior);
  dumpThetaNPZ(writer, dumpTheta);
  if (computeForwardModels)
  {
    dumpResultsNPZ(writer, dumpResults);
    dumpLikelihoodNPZ(writer, dumpLikelihood);
  }

  return 0;
}


//==================================================================================================
/*
  Copyright 2017 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)
*/
//==================================================================================================
#include <boost/simd/algorithm/replace.hpp>
#include <numeric>
#include <vector>
#include <simd_test.hpp>

using namespace boost::simd;
using namespace boost::alignment;

STF_CASE_TPL( "Check unary simd::replace", STF_NUMERIC_TYPES )
{
  static const int N = pack<T>::static_size;

  std::vector<T> values(2*N+3), ref(2*N+3);
  std::iota(values.begin(), values.end(),T(1));
  std::iota(ref.begin(), ref.end(),T(1));
  {
    std::replace(ref.begin(), ref.end(), T(1), T(0));
    boost::simd::replace(values.data(), values.data()+2*N+3, T(1), T(0));

    STF_EQUAL( values, ref );
  }
  {
    std::replace(ref.begin(), ref.end(), T(2*N+2), T(0));
    boost::simd::replace(values.data(), values.data()+2*N+3, T(2*N+2), T(0));

    STF_EQUAL( values, ref );
  }
  {
    std::replace(ref.begin(), ref.end(), T(2*N+2), T(0));
    boost::simd::replace(values.data(), values.data()+2*N+3, T(2*N+2), T(0));

    STF_EQUAL( values, ref );
  }
}


#include <boost/program_options.hpp>
#include <boost/progress.hpp>
#include <boost/format.hpp>
#include <boost/timer.hpp>
#include "luash.h"
#include "mplsh-fit-tune.h"

using namespace std;
using namespace lshkit;
namespace po = boost::program_options;

Environment *init(int dim, int N, float *data_block, unsigned K, float R, bool doTuning, const char *indexFile, int L, int T, int M, float W){
    Environment *env = new Environment;
    Index *index = new Index();

    env->data = new FloatMatrix(dim,N,data_block);

    FloatMatrix *data = env->data;

    env->metric = new metric::l2<float>(data->getDim());
    env->accessor = new FloatMatrix::Accessor(*data);
    env->R = R;
    env->K = K;
    env->dim = dim;

    unsigned H = 1017881;
    int default_M = 15;
    float default_W = 11;

    bool do_benchmark = true;
    bool use_index = false; // load the index from a file

    bool index_loaded = false;

    if(!doTuning){
        ifstream is(indexFile, ios_base::binary);
        if (is) {
            is.exceptions(ios_base::eofbit | ios_base::failbit | ios_base::badbit);
            cout << "LOADING INDEX..." << endl;
            index->load(is);
            BOOST_VERIFY(is);
            index_loaded = true;
        }
    }
    else {
        double cost = -1;

        M = -1;
        W = -1;

        try
        {
            cost = mplsh_fit_tune(*(data), *(env->metric), L, T, M, W, R, K);
            default_M = M;
            default_W = W;
        }
        catch(...)
        {
            std::cout << "Fit-Tune Crash !!" << std::endl;
            M = default_M;
            W = default_W;
        }

        cout << "TUNING DONE!" << endl;
        cout << "cost " << cost << endl;
        cout << "L " << L << endl;
        cout << "T " << T << endl;
        cout << "M " << M << endl;
        cout << "W " << W << endl;
    }

    if(!index_loaded){
        Index::Parameter param;
        param.W = W;
        param.range = H; // See H in the program parameters.  You can just use the default value.
        param.repeat = M;
        param.dim = data->getDim();
        DefaultRng rng;

        index->init(param, rng, L);

        cout << "BUILDING THE INDEX" << endl;
        for (unsigned i = 0; i < data->getSize(); ++i)
        {
            // Insert an item to the hash table.
            // Note that only the key is passed in here.
            // MPLSH will get the feature from the accessor.
            index->insert(i, (*data)[i]);
        }

        //Write the index
        cout << "SAVING INDEX..." << endl;
        {
            ofstream os(indexFile, ios_base::binary);
            os.exceptions(ios_base::eofbit | ios_base::failbit | ios_base::badbit);
            index->save(os);
        }
    }

    cout << "DONE" << endl;

    env->index = index;
    return env;
}

void query(Environment *env, float *queryData, int *response, float *dist){
    unsigned K = env->K;
    float R = env->R;

    TopkScanner<FloatMatrix::Accessor, metric::l2<float> > query(*(env->accessor), *(env->metric), K, numeric_limits<float>::max());

    query.reset(queryData);

    (env->index)->query_recall(queryData, R, query);

    for(int i = 0; i < K; i++){
        response[i] = query.topk()[i].key;
        dist[i] = query.topk()[i].dist;
    }
}