~jfcaron/+junk/Proto2BeamTest2

//////////////////////////////////////////////////////////////////
// Include statements.
//////////////////////////////////////////////////////////////////
#include "JFTrackFit.h"
#include "Utility.C"
#include "Geom.C"

//#include <TGraph2D.h>
#include <TH2.h>
#include <TMath.h>
#include <TCanvas.h>
#include <TEllipse.h>
#include <TLine.h>
#include <TMarker.h>
#include <TPad.h>
//#include <TText.h>
#include <TLatex.h>
#include <Math/Minimizer.h>
#include <Math/Factory.h>
#include <Math/Functor.h>
#include <TStyle.h>

#ifndef __CINT__
#include <array>
#endif
#include <vector>
#include <map>
#include <iostream>
#include <numeric>
#include <algorithm>
#include <functional>
#include <cassert>

//////////////////////////////////////////////////////////////////
// Global variables.
//////////////////////////////////////////////////////////////////
// Allowed range for track x0 parameter.  It must be inside the x-most cells.
// The extreme-x cells are centered at -2.1 and 2.1, and are 1.4cm wide, so the range
// should be {-3.5,3.5}
static const Double_t x0_Range[2] = {-3.5,3.5};
static const Double_t Initial_Guesses[2] = {0.0,TMath::Pi()/2.0};
static const Double_t steps[2] = {1.0,0.5};

//////////////////////////////////////////////////////////////////
// Constructors for JFTrackFit
//////////////////////////////////////////////////////////////////
JFTrackFit::JFTrackFit(Double_t run_angle,std::map<Int_t,Hot> newhots,
		       t2x::MyDxOfX * new_delta_doca)
{
  hots = newhots;
  //Track_Params[0] = Initial_Guesses[0];//AverageX(hots);
  Initialize_Track_Params(run_angle); 
  
  delta_doca = new_delta_doca;
  static_delta_doca = !Bool_t(delta_doca); // Gets set to false if delta_doca is NULL
  status = -1;
  ValidateHots(); // This excludes any hots with inf/nan values.
  have_resolutions = kFALSE;
  resolutions.clear();
}

JFTrackFit::JFTrackFit()
{
  hots.clear();
  Initial_Track_Params[0] = Initial_Guesses[0];
  Initial_Track_Params[1] = Initial_Guesses[1];
  Track_Params[0] = Initial_Guesses[0];
  Track_Params[1] = Initial_Guesses[1];
  static_delta_doca = kTRUE;
  status = -1;
  have_resolutions = kFALSE;
  resolutions.clear();
}

//////////////////////////////////////////////////////////////////
// JFTrackFit class methods
//////////////////////////////////////////////////////////////////
void JFTrackFit::Initialize_Track_Params()
{
  Initialize_Track_Params(Initial_Track_Params[1]);
}

void JFTrackFit::Initialize_Track_Params(Double_t angle)
{
  Double_t cumsum = 0;
  Double_t sumweights = 0;
  Double_t tantheta = TMath::Tan(angle);
  for(const auto &kv: hots)
    { 
      if(kv.second.excluded) continue;
      Double_t weight = 1.0/kv.second.delta_doca;
      Double_t quantity = (Geom::chPos[kv.first][0]) - (Geom::chPos[kv.first][1] )/tantheta;
      cumsum += quantity*weight;
      //cumsum += Geom::chPos[kv.first][0]*weight;
      sumweights += weight;
    }
  Track_Params[0] = cumsum/sumweights;
  Track_Params[1] = angle;
  Initial_Track_Params[0] = Track_Params[0];
  Initial_Track_Params[1] = Track_Params[1];  
}

Double_t JFTrackFit::operator()(const Double_t * pars)
{
  Double_t S2 = 0;
  for(auto &kv : hots)
    {
      if(kv.second.excluded) continue;
      
      // Distance of POCA on proposed track from sense wire.
      const Double_t signed_D = Geom::DistanceFromWire(kv.first,pars);
      const Double_t D = TMath::Abs(signed_D);
      
      // Denominator is either the static delta_doca from the hot, or
      // taken from the MyDxOfX interpolator at the POCA of the proposed track.
      
      Double_t denominator = kv.second.delta_doca;
      if(!static_delta_doca)
	{ // Dynamic delta_doca, so look it up from the MyDxOfX* member.
	  // I use Max(D,0.002) because the sense wire is 0.002cm thick (20um)
	  // and using a minimum distance avoids hitting the infinity at D = 0.
	  const Double_t d = (*delta_doca).Eval(TMath::Max(D,0.002));
	  // Often d will be NaN/inf, so only use it if it's finite.
	  // We fall back on the static one otherwise.
	  // Use the largest of the denominators, as it will underweigh
	  // cells and tracks with a large delta_doca.
	  if(TMath::Finite(d))
	    { 
	      denominator = TMath::Max(denominator,d);
	    }

	  // Same thing for the dynamic resolution.
	}
      if(have_resolutions)
	{
	  const t2x::Side side = signed_D > 0 ? t2x::RIGHT : (signed_D < 0 ? t2x::LEFT : t2x::BOTH);
          kv.second.side_used = side;
          //std::cout << static_cast<int>(side) << std::endl;
	  const Double_t reso = resolutions[side]->Eval(kv.second.doca);
          if(TMath::Finite(reso))
	    {
	      denominator = TMath::Max(denominator,reso);
	    }
	  //TMath::Sqrt(d*d + hot.delta_doca*hot.delta_doca) : hot.delta_doca; // Geometric average
	  //d : hot.delta_doca; // Trust dx_of_x.
	}
      
      const Double_t chi_contrib_unsquared = (D-kv.second.doca)/denominator;
      kv.second.chi2_contribution = chi_contrib_unsquared*chi_contrib_unsquared;
      kv.second.denominator_used = denominator;
      S2 += kv.second.chi2_contribution;

    }
  assert(TMath::Finite(S2));
  return S2;
}

// This operator only exists to allow the JFTrackFit to be used in a TF2
// constructor.  The second argument is not used.
Double_t JFTrackFit::operator()(Double_t * x, Double_t *)
{
  return operator()(x);
}

Double_t JFTrackFit::operator()()
{ // Track_Params is a member of the JFTrackFit instance.
  // This is equivalent to this(this->Track_Params);
  return operator()(Track_Params);
}

Double_t JFTrackFit::DistanceFromWire(Int_t chan)
{
  return Geom::DistanceFromWire(chan,this->Track_Params);
}

void JFTrackFit::PrintHots()
{
  Double_t chi2_from_operator = this->operator()(this->Track_Params);

  std::cout << " i ( x  ,  y  ): ri,       delta_ri, dx_at_track," 
	    << " wire_dist, chi2 contrib, denominator" 
	    << std::endl;
  Double_t totalchi2 = 0;
  for(auto &kv: hots)
    {
      Int_t chan = kv.first;
      Double_t D = Geom::DistanceFromWire(chan,Track_Params);
      TString dataline = TString::Format("%02d (%+2.1f, %+2.1f): %4f, %4f",
					 chan,Geom::chPos[chan][0],Geom::chPos[chan][1],
					 kv.second.doca,kv.second.delta_doca);
      if(this->static_delta_doca)
	{
	  dataline.Append(",          ");
	}
      else
	{
	  dataline.Append(TString::Format(", %11f",(*delta_doca).Eval(TMath::Abs(D))));
	}
      dataline.Append(TString::Format(", %+8f",D));
      if(!kv.second.excluded)
        {
          dataline.Append(TString::Format(",%11.7g",kv.second.chi2_contribution));
        }
      else
        {
          dataline.Append(TString::Format(", EXCL %5.5g",kv.second.chi2_contribution));
        }
      dataline.Append(TString::Format(", %10f",kv.second.denominator_used));
      // if(kv.second.excluded)
      //   {
      //     dataline.Append("\t\t this hot is excluded from the total chi2.");
      //   }
      totalchi2+=kv.second.chi2_contribution;
      std::cout << dataline << "\n";
    }
  std::cout << TString::Format("Total chi2 from sum: %10.10g",totalchi2) << std::endl;
  //std::cout << TString::Format("Total chi2 from () : %10.10g",chi2_from_operator) << std::endl;
  assert(totalchi2 == chi2_from_operator);
}

Int_t JFTrackFit::ChooseExcluded_old(Double_t threshold)
{
  if(hots.size() == 0)
    {
      return -1;
    }
  std::map<Int_t,Double_t> chi2s;
  for(auto &kv: hots)
    {
      if(kv.second.excluded) continue;
      Double_t newchi2 = kv.second.chi2_contribution;
      chi2s[kv.first] = newchi2;
    }
  // I need a function to accumulate from a map<int,double>.
  auto map_add = [] (Double_t lhs, const std::pair<Int_t,Double_t> & rhs) 
    {
      return lhs+rhs.second;
    };
  
  // And another to compare pair<int,double> by their values.
  auto map_comp = [] (const std::pair<Int_t,Double_t> & lhs, 
		      const std::pair<Int_t,Double_t> & rhs)
    {
      return lhs.second < rhs.second;
    };
  
  Double_t sum_chi2 = std::accumulate(chi2s.begin(),chi2s.end(),0.0,map_add);
  auto max_pair = *(std::max_element(chi2s.begin(),chi2s.end(),map_comp));
  Int_t max_chan = max_pair.first;
  Double_t max_chi2 = max_pair.second;

  if(max_chi2 > threshold*sum_chi2) 
    {
      return max_chan;
    }
  else
    {
      return -1;
    }
}

Int_t JFTrackFit::ChooseExcluded(Double_t rel_threshold, Double_t abs_threshold)
{
  if(hots.size() == 0) return -1;
  Double_t sum_chi2 = operator()(Track_Params);
  Int_t max_chan = -1;
  Double_t max_chi2 = -1;
  for(auto kv : hots)
    {
      Double_t chi2 = kv.second.chi2_contribution;
      if(chi2 > max_chi2 and chi2 > rel_threshold*sum_chi2)
	{
	  if(abs_threshold == -1 or chi2 > abs_threshold)
	    {
	      // This condition selects chi2 > rel*totchi2
	      // AND chi2 > abs.  Among those passing, it 
	      // selects the maximum chi2.
	      max_chi2 = chi2;
	      max_chan = kv.first;
	    }
	}
    }
  return max_chan;
}

Int_t JFTrackFit::ChooseExcluded_ByLayerOccupancy()
{
  if(hots.size() == 0) return -1;

  const Int_t nlayers = 8;
  Int_t layer_occupancy[nlayers] = {0};
  
  // Here we count up the hots per layer.
  std::vector<Int_t> candidates;
  for(auto const & kv : hots)
    {
      if(kv.second.excluded) continue;
      const Int_t chan = kv.first;
      const Int_t layer = Geom::chCoords[chan][1];
      layer_occupancy[layer]++;
    }

  // Here I collect candidate Hots for exclusion.
  for(auto const & kv : hots)
    {
      if(kv.second.excluded) continue;
      const Int_t chan = kv.first;
      const Int_t layer = Geom::chCoords[chan][1];
      if(layer_occupancy[layer] > 1)
	{
	  candidates.push_back(chan);
	}
    }

  if(candidates.size() < 1) return -1;

  Double_t besttoexclude = -1;  
  Int_t ndof = CountValidHots()-2;
  Double_t maxprobchi2 = TMath::Prob(operator()(),ndof);
  for(auto const & chan : candidates)
    {
      ExcludeHot(chan);
      std::cout << "Trying to exclude channel " << chan << std::endl;
      this->Minimize();
      Double_t newprobchi2 = TMath::Prob(operator()(),ndof-1);
      if(newprobchi2 > maxprobchi2)
	{
	  maxprobchi2 = newprobchi2;
	  besttoexclude = chan;
	}
      UnexcludeHot(chan);
    }
  
  return besttoexclude;
}

Int_t JFTrackFit::ChooseExcluded_ByRefit()
{
  if(hots.size() == 0) return -1;
  Int_t besttoexclude = -1;
  Int_t ndof = CountValidHots()-2;
  Double_t maxprobchi2 = TMath::Prob(operator()(),ndof);
  for(auto const & kv : hots)
    {
      if(kv.second.excluded) continue;
      Int_t chan = kv.first;
      ExcludeHot(chan);
      //MinimizeJFTrackFit(*this);
      this->Minimize();
      Double_t newprobchi2 = TMath::Prob(operator()(),ndof-1);
      if(newprobchi2 > maxprobchi2)
	{
	  maxprobchi2 = newprobchi2;
	  besttoexclude = chan;
	}
      UnexcludeHot(chan);
    }
  return besttoexclude;
}

int JFTrackFit::FitSingleLayerOccupancy(int printlevel)
{
  // First determine the occupancy of each layer.
  const int nlayers = 8;
  const int excluded_layer = 3; 
  std::array<std::vector<int>,nlayers> layer_channels;
  std::array<int,nlayers> occupancies = {};
  // Here we count up the hots per layer.
  for(auto const & kv : hots)
    {
      const Int_t chan = kv.first;
      const Int_t layer = Geom::chCoords[chan][1];
      if(layer == excluded_layer) continue;
      occupancies[layer]++;
      layer_channels[layer].push_back(chan);
    }
  
  auto mult_ifnotzero = [](int x, int y){return std::max(x,1)*std::max(y,1);};
  int npossible = std::accumulate(occupancies.begin(), occupancies.end(),
                                  1, mult_ifnotzero);
  // if(npossible > 10)
  //   {
  //     std::cout << "Number of possible tracks: " << npossible << std::endl;
  //   }

  std::vector<std::array<int,nlayers> > tracks(npossible);
  std::array<int,nlayers> counters = {};
  for(int i = 0; i < npossible; i++)
    {
      for(int layer = 0; layer < nlayers; layer++)
        {
          if(layer_channels[layer].size() == 0)
            {
              tracks[i][layer] = -1;
              continue;
            }
          tracks[i][layer] = layer_channels[layer][counters[layer]];
          counters[layer] = (counters[layer] + 1) % occupancies[layer];
        }
    }

  // Exclude all hots.
  for(auto const & kv : hots)
    {
      if(!kv.second.excluded) ExcludeHot(kv.first);
    }

  // double best_chi2prob = -1;
  double best_chi2 = std::numeric_limits<double>::max();
  int best_track = -1;
  int best_status = -9999;
  //for(const auto & track : tracks)
  for(std::size_t i = 0; i < tracks.size(); i ++)
    {
      for(auto chan : tracks[i])
        { // Unexclude the hots in the track.
          // Would normally use UnexcludeHot, but that
          // re-calculates the chi-squared each time.
          if(chan == -1) continue;
          hots.at(chan).excluded = false; 
        }
      Initialize_Track_Params();
      //this->operator()();
      //this->GridMinimize();
      int newstatus = this->Minimize(printlevel);
      //double new_chi2prob =
      //TMath::Prob(this->operator()(),CountValidHots()-2);
      double new_chi2 = this->operator()();
      // if(new_chi2prob > best_chi2prob)
      if(new_chi2 < best_chi2)
        {
          if(printlevel) std::cout << "Best track so far: " << i << std::endl;
          //best_chi2prob = new_chi2prob;
          best_chi2 = new_chi2;
          best_track = i;
          best_status = newstatus;
        }

      // Re-exclude all the hots in the track.
      // Remember ExcludeHot ignores -1.
      for(auto chan : tracks[i]) ExcludeHot(chan);
    }

  // Now re-unexclude the hots in the best track.
  for(auto chan : tracks[best_track])
    {
      if(chan == -1) continue;
      hots.at(chan).excluded = false;
      this->operator()();
    }
  
  return npossible;
}

void JFTrackFit::ExcludeHot(Int_t chan)
{
  if(chan == -1) return;
  if(hots.at(chan).excluded) 
    std::cout << "Warning, Hot to be excluded was already excluded!" << std::endl;
  hots.at(chan).excluded = true;
  hots.at(chan).chi2_contribution = 0.0;
}

void JFTrackFit::UnexcludeHot(Int_t chan)
{
  if(chan == -1) return;
  if(!hots.at(chan).excluded)
    std::cout << "Warning, Hot to be unexcluded was not excluded!" << std::endl;
  hots.at(chan).excluded = false;
  operator()(); // This line re-calculates all the chi2 contributions.
}

void JFTrackFit::ValidateHots()
{
  for(auto kv : hots)
    {
      if( not (TMath::Finite(kv.second.doca) and TMath::Finite(kv.second.delta_doca) ))
	{
	  //cout << "Excluding hot " << kv.first << endl;
	  ExcludeHot(kv.first);
	}
    }
}

Int_t JFTrackFit::CountValidHots()
{
  Int_t count = 0;
  for(auto kv : hots)
    {
      count += Int_t(!kv.second.excluded);
    }
  return count;
}

void JFTrackFit::SetResolution(t2x::Side side, t2x::MyInterpolator * interp)
{
  resolutions[side] = interp;
  if(resolutions.count(t2x::LEFT) == 1 &&
     resolutions.count(t2x::BOTH) == 1 &&
     resolutions.count(t2x::RIGHT) == 1)
    {
      have_resolutions = kTRUE;
    }
  return;
}

Int_t JFTrackFit::Minimize(int printlevel)
{
  namespace M = ROOT::Math;
  M::Minimizer * min = nullptr;
  // min = M::Factory::CreateMinimizer("Minuit2","Migrad");
  // min = M::Factory::CreateMinimizer("Minuit2","Simplex");
  min = M::Factory::CreateMinimizer("Minuit2","Combined");
  // min = M::Factory::CreateMinimizer("Minuit2","Scan");
  // min = M::Factory::CreateMinimizer("Minuit2","Fumili");
  // min = M::Factory::CreateMinimizer("GSLMultiMin","ConjugateFR");
  // min = M::Factory::CreateMinimizer("GSLMultiMin","ConjugatePR");
  // min = M::Factory::CreateMinimizer("GSLMultiMin","BFGS");
  // min = M::Factory::CreateMinimizer("GSLMultiMin","BFGS2");
  // min = M::Factory::CreateMinimizer("GSLMultiMin","SteepestDescent");
  // min = M::Factory::CreateMinimizer("GSLMultiFit","");
  // min = M::Factory::CreateMinimizer("GSLSimAn","");

  M::Functor ftor(*this,2);
  min->SetFunction(ftor);

  min->SetPrintLevel(printlevel);
  //min->SetMaxFunctionCalls(100000);
  //min->SetMaxIterations(100000);
  //min->SetTolerance(0.000001);
  //min->SetTolerance(10);

  min->SetVariable(0,"x0",this->Track_Params[0],steps[0]);
  min->SetVariable(1,"theta",this->Track_Params[1],steps[1]);

  //min->SetLimitedVariable(0,"x0",this->Track_Params[0],
  //                        steps[0],x0_Range[0],x0_Range[1]);
  //min->SetLimitedVariable(1,"theta",this->Track_Params[1],
  //                        steps[1],0.0000001,TMath::Pi());
  
  
  const Int_t old_gEIL = gErrorIgnoreLevel;
  gErrorIgnoreLevel = kWarning;
  min->Minimize();
  gErrorIgnoreLevel = old_gEIL;
  //this->Track_Params[0] = min->X()[0];
  //this->Track_Params[1] = min->X()[1];
  const double orig_params[] = {min->X()[0],min->X()[1]};

  double best_chi2 = min->MinValue();
  double best_params[] = {min->X()[0],min->X()[1]};
  int best_status = min->Status();
  double best_errors[] = {min->Errors()[0],min->Errors()[1]};
  double best_corr = min->Correlation(0,1);

  const double jiggle_threshold = 100.0;
  //const double jiggle_threshold = std::numeric_limits<double>::max();
  if(best_chi2 > jiggle_threshold)
    {
      min->SetPrintLevel(0);
      //std::cout << "\nTrying jiggle!" << std::endl;
      const double jiggle[] = {0.7,0.15};
      for(auto jig_x : {-1,1})
        {
          for(auto jig_y : {-1,1})
            {
              min->SetVariableValue(0,orig_params[0]+jig_x*jiggle[0]);
              min->SetVariableValue(1,orig_params[1]+jig_y*jiggle[1]);
              gErrorIgnoreLevel = kWarning;
              min->Minimize();
              gErrorIgnoreLevel = old_gEIL;
              const double new_chi2 = min->MinValue();
              if(new_chi2 < best_chi2)
                {
                  best_params[0] = min->X()[0];
                  best_params[1] = min->X()[1];
                  best_status = min->Status();
                  best_errors[0] = min->Errors()[0];
                  best_errors[1] = min->Errors()[1];
                  best_corr = min->Correlation(0,1);
                  // std::cout << "Jiggle from " 
                  //           << orig_params[0] << "," << orig_arams[1]
                  //           << " to "
                  //           << best_params[0] << "," << best_params[1]
                  //           << " complete.\n  chi2 went from " << best_chi2
                  //           << " to " << new_chi2 << "\n" << std::endl;
                  best_chi2 = new_chi2;
                }
            }
        }
    }
  this->status = best_status; // min->Status();
  this->Track_Params[0] = best_params[0];
  this->Track_Params[1] = best_params[1];
  
  ////////////////////////////////////////////////
  // Calculate the derivative of the ACellDist
  this->Delta_Track_Params[0] = best_errors[0]; // min->Errors()[0];
  this->Delta_Track_Params[1] = best_errors[1]; // min->Errors()[1];
  this->Covariance = best_corr*best_errors[0]*best_errors[1];
  //this->Covariance = min->Correlation(0,1)*min->Errors()[0]*min->Errors()[1];
  // cov(x,y) = corr(x,y)*dx*dy
  
  delete min;
  return this->status;
}

Int_t JFTrackFit::GridMinimize()
{
  const int npx = 100, npy = 100;
  const double x_step = (x0_Range[1]-x0_Range[1])/npx;
  const double y_step = TMath::Pi()/npy;
  double zmin = std::numeric_limits<double>::max();
  double xmin,ymin;
  for(int i_x = 0;i_x<npx;i_x++)
    {
      const double x = x0_Range[0]+i_x*x_step;
      for(int i_y = 0;i_y<npy;i_y++)
        {
          const double y = i_y*y_step;
          const double xy[] = {x,y};
          const double z = this->operator()(xy);
          if(z < zmin)
            {
              zmin = z;
              xmin = x;
              ymin = y;
            }
        }
    }
  this->status = -2;
  this->Track_Params[0] = xmin;
  this->Track_Params[1] = ymin;
  this->Delta_Track_Params[0] = x_step;
  this->Delta_Track_Params[1] = y_step;
  this->Covariance = TMath::QuietNaN();
  return this->status;
}

//////////////////////////////////////////////////////////////////
// Additional functions involving JFTrackFit
//////////////////////////////////////////////////////////////////

TCanvas * DrawTrack(JFTrackFit & jftr,const char* filename)
{
  TCanvas * c1 = new TCanvas("c1","c1",50,0,400,800);
  Double_t w = gPad->GetWw()*gPad->GetAbsWNDC();
  Double_t h = gPad->GetWh()*gPad->GetAbsHNDC();
  Double_t xmin = -3.1;
  Double_t xmax =  3.1;
  Double_t ymin = -5.9;
  Double_t ymax = ((xmax-xmin)*h/w)-5.9;
  c1->SetFixedAspectRatio();
  c1->Range(xmin,ymin,xmax,ymax); 
  c1->cd();
  TEllipse hot_circles[28];
  TMarker sense_wire(0,0,6);
  TText wire_label;
  wire_label.SetTextAlign(22);
  for(auto &kv : jftr.hots)
    {
      Int_t chan = kv.first;
      Double_t radius = kv.second.doca;
      Double_t x = Geom::chPos[chan][0];
      Double_t y = Geom::chPos[chan][1];
      if(kv.second.excluded)
	{
	  hot_circles[chan].SetLineColor(kRed);
	  wire_label.SetTextColor(kRed);
	}
      else
	{
	  hot_circles[chan].SetLineColor(kBlack);
	  wire_label.SetTextColor(kBlack);
	}
      wire_label.DrawText(x,y,TString::Format("%02d",chan))->SetBit(kCanDelete);
      //hot_circles[chan] = new TEllipse(x,y,radius);
      hot_circles[chan].SetFillStyle(0);
      hot_circles[chan].DrawEllipse(x,y,radius,0,0,360,0);
      hot_circles[chan].DrawEllipse(x,y,radius-kv.second.delta_doca,0,0,360,0);
      hot_circles[chan].DrawEllipse(x,y,radius+kv.second.delta_doca,0,0,360,0);
      hot_circles[chan].SetBit(kCanDelete);

      sense_wire.DrawMarker(x,y);
      
    }
  TMarker field_wire(0,0,2);
  for(auto &xy : Geom::FieldPos)
    {
      field_wire.DrawMarker(xy[0],xy[1]);
    }
  Double_t x0 = jftr.Track_Params[0];
  Double_t m = 1.0/TMath::Tan(jftr.Track_Params[1]);
  
  Double_t track_y1 = -10;
  Double_t track_x1 = x0+m*track_y1;
  Double_t track_y2 = 10;
  Double_t track_x2 = x0+m*track_y2;
  //cout << "Track line: " << track_x1 << ", " << track_y1 << ", "  << track_x2  << ", " << track_y2 << endl;
  TLine * tl = new TLine(track_x1,track_y1,track_x2,track_y2);
  tl->SetBit(kCanDelete);
  Utility::RestrictLine(*tl,xmin,ymin,xmax,ymax);
  tl->Draw();
  gPad->Modified();
  gPad->Update();
  if(filename)
    {
      c1->SaveAs(TString::Format("plots/track_%s.pdf",filename));
      delete c1;
      return nullptr;
    }
  else
    {
      //cout << "Remember to delete the canvas!" << endl;
      return c1;
    }
}

Double_t Initial_x0(const std::map<Int_t,Hot> &hots, double angle)
{
  Double_t cumsum = 0;
  Double_t sumweights = 0;
  Double_t tantheta = TMath::Tan(angle);
  for(const auto &kv: hots)
    {
      if(kv.second.excluded) continue;
      Double_t weight = 1.0/kv.second.delta_doca;
      Double_t quantity = (Geom::chPos[kv.first][0]) - (Geom::chPos[kv.first][1] )/tantheta;
      cumsum += quantity*weight;
      //cumsum += Geom::chPos[kv.first][0]*weight;
      sumweights += weight;
    }
  return cumsum/sumweights;
}

Double_t Delta_D(const Double_t * Track_Params, const Double_t * Delta_Track_Params, Double_t covariance)
{
  const Double_t x0 = Track_Params[0];
  const Double_t theta = Track_Params[1];
  const Double_t delta_x0 = Delta_Track_Params[0];
  const Double_t delta_theta = Delta_Track_Params[1];

  const Double_t D = Geom::DistanceFromWire(12,Track_Params);

  // Hardcoded values for cell 12.  Could also use Geom::wire_coords?
  const Double_t xi = 0.0;
  const Double_t yi = 0.7;

  const Double_t sin = TMath::Sin(theta);
  const Double_t cos = TMath::Cos(theta);

  const Double_t dD2_dx0 = 2*TMath::Power(sin,4)*x0 + sin*sin*cos*cos*2*x0 + sin*cos*2*yi;
  const Double_t dD2_dtheta = 2*sin*cos*(x0*x0-xi*xi-yi*yi) + 2*yi*x0*(cos*cos-sin*sin);

  const Double_t dD_dx0 = 1.0/(2.0*D) * dD2_dx0;
  const Double_t dD_dtheta = 1.0/(2.0*D) * dD2_dtheta;

  return TMath::Sqrt(dD_dx0*dD_dx0*delta_x0*delta_x0 + 
		     dD_dtheta*dD_dtheta*delta_theta*delta_theta + 
		     2*dD_dtheta*dD_dx0*covariance);
}

TCanvas * DrawFitMap(JFTrackFit & jftf)
{
  const int npx = 200, npy = 200;
  const double xlow = x0_Range[0], xhigh = x0_Range[1];
  const double ylow = 0.0, yhigh = TMath::Pi();

  const double fit_min_chi2 = jftf();
  const double fit_ini_chi2 = jftf(jftf.Initial_Track_Params);

  double grid_min_chi2 = std::numeric_limits<double>::max();
  double grid_max_chi2 = std::numeric_limits<double>::lowest();
  double xmin,ymin;

  TH2D * h = new TH2D("h","h",npx,xlow,xhigh,npy,ylow,yhigh);
  for(int i_x = 1;i_x <= npx;i_x++)
    {
      for(int i_y = 1;i_y <= npy;i_y++)
        {
          const double xy[2] = {h->GetXaxis()->GetBinCenter(i_x),
                                h->GetYaxis()->GetBinCenter(i_y)};
          const double z = jftf(xy);
          if(TMath::Finite(z)) h->SetBinContent(i_x,i_y,z);
          if(z < grid_min_chi2)
            {
              grid_min_chi2 = z;
              xmin = xy[0];
              ymin = xy[1];
            }
          if(z > grid_max_chi2) grid_max_chi2 = z;
        }
    }
  jftf();
  h->GetXaxis()->SetTitle("Track x0 (cm)");
  h->GetYaxis()->SetTitle("Track Theta");
  h->SetTitle("");

  const double best_min = std::min({grid_min_chi2,fit_min_chi2,fit_ini_chi2});
  const double limit = best_min + (grid_max_chi2 - best_min)/10.0;
  
  // std::cout << "grid_min_chi2: " << grid_min_chi2 
  //           << " grid_max_chi2: " << grid_max_chi2 << std::endl;
  // std::cout << "fit_min_chi2: " << fit_min_chi2  
  //           << " fit_ini_chi2: " << fit_ini_chi2 << std::endl;
  // std::cout << "graph limit: " << limit << std::endl;
  
  const int fbb_x = Utility::FindFirstBinBelow(h,limit,1);
  const int lbb_x = Utility::FindLastBinBelow(h,limit,1);
  //std::cout << "fbb_x: " << fbb_x << " lbb_x: " << lbb_x << std::endl;
  h->GetXaxis()->SetRange(fbb_x,lbb_x);
  
  const int fbb_y = Utility::FindFirstBinBelow(h,limit,2);
  const int lbb_y = Utility::FindLastBinBelow(h,limit,2);
  //std::cout << "fbb_y: " << fbb_y << " lbb_y: " << lbb_y << std::endl;
  h->GetYaxis()->SetRange(fbb_y,lbb_y);

  h->SetMaximum(limit);
  h->SetMinimum(best_min);
  
  h->SetBit(kCanDelete);
  
  gStyle->SetPalette(53);
  h->SetContour(255);
  TCanvas * c1 = new TCanvas("cfitmap","cfitmap",50,0,400,400);
  c1->cd();
  h->Draw("colz");
  gPad->SetLogz();
  c1->SetRightMargin(0.12);

  Color_t fitcolor(kGreen);
  Color_t gridcolor(kMagenta);
  double fitmarkersize = 0.5;
  double gridmarkersize = 0.5;
  
  // Print chi-squared value from fit, using the same color as the fit marker.
  TText chi2_label;
  //TLatex chi2_label;
  //chi2_label.SetTextFont(8);
  chi2_label.SetTextAlign(11);
  chi2_label.SetTextColor(fitcolor);
  //chi2_label.DrawText(1,3.0,TString::Format("Init fit: %g",jftf(jftf.Initial_Track_Params)))->SetBit(kCanDelete);
  //chi2_label.DrawText(1,2.8,TString::Format("From fit: %g",jftf()))->SetBit(kCanDelete);

  chi2_label.DrawTextNDC(0.1,0.96,TString::Format("Init fit: %g",fit_ini_chi2))->SetBit(kCanDelete);
  chi2_label.DrawTextNDC(0.1,0.92,TString::Format("From fit: %g",fit_min_chi2))->SetBit(kCanDelete);
  
  // Print chi-squared value from grid, using the same color as the
  // grid marker.
  chi2_label.SetTextColor(gridcolor);
  //chi2_label.DrawText(1,2.6,TString::Format("From grid: %g",min_chi2))->SetBit(kCanDelete);
  chi2_label.DrawTextNDC(0.5,0.96,TString::Format("From grid: %g",grid_min_chi2))->SetBit(kCanDelete);
  
  // Location of best-fit track parameters.
  TMarker fit_location(0,0,23);
  fit_location.SetMarkerColor(fitcolor);
  fit_location.SetMarkerSize(fitmarkersize);
  fit_location.DrawMarker(jftf.Track_Params[0],jftf.Track_Params[1]);

  // Location of best-chi2 on evaluated grid.
  TMarker grid_location(0,0,3);
  grid_location.SetMarkerColor(gridcolor);
  grid_location.SetMarkerSize(gridmarkersize);
  grid_location.DrawMarker(xmin,ymin);

  // Location of initial track parameters before fit.
  TMarker init_location(0,0,22);
  init_location.SetMarkerColor(fitcolor);
  init_location.SetMarkerSize(fitmarkersize);
  init_location.DrawMarker(jftf.Initial_Track_Params[0],
                           jftf.Initial_Track_Params[1]);

  gPad->Modified();
  gPad->Update();

  return c1;
}