//
// Working sample HAL program to talk with DCOL module
//

#include "VMEDevice.hh"
#include "VMEAddressTable.hh"
#include "VMEAddressTableASCIIReader.hh"
#include "hcal/BusAdapterManager.hh"
#include "CAENLinuxBusAdapter.hh"

#include "log4cplus/logger.h"
#include "log4cplus/loglevel.h"
#include "log4cplus/configurator.h"

using namespace HAL;
using namespace std;
#include <iostream>
#include <cstdio>
#include <cstdlib>

#include <vector>
#include <fstream>
#include <string>
#include <cstdio>
#include <cstdlib>

#include <readline/readline.h>
#include <readline/history.h>

extern "C" {
#include "unistd.h"
}
using namespace log4cplus;

static VMEDevice* DCp;
void chk_flash();
int FLASHburn(char *Filename, int backup, int check);
char* rl_gets();
int tokenize( char *buff, char **tokens, int maxt);

#define MAX_TOKENS 10
#define ERROR -1
#define OK 0
int main( int argc, char *argv[] ) {
  unsigned int base = 0x78000000;	// <-- FILL IN CORRECT BASE ADDRESS

  int slot = 15;
  if(argc > 1){
    slot = strtol( argv[1],NULL,0);
    if(slot > 1 && slot < 22){
      printf("slot specified as %d\n",slot);
      base = slot << 27;
    }
    else{
      printf("illegal slot\n");
      return ERROR;
    }
  }
 /* connect to PCI VME bridge */
  CAENLinuxBusAdapter* busAdapter = new CAENLinuxBusAdapter( CAENLinuxBusAdapter::V2718);

  /* create address table reader */
  VMEAddressTableASCIIReader addressTableReader( "DCOL_HAL_preliminary.txt");

  /* read the address table */
  VMEAddressTable addressTable( "Test address table", addressTableReader );

  /* create VME Device */
  DCp = new VMEDevice(addressTable, *busAdapter, base);

  // setup a logger
  log4cplus::BasicConfigurator config;
  config.configure();
  Logger mylog = log4cplus::Logger::getInstance("mylog"); 

  // log everything 
  mylog.setLogLevel( log4cplus::TRACE_LOG_LEVEL);

  static char *tokens[MAX_TOKENS];
  char *input, *input_copy, *command;
  uint32_t offset, value, num_words = 0;
  int i,j = 0,backup,k;
  // unsigned char buffer[2048];
  union int_or_char{
    unsigned int i[512];
    unsigned char c[2048];
  };

  union int_or_char buffer;
  bool quit = false;

  uint32_t cntr = 0, page = 0x200, chnl;
  int cycle, err = 0, sync = 0, evt, ii, first_sync;
  unsigned char chksum;
  while( !quit) {
    
    input = rl_gets();
    input_copy = input;
    
    if(input != NULL && strlen(input)>0) {

      j = tokenize( input, tokens, MAX_TOKENS);
      command = tokens[0];

      switch( toupper( command[0])) {

      case 'H':
	cout << " [h]elp  - display this screen" << endl;
	cout << " r addr [num_words]     - read num_words(default 1) starting from addr  " << endl;
	cout << " w addr value [num_words] [increment]     - write num_words(default 1) starting from addr with starting value and increment(default 1)  " << endl;
	cout << " br addr [num_words]     - BLT read num_words(default 1) starting from addr  " << endl;
	cout << " bw addr value [num_words] [increment]     - BLT write num_words(default 1) starting from addr with starting value and increment(default 1)  " << endl;
	cout << " mr addr [num_words]     - MBLT read num_words(default 1) starting from addr  " << endl;
	cout << " mw addr value [num_words] [increment]     - MBLT write num_words(default 1) starting from addr with starting value and increment(default 1)  " << endl;
	cout << " p file_name sector    - write FLASH with file_name to sector 0/1 (default 1)  " << endl;
	cout << " [q]uit" << endl;
	break;

      case 'R':
	offset = strtoul(tokens[1], NULL, 0);
	if(j > 2)
	  num_words = strtoul(tokens[2], NULL, 0);
	else
	  num_words = 1;
	for(i = 0; i < num_words; i++){
	  switch( toupper( command[1])){
	  case 'B':
	    DCp->read( "BYTE", &value,offset+i);
	    printf("addr: 0x%06x data: 0x%02x\n",offset+i,value & 0xff);
	    break;
	  case 'W':
	    DCp->read( "WORD", &value,offset+i*2);
	    printf("addr: 0x%06x data: 0x%04x\n",offset+i*2,value & 0xffff);
	    break;
	  default :
	    DCp->read( "CMD", &value,offset+i*4);
	    printf("addr: 0x%06x data: 0x%08x\n",offset+i*4,value);
	    break;
	  }
	}
	break;

      case 'W':
	offset = strtoul(tokens[1], NULL, 0);
	value = strtoul(tokens[2], NULL, 0);
	if(j > 3)
	  num_words = strtoul(tokens[3], NULL, 0);
	else
	  num_words = 1;
	if(j > 4)
	  k = strtoul(tokens[4], NULL, 0);
	else
	  k = 1;
	for(i = 0; i < num_words; i++){
	  switch( toupper( command[1])){
	  case 'B':
	    DCp->write( "BYTE", (value+i*k) & 0xff, HAL_NO_VERIFY,offset+i);         // write to "CMD" with no verify (preferred)
	    break;
	  case 'W':
	    DCp->write( "WORD", (value+i*k) & 0xffff, HAL_NO_VERIFY,offset+i*2);         // write to "CMD" with no verify (preferred)
	    break;
	  default :
	    DCp->write( "CMD", value+i*k, HAL_NO_VERIFY,offset+i*4);         // write to "CMD" with no verify (preferred)
	    break;
	  }
	}
	break;

      case 'B':
	switch( toupper( command[1])) {
	case 'W':
	  offset = strtoul(tokens[1], NULL, 0) & 0x1fffffc;
	  value = strtoul(tokens[2], NULL, 0);
	  if(j > 3)
	    num_words = strtoul(tokens[3], NULL, 0) << 2;
	  else
	    num_words = 4;
	  num_words &= 0x1fc;
	  if(num_words == 0)
	    num_words = 4;
	  if(num_words > 256)
	    num_words = 256;
	  if(j > 4)
	    k = strtoul(tokens[4], NULL, 0);
	  else
	    k = 1;
	  printf("offset = 0x%04x num_words = 0x%04x starting value 0x%08x increment 0x%08x\n",offset,num_words,value,k);
	  for(i = 0; i < num_words/4; i++){
	    buffer.i[i] = value;
	    value += k;
	    }
	  try {
	    DCp->writeBlock( "BLT", num_words, (char*) buffer.c, HAL_NO_VERIFY, HAL_DO_INCREMENT, offset);
	  } catch(std::exception& e){
	    printf("Exception caught (block read): %s\n", e.what());
	    abort();
	  }

	  printf("Block write done\n");
	  break;
	case 'R':
	  offset = strtoul(tokens[1], NULL, 0) & 0x1fffffc;
	  if(j > 2)
	    num_words = strtoul(tokens[2], NULL, 0) << 2;
	  else
	    num_words = 4;
	  num_words &= 0x1fc;
	  if(num_words == 0)
	    num_words = 4;
	  if(num_words > 256)
	    num_words = 256;
	  printf("offset = 0x%04x num_words = 0x%04x\n",offset,num_words);

	  try {
	    DCp->readBlock( "BLT", num_words, (char*) buffer.c, HAL_DO_INCREMENT, offset);
	  } catch(std::exception& e){
	    printf("Exception caught (block read): %s\n", e.what());
	    abort();
	  }

	  printf("Block read done\n");
	  for(i = 0; i < (num_words/4); i++){
	    value = buffer.i[i];
	    printf("address: 0x%04x data: 0x%08x\n",offset+4*i,value);
	    }
	  break;
	}
	break;
    
      case 'M':
	switch( toupper( command[1])) {
	case 'W':
	  offset = strtoul(tokens[1], NULL, 0) & 0x1fffff8;
	  value = strtoul(tokens[2], NULL, 0);
	  if(j > 3)
	    num_words = strtoul(tokens[3], NULL, 0) << 2;
	  else
	    num_words = 8;
	  num_words &= 0xff8;
	  if(num_words == 0)
	    num_words = 8;
	  if(num_words > 2048)
	    num_words = 2048;
	  if(j > 4)
	    k = strtoul(tokens[4], NULL, 0);
	  else
	    k = 1;
	  printf("offset = 0x%04x num_words = 0x%04x starting value 0x%08x increment 0x%08x\n",offset,num_words,value,k);
	  for(i = 0; i < num_words/4; i++){
	    buffer.i[i] = value;
	    value += k;
	    }
	  try {
	    DCp->writeBlock( "MBLT", num_words, (char*) buffer.c, HAL_NO_VERIFY, HAL_DO_INCREMENT, offset);
	  } catch(std::exception& e){
	    printf("Exception caught (block read): %s\n", e.what());
	    abort();
	  }

	  printf("Multiplxed Block write done\n");
	  break;
	case 'R':
	  offset = strtoul(tokens[1], NULL, 0) & 0x1fffff8;
	  if(j > 2)
	    num_words = strtoul(tokens[2], NULL, 0) << 2;
	  else
	    num_words = 8;
	  num_words &= 0xff8;
	  if(num_words == 0)
	    num_words = 8;
	  if(num_words > 2048)
	    num_words = 2048;
	  printf("offset = 0x%04x num_words = 0x%04x\n",offset,num_words);

	  try {
	    DCp->readBlock( "MBLT", num_words, (char*) buffer.c, HAL_DO_INCREMENT, offset);
	  } catch(std::exception& e){
	    printf("Exception caught (block read): %s\n", e.what());
	    abort();
	  }

	  printf("Multiplexed Block read done\n");
	  for(i = 0; i < (num_words/4); i++){
	    value = buffer.i[i];
	    printf("address: 0x%04x data: 0x%08x\n",offset+4*i,value);
	    }
	  break;
	}
	break;
    
      case 'L':
	if(j < 2){
	  printf("Usage: loop [input number] [number of test cycles]\n");
	  break;
	}
	chnl = strtoul(tokens[1],NULL, 0);
	chnl--;
	if(j > 2)
	  cycle = strtoul(tokens[2], NULL, 0);
	else
	  cycle = 0x70000000;
	DCp->write( "CMD", 0, HAL_NO_VERIFY,0x28);         // clear Tx CSR
	DCp->write( "CMD", 0xf, HAL_NO_VERIFY,4);         // enable run, skip, circular and standalone
	value = 1 << chnl;
	DCp->write( "CMD", value, HAL_NO_VERIFY,8);         // enable loop back channel
	do{
	  DCp->read( "CMD", &value,0);
	}while(!(value & 0x1)); // check loop back DCM is locked
	DCp->write( "CMD", 1, HAL_NO_VERIFY,0);         // reset all
	do{
	  DCp->read( "CMD", &value,0x8);
	}while(!(value & (1 << (chnl+16)))); // check loop back channel is locked to the incoming bit stream
	DCp->write( "CMD", 0x60000000, HAL_NO_VERIFY,0x28);         // start hit data
	err = 0;
	sync = 0;
	cntr = 0;
	for(i = 0; i < cycle ; i++){
	  do{
	    do{
	      DCp->read( "CMD", &value,0x14);
	      }while((value >> 14) == page);
	    first_sync = 1;
	    for(k = 0; k < 8; k++){
	      DCp->readBlock( "MBLT", 0x800, (char*) buffer.c, HAL_DO_INCREMENT, (page << 14) + (k << 11)); // read 2048 bytes
	      for(evt = 0; evt < 0x80; evt ++){
		chksum = 0x80;
		for(ii = 0; ii < 16 ; ii++)
		  if(ii != 3 && ii != 12)
		    chksum += buffer.c[evt*16+ii];  // chksum calculation 
		if(buffer.i[evt*4] != ((16+chnl) << 27) + (cntr++ & 0xffffff)){
		  if(first_sync == 1 && (buffer.i[evt*4] >> 24) == ((16+chnl) << 3)){
		    printf("w1 sync : 0x%02x buffer = 0x%08x, cntr = 0x%08x\n",sync,buffer.i[evt*4],cntr-1);
		    cntr = buffer.i[evt*4+1];
		    sync ++; 
		    first_sync = 0;
		  }
		  else{
		    if(err < 10)
		      printf("w1 err : 0x%02x buffer = 0x%08x, cntr = 0x%08x\n",err,buffer.i[evt*4],cntr-1);
		    err++;
		  }
		}
		if(buffer.i[evt*4+1] != cntr++){
		  if(err < 10)
		    printf("w2 err : 0x%02x buffer = 0x%08x, cntr = 0x%08x\n",err,buffer.i[evt*4+1],cntr-1);
		  err++;
		}
		if(buffer.i[evt*4+2] != cntr++){
		  if(err < 10)
		    printf("w3 err : 0x%02x buffer = 0x%08x, cntr = 0x%08x\n",err,buffer.i[evt*4+2],cntr-1);
		  err++;
		}
		if(buffer.i[evt*4+3] != chksum + (cntr++ & 0xffffff00)){
		  if(err < 10)
		    printf("w4 err : 0x%02x buffer = 0x%08x, cntr = 0x%08x chksum = 0x%02x\n",err,buffer.i[evt*4+3],cntr-1,chksum);
		  err++;
		}
	      }
	      if(err > 10)abort();
	    }
	    DCp->write( "CMD", 0, HAL_NO_VERIFY,0x20);         // move read pointer by one page
	    if((page++ % 0x100) == 1)
	      printf("cycle : 0x%08x done sync = 0x%04x err = 0x%08x\n",i,sync,err);
	  }while(page != 0x800);
	  page = 0x200;
	  printf("cycle : 0x%08x done sync = 0x%04x err = 0x%08x\n",i,sync,err);
	  if(err > 100)break;
	}
	break;

      case 'P':
	if(j == 2)
	  backup = 1;
	else
	  backup = strtoul(tokens[2], NULL, 0);
	if(j > 3)
	  FLASHburn(tokens[1],backup,1);
	else{
	  FLASHburn(tokens[1],backup,0);
	  FLASHburn(tokens[1],backup,1);
	}
	break;

      case 'Q':
	quit = true;
	break;
      }
    }
  }


}
void chk_flash()
{
  uint32_t rdata;
  //check for FLASH busy
  rdata = 1;
  while(rdata & 1)
    DCp->read( "FLASH_CSR", &rdata, 0);
  //waiting for write process to end
  rdata = 1 << 24;
  DCp->write( "FLASH_write_buffer", (0x5 << 24), HAL_DO_VERIFY, 0);
  while(rdata & (1 << 24)){
    DCp->write( "FLASH_CSR", 1, HAL_NO_VERIFY);
    DCp->read( "FLASH_read_buffer", &rdata, 0);
  }
  return;
}

int FLASHburn(char *Filename, int backup, int check)
{
  FILE *f;
  int ln = 0;
  char line[100];
  int addr;
  int i;
  uint32_t rdata,buf;
  // open mcs file
  f = fopen(Filename,"r");
  if (!f) {
    printf("FLASHburn: Cannot open file %s\n",Filename);
    return ERROR;
  }
  // check device ID
  chk_flash();
  DCp->write( "FLASH_write_buffer", (0x9f <<24), HAL_DO_VERIFY, 0);
  DCp->write( "FLASH_CSR", 0x3, HAL_NO_VERIFY);
  DCp->read( "FLASH_read_buffer", &rdata, 0);
  if((rdata & 0xffffff00) != 0x20201600){
    printf("Wrong FLASH ID : 0x%06x found\n",(rdata & 0xffffff00) >> 8);
    return ERROR;
  }
  if(!check){
    //sector erase
    for(i = 32*backup; i < 32*(1+backup); i++){
      //enable write
      DCp->write( "FLASH_write_buffer", (0x6 << 24),HAL_DO_VERIFY , 0);
      DCp->write( "FLASH_CSR", 0, HAL_NO_VERIFY);
      //waiting for write enable
      chk_flash();
      //execute erase
      DCp->write( "FLASH_write_buffer", (0xd8 << 24)+(i<<16), HAL_DO_VERIFY, 0);
      DCp->write( "FLASH_CSR", 0x3,HAL_NO_VERIFY);
      printf("\rerase secotr%d command sent!",i);
      fflush(stdout);
      //waiting for bulk erase to end
      chk_flash();
    }
    printf("\nerase done!\n");
  }
  addr = backup << 21;
  // loop over record in the file and read them in the array "line"
  while ( fgets(line, 100, f) != NULL) {
    char c1[3],c2[5];
    unsigned char data[200];
    int byte_count,address,rec_type,total;
    ln++;
    if(ln%0x1000 == 0){
      //      printf("\rcurrent line: 0x%x",ln);
      //      fflush(stdout);
      printf("current line: 0x%x\n",ln);
    }
    // check that the record has more than 9 characters
    if ( strlen(line) < 9 ) {
      printf("FLASHburn: Error while reading "
	     "line %d: it has less than 9 characters\n",ln);
      return ERROR;
    }
    // number of bytes in the record (first two hex numbers)
    strncpy(c1,line+1,2);
    c1[2] = '\0';
    sscanf(c1,"%x",&byte_count);
    // address (hex numbers 3-6)
    strncpy(c2,line+3,4);
    c2[4] = '\0';
    sscanf(c2,"%x",&address);
    // record type (hex numbers 7-8)
    strncpy(c1,line+7,2);
    c1[2] = '\0';
    sscanf(c1,"%x",&rec_type);
    // calculate checksum
    total = byte_count+address/256+address%256+rec_type;
    // loop over the rest of the characters in the record and
    // convert them to int
    for (i=0; i<(byte_count+1);i++) {
      strncpy(c1,line+2*i+9,2);
      c1[2] = '\0';
      data[i] = strtol(c1,NULL,16);
      total = total + data[i];
    }
    // check the checksum for the record
    if (total%256 != 0) {
      printf("programChip: Error while reading line "
	     "%d - a checksum error is found\n",ln);
      return ERROR;
    }
    //byte count must be even
    if(rec_type == 0 && byte_count%4 != 0){
      printf("byte number not multiple of 4 at address 0x%06x\n",addr);
      return ERROR;
    }
    // only data records are written
    if (rec_type == 0 && byte_count != 0) {
      for (i=0; i<byte_count; i += 4) {
	if (addr%256 == 0){
	  if(check == 1){
	    chk_flash();
	    DCp->write( "FLASH_write_buffer", (0x3 << 24)+(addr & 0xffff00), HAL_DO_VERIFY, 0);
	    DCp->write( "FLASH_CSR", 0x103, HAL_NO_VERIFY);
	  }
	  else{
	    //enable write
	    chk_flash();
	    DCp->write( "FLASH_write_buffer", (0x6 << 24), HAL_DO_VERIFY, 0);
	    DCp->write( "FLASH_CSR", 0, HAL_NO_VERIFY);
	    //page program command and address
	    DCp->write( "FLASH_write_buffer", (0x2 << 24)+(addr & 0xffff00), HAL_DO_VERIFY, 0);
	  }
	}
	buf = data[i+3] + (data[i+2] << 8) + (data[i+1] << 16) + (data[i] << 24);
	if(check){
	  DCp->read( "FLASH_read_buffer", &rdata, (addr & 0xfc));
	  if(rdata != buf){
	    printf("FLASH error addr = 0x%06x expected data : 0x%08x real data : 0x%08x\n",addr,buf,rdata);
	    return ERROR;
	  }
	}
	else
	  DCp->write( "FLASH_write_buffer", buf, HAL_DO_VERIFY, (addr & 0xfc)+4);
	addr += 4;
	//256 bytes of data in buffer, start page write
	if (addr%256 == 0 && check == 0) {
	  //execute page write
	  DCp->write( "FLASH_CSR", 0x103, HAL_NO_VERIFY);
	  //waiting for page write end
	  chk_flash();
	}
      } // loop over bytes in the line
    } // rec_type equal to 0
  } // loop over lines
  // close mcs file
  //if there are unwritten data in the buffer
  if (addr%256 != 0 && check == 0) {
    //fill buffer with 0xffff
    for(i = (addr & 0xfc); i < 256; i += 4)
      DCp->write( "FLASH_write_buffer", 0xffffffff, HAL_DO_VERIFY, i+4);
    //execute page write
    DCp->write( "FLASH_CSR", 0x103, HAL_NO_VERIFY);
    //waiting for page write end
    chk_flash();
  }
  fclose(f);
  if(check)
    printf("FLASH content is the same as the file %s\n",Filename);
  return OK;
}
/* A static variable for holding the line. */
/* this will probably live somewhere else later. */
static char *line_read = (char *)NULL;

/* Read a string, and return a pointer to it.  Returns NULL on EOF. 
 * totally stolen from the GNU readline documention.  Totally. -MA 
 * don't forget to link with termcap and readline libraries to compile this
 * You'll get undefined symbols if you don't.
 */
char *rl_gets() {
  /* If the buffer has already been allocated, return the memory
     to the free pool. */
  if (line_read)
    {
      free (line_read);
      line_read = (char *)NULL;
    }
  
  /* Get a line from the user. */
 line_read = readline ("> ");

  
  /* If the line has any text in it, save it on the history. */
  if (line_read && *line_read)
    add_history (line_read);

  return (line_read);
}
int tokenize(char *buff, char **tokens, int maxt) {
  int i = 0;
  char *p;

  // first free leftovers
  for( i=0; i<MAX_TOKENS; i++)
    if( tokens[i] != NULL) {
      free( tokens[i]);
      tokens[i] = NULL;
    }

  i = 0;

  p = strtok(buff, " \t");
  tokens[i++]=strdup(p);

  while ( (p = strtok(NULL, " \t")) != NULL ) {
    tokens[i++] = strdup(p);
    if (i == maxt)
      return i;
  }

  return i;
}