#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;
  unsigned int fecsA, fecsB, samples, rp;
};

//==========================================================================================
void print_usage() {
  printf("Usage: test_sparse [OPTION] hlm_file_name.txt\n");
  printf("Purpose: Configure RCU2 and FECs for testing of sparse readout\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");
}

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

  struct args args;

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

  int branch;

  char *allch = "all";

  char filename[1000];
  sprintf(filename, "/mnt/dcbro/rcu2/hlm_in.txt");

  while ( 1 ) {
    int result = getopt(argc, argv, "ohv:f:n:p:");
    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 '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 '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;
    default:
      print_usage(); 
      exit(EXIT_FAILURE);
    }
  }
  if (optind < argc) {
    if (args.verbose) printf("Using HLM input 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;

  if (args.verbose) printf("Opening file with input HLM pattern: %s\n", filename);
  FILE *hlmFile = fopen(filename, "r");
  if (hlmFile==NULL) {
    printf("ERROR: Can not open file: %s\n", filename);
    return 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("Reset 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);
  }

  // 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;
  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;
	rbm_write(get_Address_ROLM(branch)+ctr[branch], value, args.verbose);	
	ctr[branch]++;
	if ( 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);
  }
  // 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 data path (BROADCAST)
  if (args.verbose) printf("Enable ZS, set data path (BC1CFG) to 0xA (PEDMEM-FPD)\n");
  for (branch=0; branch<4; branch++) {
    altro_bcast( branch, genAltroAddr(1, 0, 0, 0, 0xB), (1<<19) | 0xA, args.verbose );
  }
  // Copy of DPCFG register 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
  printf("Writing pattern to PEDESTAL memories\n");
  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 );   // PMADD
      value = ( itb%2 ) ? 0x2AA : 0x155;
      altro_bcast( branch, genAltroAddr(1, 0, 0, 0, 0x7), value, args.verbose ); // PDTA
    }
  }

  // Set offset and zero suppression thresholds
  value = 300;
  if (args.verbose) printf("Setting ZSTHR register (threshold=%u)\n", value);
   for (branch=0; branch<4; branch++) {
    altro_bcast( branch, genAltroAddr(1, 0, 0, 0, 0x8), value&0x3FF, args.verbose );
  }

  // Set FPD values
  if (args.verbose) printf("Set FPD values according to HLM input file (to ensure empty and filled channels)\n");
  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, channel;
    for ( altro=0; altro<8; altro++ ) {
      fscanf(hlmFile, "%X\n", &value);
      if (args.verbose > 1) printf("HLM for Fec%02d Altro%d - 0x%X\n", ifec, altro, value);
      for ( channel=0; channel<16; channel++ ) {
	if ( (value>>channel)&0x1 ) altro_write( branch, genAltroAddr(0, fec&0xF, altro, channel, 0x6 ), 0x0, args.verbose );
	else                        altro_write( branch, genAltroAddr(0, fec&0xF, altro, channel, 0x6 ), 500, args.verbose );
      }
    }
  }

  // Set number of samples
  if (args.verbose) printf("Set 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 );
  }

  // 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 L1 as start-of-acquisition\n");
  rbm_write(get_Address_TRIG_SELECT(), 0x0, 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");
  rbm_write(get_Address_TTC_CONTROL(), 1<<5 | 0x3, args.verbose);

  // Readout configuration
  if (args.verbose) printf("Enable data sorting\n");
  rbm_write(get_Address_RDO_CFG1(), (0x1C50<<8) | (1<<5), 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;
}