#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 "abi_functions.h"
#include "rcu2_functions.h"

//==========================================================================================
// Used by main to communicate with ARGP parse_opt
struct args {
  int verbose, all_fecs, act_fecs, off, readrp, ramp1, ramp2, chess, sort, zs_en, offset;
  int sparse, zsthr, gf, skip, waittime, emptychan;
  unsigned int fecsA, fecsB, samples, pattern, bc1cfg, rp, sampling, trig, setwidth, pulsewidth,steppedestal;
};

//==========================================================================================
void print_usage() {
  printf("Usage: config [OPTION]...\n");
  printf("Purpose: Configure RCU2 and FECs\n");
  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("-n[SAMPLES] : Use this number of samples per channel (default is 1008)\n");
  printf("-P[PATTERN] : Use pattern (ramp1, ramp2, chess or 10bit value, default is 0x155)\n");
  printf("-b[BC1CFG]  : Configure BC1CFG register in ALTROs (default is 0xA = f(t)-FPD)\n");
  printf("-z[NUM]     : Use zero supression with threshold NUM (default=0)\n");
  printf("-g[NUM]     : Use glitch filter for zero suppression (default=0)\n");
  printf("-O[NUM]     : Add offset to ADC values (default=0)\n");
  printf("-s[0..3]    : 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[0/1]     : (De)activate data sorting (enabled by default)\n");
  printf("-x[0/1/2]   : 0 = Full readout (default), 1 = Full readout with channel skipping, 2 = sparse readout\n");
  printf("-t[TIME]    : Set SCEVL wait time in clock cycles (default = 0x1C50 = 90.6us)\n");
  printf("-e[Empt.Ch.]: Relative number empty channels (default=0)\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;

  srand(time(NULL));
  
  // Default values
  args.verbose    = 1;
  args.off        = 0;
  args.act_fecs   = 1;
  args.all_fecs   = 0;
  args.fecsA      = 0;
  args.fecsB      = 0;
  args.readrp     = 1;
  args.rp         = 999;
  args.samples    = 1008;
  args.pattern    = 0x155;
  args.ramp1      = 0;
  args.ramp2      = 0;
  args.chess      = 0;
  args.bc1cfg     = 0xA; // f(t)-FPD (pattern readout
  args.sampling   = 0;
  args.trig       = 1;   // Start acquisition on L1
  args.setwidth   = 0;
  args.pulsewidth = 0;
  args.sort       = 1;
  args.zs_en      = 0;
  args.zsthr      = 0;
  args.offset     = 0;
  args.gf         = 0;
  args.skip       = 0;
  args.sparse     = 0;
  args.waittime   = 0x1C50;  // For SCEVL
  args.emptychan  = 0;
  args.steppedestal = 0;

  int branch;

  char *ramp1ch = "ramp1";
  char *ramp2ch = "ramp2";
  char *chessch = "chess";

  char *allch = "all";

  int val = 0;

  while ( 1 ) {
    int result = getopt(argc, argv, "ohv:d:f:n:P:p:b:s:l:w:z:g:O:x:t:e: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 'h':
      print_usage();
      exit(EXIT_SUCCESS); 
    case 'v':
      args.verbose = atoi(optarg); //
      break;
    case 't':
      args.waittime = atoi(optarg); //
      break;
    case 'z':
      args.zs_en = 1;
      args.zsthr = atoi(optarg);
      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 'O':
      args.offset = atoi(optarg);
      break;
    case 'g':
      args.gf = atoi(optarg);
      break;
    case 'd':
      args.sort = atoi(optarg);
      break;
    case 'o':
      args.off = 1; //
      break;
    case 'n':
      args.samples = atoi(optarg); //
      if (args.samples<3) {
	printf("!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\n");
	printf("!!! The ALTROs should be configured with n>2 !!!\n");
	printf("!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\n");
      }
      break;
    case 'b':
      args.bc1cfg = atoi(optarg); //
      break;
    case 'e':
      args.emptychan = atoi(optarg); //
      printf("Emulated 0.%d of Channels\n",args.emptychan );
      break;
    case 'm':
      args.steppedestal = 1;//
      printf("Set step pedestals");
      break;
    case 'l':
      if (args.verbose) {
	args.trig = atoi(optarg); //
	if      (args.trig == 0) printf("Start acquisition in ALTRO on L0.\n");
	else if (args.trig == 1) printf("Start acquisition in ALTRO on L1.\n");
	else                     printf("Trigger mode should be 0 or 1.\n");
      }
      break;
    case 'P':
      if ( strcmp(optarg, ramp1ch)  == 0 ) {
	args.ramp1 = 1;
	args.ramp2 = 0;
	args.chess = 0;
      } else if ( strcmp(optarg, ramp2ch)  == 0 ) {
	args.ramp2 = 1;
	args.ramp1 = 0;
	args.chess = 0;
      } else if ( strcmp(optarg, chessch) == 0 ) {
	args.chess = 1;
	args.ramp1 = 0;
	args.ramp2 = 0;
      } else {
	sscanf( optarg, "%i", &args.pattern );
	args.ramp1  = 0;
	args.ramp2  = 0;
	args.chess  = 0;
      }
      break;
    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 '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(EXIT_FAILURE);
	  }
	} while ( (token = strtok(NULL,",")) );
	free(arg_copy);
      }
      break;
    case 's':
      args.sampling = atoi(optarg); //
      if (args.verbose) {
	if      (args.sampling == 0) printf("Sampling frequency set to 10MHz (100ns time bins).\n");
	else if (args.sampling == 1) printf("Sampling frequency set to 5MHz (200ns time bins).\n");
	else if (args.sampling == 2) printf("Sampling frequency set to 2.5MHz (400ns time bins).\n");
	else if (args.sampling == 3) printf("Sampling frequency 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); //
      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;
  int ifec, ich, fec, itb;
  int ctr[4];

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

  // 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(1000000);

  // 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(1000000);

  // 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;
  }
  int altro;
  int counter=0;
  for ( ifec=0; ifec<32; ifec++ ) {
    for ( altro=0; altro<8; altro++ ) {
      if (!checkFecOn(tfecsA, tfecsB, ifec, args.verbose)) continue;
      branch = getBranchFromFec(ifec, args.rp);
      fec    = (ifec>=16) ? (ifec-16) : ifec;
      for ( ich=0; ich<16; ich++ ) {
	value = (((branch & 0x2)>>1)<<11) + (fec<<7) + (altro<<4) + ich;
	//if ( args.sort && (ich == 15) ) value |= 1<<12;
	rbm_write(get_Address_ROLM(branch)+ctr[branch], value, args.verbose);	
        ctr[branch]++;

	if ( args.sort && (ich == 15) ) {
	  rbm_write(get_Address_ROLM(branch)+ctr[branch], 0x1700, args.verbose);
	  ctr[branch]++;
	}
      }
    }
  }
  for (branch=0; branch<4; branch++) { // Add end marker
    rbm_write(get_Address_ROLM(branch)+ctr[branch], 0xFFF, args.verbose);
    ctr[branch]++;
  }
  // Close ROLM access
  if (args.verbose) printf("Enable ROLM and HLM access for 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);
  }

  usleep(100000);

  // Set offset and zero suppression thresholds   
  if(args.emptychan != 0){
     args.zs_en      = 1;
     args.zsthr      = 300;
     printf("Enable ZS for empty channels emulation (zs=%d) \n",args.zsthr);
  }
  if (args.verbose) printf("Setting ZSTHR register (threshold=%u)\n", args.zsthr&0x3ff);
  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 number of samples
  if (args.sort) {
    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;
      int altro;
      for ( altro=0; altro<8; altro++ )
	altro_write( branch, genAltroAddr(0, fec&0xF, altro, 0, 0xA ) , args.samples, args.verbose );
    }
  } else {
    if (args.verbose) printf("Setting number of samples to %d\n", args.samples);
    for (branch=0; branch<4; branch++) {
      // Set using BROADCAST
      altro_bcast( branch, genAltroAddr(1, 0, 0, 0, 0xA), args.samples, args.verbose );
    }
  }
  if(args.emptychan == 0){
    // Set fixed pedestal value to 0x0 (BROADCAST)
    if (args.verbose) printf("Setting FPD to 0x0\n");
    for (branch=0; branch<4; branch++) {
      altro_bcast( branch, genAltroAddr(1, 0, 0, 0, 0x6), 0x0, args.verbose );
    }
  }
  else{
    int hwaddr;
    int counter=0;
     for ( hwaddr=0; hwaddr<3584; hwaddr++ ) {
      
      branch = getBranchFromFec(hwaddr>>7, args.rp);
      if(hwaddr==1664){
	hwaddr=2048;
      }
      int value=100;
      if(counter+1>args.emptychan*100/128) value=0;
      altro_write(branch, genAltroAddrHwAddr(0, hwaddr, 0x6), value&0x3ff, args.verbose);     
      counter++;
      if(counter>127) counter=0;
    }
  }
  if( args.steppedestal !=0){
    int hwaddr;
    int counter=0;
     for ( hwaddr=0; hwaddr<3584; hwaddr++ ) {
      
      branch = getBranchFromFec(hwaddr>>7, args.rp);
      if(hwaddr==1664){
	hwaddr=2048;
      }
      int value=50;
      if(counter==1) value=180;
      if(counter==2) value=240;
      if(counter==3) value=300;
      if(counter==4) value=350;
      if(counter==5) value=400;
      if(counter==6) value=440;
      altro_write(branch, genAltroAddrHwAddr(0, hwaddr, 0x6), value&0x3ff, args.verbose);     
      counter++;
      if(counter>6) counter=0;
    }
  
  }
  // Set data path (BROADCAST)
  if (args.verbose) printf("Setting data path (BC1CFG) to 0x%x\n", args.bc1cfg);
  value = (args.zs_en<<19) | ((args.gf&0x3)<<12) | args.bc1cfg;
  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);

  // Set pedestal memories
  if ( (args.bc1cfg==0x1) || (args.bc1cfg==0x5) || (args.bc1cfg==0xA) || (args.bc1cfg==0xB) ) {
    if (args.verbose) {
      if      ( args.ramp1 ) printf("Writing RAMP1 pattern to PEDESTAL memories\n");
      else if ( args.ramp2 ) printf("Writing RAMP2 pattern to PEDESTAL memories\n");
      else if ( args.chess ) printf("Writing CHESS pattern to PEDESTAL memories\n");
      else                   printf("Writing pattern 0x%x to PEDESTAL memories\n", args.pattern);
    }
    for (branch=0; branch<4; branch++) {
      for ( itb=0; itb<args.samples; itb++ ) {
	altro_bcast( branch, genAltroAddr(1, 0, 0, 0, 0xD), itb, args.verbose ); // PDTA
	unsigned int pedvalue = args.pattern; // default
	if      ( args.ramp1 ) pedvalue = itb;
	else if ( args.ramp2 ) pedvalue = 0x3FF-itb;
	else if ( args.chess ) pedvalue = (itb%2) ? 0x155 : 0x2AA;
	altro_bcast( branch, genAltroAddr(1, 0, 0, 0, 0x7), pedvalue, args.verbose ); // PMADD
      }
    }
  }

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

  // Trigger select
  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. Send CDH for L2 timeout
  if (args.verbose) printf("Configure TTC control register\n");
  value = 1<<5 | (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");
  }
  value |= ((args.waittime&0xFFFF)<<8); // SCEVL wait time
  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);


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

  return 0;
}