#include <vector>
#include "Mapping.h"

//===========================================================================
int32_t DecodedHWAddressTwig(int32_t hwAddress, uint32_t rcu) {
  // Get twig index (0..3) from hardware address
  // This is for the new 4-branch setup
  if ( hwAddress < 0 ) return -1;
  int32_t twig = ((hwAddress>>11)&0x1);
  int32_t fec  = ((hwAddress>>7)&0xf);
  int32_t nfecs = 5;
  if ( (rcu==0) || (rcu==2) ) nfecs = 4;
  else if ( rcu == 1 )        nfecs = 6;
  if ( twig==1 ) {
    if ( fec>=nfecs ) twig = 3;
    else              twig = 2;
  } else {
    if ( fec>=nfecs ) twig = 1;
    else              twig = 0;
  }
  return twig;
}


int main( int argc, char** argv ) {

  Mapping *pm;

  pm = new Mapping();
  /*
  int32_t fRCU = 1;
  int32_t hwAddress = 1; //114

  cout << "Padrow/Pad fuer Pad " << hwAddress << ": " << pm->GetPadRow(hwAddress, fRCU)  << "/" 
       << pm->GetPad(hwAddress, fRCU) << " - " << pm->GetRadialDistance(hwAddress, fRCU) << "mm\n";
  cout << pm->GetMinDriftTime(pm->GetRadialDistance(hwAddress, fRCU)) << " microsec\n" << endl;
  
  // inverse mapping
  cout << "hwAddr von pad 31 in padrow 42 in rcu 3 ist " << pm->GetHWAddress(42, 31, 3) << endl;
  
  for (int32_t i=0; i<31; i++) {
    cout << "padrow " << i << ": " << pm->GetCenterPad(i, 0) << ", " << pm->GetNPads(i, 0)
	 << " (" << pm->GetPadsPerRow(i, 0) << ")" << endl;
  }
  for (int32_t i=30; i<63; i++) {
    cout << "padrow " << i << ": " << pm->GetCenterPad(i, 1) << ", " << pm->GetNPads(i, 1)
	 << " (" << pm->GetPadsPerRow(i, 1) << ")" << endl;
  }
  */

  /*
  cout << "PadRows RP0 = " << pm->GetMinPadRow(0) << " ... " << pm->GetMaxPadRow(0) << endl;
  cout << "PadRows RP1 = " << pm->GetMinPadRow(1) << " ... " << pm->GetMaxPadRow(1) << endl;
  cout << "PadRows RP2 = " << pm->GetMinPadRow(2) << " ... " << pm->GetMaxPadRow(2) << endl;
  cout << "PadRows RP3 = " << pm->GetMinPadRow(3) << " ... " << pm->GetMaxPadRow(3) << endl;
  cout << "PadRows RP4 = " << pm->GetMinPadRow(4) << " ... " << pm->GetMaxPadRow(4) << endl;
  cout << "PadRows RP5 = " << pm->GetMinPadRow(5) << " ... " << pm->GetMaxPadRow(5) << endl;

  // RP0
  for (int32_t i=pm->GetMinPadRow(0); i<=pm->GetMaxPadRow(0); i++) {
    cout << "RP0 - padrow " << i << ": Npads=" << pm->GetNPads(i, 0) << " (" << pm->GetPadsPerRow(i, 0) << ")" << endl;
  }
  // RP1
  for (int32_t i=pm->GetMinPadRow(1); i<=pm->GetMaxPadRow(1); i++) {
    cout << "RP1 - padrow " << i << ": Npads=" << pm->GetNPads(i, 1) << " (" << pm->GetPadsPerRow(i, 1) << ")" << endl;
  }
  */

  uint32_t rcu = 5;

  vector<uint16_t> ROLM[4];

  for ( uint32_t irow=pm->GetMinPadRow(rcu); irow<=pm->GetMaxPadRow(rcu); irow++ ) {
    for ( uint32_t ipad=0; ipad<pm->GetNPads(irow, rcu); ipad++ ) {
      int16_t ihw = pm->GetHWAddress(irow, ipad, rcu);
      if ( ihw>=0 ) {
        int32_t ibr = DecodedHWAddressTwig(ihw, rcu);
        if ( (ibr>=0) && (ibr<4) ) ROLM[ibr].push_back(ihw);
      }
    }
    for (int32_t ibr=0; ibr<4; ibr++) ROLM[ibr].push_back(0x1600); // Next Chunk marker
  }
  for (int32_t ibr=0; ibr<4; ibr++) ROLM[ibr].push_back(0xFFFF); // End-Of-ROLM marker         

  uint32_t endmarker = 0;

  ROLM[0].resize(1000);
  ROLM[1].resize(1000);
  ROLM[2].resize(1000);
  ROLM[3].resize(1000);

  if ( 0 ) {
    for (int32_t ii=0; ii<1000; ii++) {
      if ( ROLM[0].at(ii)==0xFFFF ) endmarker++;
      if ( ROLM[1].at(ii)==0xFFFF ) endmarker++;
      if ( ROLM[2].at(ii)==0xFFFF ) endmarker++;
      if ( ROLM[3].at(ii)==0xFFFF ) endmarker++;
      printf("0x%04X  0x%04X  0x%04X  0x%04X\n", ROLM[0].at(ii), ROLM[1].at(ii), ROLM[2].at(ii), ROLM[3].at(ii));
      if (endmarker>3) break;
    } 
  }

  uint32_t ctr[4];
  bool br_done[4];
  for (int32_t ibr=0; ibr<4; ibr++) {
    ctr[ibr]     = 0;
    br_done[ibr] = false;
  }
  uint32_t active = 1;
  endmarker = 0;
    
  if ( 1 ) {
    for (int32_t ii=0; ii<4000; ii++) {
      if (br_done[active]) {
	if      (active == 1) active = 0;
	else if (active == 0) active = 2;
	else if (active == 2) active = 3;
	else if (active == 3) active = 1;
	continue;
      }
      uint16_t val = ROLM[active].at(ctr[active]++);
      if ( val==0xFFFF ) {
	br_done[active] = true;
	endmarker++;
	printf("Branch %d - 0x%04X\n", active, val);
      } else if ( val==0x1600 ) {
	printf("Branch %d - 0x%04X\n", active, val);
	if      (active == 1) active = 0;
	else if (active == 0) active = 2;
	else if (active == 2) active = 3;
	else if (active == 3) active = 1;
      } else {
	printf("Branch %d - 0x%04X - row %d (pad %d)\n", active, val, pm->GetPadRow(val, rcu), pm->GetPad(val, rcu));
      }
      if (endmarker>3) break;
    }  
  }

  return 1;
}