#include "GradTools.hh"

std::pair< Trk, Trk > delta_trks( const Trk &trk, uint ipar, double delta ){
  
  vector< Trk > trk_d( 2, trk );
  vector< vector<double> > dvec( 2, vector< double >( 10, 0.0 ) );
    
  dvec[0][ipar] = -delta/2.;
  dvec[1][ipar] =  delta/2.;
  // E, x, y, z, dx, dy, dz, theta, phi, t
  for( uint it = 0; it != 2; it++ ){
    trk_d[it].E     += dvec[it][0]; //trk_d[it].E * ( std::pow(10, dvec[it][9]) - 1 );
    trk_d[it].pos.x += dvec[it][1];
    trk_d[it].pos.y += dvec[it][2];
    trk_d[it].pos.z += dvec[it][3];
    trk_d[it].dir.set_angles( trk_d[it].dir.theta() + dvec[it][7],  trk_d[it].dir.phi() );
    trk_d[it].dir.set_angles( trk_d[it].dir.theta(),  dvec[it][8] + trk_d[it].dir.phi() ); //Needs to be before the cart dir
    trk_d[it].t     += dvec[it][9];
    trk_d[it].dir.x += dvec[it][4];
    trk_d[it].dir.y += dvec[it][5];
    trk_d[it].dir.z += dvec[it][6];
    
    trk_d[it].dir = trk_d[it].dir.normalize();
  }

  return std::pair< Trk, Trk >{ trk_d[0], trk_d[1] };
}
		  
std::pair< Paramst, Paramst > delta_pst( const Paramst &pst, uint ipar, double delta ){
  
  vector< Paramst > ps_d( 2, pst );
  vector< vector<double> > dvec( 2, vector< double >( 6, 0.0 ) );
    
  dvec[0][ipar] = -delta/2.;
  dvec[1][ipar] =  delta/2.;
  // E, x, y, z, dx, dy, dz, theta, phi, t
  for( uint it = 0; it != 2; it++ ){
    ps_d[it].E     += dvec[it][0]; 
    ps_d[it].d     += dvec[it][1];
    ps_d[it].z     += dvec[it][2];
    ps_d[it].theta += dvec[it][3];
    ps_d[it].phi   += dvec[it][4];
    ps_d[it].t     += dvec[it][5];
  }

  return std::pair< Paramst, Paramst >{ ps_d[0], ps_d[1] };
}

std::pair< Params, Params > delta_ps( const Params &ps, uint ipar, double delta ){
  
  vector< Params > ps_d( 2, ps );
  vector< vector<double> > dvec( 2, vector< double >( 6, 0.0 ) );
    
  dvec[0][ipar] = -delta/2.;
  dvec[1][ipar] =  delta/2.;
  // E, x, y, z, dx, dy, dz, theta, phi, t
  for( uint it = 0; it != 2; it++ ){
    ps_d[it].E     += dvec[it][0]; 
    ps_d[it].d     += dvec[it][1];
    ps_d[it].z     += dvec[it][2];
    ps_d[it].theta += dvec[it][3];
    ps_d[it].phi   += dvec[it][4];    
  }

  return std::pair< Params, Params >{ ps_d[0], ps_d[1] };
}

std::pair< Trk, Trk > delta_trks_e_frac( const Trk &trk, const double fraction, uint ipar, double delta ){
  
  std::pair< Trk, Trk > trks = delta_trks( trk, ipar, delta );
  
  return std::pair< Trk, Trk > { 
    get_elong_trk( trks.first,  fraction, 1e-10),
      get_elong_trk( trks.second, fraction, 1e-10)
      };
};

std::pair< Trk, Trk > delta_trks_e( const Trk &trk, const double len, uint ipar, double delta ){
  
  std::pair< Trk, Trk > trks = delta_trks( trk, ipar, delta );
  
  return std::pair< Trk, Trk > { propagate( trks.first, len ),
                                 propagate( trks.second, len ) };
};

vector< vector< double > > num_dX_dTs( const Trk &trk, const Hit &hit, double delta)
{
  //Numerical jacobians of pdf parameters differentiated by track parameters
  vector< vector<double> > ds( 6, vector<double>(10, 0) );
    
  // E, x, y, z, dx, dy, dz, dtheta, dphi, t
  for( uint ii = 0; ii != 10; ii++ ){
    std::pair< Trk, Trk > trks = delta_trks( trk, ii, delta );
    
    Paramst ps_l = Paramst( trks.first, hit );
    Paramst ps_h = Paramst( trks.second, hit );

    ds[0][ii] = ( ps_h.E     - ps_l.E )     / delta;
    ds[1][ii] = ( ps_h.d     - ps_l.d )     / delta;
    ds[2][ii] = ( ps_h.z     - ps_l.z )     / delta;
    ds[3][ii] = ( ps_h.theta - ps_l.theta ) / delta;
    ds[4][ii] = ( ps_h.phi   - ps_l.phi )   / delta;
    ds[5][ii] = ( ps_h.t     - ps_l.t )     / delta;
  }

  return ds;

}

vector< vector< double > > num_dX_dTs( const Trk &trk, const Pmt &unhit, double delta)
{
  //Numerical jacobians of pdf parameters differentiated by track parameters
  vector< vector<double> > ds( 6, vector<double>(10, 0) );
    
  // E, x, y, z, dx, dy, dz, dtheta, dphi, t
  for( uint ii = 0; ii != 10; ii++ ){
    std::pair< Trk, Trk > trks = delta_trks( trk, ii, delta );
    
    Params ps_l = Params( trks.first, unhit );
    Params ps_h = Params( trks.second, unhit );

    ds[0][ii] = ( ps_h.E     - ps_l.E )     / delta;
    ds[1][ii] = ( ps_h.d     - ps_l.d )     / delta;
    ds[2][ii] = ( ps_h.z     - ps_l.z )     / delta;
    ds[3][ii] = ( ps_h.theta - ps_l.theta ) / delta;
    ds[4][ii] = ( ps_h.phi   - ps_l.phi )   / delta;
    //ds[5][ii] = 0.0;
  }

  return ds;

}

vector< vector< double > > num_dTe_dTs( const Trk &trk, const double len, double delta)
{
  //Numerical jacobians of pdf parameters differentiated by track parameters
  vector< vector<double> > ds( 7, vector<double>(10, 0) );
    
  // E, x, y, z, dx, dy, dz, dtheta, dphi, t
  for( uint ii = 0; ii != 10; ii++ ){

    std::pair< Trk, Trk > trks_e = delta_trks_e( trk, len, ii, delta );

    ds[0][ii] = ( trks_e.second.E           - trks_e.first.E )           / delta;
    ds[1][ii] = ( trks_e.second.pos.x       - trks_e.first.pos.x )       / delta;
    ds[2][ii] = ( trks_e.second.pos.y       - trks_e.first.pos.y )       / delta;
    ds[3][ii] = ( trks_e.second.pos.z       - trks_e.first.pos.z )       / delta;
    ds[4][ii] = ( trks_e.second.dir.theta() - trks_e.first.dir.theta() ) / delta;
    ds[5][ii] = ( trks_e.second.dir.phi()   - trks_e.first.dir.phi() )   / delta;
    ds[6][ii] = ( trks_e.second.t           - trks_e.first.t )           / delta;
  }

  return ds;

};

double CascadePdf::dnpe_dt( const Paramst &p ) { return eval1( p, _pdf_func ).f; };
double CascadePdf::npe(     const Paramst &p ) { return eval1( p, _pdf_func ).v; };
double CascadePdf::npe(     const Params  &p ) { return eval1( p, _npe_func ); };
//double CascadePdf::npe_V(     const Paramst  &p ) { return eval1( p, _pdf_func ).V; };

double CascadePdf::dnpe_dt ( double E, double D, double cd, double theta, double phi, double t ) { return dnpe_dt( Paramst(E, D, cd, theta, phi, t )); }
double CascadePdf::npe ( double E, double D, double cd, double theta, double phi, double t ) { return npe( Paramst(E, D, cd, theta, phi, t)); }
double CascadePdf::npe ( double E, double D, double cd, double theta, double phi ) { return npe( Params(E, D, cd, theta, phi )); }
//double CascadePdf::npe ( double E, double D, double cd, double theta, double phi ) { return npe_V( Paramst(E, D, cd, theta, phi, 0.0)); }

//Need to tell compiler which instantiation will be used here
//template<> vector<double> num_dnpe_dXs<CascadePdf>( CascadePdf *pdf, double D, double cd, double theta, double phi, double t, double E, double delta );

/*
int get_trk_hit_hash( Trk &trk, Hit &hit, uint precision = 6){

  Pmt unhit;

  string s = get_trk_unhit_hash( trk, unhit.dress( hit ) ) +
    to_string( trk.t ).substr( 0, precision ) +
    to_string( hit.t ).substr( 0, precision );

  hash<string> hasher;
  return hasher( s );
}

int get_trk_unhit_hash( Trk &trk, Pmt &unhit, uint precision = 6 ){

  string s = 
    to_string( trk.E     ).substr( 0, precision ) +
    to_string( trk.pos.x ).substr( 0, precision ) +
    to_string( trk.pos.y ).substr( 0, precision ) +
    to_string( trk.pos.z ).substr( 0, precision ) +
    to_string( trk.dir.x ).substr( 0, precision ) +
    to_string( trk.dir.y ).substr( 0, precision ) +
    to_string( trk.dir.z ).substr( 0, precision ) +
    
    to_string( hit.pos.x ).substr( 0, precision ) +
    to_string( hit.pos.y ).substr( 0, precision ) +
    to_string( hit.pos.z ).substr( 0, precision ) +
    to_string( hit.dir.x ).substr( 0, precision ) +
    to_string( hit.dir.y ).substr( 0, precision ) +
    to_string( hit.dir.z ).substr( 0, precision );

  hash<string> hasher;
  return hasher(s);
}
*/