#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 "rbm_functions.h"
#include "rcu2_functions.h"

//==========================================================================================
// Used by main to communicate with ARGP parse_opt
struct args {
  int verbose, write;
  unsigned int address, value;
};

//==========================================================================================
void print_usage() {
  printf("Usage: rcu2poke [OPTION] REGISTER [VALUE]\n");
  printf("Purpose: Read and write RCU2 registers. If 2nd parameter is given, this value\n");
  printf("       : will be written to the register adress. Otherwise we simply read.\n");
  printf("Options:\n");
  printf("-v[LEVEL]   : Set verbosity level (default is 1)\n");
}

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

  struct args args;

  // Default values
  args.verbose = 0;
  args.write   = 0;

  while ( 1 ) {
    int result = getopt(argc, argv, "hv:");
    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;
    }
  }
  int ctr = 0;
  while (optind < argc) {
    if (ctr==0) args.address = strtol(argv[optind], NULL, 16);
    if (ctr==1) {
      args.value = strtol(argv[optind], NULL, 16);
      args.write = 1;
    }
    optind++;
    ctr++;
  }

  if ( args.verbose ) {
    if ( args.write ) {
      printf("WRITE\n");
    } else {
      printf("READ\n");
    }
    printf("Address: 0x%X\n", args.address);
  }

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

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

  unsigned int data;

  if (args.write) {
    if ( rbm_write(args.address, args.value, args.verbose) ) {
      printf("ERROR! Writing to address 0x%X failed!\n", args.address);
    } else{
      // read back
      if ( rbm_read(args.address, &data, args.verbose) ) {
	printf("ERROR! Readback from address 0x%X failed!\n", args.address);
      } else {
	if (args.value==data) {
	  if (args.verbose) {
	    printf("Value  : 0x%X\n", data);
	    printf("OK\n");
	  } else {
	    printf("0x%X\n", data);
	  }
	} else {
	  printf("Value read back from address 0x%X is 0x%X != 0x%X\n",
		 args.address, data, args.value);
	}
      }
    }
  } else {
    if ( rbm_read(args.address, &data, args.verbose) ) {
      printf("ERROR! Reading from address 0x%X failed!\n", args.address);
    } else {
      if (args.verbose) {
	printf("OK\n");
	printf("Value  : 0x%X\n", data);
      } else {
	printf("0x%X\n", data);
      }
    }
  }

  // Set RBM back to direct mode (for feeserver and buspoke)
  if (args.verbose > 1) printf("Setting bus back to direct mode ...\n");
  rbm_direct( args.verbose );

  return 0;
}