text: #include "MkFitter.h"
  
  #include "KalmanUtilsMPlex.h"
  #include "MatriplexPackers.h"
  
  //#define DEBUG
  #include "Debug.h"
  
  #include <sstream>
  
  namespace mkfit {
  
    void MkFitter::checkAlignment() {
      printf("MkFitter alignment check:\n");
      Matriplex::align_check("  m_Err[0]   =", &m_Err[0].fArray[0]);
      Matriplex::align_check("  m_Err[1]   =", &m_Err[1].fArray[0]);
      Matriplex::align_check("  m_Par[0]   =", &m_Par[0].fArray[0]);
      Matriplex::align_check("  m_Par[1]   =", &m_Par[1].fArray[0]);
      Matriplex::align_check("  m_msErr[0] =", &m_msErr[0].fArray[0]);
      Matriplex::align_check("  m_msPar[0] =", &m_msPar[0].fArray[0]);
    }
  
    void MkFitter::printPt(int idx) {
      for (int i = 0; i < NN; ++i) {
        printf("%5.2f  ", std::hypot(m_Par[idx].At(i, 3, 0), m_Par[idx].At(i, 4, 0)));
      }
    }
  
    int MkFitter::countValidHits(int itrack, int end_hit) const {
      int result = 0;
      for (int hi = 0; hi < end_hit; ++hi) {
        if (m_HoTArr[hi](itrack, 0, 0).index >= 0)
          result++;
      }
      return result;
    }
  
    int MkFitter::countInvalidHits(int itrack, int end_hit) const {
      int result = 0;
      for (int hi = 0; hi < end_hit; ++hi) {
        // XXXX MT: Should also count -2 hits as invalid?
        if (m_HoTArr[hi](itrack, 0, 0).index == -1)
          result++;
      }
      return result;
    }
  
    //==============================================================================
  
    void MkFitter::inputTracksAndHits(const std::vector<Track>& tracks,
                                      const std::vector<HitVec>& layerHits,
                                      int beg,
                                      int end) {
      // Assign track parameters to initial state and copy hit values in.
  
      // This might not be true for the last chunk!
      // assert(end - beg == NN);
  
      int itrack = 0;
  
      for (int i = beg; i < end; ++i, ++itrack) {
        const Track& trk = tracks[i];
  
        m_Err[iC].copyIn(itrack, trk.errors().Array());
        m_Par[iC].copyIn(itrack, trk.parameters().Array());
  
        m_Chg(itrack, 0, 0) = trk.charge();
        m_Chi2(itrack, 0, 0) = trk.chi2();
        m_Label(itrack, 0, 0) = trk.label();
  
        // CopyIn seems fast enough, but indirections are quite slow.
        for (int hi = 0; hi < m_Nhits; ++hi) {
          m_HoTArr[hi](itrack, 0, 0) = trk.getHitOnTrack(hi);
  
          const int hidx = trk.getHitIdx(hi);
          if (hidx < 0)
            continue;
  
          const Hit& hit = layerHits[hi][hidx];
          m_msErr[hi].copyIn(itrack, hit.errArray());
          m_msPar[hi].copyIn(itrack, hit.posArray());
        }
      }
    }
  
    void MkFitter::inputTracksAndHits(const std::vector<Track>& tracks,
                                      const std::vector<LayerOfHits>& layerHits,
                                      int beg,
                                      int end) {
      // Assign track parameters to initial state and copy hit values in.
  
      // This might not be true for the last chunk!
      // assert(end - beg == NN);
  
      int itrack;
  
      for (int i = beg; i < end; ++i) {
        itrack = i - beg;
        const Track& trk = tracks[i];
  
        m_Label(itrack, 0, 0) = trk.label();
  
        m_Err[iC].copyIn(itrack, trk.errors().Array());
        m_Par[iC].copyIn(itrack, trk.parameters().Array());
  
        m_Chg(itrack, 0, 0) = trk.charge();
        m_Chi2(itrack, 0, 0) = trk.chi2();
  
        // CopyIn seems fast enough, but indirections are quite slow.
        for (int hi = 0; hi < m_Nhits; ++hi) {
          const int hidx = trk.getHitIdx(hi);
          const int hlyr = trk.getHitLyr(hi);
          const Hit& hit = layerHits[hlyr].refHit(hidx);
  
          m_msErr[hi].copyIn(itrack, hit.errArray());
          m_msPar[hi].copyIn(itrack, hit.posArray());
  
          m_HoTArr[hi](itrack, 0, 0) = trk.getHitOnTrack(hi);
        }
      }
    }
  
    void MkFitter::slurpInTracksAndHits(const std::vector<Track>& tracks,
                                        const std::vector<HitVec>& layerHits,
                                        int beg,
                                        int end) {
      // Assign track parameters to initial state and copy hit values in.
  
      // This might not be true for the last chunk!
      // assert(end - beg == NN);
  
      MatriplexTrackPacker mtp(&tracks[beg]);
  
      for (int i = beg; i < end; ++i) {
        int itrack = i - beg;
        const Track& trk = tracks[i];
  
        m_Label(itrack, 0, 0) = trk.label();
  
        mtp.addInput(trk);
  
        m_Chg(itrack, 0, 0) = trk.charge();
        m_Chi2(itrack, 0, 0) = trk.chi2();
      }
  
      mtp.pack(m_Err[iC], m_Par[iC]);
  
      // CopyIn seems fast enough, but indirections are quite slow.
      for (int hi = 0; hi < m_Nhits; ++hi) {
        MatriplexHitPacker mhp(layerHits[hi].data());
  
        for (int i = beg; i < end; ++i) {
          const int hidx = tracks[i].getHitIdx(hi);
          const Hit& hit = layerHits[hi][hidx];
  
          m_HoTArr[hi](i - beg, 0, 0) = tracks[i].getHitOnTrack(hi);
  
          mhp.addInput(hit);
        }
  
        mhp.pack(m_msErr[hi], m_msPar[hi]);
      }
    }
  
    void MkFitter::inputTracksAndHitIdx(const std::vector<Track>& tracks, int beg, int end, bool inputProp) {
      // Assign track parameters to initial state and copy hit values in.
  
      // This might not be true for the last chunk!
      // assert(end - beg == NN);
  
      const int iI = inputProp ? iP : iC;
  
      int itrack = 0;
      for (int i = beg; i < end; ++i, ++itrack) {
        const Track& trk = tracks[i];
  
        m_Err[iI].copyIn(itrack, trk.errors().Array());
        m_Par[iI].copyIn(itrack, trk.parameters().Array());
  
        m_Chg(itrack, 0, 0) = trk.charge();
        m_Chi2(itrack, 0, 0) = trk.chi2();
        m_Label(itrack, 0, 0) = trk.label();
  
        for (int hi = 0; hi < m_Nhits; ++hi) {
          m_HoTArr[hi](itrack, 0, 0) = trk.getHitOnTrack(hi);
        }
      }
    }
  
    void MkFitter::inputTracksAndHitIdx(const std::vector<std::vector<Track> >& tracks,
                                        const std::vector<std::pair<int, int> >& idxs,
                                        int beg,
                                        int end,
                                        bool inputProp) {
      // Assign track parameters to initial state and copy hit values in.
  
      // This might not be true for the last chunk!
      // assert(end - beg == NN);
  
      const int iI = inputProp ? iP : iC;
  
      int itrack = 0;
      for (int i = beg; i < end; ++i, ++itrack) {
        const Track& trk = tracks[idxs[i].first][idxs[i].second];
  
        m_Label(itrack, 0, 0) = trk.label();
        m_SeedIdx(itrack, 0, 0) = idxs[i].first;
        m_CandIdx(itrack, 0, 0) = idxs[i].second;
  
        m_Err[iI].copyIn(itrack, trk.errors().Array());
        m_Par[iI].copyIn(itrack, trk.parameters().Array());
  
        m_Chg(itrack, 0, 0) = trk.charge();
        m_Chi2(itrack, 0, 0) = trk.chi2();
  
        for (int hi = 0; hi < m_Nhits; ++hi) {
          m_HoTArr[hi](itrack, 0, 0) = trk.getHitOnTrack(hi);
        }
      }
    }
  
    void MkFitter::inputSeedsTracksAndHits(const std::vector<Track>& seeds,
                                           const std::vector<Track>& tracks,
                                           const std::vector<HitVec>& layerHits,
                                           int beg,
                                           int end) {
      // Assign track parameters to initial state and copy hit values in.
  
      // This might not be true for the last chunk!
      // assert(end - beg == NN);
  
      int itrack;
      for (int i = beg; i < end; ++i) {
        itrack = i - beg;
  
        const Track& see = seeds[i];
  
        m_Label(itrack, 0, 0) = see.label();
        if (see.label() < 0)
          continue;
  
        m_Err[iC].copyIn(itrack, see.errors().Array());
        m_Par[iC].copyIn(itrack, see.parameters().Array());
  
        m_Chg(itrack, 0, 0) = see.charge();
        m_Chi2(itrack, 0, 0) = see.chi2();
  
        const Track& trk = tracks[see.label()];
  
        // CopyIn seems fast enough, but indirections are quite slow.
        for (int hi = 0; hi < m_Nhits; ++hi) {
          m_HoTArr[hi](itrack, 0, 0) = trk.getHitOnTrack(hi);
  
          const int hidx = trk.getHitIdx(hi);
          if (hidx < 0)
            continue;  //fixme, check if this is harmless
  
          const Hit& hit = layerHits[hi][hidx];
          m_msErr[hi].copyIn(itrack, hit.errArray());
          m_msPar[hi].copyIn(itrack, hit.posArray());
        }
      }
    }
  
    //------------------------------------------------------------------------------
    // Fitting with interleaved hit loading
    //------------------------------------------------------------------------------
  
    void MkFitter::inputTracksForFit(const std::vector<Track>& tracks, int beg, int end) {
      // Loads track parameters and hit indices.
  
      // XXXXMT4K has Config::nLayers: How many hits do we read in?
      // Check for max? Expect an argument?
      // What to do with invalid hits? Skip?
  
      // XXXX MT Here the same idx array WAS used for slurping in of tracks and
      // hots. With this, two index arrays are built, one within each packer.
  
      MatriplexTrackPacker mtp(&tracks[beg]);
      MatriplexHoTPacker mhotp(tracks[beg].getHitsOnTrackArray());
  
      int itrack = 0;
  
      for (int i = beg; i < end; ++i, ++itrack) {
        const Track& trk = tracks[i];
  
        m_Chg(itrack, 0, 0) = trk.charge();
        m_Chi2(itrack, 0, 0) = trk.chi2();
        m_Label(itrack, 0, 0) = trk.label();
  
        mtp.addInput(trk);
  
        mhotp.addInput(*trk.getHitsOnTrackArray());
      }
  
      mtp.pack(m_Err[iC], m_Par[iC]);
      for (int ll = 0; ll < Config::nLayers; ++ll) {
        mhotp.pack(m_HoTArr[ll], ll);
      }
    }
  
    void MkFitter::fitTracksWithInterSlurp(const std::vector<HitVec>& layersohits, const int N_proc) {
      // XXXX This has potential issues hits coming from different layers!
      // Expected to only work reliably with barrel (consecutive layers from 0 -> m_Nhits)
      // and with hits present on every layer for every track.
  
      // Loops over layers and:
      // a) slurps in hit parameters;
      // b) propagates and updates tracks
  
      for (int ii = 0; ii < m_Nhits; ++ii) {
        // XXXX Assuming hit index corresponds to layer!
        MatriplexHitPacker mhp(layersohits[ii].data());
  
        for (int i = 0; i < N_proc; ++i) {
          const int hidx = m_HoTArr[ii](i, 0, 0).index;
          const int hlyr = m_HoTArr[ii](i, 0, 0).layer;
  
          // XXXXMT4K What to do with hidx < 0 ????
          // This could solve the unbalanced fit.
          // Or, if the hidx is the "universal" missing hit, it could just work.
          // Say, hidx = 0 ... grr ... but then we don't know it is missing.
  
          if (hidx < 0 || hlyr < 0) {
            mhp.addNullInput();
          } else {
            mhp.addInput(layersohits[hlyr][hidx]);
          }
        }
  
        mhp.pack(m_msErr[0], m_msPar[0]);
  
        propagateTracksToHitR(m_msPar[0], N_proc, PropagationConfig::get_default().forward_fit_pflags);
  
        kalmanUpdate(m_Err[iP], m_Par[iP], m_msErr[0], m_msPar[0], m_Err[iC], m_Par[iC], N_proc);
      }
    }
  
    //==============================================================================
    // Fitting functions
    //==============================================================================
  
    void MkFitter::outputTracks(std::vector<Track>& tracks, int beg, int end, int iCP) const {
      // Copies last track parameters (updated) into Track objects.
      // The tracks vector should be resized to allow direct copying.
  
      int itrack = 0;
      for (int i = beg; i < end; ++i, ++itrack) {
        m_Err[iCP].copyOut(itrack, tracks[i].errors_nc().Array());
        m_Par[iCP].copyOut(itrack, tracks[i].parameters_nc().Array());
  
        tracks[i].setCharge(m_Chg(itrack, 0, 0));
  
        // XXXXX chi2 is not set (also not in SMatrix fit, it seems)
        tracks[i].setChi2(m_Chi2(itrack, 0, 0));
        tracks[i].setLabel(m_Label(itrack, 0, 0));
      }
    }
  
    void MkFitter::outputFittedTracksAndHitIdx(std::vector<Track>& tracks, int beg, int end, bool outputProp) const {
      // Copies last track parameters (updated) into Track objects and up to m_Nhits.
      // The tracks vector should be resized to allow direct copying.
  
      const int iO = outputProp ? iP : iC;
  
      int itrack = 0;
      for (int i = beg; i < end; ++i, ++itrack) {
        m_Err[iO].copyOut(itrack, tracks[i].errors_nc().Array());
        m_Par[iO].copyOut(itrack, tracks[i].parameters_nc().Array());
  
        tracks[i].setCharge(m_Chg(itrack, 0, 0));
        tracks[i].setChi2(m_Chi2(itrack, 0, 0));
        tracks[i].setLabel(m_Label(itrack, 0, 0));
  
        // QQQQ Could do resize and std::copy, as in MkFinder::copy_out(), but
        // we do not know the correct N_found_hits.
        tracks[i].resetHits();
        tracks[i].reserveHits(m_Nhits);
        for (int hi = 0; hi < m_Nhits; ++hi) {
          tracks[i].addHitIdx(m_HoTArr[hi](itrack, 0, 0), 0.);
        }
      }
    }
  
  }  // end namespace mkfit
  