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

//==========================================================================================
void print_usage() {
  printf("Usage: altro_dump [OPTIONS]...\n");
  printf("Purpose: Dump register content of ALTRO chips. If no channel is specified, global\n");
  printf("         registers are dumped. If channels are specified, channel registers are\n");
  printf("         dumped in addition.\n");
  printf("Options:\n");
  printf("-v[LEVEL]       : Set verbosity level (default is 1)\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;
  args.global   = 1;

  int branch;

  while ( 1 ) {
    int result = getopt(argc, argv, "hv:f:a:c:");
    char *arg_copy;
    char *token;
    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 'f':
      args.fecsA = 0x0;
      args.fecsB = 0x0;
      arg_copy = strdup(optarg);
      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.global = 0;
      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;
     default:
	print_usage(); 
	exit(EXIT_FAILURE);
      }
  }
  if (optind < argc) { }

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

  unsigned int address;
  unsigned int data;
  unsigned int result;
  int fec, altro, channel;
  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)

  // Read partition number
  unsigned int currentrp = 99;
  rbm_read(get_Address_PARTITION_NUMBER(), &currentrp, args.verbose);
  printf("Will use the currently configured partition number: %i\n", currentrp);

  // Read active FECs
  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);

  // Read ALTRO values
  for (fec=0; fec<32; fec++) {
    if (!checkFecOn(args.fecsA, args.fecsB, fec, args.verbose)) {
      // This FEC we are not interested in
      if (args.verbose>1) printf("Skipping FEC%02i ...\n", fec);
      continue;
    }
    if (!checkFecOn(tfecsA, tfecsB, fec, args.verbose)) {
      // This FEC is not powered
      if (args.verbose) printf("FEC%02i is not powered, skipping ...\n", fec);
      continue;
    }
    branch = getBranchFromFec(fec, currentrp);
    for (altro=0; altro<8; altro++) {
      if (!checkAltroOn(args.altros, altro, args.verbose)) {
	// This ALTRO we are not interested in
	if (args.verbose>1) printf("Skipping ALTRO%i ...\n", altro);
	continue;
      }
      printf(" =======================\n", fec, altro);
      printf(" === FEC%02d ALTRO%d ===\n", fec, altro);
      printf(" =======================\n", fec, altro);
      if ( args.global ) {
	Dump_ALTRO_ZSTHR(branch, (fec&0xFFFF), altro, args.verbose);
	Dump_ALTRO_BCTHR(branch, (fec&0xFFFF), altro, args.verbose);
	Dump_ALTRO_TRCFG(branch, (fec&0xFFFF), altro, args.verbose);
	Dump_ALTRO_DPCFG(branch, (fec&0xFFFF), altro, args.verbose);
	Dump_ALTRO_DPCF2(branch, (fec&0xFFFF), altro, args.verbose);
	Dump_ALTRO_ERSTR(branch, (fec&0xFFFF), altro, args.verbose);
	Dump_ALTRO_TRCNT(branch, (fec&0xFFFF), altro, args.verbose);
      } else {
	for (channel=0; channel<16; channel++) {
	  if (!checkChannelOn(args.channels, channel, args.verbose)) {
	    // This CHANNEL we are not interested in
	    if (args.verbose>1) printf("Skipping CHANNEL%02i ...\n", channel);
	    continue;
	  }
	  Dump_ALTRO_Channel_Registers(branch, (fec&0xFFFF), altro, channel, args.verbose);
	}
      }
      printf("\n");

    } // end ALTRO loop

  } // end FEC loop


  // DONE!
  if (args.verbose > 1) 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 RBM back to direct mode (for feeserver and buspoke)
  rbm_direct( args.verbose );

  return 0;
}