#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, rp;
};

//==========================================================================================
void print_usage() {
  printf("Usage: hlm_dump [OPTION]\n");
  printf("Purpose: Dump RCU2 Hit List List Memories\n");
  printf("Options:\n");
  printf("-v[LEVEL] : Set verbosity level (default is 1)\n");
  printf("-p[NUM]   : Use this partition number (otherwise read from RCU2)\n");
}

unsigned int bitCount(unsigned int value) {
  unsigned int count = 0;
  while (value > 0) {         // until all bits are zero
    if ((value & 1) == 1)     // check lower bit
      count++;
    value >>= 1;              // shift bits, removing lower bit
  }
  return count;
}

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

  struct args args;

  // Default values
  args.verbose    = 1;
  args.rp         = 99;

  int branch;

  while ( 1 ) {
    int result = getopt(argc, argv, "hv: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 'p':
        args.rp = atoi(optarg); //
        break;
      case 'v':
        args.verbose = atoi(optarg); //
        break;
    }
  }
  if (optind < argc) {
    //if (args.verbose) printf("Using file <%s>\n", argv[optind]);
    //sprintf(filename, argv[optind++]);
  }

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

  int ctr, eoc_ctr[0];
  unsigned int rp;
  unsigned int ifec  = 0;
  unsigned int altro = 0;
  int value = 0;

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

  rbm_init( args.verbose );    // Init bus (command mode)

  rbm_read(get_Address_RCU_VERSION(), &value, args.verbose);
  if ( args.verbose ) printf("RCU_VERSION = %X\n", value);

  if (args.rp == 99) {
    // Read partition number
    rbm_read(get_Address_PARTITION_NUMBER(), &rp, args.verbose);
    if (args.verbose > 1) printf("Using currently configured partition number: %i\n", rp);
  } else {
    rp = args.rp;
  }

  if      ( rp == 1 ) { tfecsA = 0x1FFF; tfecsB = 0xFFF; }
  else if ( rp  > 2 ) { tfecsA = 0x3FF;  tfecsB = 0x3FF; }
  else                { tfecsA = 0x1FF;  tfecsB = 0x1FF; }

  // Check and set HLM access
  if (args.verbose > 1) printf("Read status of HLM access and enable access via the RCU Bus\n"); 
  for (branch=0; branch<4; branch++) {
    rbm_read(get_Address_ROLM_SEL(branch), &value, args.verbose);
    if ( args.verbose ) printf("Branch %d ROLM/HLM SEL value before dump: 0x%X\n", branch, value);
    rbm_write(get_Address_ROLM_SEL(branch), 0x2, args.verbose);
  }

  // Dump HLM
  unsigned int chan_ctr = 0;
  if (args.verbose > 1) printf("Dumping HLMs\n");
  for ( ifec=0; ifec<32; ifec++ ) {
    if (!checkFecOn(tfecsA, tfecsB, ifec, args.verbose)) continue;
    branch = getBranchFromFec(ifec, rp);
    for ( altro=0; altro<8; altro++ ) {
      unsigned int addr = ((ifec&0xF)<<3) | altro;
      rbm_write(get_Address_HLM_GATEWAY_ADDRESS(branch), addr, args.verbose);
      rbm_read(get_Address_HLM_GATEWAY_DATA(branch), &value, args.verbose);
      unsigned int bitcount = bitCount(value);
      chan_ctr += bitcount;
      if      (args.verbose==3 )
	printf("Br=%u: FEC_%02d ALTRO_%d Address 0x%X => HLM=0x%04X (%d channels)\n",
	       branch, ifec, altro, addr, value, bitcount);
      else if (args.verbose==2 )
	printf("Br=%u: FEC_%02d ALTRO_%d Address 0x%X => HLM=0x%04X\n", branch, ifec, altro, addr, value);
      else if (args.verbose==2 )
	printf("Br=%u: FEC_%02d ALTRO_%d => HLM=0x%04X\n", branch, ifec, altro, value);
      else
	printf("0x%04X\n", value);
    }
  }

  if (args.verbose > 1) printf("Enable access to ROLM and HLM via the readout\n");
  for (branch=0; branch<4; branch++) {
    rbm_write(get_Address_ROLM_SEL(branch), 0x0, args.verbose);
  }
  if (args.verbose > 1) printf("Total channel count: %d\n", chan_ctr);

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

  return 0;
}