#include <iostream>
#include <string>

#include "TObjArray.h"

#include "CandFitTrackSR/BFieldSR.h"
#include "CandFitTrackSR/SwimObjSR.h"
#include "Swimmer/SwimGeo.h"
#include "Swimmer/SwimPlaneInterface.h"
#include "Conventions/Munits.h"
#include "RerootExodus/RerootExodus.h"
#include "UgliGeometry/UgliGeomHandle.h"
#include "UgliGeometry/UgliScintPlnHandle.h"
#include "UgliGeometry/UgliSteelPlnHandle.h"
#include "Validity/VldContext.h"
#include "MessageService/MsgService.h"
#include <cassert>
#include <cmath>

//______________________________________________________________________
CVSID("$Id: SwimObjSR.cxx,v 1.17 2007/11/11 08:44:45 rhatcher Exp $");

//______________________________________________________________________
SwimObjSR::SwimObjSR(Double_t u,Double_t v,Double_t z,Double_t dudz,Double_t dvdz,Double_t qp,Int_t izfor,const VldContext *vldc)
{
  MSG("CandFitTrackSR",Msg::kError)
    << "SwimObjSR is obsolete!" << endl;
  assert(0);

  fu = u;
  fv = v;
  fz = z;
  fdudz = dudz;
  fdvdz = dvdz;
  fqp = qp;
  fizfor = izfor;

  fvldc = const_cast<VldContext*>(vldc);
  fugh = new UgliGeomHandle(*fvldc);

  Int_t iplnmax = 0;
  Int_t iplngap0 = 9999;
  Int_t iplngap1 = -1;
  switch(fvldc->GetDetector()) {
  case Detector::kFar:
    iplnmax = 482;
    iplngap0 = 240;
    iplngap1 = 243;
    break;
  case Detector::kNear:
    iplnmax = 281;
    break;
  case Detector::kCalib:
    iplnmax = 60;
    break;
  default:
     MSG("CandFitTrackSR",Msg::kError)
        << "BFieldSR does not support the "
        << Detector::AsString(vldc->GetDetector())
        << " detector " << endl;
     assert(0);

  }

}

SwimObjSR::~SwimObjSR()
{
  assert(fugh);
  delete fugh;
}

Int_t SwimObjSR::SwimTo(Double_t zfin, Bool_t docheck)
{


  if (fz==zfin) return 0;

  if (docheck && DoCheck()) return 1;

  Int_t idir = 1;
  if (zfin<fz) {
    idir = -1;
  }


  SwimGeo* sgeo = SwimGeo::Instance(*fvldc);

  Int_t found=0;
  SwimGeo::SwimMaterial_t begmat = SwimGeo::kAir;
  SwimGeo::SwimMaterial_t endmat = SwimGeo::kAir;
  Int_t ibeg=0;
  Int_t iend=0;
  const TObjArray& interfaces = sgeo->GetInterfaces();

  if (idir>0) {
    for (Int_t i=0; i <= interfaces.GetLast() && found<2; i++) {
      SwimPlaneInterface *spi = 
        dynamic_cast<SwimPlaneInterface*>(interfaces.At(i));
      if (!found && fz<spi->GetZ()) {
        begmat = spi->GetMaterialBefore();
        found++;
        ibeg = i;
      }
      if (found==1 && zfin<spi->GetZ()) {
        endmat = spi->GetMaterialAfter();
        found++;
        iend = i+1;
      }
    }
  }
  else {
    for (Int_t i = interfaces.GetLast(); i>=0 && found<2; i--) {
      SwimPlaneInterface *spi = 
        dynamic_cast<SwimPlaneInterface*>(interfaces.At(i));
      if (!found && fz>spi->GetZ()) {
        begmat = spi->GetMaterialAfter();
        found++;
        ibeg = i;
      }
      if (found==1 && zfin>spi->GetZ()) {
        endmat = spi->GetMaterialBefore();
        found++;
        iend = i-1;
      }
    }
  }

  if (found<2) return 1;

  for (Int_t i=ibeg; i!=iend; i+=idir) {
    SwimPlaneInterface *spi = 
      dynamic_cast<SwimPlaneInterface*>(interfaces.At(i));
    Double_t zend = spi->GetZ();
    if (i==iend-idir) {
      zend = zfin;
    }

    SwimGeo::SwimMaterial_t imat = 
      (idir>0) ? spi->GetMaterialBefore() : spi->GetMaterialAfter();

    Int_t flag = 0;
    switch (imat) {
    case 0: // air
      fu += fdudz*(zend-fz);
      fv += fdvdz*(zend-fz);
      fz = zend;
      break;
    case 1: // scintillator
      fu += fdudz*(zend-fz);
      fv += fdvdz*(zend-fz);
      fz = zend;
      // should do energy loss here
      break;
    case 2: // iron
      flag = SwimToIron(zend,docheck);
      break;
    default:
      MSG("FitTrackSR",Msg::kError) << "undefined type " << imat << "\n";
      break;
    }
    if (flag!=0) return flag;
    if (docheck && DoCheck()) return 1;
  }


  return 0;
}

Int_t SwimObjSR::SwimToIron(Double_t zfin,Bool_t docheck)
{
  //unused: Int_t idir=1;

  Double_t swimdz = .00508*Munits::m;

  Double_t v0[5];
  Double_t uu[5];
  Double_t vv[5];

  Double_t psum[5];
  Double_t fvarr[5];
  Double_t pfac[4] = {1.,2.,2.,1.};
  Double_t vfac[4] = {0.5,0.5,1.,1.};

  Double_t dz = zfin-fz;

  vv[0] = fdudz;
  vv[1] = fdvdz;
  vv[2] = fu;
  vv[3] = fv;
  vv[4] = fz;

  for (Int_t i=0; i<5; i++) {
    v0[i] = vv[i];
  }

  Int_t nstep;
  BFieldSR bf(fvldc);

  if (dz>0.) {
    nstep = (Int_t)(dz/swimdz);
  } else {
    nstep = -(Int_t)(dz/swimdz);
  }
  Double_t del = swimdz;
  if (dz<0.) del *= -1.;
  for (Int_t istep=0; istep<=nstep; istep++) {
    if (istep==nstep) {
      if (dz>0.) {
        del = dz-swimdz*istep;
      } else {
        del = dz+swimdz*istep;
      }
    }
    for (Int_t i=0; i<5; i++) {
      uu[i] = vv[i];
      psum[i] = 0.;
    }
    for (Int_t ik=0; ik<4; ik++) {
// not sure if this is the correct rotation
// the old bfield maps expect cm
      //unused: Double_t x;
      //unused:Double_t y;
//      fugh->uv2xy(vv[2],vv[3],x,y);
//      TVector3 xyz(x*Munits::cm,y*Munits::cm,vv[4]*Munits::cm);
      TVector3 xyz(0.7071068*(vv[2]-vv[3])*Munits::m,
                   0.7071068*(vv[2]+vv[3])*Munits::m,vv[4]*Munits::m);
      TVector3 bxyz = bf.GetBField(xyz);
// rotate bxyz into buvz
// bxyz in Tesla, convert into GeV/c/m
// warning: numbers are hard coded here
//      TVector3 buvz = fugh->xyz2uvz(bxyz);
      TVector3 buvz(0.7071068*(bxyz[1]+bxyz[0]),
                    0.7071068*(bxyz[1]-bxyz[0]),bxyz[2]);
      for (Int_t ibf=0; ibf<3; ibf++) {
        buvz[ibf] *= 0.3;
      }
      fvarr[0] = fizfor*fqp*sqrt(1+vv[0]*vv[0]+vv[1]*vv[1])*
        (buvz[2]*vv[1]+buvz[0]*vv[0]*vv[1]-buvz[1]*(1+vv[0]*vv[0]));
      fvarr[1] = fizfor*fqp*sqrt(1+vv[0]*vv[0]+vv[1]*vv[1])*
        (-buvz[2]*vv[0]-buvz[1]*vv[0]*vv[1]+buvz[0]*(1+vv[1]*vv[1]));
      fvarr[2] = vv[0]; 
      fvarr[3] = vv[1];
      fvarr[4] = 1.;
      for (Int_t il=0; il<5; il++) {
        psum[il] += pfac[ik]*fvarr[il];
        vv[il] = uu[il]+vfac[ik]*del*fvarr[il];
      }
    }
    for (Int_t ik=0; ik<5; ik++) {
      vv[ik] = uu[ik]+del*psum[ik]/6.;
    }
// energy loss, for now consider constant energy loss (working in GeV/m)
    Double_t eloss = 1.57*Munits::GeV/Munits::m*del*(Double_t)(fizfor)*
                     sqrt(1.+vv[0]*vv[0]+vv[1]*vv[1]);
    if (fqp!=0.) {
      Double_t pnow = 1./TMath::Abs(fqp);
      if (eloss<0. || (pnow>eloss &&
          1./TMath::Abs(fqp*pnow/(pnow-eloss))>0.20*Munits::GeV)) {
        fqp = fqp*pnow/(pnow-eloss);
      }
    }
    if (docheck) {
      if (TMath::Abs(vv[2])>4. || TMath::Abs(vv[3])>4. || TMath::Abs(vv[0])>50. || TMath::Abs(vv[1])>50. || TMath::Abs(fqp)>20.) return 1;
      Double_t thisx = .707*(vv[2]-vv[3]);
      Double_t thisy = .707*(vv[2]+vv[3]);
      Double_t thisdxdz = .707*(vv[0]-vv[1]);
      Double_t thisdydz = .707*(vv[0]+vv[1]);
      if (TMath::Abs(thisx)>4. || TMath::Abs(thisy)>4. || TMath::Abs(thisdxdz)>50. || TMath::Abs(thisdydz)>50.) return 1;
    }
//     cout << "swim " << vv[2] << " " << vv[3] << " " << vv[0] << " " << vv[1] << " " << vv[4] << " " << fqp << "\n";
  }
  fdudz = vv[0];
  fdvdz = vv[1];
  fu = vv[2];
  fv = vv[3];
  fz = vv[4];

  return 0;

}

Bool_t SwimObjSR::DoCheck() {
  if (TMath::Abs(fu)>4. || TMath::Abs(fv)>4. || TMath::Abs(fdudz)>50. || TMath::Abs(fdvdz)>50. || TMath::Abs(fqp)>20.) return 1;
  Double_t thisx = .707*(fu-fv);
  Double_t thisy = .707*(fu+fv);
  Double_t thisdxdz = .707*(fdudz-fdvdz);
  Double_t thisdydz = .707*(fdudz+fdvdz);
  if (TMath::Abs(thisx)>4. || TMath::Abs(thisy)>4. || TMath::Abs(thisdxdz)>50. || TMath::Abs(thisdydz)>50.) return 1;
  return 0;
}