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

//==========================================================================================
void print_usage() {
  printf("Usage: dump_pedmem [OPTIONS] \n");
  printf("Purpose: Dump pedestal memories\n");
  printf("Options:\n");
  printf("-v[LEVEL]     : Set verbosity level (default is 1)\n");
  printf("-n[SAMPLES]   : Use this number of samples per channel (default is 1008)\n");
  printf("-f[FECLIST]     : Use these FECs (comma separated list, no spaces)\n");
  printf("-a[ALTROLIST]   : Use these ALTROs (comma separated list, no spaces)\n");
  printf("-c[CHANNELLIST] : Use these ALTROs (comma separated list, no spaces)\n");
}

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

  struct args args;

  // Default values
  args.verbose  = 1;
  args.fecsA    = 0;
  args.fecsB    = 0;
  args.altros   = 0;
  args.channels = 0;

  while ( 1 ) {
    int result = getopt(argc, argv, "hv:f:a:c:n:");
    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 'n':
        args.samples = atoi(optarg); //
        break;
      case 'f':
	args.fecsA = 0x0;
	args.fecsB = 0x0;
	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 'a':
	args.altros   = 0x0;
	arg_copy = strdup(optarg);
	token = strtok(arg_copy, ",");
	do {
	  int altro = atoi(token);
	  if ( (altro>=0) && (altro<8) ) {
	    args.altros |= 1<<altro;
	  } else {
	    printf("Invalid argument: no such ALTRO: %s", token);
	    exit(1);
	  }
	} while ( (token = strtok(NULL,",")) );
	free(arg_copy);
	break;
      case 'c':
	args.channels   = 0x0;
	arg_copy = strdup(optarg);
	token = strtok(arg_copy, ",");
	do {
	  int ch = atoi(token);
	  if ( (ch>=0) && (ch<16) ) {
	    args.channels |= 1<<ch;
	  } else {
	    printf("Invalid argument: no such channel: %s", token);
	    exit(1);
	  }
	} while ( (token = strtok(NULL,",")) );
	free(arg_copy);
	break;
      }
  }
  if (optind < argc) {
  }

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

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

  Read_ACTFECLIST(&tfecsA, &tfecsB, args.verbose);

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

  if (args.verbose) printf("Reading back from pedestal memories ...\n");
  
  for (fec=0; fec<32; fec++) {
    if (!checkFecOn(tfecsA, tfecsB, fec, args.verbose)) continue;
    if (!checkFecOn(args.fecsA, args.fecsB, fec, args.verbose)) continue;
    if (args.verbose>1) printf("\n");
    branch = getBranchFromFec(fec, args.rp);
    for (altro=0; altro<8; altro++) {
      if (!checkAltroOn(args.altros, altro, args.verbose)) continue;
      for (channel=0; channel<16; channel++) {
	if (!checkChannelOn(args.channels, channel, args.verbose)) continue;
	for (itb=0; itb<args.samples; itb++) {
	  altro_write(branch, genAltroAddr(0, fec, altro, channel, 0xD), itb, args.verbose);    // PMADD
	  altro_read(branch, genAltroAddr(0, fec, altro, channel, 0x7), &result, args.verbose); // PMDTA
	  printf("FEC%02i ALTRO%i CHAN%02i SAMPLE%04i: 0x%X\n", fec, altro, channel, itb, result&0x3FF);
	}
      }
    }
  }
  
  rbm_direct( args.verbose );  // Set RBM back to direct mode (for feeserver and buspoke)
  
  return 0;
}