#ifndef __MONOPOLE__
#define __MONOPOLE__

#include <fcntl.h>
#include <iostream>
#include <iomanip>
#include <vector>
#include <map>
#include <stdexcept>
#include <cstdarg>

#include "jeep/string_cast.hh"
#include "trigger/util.hh"

#include "geometry.hh"
#include "tools.hh"
#include "minuit.hh"

TH1D *hist;
const double lam = 60;
double sigma;

namespace MONOPOLE {

  using namespace UTIL;
  using namespace GEOMETRY;
  using namespace NR;

  static const int NUMBER_OF_PARAMETERS = 6;    //!< Number of free parameters in fit


  /**
   * Auxiliary class for track definition for minuit
   */
  class fitted_vtrack : public vtrack {
  public:
    /**
     * Default constructor
     */
    fitted_vtrack() :
      vtrack(), 
      error_()
    {
      reset();
    }

    /**
     * Constructor
     * \param  a       a position [m]
     * \param  b       b position [m]
     * \param  t       time at position [ns]
     * \param  theta   polar angle [rad]
     * \param  phi     azimuth angle [rad]
     * \param  beta    velocity [unit]
     * \param  ea      error on a [m]
     * \param  eb      error on b [m]
     * \param  et      error on t [ns]
     * \param  etheta  error on theta [rad]
     * \param  ephi    error on phi [rad]
     * \param  ebeta   error on velocity [unit]
     */
    fitted_vtrack(const double& a,
		  const double& b,
		  const double& t,
		  const double& theta,
		  const double& phi,
		  const double& beta,
		  const double& ea     = 0.0,
		  const double& eb     = 0.0,
		  const double& et     = 0.0,
		  const double& etheta = 0.0,
		  const double& ephi   = 0.0,
		  const double& ebeta  = 0.0) :
      vtrack(a, b, t, theta, phi, beta),
      error_(ea, eb, et, etheta, ephi, ebeta)
    {
      reset();
    }

    /**
     * Constructor
     * \param  start_  start value
     */
    fitted_vtrack(const vtrack& start_) :
      vtrack(start_),
      error_()
    {
      reset();
    }

    /**
     * Constructor
     * \param  in  input stream
     */
    fitted_vtrack(std::istream& in) :
      vtrack(),
      error_()
    {
      read(in);
    }

    /**
     * Constructor
     * \param  o       3D point
     * \param  dir_    direction
     * \param  beta    velocity [unit]
     */
    fitted_vtrack(const point&     o,
		  const direction& dir_,
		  const float&     beta) :
      vtrack(o,dir_,beta),
      error_()
    {
      reset();
    }

    const vtrack& value() const { return (const vtrack&) *this; }
    const vtrack& error() const { return (const vtrack&) error_; }

    /**
     * Rotate fitted track
     * \param  R  rotation matrix
     * \return    rotated fitted track
     */
    fitted_vtrack& rotate(const rotation& R)
    {
      track::rotate(R);
      //error_.rotate(R);
      return *this;
    }

    /**
     * Rotate back fitted track
     * \param  R  rotation matrix
     * \return    rotated fitted track
     */
    fitted_vtrack& rotate_back(const rotation& R)
    {
      track::rotate_back(R);
      //error_.rotate_back(R);
      return *this;
    }

    /**
     * read from input stream
     * \param  in  input stream
     * \return     input stream
     */
    std::istream& read(std::istream& in)
    {
      vtrack::read(in);
      error_.read(in);
      in >> fmin_ >>  fedm_ >> errdef_ >> npari_ >> nparx_ >> istat_ >> size_;
      return in;
    }

    /**
     * write to output stream
     * \param  out output stream
     * \return     output stream
     */
    std::ostream& write(std::ostream& out) const
    {
      vtrack::write(out);
      out << ' ';
      error_.write(out);
      out << ' ' << fmin_ << ' ' <<  fedm_ << ' ' << errdef_ << ' ' << npari_ << ' ' << nparx_ << ' ' << istat_ << ' ' << size_;
      return out;
    }

    const double& fmin()   const { return fmin_; }     //!< Best function value
    const double& fedm()   const { return fedm_; }     //!< Estimated vertical distance to minimum
    const double& errdef() const { return errdef_; }   //!< UP parameter
    const int&    npari()  const { return npari_; }    //!< Number of variable parameters
    const int&    nparx()  const { return nparx_; }    //!< Number of parameters
    const int&    istat()  const { return istat_; }    //!< Status
    const int&    size()   const { return size_; }     //!< Number of data points

    void reset()
    { 
      fmin_  = DBL_MAX;
      istat_ = -1;
      size_  = 0;
    }

    /**
     * Get current minuit status
     * \param n  number of data points (optional)
     */
    void mnstat(const int& n = 0) 
    {
      mnstat_(&fmin_, &fedm_, &errdef_, &npari_, &nparx_, &istat_);

      size_ = n;
    }

    /**
     * Get error matrix
     */
    void mnemat(FCN fcn) 
    {
      char* command = "HESSE";
      int   icondn;

      mncomd_(fcn, command, &icondn, NULL, sizeof(command));      

      const int ndim = NUMBER_OF_PARAMETERS;

      mnemat_(&(V[0][0]),&ndim);
    }

    /**
     * Get error matrix
     */
    void mnemat() 
    {
      const int ndim = NUMBER_OF_PARAMETERS;

      mnemat_(&(V[0][0]),&ndim);
    }

  protected:
    vtrack error_;
    double fmin_;
    double fedm_;
    double errdef_;
    int    npari_;
    int    nparx_;
    int    istat_;
    int    size_;
  public:
    double V[NUMBER_OF_PARAMETERS][NUMBER_OF_PARAMETERS];
  };


  /**
   * Auxiliary class for fit parameters
   */
  class parameter {
  public:
    /**
     * Constructor
     * \param  chnam_  name of parameter
     * \param  stval_  start value
     * \param  step_   step size
     * \param  bnd1_   lower bound
     * \param  bnd2_   upper bound
     */
    parameter(const char*   chnam_,
	      const double& stval_,
	      const double& step_,
	      const double& bnd1_,
	      const double& bnd2_) :
      chnam(chnam_),
      stval(stval_),
      step(step_),
      bnd1(bnd1_),
      bnd2(bnd2_)
    {}

  public:
    const char* chnam;
    double stval;
    double step;
    double bnd1;
    double bnd2;
  };


  /** 
   * Initialisation method for 3D minuit fit
   * \param debug      debug mode
   */
  void mninit(const bool& debug = false)
  {
    const int lun = 7;

    if (!debug) 
      system((std::string("ln -sf /dev/null fort.") + string_value(lun)).c_str());

    int ird  = 5;
    int iwr  = (debug ? 6 : lun);
    int isav = (debug ? 6 : lun);

    mninit_(&ird, &iwr, &isav);

    if (debug) {
      int icondn; 
      mncomd_(NULL, "SET PRINTOUT 3", &icondn, NULL, 14);
    }
  }


  namespace $$$ {
    static std::vector<hit> input;           //!< internal data for minuit fit
  }


  /**
   * 3D fit function: Likelihood
   * \param  npar   number of parameters
   * \param  grad   derivatives of chi^2
   * \param  fval   chi^2
   * \param  xval   parameter values
   * \param  iflag  steering flag
   * \param  dummy  utility function
   */
  void mnfit3M(int *npar, double *grad, double *fval, double *xval, int *iflag, void *dummy)
  {
    using namespace $$$;
    using namespace UTIL;
    using namespace std;

    double& a     = xval[0];
    double& b     = xval[1];
    double& t0    = xval[2];
    double& theta = xval[3];
    double& phi   = xval[4];
    double& beta  = xval[5];

    const double PMIN = 0.05;

    direction dir(theta,phi);
    rotation  R(dir);

    double da;
    double db;
    double r;
    double p;

    double cost;
    double A;
    double B;
    double AA;
    double path;

    *fval = 0;

    for (std::vector<hit>::const_iterator i = input.begin(); i != input.end(); ++i) {
	
      position o(i->x(),
		 i->y(),
		 i->z());

      o.rotate(R);

      da = o.x() - a;
      db = o.y() - b;

      r  = sqrt(da*da + db*db);	

      if (r < 1)
	r = 1;

      A    = ( ( beta * C )/r ) * ( i->t() - t0 - ( o.z()/( beta * C) ) );
      B    = beta * getIndexOfRefraction() ;
      double dum = A*A - B*B + 1;

      if(dum >= 0)
	{
	  cost = ( B - A * sqrt( dum ) )/( A*A + 1 );
	  
	  // cost dependence
	  
	  int bin = hist->FindBin(cost);
	  
	  p  = log( (1/(1.0 + PMIN)) * (hist->GetBinContent(bin) + PMIN/2) );
	  
	  // r dependence
	  
	  path = r / sqrt((1+cost)*(1-cost)) / lam;
	  AA   = 1/( sigma*sigma );
	  
	  p += log( (1/(1.0 + PMIN)) * (AA * path * exp(-0.5 * AA * path*path) + PMIN/4) );
	}
      else
	{
	  p  = log(PMIN/2);
	  p += log(PMIN/4);
	}

      *fval -= p;

    }
  }

  /**
   * 3D muon fit function: Likelihood
   * \param  npar   number of parameters
   * \param  grad   derivatives of chi^2
   * \param  fval   chi^2
   * \param  xval   parameter values
   * \param  iflag  steering flag
   * \param  dummy  utility function
   */
  void mnfit3mu(int *npar, double *grad, double *fval, double *xval, int *iflag, void *dummy)
  {
    using namespace $$$;
    using namespace UTIL;
    using namespace std;
    
    double& a     = xval[0];
    double& b     = xval[1];
    double& t0    = xval[2];
    double& theta = xval[3];
    double& phi   = xval[4];
    double& beta  = xval[5];
    
    const double PMIN = 0.05;

    direction dir(theta,phi);
    rotation  R(dir);
    
    double da;
    double db;
    double r;
    double p;
    
    double residual;
    
    double tant;
    double cost;
    double AA;
    double path;

    *fval = 0;

    for (std::vector<hit>::const_iterator i = input.begin(); i != input.end(); ++i) {
      
      position o(i->x(),
		 i->y(),
		 i->z());
      
      o.rotate(R);
      
      da = o.x() - a;
      db = o.y() - b;
      
      r  = sqrt(da*da + db*db);	
      
      if(r < 1)
	r = 1;

      tant = (C/r) * ( i->t() - t0 - o.z()/C );

      if (tant >= 0.001) {

	cost = cos(atan(tant));
	
	// cost dependence

	double sgm = 0.075; // 0.075
	double mn  = 1/getIndexOfRefraction();
	residual = ( cost - mn )/sgm ;

	p  = log( (1/(1.0 + PMIN)) * ( (1/(sgm*sqrt(2*PI))) * exp(-0.5 * residual * residual) + PMIN/2) );
	
	// r dependence
	
	path = r / sqrt((1+cost)*(1-cost)) / lam;
	AA   = 1/( sigma*sigma );

	p += log( (1/(1.0 + PMIN)) * (AA * path * exp(-0.5 * AA * path*path) + PMIN/4) );

      } else {

	p  = log(PMIN/2);
	p += log(PMIN/4);
      }

      *fval -= p;
    }
  }

  /**
   * Steering method for minuit fit
   * \param fit        start values
   * \param begin_     begin of hit data
   * \param end_       end of hit data
   * \param cmd        fit command
   * \return           fit values
   */
  template<class hitIterator>
  fitted_vtrack minuit(vtrack fit, 
		       hitIterator begin_, 
		       hitIterator end_, 
		       const char* cmd, 
		       ...)
  {
    using namespace $$$;
    using namespace std;

    int icondn;
    
    mncomd_(NULL, "CLEAR", &icondn, NULL, 5);      


    // data 
    input.clear();

    for (hitIterator i = begin_; i != end_; ++i) {
      input.push_back(hit(i->x(),
			  i->y(),
			  i->z(),
			  i->t(),
			  i->a(),
			  i->sigma()));
    }


    // start values
    
    parameter v[] = {
      parameter("a    ", fit.a(),     0.02,  0, 0),
      parameter("b    ", fit.b(),     0.02,  0, 0),
      parameter("t0   ", fit.t0(),    0.05,  0, 0),
      parameter("theta", fit.theta(), 0.003, 0, 0),
      parameter("phi  ", fit.phi(),   0.003, 0, 0),
      parameter("beta ", fit.beta(),  0.01,  0, 0)
    };
    

    int ierflg;
    int num = 0;
      
    for (int i = 0; i != sizeof(v)/sizeof(v[0]); ++i)
      mnparm_(&(++num), v[i].chnam, &(v[i].stval), &(v[i].step), &(v[i].bnd1), &(v[i].bnd2), &ierflg, (int) strlen(v[i].chnam));


    // minuit commands

    vector<string> command;

    command.push_back("SET ERRORDEF 0.5");   // likelihood fit
    command.push_back(cmd);

    va_list  argv;
    va_start(argv,cmd);

    for (char* args = va_arg(argv,char*); args != NULL; args = va_arg(argv,char*))
      command.push_back(args);


    // execute commands

    for (vector<string>::const_iterator i = command.begin(); i != command.end(); ++i)
      mncomd_(mnfit3M, i->c_str(), &icondn, NULL, i->length());      

    
    // result
    
    const int LENGTH = 10;
    
    char   chnam[LENGTH+1];
    double value[NUMBER_OF_PARAMETERS];
    double error[NUMBER_OF_PARAMETERS];
    double bnd1 [NUMBER_OF_PARAMETERS];
    double bnd2 [NUMBER_OF_PARAMETERS];
    
    int ivarbl;

    for (int i = 0; i != NUMBER_OF_PARAMETERS; ++i) {
      int num = i + 1;
      mnpout_(&num, chnam, &value[i], &error[i], &bnd1[i], &bnd2[i], &ivarbl, LENGTH);
    }

    fitted_vtrack result(value[0], value[1], value[2], value[3], value[4], value[5],
			 error[0], error[1], error[2], error[3], error[4], error[5]);

    result.mnstat(distance(begin_,end_));
    result.mnemat();

    return result;
  }

  /**
   * Steering method for minuit muon fit
   * \param fit        start values
   * \param begin_     begin of hit data
   * \param end_       end of hit data
   * \param cmd        fit command
   * \return           fit values
   */
  template<class hitIterator>
  fitted_vtrack minuitmu(vtrack fit, 
			 hitIterator begin_, 
			 hitIterator end_, 
			 const char* cmd, 
			 ...)
  {
    using namespace $$$;
    using namespace std;
    
    int icondn;
    
    mncomd_(NULL, "CLEAR", &icondn, NULL, 5);      
    
    
    // data 
    input.clear();
    
    for (hitIterator i = begin_; i != end_; ++i) {
      input.push_back(hit(i->x(),
			  i->y(),
			  i->z(),
			  i->t(),
			  i->a(),
			  i->sigma()));
    }
    
    
    // start values
    
    parameter v[] = {
      parameter("a    ", fit.a(),     0.02,  0, 0),
      parameter("b    ", fit.b(),     0.02,  0, 0),
      parameter("t0   ", fit.t0(),    0.05,  0, 0),
      parameter("theta", fit.theta(), 0.003, 0, 0),
      parameter("phi  ", fit.phi(),   0.003, 0, 0),
      parameter("beta ", fit.beta(),  0.01,  0, 0)
    };
    

    int ierflg;
    int num = 0;
      
    for (int i = 0; i != sizeof(v)/sizeof(v[0]); ++i)
      mnparm_(&(++num), v[i].chnam, &(v[i].stval), &(v[i].step), &(v[i].bnd1), &(v[i].bnd2), &ierflg, (int) strlen(v[i].chnam));


    // minuit commands

    vector<string> command;

    command.push_back("SET ERRORDEF 0.5");   // likelihood fit
    
    command.push_back(cmd);

    va_list  argv;
    va_start(argv,cmd);

    for (char* args = va_arg(argv,char*); args != NULL; args = va_arg(argv,char*))
      command.push_back(args);


    // execute commands

    for (vector<string>::const_iterator i = command.begin(); i != command.end(); ++i)
      mncomd_(mnfit3mu, i->c_str(), &icondn, NULL, i->length());      

    
    // result
    
    const int LENGTH = 10;
    
    char   chnam[LENGTH+1];
    double value[NUMBER_OF_PARAMETERS];
    double error[NUMBER_OF_PARAMETERS];
    double bnd1 [NUMBER_OF_PARAMETERS];
    double bnd2 [NUMBER_OF_PARAMETERS];
    
    int ivarbl;

    for (int i = 0; i != NUMBER_OF_PARAMETERS; ++i) {
      int num = i + 1;
      mnpout_(&num, chnam, &value[i], &error[i], &bnd1[i], &bnd2[i], &ivarbl, LENGTH);
    }

    fitted_vtrack result(value[0], value[1], value[2], value[3], value[4], value[5],
			 error[0], error[1], error[2], error[3], error[4], error[5]);

    result.mnstat(distance(begin_,end_));
    result.mnemat();

    return result;
  }


  /**
   * Input operator for fitted track object.
   * \param  in   input stream
   * \param  o    fitted track
   * \return      input stream
   */
  static inline std::istream& operator>>(std::istream& in, fitted_vtrack& o)
  {
    return o.read(in);
  }


  /**
   * Output operator for fitted track object.
   * \param  out  output stream
   * \param  o    fitted track
   * \return      output stream
   */
  static inline std::ostream& operator<<(std::ostream& out, const fitted_vtrack& o)
  {
    return o.write(out);
  }
}

#endif