#ifndef REVE_MCHelixLine_H
#define REVE_MCHelixLine_H

#include <Reve/Track.h>
#include <cassert>
#include <vector>

namespace Reve {

struct MCVertex
{
  Float_t x,y,z,t;

  MCVertex() : x(0), y(0), z(0), t(0) {}
  MCVertex(Float_t _x, Float_t _y, Float_t _z, Float_t _t=0) :
    x(_x), y(_y), z(_z), t(_t) {}

  Float_t Mag()  const { return TMath::Sqrt(x*x+y*y+z*z);}
  Float_t Mag2() const { return x*x+y*y+z*z;}

  Float_t Perp()  const { return TMath::Sqrt(x*x+y*y);}
  Float_t Perp2() const { return x*x+y*y;}
  Float_t R()     const { return Perp(); }

  MCVertex operator + (const MCVertex & b)
  { return MCVertex(x + b.x, y + b.y, z + b.z, t + b.t); }

  MCVertex operator - (const MCVertex & b)
  { return MCVertex(x - b.x, y - b.y, z - b.z, t - b.t); }

  MCVertex operator * (Float_t a)
  { return MCVertex(a*x, a*y, a*z, a*t); }

  MCVertex& operator +=(const MCVertex & b)
  { x += b.x; y += b.y; z += b.z; t += b.t; return *this; }
};


struct MCStruct
{
  MCStruct(const MCStruct&);            // Not implemented
  MCStruct& operator=(const MCStruct&); // Not implemented
 
  TrackRnrStyle*         fRnrMod;  
  std::vector<MCVertex>* fPoints;
  MCVertex               fV;
  Float_t                fVelocity; // size of particle velocity
  
  MCStruct(TrackRnrStyle* rs, MCVertex* v0 , Float_t vel, std::vector<MCVertex>* tpv) :
    fRnrMod   (rs),
    fPoints   (tpv),
    fV        (*v0),
    fVelocity (vel)
  {
    fPoints->push_back(fV);
  }
  virtual ~MCStruct() {}
};

/**************************************************************************/
//  HELIX
/**************************************************************************/

struct MCHelix : public MCStruct
{
  // constant
  Float_t fA;       // contains charge and magnetic field data

  //parameters dependend pT and pZ size, set in init function
  Float_t fLam;        // momentum ratio pT/pZ
  Float_t fR;          // a/pT
  Float_t fPhiStep;    // step size in xy projection, dependent of RnrMode and momentum
  Float_t fTimeStep;   
 
  Int_t   fN;           // step number in helix;
  Int_t   NMax;         // max number of points in helix
  Float_t x_off, y_off; // offset for fitting daughters
  Float_t sin, cos;
  Bool_t  crosR;       

  MCHelix(TrackRnrStyle* rs, MCVertex* v0, Float_t vel,
          std::vector<MCVertex>* tpv, Float_t a) :
    MCStruct(rs, v0 , vel, tpv),
    fA   (a),
    fLam (0),   fR (0), fPhiStep (0), fTimeStep (0),
    fN   (0), NMax (0),
    x_off(0), y_off(0),
    sin  (0), cos  (0), crosR (0)
  {}

  void Init(Float_t pT, Float_t pZ)
  {
    fN    = 0;
    crosR = false;
    x_off = 0;
    y_off = 0;
    fLam  = pZ/pT;
    fR    = pT/fA;

    fPhiStep = fRnrMod->fMinAng * TMath::DegToRad();
    if (fRnrMod->fDelta < TMath::Abs(fR))
    {
      Float_t ang  = 2*TMath::ACos(1 - fRnrMod->fDelta/TMath::Abs(fR));
      if (ang < fPhiStep) fPhiStep = ang; 
    }
    if (fA < 0) fPhiStep = -fPhiStep;

    // printf("MCHelix::init (%f/%f) labda %f time step %e phi step %f \n", pT, pZ,fLam,  fTimeStep,fPhiStep);
    fTimeStep = TMath::Abs(fR*fPhiStep)*TMath::Sqrt(1+fLam*fLam)/fVelocity;
    fTimeStep *= 0.01; //cm->m

    sin = TMath::Sin(fPhiStep); 
    cos = TMath::Cos(fPhiStep);
  }

  void SetBounds()
  {
    // check steps for max orbits
    NMax = Int_t(fRnrMod->fMaxOrbs*TMath::TwoPi()/TMath::Abs(fPhiStep));
    // check steps for Z boundaries
    Float_t nz;
    if(fLam > 0) {
      nz = (fRnrMod->fMaxZ - fV.z)/(fLam*TMath::Abs(fR*fPhiStep));
    } else {
      nz = (-fRnrMod->fMaxZ - fV.z)/(fLam*TMath::Abs(fR*fPhiStep));
    }
    //  printf("steps in helix line %d nz   %f vz %f\n", NMax, nz, fV.z);
    if (nz < NMax) NMax = Int_t(nz + 1);
    
    // check steps if circles intersect
    if(TMath::Sqrt(fV.x*fV.x+fV.y*fV.y) < fRnrMod->fMaxR + TMath::Abs(fR))
    {
      crosR = true;
    }
    // printf("end steps in helix line %d \n", NMax);
  }


  void Step(Float_t &px, Float_t &py, Float_t &/*pz*/) 
  {
    fV.t += fTimeStep;
    fV.x += (px*sin - py*(1 - cos))/fA + x_off;
    fV.y += (py*sin + px*(1 - cos))/fA + y_off;
    fV.z += fLam*TMath::Abs(fR*fPhiStep);
    fPoints->push_back(fV);
    Float_t px_t = px*cos - py*sin;
    Float_t py_t = py*cos + px*sin;
    px = px_t;
    py = py_t;
    ++fN;
  }


  Bool_t LoopToVertex(Float_t &px, Float_t &py, Float_t &pz, 
                      Float_t  ex, Float_t  ey, Float_t  ez)
  {
    Float_t p0x = px, p0y = py;
    Float_t zs = fLam*TMath::Abs(fR*fPhiStep);
    Float_t maxrsq  = fRnrMod->fMaxR * fRnrMod->fMaxR;
    Float_t fnsteps = (ez - fV.z)/zs;
    Int_t   nsteps  = Int_t((ez - fV.z)/zs);
    Float_t sinf = TMath::Sin(fnsteps*fPhiStep);
    Float_t cosf = TMath::Cos(fnsteps*fPhiStep);

    { 
      if (nsteps > 0)
      {
        Float_t xf  = fV.x + (px*sinf - py*(1 - cosf))/fA;  
        Float_t yf =  fV.y + (py*sinf + px*(1 - cosf))/fA;
        x_off =  (ex - xf)/fnsteps;
        y_off =  (ey - yf)/fnsteps;
        Float_t xforw, yforw, zforw;
        for (Int_t l=0; l<nsteps; l++)
	{
          xforw  = fV.x + (px*sin - py*(1 - cos))/fA + x_off;
          yforw =  fV.y + (py*sin + px*(1 - cos))/fA + y_off;
          zforw =  fV.z + fLam*TMath::Abs(fR*fPhiStep);
          if (xforw*xforw+yforw*yforw > maxrsq   ||
              TMath::Abs(zforw) > fRnrMod->fMaxZ)
          {
            return false;
          }
          Step(px, py, pz);
	  if (fN > NMax)
	    break;
        }
	 
      }
      // set time to the end point
      fV.t += TMath::Sqrt((fV.x-ex)*(fV.x-ex)+(fV.y-ey)*(fV.y-ey) +(fV.z-ez)*(fV.z-ez))/fVelocity;
      fV.x = ex; fV.y = ey; fV.z = ez;      
      fPoints->push_back(fV);
    }
      
    { // fix momentum in the remaining part
      Float_t cosr = TMath::Cos((fnsteps-nsteps)*fPhiStep); 
      Float_t sinr = TMath::Sin((fnsteps-nsteps)*fPhiStep); 
      Float_t px_t = px*cosr - py*sinr;
      Float_t py_t = py*cosr + px*sinr;
      px = px_t;
      py = py_t;
    }
    { // calculate direction of faked px,py
      Float_t pxf = (p0x*cosf - p0y*sinf)/TMath::Abs(fA) + x_off/fPhiStep;
      Float_t pyf = (p0y*cosf + p0x*sinf)/TMath::Abs(fA) + y_off/fPhiStep;
      Float_t fac = TMath::Sqrt((p0x*p0x + p0y*p0y) / (pxf*pxf + pyf*pyf));
      px = fac*pxf;
      py = fac*pyf;
    }
    return true;
  }
    
  Bool_t LoopToBounds(Float_t &px, Float_t &py, Float_t &pz)
  {
    //    printf("MC helix  loop_to_bounds\n");
    SetBounds();
    if (NMax > 0)
    {
      // printf("NMax MC helix  loop_to_bounds\n");
      MCVertex forw;
      Float_t maxrsq = fRnrMod->fMaxR * fRnrMod->fMaxR;
      while (fN < NMax)
      {
        forw.x = fV.x + (px*sin - py*(1 - cos))/fA + x_off;
        forw.y = fV.y + (py*sin + px*(1 - cos))/fA + y_off;
        forw.z = fV.z + fLam*TMath::Abs(fR*fPhiStep);
        forw.t = fV.t + fTimeStep;
	
        if (crosR && forw.Perp2() > maxrsq)
        {
          Float_t t = (fRnrMod->fMaxR - fV.R()) / (forw.R() - fV.R());
          assert(t >= 0 && t <= 1);
          fPoints->push_back(fV + (forw-fV)*t);
          return false;
        }
        if (TMath::Abs(forw.z) > fRnrMod->fMaxZ)
        {
          Float_t t = (fRnrMod->fMaxZ - TMath::Abs(fV.z)) / TMath::Abs((forw.z - fV.z));
          assert(t >= 0 && t <= 1);
          fPoints->push_back(fV + (forw-fV)*t);
          return false;
        }

        Step(px, py, pz);
      }
      return true;
    }
    return false;
  }
};

/**************************************************************************/
//  LINE
/**************************************************************************/

struct MCLine : public MCStruct
{
  MCLine(TrackRnrStyle* rs, MCVertex* v0 ,Float_t vel, std::vector<MCVertex>* tpv):
    MCStruct(rs, v0 , vel, tpv)
  {}

  Bool_t InBounds(Float_t ex, Float_t ey, Float_t ez)
  {
    if(TMath::Abs(ez) > fRnrMod->fMaxZ ||
       ex*ex + ey*ey  > fRnrMod->fMaxR*fRnrMod->fMaxR)
      return false;
    else
      return true;
  }

  void GotoVertex(Float_t  x1, Float_t  y1, Float_t  z1)
  {
    fV.t += TMath::Sqrt((fV.x-x1)*(fV.x-x1)+(fV.y-y1)*(fV.y-y1)+(fV.z-z1)*(fV.z-z1))/fVelocity;
    fV.x=x1; fV.y=y1; fV.z=z1;
    fPoints->push_back(fV);
  }
  

  void GotoBounds( Float_t px, Float_t py, Float_t pz)
  {
    Float_t tZ = 0,Tb = 0;
    // time where particle intersect +/- fMaxZ
    if (pz > 0) {
      tZ = (fRnrMod->fMaxZ - fV.z)/pz;
    }
    else  if (pz < 0 ) {
      tZ = (-1)*(fRnrMod->fMaxZ + fV.z)/pz;
    }
    // time where particle intersects cylinder
    Float_t tR=0;
    Double_t a = px*px + py*py;
    Double_t b = 2*(fV.x*px + fV.y*py);
    Double_t c = fV.x*fV.x + fV.y*fV.y - fRnrMod->fMaxR*fRnrMod->fMaxR;
    Double_t D = b*b - 4*a*c;
    if(D >= 0) {
      Double_t D_sqrt=TMath::Sqrt(D);
      tR = ( -b - D_sqrt )/(2*a);
      if( tR < 0) {
        tR = ( -b + D_sqrt )/(2*a);
      }

      // compare the two times
      Tb = tR < tZ ? tR : tZ;
    } else {
      Tb = tZ;
    }

    GotoVertex(fV.x+px*Tb, fV.y+py*Tb, fV.z+ pz*Tb);
  }
}; // struct Line


} // namespace Reve

#endif