/////////////////////////////////////////////////////////////////////
// RodPixController.cxx
/////////////////////////////////////////////////////////////////////
//
// 08/04/03  Version 1.0 (PM)
//           Initial release
//

#ifdef WIN32
#pragma warning(disable: 4290)
#endif

typedef struct { 
	float mu, sigma, a0, sigmaInv;
} GaussianCurve;

typedef struct {
	unsigned int pixelId, tPixel, iterations;
	GaussianCurve s;
	float chi2;
} PixelFit;

#include "PixController/RodPixController.h"
#include "PixController/PixScan.h"
#include "PixController/PixScanConfig.h"
#include "PixConfDBInterface/PixConfDBInterface.h"
#ifndef NOTDAQ
#include "RodCrate/RodModule.h"
#include "RodCrate/BocCard.h"
#endif
#include "Bits/Bits.h"
#include "Histo/Histo.h"
#include "Config/Config.h"
#include "registerIndices.h"
//#include "msgBuff.h"
#include "PixModule/PixModule.h"
#include "PixMcc/PixMcc.h"

#include <math.h>
#include <stdlib.h>
#include <iomanip>
#ifndef WIN32
#include <unistd.h>
#endif

using namespace PixLib;
using namespace SctPixelRod;

RodPixController::RodPixController(PixModuleGroup &modGrp, DBInquire *dbInquire) : 
                                   PixController(modGrp, dbInquire) { //! Constructor
  configInit();
  m_conf->read(dbInquire);
}

RodPixController::RodPixController(PixModuleGroup &modGrp) : 
                                   PixController(modGrp) { //! Constructor
  configInit();
}

RodPixController::~RodPixController() { //! Destructor
  if (m_conf != NULL) delete m_conf;
#ifndef NOTDAQ
  if (m_rod != NULL) delete m_rod;
#endif
}

void RodPixController::initHW() {
#ifndef NOTDAQ
  if (getModGroup().getVmeInterface().ok()) {
    if (m_rodSlot > 4 && m_rodSlot < 22) {
      try {
	int i;
        if (m_rod != NULL) delete m_rod;
	m_rod = new RodModule(m_rodSlot<<24, 0x01000000, getModGroup().getVmeInterface(), 4);
	m_rod->initialize(true);
	// Load the memory images and remember the file names
	for (i=0; i<4; i++) {
	  m_rod->loadSlaveImage(m_ipramFile, m_rod->getSdspMap()->ipram(), i, 'n');
	  m_rod->loadSlaveImage(m_idramFile, m_rod->getSdspMap()->idram(), i, 'n');
	  m_rod->loadSlaveImage(m_extFile, m_rod->getSdspMap()->xcode(), i, 'n');
	}
	for (i=0; i<4; i++) {
	  StartSlaveExecutingIn StartSlaveExecuting_In;
	  StartSlaveExecuting_In.slaveNumber   = i;
	  StartSlaveExecuting_In.commOnOff     = SLV_DSP_COMM_ON;
	  StartSlaveExecuting_In.slaveType     = SLAVE_MONITOR;
	  StartSlaveExecuting_In.timeoutInUsec = 0x200000;
	  RodPrimitive * startSlaveExec;
	  startSlaveExec = new RodPrimitive(8,1, START_SLAVE_EXECUTING, R_START_SLAVE_EXECUTING, (long int*) &StartSlaveExecuting_In);
	  executeMasterPrimitiveSync(*startSlaveExec);
	  delete startSlaveExec;
	}
	// Set the formatter link maps
	for (i=0; i<8; i++) {
	  writeRegister(0x48+i, 0, 32, m_fmtLinkMap[i]);     
	}    
	
	m_mode = "RESET";
      }
      catch (RodException &r) {
	throw RodPixControllerExc(RodPixControllerExc::BAD_SLOT_NUM, PixControllerExc::FATAL,  
				  getModGroup().getRodName(), m_rodSlot); 
      }
    } else {
      throw RodPixControllerExc(RodPixControllerExc::BAD_SLOT_NUM, PixControllerExc::ERROR,  
				getModGroup().getRodName(), m_rodSlot); 
    }
    m_hscanData = NULL;
    int i;
    for (i=0; i<4; i++) {
      m_trapRunning[i] = false;
      m_histoRunning[i] = false;
      m_dspModMask[i] = 0x0;
    }
    for (i=0; i<32; i++) {
      m_modPosition[i] = -1;
    }
    m_nMod = 0;
    m_lsNbin = 0;
    m_scanActive = false;
    m_runActive = false;
    m_fitDone = false;
  } else {
      throw RodPixControllerExc(RodPixControllerExc::NO_VME_INTERFACE, PixControllerExc::ERROR,  
				getModGroup().getRodName(), m_rodSlot); 
  }
#endif
}

void RodPixController::testHW() {
}

void RodPixController::configInit() {

  m_hscanData = NULL;
  int i;
  for (i=0; i<4; i++) {
    m_trapRunning[i] = false;
    m_histoRunning[i] = false;
    m_dspModMask[i] = 0x0;
  }
  for (i=0; i<32; i++) {
    m_modPosition[i] = -1;
  }
  m_nMod = 0;
  m_lsNbin = 0;
  m_scanActive = false;
  m_runActive = false;

#ifndef NOTDAQ
  m_rod = NULL;
#endif
  m_mode = "UNCONF";

  // Create the Config object
  m_conf = new Config("RodPixController"); 
  Config &conf = *m_conf;

  // Group general
  conf.addGroup("general");

  conf["general"].addInt("Slot", m_rodSlot, 10,
		  "Rod slot", true);
  conf["general"].addString("IPRAMFile", m_ipramFile, "sdsp_ipram.bin",
		  "IPRAM file name", true);
  conf["general"].addString("IDRAMFile", m_idramFile, "sdsp_idram.bin",
		  "IDRAM file name", true);
  conf["general"].addString("EXTFile", m_extFile, "sdsp_xcode.bin",
		  "XCODE file name", true);

  // Group FMT
  conf.addGroup("fmt");
  for (unsigned int i=0; i<8; i++) {
    std::ostringstream fnum;
    fnum << i;
    std::string tit = "linkMap_"+fnum.str();
    conf["fmt"].addInt("linkMap_"+fnum.str(), m_fmtLinkMap[i], 0x54320000,
		      "Formatter "+fnum.str()+" link map", true);
  }

  // Select default values
  conf.reset();
}

void RodPixController::sendCommand(Bits commands, int moduleMask) { //! Send command no out
  int i;

  // Copy the bits stream to the primitive
  int packSize = commands.size()/32;
  if (packSize*32 != commands.size()) packSize++;
  long int *tsdData = new long int[3+2*packSize];
  TransSerialDataIn* TransSerialData_In = (TransSerialDataIn*)tsdData;
  TransSerialData_In->captureSerOn      = 1; /* Capture module return data 
						in input FIFOs */;
  TransSerialData_In->streamLen[0] = packSize;
  TransSerialData_In->streamLen[1] = packSize;
  //AK  TransSerialData_In->streams      = (UINT32 *);
  //AK CHECK
  for (i=0; i<packSize; i++) {
    tsdData[i+3] = commands.pack(i);
    tsdData[i+packSize+3] = ~commands.pack(i);
  }

  // Set the output mask
  unsigned int mask1 = 0x0, mask2 = 0x0;
  for (unsigned int ib=0; ib<32; ib++) {
    if (moduleMask&(0x1<<ib)) {
      if (m_modDCI[ib]<32) {
        mask1 |= (0x1<<m_modDCI[ib]);
      } else {
        mask2 |= (0x1<<(m_modDCI[ib]-32));
      }
    }
  }
  writeRegister(FE_CMND_MASK_0_LO, 0, 32, mask1);
  writeRegister(FE_CMND_MASK_0_HI, 0, 32, mask2);
  writeRegister(FE_CMND_MASK_1_LO, 0, 32, 0x00000000);
  writeRegister(FE_CMND_MASK_1_HI, 0, 32, 0x00000000);
  writeRegister(RRIF_CMND_1, 20, 1, 0x1);
  writeRegister(RRIF_CMND_1, 2, 1, 0x1);

  // Send the serial stream using TRANS_SERIAL_DATA
#ifndef NOTDAQ
  RodPrimitive* tsdPrim = new RodPrimitive(7+2*packSize, 0, TRANS_SERIAL_DATA, R_TRANS_SERIAL_DATA, tsdData);
  executeMasterPrimitiveSync(*tsdPrim);
  delete tsdPrim;
#endif
  delete tsdData;
  
}

void RodPixController::sendCommandOutSync(Bits commands, int moduleMask, std::vector<Bits>& output) { //! Send command sync
}

void RodPixController::sendCommandOutAsync(Bits commands, int moduleMask, int commandID) { //! Send command async
}

bool RodPixController::checkOutReady(int commandID) {  //! Wait for command termination
  return false;
}

void RodPixController::getOutput(int commandID, std::vector<Bits>& output) {  //! Get command output
}

void RodPixController::writeModuleConfig(int moduleId, Module& config) {     //! Write module configuration
  
  std::cout << "writeModuleConfig " << moduleId << std::endl;

  // Update the module info
  if (moduleId < 0 || moduleId > 31) {
      throw RodPixControllerExc(RodPixControllerExc::INVALID_MODID, PixControllerExc::ERROR,  
				getModGroup().getRodName(), m_rodSlot); 
  }
  unsigned int pos;
  if (m_modPosition[moduleId] < 0) {
    // New module
    pos = m_nMod++;
    if (m_nMod >= 32) {
      throw RodPixControllerExc(RodPixControllerExc::TOO_MANY_MODULES, PixControllerExc::FATAL,  
				getModGroup().getRodName(), m_rodSlot); 
    }
    m_modPosition[moduleId] = pos; 
  } else {
    // Redefinition of a module
    pos = m_modPosition[moduleId];
  }
  m_modNum[pos] = moduleId;
  m_modGrp[pos] = config.groupId;
  m_modGrpDef[pos] = config.groupId;
  m_modDCI[pos] = config.pTTC;
  m_modActive[pos] = config.active;
  m_modTrigger[pos] = config.active;
  config.moduleIdx = moduleId;

  int configSize = sizeof(Module)/4;
  long int *configInt = (long int *)(&config);
  long int* rwData = new long int[3+configSize];
  rwData[0] = 0;
  rwData[1] = PHYSICS_MODULE_CONFIG;
  rwData[2] = moduleId;
  for (int i=0; i<configSize; i++) rwData[i+3] = configInt[i];
#ifndef NOTDAQ
  RodPrimitive* rwPrim = new RodPrimitive(7+configSize, 0, RW_MODULE_DATA, R_RW_MODULE_DATA, rwData);
  executeMasterPrimitiveSync(*rwPrim);
  delete rwPrim;
  rwData[1] = SCAN_MODULE_CONFIG;
  rwPrim = new RodPrimitive(7+configSize, 0, RW_MODULE_DATA, R_RW_MODULE_DATA, rwData);
  executeMasterPrimitiveSync(*rwPrim);
  delete rwPrim;
#endif
  delete[] rwData;
}

void RodPixController::readModuleConfig(int moduleId, Module& config) {     //! Read module configuration 
  int configSize = sizeof(Module)/4;
  std::vector<unsigned int> out;
  long int *configInt = (long int *)(&config);
#ifndef NOTDAQ
  long int rwData[4] = {1, PHYSICS_MODULE_CONFIG, moduleId, 0};
  RodPrimitive* rwPrim = new RodPrimitive(8, 0, RW_MODULE_DATA, R_RW_MODULE_DATA, rwData);
  executeMasterPrimitiveSync(*rwPrim,out);
  delete rwPrim;
#endif
  for (int i=0; i<configSize; i++) {
    configInt[i] = out[i];
  }
}

void RodPixController::sendModuleConfig(unsigned int moduleMask) {            //! Send module configuration
  SendConfigIn sconfData;
  unsigned int i;
  for (i=0; i<sizeof(SendConfigIn)/4; i++) ((long int *)&sconfData)[i] = 0;
  sconfData.port=2;
  sconfData.captureSerOn=0;
  sconfData.chipNum  = 16;
  sconfData.setLinks = 1;
  sconfData.restore  = 1;
  sconfData.cfgSet   = PHYSICS_MODULE_CONFIG; 
  sconfData.groupId  = MODULE_GROUP_ALL;
  sconfData.dataType = (0x1 << CONFIG_MODULE_ALL);
  sconfData.activeOnly       = 1;
  sconfData.enableDataTaking = 1;
  if (moduleMask == 0) {
    sconfData.module[0] = ALL_MODULES;
    sconfData.module[1] = ALL_MODULES;
#ifndef NOTDAQ
    RodPrimitive* sconfPrim = new RodPrimitive(4+sizeof(SendConfigIn)/4, 0, SEND_CONFIG, R_SEND_CONFIG, (long int *)&sconfData);
    executeMasterPrimitiveSync(*sconfPrim);
    delete sconfPrim;
#endif
  } else {
    int nMod = 0;
    for (i=0; i<32; i++) if (moduleMask & (0x1<<i)) nMod++;
    for (i=0; i<32; i++) {
      if (moduleMask & (0x1<<i)) {
        nMod++;
        if ((nMod%2) == 1) {
	  sconfData.module[0] = i;
	} else {
	  sconfData.module[1] = i;
#ifndef NOTDAQ
	  RodPrimitive* sconfPrim = new RodPrimitive(4+sizeof(SendConfigIn)/4, 0, SEND_CONFIG, R_SEND_CONFIG, (long int *)&sconfData);
	  executeMasterPrimitiveSync(*sconfPrim);
	  delete sconfPrim;
#endif
	  std::cout << "SEND_CONFIG - mod " << sconfData.module[0] << " " << sconfData.module[1] << std::endl;
	}
      }
    }
    if (nMod > 0 && (nMod%2) == 1) {
      sconfData.module[1] = NO_MODULE;
#ifndef NOTDAQ
      RodPrimitive* sconfPrim = new RodPrimitive(4+sizeof(SendConfigIn)/4, 0, SEND_CONFIG, R_SEND_CONFIG, (long int *)&sconfData);
      executeMasterPrimitiveSync(*sconfPrim);
      delete sconfPrim;
#endif
      std::cout << "SEND_CONFIG - mod " << sconfData.module[0] << " " << sconfData.module[1] << std::endl;
    } 
  }
}

void RodPixController::setCalibrationMode() { 
#ifndef NOTDAQ
  if (m_mode != "CALIBRATION") {
    // Configure links and masks
    rodConfigure();
    // Turn S-Link off
    writeRegister(RTR_CMND_STAT, 7, 1, 0x1);
    // Create and Send a SET_ROD_MODE primitive to a master DSP
    SetRodModeIn SetRodMode_In;
    SetRodMode_In.mode    = CALIBRATION_MODE+CALIBRATION_SLINK_OVERRIDE_MODE;
    SetRodMode_In.flag    = SET_MODE;
    SetRodMode_In.dataLen = 0;
    SetRodMode_In.data    = 0;

    RodPrimitive* rodmPrim = new RodPrimitive(8, 0, SET_ROD_MODE, R_SET_ROD_MODE, (long int*)&SetRodMode_In);
    executeMasterPrimitiveSync(*rodmPrim);
    delete rodmPrim;
    m_mode = "CALIBRATION";
  }
#endif
}

void RodPixController::setConfigurationMode() {
#ifndef NOTDAQ
  if (m_mode != "CONFIGURATION") {
    unsigned int i;
    for (i=0; i<4; i++) {
      // Disable histogramming
      stopHistogramming(i);
      // Disable trapping
      stopTrapping(i);
    }
    // Configure links and masks
    rodConfigure();
    // Create and Send a SET_ROD_MODE primitive to a master DSP
    SetRodModeIn SetRodMode_In;
    SetRodMode_In.mode    = CALIBRATION_MODE+CALIBRATION_SLINK_OVERRIDE_MODE;
    SetRodMode_In.flag    = SET_MODE;
    SetRodMode_In.dataLen = 0;
    SetRodMode_In.data    = 0;

    RodPrimitive* rodmPrim = new RodPrimitive(8, 0, SET_ROD_MODE, R_SET_ROD_MODE, (long int*)&SetRodMode_In);
    executeMasterPrimitiveSync(*rodmPrim);
    delete rodmPrim;
    // Disable calibration command decoding
    writeRegister(RRIF_CMND_1, 18, 1, 0x0);
    // Turn S-Link off
    writeRegister(RTR_CMND_STAT, 7, 1, 0x1);
    // Set endiannes to LE
    writeRegister(RTR_CMND_STAT,14, 1, 0x1);
    m_mode = "CONFIGURATION";
  }
#endif
}

void RodPixController::setRunMode() {
#ifndef NOTDAQ
  if (m_mode != "RUN") {
    // Configure links and masks
    rodConfigure();
    // Enable trapping
    //for (unsigned int i=0; i<4; i++) {
    //  setupTrapping(i);
    //  startTrapping(i);
    //}
    // Create and Send a SET_ROD_MODE primitive to a master DSP
    SetRodModeIn SetRodMode_In;
    SetRodMode_In.mode    = DATA_TAKING_MODE;
    SetRodMode_In.flag    = SET_MODE;
    SetRodMode_In.dataLen = 0;
    SetRodMode_In.data    = 0;

    RodPrimitive* rodmPrim = new RodPrimitive(8, 0, SET_ROD_MODE, R_SET_ROD_MODE, (long int*)&SetRodMode_In);
    executeMasterPrimitiveSync(*rodmPrim);
    delete rodmPrim;
    // Set endiannes to LE
    writeRegister(RTR_CMND_STAT,14, 1, 0x1);
    // Turn S-Link override on
    //writeRegister(RTR_CMND_STAT, 7, 1, 0x0);
    // Inhibit fifo playback
    //writeRegister(RRIF_CMND_1,24, 1, 0x1);
    // Dump RTR_CMND_STAT 
    std::cout << "RTR_CMND_STAT = " << std::hex << readRegister(RTR_CMND_STAT, 0, 32) << std::dec << std::endl;
    // Dump master reg 0 and 1
    std::cout << "RRIF_CMND_1 = "   << std::hex << readRegister(RRIF_CMND_1, 0, 32)   << std::dec << std::endl;      
    std::cout << "RRIF_CMND_0 = "   << std::hex << readRegister(RRIF_CMND_0, 0, 32)   << std::dec << std::endl;      
    // Dump EFB diagnostic regster
    std::cout << "EVT_MEM_CMND_STAT = " << std::hex << readRegister(EVT_MEM_CMND_STAT, 0, 32) << std::dec << std::endl;      
    std::cout << "EVT_MEM_MODE = "  << std::hex << readRegister(EVT_MEM_MODE, 0, 32)  << std::dec << std::endl;      
    std::cout << "EVT_MEM_FLAGS = " << std::hex << readRegister(EVT_MEM_FLAGS, 0, 32) << std::dec << std::endl;      
    std::cout << "EFB_CMND_0 = "    << std::hex << readRegister(EFB_CMND_0, 0, 32)    << std::dec << std::endl;      
    std::cout << "EFB_FORMATTER_STAT = "   << std::hex << readRegister(EFB_FORMATTER_STAT, 0, 32) 
	      << std::dec << std::endl;      
    std::cout << "EFB_RUNTIME_STAT_REG = " << std::hex << readRegister(EFB_RUNTIME_STAT_REG, 0, 32) 
	      << std::dec << std::endl;      
    // Dump FMT status
    for (int i=0; i<8; i++) {
      std::cout << "FMT_HEADER_TRAILER_LIMIT" << i << " = " << std::hex 
		<< readRegister(FMT_HEADER_TRAILER_LIMIT(i), 0, 32) << std::dec << std::endl;      
      std::cout << "FMT_ROD_BUSY_LIMIT"       << i << " = " << std::hex 
		<< readRegister(FMT_ROD_BUSY_LIMIT(i), 0, 32) << std::dec << std::endl;      
      std::cout << "FMT_PXL_LINK_L1A_CNT"     << i << " = " << std::hex 
		<< readRegister(FMT_PXL_LINK_L1A_CNT(i), 0, 32) << std::dec << std::endl;      
    }

    m_mode = "RUN";
  }
#endif
}


void RodPixController::writeScanConfig(PixScan &scn) {  //! Write scan parameters
#ifndef NOTDAQ
  // Initialize ScanPar structure
  // Zero all fields
  unsigned char *cf = (unsigned char *)(&m_scanPar);
  ScanControl *scanConfigP = &(m_scanPar);
  for (unsigned int i=0; i<sizeof(m_scanPar); i++) cf[i] = 0;
  // Set default values
  m_scanPar.general.configSet = scn.getModConfig();
  m_scanPar.general.reportErrors = 1;            
  m_scanPar.general.globalCtrl = 0;                          
  if (scn.getModScanConcurrent()) {
    m_scanPar.general.moduleScanMode = SCAN_CONCURRENT;
  } else {
    m_scanPar.general.moduleScanMode = SCAN_ONE_BY_ONE;
  }
  if (scn.getLoopActive(0) && scn.getDspProcessing(0)) {
    m_scanPar.general.scanParameter[0] = scn.getLoopParam(0);
    if (scn.getLoopVarUniform(0)) {
      m_scanPar.general.uniformPoints[0] = 1;
      m_scanPar.general.scanStart[0] = (unsigned int)scn.getLoopVarMin(0); 
      m_scanPar.general.scanEnd[0] = (unsigned int)scn.getLoopVarMax(0);
      m_scanPar.general.nBins[0] = scn.getLoopVarNSteps(0);
      m_scanPar.general.dataPtr[0] = (unsigned int *)0x0;
    } else {
      throw RodPixControllerExc(RodPixControllerExc::NON_UNIFORM_POINTS_NOT_IMPLEMENTED, 
                                PixControllerExc::FATAL, getModGroup().getRodName(), m_rodSlot); 
    }
  } else {
    m_scanPar.general.scanParameter[0] = SCAN_NONE;
    m_scanPar.general.uniformPoints[0] = 1;
    m_scanPar.general.scanStart[0] = 0; 
    m_scanPar.general.scanEnd[0] = 0;
    m_scanPar.general.nBins[0] = 0;
    m_scanPar.general.dataPtr[0] = (unsigned int *)0x0;
  }
  if (scn.getLoopActive(1) && scn.getDspProcessing(1)) {
    m_scanPar.general.scanParameter[1] = scn.getLoopParam(1);
    if (scn.getLoopVarUniform(1)) {
      m_scanPar.general.uniformPoints[1] = 1;
      m_scanPar.general.scanStart[1] = (unsigned int)scn.getLoopVarMin(1); 
      m_scanPar.general.scanEnd[1] = (unsigned int)scn.getLoopVarMax(1);
      m_scanPar.general.nBins[1] = scn.getLoopVarNSteps(1);
      m_scanPar.general.dataPtr[1] = (unsigned int *)0x0;
    } else {
      throw RodPixControllerExc(RodPixControllerExc::NON_UNIFORM_POINTS_NOT_IMPLEMENTED, 
                                PixControllerExc::FATAL, getModGroup().getRodName(), m_rodSlot); 
    }
  } else {
    m_scanPar.general.scanParameter[1] = SCAN_NONE;
    m_scanPar.general.uniformPoints[1] = 1;
    m_scanPar.general.scanStart[1] = 0; 
    m_scanPar.general.scanEnd[1] = 0;
    m_scanPar.general.nBins[1] = 0;
    m_scanPar.general.dataPtr[1] = (unsigned int *)0x0;
  }
  m_scanPar.general.repetitions = scn.getRepetitions();
  if (scn.getMaskStageMode() == PixScan::STATIC) {
    m_scanPar.general.maskMode = SCAN_STATIC;
    m_scanPar.general.stageAdvanceFirst = 0;
    m_scanPar.general.maskStages = 0; 
    m_scanPar.general.maskStageMode = MASK_STAGEMODE_SEL_ENA;
    m_scanPar.general.maskStagePattern = MASK_SCAN_32_STEP;
  } else {
    m_scanPar.general.maskMode = SCAN_STAGED;
    if (scn.getInnerLoopSwap()) {
      m_scanPar.general.stageAdvanceFirst = 0;
    } else {
      m_scanPar.general.stageAdvanceFirst = 1;
    }
    m_scanPar.general.maskStages = scn.getMaskStageSteps(); 
    m_scanPar.general.maskStageMode = scn.getMaskStageMode();
    m_scanPar.general.maskStagePattern = scn.getMaskStageTotalSteps();
  }
  m_scanPar.rodSetup.base = (unsigned int *)DEFAULT;
  m_scanPar.rodSetup.opt[0] = 1;
  m_scanPar.rodSetup.opt[1] = 1;
  m_scanPar.rodSetup.opt[2] = 1;	
  m_scanPar.rodSetup.extSetup = 2;
  m_scanPar.rodSetup.errors = 0;
  m_scanPar.rodSetup.errorsMCC = 0;    
  
  m_scanPar.strobe.duration = scn.getStrobeDuration();
  m_scanPar.strobe.delay = scn.getStrobeMCCDelay();
  m_scanPar.strobe.delayRange = scn.getStrobeMCCDelayRange();
  
  m_scanPar.trigger.accepts = scn.getConsecutiveLvl1TrigA(0);  
  m_scanPar.trigger.self = 0; 
  if (scn.getSelfTrigger()) m_scanPar.trigger.self = 1;
  m_scanPar.trigger.latency = scn.getLVL1Latency(); 
  if (scn.getStrobeLVL1DelayOverride()) {
    m_scanPar.trigger.calL1ADelay = scn.getStrobeLVL1Delay();
  } else {
    m_scanPar.trigger.calL1ADelay = ((ConfInt&)m_modGroup.config()["general"]["TriggerDelay"]).getValue();
  }

  m_scanPar.reset.binReset = 1;
  m_scanPar.reset.softFE = 1;
  m_scanPar.reset.shortSync = 1; 
  m_scanPar.reset.mediumSync = 1;
  m_scanPar.reset.longSync = 1; 
  m_scanPar.reset.ECR = 1;  
  m_scanPar.reset.BCR = 1; 
  m_scanPar.reset.moduleInit = 1; 
  
  m_scanPar.fe.vCal = scn.getFeVCal();
  m_scanPar.fe.cInject = CINJ_LOW;             
  if (scn.getChargeInjCap()==2) m_scanPar.fe.cInject = CINJ_HIGH;             
  m_scanPar.fe.phi = scn.getColumnROFreq();
  m_scanPar.fe.totThresholdMode = scn.getTotThrMode();
  m_scanPar.fe.totMinimum = scn.getTotMin();
  m_scanPar.fe.totTwalk = scn.getTotDHThr();  
  m_scanPar.fe.totLeMode = scn.getTotThrMode(); 
  m_scanPar.fe.hitbus = scn.getFeHitbus();
  m_scanPar.fe.digitalInject = 0;
  if (scn.getDigitalInjection()) m_scanPar.fe.digitalInject = 1;
  
  m_scanPar.mcc.checkRegisters = 0;
  m_scanPar.mcc.bandwidth = scn.getMccBandwidth();
  m_scanPar.mcc.enableFEContinuityCheck = 0;     
  m_scanPar.mcc.enableFECrosscheck = 0;
  
  m_scanPar.fitting.sCurves = 0;
  m_scanPar.fitting.totCalibration = 0;
  m_scanPar.fitting.tdacTune = 0;
  m_scanPar.fitting.tdacAlgorithm = 0;
  m_scanPar.fitting.fdacTune = 0;
  m_scanPar.fitting.calibrationOption = 0;
  m_scanPar.fitting.chi2Cut = 0.0;
  m_scanPar.fitting.customScale = 0.0;
  m_scanPar.fitting.customOffset = 0.0;

  for (int i=0; i<N_CMD_LIST_CMDS; i++) {
    m_scanPar.serial.triggerSequence[0].cmd[i] = 0;
    m_scanPar.serial.triggerSequence[1].cmd[i] = 0;
    m_scanPar.serial.triggerSequence[0].data[i] = 0;
    m_scanPar.serial.triggerSequence[1].data[i] = 0;
  }
  m_scanPar.serial.triggerSequence[0].cmd[0] = PIX_MC_CAL;
  m_scanPar.serial.triggerSequence[0].cmd[1] = DELAY;
  if (scn.getStrobeLVL1DelayOverride()) {
    m_scanPar.serial.triggerSequence[0].data[1] = scn.getStrobeLVL1Delay();
  } else {
    m_scanPar.serial.triggerSequence[0].data[1] = ((ConfInt&)m_modGroup.config()["general"]["TriggerDelay"]).getValue();
  }
  m_scanPar.serial.triggerSequence[0].cmd[2] = PIX_MC_TRIGGER;
  m_scanPar.serial.triggerSequence[0].cmd[3] = 0;

  m_scanPar.serial.calcFromStruct = 1;   // Pixel: indicates that the serial stream (cal. pulse
                                         // delay/L1A) should be calculated from the scan control
                                         // structure. 
  m_scanPar.serial.port = 0;             // Serial ports to use: 0, 1 or both (2)
  
  unsigned int i,sum = 0;
  for (i=0; i<4; i++) sum += scn.getModuleMask(i);

  if (sum != 0) {
    unsigned int activeModMask = 0;
    for (int mod=0; mod<32; mod++) {
      // Check if module is defined
      if (m_modPosition[mod] >=0) {
	// Find the group containing the module
	bool notInGrp = true;
	for (int grp=0; grp<4; grp++) {
	  if (scn.getModuleMask(grp) & (0x1<<mod)) {
	    notInGrp = false;
	    setGroupId(mod, grp);
	    if (scn.getReadoutEnabled(grp)) {
	      enableModuleReadout(mod);
              activeModMask |= (0x1 << mod);
	    } else {
	      disableModuleReadout(mod, false);
	    }
	  }
	}
	if (notInGrp) {
	  setGroupId(mod, 4);
	  disableModuleReadout(mod, false);
	}
      }
    }
    m_scanPar.dspDist.nDspPairs = 0;
    m_scanPar.dspDist.dspPair[0][0] = 0;
    m_scanPar.dspDist.dspPair[0][1] = 0;
    m_scanPar.dspDist.dspPair[1][0] = 0;
    m_scanPar.dspDist.dspPair[1][1] = 0;
    m_scanPar.dspDist.groupSPMap[0] = 0;
    m_scanPar.dspDist.groupSPMap[1] = 0;
    m_scanPar.dspDist.definedGroups = 0;
    m_scanPar.dspDist.slvBits = 0;
    for (int grp=0; grp<4; grp++) {
      if (scn.getReadoutEnabled(grp)) {
        m_scanPar.dspDist.definedGroups |= (0x1<<grp);
	m_scanPar.dspDist.slvBits |= (0x1<<grp);
	m_scanPar.dspDist.groupSPMap[0] |= (0x1<<grp);
	m_scanPar.dspDist.groupDSPMap[grp] = (0x1<<grp);
	m_dspModMask[grp] = scn.getModuleMask(grp) & activeModMask;
      } else {
	m_scanPar.dspDist.groupDSPMap[grp] = 0x0;
	m_dspModMask[grp] = 0x0;
      }    
    }
  } else {
    // Compute the SDSP module masks
    for (i=0; i<4; i++) m_dspModMask[i] = 0x0;
    for (i=0; i<m_nMod; i++) {
      int group = m_modGrpDef[i];
      setGroupId(i, group);      
      int modNum = m_modNum[i];
      if (m_modActive[i]) {
	if ((m_scanPar.dspDist.definedGroups & (0x1<<group)) != 0) {
	  unsigned int j;
	  for (j=0; j<4; j++) {
	    if ((m_scanPar.dspDist.groupDSPMap[j] & (0x1<<group)) != 0) {
	      m_dspModMask[j] |= (0x1<<modNum);
	    }
	  }
	}
      }
    }
  }

  // Turn S-Link off
  writeRegister(RTR_CMND_STAT, 7, 1, 0x1);
  for (i=0; i<4; i++) {
    // Enable trapping
    setupTrapping(i,scn);
    startTrapping(i);
    // Start histogramming
    setupHistogramming(i,scn);
    startHistogramming(i);
  }

  // Part of the scan structure should be passed to TEST 1 primitive
  // Do we need this ?
  long int* wsData = new long int[30];
  for (int j=0; j<30; j++) wsData[j] = 0;
  wsData[0] = 0x1e;
  wsData[1] = 1;
  wsData[2] = 0;
  wsData[3] = ((long int *)&(m_scanPar.strobe))[0];
  wsData[4] = ((long int *)&(m_scanPar.trigger))[0];
  wsData[5] = ((long int *)&(m_scanPar.fe))[0];
  wsData[6] = ((long int *)&(m_scanPar.fe))[1];
  wsData[7] = ((long int *)&(m_scanPar.fe))[2];
  wsData[8] = ((long int *)&(m_scanPar.mcc))[0];
  wsData[9] = ((long int *)&(m_scanPar.mcc))[1];
  RodPrimitive* wsPrim = new RodPrimitive(34, 0, TEST, R_TEST, wsData);
  executeMasterPrimitiveSync(*wsPrim);
  delete wsPrim;
  delete[] wsData;

  // Prepare the config primitive body
  m_hscanDataSize = sizeof(m_scanPar)/4;
  m_hscanData = new long int[m_hscanDataSize+4];
  m_hscanData[0] = HISTOGRAM_CTRL_TASK;
  m_hscanData[1] = R_HISTOGRAM_CTRL_TASK;
  m_hscanData[2] = 2; 
  m_hscanData[3] = 1;
  for (int i=0; i<m_hscanDataSize; i++) m_hscanData[i+4] = ((long int *)scanConfigP)[i];

  // Temporary pathces to clean-up ASAP
  for (int i=0; i<8; i++) {
    // Set formatters timeout to 1 ms
    writeRegister(FMT_READOUT_TIMEOUT(i), 0, 32, 0x9C40);
  }

  for (int i=0; i<2; i++) {
    for (int j=0; j<48; j++) {
      // Mask MCC and FE flag errors
      // OLD FPGA version 16bit counters
      //     writeRegister(EFB_ERROR_MASK(i,j), 0, 13, 0x10);
      // NEW FPGA version now 32 bit ...
      // maybe incorrect bit
      //      writeRegister(EFB_ERROR_MASK(i,j), 3, 1, 0x1);
      // BCID bit
      //      writeRegister(EFB_ERROR_MASK(i,j), 30, 1, 0x1);
      // all MCC flags:
      //      writeRegister(EFB_ERROR_MASK(i,j), 23,  9, 0x1ff);
      writeRegister(EFB_ERROR_MASK(i,j), 0,  32, 0xffc00008);
    }
  }
  
  // Store the total number of bins
  if (m_scanPar.general.nBins[1] > 0) {
    m_lsNbin = m_scanPar.general.nBins[0]*m_scanPar.general.nBins[1];
  } else {
    m_lsNbin = m_scanPar.general.nBins[0];
  }
#endif
}

void RodPixController::writeScanConfig(PixScanConfig &cfg) {  //! Write scan parameters
#ifndef NOTDAQ

  ScanControl &scanConfig = cfg.scanControl();
  ScanControl *scanConfigP = &(scanConfig);

  unsigned int i,sum = 0;
  for (i=0; i<4; i++) sum += cfg.getGroupMask(i);

  if (sum != 0) {
    for (int mod=0; mod<32; mod++) {
      // Check if module is defined
      if (m_modPosition[mod] >=0) {
	// Find the group containing the module
	bool notInGrp = true;
	for (int grp=0; grp<4; grp++) {
	  if (cfg.getGroupMask(grp) & (0x1<<mod)) {
	    notInGrp = false;
	    setGroupId(mod, grp);
	    if (cfg.readoutEnabled(grp)) {
	      enableModuleReadout(mod);
	    } else {
	      disableModuleReadout(mod, false);
	    }
	  }
	}
	if (notInGrp) {
	  setGroupId(mod, 4);
	  disableModuleReadout(mod, false);
	}
      }
    }
    scanConfig.dspDist.nDspPairs = 0;
    scanConfig.dspDist.dspPair[0][0] = 0;
    scanConfig.dspDist.dspPair[0][1] = 0;
    scanConfig.dspDist.dspPair[1][0] = 0;
    scanConfig.dspDist.dspPair[1][1] = 0;
    scanConfig.dspDist.groupSPMap[0] = 0;
    scanConfig.dspDist.groupSPMap[1] = 0;
    scanConfig.dspDist.definedGroups = 0;
    scanConfig.dspDist.slvBits = 0;
    for (int grp=0; grp<4; grp++) {
      if (cfg.readoutEnabled(grp)) {
	scanConfig.dspDist.definedGroups |= (0x1<<grp);
	scanConfig.dspDist.slvBits |= (0x1<<grp);
	scanConfig.dspDist.groupSPMap[0] |= (0x1<<grp);
	scanConfig.dspDist.groupDSPMap[grp] = (0x1<<grp);
	m_dspModMask[grp] = cfg.getGroupMask(grp);
      } else {
	scanConfig.dspDist.groupDSPMap[grp] = 0x0;
	m_dspModMask[grp] = 0x0;
      }    
    }
  } else {
    // Compute the SDSP module masks
    for (i=0; i<4; i++) m_dspModMask[i] = 0x0;
    for (i=0; i<m_nMod; i++) {
      int group = m_modGrpDef[i];
      setGroupId(i, group);      
      int modNum = m_modNum[i];
      if (m_modActive[i]) {
	if ((scanConfig.dspDist.definedGroups & (0x1<<group)) != 0) {
	  unsigned int j;
	  for (j=0; j<4; j++) {
	    if ((scanConfig.dspDist.groupDSPMap[j] & (0x1<<group)) != 0) {
	      m_dspModMask[j] |= (0x1<<modNum);
	    }
	  }
	}
      }
    }
  }

  // Turn S-Link off
  writeRegister(RTR_CMND_STAT, 7, 1, 0x1);
  for (i=0; i<4; i++) {
    // Enable trapping
    setupTrapping(i,cfg);
    startTrapping(i);
    // Start histogramming
    setupHistogramming(i,cfg);
    startHistogramming(i);
  }

  // Part of the scan structure should be passed to TEST 1 primitive
  // Do we need this ?
  long int* wsData = new long int[30];
  for (int j=0; j<30; j++) wsData[j] = 0;
  wsData[0] = 0x1e;
  wsData[1] = 1;
  wsData[2] = 0;
  wsData[3] = ((long int *)&(scanConfig.strobe))[0];
  wsData[4] = ((long int *)&(scanConfig.trigger))[0];
  wsData[5] = ((long int *)&(scanConfig.fe))[0];
  wsData[6] = ((long int *)&(scanConfig.fe))[1];
  wsData[7] = ((long int *)&(scanConfig.fe))[2];
  wsData[8] = ((long int *)&(scanConfig.mcc))[0];
  wsData[9] = ((long int *)&(scanConfig.mcc))[1];
  RodPrimitive* wsPrim = new RodPrimitive(34, 0, TEST, R_TEST, wsData);
  executeMasterPrimitiveSync(*wsPrim);
  delete wsPrim;
  delete[] wsData;

  // Prepare the config primitive body
  m_hscanDataSize = sizeof(scanConfig)/4;
  m_hscanData = new long int[m_hscanDataSize+4];
  m_hscanData[0] = HISTOGRAM_CTRL_TASK;
  m_hscanData[1] = R_HISTOGRAM_CTRL_TASK;
  m_hscanData[2] = 2; 
  m_hscanData[3] = 1;
  for (int i=0; i<m_hscanDataSize; i++) m_hscanData[i+4] = ((long int *)scanConfigP)[i];

  // Temporary pathces to clean-up ASAP
  for (int i=0; i<8; i++) {
    // Set formatters timeout to 1 ms
    writeRegister(FMT_READOUT_TIMEOUT(i), 0, 32, 0x9C40);
  }

  for (int i=0; i<2; i++) {
    for (int j=0; j<48; j++) {
      // Mask MCC and FE flag errors
      writeRegister(EFB_ERROR_MASK(i,j), 0,  32, 0xffc00008);
    }
  }
  
  // Store the total number of bins
  if (scanConfig.general.nBins[1] > 0) {
    m_lsNbin = scanConfig.general.nBins[0]*scanConfig.general.nBins[1];
  } else {
    m_lsNbin = scanConfig.general.nBins[0];
  }
#endif
}

void RodPixController::startScan() {                       //! Write scan parameters
#ifndef NOTDAQ
  // Dump RTR_CMND_STAT 
  std::cout << "RTR_CMND_STAT = " << std::hex << readRegister(RTR_CMND_STAT, 0, 32) << std::dec << std::endl;
  // Start a sacn
  if (m_hscanData != NULL) {
    //m_rod->mdspSingleWrite(DIAGNOSTIC_REG,0x000000);
    //m_rod->mdspSingleWrite(DIAGNOSTIC_REG,0x200040);
#ifndef NOTDAQ
    RodPrimitive *hscanPrim = new RodPrimitive(8+m_hscanDataSize, 0, START_TASK, R_START_TASK, m_hscanData);
    executeMasterPrimitiveSync(*hscanPrim);
#endif
    delete hscanPrim;
    delete[] m_hscanData;
    m_hscanData = NULL;
    m_scanActive = true;
    m_fitDone = false;
  }
#endif
}


void RodPixController::getErrorHistos(unsigned int dsp, Histo* &his) {

#ifndef NOTDAQ
  TaskOperationIn TaskOperation_In;
  TaskOperation_In.taskType      = ERROR_TASK;
  TaskOperation_In.taskOperation = TASK_QUERY;
  TaskOperation_In.data          = 0;


  RodPrimitive *herrors = new RodPrimitive(4 + sizeof(TaskOperationIn)/4, 0, 
					   TASK_OPERATION,  R_TASK_OPERATION, 
					       (long int*)&TaskOperation_In);
  std::vector<unsigned int> outs;
  unsigned int offset = 2*(sizeof(ListHeader)+sizeof(PrimHeader))/4;
  executeSlavePrimitiveSync(*herrors, outs, dsp);
   
  std::ostringstream nam, tit;
  nam << "dsp_errors_"  << dsp;
  tit << "DSP # " << dsp << " Errors";

  std::cout << "Downloading error histo: size: " << outs.size() << std::endl;
  int histsize = MAX_LINKS;
  his = new Histo(nam.str(), tit.str(), 4, histsize, -0.5, histsize-0.5, 
		  4, -0.5, 3.5);

    if(outs.size() > (offset + sizeof(ErrorTaskOut)/4)){

      ErrorTaskOut *errorOut = (ErrorTaskOut *)&outs[offset];
      std::cout << "ErrorHosts  Events: " << errorOut->errorInfo.eventCount
	       << " Errors: " << errorOut->errorInfo.errorEventCount 
	       << std::endl;

      his->set(1, 0, errorOut->nErrors);
      his->set(2, 0, errorOut->errorInfo.eventCount);
      his->set(3, 0, errorOut->errorInfo.errorEventCount);
      his->set(4, 0, errorOut->errorInfo.errorCount);
      for (unsigned int i = 0; i < ERRORTYPE_LAST; i++){ 
	his->set(i, 1, errorOut->errorInfo.type[i]);
      }    
      for (unsigned int i = 0; i < ERRORTYPE_D_LAST; i++){ 
	his->set(i, 2, errorOut->errorInfo.detailedtype[i]);
      }    
      for (unsigned int i = 0; i < MAX_LINKS; i++){ 
	his->set(i, 3, errorOut->errorInfo.linkErrorCnt[i]);
	if(errorOut->errorInfo.linkErrorCnt[i]>0)std::cout << "Error on Link: " 
							   << std::hex << errorOut->errorInfo.linkErrorCnt[i]
							   << std::dec << std::endl;
      }    
    }

#endif
}

bool RodPixController::fitHistos(){
#ifndef NOTDAQ
  if (!m_fitDone) {
    std::cout << "Start DSP Fit" << std::endl;
    FitHistogramsIn FitHistograms_In;
    FitHistograms_In.fitBase     = (UINT32*)DEFAULT;
    FitHistograms_In.fitFlags    = MAX_LIKELIHOOD;
    FitHistograms_In.routineType = FIT_ROUTINE_C;
    
    RodPrimitive *hfithisto = new RodPrimitive(4 + sizeof(FitHistogramsIn)/4, 0, FIT_HISTOGRAMS, R_FIT_HISTOGRAMS, 
					       (long int*)&FitHistograms_In);
    executeSlavePrimitiveOnAllSync(*hfithisto);
    delete hfithisto;
    std::cout << "End DSP Fit" << std::endl;
    m_fitDone = true;
    return true;
  }
#endif
  return false;
}

void RodPixController::getFitResults(HistoType type, unsigned int mod, unsigned int slv, 
std::vector< Histo * > &thr, std::vector< Histo * > &noise, std::vector< Histo * > &chi2){
#ifndef NOTDAQ

  unsigned int i, nmod = 0, xmod=0;

  SendDataIn SendData_In;
  SendData_In.dataType      = HISTOGRAM_DATA;
  SendData_In.repBufferFlag = REPLY_DIRECT_DATA;

  RodPrimitive *hsenddataPrim = new RodPrimitive(4 + sizeof(SendDataIn)/4, 0, 
                                                 SEND_DATA, R_SEND_DATA, (long int*)&SendData_In);
  std::vector<unsigned int> outs;
  executeSlavePrimitiveSync(*hsenddataPrim, outs, slv);
  delete hsenddataPrim;
  HistoInfo *hinfo;
  int infooffset = (2*sizeof(PrimHeader)+2*sizeof(ListHeader)+sizeof(SendDataOut))/4;
  hinfo = (HistoInfo *)&(outs[infooffset]);

  std::cout << "SEND_DATA DECODED: " 
            << "FIT: bins: " << std::hex << hinfo->hmeminfo[HIST_FIT].nBins 
            << " base: "     << std::hex << hinfo->hmeminfo[HIST_FIT].base
            << " boffset: "  << std::hex << hinfo->hmeminfo[HIST_FIT].binOffset
            << " moffset: "  << std::hex << hinfo->hmeminfo[HIST_FIT].moduleOffset
            << std::endl; 

  for (i=0; i<32; i++ ) {
    if ((m_dspModMask[slv]&(0x1<<i))) {
      if (i == mod) xmod = nmod;
      nmod++;
    }
  }
  if (nmod == 0) return;

  std::ostringstream mnum;
  std::string nam, tit;
  switch (type) {
  case SCURVES:
    while (thr.size() < 32) thr.push_back((Histo *)NULL);
    while (noise.size() < 32) noise.push_back((Histo *)NULL);
    while (chi2.size() < 32) chi2.push_back((Histo *)NULL);
    mnum << mod;
    if (thr[mod] == NULL) {
      nam = "Thr_" + mnum.str();
      tit = "Threshold mod " + mnum.str();
      thr[mod] = new Histo(nam, tit, 144, -0.5, 143.5, 320, -0.5, 319.5);
    }
    if (noise[mod] == NULL) {
      nam = "Noise_" + mnum.str();
      tit = "Noise mod " + mnum.str();
      noise[mod] = new Histo(nam, tit, 144, -0.5, 143.5, 320, -0.5, 319.5);
    }
    if (chi2[mod] == NULL) {
      nam = "Chi2_" + mnum.str();
      tit = "Chi2 mod " + mnum.str();
      chi2[mod] = new Histo(nam, tit, 144, -0.5, 143.5, 320, -0.5, 319.5);
    }
    if ((m_dspModMask[slv]&(0x1<<mod)) != 0) {
      std::cout << "Reading Fit for mod " << mod << " on slave " << slv << std::endl;
      PixelFit *out;
      std::vector<unsigned int> out1;
      for (i=0; i<hinfo->hmeminfo[HIST_FIT].nBins; i++) {
	out1.clear();
	readSlaveMemDirect(slv, (int)hinfo->hmeminfo[HIST_FIT].base+i*4*hinfo->hmeminfo[HIST_FIT].binOffset+xmod*4*hinfo->hmeminfo[HIST_FIT].moduleOffset, 
                           0xB400*sizeof(PixelFit)/4, out1);
	out = (PixelFit *)&(out1[0]);

	for (int is=0; is<32; is++) {
	  for (int fe=0; fe<16; fe++) {
	    for (int ic=0; ic<18; ic++) {
	      for (int ir=0; ir<5; ir++) {
		int xc,xr;
		if (fe < 8) {
		  xc = 18*fe + ic;
		  if (ic%2 == 0) {
		    xr = is+ir*32;
		  } else {
		    xr = 159-is-ir*32;
		  }
		} else {
		  xc = 143 - 18*(fe-8) - ic;
		  if (ic%2 == 0) {
		    xr = 319-is-ir*32;
		  } else {
		    xr = 319-(159-is-ir*32);
		  }
		}
		thr[mod]->set(xc,xr,(double)out[is*16*18*5+fe*18*5+ic*5+ir].s.mu);
		noise[mod]->set(xc,xr,(double)out[is*16*18*5+fe*18*5+ic*5+ir].s.sigma);  
		chi2[mod]->set(xc,xr,(double)out[is*16*18*5+fe*18*5+ic*5+ir].chi2);
	      }
	    }
	  }
	}
      }
    }
    break;
  default: 
    break;
  };
#endif
}

void RodPixController::getHisto(HistoType type, unsigned int mod, unsigned int slv, std::vector< std::vector<Histo*> >& his) {  //! Read a histogram
#ifndef NOTDAQ

  unsigned int im, nmod = 0, xmod=0;

  SendDataIn SendData_In;
  SendData_In.dataType      = HISTOGRAM_DATA;
  SendData_In.repBufferFlag = REPLY_DIRECT_DATA;

  RodPrimitive *hsenddataPrim = new RodPrimitive(4 + sizeof(SendDataIn)/4, 0, 
                                                 SEND_DATA, R_SEND_DATA, (long int*)&SendData_In);
  std::vector<unsigned int> outs;
  executeSlavePrimitiveSync(*hsenddataPrim, outs, slv);
  delete hsenddataPrim;

  //  std::cout << "SEND_DATA REPLY: " << std::endl; 
  //  for (i = 0; i<outs.size(); i++) std::cout << "SEND HISTO out["  << i 
  //                                           << "] = " << std::hex 
  //                                          << outs[i] << std::endl; 

  HistoInfo *hinfo;
  int infooffset = (2*sizeof(PrimHeader)+2*sizeof(ListHeader)+sizeof(SendDataOut))/4;
  hinfo = (HistoInfo *)&(outs[infooffset]);

  if (mod >= 32) return;
  for (im=0; im<32; im++ ) {
    if ((m_dspModMask[slv]&(0x1<<im))) {
      if (im == mod) xmod = nmod;
      nmod++;
    }
  }
  if (nmod == 0) return;
  
  std::string scanname, scanname1;
  int bins, base, modoffset, binoffset, size;
  int htype;
  switch (type) {
  case OCCUPANCY :
    scanname   = "Occup_";
    scanname1  = "Occupancy";
    htype      = HIST_OCC;
    size       = 0xb400/2;
    break;
  case HITOCC:
    scanname   = "HitOcc_";
    scanname1  = "HitOccupancy";
    htype      = HIST_HITOCC;
    size       = 0xb400/2;
    break;
  case TOTAVERAGE:
    scanname   = "ToTAvg_";
    scanname1  = "ToTAverage";
    htype      = HIST_TOTAVERAGE;
    size       = 0xb400/2;
    break;
  case TOT_MEAN :
    scanname   = "ToTMean_";
    scanname1  = "ToTMean";
    htype      = HIST_TOTMEAN;
    size       = 0xb400;
    break;
  case TOT_SIGMA :
    scanname   = "ToTSigma_";
    scanname1  = "ToTSigma";
    htype      = HIST_TOTSIGMA;
    size       = 0xb400;
    break;
  case LVL1 :
    scanname   = "LVL1_";
    scanname1  = "LVL1";
    htype      = HIST_TIMESLICE;
    size       = 0xb400;
    break;
  default: 
    break;
  };

  bins       = hinfo->hmeminfo[htype].nBins;
  base       = (int)hinfo->hmeminfo[htype].base;
  modoffset  = 4*hinfo->hmeminfo[htype].moduleOffset;
  binoffset  = 4*hinfo->hmeminfo[htype].binOffset;
  std::cout << "SEND_DATA DECODED: " 
            << scanname1
	    << " valid modules on SDSP: "  << hinfo->nValidModules
	    << " # of bins: " << std::hex << bins
	    << " base: "      << std::hex << base
	    << " boffset: "   << std::hex << binoffset/4
	    << " moffset: "   << std::hex << modoffset/4
	    << std::endl; 

  if ((m_dspModMask[slv]&(0x1<<mod)) != 0) {
    std::cout << "Reading histo for mod " << mod << " on slave " << slv << std::endl;
    while (his.size() < 32) {
      std::vector< Histo * > vh;
      his.push_back(vh);
    }
    his[mod].clear();
    //while ((int)his[mod].size() < bins) his[mod].push_back((Histo *)NULL);
    unsigned short int *out;
    FLOAT32 *outf;
    std::vector<unsigned int> out1;
    for (int i=0; i<bins; i++) {
      out1.clear();
      //readSlaveMemDirect(slv, 0xa00e2000+(i*nmod+xmod)*0x16800, 0x5A00, out1);
      //readSlaveMemDirect(slv,  (int)hinfo->base[HIST_OCC] + i*4*hinfo->binOffset[HIST_OCC] + xmod*4*hinfo->moduleOffset[HIST_OCC], 0x5A00, out1);
      readSlaveMemDirect(slv,(int)base + i*binoffset + xmod*modoffset, size, out1);
      out  = (unsigned short *)&(out1[0]);
      outf = (FLOAT32 *)&(out1[0]);
      std::ostringstream nam, tit;
      nam << scanname  << mod    << "_" << i;
      tit << scanname1 <<" mod " << mod << " bin " << i;
      Histo *h;
      if (type == OCCUPANCY) {
	h = new Histo(nam.str(), tit.str(), 1, 144, -0.5, 143.5, 
		      320, -0.5, 319.5);
      }else if(type == HITOCC || type == TOTAVERAGE) {
	h = new Histo(nam.str(), tit.str(), 46080, 0, 46079);
      } else {
	h = new Histo(nam.str(), tit.str(), 4, 144, -0.5, 143.5, 
		      320, -0.5, 319.5);
      }
      for (int is=0; is<32; is++) {
	for (int fe=0; fe<16; fe++) {
	  for (int ic=0; ic<18; ic++) {
	    for (int ir=0; ir<5; ir++) {
	      int xc,xr;
	      if (fe < 8) {
		xc = 18*fe + ic;
		if (ic%2 == 0) {
		  xr = is+ir*32;
		} else {
		  xr = 159-is-ir*32;
		}
	      } else {
		xc = 143 - 18*(fe-8) - ic;
		if (ic%2 == 0) {
		  xr = 319-is-ir*32;
		} else {
		  xr = 319-(159-is-ir*32);
		}
	      }
	      switch (type) {
	      case OCCUPANCY :
	      case LVL1 :
		h->set(xc,xr,out[is*16*18*5+fe*18*5+ic*5+ir]);
		break;
	      case HITOCC :
	      case TOTAVERAGE :
		h->set(is*16*18*5+fe*18*5+ic*5+ir,out[is*16*18*5+fe*18*5+ic*5+ir]);
		break;
	      case TOT_MEAN :
	      case TOT_SIGMA :
		h->set(xc,xr,outf[is*16*18*5+fe*18*5+ic*5+ir]);
		break;
	      default:
		break;
	      };
	    }
	  }
	}
      }
      his[mod].push_back(h);
    }
  }
#endif
}

void RodPixController::writeRunConfig(PixRunConfig &run) { //! Get the run configuration parameters
#ifndef NOTDAQ
  // Put the rod in configuration mode
  setConfigurationMode();
  for (int mod=0; mod<32; mod++) {
    // Check if module is defined
    if (m_modPosition[mod] >=0) {
      // Find the group containing the module
      bool notInGrp = true;
      bool trig = false;
      for (int grp=0; grp<4; grp++) {
        if (run.getGroupMask(grp) & (0x1<<mod)) {
          notInGrp = false;
          setGroupId(mod, grp);
          if (run.readoutEnabled(grp)) {
            enableModuleReadout(mod);
            trig = true;
	  } else {
            if (run.triggerEnabled(grp)) {
              disableModuleReadout(mod, true);
              trig = true;
	    } else {
              disableModuleReadout(mod, false);
	    }
	  }        
	}
      }
      if (notInGrp) {
        disableModuleReadout(mod, false);
        trig = false;
      }
      if (trig) {
	// Prepare for DataTaking
	getModGroup().module(mod)->pixMCC()->setRegister("LV1_Contiguous", run.consecutiveLvl1()-1);
	getModGroup().module(mod)->pixMCC()->enableDataTaking();
	getModGroup().module(mod)->pixMCC()->bcr();
	getModGroup().module(mod)->pixMCC()->ecr();     
	getModGroup().module(mod)->pixMCC()->syncFE();     
      }
    }
  }
      
  // Put the rod in Data Taking Mode
  setRunMode();

  for (int fmt=0; fmt<8; fmt++) {
    // Inform the formatters about the number of consecutive LVL1
    writeRegister(FMT_PXL_LINK_L1A_CNT(fmt), 0, 16, 0x1111*(run.consecutiveLvl1()-1));
    // Change the default BUSY and HEADER-TRAILER limits
    //writeRegister(FMT_HEADER_TRAILER_LIMIT(fmt), 0, 16, 0x5ff);
    //writeRegister(FMT_ROD_BUSY_LIMIT(fmt), 0, 16, 0x7f0);     
  }
#endif  
}

void RodPixController::startRun(int ntrig) {     //! Start a run
}

void RodPixController::stopRun() {               //! Terminates a run
}

int RodPixController::runStatus() {              //! Check the status of the run
  if (m_scanActive) {
    unsigned int sr = 0;
#ifndef NOTDAQ
    sr = m_rod->mdspSingleRead(STATUS_REG_0);
#endif
    return (sr & 0x10000) >> 16;
  } else if (m_runActive) {
    return 0;
  }
  return 0;
}

int RodPixController::nTrigger() {               //! Returns the number of trigger processed so far
  if (m_scanActive) {
    unsigned int sr = 0;
#ifndef NOTDAQ
    sr = m_rod->mdspSingleRead(HCMD_STAT_REG_0);
#endif
    return (sr & 0xfff) + ((sr & 0xff0000)>>4);
  } else if (m_runActive) {
    return 0;
  }
  return 0;
}

void RodPixController::downloadFifo(int size) {
  m_fifoA.clear();
  m_fifoB.clear();
  readFifo(INPUT_MEM, BANK_A, size, m_fifoA);
  readFifo(INPUT_MEM, BANK_B, size, m_fifoB);
}

bool RodPixController::getFifoBit(unsigned int ic, unsigned int il) {
  bool bit = false;
  if (il<48) {
    unsigned int ip = (ic*48+il)/32;
    unsigned int ib = (ic*48+il)%32;
    if (ip<m_fifoA.size()) bit = ((m_fifoA[ip]&(0x1<<ib)) != 0);
  } else {
    unsigned int ip = (ic*48+il-48)/32;
    unsigned int ib = (ic*48+il-48)%32;
    if (ip<m_fifoB.size()) bit = ((m_fifoB[ip]&(0x1<<ib)) != 0);
  } 
  return bit;
}

void RodPixController::prepareTestPattern(PixScan *scn) {
  unsigned int i,sum = 0;
  for (i=0; i<96; i++) {
    m_inmLinkTable[i] = 0;
    m_inmModuleTable[i] = 0;
  }
  for (i=0; i<4; i++) sum += scn->getModuleMask(i);
  if (sum != 0) {
    m_modMask = 0;
    bool first = true;
    for (int mod=0; mod<32; mod++) {
      // Check if module is defined
      if (m_modPosition[mod] >=0) {
	bool active = false;
	// Find the group containing the module
	bool notInGrp = true;
	for (int grp=0; grp<4; grp++) {
	  if (scn->getModuleMask(grp) & (0x1<<mod)) {
	    notInGrp = false;
	    setGroupId(mod, grp);
	    if (scn->getReadoutEnabled(grp)) {
	      enableModuleReadout(mod);
              m_modMask |= (0x1 << mod);
	      active = true;
	    } else {
	      disableModuleReadout(mod, false);
	    }
	  }
	}
	if (notInGrp) {
	  setGroupId(mod, 4);
	  disableModuleReadout(mod, false);
	}
	if (active) {
	  PixModule* pixmod = m_modGroup.module(mod);
	  int il1 = ((ConfInt&)pixmod->config()["general"]["OutputLink1"]).getValue();
	  il1 = (il1/16)*12+(((m_fmtLinkMap[il1/16]>>(16+(il1%16)*4)))&0xf);
	  int il2 = ((ConfInt&)pixmod->config()["general"]["OutputLink2"]).getValue();
	  il2 = (il2/16)*12+(((m_fmtLinkMap[il2/16]>>(16+(il2%16)*4)))&0xf);
	  int il3 = ((ConfInt&)pixmod->config()["general"]["OutputLink3"]).getValue();
	  il3 = (il3/16)*12+(((m_fmtLinkMap[il3/16]>>(16+(il3%16)*4)))&0xf);
	  int il4 = ((ConfInt&)pixmod->config()["general"]["OutputLink4"]).getValue();
	  il4 = (il4/16)*12+(((m_fmtLinkMap[il4/16]>>(16+(il4%16)*4)))&0xf);
	  if (scn->getMccBandwidth() == PixScan::SINGLE_40 || scn->getRunType() == PixScan::RAW_PATTERN) {
	    m_inmLinkTable[il1] = 100;
	    m_inmModuleTable[il1] = mod;
	    pixmod->pixMCC()->writeRegister("CSR_OutputMode", 0);
	  } else if (scn->getMccBandwidth() == PixScan::DOUBLE_80) {
	    m_inmLinkTable[il1] = il2;
	    m_inmLinkTable[il2] = -il3;
	    m_inmLinkTable[il3] = -il4;
	    m_inmLinkTable[il4] = -100;
	    m_inmModuleTable[il1] = mod;
	    m_inmModuleTable[il2] = mod;
	    m_inmModuleTable[il3] = mod;
	    m_inmModuleTable[il4] = mod;
	    pixmod->pixMCC()->writeRegister("CSR_OutputMode", 3);
	  } else {
	    m_inmLinkTable[il1] = il2;
	    m_inmLinkTable[il2] = -100;
	    m_inmModuleTable[il1] = mod;
	    m_inmModuleTable[il2] = mod;
	    if (scn->getMccBandwidth() == PixScan::DOUBLE_40) {
	      pixmod->pixMCC()->writeRegister("CSR_OutputMode", 1);
	    } else {
	      pixmod->pixMCC()->writeRegister("CSR_OutputMode", 2);
	    }
	  }
	  if (first) {
	    if (scn->getRunType() == PixScan::RAW_EVENT) {
	      pixmod->pixMCC()->prepareTestEvent(scn->getPatternSeeds(), m_prepareSeq, m_executeSeq, m_expectedOut);
	    } else {
	      pixmod->pixMCC()->prepareTestPattern(scn->getPatternSeeds(), m_prepareSeq, m_executeSeq, m_expectedOut);
	    }
	    first = false;
	  }
	  if (scn->getHistogramFilled(PixScan::RAW_DATA_REF) && scn->getHistogramKept(PixScan::RAW_DATA_REF)) {
	    Histo *hRef;
	    hRef = new Histo("RAW_DATA_REF", "Reference raw pattern", 
			     (int)(m_expectedOut.size()*1.1), -0.5, (m_expectedOut.size()*1.1)-0.5, 1, -0.5, 0.5);
	    scn->addHisto(*hRef, PixScan::RAW_DATA_REF, mod, -1, -1, -1);
	    for (int ib=0; ib<m_expectedOut.size(); ib++) {
	      if (m_expectedOut[ib]) {
		hRef->set(ib, 0, 1);
	      }
	    }
	  }
	}
      }
    }
  }
}

void RodPixController::runTestPattern(PixScan *scn, std::vector< Histo * > &vh) {
  unsigned int outSize, fifoSize;
  bool patch2ndLink = false;
  outSize = (unsigned int)(m_expectedOut.size()*1.1);
  if (scn->getMccBandwidth() == PixScan::SINGLE_40 || scn->getRunType() == PixScan::RAW_PATTERN) {
    fifoSize = (unsigned int)(outSize*2);
  } else if (scn->getMccBandwidth() == PixScan::DOUBLE_40) {
    fifoSize = (unsigned int)(outSize);
  } else if (scn->getMccBandwidth() == PixScan::SINGLE_80) {
    fifoSize = (unsigned int)(outSize);
    patch2ndLink = true;
  } else {
    fifoSize = (unsigned int)(outSize*0.5);
    patch2ndLink = true;
  }

  while (vh.size() < 32) vh.push_back((Histo *)NULL);
  for (int mod=0; mod<32; mod++) {
    vh[mod] = (Histo*)NULL;
    if ((m_modMask & (0x1<<mod)) != 0) {
      std::ostringstream nam, tit;
      nam << "RAW_" << mod << "_" << scn->scanIndex(0) << "_" << scn->scanIndex(1) << 
                              "_" << scn->scanIndex(2);
      tit << "Raw data mod " << mod << " L0=" << scn->scanIndex(0) << " L1=" << scn->scanIndex(1) << 
                                       " L2=" << scn->scanIndex(2);
      vh[mod] = new Histo(nam.str(), tit.str(), 4, outSize, -0.5, outSize-0.5, 2, -0.5, 1.5);
    }
  }
  for (int ir = 0; ir < scn->getRepetitions(); ir++) {
    sendCommand(m_prepareSeq, m_modMask);
    setCalibrationDebugMode();
    sendCommand(m_executeSeq, m_modMask);
    downloadFifo(fifoSize);
    unsigned int ic, il;
    bool oneFound[96];
    int idx[96];
    for (il=0; il<96; il++) {
      oneFound[il] = false;
      idx[il] = 0;
    }
    bool bit;
    for (ic=0; ic<m_fifoA.size(); ic++) {
      for (il=0 ; il<96; il++) {
	    if (m_inmLinkTable[il] > 0) {
	      if (getFifoBit(ic,il)) oneFound[il] = true;
	      if (oneFound[il]) {
	        unsigned int cl = il;
	        int mod = m_inmModuleTable[il];
	        while (1) {
	          // Fill histogram
	          bit = getFifoBit(ic,cl);
              int pos = idx[il];
              //if (pos%2 == 1 && pos > 2 && patch2ndLink) pos = pos-2;
	          if (bit) vh[mod]->set(pos+1, 0, (*vh[mod])(pos+1, 0)+1.0);
	          if (pos < m_expectedOut.size()) {
		        if (bit != m_expectedOut[pos]) {
		          vh[mod]->set(pos+1, 1, (*vh[mod])(pos+1, 1)+1.0);
		          vh[mod]->set(0, 1, (*vh[mod])(0, 1)+1.0);
				}
			  }
	          idx[il]++;
	          if (abs(m_inmLinkTable[cl]) == 100) break;
	          cl = abs(m_inmLinkTable[cl]);
			}
		  }
		}
      }
    }
    for (il=0 ; il<96; il++) {
	  if (m_inmLinkTable[il] > 0) {
	    int mod = m_inmModuleTable[il];
		int ip;
        for (ip = idx[il]; ip < m_expectedOut.size(); ip++) {
          if (m_expectedOut[ip]) {
	        vh[mod]->set(ip+1, 1, (*vh[mod])(ip+1, 1)+1.0);
	        vh[mod]->set(0, 1, (*vh[mod])(0, 1)+1.0);
		  }
		}
      }
    }
  }
}

bool RodPixController::primitiveExists(std::string str) {   //! Ask if a given primitive is implemented
  return false;
}

void RodPixController::rodConfigure() {
#ifndef NOTDAQ
  // Build module masks form config structure
  ModuleMaskIn modMask;
  unsigned int i,mod;
  for (i=0; i<sizeof(ModuleMaskIn)/4; i++) ((long int *)&modMask)[i] = 0;
  modMask.module = 0;
  modMask.useStructSet = 1;
  RodPrimitive* mmaskPrim = new RodPrimitive(4+sizeof(ModuleMaskIn)/4, 0, MODULE_MASK, R_MODULE_MASK, (long int *)&modMask);
  executeMasterPrimitiveSync(*mmaskPrim);
  delete mmaskPrim;
  for (mod=0; mod<32; mod++) {
    if (m_modPosition[mod] >= 0) {
      if (!m_modActive[m_modPosition[mod]]) {
	for (i=0; i<sizeof(ModuleMaskIn)/4; i++) ((long int *)&modMask)[i] = 0;
	modMask.module = mod;
	modMask.port = SP_BOTH;
	modMask.cfg = 1;
	modMask.modMask[0] = (0x1 << mod);
	modMask.modMask[1] = 0;
        if (m_modTrigger[mod]) {
	  modMask.maskType = (DATA_LINK_PLAY) +(COMMAND_LINK_ON) +(LINK_CFG_OFF);
	} else {
	  modMask.maskType = (DATA_LINK_PLAY) +(COMMAND_LINK_OFF) +(LINK_CFG_OFF);
	}
	modMask.storage = UPDATE_MASK;
	modMask.maskSet = MASK_SET_INIT;
	RodPrimitive* mmaskPrim = new RodPrimitive(4+sizeof(ModuleMaskIn)/4, 0, MODULE_MASK, R_MODULE_MASK, (long int *)&modMask);
	executeMasterPrimitiveSync(*mmaskPrim);
	delete mmaskPrim;
      }
    }
  }
  for (i=0; i<sizeof(ModuleMaskIn)/4; i++) ((long int *)&modMask)[i] = 0;
  modMask.passOn = 1;
  modMask.slvBits = 0xf;
  mmaskPrim = new RodPrimitive(4+sizeof(ModuleMaskIn)/4, 0, MODULE_MASK, R_MODULE_MASK, (long int *)&modMask);
  executeMasterPrimitiveSync(*mmaskPrim);
  delete mmaskPrim;
#endif
}

void RodPixController::setConfigurationDebugMode() {
#ifndef NOTDAQ
  if (m_mode != "CONFIGURATION_DBG") {
    unsigned int i;
    for (i=0; i<4; i++) {
      // Disable histogramming
      stopHistogramming(i);
      // Disable trapping
      stopTrapping(i);
    }
    // Configure links and masks
    rodConfigure();
    // Reset FIFOs
    writeRegister(RRIF_CMND_0, 0, 8, 0xff);     
    writeRegister(RRIF_CMND_0, 0, 8, 0x78);     
    // Create and Send a SET_ROD_MODE primitive to a master DSP
    SetRodModeIn SetRodMode_In;
    SetRodMode_In.mode    = CALIBRATION_MODE+CALIBRATION_SLINK_OVERRIDE_MODE+INMEM_EVT_CAPTURE_MODE;
    SetRodMode_In.flag    = SET_MODE;
    SetRodMode_In.dataLen = 0;
    SetRodMode_In.data    = 0;

    RodPrimitive* rodmPrim = new RodPrimitive(8, 0, SET_ROD_MODE, R_SET_ROD_MODE, (long int*)&SetRodMode_In);
    executeMasterPrimitiveSync(*rodmPrim);
    delete rodmPrim;
    // Disable calibration command decoding
    writeRegister(RRIF_CMND_1, 18, 1, 0x0);
    // Turn S-Link off
    writeRegister(RTR_CMND_STAT, 7, 1, 0x1);
    // Dump mster reg 0 and 1
    std::cout << "RRIF_CMND_1 = " << std::hex << readRegister(RRIF_CMND_1, 0, 32) << std::dec << std::endl;      
    std::cout << "RRIF_CMND_0 = " << std::hex << readRegister(RRIF_CMND_0, 0, 32) << std::dec << std::endl;      
    m_mode = "CONFIGURATION_DBG";
  }
#endif
}

void RodPixController::setCalibrationDebugMode() { 
#ifndef NOTDAQ
  if (m_mode != "CALIBRATION_DBG") {
    // Configure links and masks
    rodConfigure();
    // Reset FIFOs
    writeRegister(RRIF_CMND_0, 0, 8, 0xff);     
    writeRegister(RRIF_CMND_0, 0, 8, 0x78);     
    // Create and Send a SET_ROD_MODE primitive to a master DSP
    SetRodModeIn SetRodMode_In;
    SetRodMode_In.mode    = CALIBRATION_MODE+CALIBRATION_SLINK_OVERRIDE_MODE+INMEM_EVT_CAPTURE_MODE;
    SetRodMode_In.flag    = SET_MODE;
    SetRodMode_In.dataLen = 0;
    SetRodMode_In.data    = 0;

    RodPrimitive* rodmPrim = new RodPrimitive(8, 0, SET_ROD_MODE, R_SET_ROD_MODE, (long int*)&SetRodMode_In);
    executeMasterPrimitiveSync(*rodmPrim);
    delete rodmPrim;
    // Disable the static LV1 bit
    writeRegister(RRIF_CMND_1, 21, 2, 0x0);
    // Dump mster reg 0 and 1
    std::cout << "RRIF_CMND_1 = " << std::hex << readRegister(RRIF_CMND_1, 0, 32) << std::dec << std::endl;      
    std::cout << "RRIF_CMND_0 = " << std::hex << readRegister(RRIF_CMND_0, 0, 32) << std::dec << std::endl;      
    m_mode = "CALIBRATION_DBG";
  }
#endif
}

void RodPixController::setupTrapping(int slave, PixScan & scn) {
#ifndef NOTDAQ
  // Build module masks form config structure
  EventTrapSetupIn trpSetup;
  for (unsigned int i=0; i<sizeof(EventTrapSetupIn)/4; i++) ((long int *)&trpSetup)[i] = 0;
  trpSetup.slvBits = 0x1<<slave;
  trpSetup.numberOfEvents     = COLLECT_FOREVER;
  trpSetup.timeoutInUsec      = 0x20000;
  trpSetup.extRouterSetup     = 0;
  trpSetup.distribute         = TRAP_DISTRIB_NEITHER;
  trpSetup.releaseFrames      = 1;
  trpSetup.permitBackPressure = 1;
  trpSetup.dataMode           = 0;
  trpSetup.sLink              = 0;
  trpSetup.format             = 0;
  trpSetup.trapStray          = 0;
  trpSetup.iterLimit          = 0;
  trpSetup.trapConfig[0]        = TRAP_CFG_ROD_EVT;
  trpSetup.trapExclusionFlag[0] = 0;
  //AKDEBUG  trpSetup.trapFunction[0]      = TRAP_FXN_HISTOGRAM;
  trpSetup.trapFunction[0]      = TRAP_FXN_HISTOGRAM + TRAP_FXN_ERRORCNT;
  trpSetup.trapMatch[0]         = slave;
  trpSetup.trapModulus[0]       = 1;
  trpSetup.trapRemainder[0]     = 0;
  trpSetup.trapConfig[1]        = TRAP_CFG_IDLE;
  trpSetup.trapExclusionFlag[1] = 1;
  trpSetup.trapFunction[1]      = 0;
  trpSetup.trapMatch[1]         = slave;
  trpSetup.trapModulus[1]       = 0;
  trpSetup.trapRemainder[1]     = 0;
  RodPrimitive* strpPrim = new RodPrimitive(4+sizeof(EventTrapSetupIn)/4, 0, EVENT_TRAP_SETUP, R_EVENT_TRAP_SETUP, (long int *)&trpSetup);
  executeMasterPrimitiveSync(*strpPrim);
  delete strpPrim;
#endif
}

void RodPixController::setupTrapping(int slave, PixScanConfig & cfg) {
#ifndef NOTDAQ
  // Build module masks form config structure
  EventTrapSetupIn trpSetup;
  for (unsigned int i=0; i<sizeof(EventTrapSetupIn)/4; i++) ((long int *)&trpSetup)[i] = 0;
  trpSetup.slvBits = 0x1<<slave;
  trpSetup.numberOfEvents     = COLLECT_FOREVER;
  trpSetup.timeoutInUsec      = 0x20000;
  trpSetup.extRouterSetup     = 0;
  trpSetup.distribute         = TRAP_DISTRIB_NEITHER;
  trpSetup.releaseFrames      = 1;
  trpSetup.permitBackPressure = 1;
  trpSetup.dataMode           = 0;
  trpSetup.sLink              = 0;
  trpSetup.format             = 0;
  trpSetup.trapStray          = 0;
  trpSetup.iterLimit          = 0;
  trpSetup.trapConfig[0]        = TRAP_CFG_ROD_EVT;
  trpSetup.trapExclusionFlag[0] = 0;
  //AKDEBUG  trpSetup.trapFunction[0]      = TRAP_FXN_HISTOGRAM;
  trpSetup.trapFunction[0]      = TRAP_FXN_HISTOGRAM + TRAP_FXN_ERRORCNT;
  trpSetup.trapMatch[0]         = slave;
  trpSetup.trapModulus[0]       = 1;
  trpSetup.trapRemainder[0]     = 0;
  trpSetup.trapConfig[1]        = TRAP_CFG_IDLE;
  trpSetup.trapExclusionFlag[1] = 1;
  trpSetup.trapFunction[1]      = 0;
  trpSetup.trapMatch[1]         = slave;
  trpSetup.trapModulus[1]       = 0;
  trpSetup.trapRemainder[1]     = 0;
  RodPrimitive* strpPrim = new RodPrimitive(4+sizeof(EventTrapSetupIn)/4, 0, EVENT_TRAP_SETUP, R_EVENT_TRAP_SETUP, (long int *)&trpSetup);
  executeMasterPrimitiveSync(*strpPrim);
  delete strpPrim;
#endif
}

void RodPixController::setupTrapping(int slave) {
#ifndef NOTDAQ
  // Build module masks form config structure
  EventTrapSetupIn trpSetup;
  for (unsigned int i=0; i<sizeof(EventTrapSetupIn)/4; i++) ((long int *)&trpSetup)[i] = 0;
  trpSetup.slvBits = 0x1<<slave;
  trpSetup.numberOfEvents     = COLLECT_FOREVER;
  trpSetup.timeoutInUsec      = 0x20000;
  trpSetup.extRouterSetup     = 0;
  trpSetup.distribute         = TRAP_DISTRIB_NEITHER;
  trpSetup.releaseFrames      = 1;
  trpSetup.permitBackPressure = 0;
  trpSetup.dataMode           = 0;
  trpSetup.sLink              = 1;
  trpSetup.format             = 0;
  trpSetup.trapStray          = 0;
  trpSetup.iterLimit          = 0;
  trpSetup.trapConfig[0]        = TRAP_CFG_SLINK_EVT;
  trpSetup.trapExclusionFlag[0] = 0;
  //AKDEBUG  trpSetup.trapFunction[0]      = TRAP_FXN_HISTOGRAM;
  trpSetup.trapFunction[0]      = TRAP_FXN_HISTOGRAM + TRAP_FXN_ERRORCNT;
  trpSetup.trapMatch[0]         = slave;
  trpSetup.trapModulus[0]       = 1;
  trpSetup.trapRemainder[0]     = 0;
  trpSetup.trapConfig[1]        = TRAP_CFG_IDLE;
  trpSetup.trapExclusionFlag[1] = 1;
  trpSetup.trapFunction[1]      = 0;
  trpSetup.trapMatch[1]         = slave;
  trpSetup.trapModulus[1]       = 0;
  trpSetup.trapRemainder[1]     = 0;
  RodPrimitive* strpPrim = new RodPrimitive(4+sizeof(EventTrapSetupIn)/4, 0, EVENT_TRAP_SETUP, R_EVENT_TRAP_SETUP, (long int *)&trpSetup);
  executeMasterPrimitiveSync(*strpPrim);
  delete strpPrim;
#endif
}


void RodPixController::setupTrapping(int slave, PixRunConfig & cfg) {
}

void RodPixController::startTrapping(int slave) {
#ifndef NOTDAQ
  if (!m_trapRunning[slave]) {
    // Start trapping
    RodPrimitive *trpPrim = new RodPrimitive(4, 0, START_EVENT_TRAPPING, R_START_EVENT_TRAPPING, 0);
    executeSlavePrimitiveSync(*trpPrim, slave);
    delete trpPrim;
    m_trapRunning[slave] = true;
  }
#endif
}

void RodPixController::stopTrapping(int slave) {
#ifndef NOTDAQ
  if (m_trapRunning[slave]) {
    RodPrimitive *trpPrim = new RodPrimitive(4, 0, STOP_EVENT_TRAPPING, R_STOP_EVENT_TRAPPING, 0);
    executeSlavePrimitiveSync(*trpPrim, slave);
    delete trpPrim;
    m_trapRunning[slave] = false;
  }
#endif
}

void RodPixController::setupHistogramming(int slave, PixScan &scn) {
#ifndef NOTDAQ

  long int* hsData;
  int length;
  if (scn.getLoopActive(0) && scn.getDspProcessing(0)) {
    hsData = new long int[sizeof(HistogramSetupIn)/4+scn.getLoopVarNSteps(0)];
    length = 4+sizeof(HistogramSetupIn)/4+scn.getLoopVarNSteps(0);
  } else {
    hsData = new long int[sizeof(HistogramSetupIn)/4];
    length = 4+sizeof(HistogramSetupIn)/4;
  }

  // Configure the histograming task
  HistogramSetupIn *histoSetup = (HistogramSetupIn*)&(hsData[0]);
  for (unsigned int i=0; i<sizeof(HistogramSetupIn)/4; i++) ((long int *)histoSetup)[i] = 0;
  histoSetup->base = (UINT32 *)DEFAULT;
  if (scn.getLoopActive(0) && scn.getDspProcessing(0)) {
    if (scn.getLoopActive(1) && scn.getDspProcessing(1)) {
      histoSetup->nBins = scn.getLoopVarNSteps(0)*scn.getLoopVarNSteps(1); // ask for second loop
    } else {
      histoSetup->nBins = scn.getLoopVarNSteps(0);
    }
  } else {
    histoSetup->nBins = 1; // ask
  }
  histoSetup->routineType = scn.getDspHistogrammingCode();
  histoSetup->dataType[0] = 0x2a;
  histoSetup->dataType[1] = 0x2a;
  histoSetup->binSize = scn.getDspHistoDepth();
  histoSetup->opt = 0;
  if (scn.getHistogramFilled(PixScan::OCCUPANCY)) histoSetup->opt |= (1 << HIST_OCC);
  if (scn.getHistogramFilled(PixScan::LVL1)) {
    histoSetup->opt |= (1 << HIST_TIMESLICE);
    if (scn.getLvl1HistoBinned()) {
      histoSetup->opt |= (1 << HIST_TSBINNED);
    }
  }
  if (scn.getHistogramFilled(PixScan::HITOCC)) histoSetup->opt |= (1 << HIST_HITOCC);
  if (scn.getHistogramFilled(PixScan::TOTAVERAGE)) histoSetup->opt |= (1 << HIST_TOTAVERAGE);
  if (scn.getHistogramFilled(PixScan::TOT_MEAN)) histoSetup->opt |= (1 << HIST_TOTMEAN);
  if (scn.getHistogramFilled(PixScan::TOT_SIGMA)) histoSetup->opt |= (1 << HIST_TOTSIGMA);
  if (scn.getHistogramFilled(PixScan::TOT)) {
    histoSetup->opt |= (1 << HIST_TOTSPEC);
    if (scn.getHistoTotMin() != 0 || scn.getHistoTotMax() != 0) {
      histoSetup->opt |= (1 << HIST_TOTUSERRANGE);
    }
  }
  if (scn.getDspHistogrammingCode() == PixScan::ASM) {
    histoSetup->opt |= (1 << HIST_AUTOCACHE);
  }
  histoSetup->eventsPerL1A = scn.getConsecutiveLvl1TrigA(0);
  histoSetup->validModules = m_dspModMask[slave];
  histoSetup->totResolution = scn.getHistoTotNbin();
  histoSetup->totMin = scn.getHistoTotMax();
  histoSetup->totMax = scn.getHistoTotMax();

  if (scn.getMaskStageMode() == PixScan::STATIC) {
    // ask
  } else {
    if (scn.getMaskStageTotalSteps() == PixScan::STEPS_32) {
      histoSetup->hashScheme = 32;   
    } else {
      //throw RodPixControllerExc(RodPixControllerExc::ILLEGAL_HASHING_SCHEME, PixControllerExc::FATAL,  
      //			getModGroup().getRodName(), m_rodSlot); 
      histoSetup->hashScheme = 32;   // To be checked !!!
    }
  }
  histoSetup->xPtr[0] = (UINT32 *)DEFAULT;
  histoSetup->xPtr[1] = (UINT32 *)DEFAULT;

  //setup Vcal bins on SDSP for fitting
  if (scn.getLoopActive(0) && scn.getDspProcessing(0)) {
    if (scn.getLoopVarUniform(0)) {
      float vmin = scn.getLoopVarMin(0);
      float vmax = scn.getLoopVarMax(0);
      int nsteps = scn.getLoopVarNSteps(0);
      for (int i=0; i<nsteps;i++){
	hsData[sizeof(HistogramSetupIn)/4 + i] = (int)(vmin+i*(vmax-vmin)/nsteps);
      }
    } else {
      throw RodPixControllerExc(RodPixControllerExc::NON_UNIFORM_POINTS_NOT_IMPLEMENTED, 
                                PixControllerExc::FATAL, getModGroup().getRodName(), m_rodSlot); 
    }
  }

  RodPrimitive* histoPrim = new RodPrimitive(length, 0, HISTOGRAM_SETUP, R_HISTOGRAM_SETUP, &(hsData[0]));

  std::vector<unsigned int> outs;
  executeSlavePrimitiveSync(*histoPrim, outs, slave);

  delete histoPrim;
  delete[] hsData;
#endif
}

void RodPixController::setupHistogramming(int slave, PixScanConfig &cfg) {
#ifndef NOTDAQ

  ScanControl &scanConfig = cfg.scanControl();
  long int* hsData = new long int[sizeof(HistogramSetupIn)/4+scanConfig.general.nBins[0]];

  // Configure the histograming task
  HistogramSetupIn *histoSetup = (HistogramSetupIn*)&(hsData[0]);
  for (unsigned int i=0; i<sizeof(HistogramSetupIn)/4; i++) ((long int *)histoSetup)[i] = 0;
  histoSetup->base = (UINT32 *)DEFAULT;
  //  histoSetup->nBins = 0xc9;
  histoSetup->nBins = scanConfig.general.nBins[0];
  histoSetup->routineType = cfg.histoRoutineType();
  histoSetup->dataType[0] = 0x2a;
  histoSetup->dataType[1] = 0x2a;;
  histoSetup->binSize = HISTOGRAM_16BIT;

  if (cfg.getHOccupancy()) histoSetup->opt |= (1 << HIST_OCC);
  if (cfg.getHTimeProfile()) histoSetup->opt |= (1 << HIST_TIMESLICE);
  if (cfg.getHTotMean()) histoSetup->opt |= (1 << HIST_TOTMEAN);
  if (cfg.getHTotSigma()) histoSetup->opt |= (1 << HIST_TOTSIGMA);
  if (cfg.getHTot()) {
    histoSetup->opt |= (1 << HIST_TOTSPEC);
    if (cfg.histoTotMin() != 0 || cfg.histoTotMax() != 0) {
      histoSetup->opt |= (1 << HIST_TOTUSERRANGE);
    }
  }
  if (cfg.histoRoutineType() == HISTO_ROUTINE_ASM) {
    histoSetup->opt |= (1 << HIST_AUTOCACHE);
  }

  histoSetup->eventsPerL1A = scanConfig.trigger.accepts;
  histoSetup->validModules = m_dspModMask[slave];
  histoSetup->totResolution = cfg.histoTotResolution();
  histoSetup->totMin = cfg.histoTotMin();
  histoSetup->totMax = cfg.histoTotMax();
  // 32 MASKSTAGES
  histoSetup->hashScheme = 32;
  histoSetup->xPtr[0] = (UINT32 *)DEFAULT;
  histoSetup->xPtr[1] = (UINT32 *)DEFAULT;

  //setup Vcal bins on SDSP for fitting
  for (unsigned int i=0; i<scanConfig.general.nBins[0];i++){
    hsData[sizeof(HistogramSetupIn)/4 + i] = scanConfig.general.scanStart[0]+i*(scanConfig.general.scanEnd[0]-scanConfig.general.scanStart[0]+1)/scanConfig.general.nBins[0];
  }

  int length = 4+sizeof(HistogramSetupIn)/4+scanConfig.general.nBins[0];
  RodPrimitive* histoPrim = new RodPrimitive(length, 0, HISTOGRAM_SETUP, R_HISTOGRAM_SETUP, &(hsData[0]));

  std::vector<unsigned int> outs;
  executeSlavePrimitiveSync(*histoPrim, outs, slave);

  delete histoPrim;
  delete[] hsData;
#endif
}

void RodPixController::setupHistogramming(int slave, PixRunConfig &cfg) {
}

void RodPixController::startHistogramming(int slave) {
#ifndef NOTDAQ
  if (!m_histoRunning[slave]) {
    StartTaskIn StartTask_In;
    StartTask_In.taskType       = HISTOGRAM_TASK;
    StartTask_In.taskRevision   = R_HISTOGRAM_TASK;
    StartTask_In.priority       = 1;
    StartTask_In.completionFlag = 1;
    StartTask_In.taskStruct.histogramTaskIn.nEvents     = COLLECT_FOREVER;
    StartTask_In.taskStruct.histogramTaskIn.controlFlag = MASTER_HREG;

    RodPrimitive *histoPrim = new RodPrimitive(4+sizeof(StartTask_In)/4, 0, START_TASK, R_START_TASK, (long int*)&StartTask_In);
    executeSlavePrimitiveSync(*histoPrim, slave);
    delete histoPrim;
    m_histoRunning[slave] = true;

    //AKDEBUG
    StartTaskIn StartTask2_In;
    StartTask2_In.taskType       = ERROR_TASK;
    StartTask2_In.taskRevision   = R_ERROR_TASK;
    StartTask2_In.priority       = 1;
    StartTask2_In.completionFlag = 1;

    RodPrimitive *errorPrim = new RodPrimitive(4+sizeof(StartTask_In)/4, 0, START_TASK, R_START_TASK, (long int*)&StartTask2_In);
    executeSlavePrimitiveSync(*errorPrim, slave);
    delete errorPrim;

  }
#endif
}

void RodPixController::stopHistogramming(int slave) {
#ifndef NOTDAQ
  if (m_histoRunning[slave]) {
    TaskOperationIn TaskOperation_In;
    TaskOperation_In.taskType      = HISTOGRAM_TASK;
    TaskOperation_In.taskOperation = TASK_STOP;
    TaskOperation_In.data          = 0;
    RodPrimitive *histoPrim = new RodPrimitive(4+sizeof(TaskOperationIn)/4, 0, TASK_OPERATION, 
					       R_TASK_OPERATION, (long int*)&TaskOperation_In);
    executeSlavePrimitiveSync(*histoPrim, slave);
    delete histoPrim;
    m_histoRunning[slave] = false;

    //AKDEBUG
    TaskOperationIn TaskOperationE_In;
    TaskOperationE_In.taskType      = ERROR_TASK;
    TaskOperationE_In.taskOperation = TASK_STOP;
    TaskOperationE_In.data          = 0;
    RodPrimitive *errorPrim = new RodPrimitive(4+sizeof(TaskOperationIn)/4, 0, TASK_OPERATION, 
					       R_TASK_OPERATION, (long int*)&TaskOperationE_In);
    executeSlavePrimitiveSync(*errorPrim, slave);
    delete errorPrim;

  }
#endif
}

void RodPixController::stopScan() {
#ifndef NOTDAQ

  TaskOperationIn TaskOperation_In;
  TaskOperation_In.taskType      = HISTOGRAM_CTRL_TASK;
  TaskOperation_In.taskOperation = TASK_STOP;
  TaskOperation_In.data          = 0;
  RodPrimitive *histoPrim = new RodPrimitive(4 + sizeof(TaskOperation_In)/4, 0, 
					     TASK_OPERATION, R_TASK_OPERATION, 
					     (long int*)&TaskOperation_In);
  executeMasterPrimitiveSync(*histoPrim);
  delete histoPrim;

  for(int slave=0;slave<4;slave++){
    stopHistogramming(slave);
    stopTrapping(slave);
  }
#endif
}

void RodPixController::setGroupId(int module, int group) {
#ifndef NOTDAQ
  unsigned int i,mod;
  if (module<0 || module>31) return;
  if (group<0 || group>7) return;
  if (m_modPosition[module] >= 0) {
    mod = m_modPosition[module];
    RwModuleVariableIn modvarData;
    for (i=0; i<sizeof(RwModuleVariableIn)/4; i++) ((long int *)&modvarData)[i] = 0;
    modvarData.fRead    = 0;
    modvarData.cfgSet   = PHYSICS_MODULE_CONFIG;
    modvarData.groupId  = m_modGrp[mod];
    modvarData.module   = m_modNum[mod];
    modvarData.chip     = 0;
    modvarData.varType  = MVAR_GROUP_ID;
    modvarData.info     = 0;
    modvarData.dataLen  = 4;
    modvarData.data     = (MDAT32 *)group;
    RodPrimitive *modvarPrim = new RodPrimitive(4+sizeof(RwModuleVariableIn)/4, 0, RW_MODULE_VARIABLE, R_RW_MODULE_VARIABLE, (long int *)&modvarData);
    executeMasterPrimitiveSync(*modvarPrim);
    delete modvarPrim;  
    modvarData.cfgSet   = SCAN_MODULE_CONFIG;
    modvarPrim = new RodPrimitive(4+sizeof(RwModuleVariableIn)/4, 0, RW_MODULE_VARIABLE, R_RW_MODULE_VARIABLE, (long int *)&modvarData);
    executeMasterPrimitiveSync(*modvarPrim);
    delete modvarPrim;  
    m_modGrp[mod] = group;
  }
#endif
}
 
void RodPixController::enableModuleReadout(int module) {
#ifndef NOTDAQ
  unsigned int i,mod;
  if (module<0 || module>31) return;
  if (m_modPosition[module] >= 0) {
    mod = m_modPosition[module];
    // Update module structure
    RwModuleVariableIn modvarData;
    for (i=0; i<sizeof(RwModuleVariableIn)/4; i++) ((long int *)&modvarData)[i] = 0;
    modvarData.fRead    = 0;
    modvarData.cfgSet   = PHYSICS_MODULE_CONFIG;
    modvarData.groupId  = m_modGrp[mod];
    modvarData.module   = m_modNum[mod];
    modvarData.chip     = 0;
    modvarData.varType  = MVAR_ACTIVE;
    modvarData.info     = 0;
    modvarData.dataLen  = 4;
    modvarData.data     = (MDAT32 *)0x1;
    RodPrimitive *modvarPrim = new RodPrimitive(4+sizeof(RwModuleVariableIn)/4, 0, RW_MODULE_VARIABLE, R_RW_MODULE_VARIABLE, (long int *)&modvarData);
    executeMasterPrimitiveSync(*modvarPrim);
    delete modvarPrim;  
    modvarData.cfgSet   = SCAN_MODULE_CONFIG;
    modvarPrim = new RodPrimitive(4+sizeof(RwModuleVariableIn)/4, 0, RW_MODULE_VARIABLE, R_RW_MODULE_VARIABLE, (long int *)&modvarData);
    executeMasterPrimitiveSync(*modvarPrim);
    delete modvarPrim;  
    m_modActive[mod]  = true;
    m_modTrigger[mod] = true;
  } 
#endif 
}

void RodPixController::disableModuleReadout(int module, bool trigger) {
#ifndef NOTDAQ
  unsigned int i,mod;
  if (module<0 || module>31) return;
  if (m_modPosition[module] >= 0) {
    mod = m_modPosition[module];
    // Update module structure
    RwModuleVariableIn modvarData;
    for (i=0; i<sizeof(RwModuleVariableIn)/4; i++) ((long int *)&modvarData)[i] = 0;
    modvarData.fRead    = 0;
    modvarData.cfgSet   = PHYSICS_MODULE_CONFIG;
    modvarData.groupId  = m_modGrp[mod];
    modvarData.module   = m_modNum[mod];
    modvarData.chip     = 0;
    modvarData.varType  = MVAR_ACTIVE;
    modvarData.info     = 0;
    modvarData.dataLen  = 4;
    modvarData.data     = (MDAT32 *)0x0;
    RodPrimitive *modvarPrim = new RodPrimitive(4+sizeof(RwModuleVariableIn)/4, 0, RW_MODULE_VARIABLE, R_RW_MODULE_VARIABLE, (long int *)&modvarData);
    executeMasterPrimitiveSync(*modvarPrim);
    delete modvarPrim;  
    modvarData.cfgSet   = SCAN_MODULE_CONFIG;
    modvarPrim = new RodPrimitive(4+sizeof(RwModuleVariableIn)/4, 0, RW_MODULE_VARIABLE, R_RW_MODULE_VARIABLE, (long int *)&modvarData);
    executeMasterPrimitiveSync(*modvarPrim);
    delete modvarPrim;  
    m_modActive[mod]  = false;
    m_modTrigger[mod] = trigger;
  }
#endif
}


void RodPixController::testGlobalRegister(int module, int frontend) {
#ifndef NOTDAQ

  TestGlobalRegIn TestGlobalRegIN;
  
  TestGlobalRegIN.structId = PHYSICS_MODULE_CONFIG;
  TestGlobalRegIN.moduleId = module;
  TestGlobalRegIN.FEIndex  = frontend; 
  
  RodPrimitive *tgrPrim = new RodPrimitive(4 + sizeof(TestGlobalRegIn)/4, 0, 
					   TEST_GLOBAL_REG, R_TEST_GLOBAL_REG,
					   (long int*)&TestGlobalRegIN);
  executeMasterPrimitiveSync(*tgrPrim);
  delete tgrPrim;
#endif
}

void RodPixController::ledFlash(int period, int repet, int led) {
#ifndef NOTDAQ
  // Create and Send a FLASH_LED primitive to a master DSP
  FlashLedIn FlashLed_In;
  FlashLed_In.ledNum   = led;
  FlashLed_In.period   = period;
  FlashLed_In.numTimes = repet;
  RodPrimitive* ledPrim = new RodPrimitive(7, 0, FLASH_LED, R_FLASH_LED, (long int*)&FlashLed_In);
  executeMasterPrimitiveSync(*ledPrim);
  delete ledPrim;
#endif
}

void RodPixController::ledFlashSlave(int period, int repet, int slave) {
#ifndef NOTDAQ
  // Create and Send a FLASH_LED primitive to a slave DSP
  FlashLedIn FlashLed_In;
  FlashLed_In.ledNum   = YELLOW_LED;
  FlashLed_In.period   = period;
  FlashLed_In.numTimes = repet;
  RodPrimitive* ledPrim = new RodPrimitive(7, 0, FLASH_LED, R_FLASH_LED, (long int*)&FlashLed_In);
  std::vector<unsigned int> out;
  executeSlavePrimitiveSync(*ledPrim, slave);
  delete ledPrim;
#endif
}

void RodPixController::echo(Bits in, Bits out, unsigned int ndat) {
#ifndef NOTDAQ
  long int *echoData = new long int[ndat];
  unsigned int i;
  for (i=0; i<ndat; i++) echoData[i] = 0xf0f00000 | i;
  RodPrimitive* echoPrim = new RodPrimitive(ndat+4, 0, ECHO, R_ECHO, echoData);
  std::vector<unsigned int> outs;
  executeMasterPrimitiveSync(*echoPrim, outs);
  delete echoPrim;
  delete echoData;
  for (i=0; i<outs.size(); i++) std::cout << "out[" << i << "] = " << std::hex << outs[i] << std::endl;
#endif 
}

void RodPixController::echoSlave(Bits in, Bits out, unsigned int ndat, int slave) {
#ifndef NOTDAQ
  long int *echoData = new long int[ndat];
  unsigned int i;
  for (i=0; i<ndat; i++) echoData[i] = 0xf0f00000 | i;
  RodPrimitive* echoPrim = new RodPrimitive(ndat+4, 0, ECHO, R_ECHO, echoData);
  std::vector<unsigned int> outs;
  executeSlavePrimitiveSync(*echoPrim, outs, slave);
  delete echoPrim;
  delete echoData;
  for (i=0; i<outs.size(); i++) std::cout << "out[" << i << "] = " << std::hex << outs[i] << std::endl; 
#endif
}

void RodPixController::readMasterMem(int startAddr, int nWords, std::vector<unsigned int> &out) {
#ifndef NOTDAQ
  unsigned long *buf=0;
  buf = new unsigned long[nWords];
  m_rod->mdspBlockRead(startAddr, buf, nWords); 
  for (int i=0; i<nWords; i++) out.push_back(buf[i]);
  delete[] buf;
#endif
}

void RodPixController::writeMasterMem(int startAddr, int nWords, std::vector<unsigned int> &in) {
#ifndef NOTDAQ
  unsigned long *buf=0;
  buf = new unsigned long[nWords];
  for (int i=0; i<nWords; i++) buf[i] = in[i];
  m_rod->mdspBlockWrite(startAddr, buf, nWords);
  delete[] buf;
#endif
}

void RodPixController::readSlaveMem(int slaveId, int startAddr, int nWords, std::vector<unsigned int> &out) {
#ifndef NOTDAQ
  RwSlaveMemoryIn RSlaveMemoryIn;
  RSlaveMemoryIn.slaveNumber   = slaveId;
  RSlaveMemoryIn.fRead         = 1;
  RSlaveMemoryIn.slaveAddress  = (UINT32*)startAddr;
  RSlaveMemoryIn.masterAddress = (UINT32*)DEFAULT ;
  RSlaveMemoryIn.numWords      = nWords;

  RodPrimitive* rwsmPrim = new RodPrimitive(9, 0, RW_SLAVE_MEMORY, R_RW_SLAVE_MEMORY, (long int*)&RSlaveMemoryIn);
  RodOutList* outList;
  executeMasterPrimitiveSync(*rwsmPrim, outList);
  delete rwsmPrim;
  if (outList == NULL) {
    return;
  } else {
    out.clear();
    unsigned long *outBody = outList->getBody();
    for (int i=8; i<outList->getLength()-2; i++) {
      out.push_back(outBody[i]);
    }
    m_rod->deleteOutList();
  }
#endif
}

void RodPixController::writeSlaveMem(int slaveId, int startAddr, int nWords, std::vector<unsigned int> &in) {
#ifndef NOTDAQ
  long int* rwsmData = new long int[nWords + sizeof(RwSlaveMemoryIn)/4];
  RwSlaveMemoryIn* WSlaveMemoryIn = (RwSlaveMemoryIn*)rwsmData;
  WSlaveMemoryIn->slaveNumber   = slaveId;
  WSlaveMemoryIn->fRead         = 0;
  WSlaveMemoryIn->slaveAddress  = (UINT32*)startAddr;
  WSlaveMemoryIn->masterAddress = (UINT32*)DEFAULT ;
  WSlaveMemoryIn->numWords      = nWords;

  int i;
  for (i=0; i<nWords; i++) rwsmData[5+i] = in[i];
  RodPrimitive* rwsmPrim = new RodPrimitive(9+nWords, 0, RW_SLAVE_MEMORY, R_RW_SLAVE_MEMORY, rwsmData);
  executeMasterPrimitiveSync(*rwsmPrim);
  delete rwsmPrim;
  delete[] rwsmData;
#endif
}

void RodPixController::readSlaveMemDirect(int slaveId, int startAddr, int nWords, std::vector<unsigned int> &out) {
#ifndef NOTDAQ
  unsigned int cr0 = 0;
  cr0 = m_rod->mdspSingleRead(COMMAND_REG_0);
  m_rod->mdspSingleWrite(COMMAND_REG_0, cr0+(0x1<<CR_DMA_ACCESS_REQ));
  unsigned int sr0=0, repCount = 10000;
  do {
    sr0 = m_rod->mdspSingleRead(STATUS_REG_0);
  } while ((sr0 & (0x1<<SR_DMA_ACCESS_ACK)) == 0 && repCount-- > 0);

  if (repCount > 0) {
    unsigned long *buf=0;
    buf = new unsigned long[nWords];
    m_rod->slvBlockRead(startAddr, buf, nWords, slaveId); 
    for (int i=0; i<nWords; i++) out.push_back(buf[i]);
    delete[] buf;
    m_rod->mdspSingleWrite(COMMAND_REG_0, cr0);
  } else {
    throw RodPixControllerExc(RodPixControllerExc::DMA_NOT_PERMITTED, PixControllerExc::FATAL,  
			      getModGroup().getRodName(), m_rodSlot); 
  }
#endif
}

void RodPixController::writeSlaveMemDirect(int slaveId, int startAddr, int nWords, std::vector<unsigned int> &in) {
#ifndef NOTDAQ
  unsigned int cr0 = 0;
  cr0 = m_rod->mdspSingleRead(COMMAND_REG_0);
  m_rod->mdspSingleWrite(COMMAND_REG_0, cr0+CR_DMA_ACCESS_REQ);
  unsigned int sr0=0, repCount = 10000;
  do {
    sr0 = m_rod->mdspSingleRead(STATUS_REG_0);
  } while ((sr0 & (0x1<<SR_DMA_ACCESS_ACK)) == 0 && repCount-- > 0);
  if (repCount > 0) {
    unsigned long *buf=0;
    buf = new unsigned long[nWords];
    for (int i=0; i<nWords; i++) buf[i] = in[i];
    m_rod->slvBlockWrite(startAddr, buf, nWords, slaveId);
    delete[] buf;
  } else {
    throw RodPixControllerExc(RodPixControllerExc::DMA_NOT_PERMITTED, PixControllerExc::FATAL,  
			      getModGroup().getRodName(), m_rodSlot); 
  }
#endif
}

void RodPixController::readFifo(int fifo, int bank, int nWords, std::vector<unsigned int> &out) {
#ifndef NOTDAQ
  RwFifoIn RwFifo_In;
  RwFifo_In.fifoId      = fifo;
  RwFifo_In.bank        = bank;
  RwFifo_In.fRead       = 1;
  RwFifo_In.numElements = nWords;
  RwFifo_In.dataBaseAdr = (UINT32 *)DEFAULT;
  RodPrimitive* rwfPrim = new RodPrimitive(9, 0, RW_FIFO, R_RW_FIFO, (long int*)&RwFifo_In);
  RodOutList* outList;
  executeMasterPrimitiveSync(*rwfPrim, outList);
  delete rwfPrim;
  out.clear();
  unsigned long *outBody = outList->getBody();
  for (int i=9; i<outList->getLength()-10; i++) {
    out.push_back(outBody[i]);
  }
  m_rod->deleteOutList();
#endif
}

void RodPixController::writeFifo(int fifo, int bank, int nWords, std::vector<unsigned int> &in) {
#ifndef NOTDAQ
  long int* rwfData = new long int[5+nWords];
  rwfData[0] = fifo;
  rwfData[1] = bank;
  rwfData[2] = 0;
  rwfData[3] = nWords;
  rwfData[4] = DEFAULT;
  int i;
  for (i=0; i<nWords; i++) rwfData[5+i] = in[i];
  RodPrimitive* rwfPrim = new RodPrimitive(9+nWords, 0, RW_FIFO, R_RW_FIFO, rwfData);
  executeMasterPrimitiveSync(*rwfPrim);
  delete rwfPrim;
  delete[] rwfData;
#endif
}

void RodPixController::writeRegister(int regId, int offset, int size, unsigned int value) {
#ifndef NOTDAQ
  RwRegFieldIn 	RwRegField_In;
  RwRegField_In.registerID = regId;
  RwRegField_In.offset     = offset;
  RwRegField_In.width      = size;
  RwRegField_In.fRead      = 0;
  RwRegField_In.dataIn     = value;
  RodPrimitive* rwrPrim = new RodPrimitive(9, 0, RW_REG_FIELD, R_RW_REG_FIELD, (long int*)&RwRegField_In);
  executeMasterPrimitiveSync(*rwrPrim);
  delete rwrPrim;
#endif
}

unsigned int RodPixController::readRegister(int regId, int offset, int size) {
#ifndef NOTDAQ
  RwRegFieldIn 	RwRegField_In;
  RwRegField_In.registerID = regId;
  RwRegField_In.offset     = offset;
  RwRegField_In.width      = size;
  RwRegField_In.fRead      = 1;
  RwRegField_In.dataIn     = 0;
  RodPrimitive* rwrPrim = new RodPrimitive(9, 0, RW_REG_FIELD, R_RW_REG_FIELD, (long int*)&RwRegField_In);
  std::vector<unsigned int> outs;
  executeMasterPrimitiveSync(*rwrPrim, outs);
  delete rwrPrim;
  return outs[sizeof(ListHeader)/4+4];
#else
  return 0;
#endif

}

#ifndef NOTDAQ
void RodPixController::executeMasterPrimitive(RodPrimitive &prim) {
  RodPrimList primList(1);
  primList.insert(primList.begin(), prim);
  primList.bufferBuild();
  m_rod->sendPrimList(&primList);
}

void RodPixController::executeMasterPrimitiveSync(RodPrimitive &prim, std::vector<unsigned int> &out) {
  executeMasterPrimitive(prim);
  waitPrimitiveCompletion(out);
}

void RodPixController::executeMasterPrimitiveSync(RodPrimitive &prim, RodOutList* &out) {
  executeMasterPrimitive(prim);
  waitPrimitiveCompletion(out);
}

void RodPixController::executeMasterPrimitiveSync(RodPrimitive &prim) {
  executeMasterPrimitive(prim);
  waitPrimitiveCompletion();
}

void RodPixController::executeSlavePrimitive(RodPrimitive &prim, int slave) {
  int i;

  RodPrimList slvPrimList(1);
  slvPrimList.insert(slvPrimList.begin(), prim);
  slvPrimList.bufferBuild();
  
  long buffLength = slvPrimList.getBufferLength();
  unsigned long* slavePrimList = new unsigned long[buffLength];
  unsigned long* bufferStart = slvPrimList.getBuffer();
  for (i=0; i< buffLength; i++) {
    slavePrimList[i] = bufferStart[i];
  }
  
  // Create Send Slave List primitive
  RodPrimList primList(2);
  long int *slaveData = new long int[4+buffLength]; 
  SendSlaveListIn* SendSlaveList_In = (SendSlaveListIn*)slaveData;
  SendSlaveList_In->slaveNumber   = slave;
  SendSlaveList_In->listLength    = buffLength;
  SendSlaveList_In->slavePrimList = (UINT32 *)DEFAULT;
  SendSlaveList_In->slaveRepData  = (UINT32 *)DEFAULT;

  for (i=0; i< buffLength; i++) {
    slaveData[i+sizeof(SendSlaveListIn)/4] = slavePrimList[i];
  }
  delete slavePrimList;
  RodPrimitive* send;
  send = new RodPrimitive(8+buffLength, 0, SEND_SLAVE_LIST, R_SEND_SLAVE_LIST, slaveData);
  primList.insert(primList.begin(), *send);

  // Create Send Slave List primitive 
  StartSlaveListIn StartSlaveList_In;
  StartSlaveList_In.slaveNumber     = slave;
  StartSlaveList_In.pauseMasterList = 0;
  StartSlaveList_In.getSlaveReply   = 1;
  RodPrimitive* start;
  start = new RodPrimitive(7, 0, START_SLAVE_LIST, R_START_SLAVE_LIST, (long int*)&StartSlaveList_In);
  primList.insert(primList.end(), *start);
  primList.bufferBuild();
  m_rod->sendPrimList(&primList);
}

void RodPixController::executeSlavePrimitiveOnAll(RodPrimitive &prim) {
  int i, slave;

  RodPrimList slvPrimList(1);
  slvPrimList.insert(slvPrimList.begin(), prim);
  slvPrimList.bufferBuild();
  
  long buffLength = slvPrimList.getBufferLength();
  unsigned long* slavePrimList = new unsigned long[buffLength];
  unsigned long* bufferStart = slvPrimList.getBuffer();
  for (i=0; i< buffLength; i++) {
    slavePrimList[i] = bufferStart[i];
  }
  
  // Create Send Slave List primitive
  RodPrimList primList(2*m_rod->getNumSlaves());

  for(slave=0;slave<m_rod->getNumSlaves();slave++){
    long int *slaveData = new long int[4+buffLength]; 
    SendSlaveListIn* SendSlaveList_In = (SendSlaveListIn*)slaveData;
    SendSlaveList_In->slaveNumber   = slave;
    SendSlaveList_In->listLength    = buffLength;
    SendSlaveList_In->slavePrimList = (UINT32 *)DEFAULT;
    SendSlaveList_In->slaveRepData  = (UINT32 *)DEFAULT;

    for (i=0; i< buffLength; i++) {
      slaveData[i+sizeof(SendSlaveListIn)/4] = slavePrimList[i];
    }
    RodPrimitive* send;
    send = new RodPrimitive(8+buffLength, 0, SEND_SLAVE_LIST, R_SEND_SLAVE_LIST, slaveData);
    primList.insert(primList.begin(), *send);

    // Create Send Slave List primitive 
    long int *slaveStart = new long int[sizeof(StartSlaveListIn)/4];
    StartSlaveListIn* StartSlaveList_In = (StartSlaveListIn*) slaveStart;
    StartSlaveList_In->slaveNumber     = slave;
    StartSlaveList_In->pauseMasterList = 0;
    StartSlaveList_In->getSlaveReply   = 1;
    RodPrimitive* start;
    start = new RodPrimitive(4 + sizeof(StartSlaveListIn)/4, 0, START_SLAVE_LIST, R_START_SLAVE_LIST, slaveStart);
    primList.insert(primList.end(), *start);
    primList.bufferBuild();
  }
  delete slavePrimList;
  m_rod->sendPrimList(&primList);
}

void RodPixController::executeSlavePrimitiveOnAllSync(RodPrimitive &prim) {
  executeSlavePrimitiveOnAll(prim);
  waitPrimitiveCompletion();
}

void RodPixController::executeSlavePrimitiveOnAllSync(RodPrimitive &prim, std::vector<unsigned int> &out) {
  executeSlavePrimitiveOnAll(prim);
  waitPrimitiveCompletion(out);
}

void RodPixController::executeSlavePrimitiveOnAllSync(RodPrimitive &prim, RodOutList* &out) {
  executeSlavePrimitiveOnAll(prim);
  waitPrimitiveCompletion(out);
}

void RodPixController::executeSlavePrimitiveSync(RodPrimitive &prim, std::vector<unsigned int> &out, int slave) {
  executeSlavePrimitive(prim, slave);
  waitPrimitiveCompletion(out);
}

void RodPixController::executeSlavePrimitiveSync(RodPrimitive &prim, RodOutList* &out, int slave) {
  executeSlavePrimitive(prim, slave);
  waitPrimitiveCompletion(out);
}

void RodPixController::executeSlavePrimitiveSync(RodPrimitive &prim, int slave) {
  executeSlavePrimitive(prim, slave);
  waitPrimitiveCompletion();
}

void RodPixController::waitPrimitiveCompletion() {
  PrimState returnPState;
  do {
    returnPState = m_rod->primHandler();
  } while (returnPState != PRIM_EXECUTING); 
  usleep(10000);
  do {
    returnPState = m_rod->primHandler();
  } while ((returnPState != PRIM_WAITING)&&(returnPState != PRIM_IDLE)); 
  if (returnPState == PRIM_WAITING) {
    m_rod->getOutList();
    m_rod->deleteOutList();
  }
}

void RodPixController::waitPrimitiveCompletion(std::vector<unsigned int> &out) {
  PrimState returnPState;
  do {
    returnPState = m_rod->primHandler();
  } while (returnPState != PRIM_EXECUTING); 
  do {
    returnPState = m_rod->primHandler();
  } while ((returnPState != PRIM_WAITING)&&(returnPState != PRIM_IDLE)); 
  RodOutList* outList = m_rod->getOutList();
  out.clear();
  if (returnPState == PRIM_WAITING) {
    unsigned long *outBody = outList->getBody();
    for (int i=0; i<outList->getLength(); i++) {
      out.push_back(outBody[i]);
    }
    m_rod->deleteOutList();
  }
}

void RodPixController::waitPrimitiveCompletion(RodOutList* &out) {
  PrimState returnPState;
  do {
    returnPState = m_rod->primHandler();
  } while (returnPState != PRIM_EXECUTING); 
  do {
    returnPState = m_rod->primHandler();
  } while ((returnPState != PRIM_WAITING)&&(returnPState != PRIM_IDLE)); 
  out = m_rod->getOutList();
  if (returnPState != PRIM_WAITING) {
    m_rod->deleteOutList();
    out = NULL;
  }
}
#endif

bool RodPixController::getErrBuffer(std::string &textBuf) {
  while (getGenericBuffer());
  if (m_textErr.empty() == false) {
    textBuf = m_textErr.front();
    m_textErr.pop();
    return true;
  }
  return false;
}

bool RodPixController::getInfoBuffer(std::string &textBuf) {
  while (getGenericBuffer());
  if (m_textInfo.empty() == false) {
    textBuf = m_textInfo.front();
    m_textInfo.pop();
    return true;
  }
  return false;
}

bool RodPixController::getDiagBuffer(std::string &textBuf) {
  while (getGenericBuffer());
  if (m_textDiag.empty() == false) {
    textBuf = m_textDiag.front();
    m_textDiag.pop();
    return true;
  } 
  return false;
}

bool RodPixController::getXferBuffer(std::string &textBuf) {
  while (getGenericBuffer());
  if (m_textXfer.empty() == false) {
    textBuf = m_textXfer.front();
    m_textXfer.pop();
    return true;
  }
  return false;
}

bool RodPixController::getGenericBuffer() {
#ifndef NOTDAQ
  TEXT_BUFFER_TYPE TextType;                 
  long TextLength;                  
  TextBuffState returnTState;
  char *TextBuffer;
  bool data = false;
  std::string textBuf;

  unsigned int bsize = 0;
  for (int ib=0; ib<N_TXT_BUFFS; ib++) {
    if (m_rod->getMdspMap()->txtBufferSize(ib) > bsize) {
      bsize = m_rod->getMdspMap()->txtBufferSize(ib);
    }
  }
  TextBuffer = new char[4*bsize];

  while (m_rod->textHandler() == TEXT_RQ_SET) {
    textBuf = "";
    do {
      returnTState = m_rod->textHandler();
    } while (returnTState != TEXT_READOUT);
    m_rod->getTextBuffer(TextBuffer, TextLength, TextType);
    m_rod->clearTextBuffer();
    //std::cout << "==" << TextBuffer << "==" << std::endl;
    for (int i=0; i<4*TextLength; i++) {
      if (TextBuffer[i+1]%64 != 0) { 
        if (TextBuffer[i]<250) textBuf += TextBuffer[i];
      }
    }
    //std::cout << "++" << textBuf << "++" << std::endl;
    switch (TextType) {
      case TEXT_ERR:
        m_textErr.push(textBuf);
        break;
      case TEXT_INFO:
        m_textInfo.push(textBuf);
        break;
      case TEXT_DIAG:
	m_textDiag.push(textBuf);
	break;
      case TEXT_XFER:
	m_textXfer.push(textBuf);
	break;
      case TEXT_UNDEF:
	break;
    }
    data = true;
  }
  delete[] TextBuffer;
  return data;
#else
  return false;
#endif
}

void RodPixController::initBoc(){               
#ifndef NOTDAQ
  BocCard* m_boc = m_rod->getBocCard();
  m_boc->initialize();
#endif
  return;
}

void RodPixController::getBocStatus(){
#ifndef NOTDAQ
  if(m_rod==0) return;
  BocCard* m_boc = m_rod->getBocCard();
  m_boc->status();
#endif
  return;
}

void RodPixController::resetBoc(){
#ifndef NOTDAQ
  BocCard* m_boc = m_rod->getBocCard();
  m_boc->reset();
#endif
  return;
}

void RodPixController::setBocRegister(std::string regtype, int channel, UINT32 value){
#ifndef NOTDAQ
  BocCard* m_boc = m_rod->getBocCard();
  if (regtype == "DataDelay"){ m_boc->setRxDataDelay(channel, value);}
  if (regtype == "RxThreshold"){ m_boc->setRxThreshold(channel, value);}
  if (regtype == "BpmFineDelay"){ m_boc->setBpmFineDelay(channel, value);}
  if (regtype == "BpmCoarseDelay"){ m_boc->setBpmCoarseDelay(channel, value);}
  if (regtype == "BpmStreamInhibit"){ m_boc->setBpmStreamInhibit(channel, value);}
  if (regtype == "BpmMarkSpace"){ m_boc->setBpmMarkSpace(channel, value);}
  if (regtype == "LaserCurrent"){ m_boc->setLaserCurrent(channel, value);}
#endif
  return;
};

void RodPixController::setBocRegister(std::string regtype, UINT32 value){
#ifndef NOTDAQ
  BocCard* m_boc = m_rod->getBocCard();
  if (regtype == "ClockControl"){ m_boc->setClockControl((UINT32)value);}
  if (regtype == "BRegClockPhase"){ m_boc->setBregClockPhase(value);}
  if (regtype == "BpmClockPhase"){ m_boc->setBpmClockPhase(value);}
  if (regtype == "VernierClockPhase0"){ m_boc->setVernierClockPhase0(value);}
  if (regtype == "VernierClockPhase1"){ m_boc->setVernierClockPhase1(value);}
  if (regtype == "VernierFinePhase"){ m_boc->setVernierFinePhase(value);}
  if (regtype == "RxDataMode"){m_boc->setRxDataMode(value);}
#endif
  return;
};

UINT32 RodPixController::getBocRegister(std::string regtype, int channel){
  UINT32 value=0;
#ifndef NOTDAQ
  BocCard* m_boc = m_rod->getBocCard();
  if (regtype == "DataDelay"){value = m_boc->getRxDataDelay(channel);}
  if (regtype == "RxThreshold"){
    try {value = m_boc->getRxThreshold(channel);}
    catch (BocException &b) {
      std::cout << "BocException when setting RX threshold" << std::endl;
      std::cout << b.getDescriptor() << " ";
      std::cout << b.getData1() << " : " << b.getData2() << std::endl;
    }
  }
  if (regtype == "BpmFineDelay"){value = m_boc->getBpmFineDelay(channel);}
  if (regtype == "BpmCoarseDelay"){value = m_boc->getBpmCoarseDelay(channel);}
  if (regtype == "BpmStreamInhibit"){value = m_boc->getBpmStreamInhibit(channel);}
  if (regtype == "BpmMarkSpace"){value = m_boc->getBpmMarkSpace(channel);}
  if (regtype == "LaserCurrent"){value = m_boc->getLaserCurrent(channel);}
#endif
  return value;
};

UINT32 RodPixController::getBocRegister(std::string regtype){
  UINT32 value=0;
#ifndef NOTDAQ
  BocCard* m_boc = m_rod->getBocCard();
  if (regtype == "ClockControl"){value = m_boc->getClockControl();}
  if (regtype == "BRegClockPhase"){value = m_boc->getBregClockPhase();}
  if (regtype == "BpmClockPhase"){value = m_boc->getBpmClockPhase();}
  if (regtype == "VernierClockPhase0"){value = m_boc->getVernierClockPhase0();}
  if (regtype == "VernierClockPhase1"){value = m_boc->getVernierClockPhase1();}
  if (regtype == "VernierFinePhase"){value = m_boc->getVernierFinePhase();}
  if (regtype == "RxDataMode"){value = m_boc->getRxDataMode();}
#endif
  return value;
};

double RodPixController::getBocMonitorAdc(int channel){
#ifndef NOTDAQ
  if(m_rod==0) return 0.0;
  BocCard* m_boc = m_rod->getBocCard();
  return m_boc->getMonitorAdc(channel); 
#else
  return 0.0;
#endif
};

void RodPixController::resetBocMonitorAdc(){
#ifndef NOTDAQ
  if(m_rod==0) return;
  BocCard* m_boc = m_rod->getBocCard();
  m_boc->resetMonitorAdc();
#endif
  return;
};
void RodPixController::shiftPixMask(int mask, int steps){
}