#ifndef TPCPEDCONFIG_H
#define TPCPEDCONFIG_H

#include <math.h>
#include <mysql.h>
#include <stdio.h>
#include <stdint.h>

#define DB_DATE_HOST getenv("DATE_DB_MYSQL_HOST")
#define DB_DATE_USER getenv("DATE_DB_MYSQL_USER")
#define DB_DATE_PASS getenv("DATE_DB_MYSQL_PWD")
#define DB_DATE_DB   getenv("DATE_DB_MYSQL_DB")
#define DB_DATE_NULL   (DB_DATE_HOST==NULL || DB_DATE_USER==NULL || DB_DATE_PASS==NULL || DB_DATE_DB==NULL)
#define DB_DATE_PARAMS  DB_DATE_HOST, DB_DATE_USER, DB_DATE_PASS, DB_DATE_DB, 3306, NULL, 0

/* query size */
#define PEDCONF_QUERY_BUFFER_SIZE 8192

/* return codes */
#define DB_OK            0
#define DB_PARAMETERS   -1
#define DB_CONNECTION   -2
#define DB_QUERY        -3
#define DB_RETURN       -4
#define DB_EMPTY        -5

/* Internal commands */
#define PEDCONF_CLEAR_ALL                   1
#define PEDCONF_FECS_OFF                    2
#define PEDCONF_WRITE_RAMP_BROADCAST        3
#define PEDCONF_WRITE_TESTPATTERN_BROADCAST 4
#define PEDCONF_WRITE_TESTPATTERN           5
#define PEDCONF_WRITE_PATTERN               6
#define PEDCONF_WRITE_PATTERN2              7
#define PEDCONF_WRITE_FPED                  8
#define PEDCONF_WRITE_ZSTHR                 9
#define PEDCONF_WRITE_BCTHR                10

/* more */
#define PEDCONF_RCU_END 0x380000

//========================================================================
//========================================================================
//========================================================================

// Main logging function
void log_main(const char severity, char* msg);


//
// ArgP functions
//

struct arguments arguments;

//========================================================================
// Stuff for ARGP

const char *argp_program_version = "TPCpedConfig.app " PROG_VERSION;
const char *argp_program_bug_address = "<christian.lippmann@cern.ch>";

static char doc[] = "This program loads data into ALTROs from text files via the DDL.\
  \vRun on LDC to write pedestal data or threshold values into ALTROs. \
This program uses the fec2rorc library provided with Date. \
On an LDC the rorc information is taken from the DAQ DB). On other \
computers you must specify either the rorc serial number or the minor number. \
\n==================================================================\
\nExample usage: TPCpedConfig.app --fpedfile=bla.dat --minor=4 --channel=0\
\n               TPCpedConfig.app --patternfile=tpcPedestalMem.data\
\n==================================================================";

static char args_doc[] = "";

#define OPT_OFF 1
#define OPT_CLR 2

//========================================================================
// The options ARGP understands
static struct argp_option options[] = {
  {"ramp",         'R', 0,                0, "Write test data to pedestal memory",                                              1 },
  {"patternfile",  'P', "FILE",           0, "Write data from FILE to pedestal memory",                                         1 },
  {"pedmemfile",    0 , "FILE",           OPTION_ALIAS },
  {"patternfile2", 'Q', "FILE",           0, "Write data from FILE (FPED format) to pedestal memory",                           1 },
  {"pedmemfile2",   0 , "FILE",           OPTION_ALIAS },
  {"testpattern",  't', "OCCUPANCY",      0, "Write test pattern to pedestal memory in broadcast (Specify OCCUPANCY [0..100])", 1 },
  {"testpattern2", 'T', "OCCUPANCY",      0, "Write test pattern to pedestal memory channel by channel (OCCUPANCY [0..100])",   1 },
  {"fpedfile",     'F', "FILE",           0, "Write fixed pedestal data from FILE to ALTROs",                                   1 },
  {"bcthrfile",    'B', "FILE",           0, "Write BCTHR data from FILE to ALTROs",                                            1 },
  {"zsthrfile",    'Z', "FILE",           0, "Write ZSTHR data from FILE to ALTROs",                                            1 },
  {"noconf",       'C', 0,                0, "Dont configure RCU/ALTROs for readout of pedestal memories (default=do it)",      1 },
  {"noreset",      'N', 0,                0, "Do NOT reset RORC, DIU and SIU before starting the configuration",                1 },
  {"rev",          'r', "REVNUM",         0, "Specify rorc revision (not needed on LDC)",                                       2 },
  {"serial",       's', "SERIAL",         0, "Specify rorc serial (not needed on LDC)",                                         2 },
  {"minor",        'm', "MINOR" ,         0, "Specify minor instead of revision and serial (not needed on LDC)",                2 },
  {"channel",      'c', "CHNUM",          0, "Specify rorc channel (not needed on LDC)",                                        2 },
  {"rcu",          'p', "RCUNUM",         0, "Specify rcu (not needed on LDC, mandatory otherwise)",                            2 },
  {"partition",     0 ,       0 ,         OPTION_ALIAS },
  {"offset",       'o', "OFFSET",         0, "Specify ADC offset (for ZSTHR, default=0)",                                       3 },
  {"factor",       'b', "FACTOR",         0, "Specify multiplication factor for noise (for ZSTHR, BCTHR, default=3.0)",         3 },
  {"min",          'k', "THRVAL",         0, "Specify minimum threshold (for ZSTHR, BCTHR, default=2)",                         3 },
  {"max",          'l', "THRVAL",         0, "Specify maximum threshold (for ZSTHR, BCTHR, default=10)",                        3 },
  {"mintb",        'K', "TIMEBIN",        0, "Specify minimum time bin (for pedestal memory configuration)",                    3 },
  {"maxtb",        'L', "TIMEBIN",        0, "Specify maximum time bin (for pedestal memory configuration)",                    3 },
  {"add",          'A', "NUMBER",         0, "Add a value to pedestal values (FPED or pedestal memory)",                        3 },
  {"fecs",         'f', "FEC1[,FEC2...]", 0, "Use these FECs (0..31) Adress includes branch. First FEC in branch B is 16",      3 },
  {"off",          OPT_OFF, 0,            0, "Do nothing except switch off all FECs",                                           4 },
  {"clear",        OPT_CLR, 0,            0, "Do nothing except clearing RCU Status and Error registers",                       4 },
  { 0 }
};

//========================================================================
// Used by main to communicate with ARGP parse_opt
struct arguments {
  char *input_file;
  int32_t rev, serial, minor, channel, rcu, ramp, command;
  bool on_ldc, rdo_conf, reset, active_fecs;
  int32_t occupancy, offset, min, max, mintb, maxtb;
  float factor, add;
  uint32_t fecs;
};

//========================================================================
//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;

  switch ( key ) {   // Parse a single option

  case 'R':
    arguments->command = PEDCONF_WRITE_RAMP_BROADCAST;
    break;

  case 'P':
    arguments->command = PEDCONF_WRITE_PATTERN;
    arguments->input_file = arg;
    break;

  case 'Q':
    arguments->command = PEDCONF_WRITE_PATTERN2;
    arguments->input_file = arg;
    break;

  case 't':
    arguments->command = PEDCONF_WRITE_TESTPATTERN_BROADCAST;
    arguments->occupancy = atoi(arg);
    if (arguments->occupancy < 0  ) arguments->occupancy = 0;
    if (arguments->occupancy > 100) arguments->occupancy = 100;
    break;

  case 'T':
    arguments->command = PEDCONF_WRITE_TESTPATTERN;
    arguments->occupancy = atoi(arg);
    if (arguments->occupancy < 0  ) arguments->occupancy = 0;
    if (arguments->occupancy > 100) arguments->occupancy = 100;
    break;

  case 'C':
    arguments->rdo_conf = false;
    break;

  case 'N':
    arguments->reset = false;
    break;

  case 'F':
    arguments->command = PEDCONF_WRITE_FPED;
    arguments->input_file = arg;
    break;

  case 'B':
    arguments->command = PEDCONF_WRITE_BCTHR;
    arguments->input_file = arg;
    break;

  case 'Z':
    arguments->command = PEDCONF_WRITE_ZSTHR;
    arguments->input_file = arg;
    break;

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

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

  case 'm':
    arguments->minor = atoi(arg);
    arguments->on_ldc = false;
    arguments->rev = -1;
    break;

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

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

  case 'o':
    arguments->offset = atoi(arg);
    break;

  case 'b':
    arguments->factor = atof(arg);
    break;

  case 'k':
    arguments->min = atoi(arg);
    break;

  case 'l':
    arguments->max = atoi(arg);
    break;

  case 'K':
    arguments->mintb = atoi(arg);
    if (arguments->mintb<0   ) arguments->mintb=0;
    if (arguments->mintb>1024) arguments->mintb=1024;
    break;

  case 'L':
    arguments->maxtb = atoi(arg);
    if (arguments->maxtb<0   ) arguments->maxtb=0;
    if (arguments->maxtb>1024) arguments->maxtb=1024;
    break;

  case 'A':
    arguments->add = atof(arg);
    break;

  case OPT_OFF:
    arguments->command = PEDCONF_FECS_OFF;
    arguments->active_fecs = false;
    arguments->fecs = 0x0;
    break;

  case OPT_CLR:
    arguments->command = PEDCONF_CLEAR_ALL;
    break;

  case 'f':
    arguments->active_fecs = 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);
        log_main(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 };

//
// RCU model
//

//========================================================================
typedef struct {
  int32_t id;                // database   
  int32_t eqid;              // database
  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 pmem_retry_count;  // count IMEM restarts due to buggy -512 error
  FILE   *logfile;
} 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->pmem_retry_count = 0;
  temp->key = NULL;
  
  return temp;
}

//========================================================================
void free_rcu_model(rcu_model_t* rcu) {
  log_main(LOG_DEBUG, "Deallocate the RCU models");
  if ( rcu->logfile ) fclose(rcu->logfile);
  free(rcu);
  log_main(LOG_DEBUG, "Successfully deallocated the RCU models");
}

//
// Logging functions
//

void log_main(const char severity, char* msg) {
  // 
  char str[256];
  if ( severity != LOG_DEBUG ) {
    infoLogger_msg_xt(__FILE__, __LINE__, 0, LOG_TPCPEDCONFIG, severity,
		      INFOLOGLEVEL_OPS, msg);
    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);
  } else {
    sprintf(str, "DEBUG   : %s", msg);
  }
  // log also to logfile
  if (mainlogfile) {
    fprintf(mainlogfile, str);
    fprintf(mainlogfile, "\n");
  }
}


//========================================================================
void log_rorc(rcu_model_t* rcu, const char severity, char* msg) {
  // 
  char str1[256], str[256];
  sprintf(str1, "EqID=%d | %s", rcu->eqid, msg);
  if ( severity != LOG_DEBUG ) {
    infoLogger_msg_xt(__FILE__, __LINE__, 0, LOG_TPCPEDCONFIG, severity,
		      INFOLOGLEVEL_OPS, str1);
    if      ( severity == LOG_INFO    ) sprintf(str, "INFO    : %s", str1);
    else if ( severity == LOG_WARNING ) sprintf(str, "WARNING : %s", str1);
    else if ( severity == LOG_ERROR   ) sprintf(str, "ERROR   : %s", str1);
    else if ( severity == LOG_FATAL   ) sprintf(str, "FATAL   : %s", str1);
  } else {
    sprintf(str, "DEBUG   : %s", str1);
  }
  // log also to logfile
  if (mainlogfile) {
    fprintf(rcu->logfile, str);
    fprintf(rcu->logfile, "\n");
  }
}


//
// Misc functions
//

//========================================================================
int32_t getPatchFromEquipmentID(int32_t equipmentID) {
  // Get rcu (patch) index (0 ... 5) from equipment ID

  int32_t retval = 0;

  if ( (equipmentID < (3<<8)) || (equipmentID > 983) ) {
    char desc[256];
    sprintf(desc, "Equipment ID %d is outside valid range!", equipmentID);
    log_main(LOG_ERROR, desc);
    return -1;
  }
  if ( ( (int)equipmentID - 840 ) < 0) retval = (equipmentID-768)%2;
  else                                 retval = (equipmentID-840)%4 + 2;
  return retval;
}

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

//========================================================================
void filltestarray(uint32_t *pedestal_data, int32_t occupancy, int32_t offset) {
  //
  // Fill an array with random stuff
  //
  int32_t i, j, pos;
  int32_t base = 10;
  int32_t fillval = 50;
  for ( i = 0; i < occupancy; i++ ) {
    pos = offset+base*(rand()%100);
    if ( pedestal_data[pos] < 1 ) {
      // These bins are still empty. Fill them
      for (j=0; j<base; j++) pedestal_data[pos+j] = fillval;
    } else {
      // These bins are full. Try with next set of bins until ok.
      do {
        pos+=base;
        if ( pos >= (base*100+offset) ) pos=offset; // start from beginning
      } while ( pedestal_data[pos] > (fillval-1) );
      // Found an empty slot. Fill it
      for ( j=0; j<base; j++ ) pedestal_data[pos+j] = fillval;
    }
  }
}

//========================================================================
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);
}


//
// Functions converting hardware address
//

//========================================================================
int32_t codeAddress(int32_t Branch,  int32_t FEC,  int32_t chip, int32_t channel) {
  return ((Branch&1)<<11) + ((FEC&0xf)<<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));
}

//
// MySql db functions
//

char hostname[255];

//========================================================================
int32_t get_eqid(int32_t *eqid_array, int32_t array_size) {

  /*   connect to DATE database
   *   query the eqIds for this LDC
   *   load the RCU model with eqid_array */

  log_main(LOG_DEBUG, "Entering function get_eqid()");

  int32_t count;
  int32_t num_rows;
  
  MYSQL db;
  char query[PEDCONF_QUERY_BUFFER_SIZE];
  MYSQL_RES *res;
  MYSQL_ROW row;

  if (DB_DATE_NULL) return DB_PARAMETERS;

  log_main(LOG_DEBUG, "Function get_eqid(): mysql_init");
  mysql_init(&db);

  log_main(LOG_DEBUG, "Function get_eqid(): mysql_connect");
  if (!mysql_real_connect(&db, DB_DATE_PARAMS)) {
    log_main(LOG_ERROR, (char*)mysql_error(&db));
    return DB_CONNECTION;
  } 

  /* query */
  memset(query, '\0', PEDCONF_QUERY_BUFFER_SIZE*sizeof(char));
  snprintf(query, PEDCONF_QUERY_BUFFER_SIZE,
	   "select EqId from EQUIP_PARAM_RorcData where "
	   "ldc in (select NAME from ROLES where HOSTNAME like '%s' and ROLE = 'LDC') "
	   "and ACTIVE = 1;", hostname);
  char desc[256];
  sprintf(desc, "Function get_eqid() QUERY=%s", query);
  log_main(LOG_DEBUG, desc);
  if (mysql_real_query(&db,query,strlen(query))) {
    log_main(LOG_ERROR, (char*)mysql_error(&db));
    mysql_close(&db);
    return DB_QUERY;
  }
  
  /* result: several rows where [0] = eqId */
  log_main(LOG_DEBUG, "Function get_eqid(): mysql_store_result");
  res = mysql_store_result(&db);
  num_rows = mysql_num_rows(res);
  if (num_rows > array_size) {      // no copying of eqIds
    mysql_free_result(res);
    mysql_close(&db);
    return num_rows; 
  }
  log_main(LOG_DEBUG, "Function get_eqid(): mysql_fetch_row");
  row = mysql_fetch_row(res);
  for (count = 0; row != NULL; row = mysql_fetch_row(res)) {
    if (row[0] == NULL) {
      mysql_free_result(res);
      mysql_close(&db);
      return DB_RETURN;
    }
    eqid_array[count] = strtol(row[0], (char **)NULL, 10);
    count++;
  }

  log_main(LOG_DEBUG, "Function get_eqid(): mysql_free_result");
  mysql_free_result(res);
  log_main(LOG_DEBUG, "Function get_eqid(): mysql_close");
  mysql_close(&db);

  log_main(LOG_DEBUG, "Function get_eqid() finished ok");

  return count;
}

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

/*   connect to DATE database   
 *   query the RORC for this eqEd
 *   load the RCU model with ddl_revision, ddl_serial, ddl_channel */

  log_main(LOG_DEBUG, "Entering function get_ddl_details()");

  MYSQL db;
  char query[PEDCONF_QUERY_BUFFER_SIZE];
  MYSQL_RES *res;
  MYSQL_ROW row;

  if (DB_DATE_NULL) {
    return DB_PARAMETERS;
  }
  log_main(LOG_DEBUG, "Function get_ddl_details(): mysql_init");
  mysql_init(&db);
  log_main(LOG_DEBUG, "Function get_ddl_details(): mysql_real_connect");
  if (!mysql_real_connect(&db, DB_DATE_PARAMS)) {
    log_main(LOG_ERROR, (char*)mysql_error(&db)); 
    return DB_CONNECTION;
  }
  
  /* query */
  memset(query, '\0', PEDCONF_QUERY_BUFFER_SIZE*sizeof(char));
  snprintf(query, PEDCONF_QUERY_BUFFER_SIZE, 
	   "select rorcRevision,rorcSerialNb,rorcChannelNb from EQUIP_PARAM_RorcData where EqId=%d;",
	   rcu->eqid);
  char desc[256];
  sprintf(desc, "Function get_ddl_details(): QUERY=%s", query);
  log_main(LOG_DEBUG, desc);

  log_main(LOG_DEBUG, "Function get_ddl_details(): mysql_real_query");
  if (mysql_real_query(&db, query, strlen(query))) {
    log_main(LOG_ERROR, (char*)mysql_error(&db));
    mysql_close(&db);
    return DB_QUERY;
  }

  /* result: one row where [0] = revision, [1] = serial, [2] = channel */
  log_main(LOG_DEBUG, "Function get_ddl_details(): mysql_store_result");
  res = mysql_store_result(&db);
  log_main(LOG_DEBUG, "Function get_ddl_details(): mysql_fetch_row");
  row = mysql_fetch_row(res);
  if (row == NULL) {
    mysql_free_result(res);
    mysql_close(&db);
    return DB_EMPTY;
  } else {
    if ((row[0] == NULL) || (row[1] == NULL) || (row[2] == NULL)) {
      mysql_free_result(res);
      mysql_close(&db);
      return DB_RETURN;
    }
    rcu->ddl_revision = strtol(row[0], (char **)NULL, 10);
    rcu->ddl_serial   = strtol(row[1], (char **)NULL, 10);
    rcu->ddl_channel  = strtol(row[2], (char **)NULL, 10);
  }

  log_main(LOG_DEBUG, "Function get_ddl_details(): mysql_free_result");
  mysql_free_result(res);
  log_main(LOG_DEBUG, "Function get_ddl_details(): mysql_close");
  mysql_close(&db);

  log_main(LOG_DEBUG, "Function get_ddl_details() finished ok");

  return DB_OK;
}


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

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

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

  log_rorc(rcu, LOG_DEBUG, "Read_ACTFECLIST() successful");

  return 0;

}

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

  log_rorc(rcu, LOG_DEBUG, "Calling waitBusBusy()");

  uint32_t busbusy = 1;
  uint32_t tcount = 0;
  while ( busbusy&0x1 ) {
    usleep(2000);
    if ( rcu_readReg(rcu->key, RCUBUSBUSY, &busbusy) != 1 ) {
      log_rorc(rcu, 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);
      log_rorc(rcu, LOG_DEBUG, desc);
      if (tcount == 100) {
	log_rorc(rcu, LOG_ERROR, "RCU bus still busy after 100 iterations");
	return 1;
      }
      tcount++;   
    }
  }

  log_rorc(rcu, 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];

  log_rorc(rcu, 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);
    log_rorc(rcu, LOG_DEBUG, desc);
    if (retval != size) {
      sprintf(desc, "Function rcu_readMem() failed. %d words expected, %d words read\n",
	      size, retval);
      log_rorc(rcu, LOG_ERROR, desc);
      return 1;
    }

    // check all the words
    log_rorc(rcu, 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 %X, %X", index,
		(int)buffer_result[index], (int)buffer_result[index+1]);
	log_rorc(rcu, LOG_ERROR, desc);
	break;
      }
    }

    // execute again IMEM
    if ( check == 1) {
      log_rorc(rcu, LOG_INFO, "Executing INSTRUCTION MEM once again");
      if ( fec_execIMEM(rcu->key) ) return 1;
    }

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

  } // end while (check == 1)

  // check if in the IMEM the words have been uploaded correctly
  if (check == 1) {
    log_rorc(rcu, 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);
    log_rorc(rcu, LOG_DEBUG, desc);
    if (retval != size) {
      sprintf(desc, "RCU_Mem_Read failed. %d words expected, %d words read\n",
	      size, retval);
      log_rorc(rcu, 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); 
	log_rorc(rcu, LOG_ERROR, desc);
	return 1;
      }
    }
    log_rorc(rcu, LOG_INFO, "Comparison of IMEM ok");
    return 1;
  } // end if failure

  log_rorc(rcu, LOG_DEBUG, "Check of RESULT MEM ok");

  return 0;

}

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

  if ( rcu_writeMem(rcu->key, RCU_INSTRUCTION_MEM, imem, size) ) {
    log_rorc(rcu, 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;

  log_rorc(rcu, LOG_DEBUG, "FEC_IMEM_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
  //

  log_rorc(rcu, LOG_DEBUG, "Calling FEC_ALTROREG_Write()");

  /* Prepare Instruction Sequence for the IMEM */
  uint32_t address = (bcast<<18) | (0<<17) | (hw_add<<5) | reg_add;
  uint32_t imem[3];
  imem[0] = (2<<20) | address;
  imem[1] = reg_data;
  imem[2] = PEDCONF_RCU_END;
  char desc[256];
  sprintf(desc, "FEC_ALTROREG_Write(): imem[0]=0x%x imem[1]=0x%x imem[2]=0x%x",
	  (int)imem[0], (int)imem[1], (int)imem[2]);
  log_rorc(rcu, LOG_DEBUG, desc);
  /* Write Instruction Sequence into the IMEM */
  if ( FEC_IMEM_Write(rcu, imem, 3) ) {
    log_rorc(rcu, LOG_ERROR, "Function FEC_ALTROREG_Write(): Writing and executing IMEM failed!");
    return 1;
  }

  log_rorc(rcu, LOG_DEBUG, "Function FEC_ALTROREG_Write() successful");
  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];

  log_rorc(rcu, LOG_DEBUG, "Calling Write_ACTFECLIST()");

  // Read AFL
  retval = rcu_readReg(rcu->key, ACTFECLIST, &active_fecs);
  if ( retval != 1 ) {
    log_rorc(rcu, 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);
	log_rorc(rcu, LOG_DEBUG, desc);
        active_fecs &= ~(1<<fec);
	if ( rcu_writeReg(rcu->key, ACTFECLIST, active_fecs) ) {
	  log_rorc(rcu, 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);
	log_rorc(rcu, LOG_DEBUG, desc);
        active_fecs |= 1<<fec;
	if ( rcu_writeReg(rcu->key, ACTFECLIST, active_fecs) ) {
	  log_rorc(rcu, LOG_ERROR, "Function Write_ACTFECLIST(): rcu_writeReg failed (ACTFECLIST)");
	  return 1;
	}
        usleep(200000);
      }
  }

  log_rorc(rcu, 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);
    log_rorc(rcu, LOG_DEBUG, desc);
    return true;
  } else {
    sprintf(desc, "Function checkFecOn(): Fec with address %d is OFF!", fecaddr);
    log_rorc(rcu, LOG_INFO, desc);
    return false;
  } 
  return false;
}


//
// 6) Rorc functions
//

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

  char desc[256];

  log_rorc(rcu, 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);
  log_rorc(rcu, LOG_DEBUG, desc);
  usleep(1000); 

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

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

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

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

  log_rorc(rcu, 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);
    log_rorc(rcu, 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);
      log_rorc(rcu, 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);
    log_rorc(rcu, LOG_DEBUG, desc);
  }

  return 0;
}

#endif // TPCPEDCONFIG_H