StereoGeometryFinder.cc

Go to the documentation of this file.

#include "StereoGeometryFinder.h"


#include <fwk/CentralConfig.h>
#include <fwk/CoordinateSystemRegistry.h>
#include <fwk/LocalCoordinateSystem.h>

#include <utl/Reader.h>
#include <utl/ErrorLogger.h>
#include <utl/MathConstants.h>
#include <utl/PhysicalConstants.h>
#include <utl/Transformation.h>
#include <utl/TimeStamp.h>
#include <utl/TabulatedFunctionErrors.h>
#include <utl/Point.h>
#include <utl/UTMPoint.h>
#include <utl/ReferenceEllipsoid.h>

#include <evt/Event.h>
#include <evt/ShowerFRecData.h>
#include <evt/ShowerRecData.h>
#include <evt/ShowerSRecData.h>



#include <fevt/FEvent.h>
#include <fevt/Eye.h>
#include <fevt/Telescope.h>
#include <fevt/Pixel.h>
#include <fevt/PixelRecData.h>
#include <fevt/EyeRecData.h>
#include <fevt/TelescopeRecData.h>
#include <fevt/EyeHeader.h>

#include <det/Detector.h>

#include <fdet/FDetector.h>
#include <fdet/Eye.h>
#include <fdet/Telescope.h>
#include <fdet/Channel.h>
#include <fdet/Pixel.h>


#include <sevt/SEvent.h>
#include <sevt/Station.h>
#include <sevt/StationTriggerData.h>
#include <sevt/StationRecData.h>
#include <sevt/EventTrigger.h>

#include <sdet/SDetector.h>
#include <sdet/Station.h>


#include <TMinuit.h>

#include <algorithm>
#include <iostream>
#include <fstream>
#include <iomanip>



using namespace std;
using namespace fwk;
using namespace utl;
using namespace fevt;
using namespace evt;
using namespace StereoGeometryFinderOG;
using namespace sevt;

evt::Event* StereoGeometryFinder::fCurEvent = 0;

utl::Vector StereoGeometryFinder::fSDP = utl::Vector();

std::set<int> StereoGeometryFinder::fExcludeEye;
std::set<int> StereoGeometryFinder::fExcludeSDPEye;

int StereoGeometryFinder::fDebugging = 0;

double StereoGeometryFinder::fChi2SDP = 0;
int StereoGeometryFinder::fNDofSDP = 0;

double StereoGeometryFinder::fChi2TimeFit = 0;
int StereoGeometryFinder::fNDofTimeFit = 0;

int StereoGeometryFinder::fStereoHybrid = 0;
int StereoGeometryFinder::fHotStationId = 0;
double StereoGeometryFinder::fCelesteDelay = 0;


VModule::ResultFlag
StereoGeometryFinder::Init()
{
  CentralConfig* cc = CentralConfig::GetInstance();

  Branch topB = cc->GetTopBranch("StereoGeometryFinder");

  topB.GetChild("forDebug").GetData(fDebugging);

  vector<int> exclEyes;
  topB.GetChild("excludeEyes").GetData(exclEyes);
  fExcludeEye.clear();
  for (unsigned int i = 0; i < exclEyes.size(); ++i)
    fExcludeEye.insert(exclEyes[i]);

  vector<int> exclSDPEyes;
  topB.GetChild("excludeSDPEyes").GetData(exclSDPEyes);
  fExcludeSDPEye.clear();
  for (unsigned int i = 0; i < exclSDPEyes.size(); ++i)
    fExcludeSDPEye.insert(exclSDPEyes[i]);

  topB.GetChild("stereoHybrid").GetData(fStereoHybrid);

  topB.GetChild("celesteDelay").GetData(fCelesteDelay);

  return eSuccess;
}


VModule::ResultFlag
StereoGeometryFinder::Run(evt::Event& event)
{
  if (!event.HasFEvent())
    return eContinueLoop;

  fCurEvent = &event; // acrobacy for TMinuit
  fevt::FEvent& fevent = event.GetFEvent();

  fevt::FEvent::EyeIterator eyeIter;

  //Counting the number of Eyes with more than 4 good pixels
  unsigned int numEyes = 0;

  for (eyeIter = fevent.EyesBegin(ComponentSelector::eHasData);
       eyeIter != fevent.EyesEnd(ComponentSelector::eHasData);
       ++eyeIter) {

    fevt::Eye& eye = *eyeIter;
    if (!eye.HasRecData())
      eye.MakeRecData();

    EyeRecData& eyeRecData = eye.GetRecData();
    if (eyeRecData.GetNSDPPixels() <= 4)
      continue;

    if (fStereoHybrid == 1) {
      const std::vector<unsigned short int> SdList = eyeRecData.GetFRecShower().GetStationIds();
      fHotStationId = SdList[0];
    }

    ++numEyes;
  }


  if (numEyes < 2 ) {
    cout<<"\n";
    INFO("It is not a stereo event. The stereo reconstruction will not do anything");
    cout<<"\n";
    return eSuccess;
  }


  //------------------------------------------------
   // FindAxisStereo: It is a Multi-Eye geometry reconstruction
  // routine. Find the best Shower axis that fits better the
  // SDP fit and time Fit of all mirrors.
  cout<<"Calling FindAxisStereo"<<"\n";
  FindAxisStereo(event);

  if ( fStereoHybrid == 1 ) {

    if (event.HasSEvent()) {

      cout<<"Trying to include the Hybrid Fit in the stereo reconstruction \n";
      FindAxisStereoHybrid(event);
    }
    else {
      cout<<"There isn't any station involved with this event \n";
      cout<<"the reconstruction is purely stereo \n";
    }

  }

  // Calculate axis and core parameters.
  for (eyeIter = fevent.EyesBegin(ComponentSelector::eHasData);
       eyeIter != fevent.EyesEnd(ComponentSelector::eHasData);
       ++eyeIter) {

    fevt::Eye& eye = *eyeIter;
    fevt::EyeRecData& eyeRecData = eye.GetRecData();

    //Checking that the Eye has enough pixels
    if (eyeRecData.GetNSDPPixels() <= 3)
      continue;

    CoordinateSystemPtr eyeCS =
      det::Detector::GetInstance().GetFDetector().GetEye(eye).GetEyeCoordinateSystem();

    fChi0 = eyeRecData.GetChiZero();
    fRp = eyeRecData.GetRp();
    fT0 =  eyeRecData.GetTZero();


    Vector sdp = eyeRecData.GetSDP();
    Vector EyeVertical(0,0,1,eyeCS);
    Vector horizontalInSDP = cross(sdp,EyeVertical); // horizontal
    horizontalInSDP.Normalize();


    Transformation rot(Transformation::Rotation(-fChi0, sdp, eyeCS));
    Vector axis(rot * horizontalInSDP);             // apply rotation
    axis.Normalize();

    Vector coreEyeVec = fRp / sin( kPi - fChi0) * horizontalInSDP;
    Point eyePosition =
      det::Detector::GetInstance().GetFDetector().GetEye(eye).GetPosition();
    Point core = eyePosition + coreEyeVec;

    if (!event.HasRecShower())
      event.MakeRecShower();
    ShowerRecData& recshower = event.GetRecShower();
    recshower.SetAxis(axis);
    recshower.SetCorePosition(core);

    if (!eyeRecData.HasFRecShower())
      eyeRecData.MakeFRecShower();
    eyeRecData.GetFRecShower().SetAxis(axis);
    eyeRecData.GetFRecShower().SetCorePosition(core);
    eyeRecData.GetFRecShower().SetCoreTime(eye.GetHeader().GetTimeStamp()-
                                           TimeInterval(1000*100*ns),
                                           fRp, fChi0, fT0);
    //--------------------------------------------
    // Also fill the TelescopeRecData with the same values
    // since this module isn't able to deal with compound eyes
    // that consist of telescopes at multiple different positions
    for (fevt::Eye::TelescopeIterator telIt = eye.TelescopesBegin(ComponentSelector::eHasData);
         telIt != eye.TelescopesEnd(ComponentSelector::eHasData); ++telIt)
    {
      if (!telIt->HasRecData())
        telIt->MakeRecData();
      fevt::TelescopeRecData& telRecData = telIt->GetRecData();
      telRecData.SetSDP(eyeRecData.GetSDP());
      telRecData.SetSDPThetaError(eyeRecData.GetSDPThetaError());
      telRecData.SetSDPPhiError(eyeRecData.GetSDPPhiError());
      telRecData.SetSDPCorrThetaPhi(eyeRecData.GetSDPCorrThetaPhi());
      telRecData.SetSDPFitChiSquare(eyeRecData.GetSDPFitChiSquare(),
                                    eyeRecData.GetSDPFitNDof());

      telRecData.SetChiZero(eyeRecData.GetChiZero(), eyeRecData.GetChiZeroError());
      telRecData.SetTZero(eyeRecData.GetTZero(), eyeRecData.GetTZeroError());
      telRecData.SetRp(eyeRecData.GetRp(), eyeRecData.GetRpError());
      telRecData.SetTimeFitChiSquare(eyeRecData.GetTimeFitChiSquare(),
                                     eyeRecData.GetTimeFitNDof());
      telRecData.SetTimeFitCorrelations(eyeRecData.GetRpTZeroCorrelation(),
                                        eyeRecData.GetRpChi0Correlation(),
                                        eyeRecData.GetChi0TZeroCorrelation());
    }

  } // end for eyes
  return eSuccess;
}


void
StereoGeometryFinder::FindAxisStereo(evt::Event& event)
{
  fevt::FEvent& fevent = event.GetFEvent();
  fevt::FEvent::EyeIterator eyeIter;

  const fdet::FDetector& fdetector = det::Detector::GetInstance().GetFDetector();
  const unsigned int nEyes = fdetector.GetLastEyeId();

  CoordinateSystemPtr CS =
    det::Detector::GetInstance().GetSiteCoordinateSystem();

  const ReferenceEllipsoid& e = ReferenceEllipsoid::GetWGS84();

  vector<Vector> sdp(nEyes);
  vector<Vector> horizontal(nEyes);
  vector<Vector> EyeToCore(nEyes);
  vector<Point> eyePosition(nEyes);
  vector<double> coreDistance(nEyes);
  vector<double> EndFADCTrace(nEyes);
  vector<Point> core(nEyes);
  int count = 0;

  // Loading To from the first Eye.
  for (fevt::FEvent::EyeIterator eyeIter = fevent.EyesBegin(ComponentSelector::eHasData);
       eyeIter != fevent.EyesEnd(ComponentSelector::eHasData);
       ++eyeIter) {

    fevt::Eye& eye = *eyeIter;
    fevt::EyeRecData& eyerec = eye.GetRecData();

    //Checking that the Eye has enough pixels
    if (eyerec.GetNSDPPixels() <= 3)
      continue;

    CoordinateSystemPtr eyeCS =
      det::Detector::GetInstance().GetFDetector().GetEye(eye).GetEyeCoordinateSystem();

    fTZero = eyerec.GetTZero();
    fChi0  = eyerec.GetChiZero();
    fRp    = eyerec.GetRp();
    Vector sdp = eyerec.GetSDP();
    Vector EyeVertical(0,0,1,eyeCS);
    Vector horizontalInSDP = cross(sdp,EyeVertical); // horizontal

    Transformation rot(Transformation::Rotation(-fChi0, sdp, eyeCS));
    Vector axis(rot * horizontalInSDP);             // apply rotation
    axis.Normalize();
    Vector coreEyeVec = fRp / sin( kPi - fChi0) * horizontalInSDP;

    if (coreEyeVec.GetMag() > 100000 )
      coreEyeVec = 5000 * horizontalInSDP;

    Point eyePosition =
      det::Detector::GetInstance().GetFDetector().GetEye(eye).GetPosition();
    Point core = eyePosition + coreEyeVec;
    CoordinateSystemPtr CS =
      det::Detector::GetInstance().GetSiteCoordinateSystem();

    //vertical distance from the core at 1400m altitute.
    UTMPoint UTMCore(core,e);
    double deltaH =  UTMCore.GetHeight() - 1400;
    Vector AxisPointingUp = axis;
    if ( cos( axis.GetTheta(eyeCS) ) < 0 )
      AxisPointingUp *= -1;
    //distance along the shower axis
    double deltaLength =  deltaH /cos(AxisPointingUp.GetTheta(eyeCS));

    Point PointCore1400 = core - deltaLength * AxisPointingUp;
    UTMPoint UTMCore1400(PointCore1400,e);

    CoordinateSystemPtr core1CS = fwk::LocalCoordinateSystem::Create(core);

    fNorthing = UTMCore1400.GetNorthing(); //first guest
    fEasting = UTMCore1400.GetEasting();;  //first guest
    fAxisTheta = axis.GetTheta(core1CS);    //First guess
    fAxisPhi = axis.GetPhi(core1CS);

    break;
  }

  //-------------------------------------
  const int npar = 5; // number of parameters


  TMinuit theMinuit(npar); // 5 parameter fit

  theMinuit.SetPrintLevel(-1); // minuit verbosity

  theMinuit.SetFCN(StereoGeometryFinder::MinuitFitFuncStereo);

  double arglist[npar];  // arguments to pass to the Minuit interpreter
  int ierflag =0;

  arglist[0]=1; // [up]
  arglist[1]=1; //  unused
  theMinuit.mnexcm("SET ERR", arglist,1,ierflag);

  if (ierflag)
    ERROR("Minuit SET ERR failed");

  static double vstart[npar]; // inital point
  static double step[npar];   // step size

  // To is relative to the first Eye.

  // stereoAxis should point up.
  vstart[0] = fNorthing; //Northing
  vstart[1] = fEasting; //Easting
  vstart[2] = fAxisTheta;//axis Theta
  vstart[3] = fAxisPhi; // axis Phi
  vstart[4] = fTZero; //T0 (from FdAxisFinder)

  step[0] = 10;
  step[1] = 10;
  step[2] = 0.1;
  step[3] = 0.1;
  step[4] = 10.0;

  theMinuit.mnparm(0,"Northing ",vstart[0], step[0],0,0,ierflag);
  theMinuit.mnparm(1,"Easting ",  vstart[1], step[1],0,0,ierflag);
  theMinuit.mnparm(2,"Theta ",  vstart[2], step[2],0,0,ierflag);
  theMinuit.mnparm(3,"Phi ",  vstart[3], step[3],0,0,ierflag);
  theMinuit.mnparm(4,"T01 ",  vstart[4], step[4],0,0,ierflag);

  arglist[0]=1500;  // [maxcalls]
  arglist[1]=0.01;    // [tolerance]
  theMinuit.mnexcm("MIGRAD", arglist ,2,ierflag);

  /*
    // error contours
  //---------------------------------------
  //This is to print the contour points of two of the parameters
  int npoints = 20;
  double xptu[npoints];
  double yptu[npoints];
  theMinuit->mncont(0,1,npoints,xptu,yptu,ierflag);
  for (int i = 0 ; i<npoints ; i++)
    cout<<xptu[i]/degree<<" "<<yptu[i]/degree<<"\n";
  //------------------
  */

  if (ierflag)
    ERROR("Minuit MIGRAD failed");

  double Theta = 0;
  double Theta_err = 0;
  double Phi = 0;
  double Phi_err = 0;
  double northing = 0;
  double northing_err = 0;
  double easting = 0;
  double easting_err = 0;
  double To = 0;
  double To_err = 0;


  theMinuit.GetParameter(0,northing,northing_err);
  theMinuit.GetParameter(1,easting,easting_err);
  theMinuit.GetParameter(2,Theta,Theta_err);
  theMinuit.GetParameter(3,Phi,Phi_err);
  theMinuit.GetParameter(4,To,To_err);


  if (northing != northing) {
    northing=fNorthing;
    easting=fEasting;
    Theta=fAxisTheta;
    Phi=fAxisPhi;
    To=fTZero;

    cout<<"=======  FAILED STEREO RECONSTRUCTION =====\n";
  }


  cout<<"\n";
  cout<<"After Pure Stereo: \n";
  printf( "core UTM(E,N,H)         : %9.1f , %10.1f , 1400 [m] \n",northing,easting);
  printf("Axis(Theta,Phi)(coreCS)  : %5.2f , %6.2f [deg] \n",Theta / degree, Phi /degree);
  cout<<"\n";

  fNorthing = northing;
  fEasting = easting;
  fAxisTheta = Theta;
  fAxisPhi=  Phi;
  fTZero = To;


  //----------------
  UTMPoint UTMcore(northing,easting,1400,19,'H',e);
  Point core_1400 = UTMcore.GetPoint(CS);
  CoordinateSystemPtr coreCS = fwk::LocalCoordinateSystem::Create(core_1400);

  Vector stereoAxis(1,Theta,Phi,coreCS, Vector::kSpherical);

  bool IsUpGoing = false;
  if ( cos( stereoAxis.GetTheta(CS) ) < 0 )
    IsUpGoing = true;

  //-----------------------------
  //Estimating the core location (at the Eye level), the horizontal
  // and the SDP normal for the Eyes involved in the event
  //-----------------------------
  count = 0;
  for (eyeIter = fevent.EyesBegin(ComponentSelector::eHasData);
       eyeIter != fevent.EyesEnd(ComponentSelector::eHasData);
       ++eyeIter) {

    const fevt::Eye& eye = *eyeIter;
    const fevt::EyeRecData& eyeRecData = eye.GetRecData();

    //Checking that the Eye has enough pixels
    if (eyeRecData.GetNSDPPixels() <= 3)
      continue;

    CoordinateSystemPtr eyeCS =
      det::Detector::GetInstance().GetFDetector().GetEye(eye).GetEyeCoordinateSystem();

    eyePosition[count] =
      det::Detector::GetInstance().GetFDetector().GetEye(eye).GetPosition();

    //vertical distance from the core at 1400m altitute.
    double deltaH = core_1400.GetZ(eyeCS);
    Vector AxisPointingUp = stereoAxis;
    if( IsUpGoing )
      AxisPointingUp *= -1;
    //distance along the shower axis
    double deltaLength =  deltaH /cos(AxisPointingUp.GetTheta(eyeCS));

    core[count] = core_1400 - deltaLength *  AxisPointingUp ;
    EyeToCore[count] = core[count] - eyePosition[count];
    coreDistance[count] = EyeToCore[count].GetR(eyeCS);
    EyeToCore[count] /= EyeToCore[count].GetR(eyeCS);
    sdp[count] = cross(stereoAxis,EyeToCore[count]);
    sdp[count] /= sdp[count].GetR(eyeCS);
    Vector EyeVertical(0,0,1,eyeCS);
    horizontal[count] = cross(sdp[count],EyeVertical);
    horizontal[count] /= horizontal[count].GetR(eyeCS);

    //    eyeId[count] = eye.GetId(); // unused

    utl::TimeStamp time = eye.GetHeader().GetTimeStamp();
    EndFADCTrace[count] = double (time.GetGPSNanoSecond());

    ++count;
  }

  // Setting the Multi-Eye geometry parameters for all Eyes involved
  count = 0;

  for (fevt::FEvent::EyeIterator eyeIter = fevent.EyesBegin(ComponentSelector::eHasData);
       eyeIter != fevent.EyesEnd(ComponentSelector::eHasData);
       ++eyeIter) {

    fevt::Eye& eye = *eyeIter;
    fevt::EyeRecData& eyerec = eye.GetRecData();


    //Checking that the Eye has enough pixels
    if (eyerec.GetNSDPPixels() <= 3)
      continue;

    CoordinateSystemPtr eyeCS = fdetector.GetEye(eye).GetEyeCoordinateSystem();
    CoordinateSystemPtr eye2CS = fdetector.GetEye(eye).GetLocalCoordinateSystem();

    if (fDebugging == 1)
      cout << "Eye: " << eye.GetId()
           << " core (x,y,z): (" << core[count].GetX(eye2CS)
           << "," << core[count].GetY(eye2CS)
           << "," << core[count].GetZ(eye2CS) << ")\n";

    double chi_0 = Angle(horizontal[count],stereoAxis);
    if (IsUpGoing)
      chi_0 *= -1;
    double r_p   = coreDistance[count] * sin(kPi - chi_0);
   if (IsUpGoing)
      r_p *= -1;

    fChi0 = chi_0;
    fRp = r_p;
    double deltaT;
    //-------------------------------------
    //---  T_zero corresponds to To of the first Eye (count=0) -----
    //----------- Estimating To for the corresponding Eye ----
    Point PointRp;
    Point PointRp_eye0;
    double chi_eye0 = Angle(horizontal[0],stereoAxis);
    if (IsUpGoing)
      chi_eye0 *= -1;
    double Rp_magnitude_eye0 = coreDistance[0] * sin(chi_eye0);
    Vector Rp_unit_eye0 = cross(sdp[0],stereoAxis); // Check this definition
    if (IsUpGoing)
      Rp_unit_eye0 *= -1;
    Rp_unit_eye0 /= Rp_unit_eye0.GetR(CS);
    PointRp_eye0 = eyePosition[0] + Rp_magnitude_eye0*Rp_unit_eye0;
    double chi_00 = Angle(horizontal[count],stereoAxis);
    if (IsUpGoing)
      chi_00 *= -1;
    double Rp_magnitude = coreDistance[count] * sin(chi_00);
    Vector Rp_unit = cross(sdp[count],stereoAxis);
    if (IsUpGoing)
      Rp_unit *= -1;
    Rp_unit /= Rp_unit.GetR(CS);
    PointRp = eyePosition[count] + Rp_magnitude*Rp_unit;
    Vector delta_distance = PointRp - PointRp_eye0;
    deltaT =  delta_distance.GetR(CS) / kSpeedOfLight;

    if (PointRp.GetZ(CS) > PointRp_eye0.GetZ(CS))// asuming a downgoing shower
      deltaT *= -1;
    if ( IsUpGoing)// correting for upgoing showers
      deltaT *= -1;

    double T_zero = To + deltaT;
    T_zero += (EndFADCTrace[0] - EndFADCTrace[count]);

    eyerec.SetTZero(T_zero,To_err);
    eyerec.SetSDP(sdp[count]);

    //This variables are not estimated in the
    //current Multi_Eye reconstruction
    fChi0Error = 0;
    fRpError = 0;
    //-----------------------
    eyerec.SetChiZero(fChi0,fChi0Error);
    eyerec.SetRp(fRp,fRpError);

    //Setting chi_i for each pixel

    fevt::EyeRecData& recdata = eye.GetRecData();

    for (EyeRecData::PixelIterator iter = recdata.PulsedPixelsBegin();
         iter != recdata.PulsedPixelsEnd(); ++iter) {

      fevt::Pixel& evtPixel = *iter;
      const fdet::Pixel& detPixel = fdetector.GetPixel(evtPixel);
      Vector pixeldir = detPixel.GetDirection();
      // subtract component parallel to sdp
      Vector chi_i_vector = pixeldir - (sdp[count] * pixeldir) *  sdp[count];
      chi_i_vector /= chi_i_vector.GetR(eyeCS);

      double chi_i = Angle(chi_i_vector,horizontal[count]);

      evtPixel.GetRecData().SetChi_i(chi_i);


    } //End Setting chi_i

    ++count;
  }
}


void
StereoGeometryFinder::FindAxisStereoHybrid(evt::Event& event)
{
  fevt::FEvent& fevent = event.GetFEvent();
  fevt::FEvent::EyeIterator eyeIter;

  const fdet::FDetector& fdetector = det::Detector::GetInstance().GetFDetector();
  const unsigned int nEyes = fdetector.GetLastEyeId();

  CoordinateSystemPtr CS =
    det::Detector::GetInstance().GetSiteCoordinateSystem();

  const ReferenceEllipsoid& e = ReferenceEllipsoid::GetWGS84();

  vector<Vector> sdp(nEyes);
  vector<Vector> horizontal(nEyes);
  vector<Vector> EyeToCore(nEyes);
  vector<Point> eyePosition(nEyes);
  //  int eyeId(nEyes); // unused
  vector<double> coreDistance(nEyes);
  vector<double> EndFADCTrace(nEyes);
  vector<Point> core(nEyes);
  int count = 0;


  const int npar = 5; // number of parameters


  TMinuit theMinuit(npar); // 5 parameter fit

  theMinuit.SetPrintLevel(-1); // minuit verbosity

  theMinuit.SetFCN(StereoGeometryFinder::MinuitFitFuncStereoHybrid);

  double arglist[npar];  // arguments to pass to the Minuit interpreter
  int ierflag =0;

  arglist[0]=1; // [up]
  arglist[1]=1; //  unused
  theMinuit.mnexcm("SET ERR", arglist,1,ierflag);

  if (ierflag)
    ERROR("Minuit SET ERR failed");

  static double vstart[npar]; // inital point
  static double step[npar];   // step size

  // To is relative to the first Eye.

  // Loading first guest from pure Stereo reconstruction (No hybrid information)

  vstart[0] = fNorthing; //Northing
  vstart[1] = fEasting; //Easting
  vstart[2] = fAxisTheta;//axis Theta
  vstart[3] = fAxisPhi; // axis Phi
  vstart[4] = fTZero; //T0


  step[0] = 10;
  step[1] = 10;
  step[2] = 0.1;
  step[3] = 0.1;
  step[4] = 10.0;

  theMinuit.mnparm(0,"Northing ",vstart[0], step[0],0,0,ierflag);
  theMinuit.mnparm(1,"Easting ",  vstart[1], step[1],0,0,ierflag);
  theMinuit.mnparm(2,"Theta ",  vstart[2], step[2],0,0,ierflag);
  theMinuit.mnparm(3,"Phi ",  vstart[3], step[3],0,0,ierflag);
  theMinuit.mnparm(4,"T01 ",  vstart[4], step[4],0,0,ierflag);

  arglist[0]=1500;  // [maxcalls]
  arglist[1]=0.01;    // [tolerance]
  theMinuit.mnexcm("MIGRAD", arglist ,2,ierflag);

  /*
    // error contours
  //---------------------------------------
  //This is to print the contour points of two of the parameters
  int npoints = 20;
  double xptu[npoints];
  double yptu[npoints];
  theMinuit->mncont(0,1,npoints,xptu,yptu,ierflag);
  for (int i = 0 ; i<npoints ; i++)
    cout<<xptu[i]/degree<<" "<<yptu[i]/degree<<"\n";
  //------------------
  */

  if (ierflag)
    ERROR("Minuit MIGRAD failed");

  double Theta = 0;
  double Theta_err = 0;
  double Phi = 0;
  double Phi_err = 0;
  double northing = 0;
  double northing_err = 0;
  double easting = 0;
  double easting_err = 0;
  double To = 0;
  double To_err = 0;


  theMinuit.GetParameter(0,northing,northing_err);
  theMinuit.GetParameter(1,easting,easting_err);
  theMinuit.GetParameter(2,Theta,Theta_err);
  theMinuit.GetParameter(3,Phi,Phi_err);
  theMinuit.GetParameter(4,To,To_err);

  cout<<"\n";
  cout<<"After Stereo-hybrid: \n";
  printf( "core UTM(E,N,H)         : %9.1f , %10.1f , 1400 [m] \n",northing,easting);
  printf("Axis(Theta,Phi)(coreCS)  : %5.2f , %6.2f [deg] \n",Theta / degree, Phi /degree);

  cout<<"\n";

  if (northing != northing) {
    northing=fNorthing;
    easting=fEasting;
    Theta=fAxisTheta;
    Phi=fAxisPhi;
    To=fTZero;

    cout<<"=======  FAILED STEREO RECONSTRUCTION =====\n";
  }



  //----------------
  UTMPoint UTMcore(northing,easting,1400,19,'H',e);
  Point core_1400 = UTMcore.GetPoint(CS);
  CoordinateSystemPtr coreCS = fwk::LocalCoordinateSystem::Create(core_1400);

  Vector stereoAxis(1,Theta,Phi,coreCS, Vector::kSpherical);

  bool IsUpGoing = false;
  if ( cos( stereoAxis.GetTheta(CS) ) < 0 )
    IsUpGoing = true;

  //-----------------------------
  //Estimating the core location (at the Eye level), the horizontal
  // and the SDP normal for the Eyes involved in the event
  //-----------------------------
  count = 0;
  for (eyeIter = fevent.EyesBegin(ComponentSelector::eHasData);
       eyeIter != fevent.EyesEnd(ComponentSelector::eHasData);
       ++eyeIter) {

    const fevt::Eye& eye = *eyeIter;
    const fevt::EyeRecData& eyeRecData = eye.GetRecData();

    //Checking that the Eye has enough pixels
    if (eyeRecData.GetNSDPPixels() <= 3)
      continue;

    CoordinateSystemPtr eyeCS =
      det::Detector::GetInstance().GetFDetector().GetEye(eye).GetEyeCoordinateSystem();

    eyePosition[count] =
      det::Detector::GetInstance().GetFDetector().GetEye(eye).GetPosition();

    //vertical distance from the core at 1400m altitute.
    double deltaH = core_1400.GetZ(eyeCS);
    Vector AxisPointingUp = stereoAxis;
    if( IsUpGoing )
      AxisPointingUp *= -1;
    //distance along the shower axis
    double deltaLength =  deltaH /cos( AxisPointingUp.GetTheta(eyeCS));

    core[count] = core_1400 - deltaLength *  AxisPointingUp ;
    EyeToCore[count] = core[count] - eyePosition[count];
    coreDistance[count] = EyeToCore[count].GetR(eyeCS);
    EyeToCore[count] /= EyeToCore[count].GetR(eyeCS);
    sdp[count] = cross(stereoAxis,EyeToCore[count]);
    sdp[count] /= sdp[count].GetR(eyeCS);
    Vector EyeVertical(0,0,1,eyeCS);
    horizontal[count] = cross(sdp[count],EyeVertical);
    horizontal[count] /= horizontal[count].GetR(eyeCS);


    //    eyeId[count] = eye.GetId(); // unused

    utl::TimeStamp time = eye.GetHeader().GetTimeStamp();
    EndFADCTrace[count] = double (time.GetGPSNanoSecond());

    ++count;
  }

  // Setting the Multi-Eye geometry parameters for all Eyes involved
  count = 0;

  for (fevt::FEvent::EyeIterator eyeIter = fevent.EyesBegin(ComponentSelector::eHasData);
       eyeIter != fevent.EyesEnd(ComponentSelector::eHasData);
       ++eyeIter) {

    fevt::Eye& eye = *eyeIter;
    fevt::EyeRecData& eyerec = eye.GetRecData();


    //Checking that the Eye has enough pixels
    if (eyerec.GetNSDPPixels() <= 3)
      continue;

    CoordinateSystemPtr eyeCS = fdetector.GetEye(eye).GetEyeCoordinateSystem();
    CoordinateSystemPtr eye2CS = fdetector.GetEye(eye).GetLocalCoordinateSystem();

    if (fDebugging == 1)
      cout << "Eye: " << eye.GetId()
           << " core (x,y,z): (" << core[count].GetX(eye2CS)
           << "," << core[count].GetY(eye2CS)
           << "," << core[count].GetZ(eye2CS) << ")\n";

    double chi_0 = Angle(horizontal[count],stereoAxis);
    if (IsUpGoing)
      chi_0 *= -1;
    double r_p   = coreDistance[count] * sin(kPi - chi_0);
    if (IsUpGoing)
      r_p *= -1;

    fChi0 = chi_0;
    fRp = r_p;
    double deltaT;
    //-------------------------------------
    //---  T_zero corresponds to To of the first Eye (count=0) -----
    //----------- Estimating To for the corresponding Eye ----
    Point PointRp;
    Point PointRp_eye0;
    double chi_eye0 = Angle(horizontal[0],stereoAxis);
    if (IsUpGoing)
      chi_eye0 *= -1;
    double Rp_magnitude_eye0 = coreDistance[0] * sin(chi_eye0);
    Vector Rp_unit_eye0 = cross(sdp[0],stereoAxis); // Check this definition
    if (IsUpGoing)
      Rp_unit_eye0 *= -1;
    Rp_unit_eye0 /= Rp_unit_eye0.GetR(CS);
    PointRp_eye0 = eyePosition[0] + Rp_magnitude_eye0*Rp_unit_eye0;
    double chi_00 = Angle(horizontal[count],stereoAxis);
    if (IsUpGoing)
      chi_00 *= -1;
    double Rp_magnitude = coreDistance[count] * sin(chi_00);
    Vector Rp_unit = cross(sdp[count],stereoAxis);
    if (IsUpGoing)
      Rp_unit *= -1;
    Rp_unit /= Rp_unit.GetR(CS);
    PointRp = eyePosition[count] + Rp_magnitude*Rp_unit;
    Vector delta_distance = PointRp - PointRp_eye0;
    deltaT =  delta_distance.GetR(CS) / kSpeedOfLight;

    if (PointRp.GetZ(CS) > PointRp_eye0.GetZ(CS))// asuming a downgoing shower
      deltaT *= -1;
    if ( IsUpGoing)// correting for upgoing showers
      deltaT *= -1;

    double T_zero = To + deltaT;
    T_zero += (EndFADCTrace[0] - EndFADCTrace[count]);


    eyerec.SetTZero(T_zero,To_err);
    eyerec.SetSDP(sdp[count]);

    //This variables are not estimated in the
    //current Multi_Eye reconstruction
    fChi0Error = 0;
    fRpError = 0;
    //-----------------------
    eyerec.SetChiZero(fChi0,fChi0Error);
    eyerec.SetRp(fRp,fRpError);

    //Setting chi_i for each pixel

    fevt::EyeRecData& recdata = eye.GetRecData();

    for (EyeRecData::PixelIterator iter = recdata.PulsedPixelsBegin();
         iter != recdata.PulsedPixelsEnd(); ++iter) {

      fevt::Pixel& evtPixel = *iter;
      const fdet::Pixel& detPixel = fdetector.GetPixel(evtPixel);
      Vector pixeldir = detPixel.GetDirection();
      // subtract component parallel to sdp
      Vector chi_i_vector = pixeldir - (sdp[count] * pixeldir) *  sdp[count];
      chi_i_vector /= chi_i_vector.GetR(eyeCS);

      double chi_i = Angle(chi_i_vector,horizontal[count]);
      evtPixel.GetRecData().SetChi_i(chi_i);

    } //End Setting chi_i

    ++count;
  }
}


void
StereoGeometryFinder::MinuitFitFuncStereoHybrid(
  int& /*npar*/, double* /*gin*/, double& f, double* par, int /*iflag*/)
{
  fevt::FEvent&  fevent =  fCurEvent->GetFEvent();
  fevt::FEvent::EyeIterator eyeIter;

  const fdet::FDetector& fdetector = det::Detector::GetInstance().GetFDetector();
  const unsigned int nEyes = fdetector.GetLastEyeId();

  CoordinateSystemPtr CS =
    det::Detector::GetInstance().GetSiteCoordinateSystem();

  //--------------------------
  const ReferenceEllipsoid& e = ReferenceEllipsoid::GetWGS84();

  // This is to avoid having crazy core first guess values
  double R = sqrt((par[0]-6095767)*(par[0]-6095767) + (par[1]-469363)*(par[1]-469363));
  if( R > 60000) {
    par[0] = 6095767;
    par[1] = 469363;
  }

  UTMPoint UTMcore(par[0],par[1],1400,19,'H',e);
  Point core_1400 = UTMcore.GetPoint(CS);
  CoordinateSystemPtr coreCS = fwk::LocalCoordinateSystem::Create(core_1400);
  Vector stereoAxis(1,par[2],par[3],coreCS, Vector::kSpherical);

  bool IsUpGoing = false;
  if ( cos( stereoAxis.GetTheta(CS) ) < 0 )
    IsUpGoing = true;


  vector<Vector> sdp(nEyes);
  vector<Vector> horizontal(nEyes);
  vector<Point> eyePosition(nEyes);
  vector<int> eyeId(nEyes);
  vector<double> coreDistance(nEyes);
  vector<double> EndFADCTrace(nEyes);
  vector<Point> core(nEyes);
  int count = 0;

  utl::TimeStamp time0;
 //-------------------------------------------------------------
  //Estimating the core location (at the Eye level), the horizontal
  // and the SDP normal for the Eyes involved in the event

  for (eyeIter = fevent.EyesBegin(ComponentSelector::eHasData);
       eyeIter != fevent.EyesEnd(ComponentSelector::eHasData);
       ++eyeIter) {

    const fevt::Eye& eye = *eyeIter;

    CoordinateSystemPtr eyeCS =
      det::Detector::GetInstance().GetFDetector().GetEye(eye).GetEyeCoordinateSystem();

    eyePosition[count] =
      det::Detector::GetInstance().GetFDetector().GetEye(eye).GetPosition();

    //vertical distance from the core at 1400m altitute.
    double deltaH = core_1400.GetZ(eyeCS);
    Vector AxisPointingUp = stereoAxis;
    if( IsUpGoing )
      AxisPointingUp *= -1;
    //distance along the shower axis
    double deltaLength =  deltaH / cos(AxisPointingUp.GetTheta(eyeCS)) ;

    core[count] = core_1400 - deltaLength *  AxisPointingUp ;
    horizontal[count] = core[count] - eyePosition[count];

    coreDistance[count] = horizontal[count].GetR(eyeCS);
    horizontal[count] /= horizontal[count].GetR(eyeCS);

    sdp[count] = cross(stereoAxis,horizontal[count]);
    sdp[count] /= sdp[count].GetR(eyeCS);

    eyeId[count] = eye.GetId();

    utl::TimeStamp time = eye.GetHeader().GetTimeStamp();
    if (count == 0 )
      time0 = time;
    EndFADCTrace[count] = double (time.GetGPSNanoSecond());

    ++count;
  }
  //--------------------------

  //---------------------------------------------------
  // Starting with the Multiple Eye Fit
  count = 0;
  double chisq = 0;

  // The To used in the stereo fit corresponds to the first eye.
  // We are loading the first eye geometry information, So that
  // we can estimate the To corresponding to the other eyes.
  double chi_eye0 =  Angle(horizontal[0],stereoAxis);
  if (IsUpGoing)
    chi_eye0 *= -1;
  double Rp_magnitude_eye0 = coreDistance[0] * sin(chi_eye0);
  Vector Rp_unit_eye0 = cross(sdp[0],stereoAxis);
  if (IsUpGoing)
    Rp_unit_eye0 *= -1;
  Rp_unit_eye0 /= Rp_unit_eye0.GetR(CS);
  Point PointRp_eye0;
  PointRp_eye0 = eyePosition[0] + Rp_magnitude_eye0*Rp_unit_eye0;

  UTMPoint UTM_Rp_Eye0(PointRp_eye0,e);
  double Rp0_height = UTM_Rp_Eye0.GetHeight();
  double DeltaHeight =  Rp0_height - 1400;
  double Delta_L = DeltaHeight / cos(stereoAxis.GetTheta(coreCS));
  double tcore_1400 = par[4] + Delta_L / kSpeedOfLight;
  //------------------------------------------------------------
  // Initializing some parameters
  vector<double> chisq_time(nEyes, 0);
  vector<int> dof(nEyes, 0);
  double sum_charge = 0;
  //-------------------------------
  //Begin Time Fit

  for (eyeIter = fevent.EyesBegin(ComponentSelector::eHasData);
       eyeIter != fevent.EyesEnd(ComponentSelector::eHasData);
       ++eyeIter) {

    sum_charge = 0;

    const fevt::Eye& eye = *eyeIter;
    const fevt::EyeRecData& recdata = eye.GetRecData();

    //Checking that the Eye has enough pixels
    if (recdata.GetNSDPPixels() <= 3 )
      continue;

    CoordinateSystemPtr eyeCS = fdetector.GetEye(eye).GetEyeCoordinateSystem();


    double chi_0 = Angle(horizontal[count],stereoAxis);
    if (IsUpGoing)
      chi_0 *= -1;
    double r_p   = coreDistance[count] * sin(kPi - chi_0);


    //Estimating To for the corresponding Eye
    double deltaT;
    Point PointRp;


    double Rp_magnitude = coreDistance[count] * sin(chi_0);
    Vector Rp_unit = cross(sdp[count],stereoAxis);
    if (IsUpGoing)
      Rp_unit *= -1;
    Rp_unit /= Rp_unit.GetR(CS);
    PointRp = eyePosition[count] + Rp_magnitude*Rp_unit;
    Vector delta_distance = PointRp - PointRp_eye0;
    deltaT = delta_distance.GetR(CS) / kSpeedOfLight;


    if (PointRp.GetZ(CS)  > PointRp_eye0.GetZ(CS))//asuming a downgoing shower
      deltaT *= -1;
    if ( IsUpGoing ) // correting for upgoing showers
      deltaT *= -1;

    double t_0   = par[4] + deltaT ;
    t_0 += (EndFADCTrace[0] - EndFADCTrace[count]);
    //-----------------------------------------
    EyeRecData::ConstPixelIterator iter;
    for (iter= recdata.TimeFitPixelsBegin();
         iter!=recdata.TimeFitPixelsEnd(); ++iter) {

      const Pixel& pixel = *iter;
      const PixelRecData& pixrec = pixel.GetRecData();

      double t_i       = pixrec.GetT_i().GetNanoSecond();
      double t_i_Err   = pixrec.GetT_iError().GetInterval();
      double chi_i     = pixrec.GetChi_i();
      //double chi_i_Err = 0;

      //Re-estimate chi_i according to the current SDP
      const fevt::Pixel& evtPixel = *iter;
      const fdet::Pixel& detPixel = fdetector.GetPixel(evtPixel);
      Vector pixeldir = detPixel.GetDirection();
      pixeldir.TransformTo(eyeCS);
      sdp[count].TransformTo(eyeCS);
      // subtract component parallel to sdp
      Vector chi_i_vector = pixeldir - (sdp[count] * pixeldir) * sdp[count];
      chi_i_vector.TransformTo(eyeCS);
      chi_i_vector /= chi_i_vector.GetR(eyeCS);
      horizontal[count].TransformTo(eyeCS);
      chi_i = Angle(chi_i_vector,horizontal[count]);
      //End Re-estimating chi_i

      double t_expected =
        t_0 + r_p/kSpeedOfLight *  std::tan(0.5 * (chi_0 - chi_i));

      double tmp = t_i - t_expected ;

      // weighting according to the pixel charge

      double charge = pixrec.GetTotalCharge();
      charge = 1;

      chisq_time[count] += tmp*tmp*charge/ (t_i_Err * t_i_Err) ;
      sum_charge += charge;

      if (fDebugging == 1)
        cout << "delta t: " << tmp
             << " chisq   " << tmp*tmp / (t_i_Err * t_i_Err)
             << "  Eye:   " << eye.GetId()
             << "  Chi0 : " << chi_0/degree
             << "   Rp:   " << r_p
             << "   T0:   " <<  t_0
             << "chisq_time: " << chisq_time[count]
             << " " << count
             << endl;
    }

    dof[count] = recdata.GetNTimeFitPixels();
    chisq_time[count] /= sum_charge;
    chisq_time[count] *= recdata.GetNTimeFitPixels();
    ++count;
  }
  // end Time fit
  //---------------------------
  //Begin SDP fit
  vector<double> chisq_sdp(nEyes, 0);

  sum_charge = 0;
  count = 0;

  for (eyeIter = fevent.EyesBegin(ComponentSelector::eHasData);
       eyeIter != fevent.EyesEnd(ComponentSelector::eHasData);
       ++eyeIter) {

    const fevt::Eye& eye = *eyeIter;
    const fevt::EyeRecData& recdata = eye.GetRecData();

    //Checking that the Eye has enough pixels
    if (recdata.GetNSDPPixels() <= 3)
      continue;

    EyeRecData::ConstPixelIterator pixeliter;

    const fdet::Eye& deteye = fdetector.GetEye(eye);

    // use the eye coordinate system.
    // CS refer to the eye CS.
    CoordinateSystemPtr eyeCS= deteye.GetEyeCoordinateSystem();

    sdp[count].TransformTo(eyeCS);

    double sdpTheta = sdp[count].GetTheta(eyeCS);
    double sdpPhi   = sdp[count].GetPhi(eyeCS);

    Vector sdp2 (1,sdpTheta ,sdpPhi ,eyeCS, Vector::kSpherical);

    // iterate over pixels
    for (pixeliter = recdata.TimeFitPixelsBegin();
         pixeliter != recdata.TimeFitPixelsEnd();
         ++pixeliter) {

      const fevt::Pixel& evtPixel = *pixeliter;
      const fdet::Pixel& detPixel = fdetector.GetPixel(evtPixel);

      Vector dir = detPixel.GetDirection();
      dir.TransformTo(eyeCS);

      // distance from trial plane
      double tmp = kPiOnTwo - Angle(dir, sdp2);

      if (fDebugging == 1)
        cout << "deviation from SDP " << tmp /degree
             << " [deg] , Eye: " << eye.GetId()
             << " Pixel Id : " << detPixel.GetId()
             << endl;

      // weight to assign
      double charge = evtPixel.GetRecData().GetTotalCharge();

      chisq_sdp[count] += tmp*tmp  * charge / ((0.35 * degree * 0.35 * degree));
      sum_charge += charge;
    }

    chisq_sdp[count] /= sum_charge;
    chisq_sdp[count] *= dof[count];
    ++count;
  }
  // End SDP Fit
  //---------------------
  //Start of the Hybrid fit

  double chisq_hybrid = 0;

  sevt::SEvent& sevent = fCurEvent->GetSEvent();
  SEvent::StationIterator sditer;
  for(sditer=sevent.StationsBegin(); sditer != sevent.StationsEnd(); ++sditer)
    {

      if (!sditer->HasRecData()) {
        continue;
      }
      sevt::Station& station=*sditer;

      if (station.GetId() !=  fHotStationId )
        continue;


      double nanosecond = station.GetRecData().GetSignalStartTime().GetGPSNanoSecond();



      const sdet::Station& detstation =
        det::Detector::GetInstance().GetSDetector().GetStation(station);
      const Point& stationpos=detstation.GetPosition();
      const utl::Point&  pos = stationpos ;

      const Vector VCoreToStation = pos - core_1400;
      //const double CoreToStation = VCoreToStation.GetMag();
      const double Axis_VS_CoreToStation_angle = Angle(VCoreToStation,stereoAxis);

      const double station_to_axis =  VCoreToStation.GetMag() * sin(Axis_VS_CoreToStation_angle);


      //-----------------------------
      // Expected time at the station
      const double delta_d = VCoreToStation.GetMag() * cos(Axis_VS_CoreToStation_angle);

      double exp_t_st = tcore_1400 - delta_d/kSpeedOfLight;
      // time delay due to the shower front curvature
      // do not apply shower front corrrection for up-going events
      if (!IsUpGoing) {
        const double VerticalCurvature = 1 / 9000.;
        const double d2 = station_to_axis * station_to_axis;
        const double curvature = VerticalCurvature*stereoAxis.GetCosTheta(coreCS);
        const double timeDelay = curvature * d2 / (2*kSpeedOfLight);
        exp_t_st += timeDelay;
      }

      // Hybrid upgoing events involving station 203 (Celeste)
      // are  very likely to be laser shots
      // 180 ns is the fiber optic propagation time
      if (IsUpGoing && station.GetId() == 203) exp_t_st += fCelesteDelay;

      // The 'exp_t_st' is relative to the trigger time of the first Eye
      //-----------------------------
      double eye_trigger_time = double(time0.GetGPSNanoSecond());
      // double(time0.GetGPSNanoSecond()) gives the time at the end of the FD FADC trace from the first Eye.
      // by substracting 1000 *100 ns we get the start time of  the first Eye FD FADC trace.
      eye_trigger_time -= 1000 *100;
      // exp_t_st, is the expected trigger time of the station
      exp_t_st += eye_trigger_time;
      //Compare the expected with the observed station time
      double  chi_hybrid = nanosecond -  exp_t_st;
      double hybrid_error = 100;

      chisq_hybrid +=  chi_hybrid * chi_hybrid / (hybrid_error * hybrid_error);

    }

  //End hybrid chisqr

  //=============================
  //--------------------------------
  //Here we apply weights (if necesary) to the different
  //chisqr components
  count = 0;
  chisq = 0;
  for (eyeIter = fevent.EyesBegin(ComponentSelector::eHasData);
       eyeIter != fevent.EyesEnd(ComponentSelector::eHasData);
       ++eyeIter) {

    fevt::Eye& eye = *eyeIter;
    fevt::EyeRecData& recdata = eye.GetRecData();

    //Checking that the Eye has enough pixels
    if (recdata.GetNSDPPixels() <= 3)
      continue;


    //Storing the Stereo time and SDP chisqr values
    fNDofTimeFit = recdata.GetNTimeFitPixels() - 5 ;
    fNDofSDP = recdata.GetNTimeFitPixels() - 4 ;
    recdata.SetSDPFitChiSquare(chisq_sdp[count],fNDofSDP);
    recdata.SetTimeFitChiSquare(chisq_time[count],fNDofTimeFit);
    //-------------------------------------------------
    if (fDebugging == 1)
      cout << "chisq time: " << chisq_time[count]/dof[count]
           << " chisq SDP: " << chisq_sdp[count]/dof[count]
           << " dof " << dof[count]
           << endl;

    //----------------------------
    //NOTICE:
    // When the Eye's time offset is greater than ~100 ns is better to use
    // only one of the time chisq commponents. Otherwise it may find a
    // geometry that reduces the time offset, but having one or more
    // SDPs clearly off from the best SDP.

    if (eyeId[count] < 6 ) {  //Excluding the 6th eye from the Fit
      if (fExcludeEye.count(eyeId[count]) == 0)
        chisq += chisq_time[count];
      if (fExcludeSDPEye.count(eyeId[count]) == 0)
        chisq += chisq_sdp[count]*5; //higher weight to the SDP component
    }
    ++count;
  }
  chisq += chisq_hybrid;
  f = chisq;
}

void
StereoGeometryFinder::MinuitFitFuncStereo(
  int& /*npar*/, double* /*gin*/, double& f, double* par, int /*iflag*/)
{
  fevt::FEvent&  fevent =  fCurEvent->GetFEvent();
  fevt::FEvent::EyeIterator eyeIter;

  const fdet::FDetector& fdetector = det::Detector::GetInstance().GetFDetector();
  const unsigned int nEyes = fdetector.GetLastEyeId();

  CoordinateSystemPtr CS =
    det::Detector::GetInstance().GetSiteCoordinateSystem();

  //--------------------------
 const ReferenceEllipsoid& e = ReferenceEllipsoid::GetWGS84();

  UTMPoint UTMcore(par[0],par[1],1400,19,'H',e);
  Point core_1400 = UTMcore.GetPoint(CS);
  CoordinateSystemPtr coreCS = fwk::LocalCoordinateSystem::Create(core_1400);
  Vector stereoAxis(1,par[2],par[3],coreCS, Vector::kSpherical);

  bool IsUpGoing = false;
  if ( cos( stereoAxis.GetTheta(CS) ) < 0 )
    IsUpGoing = true;

  vector<Vector> sdp(nEyes);
  vector<Vector> horizontal(nEyes);
  vector<Point> eyePosition(nEyes);
  vector<int> eyeId(nEyes);
  vector<double> coreDistance(nEyes);
  vector<double> EndFADCTrace(nEyes);
  vector<Point> core(nEyes);
  int count = 0;

 //-------------------------------------------------------------
  //Estimating the core location (at the Eye level), the horizontal
  // and the SDP normal for the Eyes involved in the event


  for (eyeIter = fevent.EyesBegin(ComponentSelector::eHasData);
       eyeIter != fevent.EyesEnd(ComponentSelector::eHasData);
       ++eyeIter) {

    const fevt::Eye& eye = *eyeIter;

    CoordinateSystemPtr eyeCS =
      det::Detector::GetInstance().GetFDetector().GetEye(eye).GetEyeCoordinateSystem();

    eyePosition[count] =
      det::Detector::GetInstance().GetFDetector().GetEye(eye).GetPosition();

    //vertical distance from the core at 1400m altitute.
    double deltaH = core_1400.GetZ(eyeCS);
    Vector AxisPointingUp = stereoAxis;
    if( IsUpGoing )
      AxisPointingUp *= -1;
    //distance along the shower axis
    double deltaLength =  deltaH / cos( AxisPointingUp.GetTheta(eyeCS)) ;

    core[count] = core_1400 - deltaLength * AxisPointingUp ;
    horizontal[count] = core[count] - eyePosition[count];

    coreDistance[count] = horizontal[count].GetR(eyeCS);
    horizontal[count] /= horizontal[count].GetR(eyeCS);

    sdp[count] = cross(stereoAxis,horizontal[count]);
    sdp[count] /= sdp[count].GetR(eyeCS);

    eyeId[count] = eye.GetId();

    utl::TimeStamp time = eye.GetHeader().GetTimeStamp();
    EndFADCTrace[count] = double (time.GetGPSNanoSecond());

    ++count;
  }
  //--------------------------

  //---------------------------------------------------
  // Starting with the Multiple Eye Fit
  count = 0;
  double chisq = 0;

  // The To used in the stereo fit corresponds to the first eye.
  // We are loading the first eye geometry information, So that
  // we can estimate the To corresponding to the other eyes.
  double chi_eye0 =  Angle(horizontal[0],stereoAxis);
  if (IsUpGoing)
    chi_eye0 *= -1;
  double Rp_magnitude_eye0 = coreDistance[0] * sin(chi_eye0);
  Vector Rp_unit_eye0 = cross(sdp[0],stereoAxis);
  if (IsUpGoing)
    Rp_unit_eye0 *= -1;
  Rp_unit_eye0 /= Rp_unit_eye0.GetR(CS);
  Point PointRp_eye0;
  PointRp_eye0 = eyePosition[0] + Rp_magnitude_eye0*Rp_unit_eye0;

  //------------------------------------------------------------
  // Initializing some parameters
  double chisq_time[nEyes];
  int dof[nEyes];
  for (unsigned int i = 0; i < nEyes; i++) {
    chisq_time[i] = 0.;
    dof[i] = 0;
  }
  double sum_charge = 0;
  //-------------------------------
  //Begin Time Fit

  for (eyeIter = fevent.EyesBegin(ComponentSelector::eHasData);
       eyeIter != fevent.EyesEnd(ComponentSelector::eHasData);
       ++eyeIter) {

    sum_charge = 0;

    const fevt::Eye& eye = *eyeIter;
    const fevt::EyeRecData& recdata = eye.GetRecData();

    //Checking that the Eye has enough pixels
    if (recdata.GetNSDPPixels() <= 3 )
      continue;

    CoordinateSystemPtr eyeCS = fdetector.GetEye(eye).GetEyeCoordinateSystem();


    double chi_0 = Angle(horizontal[count],stereoAxis);
    if (IsUpGoing)
      chi_0 *= -1;
    double r_p   = coreDistance[count] * sin(kPi - chi_0);


    //Estimating To for the corresponding Eye
    double deltaT;
    Point PointRp;


    double Rp_magnitude = coreDistance[count] * sin(chi_0);
    Vector Rp_unit = cross(sdp[count],stereoAxis);
    if (IsUpGoing)
      Rp_unit *= -1;
    Rp_unit /= Rp_unit.GetR(CS);
    PointRp = eyePosition[count] + Rp_magnitude*Rp_unit;
    Vector delta_distance = PointRp - PointRp_eye0;
    deltaT = delta_distance.GetR(CS) / kSpeedOfLight;


    if (PointRp.GetZ(CS)  > PointRp_eye0.GetZ(CS))//asuming a downgoing shower
      deltaT *= -1;
    if ( IsUpGoing ) // correting for upgoing showers
      deltaT *= -1;

    double t_0   = par[4] + deltaT ;
    t_0 += (EndFADCTrace[0] - EndFADCTrace[count]);
    //-----------------------------------------
    EyeRecData::ConstPixelIterator iter;
    for (iter= recdata.TimeFitPixelsBegin();
         iter!=recdata.TimeFitPixelsEnd(); ++iter) {

      const Pixel& pixel = *iter;
      const PixelRecData& pixrec = pixel.GetRecData();

      double t_i       = pixrec.GetT_i().GetNanoSecond();
      double t_i_Err   = pixrec.GetT_iError().GetInterval();
      double chi_i     = pixrec.GetChi_i();
      //double chi_i_Err = 0;

      //Re-estimate chi_i according to the current SDP
      const fevt::Pixel& evtPixel = *iter;
      const fdet::Pixel& detPixel = fdetector.GetPixel(evtPixel);
      Vector pixeldir = detPixel.GetDirection();
      pixeldir.TransformTo(eyeCS);
      sdp[count].TransformTo(eyeCS);
      // subtract component parallel to sdp
      Vector chi_i_vector = pixeldir - (sdp[count] * pixeldir) * sdp[count];
      chi_i_vector.TransformTo(eyeCS);
      chi_i_vector /= chi_i_vector.GetR(eyeCS);
      horizontal[count].TransformTo(eyeCS);
      chi_i = Angle(chi_i_vector,horizontal[count]);
      //End Re-estimating chi_i

      double t_expected =
        t_0 + r_p/kSpeedOfLight *  std::tan(0.5 * (chi_0 - chi_i));

      double tmp = t_i - t_expected ;

      // weighting according to the pixel charge

      double charge = pixrec.GetTotalCharge();
      charge = 1;

      chisq_time[count] += tmp*tmp*charge/ (t_i_Err * t_i_Err) ;
      sum_charge += charge;

      if (fDebugging == 1)
        cout << "delta t: " << tmp
             << " chisq   " << tmp*tmp / (t_i_Err * t_i_Err)
             << "  Eye:   " << eye.GetId()
             << "  Chi0 : " << chi_0/degree
             << "   Rp:   " << r_p
             << "   T0:   " <<  t_0
             << "chisq_time: " << chisq_time[count]
             << " " << count
             << endl;
    }

    dof[count] = recdata.GetNTimeFitPixels();
    chisq_time[count] /= sum_charge;
    chisq_time[count] *= recdata.GetNTimeFitPixels();
    ++count;
  }
  // end Time fit
  //---------------------------
  //Begin SDP fit
  double chisq_sdp[nEyes];
  for (unsigned int i = 0; i < nEyes; i++)
    chisq_sdp[i] = 0.;

  sum_charge = 0;
  count = 0;

  for (eyeIter = fevent.EyesBegin(ComponentSelector::eHasData);
       eyeIter != fevent.EyesEnd(ComponentSelector::eHasData);
       ++eyeIter) {

    const fevt::Eye& eye = *eyeIter;
    const fevt::EyeRecData& recdata = eye.GetRecData();

    //Checking that the Eye has enough pixels
    if (recdata.GetNSDPPixels() <= 3)
      continue;

    EyeRecData::ConstPixelIterator pixeliter;

    const fdet::Eye& deteye = fdetector.GetEye(eye);

    // use the eye coordinate system.
    // CS refer to the eye CS.
    CoordinateSystemPtr CS= deteye.GetEyeCoordinateSystem();

    sdp[count].TransformTo(CS);

    double sdpTheta = sdp[count].GetTheta(CS);
    double sdpPhi   = sdp[count].GetPhi(CS);

    Vector sdp2 (1,sdpTheta ,sdpPhi ,CS, Vector::kSpherical);

    // iterate over pixels
    for (pixeliter = recdata.TimeFitPixelsBegin();
         pixeliter != recdata.TimeFitPixelsEnd();
         ++pixeliter) {

      const fevt::Pixel& evtPixel = *pixeliter;
      const fdet::Pixel& detPixel = fdetector.GetPixel(evtPixel);

      Vector dir = detPixel.GetDirection();
      dir.TransformTo(CS);

      // distance from trial plane
      double tmp = kPiOnTwo - Angle(dir, sdp2);

      if (fDebugging == 1)
        cout << "deviation from SDP " << tmp /degree
             << " [deg] , Eye: " << eye.GetId()
             << " Pixel Id : " << detPixel.GetId()
             << endl;

      // weight to assign
      double charge = evtPixel.GetRecData().GetTotalCharge();

      chisq_sdp[count] += tmp*tmp  * charge / ((0.35 * degree * 0.35 * degree));
      sum_charge += charge;
    }

    chisq_sdp[count] /= sum_charge;
    chisq_sdp[count] *= dof[count];
    ++count;
  }
  // End SDP Fit
  //--------------------------------
  //Here we apply weights (if necesary) to the different
  //chisqr components
  count = 0;
  chisq = 0;
  for (eyeIter = fevent.EyesBegin(ComponentSelector::eHasData);
       eyeIter != fevent.EyesEnd(ComponentSelector::eHasData);
       ++eyeIter) {

    fevt::Eye& eye = *eyeIter;
    fevt::EyeRecData& recdata = eye.GetRecData();

    //Checking that the Eye has enough pixels
    if (recdata.GetNSDPPixels() <= 3)
      continue;


    //Storing the Stereo time and SDP chisqr values
    fNDofTimeFit = recdata.GetNTimeFitPixels() - 5 ;
    fNDofSDP = recdata.GetNTimeFitPixels() - 4 ;
    recdata.SetSDPFitChiSquare(chisq_sdp[count],fNDofSDP);
    recdata.SetTimeFitChiSquare(chisq_time[count],fNDofTimeFit);
    //-------------------------------------------------
    if (fDebugging == 1)
      cout << "chisq time: " << chisq_time[count]/dof[count]
           << " chisq SDP: " << chisq_sdp[count]/dof[count]
           << " dof " << dof[count]
           << endl;

    //----------------------------
    //NOTICE:
    // When the Eye's time offset is greater than ~100 ns is better to use
    // only one of the time chisq commponents. Otherwise it may find a
    // geometry that reduces the time offset, but having one or more
    // SDPs clearly off from the best SDP.

    if (eyeId[count] < 6 ) {  //Excluding the 6th eye from the Fit
      if (fExcludeEye.count(eyeId[count]) == 0)
        chisq += chisq_time[count];
      if (fExcludeSDPEye.count(eyeId[count]) == 0)
        chisq += chisq_sdp[count]*5; //higher weight to the SDP component
    }
    ++count;
  }
  f = chisq;
}


VModule::ResultFlag
StereoGeometryFinder::Finish()
{
  return eSuccess;
}