#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, act_fecs, off, sleep, num_regs, use_pattern, readrp, reset, read, dump;
  unsigned int rp, fecsA, fecsB, pattern;
};

//==========================================================================================
void print_usage() {
  printf("Usage: msm_dump [OPTION]\n");
  printf("Purpose: Read monitoring values from given FECs\n");
  printf("Options:\n");
  printf("-v[LEVEL]   : Set verbosity level (default is 1)\n");
  printf("-p[0..5]    : Use this partition number (0...5. Default: Read from RCU2 register)\n");
  printf("-o          : Switch off FECs, do nothing else\n");
  printf("-d          : Dump BC registers once, do nothing else\n");
  printf("-f[FECLIST] : Use these FECs (comma separated list, no spaces. default: Use active FECs)\n");
  printf("-r[0/1]     : Reset before starting reading (default=1)\n");
  printf("-s[USECS]   : Set sleep time for usleep (default=1000000)\n");
  printf("-n[1..5]    : Set Number of registers to be monitored (default=5)\n");
  printf("-P[PATTERN] : Write, read and compare this pattern. Default: read real values\n");
  printf("-R[PATTERN] : Read and compare this pattern. Default: read real values\n");
}

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

  struct args args;

  // Default values
  args.verbose     = 1;
  args.off         = 0;
  args.readrp      = 1;
  args.rp          = 999;
  args.act_fecs    = 1;
  args.fecsA       = 0;
  args.fecsB       = 0;
  args.reset       = 0;
  args.sleep       = 1000000;
  args.num_regs    = 5;
  args.pattern     = 0x155;
  args.read        = 0;
  args.use_pattern = 0;
  args.dump        = 1;

  while ( 1 ) {
    int result = getopt(argc, argv, "hdor:v:f:p:s:n:P:R:");
    if (result == -1) break; /* end of list */
    switch (result)
      {
      case '?': /* unknown parameter */
	break;
      case ':': /* missing argument of a parameter */
	fprintf(stderr, "missing argument.\n");
	exit(EXIT_FAILURE);
      case 'h':
	print_usage();
	exit(EXIT_SUCCESS);
      case 'v':
	args.verbose = atoi(optarg); // 
	break;
      case 'o':
	args.off = 1; // 
	break;
      case 'd':
	args.dump = 0; // 
	args.read = 1; // 
	break;
      case 'r':
	args.reset = atoi(optarg); // 
	break;
      case 'p':
	// Partition [0..5]
	args.readrp = 0;
        args.rp = atoi(optarg);
        break;
      case 'n':
	// How many BC regs to monitor [1..5]
        args.num_regs = atoi(optarg);
	if (args.num_regs<1) {
	  printf("Error: Monior at least one BC Reg!\n");
	  exit(EXIT_FAILURE);
	} else if (args.num_regs>5) {
	  printf("Error: Maximum number of BC registers to be monitored is 5!\n");
	  exit(EXIT_FAILURE);
	}
        break;
      case 's':
        args.sleep = atoi(optarg);
        break;
      case 'P':
	sscanf( optarg, "%i", &args.pattern );
	args.use_pattern = 1;
	break;
      case 'R':
	sscanf( optarg, "%i", &args.pattern );
	args.use_pattern = 1;
	args.read = 1;
        break;
      case 'f':
	args.act_fecs = 0;
	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("Error: Invalid argument: no such FEC: %s", token);
	    exit(EXIT_FAILURE);
	  }
	} while ( (token = strtok(NULL,",")) );
	free(arg_copy);
	break;
      }
  }
  if (optind < argc) {
  }

  /////////////////////////
  /////////////////////////
  /////////////////////////
  
  unsigned int address;
  unsigned int data;
  unsigned int result;
  int branch = 0;

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

  int currentrp = 99;
  rbm_read(get_Address_PARTITION_NUMBER(), &currentrp, args.verbose);
  if (args.verbose > 1) printf("Currently configured partition number: %i\n", currentrp);

  if (args.reset) {
    if (args.verbose) printf("Reset MSM...\n");
    if (Reset_MSM(args.verbose)) {
      printf("Error: Reset failed ...\n");
      rbm_direct( args.verbose );  // Set RBM back to direct mode (for feeserver and buspoke)
      exit(EXIT_FAILURE);
    }
  }

  // Partition number
  if (args.readrp) {
    if (args.verbose) printf("Re-writing partition number %i\n", currentrp);
    rbm_write(get_Address_PARTITION_NUMBER(), currentrp, args.verbose);
  } else {
    if (args.verbose) printf("Writing partition number %i\n", args.rp);
    rbm_write(get_Address_PARTITION_NUMBER(), args.rp, args.verbose);
  }
  usleep(100000);

  // Switch FECs
  if ( args.off ) {
    args.fecsA = (args.fecsA & 0xFFFF);
    args.fecsB = (args.fecsB & 0xFFFF);
    if (args.verbose) printf("Switching OFF FECs and exit ...\n");
    Switch_Off_FECs(args.verbose);
    rbm_direct( args.verbose );  // Set RBM back to direct mode (for feeserver and buspoke)
    exit(EXIT_SUCCESS);
  } else if ( !args.act_fecs ) {
    args.fecsA = (args.fecsA & 0xFFFF);
    args.fecsB = (args.fecsB & 0xFFFF);
    Write_ACTFECLIST(args.fecsA, args.fecsB, args.verbose);
  }
  Read_ACTFECLIST(&args.fecsA, &args.fecsB, args.verbose);

  if (args.verbose) printf("Setting I2C clock speed ...\n");
  Write_Reg(REG_MSM_SCLK_FREQ, 0x0);

  // FEC reset
  if (args.reset) {
    if (args.verbose) printf("FEC reset ...\n");
    for (branch=0; branch<4; branch++) {
      rbm_write(get_Address_ABI_RST(branch), 0x0, args.verbose);
    }
    usleep(1000);
    for (branch=0; branch<4; branch++) {
      rbm_write(get_Address_ABI_RST(branch), 0x1, args.verbose);
    }
    usleep(1000);
  }

  // Broadcast: Set in CSR0 Automatic conversion ON
  if (args.verbose) printf("Automatic conversion ON ...\n");
  // bit 29: Broadcast
  data = (1<<29) | (0x11<<16) | 0x400;
  Write_Reg(REG_MSM_ADD_CMD_AI, data);
  Write_Reg(REG_MSM_ADD_CMD_AO, data);
  Write_Reg(REG_MSM_ADD_CMD_BI, data);
  Write_Reg(REG_MSM_ADD_CMD_BO, data);

  // Check result registers to see if transaction was ok 
  if (args.verbose > 1) printf("Check result register ...\n");
  result = Read_Reg(REG_MSM_RES_AI);
  if (args.verbose > 1) printf("Result AI: 0x%x\n", result);
  result = Read_Reg(REG_MSM_RES_AO);
  if (args.verbose > 1) printf("Result AO: 0x%x\n", result);
  result = Read_Reg(REG_MSM_RES_BI);
  if (args.verbose > 1) printf("Result BI: 0x%x\n", result);
  result = Read_Reg(REG_MSM_RES_BO);
  if (args.verbose > 1) printf("Result BO: 0x%x\n", result);

  if (args.use_pattern) { // use pattern, otherwise read real values
    
    if ( !args.read ) {

      // Broadcast: Set T_THR 
      if (args.verbose) printf("Set BC T_THR Reg to 0x%X\n", args.pattern);
      // bit 29: Broadcast
      data = (1<<29) | (0x1<<16) | args.pattern;
      Write_Reg(REG_MSM_ADD_CMD_AI, data);
      Write_Reg(REG_MSM_ADD_CMD_AO, data);
      Write_Reg(REG_MSM_ADD_CMD_BI, data);
      Write_Reg(REG_MSM_ADD_CMD_BO, data);
      
      if (args.num_regs > 1) {
	// Broadcast: Set AV_THR 
	if (args.verbose) printf("Set BC A_THR Reg to 0x%X\n", args.pattern);
	// bit 29: Broadcast
	data = (1<<29) | (0x2<<16) | args.pattern;
	Write_Reg(REG_MSM_ADD_CMD_AI, data);
	Write_Reg(REG_MSM_ADD_CMD_AO, data);
	Write_Reg(REG_MSM_ADD_CMD_BI, data);
	Write_Reg(REG_MSM_ADD_CMD_BO, data);
      }

      if (args.num_regs > 2) {
	// Broadcast: Set AC_THR 
	if (args.verbose) printf("Set BC AC_THR Reg to 0x%X\n", args.pattern);
	// bit 29: Broadcast
	data = (1<<29) | (0x3<<16) | args.pattern;
	Write_Reg(REG_MSM_ADD_CMD_AI, data);
	Write_Reg(REG_MSM_ADD_CMD_AO, data);
	Write_Reg(REG_MSM_ADD_CMD_BI, data);
	Write_Reg(REG_MSM_ADD_CMD_BO, data);
      }

      if (args.num_regs > 3) {
	// Broadcast: Set DV_THR 
	if (args.verbose) printf("Set BC DV_THR Reg to 0x%X\n", args.pattern);
	// bit 29: Broadcast
	data = (1<<29) | (0x4<<16) | args.pattern;
	Write_Reg(REG_MSM_ADD_CMD_AI, data);
	Write_Reg(REG_MSM_ADD_CMD_AO, data);
	Write_Reg(REG_MSM_ADD_CMD_BI, data);
	Write_Reg(REG_MSM_ADD_CMD_BO, data);
      }

      if (args.num_regs > 4) {
	// Broadcast: Set DC_THR 
	if (args.verbose) printf("Set BC DC_THR Reg to 0x%X\n", args.pattern);
	// bit 29: Broadcast
	data = (1<<29) | (0x5<<16) | args.pattern;
	Write_Reg(REG_MSM_ADD_CMD_AI, data);
	Write_Reg(REG_MSM_ADD_CMD_AO, data);
	Write_Reg(REG_MSM_ADD_CMD_BI, data);
	Write_Reg(REG_MSM_ADD_CMD_BO, data);
      }
    }
  }

  for (;;) {
    int fec = 0;
    time_t rawtime;
    time(&rawtime);
    for (fec=0; fec<32; fec++) {
      // Skip FECs that are OFF
      if (!checkFecOn(args.fecsA, args.fecsB, fec, args.verbose)) continue;
      branch = getBranchFromFec(fec, args.rp);
      if (args.use_pattern) {
	data = (1<<30) | (fec<<24) | (0x1<<16);
	Write_MSM_ADD_CMD(branch, data);
	result = Read_MSM_RES(branch);
	if (args.verbose) {
	  printf("FEC%02i: T_THR: 0x%x ", fec, result&0x3FF);
	  if ( (result&0x3FF) != args.pattern ) printf("=> NOT OK (expect 0x%X)\n", args.pattern);
	  else                                  printf("=> OK\n");
	}
	if (args.num_regs > 1) {
	  data = (1<<30) | (fec<<24) | (0x2<<16);
	  Write_MSM_ADD_CMD(branch, data);
	  result = Read_MSM_RES(branch);
	  if (args.verbose) {
	    printf("FEC%02i: AV_THR: 0x%x ", fec, result&0x3FF);
	    if ( (result&0x3FF) != args.pattern ) printf("=> NOT OK (expect 0x%X)\n", args.pattern);
	    else                                  printf("=> OK\n");
	  }
	}
	if (args.num_regs > 2) {
	  data = (1<<30) | (fec<<24) | (0x3<<16);
	  Write_MSM_ADD_CMD(branch, data);
	  result = Read_MSM_RES(branch);
	  if (args.verbose) {
	    printf("FEC%02i: AC_THR: 0x%x ", fec, result&0x3FF);
	    if ( (result&0x3FF) != args.pattern ) printf("=> NOT OK (expect 0x%X)\n", args.pattern);
	    else                                  printf("=> OK\n");
	  }
	}
	if (args.num_regs > 3) {
	  data = (1<<30) | (fec<<24) | (0x4<<16);
	  Write_MSM_ADD_CMD(branch, data);
	  result = Read_MSM_RES(branch);
	  if (args.verbose) {
	    printf("FEC%02i: DV_THR: 0x%x ", fec, result&0x3FF);
	    if ( (result&0x3FF) != args.pattern ) printf("=> NOT OK (expect 0x%X)\n", args.pattern);
	    else                                  printf("=> OK\n");
	  }
	}
	if (args.num_regs > 4) {
	  data = (1<<30) | (fec<<24) | (0x5<<16);
	  Write_MSM_ADD_CMD(branch, data);
	  result = Read_MSM_RES(branch);
	  if (args.verbose) {
	    printf("FEC%02i: DC_THR: 0x%x ", fec, result&0x3FF);
	    if ( (result&0x3FF) != args.pattern ) printf("=> NOT OK (expect 0x%X)\n", args.pattern);
	    else                                  printf("=> OK\n");
	  }
	}
      } else {
	if (args.dump) {
	  data = (1<<30) | (fec<<24) | (0x11<<16);
	  Write_MSM_ADD_CMD(branch, data);
	  result = Read_MSM_RES(branch);
	  if (args.verbose) printf("FEC%02i: CSR0: 0x%x\n", fec, result&0x3FF);
	  data = (1<<30) | (fec<<24) | (0x12<<16);
	  Write_MSM_ADD_CMD(branch, data);
	  result = Read_MSM_RES(branch);
	  if (args.verbose) printf("FEC%02i: CSR1: 0x%x\n", fec, result&0x3FF);
	  data = (1<<30) | (fec<<24) | (0x13<<16);
	  Write_MSM_ADD_CMD(branch, data);
	  result = Read_MSM_RES(branch);
	  if (args.verbose) printf("FEC%02i: CSR2: 0x%x\n", fec, result&0x3FF);
	  data = (1<<30) | (fec<<24) | (0x14<<16);
	  Write_MSM_ADD_CMD(branch, data);
	  result = Read_MSM_RES(branch);
	  if (args.verbose) printf("FEC%02i: CSR3: 0x%x\n", fec, result&0x3FF);
	} else {
	  data = (1<<30) | (fec<<24) | (0x6<<16);
	  Write_MSM_ADD_CMD(branch, data);
	  result = Read_MSM_RES(branch);
	  if (args.verbose) printf("FEC%02i: TEMPERATURE: 0x%x\n", fec, result&0x3FF);
	  if (args.num_regs > 1) {
	    data = (1<<30) | (fec<<24) | (0x7<<16);
	    Write_MSM_ADD_CMD(branch, data);
	    result = Read_MSM_RES(branch);
	    printf("FEC%02i: ANALOG VOLTAGE: 0x%x\n", fec, result&0x3FF);
	  }
	  if (args.num_regs > 2) {
	    data = (1<<30) | (fec<<24) | (0x8<<16);
	    Write_MSM_ADD_CMD(branch, data);
	    result = Read_MSM_RES(branch);
	    printf("FEC%02i: ANALOG CURRENT: 0x%x\n", fec, result&0x3FF);
	  }
	  if (args.num_regs > 3) {
	    data = (1<<30) | (fec<<24) | (0x9<<16);
	    Write_MSM_ADD_CMD(branch, data);
	    result = Read_MSM_RES(branch);
	    printf("FEC%02i: DIGITAL VOLTAGE: 0x%x\n", fec, result&0x3FF);
	  }
	  if (args.num_regs > 4) {
	    data = (1<<30) | (fec<<24) | (0xA<<16);
	    Write_MSM_ADD_CMD(branch, data);
	    result = Read_MSM_RES(branch);
	    printf("FEC%02i: DIGITAL CURRENT: 0x%x\n", fec, result&0x3FF);
	  }
	}
      }
    } // end FEC loop
    usleep(args.sleep);
  }
  
  rbm_direct( args.verbose );  // Set RBM back to direct mode (for feeserver and buspoke)

  return 0;
}