#ifndef PCA16_H
#define PCA16_H

#include <math.h>
#include <stdio.h>
#include <stdint.h>
#include <stdbool.h>
#include <argp.h>
#include <getopt.h>
#include <signal.h>
#include <stdio.h>
#include <string.h>
#include <sys/time.h>
#include <time.h>

#define LOG_DEBUG   5
#define LOG_INFO    4
#define LOG_WARNING 3
#define LOG_ERROR   2
#define LOG_FATAL   1
#define LOG_ALWAYS  0

#define RCU_END 0x380000

void logger(const int severity, char* msg);

//
// ArgP functions
//

struct arguments arguments;

//========================================================================
// Stuff for ArgP

const char *argp_program_version = "pca16.app v0.1";
const char *argp_program_bug_address = "<christian.lippmann@cern.ch>";

static char doc[] = "Program to configure ILC TPC FECs through the DDL.";

static char args_doc[] = "";

#define OPT_SW      1
#define OPT_SPARSE  2
#define OPT_SKIP    3
#define OPT_NOPCA16 4
#define OPT_GF      5
#define OPT_PRE     6
#define OPT_POST    7
#define OPT_POL     8

//========================================================================
// The options ARGP understands
static struct argp_option options[] = {
  {"rev",       'r',         "RORCREVISION",    0, "Specify rorc revision (default=4)",                                 1 },
  {"serial",    's',         "RORCSERIAL",      0, "Specify rorc serial (no default)",                                  1 },
  {"channel",   'c',         "RORCCHANNEL",     0, "Specify rorc channel (default=0)",                                  1 },
  {"off",       'o',         0,                 0, "Switch off all FECs (default false)",                               2 },
  {"rcu",       'p',         "RCUNUM",          0, "Specify rcu=patch=partition [0..1].",                               2 },
  {"partition",  0 ,         0 ,                OPTION_ALIAS },
  {"fec",       'f',         "FEC1[,FEC2..]",   0, "Use these FECs (0...31; address includes branch)",                  2 },
  {"sampling",  'k',         "PERIOD",          0, "Specify sampling clock (50, 100, 200 or 400ns; default=50)",        3 },
  {"sparse",    OPT_SPARSE,  0,                 0, "Use sparse readout mode (RDOMOD; default=false)",                   3 },
  {"skip",      OPT_SKIP,    0,                 0, "Skip empty channels in RCU (RDOMOD; default=false)",                3 },
  {"swtrg",     OPT_SW,      0,                 0, "Configure for software trigger (default false)",                    3 },
  {"nopca16",   OPT_NOPCA16, 0,                 0, "Dont try to configure PCA16 (for ALICE FECs, default false)",       3 },
  {"acqstop",   't',         "ENDTIMEBIN",      0, "Specify number of timebins (default=1008)",                         4 },
  {"acqstart",  'q',         "STARTTIMEBIN",    0, "Specify aquisition start (default=0 with chan-by-chan offset)",     4 },
  {"zs",        'Z',         0,                 0, "Enable zero suppression (default=DISABLED)",                        5 },
  {"zsthr",     'z',         "THRESHOLD",       0, "Specify zero suppression threshold (default=2 ADC counts)",         5 },
  {"gf",        OPT_GF,      "VALUE",           0, "Specify glitch filter configuration (0..3; default=0)",             5 },
  {"pre",       OPT_PRE,     "NUM_PRESAMPLES",  0, "Specify number of presamples excluded from ZS (0..3; default=1)",   5 },
  {"post",      OPT_POST,    "NUM_POSTSAMPLES", 0, "Specify number of postsamples excluded from ZS (0..15; default=3",  5 },
  {"pwsv",      'P',         "1/0",             0, "Enable/Disable power save mode (default=ENABLED)",                  6 },
  {"bc1cfg",    'b',         "CONFIG",          0, "Specify BC1 config (see ALTRO manual; default=0x8)",                6 },
  {"fpedfile",  'F',         "PATH/FILE",       0, "Write fixed pedestal data from FILE to ALTROs",                     6 },
  {"gain",      'g',         "GAIN",            0, "Specify PCA16 gain (12, 15, 19 or 27 mV/fC; default=12)",           7 },
  {"shaping",   'u',         "SHAPINGTIME",     0, "Specify PCA16 shaping time 30, 60, 90 or 120ns; default=120ns)",    7 },
  {"decaytime", 'd',         "DECAYTIME",       0, "Specify DAC value (PCA16 decay time [0...2500]; default=500)",      7 },
  {"polarity",  OPT_POL,     "1/0",             0, "Specify signal polarity (0 = POS,1 = NEG]; default=1)",             7 },
  {"verbosity", 'v',         "LEVEL",           0, "Specify verbosity (0=QUIET...5=DEBUG; default=4)",                  1 },
  { 0 }
};

//========================================================================
// Used by main to communicate with ARGP parse_opt
struct arguments {
  int32_t rcu, fecs, verbosity, tb, rev, serial, channel, shaping, gain, dac, acqstart;
  int32_t sampling, zsthr, bc1cfg, gf, pres, posts;
  char *inputfile;
  bool off, swtrg, sparse, skip, zsen, fpedfile, pwsv, pca16, polneg;
};

//========================================================================
//Get the input argument from argp_parse, which is a pointer to the ARGP arguments structure.
static error_t parse_opt (int key, char *arg, struct argp_state *state) {

  struct arguments *arguments = (struct arguments*)state->input;
  char *token;
  char *arg_copy;
  int32_t val;

  switch ( key ) {   // Parse a single option

  case 'v':
    arguments->verbosity = atoi(arg);
    break;

  case OPT_NOPCA16:
    arguments->pca16  = false;
    arguments->bc1cfg = 0; // din- fpd
   break;

  case 't':
    arguments->tb = atoi(arg);
    break;

  case 'r':
    arguments->rev = atoi(arg);
    break;

  case 's':
    arguments->serial = atoi(arg);
    break;

  case 'c':
    arguments->channel = atoi(arg);
    break;

  case 'q':
    arguments->acqstart = atoi(arg);
    break;

  case 'z':
    arguments->zsthr = atoi(arg);
    break;

  case OPT_GF:
    arguments->gf = atoi(arg);
    break;

  case OPT_PRE:
    arguments->pres = atoi(arg);
    break;

  case OPT_POST:
    arguments->posts = atoi(arg);
    break;

  case 'b':
    arguments->bc1cfg = atoi(arg);
    break;

  case 'Z':
    arguments->zsen = true;
    break;

  case 'P':
    val = atoi(arg);
    if      (val == 1) arguments->pwsv = true;
    else if (val == 0) arguments->pwsv = false;
    else {
      logger(LOG_FATAL, "Allowed values of pwsv are 0 and 1 (off and on).");
      exit(1);
    }
    break;

  case 'F':
    arguments->fpedfile = true;
    arguments->inputfile = arg;
    break;

  case 'k':
    val = atoi(arg);
    switch ( val ) {
    case 50:
      arguments->sampling = 0;
      break;
    case 100:
      arguments->sampling = 1;
      break;
    case 200:
      arguments->sampling = 2;
      break;
    case 400:
      arguments->sampling = 3;
      break;
    default:
      logger(LOG_FATAL, "Allowed sampling clock periods are 50, 100, 200 and 400 (unit ns).");
      exit(1);
    }
    break;

  case 'g':
    val = atoi(arg);
    switch ( val ) {
    case 12:
      arguments->gain = 0;
      break;
    case 15:
      arguments->gain = 1;
      break;
    case 19:
      arguments->gain = 2;
      break;
    case 27:
      arguments->gain = 3;
      break;
    default:
      logger(LOG_FATAL, "Allowed gains are 12, 15, 19 and 27 (unit mV/fC).");
      exit(1);
    }
    break;

  case 'u':
    val = atoi(arg);
    switch ( val ) {
    case 30:
      arguments->shaping = 7;
      break;
    case 60:
      arguments->shaping = 3;
      break;
    case 90:
      arguments->shaping = 1;
      break;
    case 120:
      arguments->shaping = 0;
      break;
    default:
      logger(LOG_FATAL, "Allowed shaping times are 30, 60, 90 and 120 (unit ns).");
      exit(1);
    }
    break;

  case 'd':
    val = atoi(arg);
    if ( (val>=0) && (val<=2500) ) arguments->dac = val;
    else {
      logger(LOG_FATAL, "DAC parameter has to be in range 0..2500 (2500 corresponds to 1.5V).");
      exit(1);
    }
    break;

  case OPT_POL:
    val = atoi(arg);
    if      (val == 1) arguments->polneg = true;
    else if (val == 0) arguments->polneg = false;
    else {
      logger(LOG_FATAL, "Allowed values of polarity are 0 and 1.");
      exit(1);
    }
    break;

  case 'o':
    arguments->off = true;
    arguments->fecs = 0x0;
    break;

  case OPT_SW:
    arguments->swtrg = true;
    break;

  case OPT_SPARSE:
    arguments->sparse = true;
    break;

  case OPT_SKIP:
    arguments->skip = true;
    break;

  case 'p':
    arguments->rcu = atoi(arg);
    break;

  case 'f':
    arguments->off = false;
    arguments->fecs = 0x0;
    arg_copy = strdup(arg);
    token = strtok(arg_copy, ",");
    do {
      int32_t fec = atoi(token);
      if ( (0<=fec) && (fec<=31) )
        arguments->fecs |= 1<<fec;
      else {
	char desc[256];
	sprintf(desc, "Invalid argument: No such FEC: %s", token);
        logger(LOG_FATAL, desc);
        exit(1);
      }
    } while ( (token = strtok(NULL,",")) );
    free(arg_copy);
    break;

  case ARGP_KEY_ARG:
    argp_usage(state);
    break;

  case ARGP_KEY_END:
    if (state->arg_num > 0) argp_usage(state);  // To many arguments
    break;
     
  default:
    return ARGP_ERR_UNKNOWN;
  }
  return 0;
}

//========================================================================
static struct argp argp = { options, parse_opt, args_doc, doc };


//
// Logging functions
//

//========================================================================
void logger(const int severity, char* msg) {
  // 
  char str[256];
  if ( severity == LOG_ALWAYS  ) {
    sprintf(str, "INFO    : %s", msg); // use not too much ...
    printf("%s\n", str);
  } else if ( severity <= arguments.verbosity ) {
    if      ( severity == LOG_DEBUG   ) sprintf(str, "DEBUG   : %s", msg);
    else if ( severity == LOG_INFO    ) sprintf(str, "INFO    : %s", msg);
    else if ( severity == LOG_WARNING ) sprintf(str, "WARNING : %s", msg);
    else if ( severity == LOG_ERROR   ) sprintf(str, "ERROR   : %s", msg);
    else if ( severity == LOG_FATAL   ) sprintf(str, "FATAL   : %s", msg);
    printf("%s\n", str);
  }
}


//
// RCU model
//

//========================================================================
typedef struct {
  chan_key* key;             // FEC2RORC
  int32_t ddl_revision;      // FEC2RORC
  int32_t ddl_serial;        // FEC2RORC
  int32_t ddl_channel;       // FEC2RORC
  int32_t afl;               // RCU register
  int32_t status;            // status of the configuration
  int32_t retry_count;       // count of IMEM restarts due to buggy -512 error
} rcu_model_t;

//========================================================================
rcu_model_t* get_new_rcu_model() {
  /*   create a new and empty RCU model  */
  rcu_model_t* temp = (rcu_model_t *)calloc(1, sizeof(rcu_model_t));  
  if (temp == NULL) return NULL;
  temp->retry_count = 0;
  temp->key = NULL;
  
  return temp;
}

//========================================================================
void free_rcu_model(rcu_model_t* rcu) {
  logger(LOG_DEBUG, "Deallocate the RCU models");
  free(rcu);
  logger(LOG_DEBUG, "Successfully deallocated the RCU models");
}

//
// Misc functions like converting hardware address
//

//========================================================================
int32_t codeAddress(int32_t FEC,  int32_t chip, int32_t channel) {
  // FEC address contains branch
  return ((FEC&0x1f)<<7) + ((chip&0x7)<<4) + (channel&0xf);
}

//========================================================================
int32_t decodedAddressBranch(int32_t Address) {
  return ((Address>>11)&1);
}

//========================================================================
int32_t decodedAddressFECaddr(int32_t Address) {
  return ((Address>>7)&0xf);
}

//========================================================================
int32_t decodedAddressChipaddr(int32_t Address) {
  return ((Address>>4)&0x7);
}

//========================================================================
int32_t decodedAddressChanneladdr(int32_t Address) {
  return ((Address&0xf));
}

//========================================================================
int32_t roundtoint(double number) {
  number = floor(number + 0.5);
  return (int32_t)number;
}

//========================================================================
float readfloat(int32_t *ok, FILE *fp) {
  //
  // Function to read a float from a file
  //
  char string[1001];
  if ( fscanf(fp,"%s",string) == EOF ) {
    *ok = 0;
    return -1;
  }
  *ok = 1;
  return (float) atof(string);
}

//========================================================================
int32_t readint(int32_t *ok, FILE *fp) {
  //
  // Function to read an integer from a file
  //
  char string[1001];
  if ( fscanf(fp,"%s",string) == EOF ) {
    *ok = 0;
    return -1;
  }
  *ok = 1;
  return (int) atoi(string);
}


//
// RCU specific functions (firmware V2)
//

//========================================================================
int32_t Read_ACTFECLIST(rcu_model_t* rcu, uint32_t *active_fecs) {
  //
  // Read the ACTFEC List
  //
  logger(LOG_DEBUG, "Calling Read_ACTFECLIST()");

  if ( rcu_readReg(rcu->key, ACTFECLIST, active_fecs) != 1 ) {
    logger(LOG_ERROR, "rcu_readReg() failed (ACTFECLIST)");
    return 1;
  }
  *active_fecs = (*active_fecs)&0xFFFFFFFF;
  // save in rcu struct
  rcu->afl = *active_fecs;

  logger(LOG_DEBUG, "Read_ACTFECLIST() successful");

  return 0;

}

//========================================================================
int32_t waitBusBusy(rcu_model_t* rcu) {
  //
  // Check if RCU is still busy with excecuting last sequence
  //

  logger(LOG_DEBUG, "Calling waitBusBusy()");

  uint32_t busbusy = 1;
  uint32_t tcount = 0;
  while ( busbusy&0x1 ) {
    usleep(5000);
    if ( rcu_readReg(rcu->key, RCUBUSBUSY, &busbusy) != 1 ) {
      logger(LOG_ERROR, "Function rcu_readReg failed (RCUBUSBSY)");
      return 1;
    }
    if ( busbusy&0x1 ) {
      char desc[256];
      sprintf(desc, "Function waitBusBusy(): RCU still busy with excecuting last sequence (tcount=%d)",
	      tcount);
      logger(LOG_DEBUG, desc);
      if (tcount == 100) {
	logger(LOG_ERROR, "RCU bus still busy after 100 iterations");
	return 1;
      }
      tcount++;   
    }
  }

  logger(LOG_DEBUG, "Function waitBusBusy() successful");
  
  return 0;
}

//========================================================================
int32_t checkResMem(rcu_model_t* rcu, int32_t size, uint32_t *imem) {
  //
  // Check in the RESULT MEMORY if the execution of the IMEM has been done correctly
  // For each instruction in the IMEM I have two words in the RESULT MEMORY
  //
  uint32_t buffer_result[RCU_RESULT_MEM_SZ];
  uint32_t tcount = 0;
  int32_t  index  = 0;
  int32_t  kk     = 0;
  int32_t  check  = 1;
  int32_t  retval;

  char desc[256];

  logger(LOG_DEBUG, "Calling checkResMem()");

  while ( check == 1 ) {

    // READ RESULT MEMORY
    retval = rcu_readMem(rcu->key, RCU_RESULT_MEM, buffer_result, size);
    sprintf(desc, "%d words read from RESULT_MEM", retval);
    logger(LOG_DEBUG, desc);
    if (retval != size) {
      sprintf(desc, "Function rcu_readMem() failed. %d words expected, %d words read\n",
	      size, retval);
      logger(LOG_ERROR, desc);
      return 1;
    }

    // check all the words
    logger(LOG_DEBUG, "Checking content of RESULT MEM");
    for ( kk=0; kk<size; kk++ ) {
      check = ( (buffer_result[kk]>>20) & 0x1 );
      if (check == 1) { // ERROR !!
	index = kk;
	sprintf(desc, "Found an error in the RMEM, index=%d, words 0x%X, 0x%X",
		tcount, index, (int)buffer_result[index], (int)buffer_result[index+1]);
	logger(LOG_ERROR, desc);
	break;
      }
    }

    // execute again IMEM
    if ( check == 1) {
      sprintf(desc, "Executing IMEM once again (count=%d)..", tcount);
      logger(LOG_INFO, desc);
      if ( fec_execIMEM(rcu->key) ) return 1;
    }

    if (tcount == 3) break;
    tcount++;

  } // end while (check == 1)

  // check if in the IMEM the words have been uploaded correctly
  if (check == 1) {
    logger(LOG_INFO, "Reading IMEM and compare to what was written");
    retval = rcu_readMem(rcu->key, RCU_INSTRUCTION_MEM, buffer_result, size);
    sprintf(desc, "%d words read from IMEM", retval);
    logger(LOG_DEBUG, desc);
    if (retval != size) {
      sprintf(desc, "RCU_Mem_Read failed. %d words expected, %d words read\n",
	      size, retval);
      logger(LOG_ERROR, desc);
      return 1;
    }
    for ( kk=0; kk<size; kk++ ) {
      if (imem[kk] != buffer_result[kk]) {
	sprintf(desc, "INSTRUCTION MEM check FAILED. IMEM WORD %X!=%X (index %d)\n",
		(int)buffer_result[kk], (int)imem[kk], kk); 
	logger(LOG_ERROR, desc);
	return 1;
      }
    }
    logger(LOG_INFO, "Comparison of instructions in IMEM ok");
    return 1;
  } // end if failure

  return 0;

}

//========================================================================
int32_t FEC_IMEM_Write(rcu_model_t* rcu, uint32_t* imem, int32_t size) {
  //
  // write to IMEM
  //
  logger(LOG_DEBUG, "Function FEC_IMEM_Write(): Writing to INSTRUCTION MEM");

  if ( rcu_writeMem(rcu->key, RCU_INSTRUCTION_MEM, imem, size) ) {
    logger(LOG_ERROR, "Writing to INSTRUCTION MEM failed!");
    return 1;
  }

  // Wait for bus to be ready
  if ( waitBusBusy(rcu)       ) return 1;  

  // Execute the sequence in the instruction memory.
  if ( fec_execIMEM(rcu->key) ) return 1;

  // Check the content of the RESULT_MEM
  if ( checkResMem(rcu, size, imem) ) return 1;

  logger(LOG_DEBUG, "FEC_IMEM_Write() successful");

  return 0;
}

//========================================================================
int32_t FEC_ROLMEM_Write(rcu_model_t* rcu, uint32_t* mema, uint32_t* memb) {
  //
  // write to ROLMEM A and B
  //
  char desc[256];

  sprintf(desc, "Function FEC_ROLMMEM_Write(): Writing to READOUT List Mem A (0x%x)",
	  RCU_READOUT_MEMA);
  logger(LOG_DEBUG, desc);
  if ( rcu_writeMem(rcu->key, RCU_READOUT_MEMA, mema, RCU_READOUT_MEM_SZ/2) ) {
    logger(LOG_ERROR, "Writing to RCU_READOUT_MEM A failed!");
    return 1;
  }

  sprintf(desc, "Function FEC_ROLMMEM_Write(): Writing to READOUT List Mem B (0x%x)",
	  RCU_READOUT_MEMB);
  logger(LOG_DEBUG, desc);
  if ( rcu_writeMem(rcu->key, RCU_READOUT_MEMB, memb, RCU_READOUT_MEM_SZ/2) ) {
    logger(LOG_ERROR, "Writing to RCU_READOUT_MEM B failed!");
    return 1;
  }

  logger(LOG_DEBUG, "FEC_ROLMEM_Write() successful");

  return 0;
}

//========================================================================
int32_t FEC_Reg_Write(rcu_model_t* rcu, uint32_t hw_add, uint32_t bcast,
		      uint32_t reg_add, uint32_t reg_data, int32_t bc_al) {
  //
  // Write data to ALTRO or FEC board controller register
  //

  logger(LOG_DEBUG, "Calling FEC_Reg_Write()");

  /* Prepare Instruction Sequence for the IMEM */
  uint32_t address = (bcast<<18) | (bc_al<<17) | (hw_add<<5) | reg_add;
  uint32_t imem[3];
  imem[0] = (2<<20) | address;
  imem[1] = reg_data;
  imem[2] = RCU_END;
  char desc[256];

  sprintf(desc, "FEC_Reg_Write(): imem[0]=0x%x imem[1]=0x%x imem[2]=0x%x",
	  (int)imem[0], (int)imem[1], (int)imem[2]);
  logger(LOG_DEBUG, desc);

  /* Write Instruction Sequence into the IMEM */
  if ( FEC_IMEM_Write(rcu, imem, 3) ) {
    logger(LOG_ERROR, "Function FEC_Reg_Write(): Writing and executing IMEM failed!");
    return 1;
  }

  logger(LOG_DEBUG, "Function FEC_Reg_Write() successful");

  return 0;
}

//========================================================================
int32_t FEC_ALTROREG_Write(rcu_model_t* rcu, uint32_t hw_add, uint32_t bcast,
			   uint32_t reg_add, uint32_t reg_data) {
  //
  // Write data to ALTRO register
  //
  int32_t bc_al = 0; // BC = 1 ALTRO = 0
  if ( FEC_Reg_Write(rcu, hw_add, bcast, reg_add, reg_data, bc_al) ) return 1;
  return 0;
}

//========================================================================
int32_t FEC_BCREG_Write(rcu_model_t* rcu, uint32_t hw_add, uint32_t bcast,
			uint32_t reg_add, uint32_t reg_data) {
  //
  // Write data to FEC board controller register
  //
  int32_t bc_al = 1; // BC = 1 ALTRO = 0
  if ( FEC_Reg_Write(rcu, hw_add, bcast, reg_add, reg_data, bc_al) ) return 1;
  return 0;
}

//========================================================================
int32_t Write_ACTFECLIST(rcu_model_t* rcu, uint32_t fecs) {
  //
  // switch the FECs on and off according to the ACTFEC List
  //
  uint32_t active_fecs;
  int32_t  fec, retval;

  char desc[256];

  logger(LOG_DEBUG, "Calling Write_ACTFECLIST()");

  // Read AFL
  retval = rcu_readReg(rcu->key, ACTFECLIST, &active_fecs);
  if ( retval != 1 ) {
    logger(LOG_ERROR, "Function Write_ACTFECLIST(): rcu_readReg() failed (ACTFECLIST)");
    return 1;
  }
  active_fecs = (active_fecs&0xFFFFFFFF);

  if (active_fecs & ~fecs) {
    for ( fec=0; fec<32; ++fec ) {
      if ( (active_fecs & ~fecs)&1<<fec ) {
	sprintf(desc, "Switching off FEC %d ..", fec);
	logger(LOG_DEBUG, desc);
        active_fecs &= ~(1<<fec);
	if ( rcu_writeReg(rcu->key, ACTFECLIST, active_fecs) ) {
	  logger(LOG_ERROR, "Function Write_ACTFECLIST(): rcu_writeReg() failed (ACTFECLIST)");
	  return 1;
	}
	usleep(200000);
      }
    }
  }
  if (~active_fecs & fecs) {
    for (fec=0;fec<32;++fec)
      if ( (~active_fecs & fecs)&1<<fec ) {
	sprintf(desc, "Switching on FEC %d ..", fec);
	logger(LOG_DEBUG, desc);
        active_fecs |= 1<<fec;
	if ( rcu_writeReg(rcu->key, ACTFECLIST, active_fecs) ) {
	  logger(LOG_ERROR, "Function Write_ACTFECLIST(): rcu_writeReg failed (ACTFECLIST)");
	  return 1;
	}
        usleep(200000);
      }
  }

  logger(LOG_DEBUG, "Function Write_ACTFECLIST() successful");

  return 0;
}

//========================================================================
bool checkFecOn(rcu_model_t* rcu, int fecaddr) {
  //
  // check if this fec is on
  //
  char desc[256];
  if ( (rcu->afl) & (1<<fecaddr) ) {
    sprintf(desc, "Function checkFecOn(): Fec with address %d is ON", fecaddr);
    //logger(LOG_DEBUG, desc);
    return true;
  } else {
    sprintf(desc, "Function checkFecOn(): Fec with address %d is OFF!", fecaddr);
    //logger(LOG_DEBUG, desc);
    return false;
  } 
  return false;
}


//
// 6) Rorc functions
//

//========================================================================
int32_t RORC_Reset_all(rcu_model_t* rcu) {

  char desc[256];

  logger(LOG_DEBUG, "Calling  RORC_Reset_all()");

  //reset RORC
  rorcReset(&(rcu->key->rorc), RORC_RESET_RORC); 
  sprintf(desc, "RORC reset executed (rorc serial %d)", rcu->ddl_serial);
  logger(LOG_DEBUG, desc);
  usleep(10000); 

  // reset DIU
  rorcReset(&(rcu->key->rorc), RORC_RESET_DIU); 
  sprintf(desc, "DIU reset executed (rorc serial %d)", rcu->ddl_serial);
  logger(LOG_DEBUG, desc);
  usleep(10000); 

  //reset SIU
  rorcReset(&(rcu->key->rorc), RORC_RESET_SIU); 
  sprintf(desc, "SIU reset executed (rorc serial %d)", rcu->ddl_serial);
  logger(LOG_DEBUG, desc);
  usleep(10000); 

  // reset DIU
  rorcReset(&(rcu->key->rorc), RORC_RESET_DIU); 
  sprintf(desc, "DIU reset executed (rorc serial %d)", rcu->ddl_serial);
  logger(LOG_DEBUG, desc);
  usleep(10000); 

  //reset RORC
  rorcReset(&(rcu->key->rorc), RORC_RESET_RORC); 
  sprintf(desc, "RORC reset executed (rorc serial %d)", rcu->ddl_serial);
  logger(LOG_DEBUG, desc);
  usleep(10000);

  logger(LOG_DEBUG, "RORC_Reset_all() successful");

  uint32_t status  = 0;
  int64_t  timeout = DDL_RESPONSE_TIME * rcu->key->rorc.pci_loop_per_usec;

  if (rorcCheckLink(&(rcu->key->rorc)) != RORC_STATUS_OK) {
    sprintf(desc, "SIU not seen. Can not clear SIU status (rorc serial %d)!", rcu->ddl_serial);
    logger(LOG_ERROR, desc);
    return 1;
  } else {
    status = ddlReadSiu(&(rcu->key->rorc), 0, timeout);
    if (status != -1) {
      sprintf(desc, "SIU status cleared. Status word: 0x%08x (rorc serial %d)", status, rcu->ddl_serial);
      logger(LOG_DEBUG, desc);
    }
  }

  status = ddlReadDiu(&(rcu->key->rorc), 0, timeout);
  if (status != -1) {
    sprintf(desc, "DIU status cleared. Status word: 0x%08x (rorc serial %d)", status, rcu->ddl_serial);
    logger(LOG_DEBUG, desc);
  }

  return 0;
}


//
// Configuration functions
//

//========================================================================
void signal_handler(int32_t signum) {
  switch(signum) {
    case SIGQUIT:
    case SIGTERM:
    case SIGINT:
    logger(LOG_INFO, "SIGTERM");
    int32_t signal_stop = 1; break;
  }
}

//========================================================================
int32_t checkFecs(rcu_model_t* rcu) {
  //
  // Switch the FECs
  //

  logger(LOG_DEBUG, "Entering function checkFecs()");

  uint32_t data;

  // Read ACTFECLIST
  if ( Read_ACTFECLIST(rcu, &data) ) {
    logger(LOG_ERROR, "Function checkFecs(): Read_ACTFECLIST failed!");
    return 1;
  }
  char desc[256];
  sprintf(desc, "Content of Register ACTFECLIST: 0x%X", (int)data);
  logger(LOG_INFO, desc);

  if (data == 0) {
    logger(LOG_WARNING, "All FECs are OFF!");
  }

  usleep(500000);

  logger(LOG_DEBUG, "Exiting function checkFecs()");

  return 0;
}

//========================================================================
int32_t switchFecs(rcu_model_t* rcu) {
  //
  // Switch the FECs
  //

  logger(LOG_DEBUG, "Entering function switchFecs()");

  uint32_t data;

  // Switch cards
  if ( Write_ACTFECLIST(rcu, arguments.fecs) ) {
    logger(LOG_ERROR, "Function switchFecs(): Write_ACTFECLIST() failed!");
    return 1;
  }

  if ( checkFecs(rcu) ) {
    logger(LOG_ERROR, "Function checkFecs() failed!");
    return 1;
  };

  logger(LOG_DEBUG, "Exiting function switchFecs()");

  return 0;
}

//========================================================================
int32_t clearall(rcu_model_t* rcu) {
  //
  // Clear RCU error and status registers. Check for ALTRO errors
  //

  logger(LOG_DEBUG, "Entering function clearall(). Clear RCU error and status registers");

  uint32_t data;
  int32_t  retval;
  int32_t  ii = 0;

  char desc[256];

  // 1) Clear RCU ERROR and Status registers
  logger(LOG_DEBUG, "Executing rcu_clearCLERRREG()!");
  retval = rcu_clearCLERRREG(rcu->key); 
  if ( retval ) {
    logger(LOG_ERROR, "Function RCU_clearCLERRREG failed!");
    return retval;
  }

  // 2) CONFGFEC (reprogram FEC board controller)
  logger(LOG_DEBUG, "Executing rcu_writeReg() (CONFFEC)!");
  if ( rcu_writeReg(rcu->key, CONFFEC, 0xffffffff) ) {
    logger(LOG_ERROR, "Function rcu_writeReg() failed (CONFFEC)!");
    return 1;
  }
  usleep(500000);

  // 3) Reset RCU and FECs
  logger(LOG_DEBUG, "Executing rcu_resetALL()!");
  retval = rcu_resetALL(rcu->key);
  if ( retval ) {
    logger(LOG_ERROR, "RCU and FEC reset failed!");
    return retval;
  }

  // 4) Read FECERR_A
  do {
    logger(LOG_DEBUG, "Executing rcu_readReg(FECERRA)!");
    retval = rcu_readReg(rcu->key, FECERRA, &data);
    if ( retval != 1 ) {
      logger(LOG_ERROR, "Function rcu_readReg() failed (FECERRA)!");
      return 1;
    }
    data = data&0xFFFFFFFF;
    sprintf(desc, "Function clearall(): Content of Register FECERRA: 0x%x", (int)data);
    logger(LOG_DEBUG, desc);
    if (data&0x1) {
      if (ii<3) {
	logger(LOG_ERROR, "ALTRO ERROR asserted on Branch A!");
	logger(LOG_INFO, "Resetting ALL");
	usleep(500000);
	if ( rcu_resetALL(rcu->key) ) {
	  logger(LOG_ERROR, "RCU and FEC reset failed!");
	  return 1;
	}
	logger(LOG_INFO, "Clearing RCU ERROR and Status registers");
	if ( rcu_clearCLERRREG(rcu->key) ) {
	  logger(LOG_ERROR, "Function RCU_clearCLERRREG failed!");
	  return retval;
	}
      } else {
	logger(LOG_ERROR, "ALTRO ERROR on Branch A can not be recovered!!");
	return 1;
      }
      ii++;
    } else break;
  } while (ii<3);

  // 5) Read FECERR_B
  ii = 0;
  do {
    logger(LOG_DEBUG, "Executing rcu_readReg(FECERRB)!");
    retval = rcu_readReg(rcu->key, FECERRB, &data);
    if ( retval != 1 ) {
      logger(LOG_ERROR, "Function rcu_readReg() failed (FECERRB)!");
      return 1;
    }
    data = data&0xFFFFFFFF;
    sprintf(desc, "Function clearall(): Content of Register FECERRB: 0x%x", (int)data);
    logger(LOG_DEBUG, desc);
    if (data&0x1) {
      if (ii<3) {
	logger(LOG_ERROR, "ALTRO ERROR asserted on Branch B.!");
	logger(LOG_INFO, "Resetting FECs");
	usleep(500000);
	if ( rcu_resetALL(rcu->key) ) {
	  logger(LOG_ERROR, "RCU and FEC reset failed!");
	  return 1;
	}
	logger(LOG_INFO, "Clearing RCU ERROR and Status registers!");
	if ( rcu_clearCLERRREG(rcu->key) ) {
	  logger(LOG_ERROR, "Function RCU_clearCLERRREG failed!");
	  return retval;
	}
      } else {
	logger(LOG_ERROR, "ALTRO ERROR on Branch B can not be recovered!!");
	return 1;
      }
      ii++;
    } else break;
  } while (ii<3);

  logger(LOG_DEBUG, "Exiting function clearall()");

  return 0;
}

//========================================================================
int32_t check_fecerr(rcu_model_t* rcu) {

  logger(LOG_DEBUG, "Entering function check_fecerr()");

  uint32_t data, retval;
  char desc[256];

  // 1) Read FECERR_A
    logger(LOG_DEBUG, "Executing rcu_readReg(FECERRA)!");
  retval = rcu_readReg(rcu->key, FECERRA, &data);
  if ( retval != 1 ) {
    logger(LOG_ERROR, "Function check_fecerr() failed (FECERRA)!");
    return 1;
  }
  data = data&0xFFFFFFFF;
  sprintf(desc, "Content of Register FECERRA: 0x%x", (int)data);
  if (data&0x1) {
    logger(LOG_ERROR, "ALTRO ERROR asserted on Branch A!");
    logger(LOG_ERROR, desc);
    return 1;
  } else {
    logger(LOG_DEBUG, desc);
  }
  
  // 10) Read FECERR_B
    logger(LOG_DEBUG, "Executing rcu_readReg(FECERRB)!");
  retval = rcu_readReg(rcu->key, FECERRB, &data);
  if ( retval != 1 ) {
    logger(LOG_ERROR, "Function check_fecerr() failed (FECERRB)!");
    return 1;
  }
  data = data&0xFFFFFFFF;
  sprintf(desc, "Content of Register FECERRB: 0x%x", (int)data);
  if (data&0x1) {
    logger(LOG_ERROR, "ALTRO ERROR asserted on Branch B!");
    logger(LOG_ERROR, desc);
    return 1;
  } else {
    logger(LOG_DEBUG, desc);
  }

  logger(LOG_DEBUG, "Exiting function check_fecerr()");

  return 0;
}

//========================================================================
int32_t writeFPED(rcu_model_t* rcu) {
  //
  // Write the fixed pedestal values from file to ALTROs
  //

  // !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
  // !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
  // The channel number as sector!
  // RORC channel 0, minor 0 => DDL 768 (set in editDb), sector 0, RCU0
  // RORC channel 1, minor 0 => DDL 769 (set in editDb), sector 0, RCU1
  // RORC channel 0, minor 1 => DDL 770 (set in editDb), sector 1, RCU0
  // RORC channel 1, minor 1 => DDL 771 (set in editDb), sector 1, RCU1
  // RORC channel 0, minor 2 => DDL 840 (set in editDb), sector 0, RCU2
  // RORC channel 1, minor 2 => NOT_USED!
  // !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
  // !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!

  char desc[256];

  logger(LOG_DEBUG, "Entering function writeFPED()");

  int32_t ii;
  uint32_t pedValue;
  int32_t side, sector, patch, hwaddr;
  int32_t ok = 1;

  // open file with data
  FILE *inputfile;
  sprintf(desc, "Opening file %s ..", arguments.inputfile);
  logger(LOG_DEBUG, desc);
  if ( (inputfile = fopen(arguments.inputfile, "r")) == NULL ) {
    sprintf(desc, "Cannot open file %s!", arguments.inputfile);
    logger(LOG_ERROR, desc);
    return 1;
  }

  uint32_t imem[RCU_INSTRUCTION_MEM_SZ];
  int32_t memctr = 0;

  // read the first number in the file to check if it is pedestal file
  int32_t test = readint(&ok, inputfile);
  if ( test != 10 ) {
    logger(LOG_ERROR, "Wrong file type (not a pedestal file)!");
    return 1;
  }

  logger(LOG_DEBUG, "Reading file and writing to RCU...!");

  while ( 1 ) {    /* Loop over all channels found in file */

    /* read numbers from one line in the file */
    ii     = readint(&ok, inputfile);
    side   = readint(&ok, inputfile);
    sector = readint(&ok, inputfile);
    patch  = readint(&ok, inputfile);
    hwaddr = readint(&ok, inputfile);
    pedValue = roundtoint(readfloat(&ok, inputfile));

    if ( ok & ((side<0) || (side>1)) ) {
      sprintf(desc, "Wrong side number: %d!", side);
      logger(LOG_ERROR, desc);
      return 1;
    }
    if ( ok & ((sector<0) || (sector>17)) ) {
      sprintf(desc, "Wrong sector number: %d!", sector);
      logger(LOG_ERROR, desc);
      return 1;
    }
    if ( ok & ((patch<0) || (patch>5)) ) {
      sprintf(desc, "Wrong rcu number: %d!", patch);
      logger(LOG_ERROR, desc);
      return 1;
    }

    //if (pedValue == 0) continue;

    // Only use the FPED values for this RCU 
    if ( ok && (arguments.rcu == patch) && (arguments.channel == sector) ) {

      if ( !checkFecOn(rcu, (hwaddr>>7)&0x1f) ) continue; // Check if this fec is on
      
      sprintf(desc, "Writing FPED=%d on FEC %d, ALTRO %d, Channel %d, (hardware address %d)!",
	      (int)pedValue, ((decodedAddressBranch(hwaddr)<<4)|decodedAddressFECaddr(hwaddr)),
	      decodedAddressChipaddr(hwaddr), decodedAddressChanneladdr(hwaddr));
      logger(LOG_DEBUG, desc);

      imem[memctr++] = ((0x200000 | (hwaddr<<5)) | 0x6);  // 0x6 -> FPED register adress
      imem[memctr++] =  pedValue&0x3ff;                   // 10bit wide fixed pedestal value
    }

    if ( memctr >= (RCU_INSTRUCTION_MEM_SZ - 4) ) logger(LOG_DEBUG, "IMEM full! ");
    if ( !ok ) logger(LOG_DEBUG, "End of file!");

    if ( (memctr >= (RCU_INSTRUCTION_MEM_SZ - 4)) || !ok ) { // IMEM almost full or finished

      imem[memctr++] = RCU_END;  /* RCU_END instruction */

      /* write to IMEM */
      if ( FEC_IMEM_Write(rcu, imem, RCU_INSTRUCTION_MEM_SZ) ) {
	logger(LOG_ERROR, "FEC_IMEM_Write failed!!");
	return 1;
      }

      memctr = 0;

    } // end if

    if ( !ok ) break;

  } // end while (channel loop)

  // close the file
  fclose(inputfile);

  logger(LOG_DEBUG, "Exiting function writeFPED()");

  return 0;
}

//========================================================================
int32_t config_common(rcu_model_t* rcu) {

  logger(LOG_DEBUG, "Entering function config_common()");

  int32_t ii, retval;
  char desc[256];

  // 1) write ALTROIF
  if ( rcu_writeReg(rcu->key, ALTROIF, ((2<<14) /* CSTB programmable delay */
					| ((arguments.sampling&0x3)<<10)
					| arguments.tb)) ) {
    logger(LOG_ERROR, "Function rcu_writeReg() failed (ALTROIF)!");
    return 1;
  }
  
  // 2) write TRGCONF
  int32_t trg_src = 0x4;                // TTC is default
  if ( arguments.swtrg ) trg_src = 0x1; // software trigger
  if ( rcu_writeReg(rcu->key, TRGCONF, ((trg_src<<14) | (1<<13) /* trigger mode PHOS (on L0)*/
					| 0x1FFF)) ) {
    logger(LOG_ERROR, "Function rcu_writeReg() failed (TRGCONF)!");
    return 1;
  }
  
  // 3) write RDOMOD
  int32_t sparse = arguments.sparse ? 1 : 0; 
  int32_t skip   = arguments.skip   ? 1 : 0; 
  if ( rcu_writeReg(rcu->key, RDOMOD, ((skip<<6) | (0x0<<5) | (0x0<<4) | (0x0<<3) | (sparse<<2) | (0x0<<1) | 0x0)) ) {
    logger(LOG_ERROR, "Function rcu_writeReg() failed (RDOMOD)!");
    return 1;
  }
  
  // 4) Write RCUID
  int32_t rcuid = arguments.rcu;
  if ( rcu_writeReg(rcu->key, RCU_ID, rcuid) ) {
    logger(LOG_ERROR, "Function rcu_writeReg() failed (RCUID)!");
    return 1;
  }

  // 5) write TTC_L1_LATENCY
  if ( rcu_writeReg(rcu->key, L1_LATENCY, ((0x2<<12) | 0x104)) ) {
    logger(LOG_ERROR, "Function rcu_writeReg() failed (TTC_L1_LATENCY)!");
    return 1;
  }
  
  // 6) write TTC_L2_LATENCY
  /*
  if ( rcu_writeReg(rcu->key, L2_LATENCY, ((0x578<<16) | 0x4e20) ) ) {
    logger(LOG_ERROR, "Function rcu_writeReg() failed (TTC_L2_LATENCY)!");
    return 1;
  }
  */

  // 7) Write ZSTHR to all ALTROs in broadcast
  if ( FEC_ALTROREG_Write(rcu, 0, 1, 0x8, (0x0<<10) | arguments.zsthr) ) {
    logger(LOG_ERROR, "Function Function FEC_ALTROREG_Write for ALTRO register ZSTHR (broadcast) failed!");
    return 1;
  }

  // 8) write TTC_L1_MSG_LATENCY
  if ( rcu_writeReg(rcu->key, L1_MSG_LATENCY, ((0xe04<<12) | 0xe20)) ) {
    logger(LOG_ERROR, "Function rcu_writeReg() failed (TTC_L1_LATENCY)!");
    return 1;
  }
      
  // 9) Pedestal RAMP data
  uint32_t imem[RCU_INSTRUCTION_MEM_SZ];
  for ( ii = 0; ii < RCU_INSTRUCTION_MEM_SZ; ii+=4 ) {
    imem[ii+0] = (2<<20) | (1<<18) | 0xD; // PMADD broadcast
    imem[ii+1] = ii/4;                    // time bin 0 ... 1023
    imem[ii+2] = (2<<20) | (1<<18) | 0x7; // PMDTA broadcast
    imem[ii+3] = ii/4;                    // dummy value (ramp)
    sprintf(desc, "0x%X 0x%X 0x%X 0x%X", imem[ii+0], ii/4, imem[ii+2], ii/4);
    logger(LOG_DEBUG, desc);
  }
  //imem[4096] = PEDCONF_RCU_END; // No space. Will be executed anyway.
  if ( FEC_IMEM_Write(rcu, imem, RCU_INSTRUCTION_MEM_SZ) ) { /* Write to rcu */
    logger(LOG_ERROR, "Pedestal memory write (broadcast) failed!");
    return 1;
  }

  // Check
  if ( check_fecerr(rcu) ) {
    logger(LOG_ERROR, "Function check_fecerr() failed!");
    return 1;
  }

  logger(LOG_DEBUG, "Exiting function config_common()");
  
  return 0;
}
  
//========================================================================
int32_t config_dpcfX(rcu_model_t* rcu) {

  logger(LOG_DEBUG, "Entering function config_dpcfX()");

  int32_t ii, retval;
  char desc[256];

  // Write DPCF2 [ PWSV ]
  int32_t pwsv = (arguments.pwsv) ? 1 : 0;
  if ( FEC_ALTROREG_Write(rcu, 0, 1, 0xC, (pwsv<<6)) ) {
    logger(LOG_ERROR, "Configure DPCF2 (broadcast) failed!");
    return 1;
  }

  // Write DPCFG [ ZS and BC1 config ]
  if ( FEC_ALTROREG_Write(rcu, 0, 1, 0xB, (arguments.zsen<<19) | (arguments.pres<<17) | (arguments.posts<<14)
			  | (arguments.gf<<12) | arguments.bc1cfg) ) {
    logger(LOG_ERROR, "Configure DPCFG (broadcast) failed!");
    return 1;
  }
  
  logger(LOG_DEBUG, "Exiting function config_dpcfX()");
  
  return 0;
}
  
//========================================================================
int32_t config_rolm_channel(rcu_model_t* rcu) {

  logger(LOG_DEBUG, "Entering function config_rolm_channel()");

  char desc[256];
  int32_t afl, ff, aa, cc, ii;

  int32_t ctra = 0;
  int32_t ctrb = 0;

  uint32_t mema[RCU_READOUT_MEM_SZ];
  uint32_t memb[RCU_READOUT_MEM_SZ];

  // Reset ROLMs
  for ( ii = 0; ii < RCU_READOUT_MEM_SZ; ii++ ) {
    mema[ii] = 0xfff;
    memb[ii] = 0xfff;
  }

  for ( ff = 0; ff<32; ff++ ) { // FEC loop

    if ( checkFecOn(rcu, ff) ) { // Check FEC on

      sprintf(desc, " -> Configure FEC%02d ..", ff);
      logger(LOG_INFO, desc);
      
      for ( aa = 0; aa<8; aa++ ) {

	// ALTRO global registers
	int32_t data, ii;
	int32_t hw_add = codeAddress(ff, aa, 0);
	int32_t chip_add;

	// Write TRCFG
	sprintf(desc, "Configure TRCFG register for FEC %02d ALTRO %02d", ff, aa);
	logger(LOG_DEBUG, desc);

	if      ( (aa == 0) || (aa == 2) ) data = ((arguments.acqstart+0)<<10) | arguments.tb;
	else if ( (aa == 1) || (aa == 3) ) data = ((arguments.acqstart+1)<<10) | arguments.tb;
	else if ( (aa == 4) || (aa == 6) ) data = ((arguments.acqstart+2)<<10) | arguments.tb;
	else if ( (aa == 5) || (aa == 7) ) data = ((arguments.acqstart+3)<<10) | arguments.tb;

	if ( FEC_ALTROREG_Write(rcu, hw_add, 0, 0xA, data) ) {
	  sprintf(desc, "Configure TRCFG on FEC %d ALTRO failed!", ff, aa);
	  logger(LOG_ERROR, desc);
	  return 1;
	}

	uint32_t imem[RCU_INSTRUCTION_MEM_SZ];    

	for ( cc = 0; cc<16; cc++ ) {

	  hw_add   = codeAddress(ff, aa, cc);

	  if ( arguments.pca16 ) {

	    if ( cc > 7 ) {  // Write pedestal ramp data for channels 8 .. 15

	      sprintf(desc, "Configure pedestal memory for FEC %02d ALTRO %02d Channel %02d", ff, aa, cc);
	      logger(LOG_DEBUG, desc);

	      chip_add = (hw_add>>4) & 0xff; // branch, FEC and ALTRO bits

	      for ( ii = 0; ii < RCU_INSTRUCTION_MEM_SZ; ii+=4 ) {
		imem[ii+0] = (2<<20) | (chip_add<<9) | 0xD;  // PMADD
		imem[ii+1] = ii/4;                           // time bin 0 ... 1023
		imem[ii+2] = (2<<20) | (hw_add<<5)   | 0x7;  // PMDTA broadcast
		imem[ii+3] = 1023-ii/4;                      // anti ramp
		//sprintf(desc, "0x%X 0x%X 0x%X 0x%X", imem[ii+0], ii/4, imem[ii+2], ii/4);
		//logger(LOG_DEBUG, desc);
	      }
	      //imem[4096] = PEDCONF_RCU_END; // Will be executed anyway.
	      if ( FEC_IMEM_Write(rcu, imem, RCU_INSTRUCTION_MEM_SZ) ) { /* Write to rcu */
		sprintf(desc, "Pedestal memory write for FEC %d ALTRO %d Channel %d failed!", ff, aa, cc);
		logger(LOG_ERROR, desc);
		return 1;
	      }

	    } // end if channel > 7

	  } // end if pca16

	  //
	  // 3) ROLM
	  //
	  sprintf(desc, "Adding hw address 0x%X to ROLM!", hw_add);
	  logger(LOG_DEBUG, desc);
	  if ( (hw_add>>11)&0x1 ) memb[ctrb++] = hw_add; // Branch B
	  else                    mema[ctra++] = hw_add; // Branch A

	} // end channel loop

      } // end chip loop

    } // end FEC on check

  } // end fec loop

  // Fill ROLM
  if ( FEC_ROLMEM_Write(rcu, mema, memb) ) {
    logger(LOG_ERROR, "Function FEC_ROLMEM_Write() failed!");
    return 1;
  }

  if ( check_fecerr(rcu) ) {
    logger(LOG_ERROR, "Function check_fecerr() failed!");
    return 1;
  }

  logger(LOG_DEBUG, "Exiting function config_rolm_channel()");

  return 0;
}

//========================================================================
int32_t config_pca16(rcu_model_t* rcu, int32_t enable, int32_t gain, int32_t shaping,
		     int32_t shutdown, int32_t polarity, int32_t dac_value) {

  logger(LOG_DEBUG, "Entering function config_pca16()");

  char desc[256];
  int32_t retval, data1, data2;

  /*
      The SPI chain is programmed with three 16 bit words

      word1:
      bits 0-3:   Command to DAC
      bits 4-11:  Shift register bits
      bits 12-15: Command to interpreter in FPGA - 0x9 clock on and load register1
      
      word2:
      bits 0-11:  DAC value (default 500 according to Ulf)
      bits 12-15: Command to interpreter in FPGA - 0xA clock on and load register2
      
      word3
      bits 0-11:  Set to 0
      bits 12-15: Command to interpreter in FPGA - 0xC clock on and send data
      
      Command to FPGA
      bit 12: load register 1
      bit 13: load register 2
      bit 14: send data
      bit 15: clock on/off
  */

  /* Shift register bits */
  /* bit 0: preamp enable */
  /* bit 1-2: gain */
  /* bit 3-5: shaper */
  /* bit 6: shutdown */
  /* bit 7: polarity */

  sprintf(desc, "Polarity: %d", polarity);
  logger(LOG_DEBUG, desc);
  sprintf(desc, "Shutdown: %d", shutdown);
  logger(LOG_DEBUG, desc);
  sprintf(desc, "Shaping: %d", shaping);
  logger(LOG_DEBUG, desc);
  sprintf(desc, "Gain: %d", gain);
  logger(LOG_DEBUG, desc);
  sprintf(desc, "Enable: %d", enable);
  logger(LOG_DEBUG, desc);

  int32_t shiftval = (polarity&1)<<7 | (shutdown&1)<<6 | (shaping&7)<<3 | (gain&3)<<1 | (enable&1);

  data1 = shiftval<<4;       // Shift register content
  data1 |= 0x9000;           // Clock on & load into register 1
  data1 |= 0xC;              // DAC command: load all 8 DAC with same value

  sprintf(desc, "Data1: %d", data1);
  logger(LOG_DEBUG, desc);

  data2 = dac_value & 0xFFF; // DAC value
  data2 |= 0xA000;           // clock on & load into register 2
  
  // FEC BC register 0x10 used for setting PCA16 parameters (shift register and DAC, programmed with SPI)
  int32_t reg = 0x10;

  // Before start sending data the SPI clock should be turned on using command 0x8000
  logger(LOG_DEBUG, "Set clock to ON");
  retval = FEC_BCREG_Write(rcu, 0, 1, reg, 0x8000);
    
  // The DAC is powered up by command 0xF. All DAC bits set and D03-D00 = 11xx
  logger(LOG_DEBUG, "Waking up DAC");   
  retval = FEC_BCREG_Write(rcu, 0, 1, reg, 0x900F);
  retval = FEC_BCREG_Write(rcu, 0, 1, reg, 0xAFFF);
  retval = FEC_BCREG_Write(rcu, 0, 1, reg, 0xC000); // send
 
  // After a 'send data' the program must wait for a while before starting the next sequence.
  usleep(100000);

  // Write data
  logger(LOG_DEBUG, "Writing data");  
  retval = FEC_BCREG_Write(rcu, 0, 1, reg, data1 );
  retval = FEC_BCREG_Write(rcu, 0, 1, reg, data2 );
  retval = FEC_BCREG_Write(rcu, 0, 1, reg, 0xC000); // send
  
  // After a 'send data' the program must wait for a while before starting the next sequence.
  usleep(100000);

  // When finished the SPI clock must be turned off using command 0x0 to avoid clock noise
  logger(LOG_DEBUG, "Set clock to OFF");
  retval = FEC_BCREG_Write(rcu, 0, 1, reg, 0x0);
    
  logger(LOG_DEBUG, "Exiting function config_pca16()");

  return 0;
}

#endif // PCA16_H