/* This file is part of MAUS: http://micewww.pp.rl.ac.uk:8080/projects/maus
 *
 * MAUS is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * MAUS is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with MAUS.  If not, see <http://www.gnu.org/licenses/>.
 *
 */

// C headers
#include <CLHEP/Matrix/Matrix.h>
#include <CLHEP/Units/PhysicalConstants.h>
#include <json/json.h>

// C++ headers
#include <fstream>
#include <vector>
#include <map>
#include <cmath>
#include <string>
#include <sstream>
#include <algorithm>

// External libs headers
#include "TROOT.h"
#include "TFile.h"
#include "TH1D.h"
#include "TMatrixD.h"

// MAUS headers
#include "src/common_cpp/Recon/SciFi/PatternRecognition.hh"
#include "src/common_cpp/Recon/SciFi/LeastSquaresFitter.hh"
#include "src/common_cpp/Recon/SciFi/RootFitter.hh"
#include "src/common_cpp/DataStructure/ThreeVector.hh"
#include "src/common_cpp/Utils/Globals.hh"
#include "src/common_cpp/Globals/GlobalsManager.hh"

namespace MAUS {

// Four predicate functions used by the stl sort algorithm to sort spacepoints in vectors
bool compare_spoints_ascending_z(const SciFiSpacePoint *sp1, const SciFiSpacePoint *sp2) {
  return (sp1->get_position().z() < sp2->get_position().z());
}

bool compare_spoints_descending_z(const SciFiSpacePoint *sp1, const SciFiSpacePoint *sp2) {
  return (sp1->get_position().z() > sp2->get_position().z());
}

bool compare_spoints_ascending_station(const SciFiSpacePoint *sp1, const SciFiSpacePoint *sp2) {
  return (sp1->get_station() < sp2->get_station());
}

bool compare_spoints_descending_station(const SciFiSpacePoint *sp1, const SciFiSpacePoint *sp2) {
  return (sp1->get_station() > sp2->get_station());
}

PatternRecognition::PatternRecognition(): _debug(false),
                                          _up_straight_pr_on(true),
                                          _down_straight_pr_on(true),
                                          _up_helical_pr_on(true),
                                          _down_helical_pr_on(true),
                                          _sp_search_on(false),
                                          _expected_handedness_t1(0),
                                          _expected_handedness_t2(0),
                                          _s_error_method(0),
                                          _longitudinal_fitter(0),
                                          _line_fitter(0),
                                          _circle_fitter(0),
                                          _verb(0),
                                          _n_trackers(2),
                                          _n_stations(5),
                                          _bz_t1(4.0),
                                          _bz_t2(4.0),
                                          _sd_1to4(0.3844),
                                          _sd_5(0.4298),
                                          _sd_phi_1to4(1.0),
                                          _sd_phi_5(1.0),
                                          _res_cut(7.0),
                                          _straight_chisq_cut(50.0),
                                          _R_res_cut(150.0),
                                          _circle_chisq_cut(5.0),
                                          _circle_minuit_cut(10.0),
                                          _n_turns_cut(1.0),
                                          _sz_chisq_cut(150.0),
                                          _long_minuit_cut(65.0),
                                          _circle_error_w(1.0),
                                          _sz_error_w(1.0),
                                          _Pt_max(180.0),
                                          _Pz_min(50.0),
                                          _missing_sp_cut(2.0),
                                          _rfile(NULL),
                                          _hx_line(NULL),
                                          _hy_line(NULL),
                                          _hxchisq_line(NULL),
                                          _hychisq_line(NULL),
                                          _hxychisq_line(NULL),
                                          _hszchisq_line(NULL),
                                          _fail_helix_tku(NULL),
                                          _fail_helix_tkd(NULL) {
  // Do nothing
}

void PatternRecognition::set_parameters_to_default() {
  _debug = false;
  _up_straight_pr_on = true;
  _down_straight_pr_on = true;
  _up_helical_pr_on = true;
  _down_helical_pr_on = true;
  _sp_search_on = false;
  _expected_handedness_t1 = 0;
  _expected_handedness_t2 = 0;
  _s_error_method = 0;
  _longitudinal_fitter = 0;
  _line_fitter = 0;
  _circle_fitter = 0;
  _verb = 0;
  _n_trackers = 2;
  _n_stations = 5;
  _bz_t1 = 4.0;
  _bz_t2 = 4.0;
  _sd_1to4 = 0.3844;
  _sd_5 = 0.4298;
  _sd_phi_1to4 = 1.0;
  _sd_phi_5 = 1.0;
  _sd_mcs = 2.0;
  _res_cut = 7.0;
  _straight_chisq_cut = 50.0;
  _R_res_cut = 150.0;
  _circle_chisq_cut = 5.0;
  _circle_minuit_cut = 10.0;
  _n_turns_cut = 1.0;
  _sz_chisq_cut = 150.0;
  _long_minuit_cut = 65.0;
  _circle_error_w = 1.0;
  _sz_error_w = 1.0;
  _Pt_max = 180.0;
  _Pz_min = 50.0;
  _missing_sp_cut = 2.0;
  _mincfg = RootFitter::MinuitSZParameters();
}

PatternRecognition::~PatternRecognition() {
  if (_debug) {
    std::cerr << "INFO: Pattern Recognition: writing debug info\n";
    if (_rfile) _rfile->cd();
    if (_hx_line) _hx_line->Write();
    if (_hy_line) _hy_line->Write();
    if (_hxchisq_line) _hxchisq_line->Write();
    if (_hychisq_line) _hychisq_line->Write();
    if (_hxychisq_line) _hxychisq_line->Write();
    if (_hszchisq_line) _hszchisq_line->Write();
    if (_fail_helix_tku) _fail_helix_tku->Write();
    if (_fail_helix_tkd) _fail_helix_tkd->Write();
  }
}

void PatternRecognition::setup_debug(std::string debug_fname) {
  _debug = true;
  _rfile = new TFile(debug_fname.c_str(), "RECREATE");
  _hx_line = new TH1D("hx_line", "Pattern Recognition Road (mm) X Residuals All Candidates", \
                      200, -150, 150);
  _hy_line = new TH1D("hy_line", "Pattern Recognition Road (mm) Y Residuals All Candidates",
                      500, -150, 150);
  _hxchisq_line = new TH1D("hxchisq_line", \
      "Pattern Recognition Straight Fit x ChiSq/DoF All Candidates", 200, 0, 500);
  _hychisq_line = new TH1D("hychisq_line", \
      "Pattern Recognition Straight Fit y ChiSq/DoF All Candidates", 200, 0, 500);
  _hxychisq_line = new TH1D("hxychisq_line", \
      "Pattern Recognition Circle Fit ChiSq/DoF All Candidates", 200, 0, 10);
  _hszchisq_line = new TH1D("hszchisq_line", \
      "Pattern Recognition s-z Fit ChiSq/DoF All Candidates", 200, 0, 400);
  _fail_helix_tku = new TH1D("fail_helix_tku", \
                             "Pattern Recognition Helix Fit Failure Point TkU", 6, 0, 6);
  _fail_helix_tkd = new TH1D("fail_helix_tkd", \
                             "Pattern Recognition Helix Fit Failure Point TkD", 6, 0, 6);

  _hx_line->Write();
  _hy_line->Write();
  _hxchisq_line->Write();
  _hychisq_line->Write();
  _hxychisq_line->Write();
  _hszchisq_line->Write();
  _fail_helix_tku->Write();
  _fail_helix_tkd->Write();
  std::cerr << "INFO: Pattern Recognition debug mode setup complete\n";
}

bool PatternRecognition::LoadGlobals() {
  if (Globals::HasInstance()) {
    Json::Value *json = Globals::GetConfigurationCards();
    _sp_search_on = (*json)["SciFiPatRecMissingSpSearchOn"].asBool();
    _s_error_method = (*json)["SciFiPatRecSErrorMethod"].asInt();
    _longitudinal_fitter = (*json)["SciFiPatRecLongitudinalFitter"].asInt();
    _line_fitter = (*json)["SciFiPatRecLineFitter"].asInt();
    _circle_fitter = (*json)["SciFiPatRecCircleFitter"].asInt();
    _verb = (*json)["SciFiPatRecVerbosity"].asInt();
    _n_trackers = (*json)["SciFinTrackers"].asInt();
    _n_stations = (*json)["SciFinStations"].asInt();
    _sd_1to4 = (*json)["SciFi_sigma_triplet"].asDouble();
    _sd_5 = (*json)["SciFi_sigma_tracker0_station5"].asDouble();
    _sd_phi_1to4 = (*json)["SciFi_sigma_phi_1to4"].asDouble();
    _sd_phi_5 = (*json)["SciFi_sigma_phi_5"].asDouble();
    _sd_mcs = (*json)["SciFiMCSStationError"].asDouble();
    _res_cut = (*json)["SciFiStraightRoadCut"].asDouble();
    _straight_chisq_cut = (*json)["SciFiStraightChi2Cut"].asDouble();
    _R_res_cut = (*json)["SciFiRadiusResCut"].asDouble();
    _circle_chisq_cut = (*json)["SciFiPatRecCircleChi2Cut"].asDouble();
    _circle_minuit_cut = (*json)["SciFiPatRecCircleMinuitChi2Cut"].asDouble();
    _n_turns_cut = (*json)["SciFiNTurnsCut"].asDouble();
    _sz_chisq_cut = (*json)["SciFiPatRecSZChi2Cut"].asDouble();
    _long_minuit_cut = (*json)["SciFiPatRecLongMinuitChi2Cut"].asDouble();
    _circle_error_w = (*json)["SciFiPatRecCircleErrorWeight"].asDouble();
    _sz_error_w = (*json)["SciFiPatRecSZErrorWeight"].asDouble();
    _Pt_max = (*json)["SciFiMaxPt"].asDouble();
    _Pz_min = (*json)["SciFiMinPz"].asDouble();
    _missing_sp_cut = (*json)["SciFiPatRecMissingSpCut"].asDouble();
    _mincfg.maxFunctionCalls = (*json)["SciFiMinuitMaxFunctionCalls"].asInt();
    _mincfg.maxIterations = (*json)["SciFiMinuitMaxIterations"].asInt();
    _mincfg.searchNStepsNormal = (*json)["SciFiMinuitSearchNStepsNormal"].asInt();
    _mincfg.searchNStepsLowRadius = (*json)["SciFiMinuitSearchNStepsLowRadius"].asInt();
    _mincfg.lowRadius = (*json)["SciFiPatRecLowRadius"].asDouble();
    _mincfg.maxSZChisq = (*json)["SciFiMinuitMaxSZChisq"].asDouble();
    _mincfg.patRecSZCut = (*json)["SciFiPatRecLongMinuitChi2Cut"].asDouble();
    _mincfg.stepSize = (*json)["SciFiMinuitStepSize"].asDouble();
    _mincfg.tolerance = (*json)["SciFiMinuitTolerance"].asDouble();
    _mincfg.dsdzLowerLimit = (*json)["SciFiMinuitDsDzLowerLimit"].asDouble();
    _mincfg.dsdzUpperLimit = (*json)["SciFiMinuitDsDzUpperLimit"].asDouble();
    return true;
  } else {
    return false;
  }
}

void PatternRecognition::process(SciFiEvent &evt) const {

  if ( evt.spacepoints().size() > 0 ) {
    if ( _verb > 0 )
      std::cerr << "Number of spoints in event: " << evt.spacepoints().size() << std::endl;

    // Some setup
    evt.set_spacepoints_used_flag(false);
    SpacePoint2dPArray spnts_by_tracker(_n_trackers);
    spnts_by_tracker = SciFiTools::sort_by_tracker(evt.spacepoints());
    if (_verb > 0) {
      std::cerr << "Number of spoints in event in T1: " << spnts_by_tracker[0].size() << std::endl;
      SciFiTools::print_spacepoint_xyz(spnts_by_tracker[0]);
      std::cerr << "Number of spoints in event in T2: " << spnts_by_tracker[1].size() << std::endl;
      SciFiTools::print_spacepoint_xyz(spnts_by_tracker[1]);
    }

    int trker_no = 0;
    // Split spacepoints according to which station they occured in
    SpacePoint2dPArray spnts_by_station_tku(_n_stations);
    SciFiTools::sort_by_station(spnts_by_tracker[trker_no], spnts_by_station_tku);

    // Make the helical and straight tracks, depending on flags
    if ( _up_helical_pr_on ) {
      bool track_type = 1;
      make_all_tracks(track_type, trker_no, spnts_by_station_tku, evt);
    }
    if ( _up_straight_pr_on ) {
      bool track_type = 0;
      make_all_tracks(track_type, trker_no, spnts_by_station_tku, evt);
    }

    trker_no = 1;
    // Split spacepoints according to which station they occured in
    SpacePoint2dPArray spnts_by_station_tkd(_n_stations);
    SciFiTools::sort_by_station(spnts_by_tracker[trker_no], spnts_by_station_tkd);

    // Make the helical and straight tracks, depending on flags
    if ( _down_helical_pr_on ) {
      bool track_type = 1;
      make_all_tracks(track_type, trker_no, spnts_by_station_tkd, evt);
    }
    if ( _down_straight_pr_on ) {
      bool track_type = 0;
      make_all_tracks(track_type, trker_no, spnts_by_station_tkd, evt);
    }
  } else {
    if (_verb > 0) std::cerr << "No spacepoints in event" << std::endl;
  }
};

void PatternRecognition::make_all_tracks(const bool track_type, const int trker_no,
                                     SpacePoint2dPArray &spnts_by_station, SciFiEvent &evt) const {

  // Count how many stations have at least one *unused* spacepoint
  int num_stations_hit = SciFiTools::num_stations_with_unused_spnts(spnts_by_station);

  if (_verb > 0) std::cerr << "Number Unused Spacepoints = " << num_stations_hit << std::endl;

  std::vector<SciFiSpacePoint*> spnts = evt.spacepoints();

  // Make the tracks
  if (num_stations_hit == 5) {
    std::vector<SciFiStraightPRTrack*> strks;
    std::vector<SciFiHelicalPRTrack*> htrks;
    make_5tracks(track_type, trker_no, spnts_by_station, strks, htrks);
    std::vector<SciFiStraightPRTrack*> accepted_strks = select_tracks(strks);
    std::vector<SciFiHelicalPRTrack*> accepted_htrks = select_tracks(htrks);
    track_processing(trker_no, 5, spnts, accepted_strks, accepted_htrks);
    add_tracks(accepted_strks, accepted_htrks, evt);
  }
  num_stations_hit = SciFiTools::num_stations_with_unused_spnts(spnts_by_station);
  if (num_stations_hit > 3) {
    std::vector<SciFiStraightPRTrack*> strks;
    std::vector<SciFiHelicalPRTrack*> htrks;
    make_4tracks(track_type, trker_no, spnts_by_station, strks, htrks);
    std::vector<SciFiStraightPRTrack*> accepted_strks = select_tracks(strks);
    std::vector<SciFiHelicalPRTrack*> accepted_htrks = select_tracks(htrks);
    track_processing(trker_no, 4, spnts, accepted_strks, accepted_htrks);
    add_tracks(accepted_strks, accepted_htrks, evt);
  }
  num_stations_hit = SciFiTools::num_stations_with_unused_spnts(spnts_by_station);
  if (num_stations_hit > 2  && track_type != 1) { // No 3pt tracks for helical
    std::vector<SciFiStraightPRTrack*> strks;
    std::vector<SciFiHelicalPRTrack*> htrks;
    make_3tracks(trker_no, spnts_by_station, strks);
    std::vector<SciFiStraightPRTrack*> accepted_strks = select_tracks(strks);
    std::vector<SciFiHelicalPRTrack*> accepted_htrks = select_tracks(htrks);
    track_processing(trker_no, 3, spnts, accepted_strks, accepted_htrks);
    add_tracks(accepted_strks, accepted_htrks, evt);
  }
}

void PatternRecognition::add_tracks(std::vector<SciFiStraightPRTrack*> &strks,
      std::vector<SciFiHelicalPRTrack*> &htrks, SciFiEvent &evt) const {
  for (auto trk : strks) {
    evt.add_straightprtrack(trk);
  }
  for (auto trk : htrks) {
    evt.add_helicalprtrack(trk);
  }
}

void PatternRecognition::track_processing(const int trker_no, const int n_points,
                                          std::vector<SciFiSpacePoint*> &spnts,
                                          std::vector<SciFiStraightPRTrack*> &strks,
                                          std::vector<SciFiHelicalPRTrack*> &htrks) const {
  // Set the tracker number
  set_tracker_number(trker_no, strks, htrks);

  for (SciFiHelicalPRTrack* trk : htrks) {

    // Look for any spacepoint seed candidates that the helical fit may have missed
    if (_sp_search_on && (n_points == 4)) {
      missing_sp_search_helical(spnts, trk);
    }

    // Calculate the seed spacepoint x-y pulls for helical tracks
    SciFiTools::calc_xy_pulls(trk);
  }
}

void PatternRecognition::set_tracker_number(const int trker_no,
                                            std::vector<SciFiStraightPRTrack*> &strks,
                                            std::vector<SciFiHelicalPRTrack*> &htrks) const {
  for ( int i = 0; i < static_cast<int>(strks.size()); ++i ) {
    strks[i]->set_tracker(trker_no);
  }
  for ( int i = 0; i < static_cast<int>(htrks.size()); ++i ) {
    htrks[i]->set_tracker(trker_no);
  }
}

void PatternRecognition::make_5tracks(const bool track_type, const int trker_no,
                                      SpacePoint2dPArray &spnts_by_station,
                                      std::vector<SciFiStraightPRTrack*> &strks,
                                      std::vector<SciFiHelicalPRTrack*> &htrks) const {
  if ( _verb > 0 ) std::cout << "INFO: PatternRecognition: Making 5 point tracks\n";
  int n_points = 5;
  std::vector<int> ignore_stations; // A zero size vector sets that all stations are to be used
  if ( track_type == 0 )
    make_straight_tracks(n_points, trker_no, ignore_stations, spnts_by_station, strks);
  if ( track_type == 1 ) {
    std::vector<SciFiSpacePoint*> current_spnts;
    make_helix(n_points, 0, ignore_stations, current_spnts, spnts_by_station, htrks);
  }
  if ( _verb > 0 ) std::cerr << "INFO: PatternRecognition: Finished making 5 pt tracks\n";
} // ~make_spr_5pt(...)

void PatternRecognition::make_4tracks(const bool track_type, const int trker_no,
                                      SpacePoint2dPArray &spnts_by_station,
                                      std::vector<SciFiStraightPRTrack*> &strks,
                                      std::vector<SciFiHelicalPRTrack*> &htrks) const {
  int n_points = 4;

  // Count how many stations have at least one *unused* spacepoint
  int num_stations_hit = SciFiTools::num_stations_with_unused_spnts(spnts_by_station);

  // Call make_tracks with parameters depending on how many stations have unused spacepoints
  if ( num_stations_hit == 5 ) {

    if ( _verb > 0 ) std::cerr <<
      "INFO: PatternRecognition: Making 4 point track: 5 stations with unused spacepoints\n";

    for (int i = 0; i < 5; ++i) { // Loop of stations, ignoring each one in turn
      // Recount how many stations have at least one unused spacepoint
      num_stations_hit = SciFiTools::num_stations_with_unused_spnts(spnts_by_station);
      // If there are enough occupied stations left to make a 4 point track, keep making tracks
      if ( num_stations_hit  >= n_points ) {
        std::vector<int> ignore_stations(1, i);
        if ( track_type == 0 )
          make_straight_tracks(n_points, trker_no, ignore_stations, spnts_by_station, strks);
        if ( track_type == 1 ) {
          std::vector<SciFiSpacePoint*> current_spnts;
          make_helix(n_points, 0, ignore_stations, current_spnts, spnts_by_station, htrks);
        }
      } else {
        break;
      }
    } // ~Loop of stations, ignoring each one in turn
  } else if ( num_stations_hit == 4 ) {

    if ( _verb > 0 ) std::cout <<
      "INFO: PatternRecognition: Making 4 point track: 4 stations with unused spacepoints\n";

    // Find out which station has no unused hits (1st entry in stations_not_hit vector)
    std::vector<int> stations_hit, stations_not_hit;
    SciFiTools::stations_with_unused_spnts(spnts_by_station, stations_hit, stations_not_hit);

    // Make the tracks
    if ( static_cast<int>(stations_not_hit.size()) == 1 ) {
      if ( track_type == 0 )
        make_straight_tracks(n_points, trker_no, stations_not_hit, spnts_by_station, strks);
      if ( track_type == 1 ) {
        std::vector<SciFiSpacePoint*> current_spnts;
        make_helix(n_points, 0, stations_not_hit, current_spnts, spnts_by_station, htrks);
      }
    } else {
      if ( _verb > 0 ) {
        std::cerr << "WARNING: PatternRecognition: Wrong number of stations without spacepoints, ";
        std::cerr << "aborting 4 pt track." << std::endl;
      }
    }
  } else if ( num_stations_hit < 4 ) {
    if ( _verb > 0 ) std::cout <<
      "INFO: PatternRecognition: Not enough unused spacepoints, quiting 4 point track\n";
  } else if ( num_stations_hit > 6 ) {
    if ( _verb > 0 ) {
      std::cerr << "WARNING: PatternRecognition: Wrong number of stations without spacepoints, ";
      std::cerr << "aborting 4 pt track." << std::endl;
    }
  }
  if ( _verb > 0 ) std::cout << "INFO: PatternRecognition:Finished making 4 point tracks\n";
} // ~make_straight_4tracks(...)

void PatternRecognition::make_3tracks(const int trker_no, SpacePoint2dPArray &spnts_by_station,
                                      std::vector<SciFiStraightPRTrack*> &strks) const {
  int n_points = 3;

  // Count how many stations have at least one *unused* spacepoint
  int num_stations_hit = SciFiTools::num_stations_with_unused_spnts(spnts_by_station);

  bool sufficient_hit_stations = true;

  // Call make_tracks with parameters depending on how many stations have unused spacepoints
  if ( num_stations_hit == 5 ) {

    if ( _verb > 0 ) std::cout <<
      "INFO: PatternRecognition: Making 3 point track: 5 stations with unused spacepoints\n";

    for (int i = 0; i < 4; ++i) { // Loop of first station to ignore
      if ( sufficient_hit_stations ) {
        for (int j = i + 1; j < 5; ++j) { // Loop of second station to ignore
          if ( sufficient_hit_stations ) {
            // Recount how many stations have at least one unused spacepoint
            num_stations_hit = SciFiTools::num_stations_with_unused_spnts(spnts_by_station);
            // If there are enough occupied stations left to make a 3pt track, keep making tracks
            if ( num_stations_hit  >= n_points ) {
              std::vector<int> ignore_stations;
              ignore_stations.push_back(i);
              ignore_stations.push_back(j);
              make_straight_tracks(n_points, trker_no, ignore_stations, spnts_by_station, strks);
            } else {
                sufficient_hit_stations = false;
            }
          } // ~if ( sufficient_hit_stations )
        } // ~Loop of second station to ignore
      } // ~if ( sufficient_hit_stations )
    } // ~Loop of first station to ignore
  } else if ( num_stations_hit == 4 ) {

    if ( _verb > 0 ) std::cout <<
      "INFO: PatternRecognition: Making 3 point track: 4 stations with unused spacepoints\n";

    // Find out which station has no unused hits (1st entry in stations_not_hit vector)
    std::vector<int> stations_hit, stations_not_hit;
    SciFiTools::stations_with_unused_spnts(spnts_by_station, stations_hit, stations_not_hit);
    std::vector<int> ignore_stations;

    // Make the tracks
    if ( static_cast<int>(stations_not_hit.size()) == 1 ) {
      for (int i = 0; i < 5; ++i) { // Loop of stations, ignoring each one in turn
        // Recount how many stations have at least one unused spacepoint
        num_stations_hit = SciFiTools::num_stations_with_unused_spnts(spnts_by_station);
        // If there are enough occupied stations left to make a 4 point track, keep making tracks
        if ( num_stations_hit  >= n_points ) {
          ignore_stations.clear();
          if ( stations_not_hit[0] != i ) { // Don't send the same 2 ignore stations
            ignore_stations.push_back(stations_not_hit[0]);
            ignore_stations.push_back(i);
            make_straight_tracks(n_points, trker_no, ignore_stations, spnts_by_station, strks);
          }
        } else {
          break;
        }
      }
    }
  } else if ( num_stations_hit == 3 ) {

    if ( _verb > 0 ) std::cout <<
      "INFO: PatternRecognition: Making 3 point track: 3 stations with unused spacepoints\n";

    // Find out which station has no unused hits (1st entry in stations_not_hit vector)
    std::vector<int> stations_hit, stations_not_hit;
    SciFiTools::stations_with_unused_spnts(spnts_by_station, stations_hit, stations_not_hit);

    // Make the tracks
    if ( static_cast<int>(stations_not_hit.size()) == 2 ) {
      make_straight_tracks(n_points, trker_no, stations_not_hit, spnts_by_station, strks);
    } else {
      if ( _verb > 0 ) {
        std::cerr << "WARNING: PatternRecognition: Wrong number of stations without spacepoints, ";
        std::cerr << "aborting 3 pt track." << std::endl;
      }
    }
  } else if ( num_stations_hit < 3 ) {
    if ( _verb > 0 )
      std::cout << "INFO: PatternRecognition: Not enough unused spacepoints, quiting 3 pt track\n";
  } else if ( num_stations_hit > 6 ) {
      if ( _verb > 0 ) {
        std::cerr << "WARNING: PatternRecognition: Wrong number of stations without spacepoints, ";
        std::cerr << "aborting 3 pt track." << std::endl;
      }
  }
  if ( _verb > 0 ) std::cout << "INFO: PatternRecognition: Finished making 3 pt tracks\n";
} // ~make_straight_3tracks(...)

void PatternRecognition::make_straight_tracks(const int n_points, const int trker_no,
                                     const std::vector<int> ignore_stations,
                                     SpacePoint2dPArray &spnts_by_station,
                                     std::vector<SciFiStraightPRTrack*> &strks) const {

  // Set up a secondary ROOT file to hold non-standard debug data
  if (_rfile && _debug) _rfile->cd();

  // Set inner and outer stations
  int o_st_num = -1, i_st_num = -1;
  set_end_stations(ignore_stations, o_st_num, i_st_num);

  if (static_cast<int>(spnts_by_station.size()) != _n_stations
      || o_st_num < 0 || o_st_num >= _n_stations || i_st_num < 0 || i_st_num >= _n_stations) {
    if ( _verb > 0 ) {
      std::cerr << "WARNING: PatternRecognition:Bad spnts_by_station passed, ";
      std::cerr << "aborting make_straight_tracks.\n";
    }
    return;
  }

  // Loop over spacepoints in outer station
  for ( size_t outer_sp = 0; outer_sp < spnts_by_station[o_st_num].size(); ++outer_sp ) {

    // Check the outer spacepoint is unused and enough stations are left with unused sp
    if ( spnts_by_station[o_st_num][outer_sp]->get_used() ||
          SciFiTools::num_stations_with_unused_spnts(spnts_by_station) < n_points) continue;

    // Loop over spacepoints in inner station
    for ( size_t inner_sp = 0; inner_sp < spnts_by_station[i_st_num].size(); ++inner_sp ) {

      // Check the inner spacepoint is unused and enough stations are left with unused sp
      if ( spnts_by_station[i_st_num][inner_sp]->get_used() ||
           SciFiTools::num_stations_with_unused_spnts(spnts_by_station) < n_points ) continue;

      // Vector to hold the good spacepoints in each station
      std::vector<SciFiSpacePoint*> good_spnts;

      // Set variables to hold which stations are to be ignored
      int ignore_st_1 = -1, ignore_st_2 = -1;
      set_ignore_stations(ignore_stations, ignore_st_1, ignore_st_2);

      // Draw a straight line between spacepoints in outer most and inner most stations
      SimpleLine line_x, line_y;
      SciFiTools::draw_line(spnts_by_station[o_st_num][outer_sp],
                            spnts_by_station[i_st_num][inner_sp], line_x, line_y);

      // Loop over intermediate stations and compare spacepoints with the line
      for ( int st_num = i_st_num + 1; st_num < o_st_num; ++st_num ) {
        if (st_num != ignore_st_1 && st_num != ignore_st_2) {

          // A large number so initial value is set as best first
          double delta_sq = 1000000;
          int best_sp = -1;

          // Loop over spacepoints
          for ( size_t sp_no = 0; sp_no < spnts_by_station[st_num].size(); ++sp_no ) {
            SciFiSpacePoint* sp = spnts_by_station[st_num][sp_no];
            // If the spacepoint has not already been used in a track fit
            if ( !sp->get_used() ) {
              // Apply roadcuts & find spoints with smallest residuals to the line
              double cx = line_x.get_c();
              double mx = line_x.get_m();
              double cy = line_y.get_c();
              double my = line_y.get_m();
              ThreeVector pos = sp->get_position();
              // Vertical residuals
              double dx = (mx * pos.z() + cx) - pos.x();
              double dy = (my * pos.z() + cy) - pos.y();
              // Perpendicular residuals
              // double dx = fabs(pos.x() - (cx + mx*pos.z())) / sqrt(1.0 + mx*mx);
              // double dy = fabs(pos.y() - (cy + my*pos.z())) / sqrt(1.0 + my*my);
              if (_hx_line && _debug) {
                _hx_line->Fill(dx);
                std::cerr << "Filling debug histogram hx\n";
              }
              if (_hy_line && _debug) _hy_line->Fill(dy);
              if ( fabs(dx) < _res_cut && fabs(dy) < _res_cut )  {
                if ( delta_sq > (dx*dx + dy*dy) ) {
                  delta_sq = dx*dx + dy*dy;
                  best_sp = sp_no;
                }
              } // ~If pass roadcuts and beats previous best fit point
            } // ~If spacepoint is unused
          } // ~Loop over spacepoints
          // Push back the best spacepoint found for the current station
          if (best_sp > -1) {
            SciFiSpacePoint * sp = spnts_by_station[st_num][best_sp];
            good_spnts.push_back(sp);
          }// ~if (counter > 0)
        } // ~if (st_num != ignore_station)
      } // ~Loop over intermediate stations

      // Clear the line objects so we can reuse them
      line_x.clear();
      line_y.clear();

      // Check we have at least 1 good spacepoint in each of the intermediate stations
      if ( static_cast<int>(good_spnts.size()) > (n_points - 3) ) {
        good_spnts.insert(good_spnts.begin(), spnts_by_station[i_st_num][inner_sp]);
        good_spnts.push_back(spnts_by_station[o_st_num][outer_sp]);

        // Fit the track, and if it succeeds add to the tracks vector
        SciFiStraightPRTrack* track = fit_straight_track(n_points, good_spnts);
        if (track) strks.push_back(track);
      } else {
        // std::cerr << "DEBUG: PatternRecognition: No good trial track found\n";
      }// ~ if ( good_spnts.size() > 1 )
    } // ~Loop over sp in station 1
  } // ~Loop over sp in station 5
} // ~make_straight_tracks(...)



SciFiStraightPRTrack* PatternRecognition::fit_straight_track(const int n_points,
    std::vector<SciFiSpacePoint*> spnts) const {
  SciFiStraightPRTrack* track = NULL;

  std::vector<double> x, x_err, y, y_err, z;
  for ( int i = 0; i < static_cast<int>(spnts.size()); ++i ) {
    x.push_back(spnts[i]->get_position().x());
    y.push_back(spnts[i]->get_position().y());
    z.push_back(spnts[i]->get_position().z());

    // The error on the position measurements of sp in a tracker (same in x and y)
    double sd = -1.0;
    if ( spnts[i]->get_station() == 5 )
      sd = _sd_5;
    else
      sd = _sd_1to4;
    x_err.push_back(sd);
    y_err.push_back(sd);
  }

  // Fit track
  SimpleLine line_x, line_y;
  TMatrixD cov_x(2, 2), cov_y(2, 2);
  if (_line_fitter == 0) { // Custom LSQ fitter
    LeastSquaresFitter::linear_fit(z, x, x_err, line_x, cov_x);
    LeastSquaresFitter::linear_fit(z, y, y_err, line_y, cov_y);
  } else if (_line_fitter == 1) { // ROOT based linear fitter
    RootFitter::FitLineLinear(z, x, x_err, line_x, cov_x);
    RootFitter::FitLineLinear(z, y, y_err, line_y, cov_y);
  }

  // Squash the covariances of each fit into one vector
  std::vector<double> covariance;
  for ( int i = 0; i < 2; ++i ) {
    for ( int j = 0; j < 2; ++j ) {
      covariance.push_back(cov_x[i][j]);
    }
  }
  for ( int i = 0; i < 2; ++i ) {
    for ( int j = 0; j < 2; ++j ) {
      covariance.push_back(cov_y[i][j]);
    }
  }

  // Check track passes chisq test, then create SciFiStraightPRTrack
  if (_hxchisq_line && _debug) _hxchisq_line->Fill(( line_x.get_chisq() / ( n_points - 2 )));
  if (_hychisq_line && _debug) _hychisq_line->Fill(( line_y.get_chisq() / ( n_points - 2 )));
  if ( ( line_x.get_chisq() / ( n_points - 2 ) < _straight_chisq_cut ) &&
      ( line_y.get_chisq() / ( n_points - 2 ) < _straight_chisq_cut ) ) {

    if ( _verb > 0 ) {
      std::cout << "INFO: Pattern Recognition: chisq test passed, ";
      std::cout << "adding " << n_points << "pt candidate track\n";
    }
    track = new SciFiStraightPRTrack(-1, line_x, line_y, covariance);
    track->set_n_fit_points(n_points);

    // Set all the good sp to used
    // for ( int i = 0; i < static_cast<int>(spnts.size()); ++i ) {
    //   spnts[i]->set_used(true);
    // }

    // Populate the sp of the track and then push the track back into the strks vector
    track->set_spacepoints_pointers(spnts);
  } else {
    if ( _verb > 0 ) {
      std::cout << "INFO: Pattern Recognition: x_chisq = " << line_x.get_chisq();
      std::cout << "\ty_chisq = " << line_y.get_chisq() << std::endl;
      std::cout << "chisq test failed, " << n_points << "pt track rejected\n";
    }
  } // ~Check track passes chisq test
  return track;
}

void PatternRecognition::make_helix(const int n_points, const int stat_num,
                                    const std::vector<int> ignore_stations,
                                    std::vector<SciFiSpacePoint*> &current_spnts,
                                    SpacePoint2dPArray &spnts_by_station,
                                    std::vector<SciFiHelicalPRTrack*> &htrks) const {

  // Set variables to hold which stations are to be ignored
  int ignore_st_1 = -1, ignore_st_2 = -1;
  set_ignore_stations(ignore_stations, ignore_st_1, ignore_st_2);

  // If we are on an ignore station, move to the next station without doing anything else
  // (this is the first possible recursive bit)
  if ( stat_num == ignore_st_1 || stat_num == ignore_st_2 ) {
      make_helix(n_points, stat_num+1, ignore_stations, current_spnts, spnts_by_station, htrks);
      return;
  }

  // Set the maximum station number to 4, unless that is an ignore station
  int stat_num_max = 4;
  if ( ignore_st_1 == 4 || ignore_st_2 == 4 ) stat_num_max = 3;

  // Loop over spacepoints in current station
  for ( size_t sp_num = 0; sp_num < spnts_by_station[stat_num].size(); ++sp_num ) {
    // If the current sp is used, skip it
    if ( spnts_by_station[stat_num][sp_num]->get_used() ) {
      continue;
    }
    current_spnts.push_back(spnts_by_station[stat_num][sp_num]);

    // If we are on the last station, attempt to form a track using the spacepoints collected
    if (stat_num == stat_num_max) {
      SciFiHelicalPRTrack* trk = form_track(n_points, current_spnts);

      // If we found a track, add it to the list of candidates
      if ( trk != NULL ) {
        if (_verb > 0)
          std::cout << "INFO: Pattern Recognition: Found track candidate, adding" << std::endl;
        htrks.push_back(trk);
      }
      // Remove the last spacepoint tried to make room for another from the same station
      current_spnts.pop_back();

    // If not on the final station, move on to next (the last recursive call)
    } else {
      make_helix(n_points, stat_num+1, ignore_stations, current_spnts, spnts_by_station, htrks);

      // If we have current spnts, remove the last tried, before trying another from this station
      if ( current_spnts.size() != 0 )
        current_spnts.pop_back();
    }
  }
  return;
}

SciFiHelicalPRTrack* PatternRecognition::form_track(const int n_points,
                                                    std::vector<SciFiSpacePoint*> spnts ) const {
  // Some initial set up
  SciFiHelicalPRTrack* track = NULL;
  bool good_radius = false;
  int handedness = 0; // The particle track handedness
  std::vector<double> x;
  std::vector<double> y;
  std::vector<double> z;
  for (auto sp : spnts) {
      x.push_back(sp->get_position().x());
      y.push_back(sp->get_position().y());
      z.push_back(sp->get_position().z());
  }


  // Fit the transverse projection (a circle)
  // ----------------------------------------
  SimpleCircle c_trial;
  TMatrixD cov_circle(3, 3); // The covariance matrix for the circle parameters alpha, beta, gamma
  double s1to4_error = _sd_1to4 * _circle_error_w;
  double s5_error = _sd_5 * _circle_error_w;

  if (_circle_fitter == 0) {
    // With a custom linear least squares fit
    good_radius = LeastSquaresFitter::circle_fit(s1to4_error, s5_error, _R_res_cut,
                                                 spnts, c_trial, cov_circle);
    if ((c_trial.get_chisq() / calc_circle_ndf(n_points)) > _circle_chisq_cut)
      good_radius = false;

  } else if (_circle_fitter == 1) {
    // Set up a position error vector (constant error, estimated to account for MCS)
    std::vector<double> err(n_points, _sd_mcs);

    // With a ROOT MINUIT based fitter
    good_radius = RootFitter::FitCircleMinuit(x, y, err, c_trial, cov_circle);
    if ( (c_trial.get_R() > _R_res_cut) || \
         (c_trial.get_chisq() / calc_circle_ndf(n_points)) > _circle_minuit_cut) {
      good_radius = false;
    }
  }

  // If the radius calculated is too large or chisq fails, return NULL
  if (!good_radius) {
    if (n_points == 5 && _debug) {
      if (spnts[0]->get_tracker() == 0) {
        _fail_helix_tku->Fill(1);
      } else if (spnts[0]->get_tracker() == 1) {
        _fail_helix_tkd->Fill(1);
      }
    }
    if ( _verb > 0 ) std::cout << "INFO: Pattern Recognition: Failed circle cut, chisq = "
                               << c_trial.get_chisq() << "\n";
    return NULL;
  }

  // Fit longitudinal parameters
  // ---------------------------
  if (_longitudinal_fitter == 0) { // Use the nturns algorithm and s-z linear fit
    // Calculate the standard deviations on the values of s found
    std::vector<double> sigma_s;
    for (auto sp : spnts) {
      double sig = -1.0;
      if (_s_error_method == 0) {  // 1st method: just use station resolution
        if ( (sp->get_station() == 5) )
          sig = _sd_phi_5;
        else
          sig = _sd_phi_1to4;
      } else if (_s_error_method == 1) {
        sig = sigma_on_s(c_trial, cov_circle, sp); // 2nd method: error propagation
      }
      sigma_s.push_back(sig * _sz_error_w);
    }

    // Perform the s - z fit
    SimpleLine line_sz;
    std::vector<double> phi_i;  // The change between turning angles wrt first spacepoint
    TMatrixD cov_sz(2, 2);      // The covariance matrix of the sz fit paramters c_sz, dsdz
    bool good_dsdz = find_dsdz(n_points, spnts, c_trial, sigma_s, phi_i, line_sz, cov_sz, \
                               handedness);
    if (!good_dsdz) {
      if ( _verb > 0 ) std::cout << "INFO: Pattern Recognition: dsdz fit failed, looping...\n";
      return NULL;
    }

    // Squash the covariances of each fit into one vector
    std::vector<double> covariance;
    for ( int i = 0; i < 3; ++i ) {
      for ( int j = 0; j < 3; ++j ) {
        covariance.push_back(cov_circle[i][j]);
      }
    }
    for ( int i = 0; i < 2; ++i ) {
      for ( int j = 0; j < 2; ++j ) {
        covariance.push_back(cov_sz[i][j]);
      }
    }

    // Make the track
    track = new SciFiHelicalPRTrack(-1, 0.0, c_trial, line_sz, spnts, covariance);
    track->set_circle_ndf(calc_circle_ndf(n_points));
    track->set_line_sz_ndf(n_points - 2);
  } else if (_longitudinal_fitter == 1) { // Fit the longitudinal parameters via ROOT (MINUIT2)
    SimpleHelix helix;
    // Hand the spacepoint positions to the fitter order by ascending z
    std::vector<SciFiSpacePoint*> ordered_spnts = spnts;
    std::sort(ordered_spnts.begin(), ordered_spnts.end(), \
        [](SciFiSpacePoint* a, SciFiSpacePoint* b) {
          return a->get_position().z() < b->get_position().z();
        });
    std::vector<double> ox;
    std::vector<double> oy;
    std::vector<double> oz;
    for (auto sp : ordered_spnts) {
      ox.push_back(sp->get_position().x());
      oy.push_back(sp->get_position().y());
      oz.push_back(sp->get_position().z());
    }

    // Help the fitter with the handedness if the radius is small
    int expected_handedness = 0;
    if (c_trial.get_R() < _mincfg.lowRadius) {
      if (spnts[0]->get_tracker() == 0)
        expected_handedness = _expected_handedness_t1;
      else
        expected_handedness = _expected_handedness_t2;
    }

    // Set up a position error vector (constant error, estimated to account for MCS)
    std::vector<double> err(n_points, _sd_mcs);

    // Call the fitter
    RootFitter::MinuitSZParameters cfg;
    cfg.handedness = expected_handedness;
    cfg.patRecSZCut = _long_minuit_cut;
    const double pStart[4] = {c_trial.get_x0(), c_trial.get_y0(), c_trial.get_R(), 0.0};
    bool result = RootFitter::FitHelixMinuit(ox, oy, oz, err, pStart, cfg, helix);

    if (!result) {
      if (n_points == 5 && _debug) {
        if (spnts[0]->get_tracker() == 0) {
          _fail_helix_tku->Fill(2);
        } else if (spnts[0]->get_tracker() == 1) {
          _fail_helix_tkd->Fill(2);
        }
      }
      return NULL;
    }
    if ((helix.get_longitudinal_chisq() / calc_minuit_longitudinal_ndf(n_points))  \
        > _long_minuit_cut) {
      if (n_points == 5 && _debug) {
        if (spnts[0]->get_tracker() == 0) {
          _fail_helix_tku->Fill(3);
        } else if (spnts[0]->get_tracker() == 1) {
          _fail_helix_tkd->Fill(3);
        }
      }
      return NULL;
    }
    if (helix.get_R() > _R_res_cut) {
            if (n_points == 5 && _debug) {
        if (spnts[0]->get_tracker() == 0) {
          _fail_helix_tku->Fill(4);
        } else if (spnts[0]->get_tracker() == 1) {
          _fail_helix_tkd->Fill(4);
        }
      }
      return NULL;
    }

    double dsdz = helix.get_dsdz();
    handedness = 1;
    if (dsdz < 0)
      handedness = -1;

    // Make the track
    helix.set_transverse_chisq(c_trial.get_chisq());
    helix.set_transverse_ndf(calc_circle_ndf(n_points));
    helix.set_longitudinal_ndf(calc_minuit_longitudinal_ndf(n_points));
    track = new SciFiHelicalPRTrack(helix, spnts);
  }

  // Set the charge
  int charge;
  if (spnts[0]->get_tracker() == 0) {
    if ( _bz_t1 > 0 ) {
      charge = handedness;
    } else {
      charge = - handedness;
    }
  } else {
    if ( _bz_t2 > 0 ) {
      charge = - handedness;
    } else {
      charge = handedness;
    }
  }
  track->set_charge(charge);

  // Set the number of points fitted and the fit algorithms used
  track->set_n_fit_points(n_points);
  track->set_alg_used_circle(_circle_fitter);
  track->set_alg_used_longitudinal(_longitudinal_fitter);

  return track;
}

bool PatternRecognition::find_dsdz(int n_points, std::vector<SciFiSpacePoint*> &spnts,
                                   const SimpleCircle& circle, const std::vector<double>& sigma_s,
                                   std::vector<double>& phi_i, SimpleLine& line_sz,
                                   TMatrixD& cov_sz, int& handedness) const {

  // Sort spacepoints in order seen by the beam (descending z for T1, ascending z for T2)
//  if (spnts[0]->get_tracker() == 0)
//    std::sort(spnts.begin(), spnts.end(), compare_spoints_descending_z);
//  else if (spnts[0]->get_tracker() == 1)
    std::sort(spnts.begin(), spnts.end(), compare_spoints_ascending_z);
//    std::sort(spnts.begin(), spnts.end(), compare_spoints_ascending_z);

  // Find each z_i and phi_i value for each spacepoint relative to the first spacepoint
  std::vector<double> z_i;         // Vector of the z coord of each successive spacepoint

  // Loop over the spacepoints
  for (std::vector<SciFiSpacePoint*>::const_iterator it = spnts.begin(); it != spnts.end(); ++it) {
    z_i.push_back((*it)->get_position().z());
    phi_i.push_back(SciFiTools::calc_phi((*it)->get_position().x(),
                    (*it)->get_position().y(), circle));
  }

  // Find the track handedness and the number of turns made between tracker stations
  std::vector<double> true_phi_i;  // phi corrected for any extra 2*n*pi rotations
  bool success = find_n_turns(z_i, phi_i, true_phi_i, handedness);
  if (success) {
    phi_i = true_phi_i;
  } else {
    if (n_points == 5 && _debug) {
      if (spnts[0]->get_tracker() == 0) {
        _fail_helix_tku->Fill(2);
      } else if (spnts[0]->get_tracker() == 1) {
        _fail_helix_tkd->Fill(2);
      }
    }
    if ( _verb > 0 ) std::cout << "INFO: Pattern Recognition: Failed to find n turns\n";
    return false;
  }

  // Using the circle radius and the true_dphi, calc the s_i (distance in x-y between sp)
  std::vector<double> s_i = SciFiTools::phi_to_s(circle.get_R(), true_phi_i);

  if ( _verb > 0 ) {
    for (size_t i = 0; i < s_i.size(); ++i) {
      std::cout << "(" << z_i[i] << ", " << s_i[i] << ")  ";
    }
    std::cout << std::endl;
  }

  // Fit ds and dz to a straight line, to get the gradient, which equals ds/dz
  if (_line_fitter == 0) { // Custom LSQ fitter
    LeastSquaresFitter::linear_fit(z_i, s_i, sigma_s, line_sz, cov_sz);
  } else if (_line_fitter == 1) { // ROOT based linear fitter
    RootFitter::FitLineLinear(z_i, s_i, sigma_s, line_sz, cov_sz);
  }

  // Check linear fit passes chisq test
  if ( !((line_sz.get_chisq() / calc_lsq_longitudinal_ndf(n_points)) < _sz_chisq_cut) ) {
    if (n_points == 5 && _debug) {
      if (spnts[0]->get_tracker() == 0) {
        _fail_helix_tku->Fill(3);
      } else if (spnts[0]->get_tracker() == 1) {
        _fail_helix_tkd->Fill(3);
      }
    }
    if ( _verb > 0 ) {
      std::cout << "INFO: Pattern Recognition: Failed s-z cut, ds/dz = " << line_sz.get_m() << ", "
                << "intercept = " << line_sz.get_c() << ", "
                << "chisq = " << line_sz.get_chisq() << std::endl;
    }
    return false;
  } else {
    if ( _verb > 0 ) {
      std::cout << "INFO: Pattern Recognition: Passed s-z cut, ds/dz is " << line_sz.get_m()
                << ", intercept = " << line_sz.get_c() << ", "
                << "chisq = " << line_sz.get_chisq() << std::endl;
    }
    return true;
  }
}

bool PatternRecognition::find_n_turns(const std::vector<double> &z, const std::vector<double> &phi,
                                      std::vector<double> &true_phi, int &handedness) const {
  // Sanity checks
  if ( (z.size() != phi.size()) || (z.size() < 3) || (z.size() > 5) ) {
    return false;
  }

  if (_verb > 0) {
    std::cout << "INFO: Pattern Recognition: n_turns_cut: " << _n_turns_cut << ", and using phi: ";
    for (size_t i = 0; i < phi.size(); ++i) {
      std::cout << phi[i] << " ";
    }
    std::cout << std::endl;
  }

  true_phi.clear();
  true_phi.resize(phi.size());
  // Separations are calculated in z and phi between each station and the first station seen by
  // the beam (station 5 for T1, station 1 for T2).
  std::vector<double> dz;
  std::vector<double> dphi;
  // 'Close' refers to a calculation of the corrected angle based on a value on our current value
  // of n for the last angle sep, then inferring the other angle seps using the station z separation
  std::vector<double> close_dphi(phi.size(), 0.0);
  std::vector<double> close_phi(phi.size(), 0.0);

  // Calculate the separations in phi and z between stations
//  std::cout << "i\tphi\tdphi\tz\tdz" << std::endl;
  bool add_2pi = false;
  for (size_t i = 0; i < phi.size(); ++i) {
    dz.push_back(z[i] - z[0]);
    if (add_2pi) dphi[i] = dphi[i] + 2*CLHEP::pi;
    dphi.push_back(phi[i] - phi[0]);
    if ( dphi[i] < 0.0 ) {
      dphi[i] = dphi[i] + 2*CLHEP::pi;
      add_2pi = true;
    }
//    std::cout << i << "\t" << phi[i] << "\t" << dphi[i] << "\t" << z[i] << "\t" << dz[i] << "\n";
  }

  // Setup a vector holding the values of n to try
  int myints[] = {0, 1, -1, 2, -2, 3, -3};
  std::vector<int> n_values(myints, myints + sizeof(myints) / sizeof(int));

  // Loop over n_values, the number of extra turns completed by the particle by the *final* station
  bool found = false;
  int true_n = 0;
  // std::cout << "n\tj\tdphi\tclose_dphi\trem\tres" << std::endl;
  for (size_t i = 0; i < n_values.size(); ++i) {
    // Calc a value of the last turning angle sep based on the current value of n
    close_dphi.back() = dphi.back() + 2*n_values[i]*CLHEP::pi;

    // Loop over the other turning angles
    bool pass = true;
    for (size_t j = 1; j < close_dphi.size(); ++j) {
      // Use this last angle sep and the separation between stations in z to calc the
      // other corrected turning angle separations
      close_dphi[j] = close_dphi.back() * ( dz[j] / dz.back());

      // If a sufficiently small residual is produced, accept current n as correct, for this angle
      double remainder = SciFiTools::my_mod(close_dphi[j], 2*CLHEP::pi);
      double residual = fabs(remainder) - fabs(dphi[j]);
      if ( fabs(residual) > _n_turns_cut ) pass = false;
//       std::cout << n_values[i] << "\t" << j << "\t" << dphi[j] << "\t" << close_dphi[j] << "\t"
//                 << remainder << "\t" << residual << std::endl;
    }

    // If n was accepted for all the turning angles
    if (pass) {
      found = true;
      true_n = n_values[i];
      if ( _verb > 0 ) std::cout << "INFO: Pattern Recognition: Found n = " << true_n << std::endl;
      break;
    }
    if ( _verb > 0 ) std::cout << std::endl;
  } // ~Loop over n_values

  // If we have found a value of n which was accepted, calc the true turning angles
  if (found) {
    if ( true_n < 0 ) {
      handedness = -1;
    } else {
      handedness = 1;
    }
    // std::cout << "Found particle track with handedness " << handedness << std::endl;

    // Transform dphi to phi
    for (size_t i = 0; i < close_dphi.size(); ++i) {
      close_phi[i] = close_dphi[i] + phi[0];
    }
    // Correct close phi to true phi, by working the number of turns that need to be added to each
    // angle, then using this to correct the angles directly without relying on the z ratios
    for (size_t i = 0; i < close_phi.size(); ++i) {
      double diff = fabs(phi[i] - close_phi[i]);
      if ( diff < CLHEP::pi ) {
        true_phi[i] = phi[i];
      } else if ( diff < 3*CLHEP::pi ) {
        true_phi[i] = phi[i] + 2*handedness*CLHEP::pi;
      } else if ( diff < 5*CLHEP::pi ) {
        true_phi[i] = phi[i] + 4*handedness*CLHEP::pi;
      } else if ( diff < 7*CLHEP::pi ) {
        true_phi[i] = phi[i] + 6*handedness*CLHEP::pi;
      }
    }
    return true;
  } else {
    return false;
  }
};

bool PatternRecognition::check_time_consistency(const std::vector<SciFiSpacePoint*> good_spnts,
                                                                            int tracker_id) const {

  bool helical_flag = (tracker_id == 0 ? _up_helical_pr_on : _down_helical_pr_on);
  bool straight_flag = (tracker_id == 0 ? _up_straight_pr_on : _down_straight_pr_on);
  double dT_first = 0.0;
  double dT_last = 0.0;
  /* TODO Waiting for Spacepoints to have time added ****
  double dT_first = good_spnts.front()->get_time();
  double dT_last  = good_spnts.back()->get_time();
  */
  double dZ = fabs(good_spnts.back()->get_position().z() - good_spnts.front()->get_position().z());

  double dS = 0.0;
  if ( straight_flag && !helical_flag ) // If you are ONLY looking at straight tracks
    dS = dZ;
  else if ( helical_flag ) // if you are trying to reconstruc EITHER straight OR helical tracks
    dS = dZ * _Pt_max / _Pz_min; // TODO _Pz_min is a guess right now. (both defined in header)

  double longest_allowed_time = dS / CLHEP::c_light;

  if ( fabs(dT_last - dT_first) > longest_allowed_time )
    return false;
  else
    return true;
}

void PatternRecognition::get_cuts(double& res_cut, double& straight_chisq_cut, double& R_res_cut,
                                  double& circle_chisq_cut, double& _circle_minuit_cut,
                                  double& n_turns_cut, double& sz_chisq_cut,
                                  double& long_minuit_cut) {
  res_cut = _res_cut;
  straight_chisq_cut = _straight_chisq_cut;
  R_res_cut = _R_res_cut;
  circle_chisq_cut = _circle_chisq_cut;
  n_turns_cut = _n_turns_cut;
  sz_chisq_cut = _sz_chisq_cut;
  long_minuit_cut = _long_minuit_cut;
}

void PatternRecognition::set_cuts(double res_cut, double straight_chisq_cut, double R_res_cut,
                                  double circle_chisq_cut, double circle_minuit_cut,
                                  double n_turns_cut, double sz_chisq_cut, double long_minuit_cut) {
  _res_cut = res_cut;
  _straight_chisq_cut = straight_chisq_cut;
  _R_res_cut = R_res_cut;
  _circle_chisq_cut = circle_chisq_cut;
  _n_turns_cut = n_turns_cut;
  _sz_chisq_cut = sz_chisq_cut;
  _circle_minuit_cut = circle_minuit_cut;
  _long_minuit_cut = long_minuit_cut;
}

// For linear Pattern Recognition use
bool PatternRecognition::set_end_stations(const std::vector<int> ignore_stations,
                                          int &o_st_num, int &i_st_num) const {

  if ( static_cast<int>(ignore_stations.size()) == 0 ) { // 5pt track case
    o_st_num = 4;
    i_st_num = 0;
  } else if ( static_cast<int>(ignore_stations.size()) == 1 ) { // 4pt track case
      // Check a valid station number has been supplied
      bool ok = false;
      for ( int i = 0; i < 5; ++i ) {
        if ( ignore_stations[0] == i ) ok = true;
      }
      if ( !ok ) {
        if ( _verb > 0 )
          std::cerr << "WARNING: Pattern Recognition: Invalid ignore station argument\n";
        return false;
      }
      // Set outer station number
      if ( ignore_stations[0] != 4 )
        o_st_num = 4;
      else
        o_st_num = 3;
      // Set inner station number
      if ( ignore_stations[0] != 0 )
        i_st_num = 0;
      else
        i_st_num = 1;
  } else if ( static_cast<int>(ignore_stations.size()) == 2 ) { // 3pt track case
      // Check valid station numbers have been supplied
      bool ok0 = false;
      bool ok1 = false;
      for ( int i = 0; i < 5; ++i ) {
        if ( ignore_stations[0] == i ) ok0 = true;
        if ( ignore_stations[1] == i ) ok1 = true;
      }
      if ( ignore_stations[0] == ignore_stations[1] ) ok0 = false;
      if ( !ok0 || !ok1 ) {
        if ( _verb > 0 )
          std::cerr << "WARNING: Pattern Recognition: Invalid ignore station argument\n";
        return false;
      }
      // Set outer station number
      if ( ignore_stations[0] != 4 && ignore_stations[1] != 4 )
        o_st_num = 4;
      else if ( ignore_stations[0] != 3 && ignore_stations[1] != 3)
        o_st_num = 3;
      else
        o_st_num = 2;
      // Set inner station number
      if ( ignore_stations[0] != 0 && ignore_stations[1] != 0 )
        i_st_num = 0;
      else if ( ignore_stations[0] != 1 && ignore_stations[1] != 1 )
        i_st_num = 1;
      else
        i_st_num = 2;
  } else {
      if ( _verb > 0 )
        std::cerr << "WARNING: Pattern Recognition: Invalid ignore station argument\n";
    return false;
  }
  return true;
} // ~set_end_stations(...)

// For straight Pattern Recognition use
bool PatternRecognition::set_seed_stations(const std::vector<int> ignore_stations,
                      int &o_st_num, int &i_st_num, int &mid_st_num) const {
  if ( static_cast<int>(ignore_stations.size()) == 0 ) { // 5pt track case
    o_st_num = 4;
    i_st_num = 0;
    mid_st_num = 2;
  } else if ( static_cast<int>(ignore_stations.size()) == 1 ) { // 4pt track case
      // Check a valid station number has been supplied
      bool ok = false;
      for ( int i = 0; i < 5; ++i ) {
        if ( ignore_stations[0] == i ) ok = true;
      }
      if ( !ok ) {
        if ( _verb > 0 )
          std::cerr << "WARNING: Pattern Recognition: Invalid ignore station argument\n";
        return false;
      }
      // Set outer station number
      if ( ignore_stations[0] != 4  )
        o_st_num = 4;
      else
        o_st_num = 3;
      // Set inner station number
      if ( ignore_stations[0] != 0 )
        i_st_num = 0;
      else
        i_st_num = 1;
       // Set middle station number
       if ( ignore_stations[0] != 2 )
         mid_st_num = 2;
       else
         mid_st_num = 1;
  } else if ( static_cast<int>(ignore_stations.size()) == 2 ) { // 3pt track case
      // Check valid station numbers have been supplied
      bool ok0 = false;
      bool ok1 = false;
      for ( int i = 0; i < 5; ++i ) {
        if ( ignore_stations[0] == i ) ok0 = true;
        if ( ignore_stations[1] == i ) ok1 = true;
      }
      if ( ignore_stations[0] == ignore_stations[1] ) ok0 = false;
      if ( !ok0 || !ok1 ) {
        if ( _verb > 0 )
          std::cerr << "WARNING: Pattern Recognition: Invalid ignore station argument\n";
        return false;
      }

      if ( ignore_stations[0] == 4 || ignore_stations[1] == 4 ) { // 1 of ignore stats is 4
        if ( ignore_stations[0] == 3 || ignore_stations[1] == 3 ) {
          o_st_num = 2;
          mid_st_num = 1;
          i_st_num = 0;
        } else if ( ignore_stations[0] == 2 || ignore_stations[1] == 2 ) {
          o_st_num = 3;
          mid_st_num = 1;
          i_st_num = 0;
        } else if ( ignore_stations[0] == 1 || ignore_stations[1] == 1 ) {
          o_st_num = 3;
          mid_st_num = 2;
          i_st_num = 0;
        } else if ( ignore_stations[0] == 0 || ignore_stations[1] == 0 ) {
          o_st_num = 3;
          mid_st_num = 2;
          i_st_num = 1;
        }
      } else if ( ignore_stations[0] == 3 || ignore_stations[1] == 3 ) {// 1 of ignore stats is 3
        if ( ignore_stations[0] == 2 || ignore_stations[1] == 2 ) {
          o_st_num = 4;
          mid_st_num = 1;
          i_st_num = 0;
        } else if ( ignore_stations[0] == 1 || ignore_stations[1] == 1 ) {
          o_st_num = 4;
          mid_st_num = 2;
          i_st_num = 0;
        } else if ( ignore_stations[0] == 0 || ignore_stations[1] == 0 ) {
          o_st_num = 4;
          mid_st_num = 2;
          i_st_num = 1;
        }
      } else if ( ignore_stations[0] == 2 || ignore_stations[1] == 2 ) {// 1 of ignore stats is 2
        if ( ignore_stations[0] == 1 || ignore_stations[1] == 1 ) {
          o_st_num = 4;
          mid_st_num = 3;
          i_st_num = 0;
        } else if ( ignore_stations[0] == 0 || ignore_stations[1] == 0 ) {
          o_st_num = 4;
          mid_st_num = 3;
          i_st_num = 1;
        }
      } else if ( ignore_stations[0] == 1 || ignore_stations[1] == 1 ) {// 1 of ignore stats is 1
        if ( ignore_stations[0] == 0 || ignore_stations[1] == 0 ) {
        o_st_num = 4;
        mid_st_num = 3;
        i_st_num = 2;
        }
      }
    } else {
      if ( _verb > 0 )
        std::cerr << "WARNING: Pattern Recognition: Invalid ignore station argument\n";
    return false;
  }
  return true;
} // ~set_seed_stations(...)

bool PatternRecognition::set_ignore_stations(const std::vector<int> &ignore_stations,
                                             int &ignore_st_1, int &ignore_st_2) const {
  ignore_st_1 = -1, ignore_st_2 = -1;
  if ( ignore_stations.size() == 0 ) {
    // Leave ignore stations as -1
  } else if ( ignore_stations.size() == 1 ) {
    ignore_st_1 = ignore_stations[0];
  } else if ( ignore_stations.size() == 2 ) {
    ignore_st_1 = ignore_stations[0];
    ignore_st_2 = ignore_stations[1];
  } else {
      if ( _verb > 0 )
        std::cerr << "WARNING: Pattern Recognition: Invalid ignore station argument\n";
    return false;
  }
  return true;
} // ~set_ignore_stations(...)

double PatternRecognition::sigma_on_s(const SimpleCircle& circ, const TMatrixD& cov_circ,
                                      const SciFiSpacePoint* const spnt) const {
  // Check the spnt pointer
  if (!spnt) {
    return 0.0;
  }
  // Set up the variables needed
  double xc = circ.get_x0();
  double yc = circ.get_y0();
  double rad = circ.get_R();
  double x = spnt->get_position().x();
  double y = spnt->get_position().y();
  double sigma_x = 0.0;
  double sigma_y = 0.0;
  if ( spnt->get_station() == 5 ) {
      sigma_x = _sd_5;
      sigma_y = _sd_5;
  } else {
    sigma_x = _sd_1to4;
    sigma_y = _sd_1to4;
  }

  // std::cerr << "Input circle parameters: " << xc << " " << yc << " " << rad << " " << x << " "
  //           << y << " " << sigma_x << " " << sigma_y << std::endl;

  // Set up the full covariance matrix for circle fit, including station resolutions
  TMatrixD covariance(5, 5);

  // Initially all entries are 0.0, set automatically when matrix was intialised
  // Top right 2*2 entries are the covariance matrix of the station resolutions (diagonal)
  covariance[0][0] = sigma_x*sigma_x;
  covariance[1][1] = sigma_y*sigma_y;
  // Bottom left 3*3 entries are the covariance matrix of the circle
  covariance[2][2] = cov_circ[0][0];
  covariance[2][3] = cov_circ[0][1];
  covariance[2][4] = cov_circ[0][2];
  covariance[3][2] = cov_circ[1][0];
  covariance[3][3] = cov_circ[1][1];
  covariance[3][4] = cov_circ[1][2];
  covariance[4][2] = cov_circ[2][0];
  covariance[4][3] = cov_circ[2][1];
  covariance[4][4] = cov_circ[2][2];

  // std::cerr << "Full covariance matrix: " << std::endl;
  // covariance.Print();

  // Now form delta, a vector holding the partial differentials of s wrt  x, y, r, xc, yc
  double denom = xc*xc - 2*xc*x + yc*yc - 2*yc*y + x*x + y*y;

  double dsx = (rad*(yc-y)) / denom;
  double dsy = (-rad*(xc-x)) / denom;
  double dsr = atan2((y-yc), (x-xc)); // NOTE: Should this be atan, or a mod done too?
  double dsxc = (rad*(yc-y)) / denom;
  double dsyc = (rad*(xc-x)) / denom;

  // std::cerr << "denom: " << denom << std::endl;
  // std::cerr << "Delta differentials: " << dsx << " " << dsy << " " << dsr << " " << dsxc << " "
  //           << dsyc << std::endl;

  TMatrixD delta(5, 1);
  delta[0] = dsx;
  delta[1] = dsy;
  delta[2] = dsr;
  delta[3] = dsxc;
  delta[4] = dsyc;

  // std::cerr << "Delta matrix: " << std::endl;
  //  delta.Print();

  // Next the transpose
  TMatrixD deltaT(1, 5);
  deltaT.Transpose(delta);

  // std::cerr << "Delta matrix: " << std::endl;
  // delta.Print();
  // std::cerr << "DeltaT matrix: " << std::endl;
  // deltaT.Print();

  // Now finally calculate the variance of s, and return the sqrt for the standard deviation
  TMatrix var_s = deltaT * (covariance * delta);

  // std::cerr << "var_s matrix: " << std::endl;
  // var_s.Print();

  return sqrt(var_s[0][0]);
}

bool PatternRecognition::missing_sp_search_helical(std::vector<SciFiSpacePoint*>& spnts,
                                                   SciFiHelicalPRTrack* trk) const {
  if (trk->get_num_points() != 4)  // Must have a 4 point helical track to search for missing sp
    return false;

  // Search for a station for which the track does not have a spacepoint
  int empty_station_num = -1;
  for (int i = 1; i < 6; ++i) {
    bool found = false;
    for (SciFiSpacePoint* sp : trk->get_spacepoints_pointers()) {
      if (sp->get_station() == i) {
        found = true;
        break;
      }
    }
    if (found == false) {
      empty_station_num = i;
      break;
    }
  }

  if (empty_station_num == -1) // No station for which track does not have a sp, so give up
    return false;

  // Do we have 1 and only 1 unused sp in the missed station which passes the closeness cut?
  SciFiSpacePoint* the_missing_sp = NULL;
  int nfound = 0;
  for (SciFiSpacePoint* sp : spnts) {
    if ((sp->get_tracker() != trk->get_tracker()) || (sp->get_station() != empty_station_num))
      continue;
    if (sp->get_used() == false) {
      double delta = SciFiTools::calc_circle_residual(sp, trk->get_circle_x0(),
                                                      trk->get_circle_y0(), trk->get_R());
      if (fabs(delta) < _missing_sp_cut) {
        ++nfound;
        the_missing_sp = sp;
      }
    }
  }
  if (nfound != 1) { // Require one candidate only, otherwise give up
    return false;
  } else { // Got one, add to the track
    the_missing_sp->set_used(true);
    the_missing_sp->set_add_on(true);
    trk->add_spacepoint_pointer(the_missing_sp);
  }
  return true;
}

void PatternRecognition::calculate_expected_handedness(double bz_t1, double bz_t2,
                                                       int D2_polarity) {
  int t1_field_polarity = (bz_t1 > 0.00001) - (bz_t1 < -0.00001); // 10 Gauss
  int t2_field_polarity = (bz_t2 > 0.00001) - (bz_t2 < -0.00001);

  _expected_handedness_t1 = t1_field_polarity * D2_polarity;
  _expected_handedness_t2 = - t2_field_polarity * D2_polarity;
  std::cerr << "PatternRecognition: Expected TkU & TkD handedness: " << _expected_handedness_t1
            << " " << _expected_handedness_t2 << "\n";
}

void PatternRecognition::set_expected_handedness(int t1, int t2) {
  this->_expected_handedness_t1 = t1;
  this->_expected_handedness_t2 = t2;
}
} // ~namespace MAUS