#include "FeeSampleClient.hpp"
#include "../CommandCoder/TPCCommandCoderReadback.h"
#include <iomanip>
#include <errno.h>
#include <string>
#include <argp.h>
#include <unistd.h>

//============================================================================
// Stuff for ARGP
const char *argp_program_version = "Readback.app";
const char *argp_program_bug_address = "<christian.lippmann@cern.ch>";
static char doc[] = "Read back ALTRO configuration values through feeserver.";
static char args_doc[] = "SERVERNAME (e.g. TPC-FEE_1_11_1)";

//============================================================================
// The options ARGP understands
static struct argp_option options[] = {
  {"verbose",   'v', 0,            0,  "Produce verbose output" },
  {"quiet",     'q', 0,            0,  "Don't produce any output" },
  {"branch",    'b', "BRANCH",     0,  "specify the branch number [0,1]" },
  {"fec",       'f', "FEC",        0,  "specify the fec number [0..12]" },
  {"altro",     'a', "ALTRO",      0,  "specify the altro number [0..7]" },
  { 0 }
};

//============================================================================
// Used by main to communicate with ARGP parse_opt
struct arguments {
  int32_t silent, verbose;
  int32_t branch, fec, altro;
  string server;
};

//============================================================================
//Get the input argument from argp_parse, which is a pointer to the ARGP arguments structure.
static error_t parse_opt (int key, char *arg, struct argp_state *state) {
  struct arguments *arguments = (struct arguments*)state->input;
  
  switch (key) {   // Parse a single option
    
  case 'q':
    arguments->silent = 1;
    break;

  case 'v':
    arguments->verbose = 1;
    break;

  case 'b':
    arguments->branch = atoi(arg);
    break;
      
  case 'f':
    arguments->fec = atoi(arg);
    break;
      
  case 'a':
    arguments->altro = atoi(arg);
    break;
      
  case ARGP_KEY_ARG:
    arguments->server = arg;
    break;

  case ARGP_KEY_END:
    if (state->arg_num < 1) argp_usage(state);  // Not enough arguments
    break;

  default:
    return ARGP_ERR_UNKNOWN;
  }
  return 0;
}

//============================================================================
static struct argp argp = { options, parse_opt, args_doc, doc };

//============================================================================
//============================================================================
//============================================================================

using namespace std;
using namespace dcs::fee;

/**
 * send a command block to a FeeServer and write result to std output or a file stream
 * The command buffer is transferred to the FeeServer command channel as it is.
 * The target FeeServer must be specified by its name and a FeeClient has to be stated 
 * for the server. NB: one FeeClient can handle more than one FeeServer. 
 *
 * @param pFeeClient  instance of the FeeClient
 * @param pServerName name of the FeeServer to send the command to
 * @param buffer      [in] buffer with the command block <br>
 *                    [out] data received from the FeeServer
 * @return length of received data in bytes, neg. error code if failed
 */
int SendFeeServerCmd(FeeSampleClient* pFeeClient, const char* pServerName,
		     FeeSampleClient::DataArray& buffer) {
  
  int32_t iResult = 0;

  if (pFeeClient && pServerName) {
    /* those variables are used to pass arguments to and fetch results from the
     * specific function of the client */

    uint16_t flags = 0;
    int16_t errorCode = 0;
    int16_t status    = 0;
    uint32_t size = buffer.size()*sizeof(FeeSampleClient::DataArray::value_type);
    string serverName(pServerName);

    iResult = pFeeClient->writeReadData(serverName, size, buffer, flags, errorCode, status);

    if (iResult) {
      cerr << "info: command result size " << size << endl;
      iResult=size;
    } else {
      cerr << "error: readWriteData errorCode = " << errorCode << endl;;
      iResult=-EIO;
    }

  } else {
    iResult=-EINVAL;
  }

  return iResult;
}


//===========================================================================
int32_t strtonum(string number) {
  // convert string to number
  char * strolConversionCheck;
  int32_t retval = strtol(number.c_str(),&strolConversionCheck,0);
  return retval;
}


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

  //
  // ...
  //

  struct arguments arguments;

  // Default values
  arguments.silent = 0;
  arguments.verbose = 0;
  arguments.branch = -1;
  arguments.fec = -1;
  arguments.altro = -1;
  arguments.server = "TPC-FEE_1_11_1";

  // Parse arguments; every option seen by parse_opt will be reflected in arguments
  argp_parse (&argp, argc, argv, 0, 0, &arguments);

  if ( arguments.branch < 0 ) {
    cerr << "*No branch specified!" << endl;
    return -1;
  }
  if ( arguments.fec < 0 ) {
    cerr << "*No fec specified!" << endl;
    return -1;
  }
  if ( arguments.altro < 0 ) {
    cerr << "*No altro specified!" << endl;
    return -1;
  }

  // Get side, sector and partition from target name (e.g. TPC-FEE_1_12_1)
  uint32_t pos1_ = arguments.server.find( "_", 0         );
  uint32_t pos2_ = arguments.server.find( "_", (pos1_+1) );
  uint32_t pos3_ = arguments.server.find( "_", (pos2_+2) );

  uint32_t side   = strtonum(arguments.server.substr((pos1_+1), (pos2_-pos1_-1 )));
  uint32_t sector = strtonum(arguments.server.substr((pos2_+1), (pos3_-pos2_-1 )));
  uint32_t rcu    = strtonum(arguments.server.substr((pos3_+1), (arguments.server.size()-pos3_-1)));

  // The Command Coder creates the data block to be sent to RCU. This will
  // cause the Rcu to read back configuration data into the result mem.
  TPCCommandCoderReadback *CoCo = new TPCCommandCoderReadback();
  CoCo->reset();
  CoCo->setBranch(arguments.branch);
  CoCo->setFec(arguments.fec);
  CoCo->setAltro(arguments.altro);

  uint32_t length = CoCo->createDataBlock((char*)arguments.server.c_str(), 0);
  uint32_t *instrmem  = CoCo->getDataBlock();
  int32_t iResult = 0;

  cout << endl << "*Readback: Instruction block to be sent has " << length << " bytes !" << endl;

  // Write this configuration to the server
  FeeSampleClient* feeClient = new FeeSampleClient();

  if ( arguments.server.c_str() ) {

    if ( feeClient->registerFeeServerName(arguments.server.c_str()) ) {

      if ( feeClient->startFeeClient() ) {

	int32_t iArraySize = length / sizeof(FeeSampleClient::DataArray::value_type);
	FeeSampleClient::DataArray buffer(iArraySize);
	memcpy(&(buffer[0]), instrmem, length);

	// Send buffer once:
	iResult = SendFeeServerCmd(feeClient, arguments.server.c_str(), buffer);
	//feeClient->stopFeeClient();
	//sleep(1);
	//feeClient->startFeeClient();
	// Send buffer second time and get data:
	//memcpy(&(buffer[0]), instrmem, length);
	//iResult = SendFeeServerCmd(feeClient, arguments.server.c_str(), buffer);

	if ( iResult > 0 ) {

	  cout << endl << "*Readback: Received " << iResult << " bytes from "
	       << arguments.server << endl;

	  printf("\n");
	  printf("Side %d, Sector %d, Rcu %d, branch %d, fec: %d, altro %d\n",
		 side, sector, rcu, arguments.branch, arguments.fec, arguments.altro);
	  printf("====================================");
	  printf("========================================\n");
	  printf("             K1         K2         K3         ");
	  printf("L1         L2         L3         VFPED\n");
	  printf("-------------------------------------");
	  printf("---------------------------------------\n");
	  for (int32_t channel=0; channel<16; channel++) {
	    printf("CHANNEL%02d    ", channel);
	    if (!(buffer[channel*14+1 ]>>20)) printf("0x%05x    ", buffer[channel*14+1]);
	    else                              printf("READ_ERROR ");
	    if (!(buffer[channel*14+3 ]>>20)) printf("0x%05x    ", buffer[channel*14+3]);
	    else                              printf("READ_ERROR ");
	    if (!(buffer[channel*14+5 ]>>20)) printf("0x%05x    ", buffer[channel*14+5]);
	    else                              printf("READ_ERROR ");
	    if (!(buffer[channel*14+7 ]>>20)) printf("0x%05x    ", buffer[channel*14+7]);
	    else                              printf("READ_ERROR ");
	    if (!(buffer[channel*14+8 ]>>20)) printf("0x%05x    ", buffer[channel*14+9]);
	    else                              printf("READ_ERROR ");
	    if (!(buffer[channel*14+11]>>20)) printf("0x%05x    ", buffer[channel*14+11]);
	    else                              printf("READ_ERROR ");
	    if (!(buffer[channel*14+13]>>20)) printf("0x%05x    \n", buffer[channel*14+13]);
	    else                              printf("READ_ERROR\n");
	  }
	  printf("--------------------------------------------");
	  printf("---------------------------------\n");
	  printf("ZSTHR=");
	  if (!(buffer[16*14+1]>>20)) printf("0x%05x", buffer[16*14+1]);
	  else                        printf("READ_ERROR");
	  printf(", BCTHR=");
	  if (!(buffer[16*14+3]>>20)) printf("0x%05x", buffer[16*14+3]);
	  else                        printf("READ_ERROR");
	  printf(", TRCFG=");
	  if (!(buffer[16*14+5]>>20)) printf("0x%05x", buffer[16*14+5]);
	  else                        printf("READ_ERROR");
	  printf(", DPCFG=");
	  if (!(buffer[16*14+7]>>20)) printf("0x%05x", buffer[16*14+7]);
	  else                        printf("READ_ERROR");
	  printf(", DPCF2=");
	  if (!(buffer[16*14+9]>>20)) printf("0x%05x\n", buffer[16*14+9]);
	  else                        printf("READ_ERROR\n");
	  printf("=============================================");
	  printf("===============================\n");

	} // end if (iResult > 0)

	feeClient->stopFeeClient();

	delete feeClient;
	feeClient = NULL;

      }

    }

  }

  delete CoCo;
  CoCo = NULL;

  return 1;

}