CDCTrigger2DFinderFirmwareModule.cc

/**************************************************************************
 * BASF2 (Belle Analysis Framework 2)                                     *
 * Copyright(C) 2017 - Belle II Collaboration                             *
 *                                                                        *
 * Author: The Belle II Collaboration                                     *
 * Contributors: Tzu-An Sheng                                             *
 *                                                                        *
 * This software is provided "as is" without any warranty.                *
 **************************************************************************/
 
#include "trg/cdc/modules/houghtracking/xsi_loader.h"
 
#include <trg/cdc/modules/houghtracking/CDCTrigger2DFinderFirmwareModule.h>
#include <trg/cdc/Cosim.h>
#include <trg/cdc/CDCTrigger.h>
#include <framework/datastore/StoreArray.h>
#include <framework/logging/Logger.h>
#include <trg/cdc/dataobjects/CDCTriggerSegmentHit.h>
#include <trg/cdc/dataobjects/CDCTriggerTrack.h>
#include <trg/cdc/dataobjects/Bitstream.h>
 
#include <string>
#include <cstring>
#include <iostream>
#include <vector>
#include <array>
#include <algorithm>
#include <numeric>
#include <bitset>
#include <experimental/array>
 
using std::experimental::to_array;
constexpr double pi() { return std::atan(1) * 4; }
 
int Xsi::Loader::get_port_number_or_exit(std::string name)
{
  int portNumber = get_port_number(name.c_str());
  if (portNumber < 0) {
    std::cerr << "ERROR: " << name << " not found" << std::endl;
    throw;
  }
  return portNumber;
}
 
 
using namespace Belle2;
using namespace Cosim;
using namespace std;
using namespace CDCTrigger;
 
 
REG_MODULE(CDCTrigger2DFinderFirmware)
 
CDCTrigger2DFinderFirmwareModule::CDCTrigger2DFinderFirmwareModule() : Module(),
  Xsi_Instance(0)
//  Xsi_Instance->design_libname, simengine_libname)
{
  //Set module properties
  setDescription("Firmware simulation of Hough tracking algorithm for CDC trigger.");
 
  // Define module parameters
  addParam("hitCollectionName", m_hitCollectionName,
           "Name of the input StoreArray of CDCTriggerSegmentHits.",
           string(""));
  addParam("outputCollectionName", m_outputCollectionName,
           "Name of the StoreArray holding the tracks found in the Hough tracking.",
           string("TRGCDC2DFinderFirmwareTracks"));
  addParam("bitstreamNameTo2D", m_bitstreamNameTo2D,
           "Name of the StoreArray holding the raw bit content to 2D trackers",
           string("BitstreamTSF-2D"));
  addParam("fromFastSim", m_fromFastSim,
           "flag to read TS hits from the output of TSF firmware simulation",
           false);
  addParam("nClocks", m_nClockPerEvent,
           "how many clocks to simulate in each event",
           32);
 
}
 
CDCTrigger2DFinderFirmwareModule::~CDCTrigger2DFinderFirmwareModule()
{
  if (Xsi_Instance) {
    Xsi_Instance->close();
    delete Xsi_Instance;
    Xsi_Instance = 0;
  }
}
 
void CDCTrigger2DFinderFirmwareModule::initialize()
{
  Xsi_Instance = new Xsi::Loader(design_libname, simengine_libname);
  //StoreArray<CDCTriggerTrack>::registerPersistent(m_outputCollectionName);
  StoreArray<CDCTriggerSegmentHit> segmentHits(m_hitCollectionName);
  //StoreArray<CDCTriggerTrack> tracks(m_outputCollectionName);
  StoreArray<CDCTriggerTrack> tracks(m_outputCollectionName);
  tracks.registerInDataStore();
  tracks.registerRelationTo(segmentHits);
 
  m_debugLevel = getLogConfig().getDebugLevel();
  std::cout << "Design DLL     : " << design_libname << std::endl;
  std::cout << "Sim Engine DLL : " << simengine_libname << std::endl;
  try {
    memset(&info, 0, sizeof(info));
    char logName[] = "firmware.log";
    // info.logFileName = NULL;
    info.logFileName = logName;
    char wdbName[] = "test.wdb";
    info.wdbFileName = wdbName;
    Xsi_Instance->open(&info);
    Xsi_Instance->trace_all();
 
    clk  = Xsi_Instance->get_port_number_or_exit("Top_clkData_s");
    for (int iSL = 0; iSL < 5; ++iSL) {
      string name = "TSF" + to_string(iSL * 2) + "_input_i";
      tsfPort[iSL] = Xsi_Instance->get_port_number_or_exit(name);
      tsfInput[iSL].fill(zero_val);
      Xsi_Instance->put_value(tsfPort[iSL], tsfInput[iSL].data());
    }
    out  = Xsi_Instance->get_port_number_or_exit("Main_out");
 
    // Start low clock
    Xsi_Instance->put_value(clk, &zero_val);
    Xsi_Instance->run(10);
  } catch (std::exception& e) {
    std::cerr << "ERROR: An exception occurred: " << e.what() << std::endl;
  } catch (...) {
    std::cerr << "ERROR: An unknown exception occurred." << std::endl;
    // Xsi_Instance->get_error_info();
    throw;
  }
}
 
int CDCTrigger2DFinderFirmwareModule::localSegmentID(int globalID)
{
  std::array<int, 9> nWiresPerLayer = {160, 160, 192, 224, 256, 288, 320, 352, 384};
  // std::array<int, 8> nFEPerLayer = {5, 5, 6, 7, 8, 9, 10, 11, 12};
  std::array<int, 10> nAccumulate;
  std::partial_sum(nWiresPerLayer.begin(), nWiresPerLayer.end(), nAccumulate.begin() + 1);
  // shift the ID by 16 in SL8
  if (globalID > nAccumulate[7]) {
    globalID += 16;
  }
  auto nWiresInside = std::lower_bound(nAccumulate.begin(), nAccumulate.end(), globalID) - 1;
  return globalID - *nWiresInside;
}
 
auto CDCTrigger2DFinderFirmwareModule::encodeTSHit(CDCTriggerSegmentHit const* hit)
{
  bitset<8> id(localSegmentID(hit->getSegmentID()));
  bitset<9> priorityTime(hit->priorityTime());
  bitset<2> LR(hit->getLeftRight());
  bitset<2> priorityPosition(hit->getPriorityPosition());
  string joined = id.to_string() + priorityTime.to_string() +
                  LR.to_string() + priorityPosition.to_string();
  bitset<m_tsVectorWidth> tsInfo(joined);
  B2DEBUG(10, "encoded ts hit: " << tsInfo << " with local ID " << localSegmentID(hit->getSegmentID()));
  return tsInfo;
}
 
auto CDCTrigger2DFinderFirmwareModule::toTSSLV(const std::bitset<m_tsVectorWidth> ts)
{
  std::string word = ts.to_string();
  std::array<char, m_tsVectorWidth> vec;
  for (unsigned i = 0; i < word.size(); ++i) {
    vec[i] = (word[i] == '0') ? zero_val : one_val;
  }
  return vec;
}
 
TRGFinderTrack CDCTrigger2DFinderFirmwareModule::decodeTrack(string trackIn)
{
  constexpr unsigned lenCharge = 2;
  constexpr unsigned lenOmega = 7;
  constexpr unsigned lenPhi0 = 7;
  constexpr unsigned lenTS = 21;
  constexpr array<unsigned, 3> trackLens = {lenCharge, lenOmega, lenPhi0};
  array<unsigned, 4> trackPos{ 0 };
  partial_sum(trackLens.begin(), trackLens.end(), trackPos.begin() + 1);
  const unsigned shift = 16 - lenOmega;
  TRGFinderTrack trackOut;
  bitset<trackLens[1]> omega(trackIn.substr(trackPos[1], trackLens[1]));
  // shift omega to 16 bits, cast it to signed 16-bit int, and shift it back to 7 bits
  // thus the signed bit is preserved (when right-shifting)
  int omegaFirm = (int16_t (omega.to_ulong() << shift)) >> shift;
  trackOut.omega = 29.97 * 1.5e-4 * omegaFirm;
  int phi0 = bitset<trackLens[2]>(trackIn.substr(trackPos[2], trackLens[2])).to_ulong();
  trackOut.phi0 = pi() / 4 + pi() / 2 / 80 * (phi0 + 1);
  for (unsigned i = 0; i < 5; ++i) {
    trackOut.ts[i] = decodeTSHit(trackIn.substr(trackPos.back() + i * lenTS, lenTS));
  }
  return trackOut;
}
 
void CDCTrigger2DFinderFirmwareModule::decodeOutput(short latency)
{
  const unsigned lenTrack = 121;
  array<int, 4> posTrack;
  for (unsigned i = 0; i < posTrack.size(); ++i) {
    posTrack[i] = 6 + lenTrack * i;
  }
  string strOutput = slv_to_bin_string(finderOutput);
  StoreArray<CDCTriggerTrack> storeTracks(m_outputCollectionName);
  for (unsigned i = 0; i < m_nOutTracksPerClock; ++i) {
    if (finderOutput[i] == one_val) {
      TRGFinderTrack trk = decodeTrack(strOutput.substr(posTrack[i], lenTrack));
      // const CDCTriggerTrack* track =
      storeTracks.appendNew(trk.phi0, trk.omega, 0., latency);
      // TODO: dig out the TS hits in datastore, and
      // add relations to them
      B2DEBUG(10, "phi0:" << trk.phi0 << ", omega:" << trk.omega
              << ", at clock " << latency);
    }
  }
}
 
void CDCTrigger2DFinderFirmwareModule::loadFromFirmware(int iclock)
{
  static constexpr int inWidth = 429;
  // nTrackers is already declared in trg/cdc/include/CDCTrigger.h
  //static constexpr int nTrackers = 4;
  static constexpr int nTSFs = 5;
  using localInputVector = std::array<char, inWidth>;
  using inputArray = std::array<localInputVector, nTrackers>;
  using signalBus = std::array<inputArray, nTSFs>;
  using signalBitStream = Bitstream<signalBus>;
  StoreArray<signalBitStream> bitsTo2D;
  if (iclock < bitsTo2D.getEntries()) {
    int iAx = 0;
    for (const auto& sl : bitsTo2D[iclock]->signal()) {
      // copy input to 2D0
      std::copy(sl[0].cbegin(), sl[0].cbegin() + width_in, tsfInput[iAx].begin());
      iAx++;
    }
  } else {
    for (auto& sl : tsfInput) {
      sl.fill(zero_val);
    }
  }
}
 
void CDCTrigger2DFinderFirmwareModule::initializeFastSim()
{
  StoreArray<CDCTriggerSegmentHit> tsHits(m_hitCollectionName);
 
  B2DEBUG(10, tsHits.getEntries() << " hits in this event");
  // index of TS hits, sorted by TS found time
  // std::vector<int> iHit(tsHits.getEntries());
  // std::iota(iHit.begin(), iHit.end(), 0);
  std::vector<int> iHit;
  for (int i = 0; i < tsHits.getEntries(); ++i) {
    if (tsHits[i]->getISuperLayer() % 2 == 0) {
      iHit.push_back(i);
    }
  }
  std::sort(iHit.begin(), iHit.end(), [tsHits](int i, int j) {
    return tsHits[i]->foundTime() < tsHits[j]->foundTime();
  });
 
  for (auto hit : iHit) {
    B2DEBUG(10, "sorted found time: " << tsHits[hit]->foundTime());
  }
  // data clock period (32ns) in unit of 2ns
  const int clockPeriod = 16;
 
  // classify hits by clocks
  // number of clocks before the last input hit
  int nClocks = tsHits[iHit.back()]->foundTime() / clockPeriod;
  // each element of seqHit holds a list of all the hits within a clock cycle
  vector<vector<int> > seqHit(nClocks);
  auto itrHit = iHit.begin();
  // For each clock,
  for (int i = 0; i < nClocks; ++i) {
    if (tsHits[*itrHit]->foundTime() < 0) {
      B2WARNING("Negative found time! The clock assignment will be wrong.");
    }
    // Fill in all the hits whose found time is less than the next clock edge,
    while (tsHits[*itrHit]->foundTime() < (i + 1) * clockPeriod &&
           itrHit != iHit.end()) {
      seqHit[i].push_back(*itrHit);
      ++itrHit;
    }
    if (seqHit[i].size() > m_nInTSPerClock) {
      B2WARNING("Oops! Too many input hits in a clock!");
    }
  }
 
  m_tsHitVector.clear();
  m_tsHitVector.resize(nClocks);
 
  // assign hits to SLV
  for (unsigned iClock = 0; iClock < seqHit.size(); ++iClock) {
    for (auto iTS : seqHit[iClock]) {
      B2DEBUG(10, "encoding ts ID: " << tsHits[iTS]->getSegmentID() <<
              ", SL " << tsHits[iTS]->getISuperLayer() <<
              ", clock " << iClock
             );
      bitset<m_tsVectorWidth> ts(encodeTSHit(tsHits[iTS]));
      m_tsHitVector[iClock][tsHits[iTS]->getISuperLayer() / 2].push_back(ts);
    }
  }
 
}
 
void CDCTrigger2DFinderFirmwareModule::loadFromFastSim(unsigned iclock)
{
  for (unsigned iSL = 0; iSL < nAxialSuperLayer; ++iSL) {
    // firstly, clear the input signal vectors
    tsfInput[iSL].fill(zero_val);
    // then, copy the TS info to the input signal vectors
    // unless there is no more hit in the SL
    auto itr = tsfInput[iSL].begin() + 9;
    if (iclock < m_tsHitVector.size()) {
      for (auto ts : m_tsHitVector[iSL][iclock]) {
        tsVector inTS(toTSSLV(ts));
        copy(inTS.begin(), inTS.end(), itr);
        advance(itr, m_tsVectorWidth);
      }
    }
  }
}
 
void CDCTrigger2DFinderFirmwareModule::event()
{
  int status = 0;
  if (m_fromFastSim) {
    initializeFastSim();
  }
  for (int iClock = 0; iClock < m_nClockPerEvent; iClock++) {
    if (m_fromFastSim) {
      loadFromFastSim(iClock);
    } else {
      loadFromFirmware(iClock);
    }
    std::cout << "------------------" << "\n";
    std::cout << "clock #" << iClock << "\n";
    // assign input signals
    try {
      // Put clk to one
      Xsi_Instance->put_value(clk, &one_val);
      Xsi_Instance->run(10);
      // cout << display_value(tsfInput[iSL].data(), 9) << " " <<
      //      display_value(tsfInput[iSL].data() + 9, 210) << "\n";
      for (int iAx = 0; iAx < 5; ++iAx) {
        if (m_debugLevel >= 100  ||
        std::any_of(tsfInput[iAx].cbegin() + 9, tsfInput[iAx].cend(), [](char i) {
        return i != zero_val;
      })) {
          cout << "TSF" << iAx * 2 << " input: ";
          display_hex(tsfInput[iAx]);
        }
        // write the input
        Xsi_Instance->put_value(tsfPort[iAx], tsfInput[iAx].data());
      }
      // read the output
      Xsi_Instance->get_value(out, t2d_out);
 
      // Put clk to zero
      Xsi_Instance->put_value(clk, &zero_val);
      Xsi_Instance->run(10);
 
      string found = display_value(t2d_out, 6);
      if (m_debugLevel >= 100 ||
          found.find("1") != std::string::npos) {
        std::cout << "output: " << found << "\n";
        for (unsigned j = 0; j < count(found.begin(), found.end(), '1'); j++) {
          std::cout << display_value(t2d_out + 6 + 121 * j, 121) << "\n";
        }
      }
      finderOutput = to_array(t2d_out);
      decodeOutput(iClock);
    } catch (std::exception& e) {
      std::cerr << "ERROR: An exception occurred: " << e.what() << std::endl;
      status = 2;
    } catch (...) {
      std::cerr << "ERROR: An unknown exception occurred." << std::endl;
      status = 3;
    }
  }
  std::cout << "status code:" << status << std::endl;
}