#include <cstring>

#define NSAMP 5

/* === Accumulators for averages and RMS === */
int avgL[NSAMP][32];
int avgR[NSAMP][32];
int rmsL[NSAMP][32];
int rmsR[NSAMP][32];
int eventCounter;

/* === I/O buffer for sending the accumulated data to VME === */
int iobuf[NSAMP][128];

#define SIZE_IN NSAMP*32+20

#define FREE1 1
#define RDY1  2
#define FREE2 4
#define RDY2  8
#define nBSY1 (FREE1 | RDY1)
#define nBSY2 (FREE2 | RDY2)

extern cregister volatile unsigned int IFR, ICR, ISR;

/* We may use DMA_1[i] = DMA_0[i+16] and DMA_2[i] = DMA_0[i+1] */
volatile unsigned int *const DMA_0 = (unsigned int *) 0x01840000;
volatile unsigned int *const DMA_1 = (unsigned int *) 0x01840040;
volatile unsigned int *const DMA_2 = (unsigned int *) 0x01840004;
volatile unsigned int *const DMA_3 = (unsigned int *) 0x01840044;

volatile unsigned int *const TCTL = (unsigned int *) 0x01940000; /* Timer 0 */
volatile unsigned int *const SER2 = (unsigned int *) 0x01a40000;
volatile unsigned int *const SER1 = (unsigned int *) 0x01900000;
volatile unsigned int *const SER0 = (unsigned int *) 0x018c0000;
volatile unsigned int *const RRAM = (unsigned int *) 0x01400000;
#define src_ptr_in(n) (RRAM + (((n) & 127) << 8))
#define COUNTERS 0x7fff

/* McBSP2 Pin Control Register values */
#define BUSY_ON  0x3101
#define BUSY_OFF 0x3100

/* Control of the BUSY logic. BUSY will be ON when there are more than
   NEVT_BUSY_ON events waiting in the dual-port RAM. BUSY will be off
   when there are less than NEVT_BUSY_OFF events waiting. */
#define NEVT_BUSY_ON  20
#define NEVT_BUSY_OFF 10

/* Data sections shared with DMA are in block 1, everything else in block 0 */
#pragma DATA_SECTION(".febdata")
unsigned int data_in1[512];
#pragma DATA_SECTION(".febdata")
unsigned int data_in2[512];

extern "C"{
  void averages(unsigned int *);
  void serviceSerial(void);
  void periph_init(void);
  int  pause(void);
}

int main()
{
  int nevt_input, nevt_input_next;

  /* === Peripherals initialization */
  periph_init();

  /* === Initialization of DMA channel 3 destination address */
  DMA_3[6] = 0x60000000;
  /* === DMA Global Counter Reload B register, for histograms */
  DMA_0[11] = 256;


  /* --- Debugging only: set timer period */
  TCTL[1] = -1;
  TCTL[0] = 0x2c0;


#ifdef WAIT
  /* === Wait for INT7 */
  {
    int ier = pause();
    SER0[1] = (unsigned int) ier;
  }
#endif

  nevt_input_next = 0; /* Next event to be copied by Input DMA */

  unsigned short bufStates = (FREE1 | FREE2);

  ISR = 0x80;  /* Set INT7 which causes the accumulators to be reset */

  nevt_input = RRAM[COUNTERS] & 0xffff;

  /* Clear the BUSY flag */
  if (nevt_input < NEVT_BUSY_OFF) SER2[9] = BUSY_OFF;

  while(1){

    /* === Polling [see note] */
    if (bufStates & (FREE1 | FREE2 | RDY1) &&
	_extu(nevt_input, 16, 16) != _extu(RRAM[COUNTERS], 16, 16)){

      /* Read the counter a second time */
      nevt_input  = _clr(nevt_input, 0, 15);
      nevt_input += _extu(RRAM[COUNTERS], 16, 16);

      /* The input event counter is a 16-bit counter!*/
      if (nevt_input < nevt_input_next)
	nevt_input += 0x10000;

      /* Busy logic: set the busy flag */
      if (nevt_input - nevt_input_next > NEVT_BUSY_ON)
	SER2[9] = BUSY_ON;
    }

    /* Busy logic: clear the BUSY flag */
    if (nevt_input - nevt_input_next == NEVT_BUSY_OFF)
      SER2[9] = BUSY_OFF;


    if (nevt_input - nevt_input_next != 0 && bufStates & FREE1){
      DMA_0[4] = (unsigned int) src_ptr_in(nevt_input_next);
      DMA_0[6] = (unsigned int) data_in1;
      DMA_0[8] = SIZE_IN;
      DMA_0[0] = 0x51;
      bufStates ^= FREE1; // FREE -> BUSY
      nevt_input_next++;
    }
    else if (!(bufStates & nBSY1) && !(DMA_0[0] & 12))
      bufStates |= RDY1;  // BUSY -> RDY

    if (bufStates & FREE2 && nevt_input - nevt_input_next != 0){
      DMA_0[20] = (unsigned int) src_ptr_in(nevt_input_next);
      DMA_0[22] = (unsigned int) data_in2;
      DMA_0[24] = SIZE_IN;
      DMA_0[16] = 0x51;
      bufStates ^= FREE2; // FREE -> BUSY
      nevt_input_next++;
    }

    /* Ready input */
    if (bufStates & RDY1){

#ifdef scope_debug
      /* --- Set FSX pin (scope probe) */
      SER1[9] = 0x3808;
#endif

      /* --- Process event */
      averages(data_in1);

      /* --- Input buffer becomes free */
      bufStates ^= (FREE1 | RDY1);

#ifdef scope_debug
      /* --- Clear FSX pin (scope probe) */
      SER1[9] = 0x3800;
#endif

      /* goto loop2 (fallthrough, likely to occur at high trigger rate) */
    }
    else{
      /* Testing INT10. If any serial data was received from VME,
	 then the serviceSerial() service subroutine is called */
      if (_ext(IFR, 21, 31)) serviceSerial();
      continue; /* goto loop1 */
    }


  loop2:

    /* === Polling [see note] */
    if (bufStates & (FREE1 | FREE2 | RDY2) &&
	_extu(nevt_input, 16, 16) != _extu(RRAM[COUNTERS], 16, 16)){

      /* Read the counter a second time */
      nevt_input  = _clr(nevt_input, 0, 15);
      nevt_input += _extu(RRAM[COUNTERS], 16, 16);

      /* The input event counter is a 16-bit counter!*/
      if (nevt_input < nevt_input_next)
	nevt_input += 0x10000;

      /* Busy logic: set the busy flag */
      if (nevt_input - nevt_input_next > NEVT_BUSY_ON)
	SER2[9] = BUSY_ON;
    }

    /* Busy logic: clear the BUSY flag */
    if (nevt_input - nevt_input_next == NEVT_BUSY_OFF)
      SER2[9] = BUSY_OFF;


    if (nevt_input - nevt_input_next != 0 && bufStates & FREE2){
      DMA_0[20] = (unsigned int) src_ptr_in(nevt_input_next);
      DMA_0[22] = (unsigned int) data_in2;
      DMA_0[24] = SIZE_IN;
      DMA_0[16] = 0x51;
      bufStates ^= FREE2;
      nevt_input_next++;
    }
    else if (!(bufStates & nBSY2) && !(DMA_0[16] & 12))
      bufStates |= RDY2;

    if (bufStates & FREE1 && nevt_input - nevt_input_next != 0){
      DMA_0[4] = (unsigned int) src_ptr_in(nevt_input_next);
      DMA_0[6] = (unsigned int) data_in1;
      DMA_0[8] = SIZE_IN;
      DMA_0[0] = 0x51;
      bufStates ^= FREE1;
      nevt_input_next++;
    }

    /* Ready input */

    if (bufStates & RDY2){

#ifdef scope_debug
      /* --- Set FSX pin (scope probe) */
      SER1[9] = 0x3808;
#endif

      /* --- Process event */
      averages(data_in2);

      /* --- Input buffer becomes free */
      bufStates ^= (FREE2 | RDY2);

#ifdef scope_debug
      /* --- Clear FSX pin (scope probe) */
      SER1[9] = 0x3800;
#endif

      /* goto loop1 (likely to occur at high trigger rate) */
    }
    else{
      /* Testing INT10. If any serial data was received from VME,
	 then the serviceSerial() service subroutine is called */
      if (_ext(IFR, 21, 31)) serviceSerial();
      goto loop2;
    }
  }
}


/* ----------------------------------------------------------------------
   This subroutine expects the following data arrangement in the
   dual-port RAM: data starts with one full block of 32 ADC words
   for the 64 channels. The channel number varies first, and then
   the sampling number.

   Please note that the data arrangement by the FPGA is defined
   in the subroutine fpga_map() on the host side. The correct
   arrangement for this routine is achieved with the following
   statement in fpga_map():
   WRITE (LUN,'(I6)') ((ADC1(i,isamp),i=0,31),isamp=0,NSAMP-1), ERR_FLAG

   Later we will add a condition such that the data are accumulated
   only if the event is error-free, by testing the FPGA error status
   word.

   For the moment it only works with one gain read out, but could be
   easily extended for more than one gain.

   For the C/C++ intrinsics _sub2, _mpy and others, please refer to the
   TMS320C6000 Optimizing C Compiler User's Guide SPRU187G Section 8.5.2.
   ---------------------------------------------------------------------- */
void averages(unsigned int *data)
{
  /* === Check error status === */
  if (data[NSAMP*32] & 0xfd) return;

  /* === First event after INT7 resets all accumulators === */
  if (IFR & 0x80){

    ICR = 0x80;         /* Manual clear of INT7 */
    eventCounter = 1;   /* The number of events accumulated for averaging */

    /* Loop over samples */
    for(int isamp=0; isamp<NSAMP; isamp++)

      /* Loop over 64 channels. There is one 32-bit word for 2 channels */
      for(int ich=0; ich<32; ich++){

	/* Load the ADC for two paired channels */
	int adcWord = *data++;

	/* Subtract 1000 ADC counts from each side to preserve dynamic range */
	adcWord = _sub2(adcWord, 0x03e803e8);

	/* Set the left and right ADC values */
	avgR[isamp][ich] = _ext(adcWord, 16, 16);
	avgL[isamp][ich] = _ext(adcWord,  0, 16);

	/* Set the left and right ADC^2 */
	rmsR[isamp][ich] = _mpy(adcWord, adcWord);
	rmsL[isamp][ich] =_mpyh(adcWord, adcWord);
      }
  }


  /* === Normal accumulation === */
  else{

    eventCounter++;   /* The number of events accumulated for averaging */

    /* Loop over samples */
    for(int isamp=0; isamp<NSAMP; isamp++)

      /* Loop over 64 channels. There is one 32-bit word for 2 channels */
      for(int ich=0; ich<32; ich++){

	/* Load the ADC for two paired channels */
	int adcWord = *data++;

	/* Subtract 1000 ADC counts from each side to preserve dynamic range */
	adcWord = _sub2(adcWord, 0x03e803e8);

	/* Accumulate the left and right ADC values */
	avgR[isamp][ich] += _ext(adcWord, 16, 16);
	avgL[isamp][ich] += _ext(adcWord,  0, 16);

	/* Accumulate the left and right ADC^2 */
	rmsR[isamp][ich] += _mpy(adcWord, adcWord);
	rmsL[isamp][ich] +=_mpyh(adcWord, adcWord);
      }
  }
}


/* ----------------------------------------------------------------------
   Service a serial command. In this case, the accumulated data
   (pedestals and RMS) are copied to a dedicated I/O buffer and
   then a DMA is scheduled to transfer the data to the histogram FIFO.
   In addition, the number of events accumulated is sent back serially.
   ---------------------------------------------------------------------- */
void serviceSerial()
{
  /* --- Manual clear of INT10 */
  ICR = 0x400;

  /* --- Test if the DMA channel 3 is busy. If so, send -1 back to the FPGA */
  if (DMA_3[0] & 12){
    SER0[1] = -1;
    return;
  }

  /* --- Send the number of events accumulated so far */
  SER0[1] = eventCounter;

  /* --- If only the event counter was requested, then we are done */
  if (SER0[0] == 0) return;

#ifdef scope_debug
  /* --- Set FSX pin (scope probe) */
  SER1[9] = 0x3808;
#endif

  /* --- Copy the data to the I/O buffer */
  for(int isamp=0; isamp<NSAMP; isamp++)
    for(int ich=0; ich<32; ich++){
      iobuf[isamp][ich]    = avgL[isamp][ich];
      iobuf[isamp][ich+32] = avgR[isamp][ich];
      iobuf[isamp][ich+64] = rmsL[isamp][ich];
      iobuf[isamp][ich+96] = rmsR[isamp][ich];
    }

  /* --- Start copying the I/O buffer to the histogram FIFO via DMA */
  int fc, wc;                          /* Frame count and word count */

  DMA_3[2] = 0x00180008;               /* Secondary control register */
  DMA_3[4] = (unsigned int) &iobuf;    /* Source address */
  if (NSAMP & 1){
    fc = 1 + NSAMP >> 1;
    wc = 128; /* First frame is 128 words */
  }
  else{
    fc = NSAMP >> 1;
    wc = 256; /* All frames are 256 words */
  }    
  DMA_3[8] = (fc << 16) + wc;
  DMA_3[0] = 0x04015011;               /* Trigger the transfer */

#ifdef scope_debug
  /* --- Clear FSX pin (scope probe) */
  SER1[9] = 0x3800;
#endif
}