#ifndef __SHOWER_GRAD_E__
#define __SHOWER_GRAD_E__

#include <string>
#include <iostream>
#include <fstream>
#include <iomanip>
#include <set>
#include <map>
#include <cmath>


#include "JTools/JFunction1D_t.hh"
#include "JTools/JFunctionalMap_t.hh"
#include "JTools/JQuadrature.hh"
#include "JPhysics/JPDF.hh"
#include "JPhysics/JPDFTable.hh"
#include "JPhysics/Antares.hh"
#include "JPhysics/KM3NeT.hh"

#include "JTools/JElement.hh"
#include "JTools/JGridCollection.hh"
#include "JTools/JGridMap.hh"
#include "JTools/JPolint.hh"
#include "JTools/JMapList.hh"
#include "JTools/JMultiFunction.hh"
#include "Params.hh"

#include "JPhysics/JPDFTable.hh"
#include "JPhysics/JNPETable.hh"
#include "Math/IFunction.h"

#include "../utilJpp.hh"
#include "rootutil.hh"
#include "GradTools.hh"
#include "Jacobians.hh"

using namespace std;
using namespace JPP;

/**
 * Scaling of absorption and scattering length.
 */

namespace {

  using namespace JPP;
  
  typedef JPolintFunction1D<1, JPolintElement2S<double, double>, JCollection, JResultPDF<double> >    PDE_Function1D_t;

  typedef JMAPLIST<JPolint1FunctionalMapH,
		   JPolint1FunctionalMapH,
		   JPolint1FunctionalMapH,
		   JPolint1FunctionalGridMapH,
		   JPolint1FunctionalGridMapH>::maplist       JMapListGradE_t;
  typedef JPDFTable<PDE_Function1D_t, JMapListGradE_t>        JCascadeGradEPdf_t;
  typedef JNPETable<double, double, JMapListGradE_t>          JCascadeGradENpe_t;

};


/**
 * \file
 *
 * Program to create interpolation tables of the PDF of the arrival time of the Cherenkov light from a shower with derivatives functionality.
 *
 * The PDFs are tabulated as a function of <tt>(D, cos(theta), theta, phi, t)</tt>, where:
 * - <tt>D</tt> is the distance between the vertex and the PMT;
 * - <tt>cos(theta)</tt> the cosine of the photon emission angle;
 * - <tt>(theta, phi)</tt> the orientation of the PMT; and
 * - <tt>t</tt> the arrival time of the light with respect to the Cherenkov hypothesis.
 *
 *The orientation of the PMT is defined in the coordinate system in which
 * the shower developes along the z-axis and the PMT is located in the x-z plane.
 * \author jseneca
 */
class JppGradE
{

private:

  Paramst         _paramst;
  Params          _params;
  double          _background_rate;
  const double    _fudge1 = 1e-12; //minimum expected npe per second
  const double    _fudge2 = 1000000;    //maximum integrated npe

  const double ln10 = 2.302585092994;
  
  const uint _N = 7; //E, x, y, z, theta, phi, t

  int             _verbose;
  uint            _debug;

  JCascadeGradEPdf_t* _pdf_gradfunc;
  JCascadeGradENpe_t* _npe_gradfunc;

  uint jj_jji[7];

  vector< vector< double > > _dXs;    //Pdf derivatives
  vector< vector< double > > _dX_dTs; //Parameter transform jacobians

  double _A;

  JCascadeGradEPdf_t::result_type _pdf_cache;
  JCascadeGradENpe_t::result_type _npe_cache;
     
  dX_dTs _dx_dts;

public:
   
  double          _window_start;
  double          _window_end;
   
  //debug objects
  vector<double>         _dL_a;
  vector<vector<double>> _sum_dnpes_a;
  vector<vector<double>> _sum_dXs_a;
  vector<vector<double>> _sum_dTs_a;
  vector<vector<double>> _sum_dTes_a;
  vector<double>         _dL_n;
  vector<vector<double>> _sum_dnpes_n;
  vector<vector<double>> _sum_dXs_n;
  vector<vector<double>> _sum_dTs_n;
  vector<vector<double>> _sum_dTes_n;  
  vector<double> _times;
  uint bad_dXs;
  uint bad_int_dXs;
  uint bad_dtdXs;
  uint call_dXs;
  uint call_int_dXs;
  uint call_dtdXs;
  uint pdf_call_count;
  uint npe_call_count;
  uint bad_pdf_call_count;
  uint bad_npe_call_count;
   

  JppGradE( vector< string > shower_pdfs  = { "/sps/km3net/users/jseneca/jpp/data/J14p.dat" },
	    double sigma_blur  = 0, 
	    double R_bg        = 0,
	    double t_start     = -100,
	    double t_end       = 900,
	    uint verbose       = 0,
	    uint debug         = 0 );
   
  void set_debug( const uint debug ){
    _debug = debug;
  }
   
  void set_sample_count( uint N ){}; //Don't ask... To make templates work.

  inline double get_k40_npe( double time_window ) const 
  {
    if ( time_window > 0 ) return _background_rate*1e-9*time_window;
    else                   return 0; // for residuals smaller than -100, we don't add background
  }
  
  inline double dnpe_dt( Paramst p ) {
    try{
      _pdf_cache = (*_pdf_gradfunc)( log10(p.E), p.d, p.z, p.theta, p.phi, p.t );
      if( _pdf_cache.f.f.f.f.f.f != _pdf_cache.f.f.f.f.f.f ) return 0.0;
    } catch( JException e ){
      if(_verbose > 1) cout << e << " returning 0..." << endl;
      return 0.0;
    }
    return _pdf_cache.f.f.f.f.f.f * p.E;
  };
   
  inline double npe( Paramst p ) {
    try{
      _pdf_cache = (*_pdf_gradfunc)( log10(p.E), p.d, p.z, p.theta, p.phi, p.t );
      if( _pdf_cache.f.f.f.f.f.v != _pdf_cache.f.f.f.f.f.v ) return 0.0;
    } catch( JException e ){
      if(_verbose > 1) cout << e << " returning 0..." << endl;
      return 0.0;
    }
    return _pdf_cache.f.f.f.f.f.v * p.E;
  };
   
  inline double npe( Params p ) {
    try{
      _npe_cache = (*_npe_gradfunc)( log10(p.E), p.d, p.z, p.theta, p.phi );
      if( _npe_cache.f.f.f.f.f != _npe_cache.f.f.f.f.f ) return 0.0;
    } catch( JException e ){
      if(_verbose > 1) cout << e << " returning 0..." << endl;
      return 0.0;
    }
    return _npe_cache.f.f.f.f.f * p.E;
  };
   
  inline vector<double> dnpe_dtdXs ( Trk trk, Hit hit ){
    return dnpe_dtdXs( Paramst( trk, hit ) );
  };

  inline vector<double> dnpe_dtdXs ( Paramst pst ){
    return dnpe_dtdXs( pst.E, pst.d, pst.z, pst.theta, pst.phi, pst.t );
  };
    
  inline vector<double> dnpe_dtdXs ( float E, float D, double cd, double theta, double phi, float t ) {
    //Per time
    try{
      if( _debug ) call_dtdXs++;
      _pdf_cache = (*_pdf_gradfunc)( log10(E), D, cd, theta, phi, t);
      if( _pdf_cache.f.f.f.f.f.f != _pdf_cache.f.f.f.f.f.f ) throw( JException( "NaN value encountered" ) );
      return dnpe_dtdXs( _pdf_cache, E );
    } catch( JException e ){
      if(_debug > 1){
	cout << e << " returning 0..." << endl;
	cout << "D " << D << ", cd " << cd << ", theta " << theta << ", phi " << phi << ", t " << t << ", E " << E << endl;
      }
      if( _debug ) bad_dtdXs++;
      return {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
    }
  };
  
  inline vector<double> dnpe_dXs ( Trk trk, Hit hit ){
    return dnpe_dXs( Paramst( trk, hit ) );
  };

  inline vector<double> dnpe_dXs ( Paramst pst ){
    return dnpe_dXs( pst.E, pst.d, pst.z, pst.theta, pst.phi, pst.t );
  };
  
  inline vector<double> dnpe_dXs ( float E, float D, double cd, double theta, double phi, float t ) {
    //Integral over time
    try{
      if( _debug ) call_dXs++;
      _pdf_cache = (*_pdf_gradfunc)( log10(E), D, cd, theta, phi, t );
      if( _pdf_cache.f.f.f.f.f.f != _pdf_cache.f.f.f.f.f.f ) throw( JException( "NaN value encountered" ) );
      return dnpe_dXs( _pdf_cache, E );
    } catch( JException e ){
      if(_debug > 1){
	cout << e << " returning 0..." << endl;
	cout << "D " << D << ", cd " << cd << ", theta " << theta << ", phi " << phi << ", t " << t << ", E " << E << endl;
      }
      if( _debug ) bad_dXs++;
      return {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
    }
  };

  inline vector<double> dnpe_dXs ( Trk trk, Pmt unhit ){
    return dnpe_dXs( Params( trk, unhit ) );
  };

  inline vector<double> dnpe_dXs ( Params ps ){
    return dnpe_dXs( ps.E, ps.d, ps.z, ps.theta, ps.phi );
  };
  
  inline vector<double> dnpe_dXs ( float E, float D, double cd, double theta, double phi ) {
    //Integral over all time
    try{
      if( _debug ) call_int_dXs++;
      _npe_cache = (*_npe_gradfunc)( log10(E), D, cd, theta, phi );
      if( _npe_cache.f.f.f.f.f != _npe_cache.f.f.f.f.f ) throw( JException( "NaN value encountered" ) );
      return dnpe_dXs( _npe_cache, E );
    } catch( JException e ){
      if(_debug > 1){
	cout << e << " returning 0..." << endl;
	cout << "D " << D << ", cd " << cd << ", theta " << theta << ", phi " << phi << ", E " << E << endl;
      }
      if( _debug ) bad_int_dXs++;
      return {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
    }
  };

  inline vector<double> dnpe_dtdXs ( const JCascadeGradEPdf_t::result_type &pdf_res, const double &E ) const {
    
    return { 
      pdf_res.fp.f.f.f.f.f / ln10 + pdf_res.f.f.f.f.f.f,
	pdf_res.f.fp.f.f.f.f * E, 
	pdf_res.f.f.fp.f.f.f * E, 
	pdf_res.f.f.f.fp.f.f * E, 
	pdf_res.f.f.f.f.fp.f * E,
	pdf_res.f.f.f.f.f.fp * E,
	pdf_res.f.f.f.f.f.f  * E, 
	};
  };
  
  inline vector<double> dnpe_dXs ( const JCascadeGradEPdf_t::result_type &pdf_res, const double &E ) const {
    //Integral over time
    return { 
        pdf_res.fp.f.f.f.f.v / ln10 + pdf_res.f.f.f.f.f.v,
	pdf_res.f.fp.f.f.f.v * E, 
	pdf_res.f.f.fp.f.f.v * E, 
	pdf_res.f.f.f.fp.f.v * E, 
	pdf_res.f.f.f.f.fp.v * E,
	pdf_res.f.f.f.f.f.f  * E,
	pdf_res.f.f.f.f.f.v  * E, 
	};
  };
  
  inline vector<double> dnpe_dXs ( const JCascadeGradENpe_t::result_type &npe_res, const double &E ) const {
    //Integral over all time
    return { 
        npe_res.fp.f.f.f.f / ln10 + npe_res.f.f.f.f.f,
	npe_res.f.fp.f.f.f * E, 
	npe_res.f.f.fp.f.f * E, 
	npe_res.f.f.f.fp.f * E,
	npe_res.f.f.f.f.fp * E,
	0,
	npe_res.f.f.f.f.f  * E};
  };

  inline double dnpe_dt( const Trk &trk, const Hit &hit ){ return dnpe_dt( Paramst( trk, hit ) ); };
  inline double npe( const Trk &trk, const Hit &hit )    { return npe( Paramst( trk, hit ) ); };
  inline double npe( const Trk &trk, const Pmt &unhit )  { return npe( Params( trk, unhit ) ); };
  
  inline double dnpe_dt( const Hit &hit ){ return _pdf_cache.f.f.f.f.f.f * _paramst.E; };
  inline double dnpe_dt( float E, float D, double cd, double theta, double phi, float t ){ return dnpe_dt( Paramst( E, D, cd, theta, phi, t ) ); };
  
  inline double npe( const Hit &hit )   { return _pdf_cache.f.f.f.f.f.v * _paramst.E; };
  inline double npe( float E, float D, double cd, double theta, double phi, float t ){ return npe( Paramst( E, D, cd, theta, phi, t ) ); };
  
  inline double npe( const Pmt &unhit ) { return _npe_cache.f.f.f.f.f * _params.E; };
  inline double npe( float E, float D, double cd, double theta, double phi ){ return npe( Params( E, D, cd, theta, phi ) ); };


  //double dP1_dt_a_( const Paramst& p  )
  //{
  //return ( dnpe_dt( p ) + get_k40_npe(1) ) * exp( -npe( p ) - get_k40_npe( p.t - _window_start ) );
  //}
  
  //double log_dP1_dt_a_( const Paramst& p  )
  //{
  //return log( dnpe_dt( p ) + get_k40_npe(1) ) + ( -npe( p ) - get_k40_npe( p.t - _window_start ) );
  //}
  
  //template<typename ...Ts> double dP1_dt_a( Ts ...args ) { return dP1_dt_a_( Paramst(args...)); };
  
  //double Ntot_( const Params& p  ) 
  //{
  //return npe( p ) + get_k40_npe( _window_end - _window_start );
  //}

  //template<typename ...Ts> double Ntot( Ts ...args ) { return Ntot_( Params(args...)); };

  /*
  vector<double> dnpe_dXs ( Params ps ){
    return dnpe_dXs( ps.E, ps.d, ps.z, ps.theta, ps.phi );
  };

  vector<double> dnpe_dXs ( Paramst ps ){
    return dnpe_dXs( ps.E, ps.d, ps.z, ps.theta, ps.phi, ps.t );
  };

  vector<double> dnpe_dtdXs ( Paramst ps ){
    return dnpe_dtdXs( ps.E, ps.d, ps.z, ps.theta, ps.phi, ps.t );
  };
  */

  inline vector<double> dnpe_dtdTs( const Hit &hit ){
    vector< double > ds( 7, 0 );
    dnpe_dtdTs1( ds, _dXs[0], _paramst, _pdf_cache, _dX_dTs ); //Recycle dnpe_dtdXs
    return ds;
  };

  inline vector<double> dnpe_dTs( const Hit &hit ){
    vector< double > ds( 7, 0 );
    dnpe_dTs1( ds, _dXs[1], _paramst, _pdf_cache, _dX_dTs ); //Recycle dnpe_dXs
    return ds;
  };

  inline vector<double> dnpe_int_dTs( const Pmt &unhit ){
    vector< double > ds( 7, 0 );
    dnpe_int_dTs1( ds, _dXs[2], _params, _npe_cache, _dX_dTs );
    return ds;
  };

  inline vector<double> dnpe_dtdTs1( vector< double > &ds,
				     vector< double > &dXs,
				     const Paramst &pst,
				     const JCascadeGradEPdf_t::result_type &pdf_res,
				     const vector< vector< double > > &dTs ){
    dXs = dnpe_dtdXs( pdf_res, pst.E );

    // E, x, y, z, dtheta, dphi, t
    for( uint jj = 0; jj != 7; ++jj ){
      for( uint ii = 0; ii != 6; ++ii ){
	ds[jj] += dXs[ii] * dTs[ii][jj_jji[jj]];
      }
    }

    return ds;
    
  }

  inline vector<double> dnpe_dTs1( vector< double > &ds,
				   vector< double > &dXs,
				   const Paramst &pst,
				   const JCascadeGradEPdf_t::result_type &pdf_res,
				   const vector< vector< double > > &dTs ){

    dXs = dnpe_dXs( pdf_res, pst.E );

    // E, x, y, z, dtheta, dphi, 
    for( uint jj = 0; jj != 7; ++jj ){
      for( uint ii = 0; ii != 6; ++ii ){
	ds[jj] += dXs[ii] * dTs[ii][jj_jji[jj]];
      }
    }
    
    return ds;
    
  }

  inline vector<double> dnpe_int_dTs1( vector< double > &ds,
				       vector< double > &dXs,
				       const Params &ps,
				       const JCascadeGradENpe_t::result_type &npe_res,
				       vector< vector< double > > &dTs ){
    dXs = dnpe_dXs( npe_res, ps.E );

    for( uint ii = 0; ii != dTs[5].size(); ii++ ) dTs[5][ii] = 0.0; //time derivative is 0

    // E, x, y, z, dtheta, dphi, t
    for( uint jj = 0; jj != 7; ++jj ){
      for( uint ii = 0; ii != 6; ++ii ){
	ds[jj] += dXs[ii] * dTs[ii][jj_jji[jj]];
      }
    }

    return ds;

  };

  void set_paramst( const Trk &trk, Hit &hit ){
    
    _paramst.init_t( trk, hit );
    try{
      if( _debug ) pdf_call_count++;
      _pdf_cache = (*_pdf_gradfunc)( log10(_paramst.E), _paramst.d, _paramst.z, _paramst.theta, _paramst.phi, _paramst.t );
      if( ( _pdf_cache.f.f.f.f.f.f != _pdf_cache.f.f.f.f.f.f ) ||
	  ( _pdf_cache.f.f.f.f.f.v != _pdf_cache.f.f.f.f.f.v ) ||
	  ( _pdf_cache.f.f.f.f.f.f < 0 ) ||
	  ( _pdf_cache.f.f.f.f.f.v < 0 ) ){
	throw( JException( "Bad pdf value encountered" ) );
      }
    } catch( JException e ){
      if( _debug ) bad_pdf_call_count++;
      if( _debug > 2) cout << e << " out of range pdf parameters hit..." << endl;
      if( _debug > 2) cout << "Paramst: " << _paramst << ", .f: " << _pdf_cache.f.f.f.f.f.f << ", .v: " << _pdf_cache.f.f.f.f.f.v << endl;
      if( _debug > 2 ) cin.ignore();
      hit.pattern_flags = 2; //Flag bad hit
    };
  };
   
  void set_params( const Trk &trk, Pmt &unhit ){
    
    _params.init( trk, unhit.pos, unhit.dir );
    try{
      if( _debug ) npe_call_count++;
      _npe_cache = (*_npe_gradfunc)( log10(_params.E), _params.d, _params.z, _params.theta, _params.phi );
      if( ( _npe_cache.f.f.f.f.f != _npe_cache.f.f.f.f.f ) ||
	  ( _npe_cache.f.f.f.f.f < 0 ) ) throw( JException( "NaN value encountered" ) );
    } catch( JException e ){
      if( _debug ) bad_npe_call_count++;
      if( _debug > 2 ) cout << e << " out of range pdf parameters for unhit..." << endl;
      if( _debug > 2 ) cout << "Params: " << _params << ", .f: " << _npe_cache.f.f.f.f.f << endl;
      if( _debug > 2 ) cin.ignore();
      unhit.flag = 2; //Flag bad pmt
    };
  };
  
  inline void set_jacobians( const Trk &trk, const Hit &hit ){
    _dx_dts.init( trk, hit.pos, hit.dir, _paramst );
    _dx_dts.spherical();
    _dX_dTs = _dx_dts.ds;
  };

  void set_jacobians( const Trk &trk, const Pmt &unhit ){
    _dx_dts.init( trk, unhit.pos, unhit.dir, _params );
    _dx_dts.spherical();
    _dX_dTs = _dx_dts.ds;
    for( uint jj = 0; jj != 10; ++jj ){
      (_dX_dTs)[5][jj] = 0.0; //Time does not change for unhits (but this shouldnt matter for LL
    };
  };

  inline double dhit_L( const double &dnpe_dtdT, 
			const double &dnpe_dT,
			const double &A,
			const double &dt_e ) const{

    double res = dnpe_dtdT / ( A + _fudge1 ) - dnpe_dT - get_k40_npe( 1.0 ) * dt_e;

    return res;
  };

  inline double get_hit_L( const double nn,
			   const double NN,
			   bool factorized_fudge = false ) {

    if( factorized_fudge ) return log( _fudge1 + nn ) - min( NN, _fudge2 );
    else                   return log( _fudge1 + ( nn ) * exp( -NN ));
  };

  double eval_lik( const Trk& trk,
		   vector< Hit > hits,
		   vector< Pmt > unhits,
		   bool factorized_fudge = false) {

    if( _debug ){
      pdf_call_count = 0;
      npe_call_count = 0;
      bad_pdf_call_count = 0;
      bad_npe_call_count = 0;
    }

    assert( trk.pos == trk.pos );
    assert( trk.dir == trk.dir );
    assert( trk.t == trk.t );
    assert( trk.E == trk.E );

    double L=0;
    
    foreach( hit, hits ){

      set_paramst( trk, hit );

      if( hit.pattern_flags == 2 ){ //Hit flagged as bad
	L -= get_hit_L( get_k40_npe( 1.0 ), 
			get_k40_npe( _paramst.t - _window_start ),
			factorized_fudge ); //Typically happens when hit is really unlikely
      } else {
	//cout << "doing hit: " << hit;
	L -= get_hit_L( dnpe_dt( hit ) + get_k40_npe( 1.0 ), 
			npe( hit )     + get_k40_npe( _paramst.t - _window_start ),
			factorized_fudge );
	//cout << ", L after: " << L << endl;
      };

      if( L != L ){
	cout << "Factorized fudge: " << factorized_fudge << endl;
	cout << "trk : " << trk << endl;
	cout << "dnpe_dt: " << dnpe_dt( hit ) << endl;
	cout << "npe: " << npe( hit ) << endl;
	cout << "paramst: " << Paramst( trk, hit ) << endl;
	cin.ignore();
      }
    }

    foreach( unhit, unhits ){

      set_params( trk, unhit );

      if( unhit.flag == 2 ){  //Hit flagged as bad
	L -=                - get_k40_npe( _window_end - _window_start );
      } else {
	L -= - npe( unhit ) - get_k40_npe( _window_end - _window_start );
      };
    }

    //cout << ", L after unhits: " << L << endl;
    assert( L == L );

    if( _debug && _verbose ){
      cout << "Good pdf calls: " << pdf_call_count - bad_pdf_call_count << ", bad: " << bad_pdf_call_count << endl;
      cout << "Good npe calls: " << npe_call_count - bad_npe_call_count << ", bad: " << bad_npe_call_count << endl;
      //cout << "Good calls to dnpe_int_dXs: " << call_int_dXs - bad_int_dXs << ", bad: " << bad_int_dXs << endl;
    }

    return L;
  };

  void eval_lik_gradient( double &r, vector<double> &ds, const Trk &trk, vector< Hit > hits, vector< Pmt > unhits ){

    assert( trk.pos == trk.pos );
    assert( trk.dir == trk.dir );
    assert( trk.t == trk.t );
    assert( trk.E == trk.E );

    //Timer time0("time0");

    //out debug : 1.66632e-07
    //set elong : 0.000437399
    //fill ds   : 3.44014e-07
    //hit loop  : 0.00382506
    //unhit loop: 1.06737e-07
    //----------inner----------
    //thit norm      : 1.72007e-07
    //thit paramst   : 3.31191e-06
    //thit jacob     : 0.000140122
    //thit A         : 0.00121552
    //thit dnpedtdts : 0.00123817
    //thit dnpedts   : 0.00122572
    //thit dte       : 1.07735e-06
    //thit dhitL     : 2.48028e-07
    //thit sanity    : 2.61082e-07
    //thit debug     : 1.55114e-07
    //tunhit norm    : 0
    //tunhit params  : 0
    //tunhit jacob   : 0
    //tunhit dnpedts : 0
    //tunhit dunhitL : 0


    //double t0 = time0.time_since_last_start();

    if( _debug ){
      bad_dXs = 0;
      bad_int_dXs = 0;
      bad_dtdXs = 0;
      call_dXs = 0;
      call_int_dXs = 0;
      call_dtdXs = 0;
      pdf_call_count = 0;
      npe_call_count = 0;
      bad_pdf_call_count = 0;
      bad_npe_call_count = 0;
      if( _debug > 1 ){
	_sum_dnpes_a.assign( 7, vector<double>( 3, 0 ) );
	_sum_dXs_a.assign(   7, vector<double>( 3, 0 ) );
	_sum_dTs_a.assign(   7, vector<double>( 6, 0 ) );
	_sum_dTes_a.assign(  7, vector<double>( 7, 0 ) );
	
	_sum_dnpes_n.assign( 7, vector<double>( 3, 0 ) );
	_sum_dXs_n.assign(   7, vector<double>( 3, 0 ) );
	_sum_dTs_n.assign(   7, vector<double>( 6, 0 ) );
	_sum_dTes_n.assign(  7, vector<double>( 7, 0 ) );
	
	_dL_a.assign( 7, 0.0 );
	_dL_n.assign( 7, 0.0 );
      };
    };

    //double t1 = time0.time_since_last_start();

    //double t2 = time0.time_since_last_start();

    //vector< double > ds ( _N, 0 );

    ds.assign( _N, 0.0 );

    r = 0.0;

    foreach( hit, hits ) {

      //hit.dir = hit.dir.normalize(); //0.47%, 8.6 e-8 //necessary?

      //Here we access the pdf for the first and only time
      set_paramst( trk, hit ); //0.96%, 1.74e-7

      if( hit.pattern_flags == 2 ){ //Hit flagged as bad
	r -= get_hit_L( get_k40_npe( 1.0 ), 
			get_k40_npe( _paramst.t - _window_start ),
			true ); //Typically happens when hit is really unlikely
	//Assume ds[xx] is 0.0
	continue;
      }

      set_jacobians( trk, hit ); //39.1%, 7.1e-6

      const vector< double > dnpe_dtdTs = this->dnpe_dtdTs( hit );

      vector< double > dnpe_dTs;

      if( npe( hit ) < _fudge2 ) dnpe_dTs = this->dnpe_dTs( hit ); //26.0%, 4.7e-6
      else                       dnpe_dTs.assign( 7, 0.0 );

      _A = dnpe_dt( hit ) + get_k40_npe( 1.0 ); 
      
      r -= get_hit_L( _A, npe( hit ) + get_k40_npe( _paramst.t - _window_start ), true );

      for( uint ii = 0; ii != _N; ii++ ){

	double dt_e = _dX_dTs[5][ii];

	ds[ii] -= dhit_L( dnpe_dtdTs[ii], dnpe_dTs[ii], _A, dt_e );

	if( ds[ii] < -1e6 or ds[ii] != ds[ii] ){
	  cout << "ds[ii]: " << ds[ii] << endl;
	  cout << "bad LL grad ii: " << ii << endl;
	  cout << "dnpe_dtdTs:  " << dnpe_dtdTs[ii] << endl;
	  cout << "dnpe_dTs: " << dnpe_dTs[ii] << endl;
	  cout << "dt_e: " << dt_e << endl;
	  cout << "_A: " << _A << endl;
	  cout << "trk: " << trk << endl;
	  cout << "hit: " << hit << endl;
	  if( _debug > 2) cin.ignore();
	};

	if( _debug > 1 ){

	  fill_components( trk, hit ); //Must come first as the trk is set

	  if( _debug > 2 ) cout << "Numerical likelihood gradient [" << ii << "]" << endl;

	  double dnpe_dtdT = this->num_dnpe_dtdTs( trk, hit, 1e-5 )[ii];
	  double dnpe_dT;
	  if( npe( hit ) < _fudge2 ) dnpe_dT = this->num_dnpe_dTs( trk, hit, 1e-5 )[ii];
	  else                       dnpe_dT = 0.0;

	  double num_dt_e = num_dX_dTs( trk, hit, 1e-5 )[5][ii];

	  _dL_n[ii] -= dhit_L( dnpe_dtdT, dnpe_dT, _A, num_dt_e );
	}

      }
    }


    foreach( unhit, unhits  ) { 

      //unhit.dir = unhit.dir.normalize();

      set_params( trk, unhit ); //has to be first

      if( unhit.flag == 2 ){  //Hit flagged as bad
	r -= - get_k40_npe( _window_end - _window_start );
	continue;
      }

      set_jacobians( trk, unhit );

      const vector< double > dnpe_int_dTs = this->dnpe_int_dTs( unhit );

      r -= - npe( unhit ) - get_k40_npe( _window_end - _window_start );

      for( uint ii = 0; ii != _N; ii++ ){

	double dL_unhit = - dnpe_int_dTs[ii];
	ds[ii] -= dL_unhit;

	if( ds[ii] != ds[ii] ){
	  cout << "nan encountered in ll grad ii: " << ii << endl;
	  cout << "trk: " << trk << endl;
	  cout << "unhit: " << unhit << endl;
	  if( _debug > 2 ) cin.ignore();
	};

	if( _debug > 1 ) _dL_n[ii] -= -this->num_dnpe_dTs( trk, unhit, 1e-5 )[ii]; //Yes, 2 negative signs
      };
      if( _debug > 1 ) fill_components( trk, unhit );
    }

    if( _debug && _verbose ){
      cout << "Good pdf calls: " << pdf_call_count - bad_pdf_call_count << ", bad: " << bad_pdf_call_count << endl;
      cout << "Good npe calls: " << npe_call_count - bad_npe_call_count << ", bad: " << bad_npe_call_count << endl;
      //cout << "Good calls to dnpe_int_dXs: " << call_int_dXs - bad_int_dXs << ", bad: " << bad_int_dXs << endl;
    }

    //double t23 = time0.time_since_last_start();

    /*
      cout << "out debug : " << t1 - t0 << endl;
      cout << "set elong : " << t2 - t1 << endl;
      cout << "fill ds   : " << t3 - t2 << endl;
      cout << "hit loop  : " << t16 - t3  << endl;
      cout << "unhit loop: " << t23 - t16 << endl;
      cout << "grand tot : " << t23 - t0 << endl;

      cout << "----------inner----------" << endl;

      cout <<  "thit norm      : " << thit_norm << endl;
      cout <<  "thit paramst   : " << thit_paramst << endl;
      cout <<  "thit jacob     : " << thit_jacob << endl;
      cout <<  "thit A         : " << thit_A << endl;
      cout <<  "thit dnpedtdts : " << thit_dnpedtdts << endl;
      cout <<  "thit dnpedts   : " << thit_dnpedts << endl;
      cout <<  "thit dte       : " << thit_dte << endl;
      cout <<  "thit dhitL     : " << thit_dhitL << endl;
      cout <<  "thit dhitL     : " << thit_hitL << endl;
      cout <<  "thit sanity    : " << thit_sanity << endl;
      cout <<  "thit debug     : " << thit_debug << endl;

      cout <<  "tunhit norm    : " << tunhit_norm << endl;
      cout <<  "tunhit params  : " << tunhit_params << endl;
      cout <<  "tunhit jacob   : " << tunhit_jacob << endl;
      cout <<  "tunhit dnpedts : " << tunhit_dnpedts << endl;
      cout <<  "tunhit dunhitL : " << tunhit_dunhitL << endl;
      cout <<  "tunhit unhitL : " << tunhit_unhitL << endl;

      cin.ignore();
    */

  };

  // Debug

  void fill_components( const Trk &trk, Hit &hit ){

    numFuncs num;

    set_paramst( trk, hit ); //could remove
    set_jacobians( trk, hit );

    const vector< double > dnpe_dtdTs = this->dnpe_dtdTs( hit );
    const vector< double > dnpe_dTs   = this->dnpe_dTs( hit ); 

    vector< double > dnpe_dtdTs_n = this->num_dnpe_dtdTs( trk, hit, 1e-5 );
    vector< double > dnpe_dTs_n   = this->num_dnpe_dTs(   trk, hit, 1e-5 );

    for( uint kk = 0; kk != 7; kk++ ){

      _sum_dnpes_a[kk][0] += dnpe_dtdTs[kk];
      _sum_dnpes_a[kk][1] += dnpe_dTs[kk];

      _sum_dnpes_n[kk][0] += dnpe_dtdTs_n[kk];
      _sum_dnpes_n[kk][1] += dnpe_dTs_n[kk];
    }
    Paramst ps = _paramst;
    
    vector< double > dtdXs_n = num.num_dnpe_dtdXs( this, ps.E, ps.d, ps.z, ps.theta, ps.phi, ps.t, 1e-6 );
    vector< double > dXs_n   = num.num_dnpe_dXs  ( this, ps.E, ps.d, ps.z, ps.theta, ps.phi, ps.t, 1e-6 );
    vector< vector< double > > dTs_n = num_dX_dTs ( trk, hit, 1e-6 );
    //vector< vector< double > > dTes_n = num_dTe_dTs( _elong_trks[iTe], _elong_lens[iTe], 1e-6 );
    for( uint jj = 0; jj != 7; jj++ ){
      for( uint ii = 0; ii != 6; ii++ ){
	
	_sum_dXs_a[jj][0] += _dXs [0][ii];
	_sum_dXs_n[jj][0] +=  dtdXs_n     [ii];
	_sum_dXs_a[jj][1] += _dXs [1][ii];
	_sum_dXs_n[jj][1] +=  dXs_n       [ii];
      
	_sum_dTs_a[jj][ii]  += _dX_dTs[ii][jj_jji[jj]];
	//cout << "_dX_dTes[" << iTe << "][" << ii << "][" << jj_jji[jj] << "]: " << _dX_dTes[iTe][ii][jj_jji[jj]] << endl;
	if( ii == 0 && jj == 0 && _dX_dTs[ii][jj_jji[jj]] != 1.0 ){
	  cout << "dE_dE != 1.0" << endl;
	  if( _debug > 2) cin.ignore();
	}
	  
	//_sum_dTes_a[kk][jj] += _dTe_dTs[iTe][jj][kk_kki[kk]];
	_sum_dTs_n[jj][ii]  += dTs_n[ii][jj_jji[jj]];
	if( abs(_sum_dTs_n[jj][ii]) > 1e12 ){
	  cout << "dTs_n[  " << ii << "]" << "[" << jj_jji[jj] << "]" << dTs_n[ii][jj_jji[jj]] << endl;
	  cout << "sum_dTs_n[" << jj << "]" << "[" << ii << "]" << _sum_dTs_n[jj][ii] << endl;
	  if( _debug > 2) cin.ignore();
	}
	//_sum_dTes_n[kk][jj] += dTes_n[jj][kk_kki[kk]];
      }
    }
  };

  void fill_components( const Trk &trk, Pmt &unhit ){

    numFuncs num;

    set_params( trk, unhit ); //not necessary actually...
    set_jacobians( trk, unhit );

    const vector< double > dnpe_dTs   = this->dnpe_int_dTs( unhit ); 
    const vector< double > dnpe_dTs_n = this->num_dnpe_dTs( trk, unhit, 1e-5 );

    for( uint kk = 0; kk != 7; kk++ ){
      _sum_dnpes_n[kk][2] += dnpe_dTs_n[kk];
      _sum_dnpes_a[kk][2] += dnpe_dTs[kk];
    }

    Params ps = _params;

    vector< double > dXs_n = num.num_dnpe_dXs( this, ps.E, ps.d, ps.z, ps.theta, ps.phi, 1e-6 );

    vector< vector< double > > dTs_n  = num_dX_dTs(  trk, unhit, 1e-6 );
    //vector< vector< double > > dTes_n = num_dTe_dTs( trk, _elong_lens[iTe], 1e-6 );  

    for( uint jj = 0; jj != 7; jj++ ){
      for( uint ii = 0; ii != 6; ii++ ){
	
	_sum_dXs_a[jj][2] += _dXs [2][ii];
	_sum_dXs_n[jj][2] +=  dXs_n       [ii];
	
	_sum_dTs_a[jj][ii]  += _dX_dTs[ii][jj_jji[jj]];
	//_sum_dTes_a[kk][jj] += _dTe_dTs[iTe][jj][kk_kki[kk]];
	_sum_dTs_n[jj][ii]  += dTs_n[ii][jj_jji[jj]];
	//_sum_dTes_n[kk][jj] += dTes_n[jj][kk_kki[kk]];
      }
    }
  };

  //Numerical functions 

  vector<double> num_dnpe_dtdTs( const Trk &trk, const Hit &hit, double delta = 1e-5){
    vector<double> ds( 7, 0.0 );
    // E, x, y, z, dx, dy, dz, dtheta, dphi, t
    for( uint jj = 0; jj != 7; jj++ ){
      std::pair< Trk, Trk > trks = delta_trks( trk, jj_jji[jj], delta );
      double dnpe_dt_l = dnpe_dt( trks.first, hit );
      double dnpe_dt_h = dnpe_dt( trks.second, hit );
      ds[jj] += ( dnpe_dt_h - dnpe_dt_l ) / delta;
    }
    return ds;
  }

  vector<double> num_dnpe_dTs( const Trk &trk, const Hit &hit, double delta = 1e-5){
    vector<double> ds( 7, 0.0 );
    // E, x, y, z, dx, dy, dz, dphi, dtheta, t
    for( uint jj = 0; jj != 7; jj++ ){	
      std::pair< Trk, Trk > trks = delta_trks( trk, jj_jji[jj], delta );
      double npe_l = npe( trks.first, hit );
      double npe_h = npe( trks.second, hit );
      ds[jj] += ( npe_h - npe_l ) / delta;
    }
    return ds;
  }


  vector<double> num_dnpe_dTs( const Trk &trk, const Pmt &unhit, double delta = 1e-5){
    vector<double> ds( 7, 0.0 );
    // E, x, y, z, dx, dy, dz, dphi, dtheta, t
    for( uint jj = 0; jj != 7; jj++ ){	
      std::pair< Trk, Trk > trks = delta_trks( trk, jj_jji[jj], delta );
      double npe_l = npe( trks.first, unhit );
      double npe_h = npe( trks.second, unhit );
      ds[jj] += ( npe_h - npe_l ) / delta;
    }
    return ds;
  }

};

#endif