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

//==========================================================================================
void print_usage() {
  printf("Usage: ped_loop [OPTION]... LOGFILE\n");
  printf("Purpose: Configure RCU2 and FEC pedestal memories and exercise loopback test\n");
  printf("Arguments :LOGFILE is optional argument\n");
  printf("Options:\n");
  printf("-v[LEVEL]     : Set verbosity level (default is 1)\n");
  printf("-p[PARTITION] : 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("-N[NLOOPS]    : Configure number of loops to be done through all channels\n");
  printf("-s[NUM]       : Set sampling frequency (0=10MHz=default, 1=5MHz, 2=2.5MHz, 3=20MHz)\n");
}

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

  struct args args;

  // Default values
  args.verbose  = 1;
  args.nloops   = 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  = 0x3FF;
  args.pattern  = 0x155;
  args.ramp1    = 1;
  args.ramp2    = 0;
  args.chess    = 0;
  args.sampling = 0;

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

  char *allch = "all";

  char filename[1000];
  sprintf(filename, "/mnt/dcbrw/ped_loop_log.txt");

  while ( 1 ) {
    int result = getopt(argc, argv, "ohv:f:n:P:p:N:s:");
    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.nloops = atoi(optarg); //
        break;
      case 'n':
        args.samples = atoi(optarg); //
        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: %s", 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(1);
	    }
	  } while ( (token = strtok(NULL,",")) );
	  free(arg_copy);
	}
	break;
      case 's':
        args.sampling = atoi(optarg); //
	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;
      }
  }
  if (optind < argc) {
    if (args.verbose) printf("Using file <%s>\n", argv[optind]);
    sprintf(filename, argv[optind++]);
  }

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

  unsigned int address;
  unsigned int data;
  unsigned int result;
  int ifec, ich, branch, fec, altro, channel, itb, errorcode;
  int ctr[4];

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

  if (args.verbose > 1) printf("Initializing bus ...\n");
  rbm_init( args.verbose );    // Init bus (command mode)

  unsigned int currentrp = 0;
  rbm_read(get_Address_PARTITION_NUMBER(), &currentrp, args.verbose);
  if (args.verbose > 1) printf("Found configured partition number: %i\n", currentrp);

  if (args.verbose > 1) printf("Reset ...\n");
  if (Reset_All(args.verbose)) {
    printf("ERROR: Reset failed ...\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: Found bad partition configuration: %i\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 ) {
    args.fecsA = (args.fecsA & 0xFFFF);
    args.fecsB = (args.fecsB & 0xFFFF);
    if (args.verbose) printf("Switching OFF FECs and exit ...\n");
    Switch_Off_FECs(args.verbose);
    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);

  usleep(100000);

  // ABI control
  if (args.verbose > 1) 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);

  // FEC reset
  if (args.verbose > 1) printf("FEC reset ...\n");
  for (branch=0; branch<4; branch++) {
    rbm_write(get_Address_ABI_RST(branch), 0x0, args.verbose);
  }
  usleep(1000000); // 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 > 1) printf("Reset FECERR registers ...\n");
  for (branch=0; branch<4; branch++) {
    rbm_write(get_Address_ABI_FECERR(branch), 0x1, args.verbose);
  }

  // Set sampling frequency. Enable ChA & chB input. Mask illegal triggers
  if (args.verbose > 1) printf("Configure TTC control register ...\n");
  int value = (args.sampling&0x3)<<2 | 0x3;
  rbm_write(get_Address_TTC_CONTROL(), value, args.verbose);
  usleep(100000);

  int bcast_error_ctr[4];
  for (branch=0; branch<4; branch++) bcast_error_ctr[branch] = 0;

  // Write into pedestal memories
  if (args.verbose) {
    if      ( args.ramp1 ) printf("Writing RAMP1 pattern to PEDESTAL memories (broadcast) ...\n");
    else if ( args.ramp2 ) printf("Writing RAMP2 pattern to PEDESTAL memories (broadcast) ...\n");
    else if ( args.chess ) printf("Writing CHESS pattern to PEDESTAL memories (broadcast) ...\n");
    else                   printf("Writing pattern 0x%X to PEDESTAL memories (broadcast) ...\n", args.pattern);
  }
  for (branch=0; branch<4; branch++) {
    for ( itb=0; itb<args.samples; itb++ ) {
      errorcode = altro_bcast( branch, genAltroAddr(1, 0, 0, 0, 0xD), itb, args.verbose );      // PMADD
      if (errorcode) {
	if (args.verbose) printf("ERROR: PMADD BCAST WRITE failed on branch %i SAMPLE%04i: Errorcode=0x%X\n", branch, itb, errorcode);
	bcast_error_ctr[branch]++;
      }
      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;
      errorcode = altro_bcast( branch, genAltroAddr(1, 0, 0, 0, 0x7), pedvalue, args.verbose ); // PMDTA
      if (errorcode) {
	if (args.verbose) printf("ERROR: PMDTA BCAST WRITE failed on branch %i SAMPLE%04i: Errorcode=0x%X\n", branch, itb, errorcode);
	bcast_error_ctr[branch]++;
      }
    }
  }
  printf("Writing done.\n");
  for (branch=0; branch<4; branch++) {
    if (bcast_error_ctr[branch])
      printf("BRANCH %i broadcast writing done: %i errors!\n", branch, bcast_error_ctr[branch]);
    else
      printf("BRANCH %i broadcast writing done: OK\n", branch);
  }  

  int loop_ctr;
  int write_error_ctr[4];
  int read_error_ctr[4];
  int data_error_ctr[4];

  for (branch=0; branch<4; branch++) {
    write_error_ctr[branch] = 0;
    read_error_ctr[branch]  = 0;
    data_error_ctr[branch]  = 0;
  }

  if (args.verbose) printf("Opening file for log data: %s\n", filename);

  FILE *resFile = fopen(filename, "a");
  if (resFile==NULL) {
    printf("ERROR: Can not open file: %s\n", filename);
    rbm_direct( args.verbose );  // Set RBM back to direct mode (for feeserver and buspoke)
    return 1;
  }

  if (args.verbose) printf("Reading back from pedestal memories ...\n");

  for (loop_ctr=0; loop_ctr<args.nloops; loop_ctr++) {
    if (args.verbose) printf("Loop %i/%i ...\n", loop_ctr, args.nloops);
    fprintf(resFile, "Loop %i/%i ...\n", loop_ctr, args.nloops);
    time_t rawtime;
    time(&rawtime);
    printf("  %s", (unsigned)ctime(&rawtime));
    fprintf(resFile, "  %s", (unsigned)ctime(&rawtime));
    for (fec=0; fec<32; fec++) {
      if (!checkFecOn(tfecsA, tfecsB, fec, args.verbose)) continue;
      int write_error_ctr_fec = 0;
      int read_error_ctr_fec = 0;
      int data_error_ctr_fec = 0;
      printf("  Checking FEC%02i ... ", fec);
      fprintf(resFile,"  Checking FEC%02i ... ", fec);
      if (args.verbose>1) printf("\n");
      branch = getBranchFromFec(fec, args.rp);
      for (altro=0; altro<8; altro++) {
	if (args.verbose>1) printf("    Checking ALTRO%i ...\n", altro);
	for (channel=0; channel<16; channel++) {
	  if (args.verbose>2) printf("      Checking CHAN%02i ...\n", channel);
	  for (itb=0; itb<args.samples; itb++) {
	    errorcode = altro_write(branch, genAltroAddr(0, fec, altro, channel, 0xD), itb, args.verbose); // PMADD
	    if (errorcode) {
	      if (args.verbose) printf("ERROR (loop %i, %i write errors): WRITE failed, FEC%02i ALTRO%i CHAN%02i SAMPLE%04i: Errorcode=0x%X\n",
				       loop_ctr, write_error_ctr[branch], fec, altro, channel, itb, errorcode);
	      fprintf(resFile, "ERROR (loop %i, %i write errors): WRITE failed, FEC%02i ALTRO%i CHAN%02i SAMPLE%04i: Errorcode=0x%X\n",
		      loop_ctr, write_error_ctr[branch], fec, altro, channel, itb, errorcode);
	      write_error_ctr[branch]++;
	      write_error_ctr_fec++;
	    }
	    altro_read(branch, genAltroAddr(0, fec, altro, channel, 0x7), &result, args.verbose);          // PMDTA
	    unsigned int expected = args.pattern; // default
	    if      ( args.ramp1 ) expected = itb;
	    else if ( args.ramp2 ) expected = 0x3FF-itb;
	    else if ( args.chess ) expected = (itb%2) ? 0x155 : 0x2AA;
	    if ( (result&0x3FF)!=(expected&0x3FF) ) {
	      if (args.verbose) printf("ERROR (loop %i, %i data errors): Wrong DATA 0x%X != 0x%X, FEC%02i ALTRO%i CHAN%02i SAMPLE%04i\n",
				       loop_ctr, data_error_ctr[branch], result&0x3FF, expected&0x3FF, fec, altro, channel, itb);
	      fprintf(resFile, "ERROR (loop %i, %i data errors): Wrong DATA 0x%X != 0x%X, FEC%02i ALTRO%i CHAN%02i SAMPLE%04i\n",
		      loop_ctr, data_error_ctr[branch], result&0x3FF, expected&0x3FF, fec, altro, channel, itb);
	      data_error_ctr[branch]++;
	      data_error_ctr_fec++;
	    } 
	    if (args.verbose > 3) printf("FEC%02i ALTRO%i CHAN%02i SAMPLE%04i: 0x%X\n", fec, altro, channel, itb, result&0x3FF);
	    errorcode = check_ABI_CSR_Error(branch, args.verbose);
	    if (errorcode) {
	      if (args.verbose) printf("ERROR (loop %i, %i read errors): READ failed, FEC%02i ALTRO%i CHAN%02i SAMPLE%04i: Errorcode=0x%X\n",
				       loop_ctr, read_error_ctr[branch], fec, altro, channel, itb, errorcode);
	      fprintf(resFile, "ERROR (loop %i, %i read errors): READ failed, FEC%02i ALTRO%i CHAN%02i SAMPLE%04i: Errorcode=0x%X\n",
		      loop_ctr, read_error_ctr[branch], fec, altro, channel, itb, errorcode);
	      read_error_ctr[branch]++;
	      read_error_ctr_fec++;
	    }
	  }
	}
      }
      if ( read_error_ctr_fec || write_error_ctr_fec || data_error_ctr_fec ) {
	if ( write_error_ctr_fec ) {
	  printf("%i write errors. ", write_error_ctr_fec);
	  fprintf(resFile, "%i write errors. ", write_error_ctr_fec);
	}
	if ( read_error_ctr_fec ) {
	  printf("%i read errors. ", read_error_ctr_fec);
	  fprintf(resFile, "%i read errors. ", read_error_ctr_fec);
	}
	if ( data_error_ctr_fec ) {
	  printf("%i data errors. ", data_error_ctr_fec);
	  fprintf(resFile, "%i data errors. ", data_error_ctr_fec);
	}
      } else {
	printf("OK");
	fprintf(resFile, "OK");
      }
      printf("\n");
      fprintf(resFile, "\n");
    }
  }

  printf("Readback done. Loop_ctr=%i.\n", loop_ctr);
  for (branch=0; branch<4; branch++) {
    printf("  BRANCH %i saw %i data errors, %i write errors and %i read errors\n",
	   branch, data_error_ctr[branch], write_error_ctr[branch], read_error_ctr[branch]);
    fprintf(resFile, "  BRANCH %i saw %i data errors, %i write errors and %i read errors\n",
	    branch, data_error_ctr[branch], write_error_ctr[branch], read_error_ctr[branch]);
  }

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

  return 0;
}