#include "JFTrackFit.h"
#include <map>
#include <TMath.h>

// This is a map of the actual sense wire coordinates in cm.
const Double_t chPos[28][2] = {{-2.1,4.9},{-0.7,4.9},{0.7,4.9},{2.1,4.9},
                               {-1.4,3.5},{0.0,3.5},{1.4,3.5},
                               {-2.1,2.1},{-0.7,2.1},{0.7,2.1},{2.1,2.1},
                               {-1.4,0.7},{0.0,0.7},{1.4,0.7},
                               {-2.1,-0.7},{-0.7,-0.7},{0.7,-0.7},{2.1,-0.7},
                               {-1.4,-2.1},{0.0,-2.1},{1.4,-2.1},
                               {-2.1,-3.5},{-0.7,-3.5},{0.7,-3.5},{2.1,-3.5},
                               {-1.4,-4.9},{0.0,-4.9},{1.4,-4.9}};

JFTrackFit::JFTrackFit(std::map<Int_t,Double_t> newhots)
{
  hots = newhots;
}

Double_t JFTrackFit::operator()(const Double_t * pars)
{
  Double_t xt = pars[0];
  Double_t theta = pars[1];
    
  Double_t S = 0;
  for(auto &kv : hots)
    {
      Int_t chan = kv.first;
      Double_t doca = kv.second;
      Double_t xi = chPos[chan][0];
      Double_t yi = chPos[chan][1];
      Double_t tantheta = TMath::Tan(theta);
      Double_t x = TMath::Power(TMath::Sin(theta),2)*(
						      xi + xt/(tantheta*tantheta) + yi/tantheta);
      Double_t y = (x-xt)/tantheta;
      Double_t s0 = TMath::Sqrt( (x-xi)*(x-xi)+(y-yi)*(y-yi) );
      S += TMath::Abs(doca-s0);
    }
  return S;
}