#include "pcie40_ecs.h"
#include "pcie40_b2slc.h"
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>

typedef struct {
  unsigned int chip_no;
  unsigned int rpc_board;
  __uint16_t thresh_values[48];
}KLMFPGAconfig;

typedef union {
  char charval[2];
  __uint16_t uint16val;
}uint16toChar;

int writeConfig(int fd, KLMFPGAconfig * config) {
  const unsigned int lane_no = 8 + (*config).rpc_board;
  const unsigned int chip_no = (*config).chip_no;

  const __uint16_t sof = 0xF000; // start of frame pre-shifted
  const __uint16_t eof = 0x0008;
  const __uint16_t type_wr = 0x0200; // write command (1) pre-shifted
  const __uint16_t type_ud = 0x0E00; // update command (7) pre-shifted
  const size_t sof_eof_nbits = 4; // SOF/EOF bits
  //    const size_t lane_nbits = 5; // lane bits
  const size_t chip_nbits = 4; // channel bits
  const size_t rc_pkt_size = 3; // 16-bit words

  //    const unsigned int rpc_nbds = 13; // number of boards
  //    const unsigned int rpc_nfpga = 2; // number of FPGAs
  const unsigned int rpc_nchan = 48; // number of channels per FPGA
  const unsigned int rpc_naddr = rpc_nchan / 16 + rpc_nchan; // number of addresses/FPGA

  //    const size_t rpc_pkt_size = rpc_nbds * rpc_nfpga * rpc_naddr * rc_pkt_size; // total size
  const unsigned int fpga_pkt_size = rpc_naddr * rc_pkt_size; // packet size per FPGA

  const __uint16_t rpc_pulser[rpc_nchan / 16] = {0};
  __uint16_t rpc_thresh[48] = {0} ;
  //  const __uint16_t (&rpc_thresh)[48] = config.thresh_values;
  int ii = 0 ;
  for ( ii = 0 ; ii < 48 ; ++ii ) rpc_thresh[ ii ] = (*config).thresh_values[ ii ] ;

  __uint16_t fpga_word[48*3] = {0};
  __uint16_t data, temp;
  unsigned int idx = 0;
  unsigned int addr = 0 ;

  for (addr = 0; addr < rpc_naddr; addr++) {
    if (addr <= 47)
      data = rpc_thresh[addr];
    else
      data = rpc_pulser[addr - 48];
    temp = chip_no | (lane_no << chip_nbits);
    if (addr == rpc_naddr - 1)
      temp = temp | sof | type_ud;
    else
      temp = temp | sof | type_wr;
    fpga_word[idx] = temp;
    temp = (addr << 4) | (data >> 12);
    fpga_word[idx + 1] = temp;
    temp = (data << sof_eof_nbits) | eof;
    fpga_word[idx + 2] = temp;
    idx = idx + 3;
  }

  __uint16_t word;
  uint16toChar uch;
  const size_t fpga_pkt_n_bytes = 2 * fpga_pkt_size;
  char stream_data[fpga_pkt_n_bytes];
  size_t i = 0 ;
  for (i = 0; i < fpga_pkt_size; i++) {
    word = fpga_word[i];
    uch.uint16val = word;
    stream_data[2 * i] = uch.charval[1];
    stream_data[2 * i + 1] = uch.charval[0];
  }
    
  int ret = pcie40_writebytestream(0,fd,fpga_pkt_n_bytes,stream_data);
  return ret;
}

int main(int argc, char *argv[]) { 
  ecs_open( 0 , 2 ) ;

  KLMFPGAconfig config;
  unsigned int thresh = strtoul(argv[1],0,16);
  size_t i = 0 ;
  for (i = 0; i < 48; i++)
    config.thresh_values[i] = thresh;
  printf( "Setting threshold value %d for all channels.\n" , thresh );
  int b = 0 ;
  int c = 0 ;
  int hslb_id = 0 ;
  int try_no = 0 ;
  int ret;

  for(b=0;b<13;b++){
    for( c=0;c<2;c++){
      config.chip_no = c;
      config.rpc_board = b;
                     
      // Stream the config through HSLBs    
      for(hslb_id=0;hslb_id<4;hslb_id++){
	for(try_no = 0; try_no < 3; try_no++){
	  ret = writeConfig(hslb_id, &config);
	  if(ret == 0)
	    break;
	}
	if(ret)
	  printf("Packet streaming failed for HSLB= %d, board= %d, chip= %d\n", hslb_id , b ,  c ) ;
	else
	  printf( "Packet streamed successfully for HSLB= %d, board = %d ,chip= %d\n" , hslb_id , b , c ) ;
	usleep(500);
      }
    }
  }

  
  ecs_close( 0 , 2 ) ;

  return 0 ;
}