#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, fec_ok, altro_ok, channel_ok, val_ok;
  unsigned int fecsA, fecsB, altros, channels, value, reg;
};

//==========================================================================================
void print_usage() {
  printf("Usage: altro_access [OPTIONS] NEWVALUE\n");
  printf("Purpose: Register access for ALTRO chips.\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");
  printf("-r[REGISTER]    : ALTRO register adress\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.fec_ok     = 0;
  args.altro_ok   = 0;
  args.channel_ok = 0;
  args.reg        = 99;
  args.value      = 0;

  int branch;

  while ( 1 ) {
    int result = getopt(argc, argv, "hv:f:a:c:r:");
    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 'r':
        args.reg = atoi(optarg); //
        break;
    case 'f':
      args.fec_ok = 1;
      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.altro_ok = 1;
      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.channel_ok = 1;
      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) {
    args.val_ok = 1;
    args.value = strtol(argv[optind], NULL, 16);
  }


  

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

  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.fec_ok ) {
    printf("No FEC specified (0..31). Exiting.\n");
    exit(EXIT_FAILURE);
  }    

  if ( !args.altro_ok ) {
    printf("No ALTRO specified (0..7). Exiting.\n");
    exit(EXIT_FAILURE);
  }    

  if ( args.reg>0x1D ) {
    printf("No ALTRO register specified (0..0x1D). Exiting.\n");
    exit(EXIT_FAILURE);
  }    

  if ( (args.reg<8) && !args.channel_ok ) {
    printf("No channel specified (0..15). Exiting.\n");
    exit(EXIT_FAILURE);
  }    

  if ( !args.val_ok ) {
    printf("No new value specified. Exiting.\n");
    exit(EXIT_FAILURE);
  }    

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

  // Write 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("Register 0x%X\n", args.reg);
      if ( (args.reg<11) ) { // channel register
	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;
	  }
	  if (altro_write( branch, genAltroAddr(0, fec&0xF, altro, channel, args.reg), args.value, args.verbose ) ) {
	    printf("ERROR! Writing to fec %d altro %d channel %d register 0x%X failed!\n", fec, altro, channel, args.reg);
	  } else{
	    // read back
	    if ( altro_read(branch, genAltroAddr(0, fec&0xF, altro, channel, args.reg ), &data, args.verbose) ) {
	      printf("ERROR! Readback from fec %d altro %d channel %d register 0x%X failed!\n", fec, altro, channel, args.reg);
	    } else {
	      if (args.value ==(data & 0x3FF) ) {
		printf("0x%X\n", data);
	      } else {
		printf("Value read back from address 0x%X is 0x%X != 0x%X\n",
		       args.reg, data, args.value);
	      }
	    }	    
	  }
	}
      } else {
	if (altro_write( branch, genAltroAddr(0, fec&0xF, altro, 0, args.reg ), args.value, args.verbose )) {
	  printf("ERROR! Writing to fec %d altro %d register 0x%X failed!\n", fec, altro, args.reg);
	} else{
	  // read back
	  /*
	  if ( altro_read(branch, genAltroAddr(0, fec&0xF, altro, 0, args.reg ), &data, args.verbose) ) {
	    printf("ERROR! Readback from fec %d altro %d register 0x%X failed!\n", fec, altro, args.reg);
	  } else {
	    if (args.value==data) {
	      printf("0x%X\n", data);
	    } else {
	      printf("Value read back from address 0x%X is 0x%X != 0x%X\n",
		     args.reg, data, args.value);
	    }
	  }
	  */
	}
      }
    } // end ALTRO loop

  } // end FEC loop

  // DONE!
  sleep(1);
  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;
}