#include <unistd.h>
#include <sys/types.h>
#include <sys/mman.h>
#include <stdio.h>
#include <fcntl.h>
#include <unistd.h>
#include <stdlib.h>
#include <string.h>
#include <time.h>
//#include <math.h>

#include "abi_functions.h"
#include "rcu2_functions.h"

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

//========================================================================
float readfloat(int *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);
}

//========================================================================
int readint(int *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);
}

//==========================================================================================
// Used by main to communicate with ARGP parse_opt
struct args {
  int verbose, readrp;
  unsigned int rp, sampling, offset, zs_en, zsthr, acq_start, acq_end, gf, sparse, skip, sort;
  unsigned int act_fecs, all_fecs, off, fecsA, fecsB, trig, setwidth, pulsewidth,steppedestal;
};

//==========================================================================================
void print_usage() {
  printf("Usage: cfg_fped [OPTION]... FILENAME\n");
  printf("Purpose: Configure RCU2 and FECs using fixed pedestal data from file\n");
  printf("Arguments: Filename with fped input data must be given.");
  printf("Options:\n");
  printf("-v[LEVEL]   : Set verbosity level (default is 1)\n");
  printf("-p[0..5]    : Use this partition number (0...5. Default: Read from RCU2 register)\n");
  printf("-o          : Switch off FECs, do nothing else\n");
  printf("-f[FECLIST] : Use these FECs (comma separated list, no spaces. default: Use active FECs)\n");
  printf("-t[NUM]     : Start reading data from time bin NUM (default=50)\n");
  printf("-e[NUM]     : Stop reading data after time bin NUM (default=1023)\n");
  printf("-z[NUM]     : Use zero supression with threshold NUM (default=0)\n");
  printf("-g[NUM]     : Use glitch filter (default setting 2, applies only if ZS is on)\n");
  printf("-O[NUM]     : Add offset to ADC values (default=0)\n");
  printf("-s[NUM]     : Set sampling frequency (0=10MHz=default, 1=5MHz, 2=2.5MHz, 3=20MHz)\n");
  printf("-l[LEVEL]   : Set trigger level to start ALTROs (L1=default or L0)\n");
  printf("-w[WIDTH]   : Set Trigger pulse (L1 & L2) width (default = automatically determined value)\n");
  printf("-d          : Activate dat sorting (off by default)\n");
  printf("-x[0/1/2]   : 0 = Full readout (default), 1 = Full readout with channel skipping, 2 = sparse readout\n");
  printf("-m          : Set stepwise Pedestal for meb sparse test (130/170/210/240/290/340/400) (default off)\n");
}

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

  struct args args;

  // Default values
  args.verbose     = 1;
  args.readrp      = 1;
  args.rp          = 999;
  args.zs_en       = 0;
  args.zsthr       = 0;
  args.acq_start   = 50;
  args.acq_end     = 1023;
  args.gf          = 2;
  args.offset      = 0;
  args.act_fecs    = 1;
  args.all_fecs    = 0;
  args.fecsA       = 0;
  args.fecsB       = 0;
  args.off         = 0;
  args.sampling    = 0;
  args.trig        = 1;  // Start acquisition on L1
  args.setwidth    = 0;
  args.pulsewidth  = 0;
  args.skip        = 0;
  args.sparse      = 0;
  args.steppedestal =0;
  char *allch = "all";

  char filename[256];

  int val = 0;

  while ( 1 ) {
    int result = getopt(argc, argv, "odhv:p:z:O:t:s:e:g:f:l:w:x:m:");
    if (result == -1) break; /* end of list */
    switch (result)
      {
      case '?': /* unknown parameter */
	fprintf(stderr, "ERROR: Unknown parameter\n");
        exit(EXIT_FAILURE);
      case ':': /* missing argument of a parameter */
        fprintf(stderr, "ERROR: Missing argument.\n");
        exit(EXIT_FAILURE);
      case 'v':
        args.verbose = atoi(optarg); //
	printf("Verbose Mode: %i", args.verbose);
        break;
      case 'h':
        print_usage();
        exit(EXIT_SUCCESS);
      case 'p':
        // Partition [0..5]
        args.readrp = 0;
        args.rp = atoi(optarg);
	if ( args.rp>5 ) {
	  printf("Invalid argument: no such partition: %i", args.rp);
	  exit(EXIT_FAILURE);
	}
        break;
      case 'd':
	args.sort = 1;
	break;
      case 'z':
        args.zs_en = 1;
        args.zsthr = atoi(optarg);
        break;
      case 'O':
        args.offset = atoi(optarg);
        break;
      case 't':
        args.acq_start = atoi(optarg);
        break;
      case 'e':
        args.acq_end = atoi(optarg);
        break;
      case 'g':
        args.gf = atoi(optarg);
        break;
      case 'o':
        args.off = 1;
        break;
      case 'l':
        args.trig = atoi(optarg);
        break;
      case 'm':
	args.steppedestal = 1;//
	printf("Set step pedestals\n");
	break;
      case 'x':
	val = atoi(optarg);
	if (val == 0) { args.skip = 0; args.sparse = 0; }
	if (val == 1) { args.skip = 1; args.sparse = 0; }
	if (val == 2) { args.skip = 0; args.sparse = 1; }
	break;
      case 'f':
        args.act_fecs = 0;
        args.fecsA = 0x0;
        args.fecsB = 0x0;
        if ( strcmp(optarg, allch)  == 0 ) {
          args.all_fecs  = 1;
          if (args.verbose) printf("Using all FECs for this partition\n");
        } else {
          char *arg_copy = strdup(optarg);
          char *token = strtok(arg_copy, ",");
          do {
            int fec = atoi(token);
            if      ( (0 <=fec) && (fec<=15) ) args.fecsA |= 1<<fec;
            else if ( (16<=fec) && (fec<=31) ) args.fecsB |= 1<<(fec-16);
            else {
              printf("Invalid argument: no such FEC: %s", token);
              exit(1);
            }
          } while ( (token = strtok(NULL,",")) );
          free(arg_copy);
        }
        break;
      case 's':
        args.sampling = atoi(optarg); //                                                                                                                                                                    
        if       (args.sampling == 0) printf("Sampling frequency will be set to 10MHz (100ns time bins).\n");
        else if (args.sampling == 1) printf("Sampling frequency will be set to 5MHz (200ns time bins).\n");
        else if (args.sampling == 2) printf("Sampling frequency will be set to 2.5MHz (400ns time bins).\n");
        else if (args.sampling == 3) printf("Sampling frequency will be set to 20MHz (50ns time bins).\n");
        else {
          printf("Allowed values are 0, 1, 2 and 3.\n");
          exit(EXIT_FAILURE);
        }
        break;
      case 'w':
        args.setwidth = 1;
        args.pulsewidth = atoi(optarg); //
        printf("Pulse width will be set to %d (80MHz clock cycles).\n", args.pulsewidth);
        break;
      default:
	print_usage(); 
	exit(EXIT_FAILURE);
      }
  }
  if (optind < argc) {
    if (args.verbose) printf("Using file <%s>\n", argv[optind]);
    sprintf(filename, argv[optind++]);
  }

  //////////////////////////////////////////////////////////////////////
  //////////////////////////////////////////////////////////////////////
  //////////////////////////////////////////////////////////////////////

  unsigned int address;
  unsigned int value;
  unsigned int pedValue;
  int ifec, ich, itb;
  int ctr[4];

  int side, sector, patch, hwaddr, fec, branch;

  unsigned int tfecsA = 0;
  unsigned int tfecsB = 0;

  int ok = 1;

  // Initiatize bus (command mode)
  if (args.verbose) printf("Initializing bus (command mode)~\n");
  rbm_init( args.verbose );

  // Read partition number before reset
  unsigned int currentrp = 99;
  rbm_read(get_Address_PARTITION_NUMBER(), &currentrp, args.verbose);
  if (args.verbose) printf("Currently configured partition number: %i\n", currentrp);

  // Reset RCU2
  if (args.verbose) printf("Reset\n");
  if (Reset_All(args.verbose)) {
    printf("ERROR: Reset failed. Back to RBM direct mode and exit\n");
    rbm_direct( args.verbose );  // Set RBM back to direct mode (for feeserver and buspoke)
    return 1;
  }

  // Partition number
  if (args.readrp) {
    if (args.verbose) printf("Re-writing partition number %i\n", currentrp);
    if ( currentrp>5 ) {
      printf("ERROR: Bad  partition configuration: %i. Back to RBM direct mode and exit\n", currentrp);
      rbm_direct( args.verbose );  // Set RBM back to direct mode (for feeserver and buspoke)
      return 1;
    }
    rbm_write(get_Address_PARTITION_NUMBER(), currentrp, args.verbose);
    args.rp = currentrp;
  } else {
    if (args.verbose) printf("Writing partition number %i\n", args.rp);
    rbm_write(get_Address_PARTITION_NUMBER(), args.rp, args.verbose);
    currentrp = args.rp;
  }

  usleep(1000);

  // Switch FECs
  if ( args.off ) {
    if (args.verbose) printf("Switching OFF FECs and exit\n");
    Switch_Off_FECs(args.verbose);
    if (args.verbose) printf("Back to RBM direct mode and exit\n");
    rbm_direct( args.verbose );  // Set RBM back to direct mode (for feeserver and buspoke)
    return 0;
  } else if ( args.all_fecs ) {
    if ( (args.rp==0) || (args.rp==2) ) {
      args.fecsA = 0x1FF;
      args.fecsB = 0x1FF;
    } else if ( args.rp==1 ) {
      args.fecsA = 0x1FFF;
      args.fecsB = 0xFFF;
    } else {
      args.fecsA = 0x3FF;
      args.fecsB = 0x3FF;
    }
    Write_ACTFECLIST(args.fecsA, args.fecsB, args.verbose);
  } else if ( !args.act_fecs ) {
    args.fecsA = (args.fecsA & 0xFFFF);
    args.fecsB = (args.fecsB & 0xFFFF);
    Write_ACTFECLIST(args.fecsA, args.fecsB, args.verbose);
  }
  Read_ACTFECLIST(&tfecsA, &tfecsB, args.verbose);

  // Board controller reset
  if (args.verbose) printf("Resetting board controller\n");
  rbm_write(get_Address_ABI_BC_RESET_MASK(0), args.fecsA, args.verbose);
  rbm_write(get_Address_ABI_BC_RESET_MASK(2), args.fecsB, args.verbose);
  usleep(500000);

  // FEC reset
  if (args.verbose) printf("FEC reset\n");
  for (branch=0; branch<4; branch++) {
    rbm_write(get_Address_ABI_RST(branch), 0x0, args.verbose);
  }
  usleep(1000);
  for (branch=0; branch<4; branch++) {
    rbm_write(get_Address_ABI_RST(branch), 0x1, args.verbose);
  }
  usleep(100000);

  // Reset FECERR registers (sticky ALTRO errors bits)
  if (args.verbose) printf("Reset FECERR registers\n");
  for (branch=0; branch<4; branch++) {
    rbm_write(get_Address_ABI_FECERR(branch), 0x0, args.verbose);
  }

  // Reset PAR request registers
  if (args.verbose) printf("Reset PAR request registers\n");
  for (branch=0; branch<4; branch++) {
    rbm_write(get_Address_PAR_REQUEST(branch), 0x0, args.verbose);
  }

  // ROLM access
  if (args.verbose) printf("Enable access to the ROLM via the RCU Bus\n");
  for (branch=0; branch<4; branch++) {
    rbm_write(get_Address_ROLM_SEL(branch), 0x1, args.verbose);
  }
  // Configure ROLMs
  if (args.verbose) printf("Adding all channels of powered FECs to ROLMs\n");
  for (branch=0; branch<4; branch++) {
    ctr[branch] = 0;
  }
  for ( ifec=0; ifec<32; ifec++ ) {
    if (!checkFecOn(tfecsA, tfecsB, ifec, args.verbose)) continue;
    branch = getBranchFromFec(ifec, args.rp);
    fec    = (ifec>=16) ? (ifec-16) : ifec;
    for ( ich=127; ich>=0; ich-- ) {
      value = (((branch & 0x2)>>1)<<11) + (fec<<7) + ich;
      rbm_write(get_Address_ROLM(branch)+ctr[branch], value, args.verbose);
      ctr[branch]++;
    }
  }
  for (branch=0; branch<4; branch++) { // Add end marker
    rbm_write(get_Address_ROLM(branch)+ctr[branch], 0xFFF, args.verbose);
  }
  // Close ROLM access
  if (args.verbose) printf("Enable access to the ROLM via the readout\n");
  for (branch=0; branch<4; branch++) {
    rbm_write(get_Address_ROLM_SEL(branch), 0x0, args.verbose);
  }

  // ABI control
  if (args.verbose) printf("Enable control of the ABI via the RCU Bus\n");
  for (branch=0; branch<4; branch++) {
    rbm_write(get_Address_ABI_SEL(branch), 0x1, args.verbose);
  }

  // Set offset and zero suppression thresholds (BCAST)
  if (args.verbose) printf("Setting ZSTHR register\n");
  value = ((args.offset&0x3ff)<<10) | (args.zsthr&0x3ff);
  for (branch=0; branch<4; branch++) {
    altro_bcast( branch, genAltroAddr(1, 0, 0, 0, 0x8), value, args.verbose );
  }

  // Set start and end of acq (BCAST)
  if (args.verbose) printf("Setting TRCFG register\n");
  value = ((args.acq_start&0x3ff)<<10) | (args.acq_end&0x3ff);
  for (branch=0; branch<4; branch++) {
    altro_bcast( branch, genAltroAddr(1, 0, 0, 0, 0xA), value, args.verbose );
  }

  // Set data path (BCAST)
  if (args.verbose) printf("Setting data path (BC1CFG)\n");
  value = (args.zs_en<<19) | ((args.gf&0x3)<<12);
  for (branch=0; branch<4; branch++) {
    altro_bcast( branch, genAltroAddr(1, 0, 0, 0, 0xB), value, args.verbose );
  }
  // Copy for RCU data trailer
  rbm_write(get_Address_DPCFG(), value, args.verbose);

  // DPCF2 for RCU data trailer
  rbm_write(get_Address_DPCF2(), 0x0, args.verbose);

  // Open file with data
  FILE *input_file;
  if (args.verbose) printf("Opening file %s\n", filename);
  if ( (input_file = fopen(filename, "r")) == NULL ) {
    printf("Cannot open file %s!\n", filename);
    rbm_direct( args.verbose );  // Set RBM back to dirtect mode (for feeserver and buspoke)
    return 1;
  }

  // Read the first number in the file to check if it is pedestal file
  int test = readint(&ok, input_file);
  if ( test != 10 ) {
    printf("ERROR: Wrong file type (not a pedestal file)!\n");
    rbm_direct( args.verbose );  // Set RBM back to dirtect mode (for feeserver and buspoke)
    return 1;
  }

  if (args.verbose) printf("Configuring FPED values from file \n");

  int ii;
  int counter=0;
  while ( 1 ) {    /* Loop over all channels found in file */
    
      /* read numbers from one line in the file */
    ii     = readint(&ok, input_file);
    side   = readint(&ok, input_file);
    sector = readint(&ok, input_file);
    patch  = readint(&ok, input_file);
    hwaddr = readint(&ok, input_file);
    pedValue = roundtoint(readfloat(&ok, input_file));
    if( args.steppedestal == 1 ){
      pedValue=50;
      if(counter==1) pedValue=180;
      if(counter==2) pedValue=240;
      if(counter==3) pedValue=300;
      if(counter==4) pedValue=350;
      if(counter==5) pedValue=400;
      if(counter==6) pedValue=440;
      counter++;
      if(counter>6) counter=0;
    }
    
    if ( ok & ((side<0) || (side>1)) ) {
      printf("Warning: Wrong side number in file: %d!", side);
      rbm_direct( args.verbose );  // Set RBM back to dirtect mode (for feeserver and buspoke)
      return 1;
    }
    if ( ok & ((sector<0) || (sector>17)) ) {
      printf("Warning: Wrong sector number in file: %d!", sector);
      rbm_direct( args.verbose );  // Set RBM back to dirtect mode (for feeserver and buspoke)
      return 1;
    }
    if ( ok & ((patch<0) || (patch>5)) ) {
      printf("Warning: Wrong rcu number in file: %d!", patch);
      rbm_direct( args.verbose );  // Set RBM back to dirtect mode (for feeserver and buspoke)
      return 1;
    }

    if ( ok && (args.rp == patch) ) {  // check RCU number [0..5]

      // Check if this fec is on
      fec = hwaddr>>7;
      if ( !checkFecOn(tfecsA, tfecsB, fec, args.verbose) ) continue;

      branch = getBranchFromFec(fec, args.rp);

      if (args.verbose > 1) printf("Writing FPED=%d, FEC%02d, branch %d, hwadd %d\n",
				   pedValue, fec, branch, hwaddr);

      altro_write(branch, genAltroAddrHwAddr(0, hwaddr, 0x6), pedValue&0x3ff, args.verbose);

    }
    if ( !ok ) break;
    
  } // end file loop
  // close the file
  fclose(input_file);

  // DONE!
  if (args.verbose) printf("Give back control of the ABI to fabric\n");
  for (branch=0; branch<4; branch++) {
    rbm_write(get_Address_ABI_SEL(branch), 0, args.verbose);
  }

  // Set trigger and pulse width (optional)
  if (args.verbose) printf("Select TTC L%d as start-of-acquisition\n", args.trig);
  value = 1-args.trig;
  if (args.setwidth) {
    if (args.verbose) printf("Manual select of trigger pulse (L1 and L2 to ALTRO) width: 0x%X\n",
                                 args.pulsewidth&0xFF);
    value = (1<<12) | ((args.pulsewidth&0xFF)<<4) | value;
  }
  rbm_write(get_Address_TRIG_SELECT(), value, args.verbose);

  // Sampling frequency. Enable ChA & chB input. Mask illegal triggers
  if (args.verbose) {
    printf("Configure TTC control register\n");
    if       (args.sampling == 0) printf("Sampling frequency: 10MHz (100ns).\n");
    else if (args.sampling == 1) printf("Sampling frequency: 5MHz (200ns).\n");
    else if (args.sampling == 2) printf("Sampling frequency: 2.5MHz (400ns).\n");
    else if (args.sampling == 3) printf("Sampling frequency: 20MHz (50ns).\n");
  }
  value = (args.sampling&0x3)<<2 | 0x3;
  rbm_write(get_Address_TTC_CONTROL(), value, args.verbose);

  // Readout configuration
  value = 0;
  if (args.sort) {
    if (args.verbose) printf("Enable data sorting\n");
  } else {
    if (args.verbose) printf("Disable data sorting\n");
    value = 0xF;
  }
  if (args.skip) {
    if (args.verbose) {
      printf("Enable skipping of empty channels\n");
      value |= (1<<4);
    }
  } else {
    if (args.verbose) printf("Disable skipping of empty channels\n");
  }
  if (args.sparse) {
    if (args.verbose) {
      printf("Configure sparse readout\n");
      value |= (1<<5);
    }
  } else {
    if (args.verbose) printf("Configure full readout\n");
  }
  rbm_write(get_Address_RDO_CFG1(), value, args.verbose);
  // Reset FECERR registers (sticky ALTRO errors bits)
  if (args.verbose) printf("Reset FECERR registers\n");
  for (branch=0; branch<4; branch++) {
    rbm_write(get_Address_ABI_FECERR(branch), 0x0, args.verbose);
  }

  // Reset PAR request registers
  if (args.verbose) printf("Reset PAR request registers\n");
  for (branch=0; branch<4; branch++) {
    rbm_write(get_Address_PAR_REQUEST(branch), 0x0, args.verbose);
  }

  // Set PAR masks
  if (args.verbose) printf("Set PAR masks\n");
  rbm_write(get_Address_PAR_REQUEST_MASK_A(), 0x1FFF, args.verbose);
  rbm_write(get_Address_PAR_REQUEST_MASK_B(), 0x1FFF, args.verbose);

  // ARM_SYNC
  if (args.verbose) printf("Arm Sync\n");
  rbm_write(get_Address_TTC_ARM_SYNC(), 1, args.verbose);

  rbm_direct( args.verbose );  // Set RBM back to dirtect mode (for feeserver and buspoke)

  return 0;
}