// iTrkAlign.cxx
 // Richard Hawkings, started 31/3/04
 // alignment ntuple methods specific to new Trk::Track class
 
 
 #include "GaudiKernel/SmartDataPtr.h"
 #include "GeneratorObjects/McEventCollection.h"
 #include "InDetAlignTrkInfo/AlignTrkContainer.h"
 #include "TrkTrack/Track.h"
 #include "TrkTrack/TrackCollection.h"
 #include "TrkTruthData/TrackTruthCollection.h"
 #include "TrkMaterialOnTrack/MaterialEffectsOnTrack.h"
 #include "TrkMaterialOnTrack/ScatteringAngles.h" 
 #include "TrkParameters/MeasuredPerigee.h"
 #include "TrkRIO_OnTrack/RIO_OnTrack.h"
 #include "InDetRIO_OnTrack/SiClusterOnTrack.h"
 #include "InDetRIO_OnTrack/TRT_DriftCircleOnTrack.h"
 #include "TrkEventPrimitives/ParamDefs.h"
 #include "TrkExInterfaces/IExtrapolator.h"
 #include "TrkEventPrimitives/PropDirection.h"
 #include "TrkPrepRawData/PrepRawData.h"
 #include "InDetPrepRawData/TRT_DriftCircle.h"
 #include "InDetIdentifier/PixelID.h"
 #include "InDetIdentifier/SCT_ID.h"
 #include "InDetIdentifier/TRT_ID.h"
 #include "InDetAlignGenAlgs/AlignSiModuleList.h"
 #include "InDetAlignGenTools/InDetAlignTrackSelTool.h"
 #include "InDetAlignGenAlgs/InDetAlignNt.h"
 #include "TrkPseudoMeasurementOnTrack/PseudoMeasurementOnTrack.h"
 
 
 
 
 // max hits on a track for fixed size scatterer arrays
 #define MAXHITS 60
 
 void InDetAlignNt::FillFromTrkTrack(AlignTrkContainer* tracks, int& ntrk) {
   // retrieve all tracks from TDS
   const DataVector<Trk::Track>* trks;
   if (StatusCode::SUCCESS!=evtStore()->retrieve(trks,par_newtrkcol)) {
     msg (MSG::ERROR) << "Cannot find " << par_newtrkcol  << endreq;
    
   } else {
     ATH_MSG_DEBUG ( "New track filling called found " << trks->size() );
     // setup track selection map if needed
     // no longer needed 
     // if (par_trksel) m_trkseltool->fillMap(trks);
 
     for (DataVector<Trk::Track>::const_iterator t=trks->begin(); 
          t!=trks->end();++t) {
       ++ntrk;
       bool status=false;
       if (iTrkTrkSel(*t)) {
         AlignTrk* atp=new AlignTrk(TrkTrkProc(trks,*t,ntrk,status));
         if (status) {
           if (par_alignnt) FillTrkNtuple(*atp);
           if (par_overlap) {
             OverlapProc(*atp);
             FillOverlap(*atp);
           }
           tracks->push_back(atp);
         }
       }
       ATH_MSG_DEBUG( "Exec loop for track " << ntrk );
     }
   }
   msg (MSG::INFO)<<"Created a total of " << tracks->size() << " aligntrks" << endreq;
 }
 
 bool InDetAlignNt::iTrkTrkSel(const Trk::Track* pnt) {
   // selection for alignment quality tracks
   if (par_trksel) {
     return (m_trkseltool->getStatus(*pnt)!=0);
   } else {
     // native selection
     // only selection on momentum currently implemented, no hole/shared info
     bool momok;
 
     const Trk::MeasuredPerigee *mesp = 0;
     DataVector<const Trk::TrackStateOnSurface>::const_iterator tsos_it =
       pnt->trackStateOnSurfaces()->begin();
     DataVector<const Trk::TrackStateOnSurface>::const_iterator tsos_end =
       pnt->trackStateOnSurfaces()->end();
     for (; tsos_it != tsos_end; ++tsos_it) {
       if ((**tsos_it).type(Trk::TrackStateOnSurface::Perigee))
         mesp = dynamic_cast<const Trk::MeasuredPerigee *>((**tsos_it).trackParameters());
     }
 
     if (mesp!=0) {
       momok=(fabs(mesp->parameters()[Trk::qOverP])/
              sin(mesp->parameters()[Trk::theta]) < 1/par_ptmin);
     } else {
       momok=false;
     }
     return momok;
   }
 }
 
 AlignTrk InDetAlignNt::TrkTrkProc(const DataVector<Trk::Track>* trks,
                                   const Trk::Track* pnt,const int nt,bool& status) {
   AlignTrk aligntrk;
   SetCommon(aligntrk,nt);
   ATH_MSG_DEBUG( "TrkTrkProc for track " << nt );
   ATH_MSG_VERBOSE(*pnt );
 
     //  msg << MSG::VERBOSE << *pnt << endreq;
 
   // track parameters
   float dx1=1.;
   float dx2=1.;
   const Trk::MeasuredPerigee *mesp = 0;
   DataVector<const Trk::TrackStateOnSurface>::const_iterator tsos_it =
     pnt->trackStateOnSurfaces()->begin();
   DataVector<const Trk::TrackStateOnSurface>::const_iterator tsos_end =
     pnt->trackStateOnSurfaces()->end();
   for (; tsos_it != tsos_end; ++tsos_it) {
     if ((**tsos_it).type(Trk::TrackStateOnSurface::Perigee))
       mesp = dynamic_cast<const Trk::MeasuredPerigee *>((**tsos_it).trackParameters());
   }
 
   if (mesp!=NULL) {
 
     double d0 = mesp->parameters()[Trk::d0];
     double z0 = mesp->parameters()[Trk::z0];
     double phi0 = mesp->parameters()[Trk::phi0];
     double theta = mesp->parameters()[Trk::theta];
     double qOverP = mesp->parameters()[Trk::qOverP];
     
     ATH_MSG_DEBUG( nt << ". Track Parameters (d0, z0, phi0, theta, q/p)" );
     ATH_MSG_DEBUG( " " << d0 << ", " << z0  << ", " 
         << phi0 << ", " << theta  << ", " << qOverP );
 
     // transform back to coth(theta), q/pt representation for alignment ntuple
     dx1=1./(pow(cos(mesp->parameters()[Trk::theta]),2));
     dx2=1./sin(mesp->parameters()[Trk::theta]);
     aligntrk.set_trkpar(mesp->parameters()[Trk::d0],1);
     aligntrk.set_trkpar(mesp->parameters()[Trk::z0],2);
     aligntrk.set_trkpar(mesp->parameters()[Trk::phi],3);
     aligntrk.set_trkpar(1/tan(mesp->parameters()[Trk::theta]),4);
     aligntrk.set_trkpar(dx2*(mesp->parameters()[Trk::qOverP]),5);
     const Trk::ErrorMatrix& ermatrix=mesp->localErrorMatrix();
     aligntrk.set_trkcov(ermatrix.covValue(Trk::d0,Trk::d0),1);
     aligntrk.set_trkcov(ermatrix.covValue(Trk::z0,Trk::d0),2);
     aligntrk.set_trkcov(ermatrix.covValue(Trk::z0,Trk::z0),3);
     aligntrk.set_trkcov(ermatrix.covValue(Trk::phi,Trk::d0),4);
     aligntrk.set_trkcov(ermatrix.covValue(Trk::phi,Trk::z0),5);
     aligntrk.set_trkcov(ermatrix.covValue(Trk::phi,Trk::phi),6);
     aligntrk.set_trkcov(dx1*ermatrix.covValue(Trk::theta,Trk::d0),7);
     aligntrk.set_trkcov(dx1*ermatrix.covValue(Trk::theta,Trk::z0),8);
     aligntrk.set_trkcov(dx1*ermatrix.covValue(Trk::theta,Trk::phi),9);
     aligntrk.set_trkcov(dx1*dx1*ermatrix.covValue(Trk::theta,Trk::theta),10);
     aligntrk.set_trkcov(dx2*ermatrix.covValue(Trk::qOverP,Trk::d0),11);
     aligntrk.set_trkcov(dx2*ermatrix.covValue(Trk::qOverP,Trk::z0),12);
     aligntrk.set_trkcov(dx2*ermatrix.covValue(Trk::qOverP,Trk::phi),13);
     aligntrk.set_trkcov(dx1*dx2*ermatrix.covValue(Trk::qOverP,Trk::theta),14);
     aligntrk.set_trkcov(dx2*dx2*ermatrix.covValue(Trk::qOverP,Trk::qOverP),15);
   } else {
     msg (MSG::ERROR) << "Could not get Trk::MeasuredPerigee" << endreq;
     for (int i=1;i<=5;++i) aligntrk.set_trkpar(0.,i);
     for (int i=1;i<=15;++i) aligntrk.set_trkcov(0.,i);
   }
 
   // truth info
   if (par_truth) TrkSetTruth(trks,pnt,aligntrk);
   bool outofslice=false;
 
   // make a preliminary table of trackpar and scatterers, with positions
   // this is needed for the iPatRec track model where scatters and trackpars
   // are not associated to RIO_OnTracks
   int nscat=0;
   int nhitscat[MAXHITS];
   HepPoint3D posscat[MAXHITS];
   // reset the curvature measurement from the TRT segment:
   aligntrk.set_trtcons(0., 0.);
   const Trk::TrackParameters* tparpscat[MAXHITS];
   const Trk::ScatteringAngles* saospscat[MAXHITS];
 
   for (std::vector<const Trk::TrackStateOnSurface*>::const_iterator tsos=pnt->trackStateOnSurfaces()->begin(); 
        tsos!=pnt->trackStateOnSurfaces()->end(); ++tsos) {
 
     const Trk::TrackParameters* tparp=((*tsos)->trackParameters());
     // Try to recover the Q/p measurement from the TRT segment:
     const Trk::MeasurementBase* mesb=(*tsos)->measurementOnTrack();
     const Trk::PseudoMeasurementOnTrack* trtm=dynamic_cast<const Trk::PseudoMeasurementOnTrack*>(mesb);
     if (trtm!=0) {
       // the pseudo measurement has been found!
       ATH_MSG_DEBUG( "The TRT pseudo-measurement has been found! " );
       ATH_MSG_DEBUG( "Q/p_t(perigee), Q/p_t(TRT), sigma(1,1) =  " << 
         dx2*(mesp->parameters()[Trk::qOverP]) << ",  " <<
         dx2*(trtm->localParameters()[Trk::qOverP]) << ",  " <<
         dx2*getErrorQonP(trtm) );    // This is NOT a bug. This is a FEATURE!!!
       aligntrk.set_trtcons(dx2*(trtm->localParameters()[Trk::qOverP]),  dx2*getErrorQonP(trtm));
     }
 
     if ((*tsos)->type(Trk::TrackStateOnSurface::Scatterer)) {
       const Trk::GlobalPosition& globp=tparp->position();
       posscat[nscat]=globp;
       tparpscat[nscat]=tparp;
       const Trk::MaterialEffectsBase* meb = (*tsos)->materialEffectsOnTrack();
       const Trk::MaterialEffectsOnTrack* meot = dynamic_cast<const Trk::MaterialEffectsOnTrack*>(meb);
       if (meot!=NULL) {
         saospscat[nscat]=(meot)->scatteringAngles();
         ATH_MSG_VERBOSE( "Scattering angle (" << nscat << "): " << saospscat[nscat]);
           //msg << MSG::VERBOSE << "Scattering angle (" << nscat << "): " << saospscat[nscat] << endreq;
       }
       nhitscat[nscat]=0;
       ++nscat;
     }
 
   } // end of preliminary trackpar/scatterer loop
 
   ATH_MSG_DEBUG( "Number of Scatterers found: " << nscat );
 
   // loop over all the silicon hits
   int nhits=0;
   bool seenhit=false;
   Trk::GlobalPosition lastpos,pos;
   ATH_MSG_DEBUG( "Begin loop over " << pnt->trackStateOnSurfaces()->size() << " TSOS" );
   for (std::vector<const Trk::TrackStateOnSurface*>::const_iterator tsos=
          pnt->trackStateOnSurfaces()->begin(); 
        tsos!=pnt->trackStateOnSurfaces()->end(); ++tsos) {
 
     const Trk::MeasurementBase* mesb=(*tsos)->measurementOnTrack();
     const Trk::RIO_OnTrack* rotp=dynamic_cast<const Trk::RIO_OnTrack*>(mesb);
     const Trk::TrackParameters* tparp=((*tsos)->trackParameters());
     const Trk::ScatteringAngles* saosp;
     const Trk::MaterialEffectsBase* meb = (*tsos)->materialEffectsOnTrack();
     const Trk::MaterialEffectsOnTrack* meot = dynamic_cast<const Trk::MaterialEffectsOnTrack*>(meb);
     if (meot!=NULL) {
       saosp=(meot)->scatteringAngles();
       ATH_MSG_DEBUG( "Scattering angles del " << saosp->deltaTheta() 
           << "," << saosp->deltaPhi() << "," << saosp->sigmaDeltaTheta() << "," << saosp->sigmaDeltaPhi() );
     }
     else saosp = NULL;
         ATH_MSG_VERBOSE(  "Si " << nhits 
         << " pointers ROT/TP/SO " << rotp 
         << "," << tparp << "," << saosp);
         //msg << MSG::VERBOSE << "Si " << nhits << " pointers ROT/TP/SO " << rotp << "," << tparp << "," << saosp << endreq;
     
     // if we have track parameters, print their location
     if (tparp!=0) {
       const Trk::GlobalPosition& globp=tparp->position();
       ATH_MSG_DEBUG( "Track par position [" << globp.x() << "," <<
         globp.y() << "," << globp.z() << "]" );
     }
   
     bool sharedscat=false;
     if (rotp!=0 && (*tsos)->type(Trk::TrackStateOnSurface::Measurement)) {
       const Trk::PrepRawData* rio=rotp->prepRawData();
         ATH_MSG_VERBOSE( "RIO PRD pointer " << rio );
       // Identifier ident=rio->identify();
       Identifier ident=rotp->identify();
       Identifier mident;
       bool sihit=false;
       bool pix=false;
       bool sctec=false;
       // work around the pixel_id problem for testbeam geometry only 
       // assume all hits which are not SCT or TRT are actually pixels
       if (m_pixid->is_pixel(ident) || (par_layout==11 && 
                                        (!(m_sctid->is_sct(ident) || m_trtid->is_trt(ident))))) {
         mident=m_pixid->wafer_id(ident);
         sihit=true;
         pix=true;
       } else if (m_sctid->is_sct(ident)) {
         // for SCT, wafter does not include side, so construct ID by hand
         mident=m_sctid->wafer_id(m_sctid->barrel_ec(ident),
                                  m_sctid->layer_disk(ident),m_sctid->phi_module(ident),
                                  m_sctid->eta_module(ident),m_sctid->side(ident));
         sihit=true;
       }
       if (sihit) {
         AlignSiHit newhit;
         nhits++;
         // find associated SiClusterOnTrack and hit position
         const InDet::SiClusterOnTrack* siclus=
           dynamic_cast<const InDet::SiClusterOnTrack*>(rotp);
         if (siclus!=0) {
           pos=siclus->globalPosition();
         } else {
           msg (MSG::ERROR) << "Could not determine hit position" << endreq;
         }
         newhit.set_sinlocal(0.0);       // reset the sinlocal info
         AlignSiModuleList::ModuleIDMap::const_iterator 
           itmod=p_modlist->idmap.find(mident);
         if (itmod != p_modlist->idmap.end()) {
           newhit.set_index(itmod->second);
           if (itmod->second!=0) {
             if (!pix) newhit.set_sinlocal((m_detelep[(itmod->second)-1])->
                                           sinStereoLocal(pos));
             const AlignSiModule& simod=p_modlist->vec[(itmod->second)-1];
             simod.inchits();
             // compute the stereo angle for SCT modules
             // sign convention is the same as for ipatrec native
             if (!pix) {
               const InDetDD::SiDetectorElement* 
                 detelm=m_detelep[(itmod->second)-1];
               if (detelm!=0 && detelm->design().shape()==
                   InDetDD::Trapezoid) {
                 // forward SCT stereo angles
                 Hep3Vector globy=
                   ((m_global2local[itmod->second-1]).inverse()*HepYHat);
                 float ster=atan2(simod.pos(1),simod.pos(0))-
                   atan2(globy.y(),globy.x());
                 if (ster<-M_PI) ster+=2*M_PI;
                 if (ster>M_PI) ster-=2*M_PI;
                 newhit.set_stereo(ster);
                 sctec=true;
               } else {
                 // barrel SCT stereo angle
                 Hep3Vector globx=
                   ((m_global2local[itmod->second-1]).inverse()*HepXHat);
                 newhit.set_stereo(asin(globx.z()));
               }
             }
           } else {
             outofslice=true;
           }
         } else {
           msg (MSG::ERROR) << "Module ident " << mident.getString() << 
             "not found in moduleidmap" << endreq;
           newhit.set_index(-1);
         }
 
         /*
         // find associated SiClusterOnTrack and hit position
         const InDet::SiClusterOnTrack* siclus=
         dynamic_cast<const InDet::SiClusterOnTrack*>(rotp);
         */
         if (siclus!=0) {
           pos=siclus->globalPosition();
           newhit.set_r(pos.perp());
           newhit.set_phi(pos.phi());
           newhit.set_z(pos.z());
           if (seenhit) {
             newhit.set_trklen((pos-lastpos).mag());
             newhit.set_status(2);
           } else {
             newhit.set_trklen(0); // this is first hit
             newhit.set_status(1);
             seenhit=true;
           }
         } else {
           msg (MSG::ERROR) << "RIO cast to SiClusterOnTrack failed" << endreq;
         }
 
         // set residual errors from RIO_OnTrack
         const Trk::ErrorMatrix& locale=rotp->localErrorMatrix();
         if (pix) {
           newhit.set_erphi_resid(locale.error(Trk::locX));
           newhit.set_ez_resid(locale.error(Trk::locY));
         } else {
           // for SCT set rphi residual only, take into account endcap 
           // two-dimensional error matrix
           if (sctec) {
             newhit.set_erphi_resid(locale.error(Trk::locX)*sqrt
                              (1.-locale.covValue(Trk::locX,Trk::locY)*
                               locale.covValue(Trk::locX,Trk::locY)/
                               (locale.covValue(Trk::locX)*locale.covValue(Trk::locY))));
           } else {
             newhit.set_erphi_resid(locale.error(Trk::locX));
           }
           newhit.set_ez_resid(0.);
         }
         
         // if we have track parameters, extract the extrapolation errors
         const Trk::MeasuredTrackParameters* mtparp = dynamic_cast<const Trk::MeasuredTrackParameters*>(tparp);
         if (mtparp) {
           const Trk::ErrorMatrix& extrapolationError=mtparp->errorMatrixInMeasurementFrame();
           newhit.set_trkExtrapLocalXError(extrapolationError.error(Trk::loc1));
           if(pix)
             newhit.set_trkExtrapLocalYError(extrapolationError.error(Trk::loc2));
           
         }
       
         
         // derive residuals from local track parameter
         // if local track parameter is zero, first try lookup in separate table
         bool tplook=false;
         if (tparp==0 || saosp==0) {
           float d2min=1.E10;
           int jhit=-1;
           for (int j=0;j<nscat;++j) {
             float d2=pow(pos.x()-posscat[j].x(),2.)+
               pow(pos.y()-posscat[j].y(),2.)+
               pow(pos.z()-posscat[j].z(),2.);
             if (d2<d2min) {
               d2min=d2;
               jhit=j;
             }
           }
           if (jhit>=0) {
             // found a scatterer - setup pointers
                 ATH_MSG_VERBOSE( "Associated scatterer " << jhit << 
               " at distance " << sqrt(d2min) << " hit at r/z " << pos.perp() <<
               "/" << pos.z() );
             // only set tparp here if we didn't already have one at the point
             if (tparp==0) {
               tparp=tparpscat[jhit];
               tplook=true;
             }
             saosp=saospscat[jhit];
             // increment scatterer usage count
             if (nhitscat[jhit]>0) sharedscat=true;
             ++nhitscat[jhit];
           }
         }
         if (tparp!=0) {
           // for xkalman, already have track parameters at local position
           const HepVector& tparv=tparp->parameters();
           if (!tplook) {
             // for track parameters stored with measurement, local parameters
             // give residual information directly
             Trk::LocalParameters localp=rotp->localParameters();
             // deal with pixel and SCT measurements seperately
             if (pix) {
               newhit.set_rphi_resid(localp[Trk::locX]-tparv[Trk::locX]);
               newhit.set_z_resid(localp[Trk::locY]-tparv[Trk::locY]);
               //newhit.set_erphi_resid(locale.error(Trk::locY));
               //newhit.set_ez_resid(locale.error(Trk::locY));
             } else {
               // correct for trapezoidal modules in SCT 
               // note for barrel sinStereoLocal returns one
               // and matrix is diagonal
               float trapcor=0.0;
               if ( sctec ) {
                 const InDetDD::SiDetectorElement* detelm=
                   m_detelep[newhit.index()-1];
                 if (localp.contains(Trk::locY) && detelm!=0) {
                   trapcor = detelm->sinStereoLocal(Trk::LocalPosition(localp[Trk::locX],localp[Trk::locY]))
                                                        *(tparv[Trk::locY]-localp[Trk::locY]);
                   newhit.set_sinlocal(detelm->sinStereoLocal(Trk::LocalPosition(localp[Trk::locX],localp[Trk::locY])));   // redefinition of sinlocal (any better???)
                 } else {
                   trapcor = newhit.sinlocal()*tparv[Trk::locY];
                 }
                 if (newhit.sinlocal()!=0 && !localp.contains(Trk::locY)) msg(MSG::WARNING) << 
                                "No hit locY information for an SCT EC module" << endreq;
                 if (detelm==0) msg (MSG::ERROR) << 
                                "Zero detelm pointer - no trapezoid correction" << endreq;
               }  
               //newhit.set_rphi_resid(localp[Trk::locX]-trapcor
               //                    -tparv[Trk::locX]);
               newhit.set_rphi_resid((localp[Trk::locX]-trapcor
                                    -tparv[Trk::locX])*sqrt(1-newhit.sinlocal()*newhit.sinlocal()));
               //newhit.set_erphi_resid(locale.error(Trk::locX)*sqrt
               //     (1.-locale.covValue(Trk::locX,Trk::locY)*
               //                   locale.covValue(Trk::locX,Trk::locY)/
               //  (locale.covValue(Trk::locX)*locale.covValue(Trk::locY))));
             }
             if (localp.contains(Trk::locY)) {
               newhit.set_delzr(tparv[Trk::locY]-localp[Trk::locY]);
             } else {
               newhit.set_delzr(tparv[Trk::locY]);
             }
             //if (fabs(newhit.delzr())>1.E3) msg (MSG::ERROR) << "Huge delzr " <<
             // tparv[Trk::locY] << "," << localp[Trk::locY] << endreq;
           } else {
             // for trackparameters associated by lookup, have to extrapolate
             FillResid(rotp,tparp,newhit);
           }
           // calculate derivatives analytically if any are needed
           if (par_numdiv<31)
             FillDeriv(newhit,aligntrk.trkpar(1),aligntrk.trkpar(3),
                       aligntrk.trkpar(5),pos,1/tan(tparv[Trk::theta]),tparv[Trk::phi]);
         
           // calculate numerical derivatives if any are needed
           if (par_numdiv>0) FillNumDeriv(aligntrk,tparp,newhit);
         }
         // channel information
         if (pix) {
           newhit.set_chan1(m_pixid->phi_index(ident));
           newhit.set_chan2(m_pixid->eta_index(ident));
         } else {
           newhit.set_z_resid(0.);
           newhit.set_ez_resid(0.);
           newhit.set_chan1(m_sctid->strip(ident));
           newhit.set_chan2(0);
         }
 
         // scatterer information
         // assume scatterers come along with hits, may not be true for iPatRec
         // will then have to set saosp from prior association loop
         // initially set to zero in case no or shared scatterer
         newhit.set_locdel(0.,0);
         newhit.set_locdel(0.,1);
         newhit.set_locedel(0.,0);
         newhit.set_locedel(0.,1);
         newhit.set_globdel(0.,0);
         newhit.set_globdel(0.,1);
         newhit.set_globedel(0.,0);
         newhit.set_globedel(0.,1);
         // kill the scatterer if it has already been used
         if (sharedscat) saosp=0;
         if (tparp!=0) {
           const HepVector& tparv=tparp->parameters();
           FillAngles(newhit,tparv,saosp);
           // save the hit location to calculate the track length to next
           lastpos=pos;
         }
         // add new hit to list
         aligntrk.add_hit(newhit);
       } // silicon hit
     } // rotp OK
   } // loop over TSOS
   aligntrk.set_nscat(nscat);
   ATH_MSG_DEBUG( "Track has " << nhits << " Si hits and " << nscat << 
     " scatterers" );
 
   // now look at TRT if requested - loop over all hits again
   if (par_trt!=0) {
     int ntrthits=0;
     // loop over the TrackStateonSurfaces which contain the RIOs and 
     // also residuals, hit quality information etc
     for (std::vector<const Trk::TrackStateOnSurface*>::const_iterator tsos=
            pnt->trackStateOnSurfaces()->begin();
          tsos!=pnt->trackStateOnSurfaces()->end();++tsos) {
       const Trk::MeasurementBase* mesb=(*tsos)->measurementOnTrack();
       const Trk::RIO_OnTrack* rotp=dynamic_cast<const Trk::RIO_OnTrack*>(mesb);
       const Trk::TrackParameters* tparp=((*tsos)->trackParameters());
       ATH_MSG_VERBOSE( "TRT " << ntrthits <<
         " pointers ROT/TP " << rotp << "," << tparp );
       if (rotp!=0 && (*tsos)->type(Trk::TrackStateOnSurface::Measurement)) {
         const Trk::PrepRawData* rio=rotp->prepRawData();
         Identifier ident=rotp->identify();
         //        if (rio!=0) ident=rio->identify();
         if (m_trtid->is_trt(ident)) {
           AlignTRTHit newhit;
           newhit.set_index(m_trtid->barrel_ec(ident),
                            m_trtid->layer_or_wheel(ident),m_trtid->phi_module(ident),
                            m_trtid->straw_layer(ident),m_trtid->straw(ident));
 
           const InDet::TRT_DriftCircleOnTrack* trtcirc=
             dynamic_cast<const InDet::TRT_DriftCircleOnTrack*>(rotp);
           if (trtcirc!=0) {
             Trk::GlobalPosition tpos=trtcirc->globalPosition();
             newhit.set_r(tpos.perp());
             newhit.set_phi(tpos.phi());
             newhit.set_z(tpos.z());
             newhit.set_highth(trtcirc->highLevel());
             // this is giving problems for xKalman tracks on 4/8/05
             //  float xdrift=rotp->localPosition().x();
             float xdrift=rotp->localParameters()[Trk::locX];
             newhit.set_xdrift(xdrift);
             bool isvalid=false;
             //cast will fail if preprawdata is not present
             const InDet::TRT_DriftCircle* rdc=dynamic_cast<const
               InDet::TRT_DriftCircle*>(rio);
             if (rdc) {
               float tdrift=rdc->driftTime(isvalid);
               if (!isvalid) tdrift=-1.;
               newhit.set_tdrift(tdrift);
             }
             // if have track parameters, set residual directly
             if (tparp!=0) {
               const HepVector& tparv=tparp->parameters();
               newhit.set_res(xdrift-tparv[0]);
             } else {
               // have to calculate residual by extrap from nearest scatterer
               float d2min=1.E10;
               int jhit=-1;
               for (int j=0;j<nscat;++j) {
                 float d2=pow(tpos.x()-posscat[j].x(),2.)+
                   pow(tpos.y()-posscat[j].y(),2.)+
                   pow(tpos.z()-posscat[j].z(),2.);
                 if (d2<d2min) {
                   d2min=d2;
                   jhit=j;
                 }
               }
               if (jhit>=0) {
                 // could now extrapolate to TRThit 
                 // not yet implemneted - how to get the surface for a TRThit?
               }
             }  
             ntrthits++;
             aligntrk.add_trthit(newhit);
           } else {
             msg (MSG::ERROR) << "TRT drift RIO cast failed - no hit stored" 
                 << endreq;
           }
         } // identified TRT hit
       } // non-zero ROTpointer
     } // end of test for RIO
     ATH_MSG_DEBUG( "Track has " << ntrthits << " TRT hits " );
   } // end of TRT-specific code
   status=true;
   return aligntrk;
 }
 
 void InDetAlignNt::FillAngles(AlignSiHit& ahit,const HepVector& tparv,
                               const Trk::ScatteringAngles* saosp) {
 
   // extract local track direction
   HepRotation& global2local=m_global2local[ahit.index()-1];
   // fill track direction even if no associated scatterer
   float gtheta=tparv[Trk::theta];
   float gphi=tparv[Trk::phi];
 
   HepPoint3D local_dir=global2local*
     HepPoint3D(sin(gtheta)*cos(gphi),sin(gtheta)*sin(gphi),cos(gtheta));
 
   ahit.set_locang(local_dir.theta(),0);
   ahit.set_locang(local_dir.phi(),1);
   ahit.set_globang(1/tan(gtheta),0);
   ahit.set_globang(gphi,1);
 
   // fill delta and sigma of track direction only if scatterer present
   if (saosp!=NULL) {
 
     ATH_MSG_DEBUG ( "-> gtheta " << gtheta << ", gphi " << gphi );
     ATH_MSG_DEBUG ( "Filled Scattering angles del " << saosp->deltaTheta() 
         << "," << saosp->deltaPhi() << "," << saosp->sigmaDeltaTheta() << "," << 
       saosp->sigmaDeltaPhi() );
 
     float ngtheta=gtheta+saosp->deltaTheta();
     float ngphi=gphi+saosp->deltaPhi();
     float rgtheta=gtheta+saosp->sigmaDeltaTheta();
     float rgphi=gphi+saosp->sigmaDeltaPhi();
 
     HepPoint3D new_local_dir=global2local*
       HepPoint3D(sin(ngtheta)*cos(ngphi),sin(ngtheta)*sin(ngphi),cos(ngtheta));
     HepPoint3D rms_local_dir=global2local*
       HepPoint3D(sin(rgtheta)*cos(rgphi),sin(rgtheta)*sin(rgphi),cos(rgtheta));
 
     ahit.set_locdel(new_local_dir.theta()-local_dir.theta(),0);
     ahit.set_locdel(new_local_dir.phi()-local_dir.phi(),1);
     ahit.set_locedel(fabs(rms_local_dir.theta()-local_dir.theta()),0);
     ahit.set_locedel(fabs(rms_local_dir.phi()-local_dir.phi()),1);
     // for iPat legacy tracks, need to convert from variance to RMS
     if (m_ipattrk) {
       ahit.set_locedel(sqrt(ahit.locedel(0)),0);
       ahit.set_locedel(sqrt(ahit.locedel(1)),1);
     }
     ahit.set_globdel(saosp->deltaTheta(),0);
     ahit.set_globdel(saosp->deltaPhi(),1);
     ahit.set_globedel(saosp->sigmaDeltaTheta(),0);
     ahit.set_globedel(saosp->sigmaDeltaPhi(),1);
   }
 }
 
 void InDetAlignNt::FillResid(const Trk::RIO_OnTrack* rotp,
                              const Trk::TrackParameters* tparp, AlignSiHit& newhit) {
   Trk::LocalParameters localp=rotp->localParameters();
 
   //msg (MSG::INFO) << "Hit local x/y " << localp[Trk::locX] << "," << 
   //   localp[Trk::locY] << 
   // " global r,phi,z " << newhit.r() << "," << newhit.phi() << "," << 
   // newhit.z() << endreq;
 
   // first set to -1 in case extrapolation fails
   newhit.set_rphi_resid(-1.);
   newhit.set_z_resid(-1.);
 
   if (newhit.index()>0) {
     //msg (MSG::INFO) << "Call extrap for module " << newhit.index()-1 <<
     //  " surf ptr " << (m_detelep[newhit.index()-1])->surface() << endreq;
     //msg (MSG::INFO) << "track associated surface ptr " << 
     //  tparp->associatedSurface() << " resolving to " << 
     //    *(tparp->associatedSurface()) << endreq;
     const InDetDD::SiDetectorElement* detelm=m_detelep[newhit.index()-1];
     const Trk::TrackParameters* paratsurf=
       m_extraptool->extrapolate(*tparp,
                                 detelm->surface(),Trk::anyDirection,false);
     const HepVector& tpars=paratsurf->parameters();
     float xresid;
     if (detelm->isSCT() && detelm->design().shape()==InDetDD::Trapezoid) {
       // special correction for SCT endcap to account for strip fanout
       const HepPoint3D& rtrk=paratsurf->position();
       float rcor=newhit.r()/sqrt(rtrk.x()*rtrk.x()+rtrk.y()*rtrk.y());
       xresid=localp[Trk::locX]-rcor*tpars[Trk::locX];
       xresid*=sqrt(1-newhit.sinlocal()*newhit.sinlocal());
     } else {
       // conventional planar residual
       xresid=localp[Trk::locX]-tpars[Trk::locX];
     }
     // msg (MSG::INFO) << " extrap local x,y " << tpars[Trk::locX] << ","
     //   << tpars[Trk::locY] << " cf hit " << localp[Trk::locX] << "," << 
     // localp[Trk::locY] 
     //   << " calc xresid " << xresid << endreq;
     newhit.set_rphi_resid(xresid);
     if (detelm->isPixel()) {
       // note endcap pixels are planer - no fanout correction
       newhit.set_z_resid(localp[Trk::locY]-tpars[Trk::locY]);
     }
     newhit.set_delzr(tpars[Trk::locY]-localp[Trk::locY]);
   } else {
     msg (MSG::ERROR) << "Cannot extrapolate as hit index zero " << endreq;
   }
 }
 
 void InDetAlignNt::TrkSetTruth(const DataVector<Trk::Track>* trks,
                                const Trk::Track* ptrk,AlignTrk& aligntrk) {
   // set up truth information in AlignTrk from TrackTrk
   // used by both standard AlignTrk production from TrkTrack and refit,
   // where truth information must be set from parent track
  
   const TrackTruthCollection* trucp;
   if (StatusCode::SUCCESS==evtStore()->retrieve(trucp,par_trktruthcol)) {
     ElementLink<TrackCollection> tlink;
     tlink.setElement(const_cast<Trk::Track*>(ptrk));
     tlink.setStorableObject(*trks);
     TrackTruthCollection::const_iterator itmp=
       trucp->find(Trk::TrackTruthKey(tlink));
     if (itmp!=trucp->end()) {
       ATH_MSG_DEBUG( "Track has truth barcode " << 
         itmp->second.barcode());
       if ((itmp->second.barcode()!=0) && m_mccolln!=0)
         FillTruth(aligntrk,(*m_mccolln->begin())
                   ->barcode_to_particle(itmp->second.barcode()));
     } else {
       ATH_MSG_DEBUG( "Cannot find truth in map!" );
     }
   } else {
     msg (MSG::ERROR) << "Cannot find " << par_trktruthcol << endreq;
   }
 }
 
 void InDetAlignNt::FillNumDeriv(const AlignTrk& altrk,
                                 const Trk::TrackParameters* tparp,
                                 AlignSiHit& newhit) {
 
   // extrapolate track back to origin perigee parameters, then calculate
   // derivatives of alignment wrt to each
   // skip derivatives which are not requested to be calculated analytically
  
   // first determine perigee parameters in this track model by extrapolation
   // back to origin
   double peritrk[5];
   const Trk::PerigeeSurface perisurf;
   const Trk::TrackParameters* pperitrk=
     m_extraptool->extrapolate(*tparp,perisurf,
                               Trk::anyDirection,false);
   if (pperitrk!=0) {
     const HepVector& peripars=pperitrk->parameters();
     peritrk[0]=peripars[Trk::d0];
     peritrk[1]=peripars[Trk::z0];
     peritrk[2]=peripars[Trk::phi0];
     peritrk[3]=peripars[Trk::theta];
     peritrk[4]=peripars[Trk::qOverP];
     double sinth=sin(peritrk[3]);
     double jac[5];
     jac[0]=1.;
     jac[1]=1.;
     jac[2]=1.;
     jac[3]=-sinth*sinth;
     jac[4]=sinth;
 
     // now extrapolate to measurement surface
     const Trk::Surface& detsurf=(m_detelep[newhit.index()-1])->surface();
     const Trk::TrackParameters* pbasetrk=
       m_extraptool->extrapolate(*pperitrk,detsurf,
                                 Trk::alongMomentum,false);
     if (pbasetrk!=0) {
       const HepVector& basepars=pbasetrk->parameters();
       // now loop over all parameters, vary one at a time
       // variations based on 0.2 sigma of track parameter error
       // note aligntrk errors are in cottheta,q/pt
       double varsig=0.2;
       double varpar[5],deltrkp[5];
       varpar[0]=varsig*sqrt(altrk.trkcov(1));
       varpar[1]=varsig*sqrt(altrk.trkcov(3));
       varpar[2]=varsig*sqrt(altrk.trkcov(6));
       varpar[3]=varsig*sqrt(altrk.trkcov(10))*jac[3];
       varpar[4]=varsig*sqrt(altrk.trkcov(15))*jac[4];
 
       for (int ipar=0;ipar<5;++ipar) {
         // only extrapolate for derivatives which are requested numerically
         if (par_numdiv & (1 << ipar)) {
           // create new perturbed track
           for (int j=0;j<5;++j) deltrkp[j]=peritrk[j];
           deltrkp[ipar]=deltrkp[ipar]+varpar[ipar];
           // fix for Q/pT vs Q/p:
           if (ipar==3) {
             deltrkp[4]=deltrkp[4]-peritrk[4]*varpar[3]/tan(peritrk[3]);
           }
           const Trk::TrackParameters* pperivar=new Trk::Perigee(deltrkp[0],
                                                                 deltrkp[1],
                                                                 deltrkp[2],
                                                                 deltrkp[3],
                                                                 deltrkp[4],
                                                                 perisurf);
 
           // extrapolate new track to measurement surface
           const Trk::TrackParameters* pdeltrk= m_extraptool->extrapolate(*pperivar,detsurf,
                                                                          Trk::alongMomentum,false);
           if (pdeltrk!=0) {
             const HepVector& delpars=pdeltrk->parameters();
    
             if(std::isnan(delpars[Trk::locX])) msg (MSG::WARNING) << " delpars[Trk::locX] is nan" << endreq;
             if(std::isnan(delpars[Trk::locY])) msg (MSG::WARNING) << " delpars[Trk::locY] is nan" << endreq;
             
             // protection for nan values (should be outliers)
             if (std::isnan(delpars[Trk::locX]) || std::isnan(delpars[Trk::locY])) {
               msg (MSG::WARNING )<< " -> skip extrapolation" << endreq;
               continue;
             }
 
             if(std::isnan(delpars[Trk::locX])) msg (MSG::ERROR) << " delpars[Trk::locX] is nan" << endreq;
             if(std::isnan(delpars[Trk::locY])) msg (MSG::ERROR) << " delpars[Trk::locY] is nan" << endreq;
 
             ATH_MSG_VERBOSE( "Extrapolation " << ipar << " new local " <<
               delpars[Trk::locX] << "," << delpars[Trk::locY] );
 
             ATH_MSG_VERBOSE( "Shifts are " << ipar << " by " << 
               varpar[ipar] << " : " << delpars[Trk::locX]-basepars[Trk::locX] << 
               "," << delpars[Trk::locY]-basepars[Trk::locY]);
 
             // take care of SCT fanout:
             double sl(newhit.sinlocal());
             double cl(sqrt(1.0-newhit.sinlocal()*newhit.sinlocal()));
             double dlocx(cl*(basepars[Trk::locX]-delpars[Trk::locX])
                          + sl*(basepars[Trk::locY]-delpars[Trk::locY]));
             double dlocy(cl*(basepars[Trk::locY]-delpars[Trk::locY])
                          - sl*(basepars[Trk::locX]-delpars[Trk::locX]));
 
             // fill in derivatives - note convert back to cottheta,q/pt
             newhit.set_dtpar1(jac[ipar]*dlocx/varpar[ipar],ipar);
             newhit.set_dtpar2(jac[ipar]*dlocy/varpar[ipar],ipar);
             delete pdeltrk;
           } 
           else {
             msg (MSG::ERROR) << "FillNumDeriv Detla extraplation " << ipar << "failed" << endreq;
           }
           delete pperivar;
         }
       }
       delete pbasetrk;
     } else {
       msg (MSG::ERROR) << "FillNumDeriv base extrapolation failed" << endreq;
     }
     delete pperitrk;
   } else {
     msg (MSG::ERROR) << "FillNumDeriv backextrapolation failed from track: "
         << *tparp << endreq;
   }
 }
 
 void InDetAlignNt::FillNumDeriviPat(const AlignTrk& aligntrk,
                                     AlignSiHit& newhit) {
   // interface to FillNumDeriv for iPat native tracks
   // create a temporary track based on the iPatTrack parameters to start
   // extrapolation - simplify by using perigee rather than local tpars
   const Trk::PerigeeSurface perisurf;
   float theta=atan(1./aligntrk.trkpar(4));
   if (theta<0) theta+=3.1415926;
   const Trk::TrackParameters* tparp=new Trk::Perigee(aligntrk.trkpar(1),
                                                      aligntrk.trkpar(2),
                                                      aligntrk.trkpar(3),
                                                      theta,
                                                      sin(theta)*aligntrk.trkpar(5),
                                                      perisurf);
   FillNumDeriv(aligntrk,tparp,newhit);
   delete tparp;
 }
 
 
 double InDetAlignNt::getErrorQonP( const  Trk::PseudoMeasurementOnTrack* pmot ) const{
         // The Size of the error matrix of the PseudoMeasurment is dependant on the measurements 
         // within the PM (ie 1 PM 1x1 matrix etc)
         
         int nmeas(0); 
         if (pmot->localParameters().contains(Trk::locRPhi)){
                 nmeas++;
         }
         if (pmot->localParameters().contains(Trk::locZ)){
                 nmeas++;
         }
         if (pmot->localParameters().contains(Trk::phi)){
                 nmeas++;
         }
         if (pmot->localParameters().contains(Trk::theta)){
                 nmeas++;        
         }
         if (pmot->localParameters().contains(Trk::qOverP)){
                 nmeas++;
         }
         if (nmeas == 1){
                 return  pmot->localErrorMatrix().error(Trk::locRPhi);   
         } else if (nmeas == 2){
                 return  pmot->localErrorMatrix().error(Trk::locZ);
         } else if (nmeas == 3){
                 return  pmot->localErrorMatrix().error(Trk::phi);
         } else if (nmeas == 4){
                 return  pmot->localErrorMatrix().error(Trk::theta);
         } else if (nmeas == 5){
                 return  pmot->localErrorMatrix().error(Trk::qOverP);
         }
         
         return 0.; 
 
 }
 

Due to the LXR bug, the updates fail sometimes to remove references to deleted files. The Saturday's full rebuilds fix these problems
This page was automatically generated by the LXR engine.