#include <iostream>
#include <string>

#include "TObjArray.h"

#include "CandFitTrackSR/BFieldSR.h"
#include "CandFitTrack3/SwimObj3.h"
#include "Swimmer/SwimGeo.h"
#include "Swimmer/SwimSwimmer.h"
#include "Swimmer/SwimStepper.h"
#include "Swimmer/SwimPrintStepAction.h"
#include "Swimmer/SwimParticle.h"
#include "Swimmer/SwimZCondition.h"
#include "Swimmer/SwimPlaneInterface.h"
#include "TVector3.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 "Conventions/MinosMaterial.h"
#include <cassert>
#include <cmath>
#include "TMath.h"


static const Double_t radfe=13.84e-2;
static const Double_t radsc=43.8e-2;
static const Double_t radair=30420e-2;

static const MinosMaterial::StdMaterial_t fmatSc=MinosMaterial::ePolystyreneMinos;
static const MinosMaterial::StdMaterial_t fmatSteel=MinosMaterial::eIronMinos;

static const Double_t rhofe=MinosMaterial::Density(fmatSteel)*1e3; // used to be 7.87e3;

static const Double_t rhosc=MinosMaterial::Density(fmatSc)*1e3; // used to be 0.86e3 or 1.032e3 as in Roys code

static const Double_t rhoair=1.2;


//______________________________________________________________________
CVSID("$Id: SwimObj3.cxx,v 1.7 2008/06/02 16:00:40 rhatcher Exp $");

//______________________________________________________________________
SwimObj3::SwimObj3(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("FitTrack3",Msg::kVerbose)
        << "SwimObj3::SwimObj3( "
	<< "u= "  << u << " , "
	<< "v= "  << v << " , "
	<< "z= "  << z << " , "
	<< "dudz= "  << dudz << " , "
	<< "dvdz= "  << dvdz << " , "
	<< "qp= "  << qp << " , "
	<< "izfor= "  << izfor << endl;
      
      


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

  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("FitTrack3",Msg::kError)
        << "BFieldSR does not support the "
        << Detector::AsString(vldc->GetDetector())
        << " detector " << endl;
     assert(0);

  }

}

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

Double_t SwimObj3::GetPath() {
   MSG("FitTrack3",Msg::kVerbose)
	<< "tfe= "  << fTfe << " , "
	<< "tair= "  << fTair << " , "
	<< "tsc= "  << fTsc << endl;
   MSG("FitTrack3",Msg::kVerbose)
      << "fu "  << fu << " startU " << fStartu << endl
      << "fv "  << fv << " startV " << fStartv << endl
      << "fz "  << fz << " startZ " << fStartz << endl;
   
    Double_t dtot=fTfe+fTair+fTsc;
    if(dtot==0) return 0; //Maybe do something cleverer

    double path = TMath::Sqrt(TMath::Power((fu-fStartu),2)+
			      TMath::Power((fv-fStartv),2)+
			      TMath::Power((fz-fStartz),2));
   return path;

}


Double_t SwimObj3::GetRange() {
   MSG("FitTrack3",Msg::kVerbose)
	<< "tfe= "  << fTfe << " , "
	<< "tair= "  << fTair << " , "
	<< "tsc= "  << fTsc << endl;
   MSG("FitTrack3",Msg::kVerbose)
      << "fu "  << fu << " startU " << fStartu << endl
      << "fv "  << fv << " startV " << fStartv << endl
      << "fz "  << fz << " startZ " << fStartz << endl;
   
    Double_t dtot=fTfe+fTair+fTsc;
    if(dtot==0) return 0; //Maybe do something cleverer

    double path = TMath::Sqrt(TMath::Power((fu-fStartu),2)+
			      TMath::Power((fv-fStartv),2)+
			      TMath::Power((fz-fStartz),2));

    Double_t tfe=fTfe;
    //Double_t tair=fTair;
    Double_t tsc=fTsc;
    Double_t ffe = tfe/dtot;
    //Double_t fair = tair/dtot;
    Double_t fsc = tsc/dtot;
    Double_t dsc = fsc*path;
    //    Double_t dair = fair*path;
    Double_t dfe = ffe*path;

    Double_t range=0;
    range+=(dsc*rhosc)/10.0; //Converting from kg/m^2 -> g/cm^2
    range+=(dfe*rhofe)/10.0;

    MSG("FitTrack3",Msg::kVerbose)
       << "Returning Range: " << range << endl;
    return range;

}

Double_t SwimObj3::GetTheta() {

    Double_t dtot=fTfe+fTair+fTsc;
    if(dtot==0) return 0; //Maybe do something cleverer

    double path = TMath::Sqrt(TMath::Power((fu-fStartu),2)+
			      TMath::Power((fv-fStartv),2)+
			      TMath::Power((fz-fStartz),2));
    if(path==0) return 0;
    Double_t tfe=fTfe;
    Double_t tair=fTair;
    Double_t tsc=fTsc;

    
    Double_t ffe = tfe/dtot;
    Double_t fair = tair/dtot;
    Double_t fsc = tsc/dtot;
    Double_t dsc = fsc*path;
    Double_t dair = fair*path;
    Double_t dfe = ffe*path;

    Double_t wtot = tfe*rhofe+tsc*rhosc+tair*rhoair;
    Double_t rfe = tfe*rhofe/wtot;
    Double_t rsc = tsc*rhosc/wtot;
    Double_t rair = tair*rhoair/wtot;

    Double_t beta = fPmag/TMath::Sqrt(fPmag*fPmag+(0.105*0.105));
    Double_t rhoav = ffe*rhofe+fsc*rhosc+fair*rhoair;
    Double_t radlen = 1.0/(rfe/radfe+rsc/radsc+rair/radair)/rhoav;

    Double_t theta = (0.0136/(fPmag*beta))*TMath::Sqrt((dfe+dsc+dair)/radlen)*
	(1.0+0.038*TMath::Log((dfe+dsc+dair)/radlen));

    return theta;
}

Int_t SwimObj3::SwimTo(Double_t zfin, Bool_t docheck)
{
   if(docheck) {
   }
   
   MSG("FitTrack3",Msg::kVerbose)
      << "SwimTo(Double_t " << zfin << ") From " << fz << endl;
   if (fz==zfin) return 0;
   fTfe=0;
   fTair=0;
   fTsc=0;
   fStartu=fu;
   fStartv=fv;
   fStartz=fz;
   fPmag=fqp;
   Double_t charge=1;
   if(fPmag<0) {
      fPmag*=-1.0;
      charge=-1;
   }
   
   SwimSwimmer s(*fvldc);
   TVector3 position(fu,fv,fz);
   
   Double_t bottom=sqrt(1.0 + fdudz*fdudz+fdvdz*fdvdz);
   Double_t pu=fPmag*fizfor*fdudz/bottom;
   Double_t pv=fPmag*fizfor*fdvdz/bottom;
   Double_t pz=fPmag*fizfor/bottom;
   
   TVector3 momentum(pu,pv,pz);
   
   SwimParticle muon(position,momentum);
   muon.SetCharge(charge);
   SwimZCondition zc(zfin);
   
   
   bool done;
   done = s.SwimForward(muon,zc);

   Double_t newfu=muon.GetPosition().X();
   Double_t newfv=muon.GetPosition().Y();
   Double_t newfz=muon.GetPosition().Z();
   Double_t newpu=muon.GetMomentum().X();
   Double_t newpv=muon.GetMomentum().Y();
   Double_t newpz=muon.GetMomentum().Z();
   Double_t newpmag=muon.GetMomentumModulus();
   Double_t newdz=muon.GetDirection().Z();
   Double_t newdudz;
   Double_t newdvdz;
   if(newdz!=0) {
      newdudz=muon.GetDirection().X()/newdz;
      newdvdz=muon.GetDirection().Y()/newdz;
   }
   MSG("FitTrack3",Msg::kVerbose)
      << "newfu "  << newfu << " startU " << fStartu << endl
      << "newfv "  << newfv << " startV " << fStartv << endl
      << "newfz "  << newfz << " startZ " << fStartz << endl
      << "newpu "  << newpu << " startpu " << pu << endl
      << "newpv "  << newpv << " startpv " << pv << endl
      << "newpz "  << newpz << " startpz " << pz << endl
      << "newdudz " << newdudz << endl
      << "newdvdz " << newdvdz << endl
      << "newpmag " << newpmag << endl;
   

   fu=newfu;
   fv=newfv;
   fz=newfz;
   fdudz=newdudz;
   fdvdz=newdvdz;
   fqp=charge*newpmag;
   



   //Now look at what we just swam through
   Double_t oldfz=fStartz;
   Int_t idir = 1;
   if (zfin<oldfz) {
      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 && oldfz<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));
	 MSG("FitTrack3",Msg::kVerbose) 
	    << "i " << i << " spi->GetZ " << spi->GetZ() 
	    << " oldfz " << oldfz << " zfin " << zfin << endl;
	 if (!found && oldfz>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();
      MSG("FitTrack3",Msg::kVerbose) 
	 << "imat " << imat << " zend " << zend << " oldfz " << oldfz << endl;
      Int_t flag = 0;
      switch (imat) {
      case 0: // air
	 fTair+=fabs(zend-oldfz);
	 oldfz=zend;
	 break;
      case 1: // scintillator
	 fTsc+=fabs(zend-oldfz);
	 oldfz=zend;
	 break;
      case 2: // iron
	 fTfe+=fabs(zend-oldfz);
	 oldfz=zend;
	 break;
      default:
	 MSG("FitTrack3",Msg::kError) << "undefined type " << imat << "\n";
	 break;
      }
      if (flag!=0) return flag;
   }
   
   
  return 0;
}