TrackFit_module.cc

Go to the documentation of this file.

////////////////////////////////////////////////////////////////////////
// Class:       TrackFit
// Module Type: producer
// File:        TrackFit_module.cc
//
// Generated at Tue Oct 30 14:07:52 2012 by Hongyue Duyang using artmod
// from art v1_01_01.
////////////////////////////////////////////////////////////////////////

#include "art/Framework/Core/EDProducer.h"
#include "art/Framework/Core/ModuleMacros.h"
#include "canvas/Persistency/Common/Assns.h"
#include "art/Framework/Principal/Handle.h"
#include "art/Framework/Principal/Event.h"
#include "canvas/Persistency/Common/Ptr.h"
#include "canvas/Persistency/Common/PtrVector.h"
#include "cetlib_except/exception.h"

#include "DDTBaseDataProducts/BaseProducts.h"
#include "DDTBaseDataProducts/Track.h"
#include "DDTBaseDataProducts/DAQHit.h"
#include "DDTBaseDataProducts/CompareDAQHit.h"
#include "DDTBaseDataProducts/HitList.h"
#include "DDTUtilities/AssociationUtil.h"

#include "DAQChannelMap/DAQChannelMap.h"

#include <cmath>
#include <string>
#include <vector>

namespace novaddt {
  class TrackFit;
}

class novaddt::TrackFit : public art::EDProducer {
public:
  explicit TrackFit(fhicl::ParameterSet const & p);
  virtual ~TrackFit();
  void endJob() override;
  virtual void produce(art::Event & e);
  
private:

  virtual void SeedWeights(const std::vector<double>& x, 
                           const std::vector<double>& zx,
                           const std::vector<double>& y,
                           const std::vector<double>& zy,
                           std::vector<double>& w);
  virtual void FitView(const std::vector<double>& x, 
                       const std::vector<double>& zx,
                       std::vector<double>& wx,
                       double *start,
                       double *End,
                       const std::vector<double>& tdc, 
                       double &eTDC,
                       double &lTDC 
                       );
  virtual double DsqrToLine(double x0, double y0,
                            double x1, double y1,
                            double x2, double y2);
  virtual double LinFitMinDperp(const std::vector<double>& x,
                                const std::vector<double>& y,
                                const std::vector<double>& w,
                                double* x1, double* y1,
                                double* x2, double* y2);
  virtual double LinFit(const std::vector<double>& x,
                        const std::vector<double>& y,
                        const std::vector<double>& w,
                        double* x1, double* y1,
                        double* x2, double* y2);    
  virtual double UtilLinFit(const std::vector<double>& x,
                            const std::vector<double>& y,
                            const std::vector<double>& w,
                            double& m, double& c);

  // Declare member data here.
  std::string fSliceLabel;
  std::string fSliceInstance;
  double fDHitGood;
  bool fSortOutputHits;
  std::vector<double> fR; ///< Range of weight function
  std::vector<double> fT; ///< Transition width of weight function
  std::string _assn_to_slice_instance;
  int n_events          = 0;
  int n_slices_input    = 0;
  int n_tracks_output   = 0;
};

//------------------------------------------------------------------------------
novaddt::TrackFit::TrackFit(fhicl::ParameterSet const & p)
  : fSliceLabel    (p.get< std::string >("slice_label")   )
  , fSliceInstance (p.get< std::string >("slice_instance"))
  , fDHitGood      (p.get<double>       ("DHitGood")      )
  , fSortOutputHits(p.get<bool>         ("SortOutputHits"))
  , fR(8)
  , fT(8)
  , _assn_to_slice_instance(p.get<std::string>("assn_to_slice_instance"))
{
  fR[0] =  0.0;      fT[0] =   0.0;
  fR[1] = fDHitGood; fT[1] = 1000.0;//fR = 6.0 from 1 
  fR[2] = fDHitGood; fT[2] =  500.0;
  fR[3] = fDHitGood; fT[3] =  250.0;
  fR[4] = fDHitGood; fT[4] =  100.0;
  fR[5] = fDHitGood; fT[5] =   50.0;
  fR[6] = fDHitGood; fT[6] =   25.0;
  fR[7] = fDHitGood; fT[7] =   10.0;
  
  // Call appropriate Produces<>() functions here.
  produces< std::vector<novaddt::Track>   >();
  produces< std::vector<novaddt::HitList> >();

  // Associate Tracks with the Slice they came from
  produces< art::Assns<novaddt::Track, novaddt::HitList> >();
  produces< art::Assns<novaddt::Track, novaddt::HitList> >(_assn_to_slice_instance);
}

//------------------------------------------------------------------------------
novaddt::TrackFit::~TrackFit()
{
  // Clean up dynamic memory and other resources here.
}

//------------------------------------------------------------------------------
void novaddt::TrackFit::produce(art::Event & e)
{
  LOG_DEBUG("Tracking") << "event: " << e.id().event() << std::endl;
  n_events++;
  //get sliced hits
  art::Handle< std::vector<novaddt::HitList> > slices;
  e.getByLabel(fSliceLabel, fSliceInstance, slices);//TimeSlice
  //Now make it an art::PtrVector to use in the associations later
  art::PtrVector<novaddt::HitList> slicelist;
  for(unsigned int i = 0; i < slices->size(); ++i){
    art::Ptr<novaddt::HitList>slice(slices,i);
    slicelist.push_back(slice);
  }

  //a container of track object to be write into event
  std::unique_ptr< std::vector<novaddt::Track>   > trackcol  (new std::vector<novaddt::Track>  );
  std::unique_ptr< std::vector<novaddt::HitList> > hitListcol(new std::vector<novaddt::HitList>);
  std::unique_ptr< art::Assns<novaddt::Track, novaddt::HitList> > assn(new art::Assns<novaddt::Track, novaddt::HitList>);
  std::unique_ptr< art::Assns<novaddt::Track,   novaddt::HitList> > assn_b(new art::Assns<novaddt::Track, novaddt::HitList>);

  //loop over grouped hit list
  for(size_t i = 0; i < slices->size(); ++i){
    n_slices_input++;
    art::Ptr<novaddt::HitList> slicedhits(slices,i);

    std::vector<double> xzViewX;
    std::vector<double> xzViewZ;
    std::vector<double> yzViewY;
    std::vector<double> yzViewZ;
    std::vector<double> xzViewW; 
    std::vector<double> yzViewW; 

    std::vector<double> xzTDC;
    std::vector<double> yzTDC;

    //earliest and latest TDC
    double eTDC = 0;
    double lTDC = slicedhits->at(0).TDC().val;
    
    LOG_DEBUG("Tracking") << "new slice" << std::endl;
    unsigned int counter = 0;
    //loop over hits in each slice
    for(auto const& j : *slicedhits){
        std::string view = "y";
        if( j.View().val == daqchannelmap::X_VIEW ){
            view = "x";
        }
        LOG_DEBUG("Tracking") << "\thit[" << counter
                  << "]: TDC: "   << j.TDC().val
                  << ", ADC: "    << j.ADC().val
                  << ", plane: "  << j.Plane().val
                  << ", "         << view
                  << "-cell: "    << j.Cell().val
                  << std::endl;            
        counter++;
      //DAQHit hit(slicedhits[j]);

      //initiallize eTDC and lTDC
      eTDC = j.TDC().val;

      //get hit position in xz view and yz view
      if( j.View().val == daqchannelmap::X_VIEW ){//xz view
        xzViewX.push_back(j.Cell().val);
        xzViewZ.push_back(j.Plane().val);
        xzViewW.push_back(0);
        xzTDC.push_back(j.TDC().val);
      }
      else if( j.View().val == daqchannelmap::Y_VIEW ){//yz view
        yzViewY.push_back(j.Cell().val);
        yzViewZ.push_back(j.Plane().val);
        yzViewW.push_back(0);
        yzTDC.push_back(j.TDC().val);
      }
      //LOG_DEBUG("Tracking") << "plane: " << j.Plane().val
                            //<< " cell: " << j.Cell().val
                            //<< " tdc: " << j.TDC().val
                            //<< std::endl;
    }//end loop over hits in each slice

    //get seed weight
    this->SeedWeights(xzViewX, xzViewZ, yzViewY, yzViewZ, xzViewW);
    this->SeedWeights(yzViewY, yzViewZ, xzViewX, xzViewZ, yzViewW);    

    //do a 2D fit, 0 is z (plane), 1 is x/y (cell)
    double xzStart[2] = {0.};//[0]=z, [1]=x
    double xzEnd[2]   = {0.};
    double yzStart[2] = {0.};//[0]=z, [1]=y
    double yzEnd[2]   = {0.};

    this->FitView(xzViewX, xzViewZ, xzViewW, xzStart, xzEnd, xzTDC, eTDC, lTDC);
    this->FitView(yzViewY, yzViewZ, yzViewW, yzStart, yzEnd, yzTDC, eTDC, lTDC);        

    // protect against bad fits
    if(xzStart[0] == 0. &&
       xzStart[1] == 0. &&
       xzEnd[0]   == 0. &&
       xzEnd[1]   == 0. &&
       yzStart[0] == 0. &&
       yzStart[1] == 0. &&
       yzEnd[0]   == 0. &&
       yzEnd[1]   == 0.) continue;

    double xzSlope = (xzStart[1] - xzEnd[1])/(xzStart[0] - xzEnd[0]);
    double yzSlope = (yzStart[1] - yzEnd[1])/(yzStart[0] - yzEnd[0]);
    double xzInt   = xzStart[1] - xzSlope*xzStart[0];
    double yzInt   = yzStart[1] - yzSlope*yzStart[0];
    
    // go through and make an association between hits on track
    // and the track
    novaddt::HitList hl_xz;
    novaddt::HitList hl_yz;
    
    for(auto const& h : *slicedhits){
      if( h.View().val == daqchannelmap::X_VIEW ){//xz view
        if(std::abs(h.Plane().val*xzSlope + xzInt - h.Cell().val) < fDHitGood){
            hl_xz.push_back(h);
        }
      }
      else if( h.View().val == daqchannelmap::Y_VIEW ){//yz view
        if(std::abs(h.Plane().val*yzSlope + yzInt - h.Cell().val) < fDHitGood){
            hl_yz.push_back(h);
        }
      }
    }//end loop over hits in each slice
    
    // protect against bad fits
    if((hl_xz.size() < 2) && 
       (hl_yz.size() < 2)) continue;
       
    LOG_DEBUG("Tracking") << "--- Track found: x: " << xzStart[1]
              << " - "          << xzEnd[1]
              << ", z: "        << xzStart[0]
              << " - "          << xzEnd[0]
              << ", n hits: "   << hl_xz.size()
              << std::endl;
    LOG_DEBUG("Tracking") << "--- Track found: y: " << yzStart[1]
              << " - "          << yzEnd[1]
              << ", z: "        << yzStart[0]
              << " - "          << yzEnd[0]
              << ", n hits: "   << hl_yz.size()
              << std::endl;    
    
    if (fSortOutputHits){
        sort(hl_xz.begin(), hl_xz.end(), CompareDAQHit<TDC>());
        sort(hl_yz.begin(), hl_yz.end(), CompareDAQHit<TDC>());
    }
    //Implement the starting and end time of the track according to your own algorithm:
    float startt =0;
    float endt = 0;
    n_tracks_output+=2;
    novaddt::Track x_track(daqchannelmap::X_VIEW, 0,
                           xzStart[1], xzStart[0],startt,
                           xzEnd[1], xzEnd[0],endt
                          );
    novaddt::Track y_track(daqchannelmap::Y_VIEW, 0,
                           yzStart[1], yzStart[0],startt,
                           yzEnd[1], yzEnd[0],endt
                          );    
    //All tracks registered here are supposed to be c speed, but I made them temperarily all yz view as well. Will be changed.
    trackcol->push_back(x_track); 
    hitListcol->push_back(hl_xz);
    util::CreateAssn(*this, e, *trackcol, *hitListcol, *assn, hitListcol->size()-1, hitListcol->size());
    util::CreateAssn(*this, e, *trackcol, slicelist[i], *assn_b);
    trackcol->push_back(y_track); // push back a second time for the yz view
    hitListcol->push_back(hl_yz); 
    util::CreateAssn(*this, e, *trackcol, *hitListcol, *assn, hitListcol->size()-1, hitListcol->size());
    util::CreateAssn(*this, e, *trackcol, slicelist[i], *assn_b);
    
  }//end loop over groups
  
  //save the fitted track into the event
  e.put(std::move(trackcol));
  e.put(std::move(hitListcol));
  e.put(std::move(assn));
  e.put(std::move(assn_b), _assn_to_slice_instance);

}//end produce
//---------------------------------------------------------------------
void novaddt::TrackFit::endJob()
{
    std::cout << "=== novaddt::TrackFit endJob"   << std::endl;
    std::cout << "\tNumber of events:             " << n_events           << std::endl;
    std::cout << "\tNumber of slices input:       " << n_slices_input     << std::endl;
    std::cout << "\tNumber of tracks output:      " << n_tracks_output    << std::endl;
}

//---------------------------------------------------------------------
//................................................................
//Get seed weights. copied from offline RLFit.cxx 
void novaddt::TrackFit::SeedWeights(const std::vector<double>& x, 
                                    const std::vector<double>& zx,
                                    const std::vector<double>& y,
                                    const std::vector<double>& zy,
                                    std::vector<double>& w)
{
  for (unsigned int i=0; i<zx.size(); ++i) {
    double d;
    w[i] = 1.0E-9;
    //
    // Find closest hit in same view
    //
    for (unsigned int j=0; j<zx.size(); ++j) {
      if (i==j) continue;
      //d = util::pythag(zx[i]-zx[j], x[i]-x[j]);
      d = sqrt( (zx[i]-zx[j])*(zx[i]-zx[j]) + (x[i]-x[j])*(x[i]-x[j]) );
      if (d<3.0) w[i] += 1.0;
    }
    //
    // Find closest hit in orthogonal view
    //
    for (unsigned int j=0; j<zy.size(); ++j) {
      d = std::abs(zx[i]-zy[j]);
      if (d<3.0) w[i] += 0.1;
    }
  }
}//end SeedWeights


//...................................................................
//Modify RLFit to fit 2D track
void novaddt::TrackFit::FitView(const std::vector<double>& x, 
                                const std::vector<double>& zx,
                                std::vector<double>& wx,
                                double *start,
                                double *end, 
                                const std::vector<double>& tdc, 
                                double &eTDC,
                                double &lTDC)
{
  //
  // Make a line fit to the hits. To improve preformance in a noisy
  // environment, use a deterministic annealing algoritm
  //
  double x1  = 0;
  double x2  = 0;
  double zx1 = 0;
  double zx2 = 0;

  double slope = 0;
  double intercept = 0;

  //loop over fR
  for (unsigned int i=0; i<fR.size(); ++i) {
    static const double eps = 1.0E-6;
    
    // Calculate weights for this iteration. First iteration uses seed weights    
    if (i>0) {
      for (unsigned int j=0; j<wx.size(); ++j) {
        double d = this->DsqrToLine(zx[j], x[j], zx1, x1, zx2, x2);

        wx[j] = (1.0+exp(-fR[i]/fT[i]))/(1.0+exp((d-fR[i])/fT[i]));
        
        // Penalty for hits outside the detector in the other view      
        //double y = fMzy*fZx[j] + fBzy;
        //if (y<fBoxYlo) fWx[j] *= exp(-(fBoxYlo-y)/fT[i]);
        //if (y>fBoxYhi) fWx[j] *= exp(-(y-fBoxYhi)/fT[i]);
        
        if (wx[j]<eps) wx[j] = eps;
      }
    }
    
    // Perform the fit
    if(zx.size() != x.size() ||
       wx.size() != x.size()  || 
       x.size()  <  2){
      return;
    }

    this->LinFitMinDperp(zx, x, wx, &zx1, &x1, &zx2, &x2);
    
    // Compute slopes and intercepts    
    double dzx = zx2-zx1;
    if (dzx<=0.0) dzx = 1.0E-9;

    slope     = (x2-x1)/dzx;
    intercept = (x1*zx2-x2*zx1)/dzx;    
  }//end fR loop
  LOG_DEBUG("Tracking") << "x1: "    << x1
            << ", x2: "  << x2
            << ", zx1: " << zx1
            << ", zx2: " << zx2
            << std::endl;
  // Adjust z ranges to completely contain hits on the track. 
  //"On track" is defined as having significant weight
  
  start[0] = zx[0];
  end[0]   = zx[0]+0.01;
  
  for (size_t i = 0; i < wx.size(); ++i) {
    if (wx[i]>0.001) {
      if (zx[i]  < start[0]) start[0] = zx[i];
      if (zx[i]  > end[0])   end[0]   = zx[i];
      if (tdc[i] < eTDC)     eTDC     = tdc[i];
      if (tdc[i] > lTDC)     lTDC     = tdc[i];
    }
  }  
  // Use line fits to compute x and y locations at the start and end z positions
  LOG_DEBUG("Tracking") << "start: " << start[0] << ", end: " << end[0] << ", slope: " << slope << ", intercept: " << intercept << std::endl;
  start[1] = slope*start[0] + intercept;
  end[1]   = slope*end[0]   + intercept;
  // protect against lines exiting the detector, 
  // at the moment the upper cell number is hard coded for the FD
  if (start[1]<=0.)  start[1] = 1.0E-9;
  if (end[1]<=0.)    end[1]   = 1.0E-9;
  if (start[1]>383.) start[1] = 383.;
  if (end[1]>383.)   end[1]   = 383.;
  
}//end FitView

//...................................................................
//
double novaddt::TrackFit::DsqrToLine(double x0, double y0,
                                     double x1, double y1,
                                     double x2, double y2)
{ 
  // If we've been handed a point instead of a line, return distance
  // to the point
  if (x1==x2 && y1==y2) return (x0-x1)*(x0-x1) +(y0-y1)*(y0-y1);
  
  return
    ((x2-x1)*(y1-y0)-(x1-x0)*(y2-y1))*((x2-x1)*(y1-y0)-(x1-x0)*(y2-y1)) / 
    ( (x2-x1)*(x2-x1) + (y2-y1)*(y2-y1) );
}

//....................................................................
// 
double novaddt::TrackFit::LinFitMinDperp(const std::vector<double>& x,
                                         const std::vector<double>& y,
                                         const std::vector<double>& w,
                                         double* x1, double* y1,
                                         double* x2, double* y2)
{
  // check array sizes before passing them to this method to avoid
  // cases where we would be tempted to throw exceptions
  // x, y, and w must all be the same size and have size > 2

  unsigned register int i;
  
  // Find the extremes of the inputs
  double xlo = DBL_MAX;
  double xhi = -DBL_MAX;
  double ylo = DBL_MAX;
  double yhi = -DBL_MAX;
  for (i=0; i<w.size(); ++i) {
    if(w[i] == 0) continue;
    if (x[i]<xlo) xlo = x[i];
    if (y[i]<ylo) ylo = y[i];
    if (x[i]>xhi) xhi = x[i];
    if (y[i]>yhi) yhi = y[i];
  }
  
  // This algorithm really fails if the y values are identical
  // return the low and hi values if it's too flat
  if(std::abs(yhi-ylo) < 0.1){
    *x1 = xlo;
    *y1 = ylo;
    *x2 = xhi;
    *y2 = yhi;
    return 0.; //its a flat line, chi^2 is 0 by definition
  }
  
  // For stability, fit in a way that avoids infinite slopes,
  // exchanging the x and y variables as needed.
  if (std::abs(xlo - xhi) < 1.e-3) {
    double chi2;
    double xx1, yy1, xx2, yy2;
    // Notice: (x,y) -> (y,x)
    chi2 = LinFitMinDperp(y,x,w,&xx1, &yy1,&xx2, &yy2);
    
    *x1 = xlo; // Infinite slope means that your initial x positions should be the input
    *y1 = xx1;
    *x2 = xhi;
    *y2 = xx2;
    return chi2;
  }
  
  // Accumulate the sums needed to compute the fit line
  double Sw  = 0.0;
  double Swx = 0.0;
  double Swy = 0.0;
  double Swxy= 0.0;
  double Swy2= 0.0;
  double Swx2= 0.0;
  for (i=0; i<w.size(); ++i) {
    Sw   += w[i];
    Swx  += w[i]*x[i];
    Swy  += w[i]*y[i];
    Swxy += w[i]*x[i]*y[i];
    Swy2 += w[i]*y[i]*y[i];
    Swx2 += w[i]*x[i]*x[i];
  }
  if(Sw < 0.0)
    throw cet::exception("TrackFit") << "Sw < 0";
  
  // Precalculate reused squares
  //using util::sqr;
  const double SwSq  = Sw*Sw;//sqr(Sw);
  const double SwxSq = Swx*Swx;//sqr(Swx);
  const double SwySq = Swy*Swy;//sqr(Swy);
  
    // C and D are two handy temporary variables
  double C =
    +SwxSq*SwxSq // Swx^4
    -2.0*SwxSq*Swx2*Sw
    +2.0*SwxSq*SwySq
    +2.0*SwxSq*Swy2*Sw
    +Swx2*Swx2*SwSq
    +2.0*Swx2*Sw*SwySq
    -2.0*Swx2*SwSq*Swy2
    +pow(Swy, 4) // Swy^4
    -2.0*SwySq*Swy2*Sw
    +Swy2*Swy2*SwSq
    -8.0*Swx*Swy*Swxy*Sw
    +4.0*Swxy*Swxy*SwSq;
  if (C>0.0) C = sqrt(C);
  else       C = 0.0;
  double D = -Swx*Swy+Swxy*Sw;
  
  // The first of the two solutions. This one is the best fit through
    // the points
  double M1;
  double B1;
  if (D==0.0) {
    // D could be zero, this might happen in a segmented detector if
    // the track fit passed through only a single readout plane and
    // all hits share the same x coordinates. I've tried to avoid
    // this by checking if it makes more sense to fit with x and y
    // exchanged above. This is an extra precaution.
    D = 1.0E-9;
  }
  M1 = -(-SwxSq + Swx2*Sw + SwySq - Swy2*Sw - C)/(2.0*D);
  B1 = -(-Swy+M1*Swx)/Sw;
  
  // This second solution is perpendicular to the first and represents
  // the worst fit
  // M2 = -(-SwxSq + Swx2*Sw + SwySq - Swy2*Sw + C)/(2.0*D);
  // B2 = -(-Swy+M2*Swx)/Sw;
  
  *x1 = xlo;
  *y1 = M1*xlo + B1;
  *x2 = xhi;
  *y2 = M1*xhi + B1;
  
  // Compute the chi^2 function for return
  double chi2 = 0.0;
  for (i=0; i<w.size(); ++i) {
    chi2 += w[i]*this->DsqrToLine(x[i],y[i],*x1,*y1,*x2, *y2);
  }  
  // test if linfit is better
  
  double chi2lin, linx1, linx2, liny1, liny2;
  
  chi2lin = LinFit(x,y,w,&linx1,&liny1,&linx2,&liny2);
  
  if(chi2lin < chi2) { //something is wrong with the perp fit, use the lin fit
    *x1 = linx1;
    *y1 = liny1;
    *x2 = linx2;
    *y2 = liny2;
    
    chi2 = chi2lin;
  }
  
  
  return chi2;
}//end LinFitMinDperp


//...............................................................
//
double novaddt::TrackFit::LinFit(const std::vector<double>& x,
                                 const std::vector<double>& y,
                                 const std::vector<double>& w,
                                 double* x1, double* y1,
                                 double* x2, double* y2)
{
  register unsigned int i;
  
  // Before going ahead, make sure we have sensible arrays
  assert(x.size()==y.size());
  assert(x.size()==w.size());
  assert(x.size()>=2);
  
  // Find the ranges of the input variables
  double xlo = x[0];
  double xhi = x[0];
  double ylo = y[0];
  double yhi = y[0];

    for (i=1; i<w.size(); ++i) {
      if (x[i]<xlo) xlo = x[i];
      if (x[i]>xhi) xhi = x[i];
      if (y[i]<ylo) ylo = y[i];
      if (y[i]>yhi) yhi = y[i];
    }
    // Make sure the fit has some span
    if (yhi-ylo==0.0 && xhi-xlo==0.0) {
      std::cerr << __FILE__ << ":" << __LINE__
                << " All points identical in line fit!" << std::endl;
      *x1 = xlo;
      *y1 = ylo;
      *x2 = xhi;
      *y2 = yhi;
      return 0.0;
    }

    //
    // If the y extent is larger than the x extent, flip the variables
    // and fit
    //
    if (yhi-ylo > xhi-xlo) {
      return this->LinFit(y,x,w,y1,x1,y2,x2);
    }

    double m, b;
    const double chi2 = this->UtilLinFit(x, y, w, m, b);
    
    *x1 = xlo;
    *x2 = xhi;
    *y1 = m*(*x1)+b;
    *y2 = m*(*x2)+b;

    return chi2;
}

//..................................................................
//copied from offline Util::LinFit
double novaddt::TrackFit::UtilLinFit(const std::vector<double>& x,
                                     const std::vector<double>& y,
                                     const std::vector<double>& w,
                                     double& m, double& c)
{
  // Before going ahead, make sure we have sensible arrays
  assert(x.size() == y.size());
  assert(x.size() == w.size());
  assert(x.size() >= 2);
  
  // Accumulate the sums for the fit
  double Sw   = 0;
  double Swx  = 0;
  double Swy  = 0;
  double Swxy = 0;
  double Swy2 = 0;
  double Swx2 = 0;
  for(unsigned int i = 0; i < w.size(); ++i) {
    Sw   += w[i];
    Swx  += w[i]*x[i];
    Swy  += w[i]*y[i];
    Swx2 += w[i]*x[i]*x[i];
    Swxy += w[i]*x[i]*y[i];
    Swy2 += w[i]*y[i]*y[i];
  }
  const double d = Sw*Swx2 - Swx*Swx;
  m = (Sw*Swxy  - Swx*Swy)/d;
  c = (Swy*Swx2 - Swx*Swxy)/d;
  
  const double chi2 =
    Swy2 - 2.0*m*Swxy - 2.0*c*Swy + 2.0*m*c*Swx +
    c*c*Sw + m*m*Swx2;
  
  return chi2;
}

DEFINE_ART_MODULE(novaddt::TrackFit)