component latencybins
"""comp utility for scripts/latency-histogram

Usage:
  Read availablebins pin for the number of bins available.
  Set the maxbinnumber pin for the number of \(+- bins.
    Ensure maxbinnumber <= availablebins
    For maxbinnumber = N, the bins are numbered:
       \-N ... 0 ... + N bins
    (the \-0 bin is not populated)
    (total effective bins = 2*maxbinnumber +1)
  Set nsbinsize pin for the binsize (ns)
  Iterate:
    Set index pin to a bin number: 0 <= index <= maxbinnumber.
    Read check pin and verify that check pin == index pin.
    Read output pins:
         pbinvalue is count for bin = +index
         nbinvalue is count for bin = \-index
         pextra    is count for all bins > maxbinnumber
         nextra    is count for all bins < maxbinnumber
         latency-min is max negative latency
         latency-max is max positive latency

   If index is out of range ( index < 0 or index > maxbinnumber)
   then pbinvalue = nbinvalue = \-1.
   The reset pin may be used to restart.
   The latency pin outputs the instantaneous latency.

Maintainers note: hardcoded for MAXBINNUMBER==1000
""";

pin in  s32 maxbinnumber = 1000;  // MAXBINNUMBER
pin in  s32 index;  //use s32 to avoid 0x hex display in hal
pin in  bit reset;
pin in  s32 nsbinsize;

pin out s32 check;
pin out s32 latency;
pin out s32 latency_max;
pin out s32 latency_min;
pin out s32 pbinvalue;
pin out s32 nbinvalue;
pin out s32 pextra;
pin out s32 nextra;
pin out s32 variance;

// user may interrogate available bins to determine this compiled-in limit
pin out s32 availablebins = 1000; // MAXBINNUMBER

function _ nofp;
variable rtapi_s64  last_timer = 0;
variable int    last_binmax = 0;
variable int    first = 1;
variable int    pbins[1001]; // MAXBINNUMBER+1
variable int    nbins[1001]; // MAXBINNUMBER+1
variable int    binmax = 0;

variable rtapi_u32 nsamples;
variable rtapi_u64 sum;
variable rtapi_u64 sq_sum;

license "GPL";
author "Dewey Garrett";
;;

rtapi_s64     now = rtapi_get_time();
rtapi_s32   lat32 = (rtapi_s32)(now - last_timer - period);
                //(2^31-1)*1nS = 2.147 seconds max

int i;

last_timer = now;

binmax =  maxbinnumber;
if (binmax > availablebins) binmax = availablebins;
last_binmax = binmax;

if (reset) {first = 1;}

if (   first
     || binmax != last_binmax
     || nsbinsize == 0  // important to avoid divide by zero
  ) {
  first = 0;
  latency = 0;
  latency_min = 0x7FFFFFFF;
  latency_max = 0x80000000;
  pextra = 0; nextra = 0;
  for (i = 0; i <= binmax; i++) {
    pbins[i] = 0; nbins[i] = 0;
  }
  nsamples = 0;
  sum = 0;
  sq_sum = 0;
} else {
  latency = lat32;
  i = lat32/nsbinsize;
  if (lat32 > latency_max) latency_max = lat32;
  if (lat32 < latency_min) latency_min = lat32;
  if (i >= 0) {
     if (i > binmax) {
       pextra++;
     } else {
       pbins[i]++;
     }
  } else {
     i = -i;
     if (i > binmax) {
       nextra++;
     } else {
       nbins[i]++;
     }
  }
  nsamples++;
  sum    += lat32;
  sq_sum += lat32 * lat32;
  if (nsamples > 1) {
    // note: division required is: u64/u32
    rtapi_u64 dividend;
    rtapi_u64 divisor;
    dividend = sq_sum - rtapi_div_u64(sum * sum,nsamples);
    divisor  = nsamples -1;
    variance = rtapi_div_u64(dividend,divisor);
  }
}

check = index; // user should verify check==index for reading values
// -1 value indicates illegal index
if (index < 0) {
  pbinvalue = -1;
  nbinvalue = -1;
} else if (index <=binmax) {
  pbinvalue = pbins[index];
  nbinvalue = nbins[index];
} else {
  pbinvalue = -1;
  nbinvalue = -1;
}