Bangle.js HRM basic

hrm.js

var squareroot = x=>Math.sqrt(x);
var c = E.compiledC(`
// void onHRM(int,int,int,JsVar)
// void onInit(void)
// void setCutOffUp(int)
// void setCutOffLow(int)
// void setThreshold(int)

float doubleToFloat(double value);
double floatToDouble(float value);
void print(short num);
void printS(const char * input);

// Function to reverse a string
void reverse(char str[], int length) {
    int start = 0;
    int end = length - 1;
    while (start < end) {
        char temp = str[start];
        str[start] = str[end];
        str[end] = temp;
        start++;
        end--;
    }
}
// Function to convert an integer to a string
void intToStr(int num, char str[]) {
    int i = 0;
    int isNegative = 0;

    // Handle zero separately
    if (num == 0) {
        str[i++] = '0';
    } else {
        // Handle negative numbers
        if (num < 0) {
            isNegative = 1;
            num = -num;
        }

        // Process individual digits
        while (num != 0) {
            int digit = num % 10;
            str[i++] = digit + '0';
            num = num / 10;
        }

        // Add negative sign if the number was negative
        if (isNegative) {
            str[i++] = '-';
        }

        // Reverse the string to get the correct order
        reverse(str, i);
    }

    // Add null terminator
    str[i] = '\0';
}

float sqrt(float infloat) {
  JsVar * G = jspGetNamedVariable("global");
  //JsVar * M = jspGetNamedField(G,"Math",false);
  //JsVar * M = jspGetNamedVariable("Math");
  //JsVar * sqrtt = jspGetNamedField(M,"sqrt",false);
  JsVar * sqrtt = jspGetNamedField(G,"squareroot",false);
  
  //JsVar *f = jsvNewFromFloat(floatToDouble(infloat));
  JsVar *f = jsvNewFromFloat(360.0);

  JsVar * res = jspeFunctionCall(sqrtt,0,0,false,1,&f);
  double d = jsvGetFloat(res);

  float outfloat = doubleToFloat(d);
  outfloat = outfloat / 8;
  outfloat = doubleToFloat(floatToDouble(outfloat));
  jsvUnLock(res);
  jsvUnLock(f);
  jsvUnLock(sqrtt);
  //jsvUnLock(M);
  jsvUnLock(G);

  return outfloat;
}

void print(short num) {
      JsVar *p = jspGetNamedVariable("print");
      char numS[16];
      intToStr(num,numS);
      JsVar *s = jsvNewFromString(numS);
      jsvUnLock(jspeFunctionCall(p,0,0,false,1,&s));

      jsvUnLock(s);
      jsvUnLock(p);
}
void printS(const char * input) {
      JsVar *p = jspGetNamedVariable("print");

      JsVar *s = jsvNewFromString(input);
      jsvUnLock(jspeFunctionCall(p,0,0,false,1,&s));

      jsvUnLock(s);
      jsvUnLock(p);
}

float doubleToFloat(double value) {
    union {
        double d;
        unsigned long long u64;
    } doubleUnion;

    union {
        float f;
        unsigned int u32;
    } floatUnion;

    doubleUnion.d = value;
    int sign = (doubleUnion.u64 >> 63) & 1;
    int exponent = (int)((doubleUnion.u64 >> 52) & 0x7FF);
    unsigned long long fraction = doubleUnion.u64 & 0xFFFFFFFFFFFFF;

    if (exponent == 0x7FF) {
        floatUnion.u32 = (sign << 31) | 0x7F800000 | ((fraction >> 29) & 0x007FFFFF);
    } else if (exponent == 0) {
        int shift = 1023 - 127;  // Double bias - Float bias
        unsigned int denormalized_exp = 1 - shift;
        fraction = fraction >> shift;
        floatUnion.u32 = (sign << 31) | (denormalized_exp << 23) | ((fraction >> 29) & 0x007FFFFF);
    } else {
        exponent -= 1023;  // Double bias
        if (exponent > 127) {
            exponent = 127;
        } else if (exponent < -126) {
            exponent = -126;
        }
        floatUnion.u32 = (sign << 31) | ((exponent + 127) << 23) | ((fraction >> 29) & 0x007FFFFF);
    }
    return floatUnion.f;
}

double floatToDouble(float value) {
    union {
        float f;
        unsigned int u32;
    } floatUnion;
    union {
        double d;
        unsigned long long u64;
    } doubleUnion;
    floatUnion.f = value;
    int sign = (floatUnion.u32 >> 31) & 1;
    int exponent = (floatUnion.u32 >> 23) & 0xFF;
    unsigned int fraction = floatUnion.u32 & 0x7FFFFF;
    
    if (exponent == 0xFF) {
        doubleUnion.u64 = ((unsigned long long)exponent << 52) | ((unsigned long long)fraction << 29);
    } else if (exponent == 0) {
        int shift = 1;
        while ((fraction & 0x40000000) == 0) {
            fraction <<= 1;
            shift++;
        }
        exponent -= (127 + shift - 1);
        doubleUnion.u64 = (sign << 63) | ((unsigned long long)exponent << 52) | ((unsigned long long)fraction << 29);
    } else {
        exponent += (1023 - 127);
        doubleUnion.u64 = (sign << 63) | ((unsigned long long)exponent << 52) | ((unsigned long long)fraction << 29);
    }
    
    return doubleUnion.d;
}
short findMedian(short sorted[], int size ) {
    for (int i = 0; i < size - 1; i++) {
        for (int j = 0; j < size - i - 1; j++) {
            if (sorted[j] > sorted[j + 1]) {
                // Swap elements
                int temp = sorted[j];
                sorted[j] = sorted[j + 1];
                sorted[j + 1] = temp;
            }
        }
    }
    if (size % 2 == 1) {
        return sorted[size / 2];
    } else {
        return (sorted[size / 2 - 1] + sorted[size / 2]) / 2;
    }
}
void identifyAndModifyExtremeValues(short arr[], int size, int lowerPercentile, int upperPercentile, float fraction) {
    short sortedArray[size];
    for (int i = 0; i < size; i++) {
        sortedArray[i] = arr[i];
    }
    short median = findMedian(sortedArray, size);
    for (int i = 0; i < size; i++) {
      if (arr[i] < sortedArray[(lowerPercentile * size) / 100] || arr[i] > sortedArray[(upperPercentile * size) / 100]) {
            // Modify extreme values towards the median
            arr[i] = (int)(arr[i] + fraction * (median - arr[i]));
        }
    }
}

int iabs(int num) {
    return (num < 0) ? -num : num;
}

float fabs(float num) {
    return (num < 0) ? -num : num;
}

float fmin(float a, float b) {
    return (a < b) ? a : b;
}

float fmax(float a, float b) {
    return (a > b) ? a : b;
}

int fsign(float num) {
    if (num > 0) {
        return 1;  // Positive
    } else if (num < 0) {
        return -1; // Negative
    } else {
        return 0;  // Zero
    }
}
int isign(int num) {
    if (num > 0) {
        return 1;  // Positive
    } else if (num < 0) {
        return -1; // Negative
    } else {
        return 0;  // Zero
    }
}

float custom_pow(float base, int exponent) {
    if (exponent == 0) {
        return 1.0f;
    }
    if (base == 0.0f) {
        return 0.0f;
    }
    //printS("base");
    //print(base);
    //printS("expo");
    //print(exponent);

    float result = 1.0f;

    //return result;

    if (exponent > 0) {
        for (int i = 0; i < exponent; ++i) {
            result *= base;
        }
    } else {
        for (int i = 0; i > exponent; --i) {
            result /= base;
        }
    }
    return result;
}

float customLog(float x, float base) {
    if (base <= 0 || base == 1) {
        return -1;  // Handle invalid bases
    }
    if (x <= 0) {
        return -1;  // Handle negative or zero x
    }

    // Use a larger tolerance for faster approximation, but less accuracy
    float tolerance = 0.0001;
    // Approximate ln(x) and ln(base) with fewer terms
    float ln_x = 0;
    float term = 1;
    int i = 1;
    while (term > tolerance) {
        ln_x += term;
        term = term * (x - 1) / (i * x);
        i++;
    }

    float ln_base = 0;
    term = 1;
    i = 1;
    while (term > tolerance) {
        ln_base += term;
        term = term * (base - 1) / (i * base);
        i++;
    }

    return ln_x / ln_base;
}

void normalizeValues(short arr[], int size) {
    short sortedArray[size];
    for (int i = 0; i < size; i++) {
        sortedArray[i] = arr[i];
    }
    short median = findMedian(sortedArray, size);

    /*
    float sumSquaredDeviations = 0;
    for (int i = 0; i < size; i++) {
        sumSquaredDeviations += (arr[i] - median) * (arr[i] - median);
    }

    sqrt(sumSquaredDeviations/size);
    */

    float sumAbsDeviation = 0;
    for (int i = 0; i < size; i++) {
        sumAbsDeviation += iabs(arr[i] - median);
    }

    sumAbsDeviation = sumAbsDeviation / size;

    
    // Normalize values based on Z-score with scaling factor
    for (int i = 0; i < size; i++) {
      short diff = arr[i] - median;
      float zscore =diff/sumAbsDeviation;
      float abs_z = fabs(zscore);

      //printS("ZScore");
      //print(zscore);
      //0 -> 6
      //1/x^2
      //0.5 opens it wider. Inverse proportion
      float k = 0.1*(abs_z*abs_z);

      if ( k < 0.005f ) k = 0.005f;
      float j = 4 / k;
      //arr[i] = median + diff * fmin(j,1.0f);
      //arr[i] = arr[i];
      //remove 30% off of zscore.
      //arr[i] = median + (zscore-0.3*zscore) * sumAbsDeviation;

      //arr[i] = median + diff * fmin(j,1.0f);

      const float squash_th_u = 0.8f;
      const float squash_th_l = -1.6f;

      //try to make outliers sit on the lower area
      //as to not intefere with peaks?
      if ( zscore > 3 ) {
        short newval = median + squash_th_u * sumAbsDeviation;

        arr[i] = newval;
        if ( i>0 ) arr[i-1] = newval;
        if ( i<size-1 ) arr[i+1] = newval;
      }
      else if ( zscore < -3 ) {
        short newval = median + squash_th_l * sumAbsDeviation;

        arr[i] = newval;
        if ( i>0 ) arr[i-1] = newval;
        if ( i<size-1 ) arr[i+1] = newval;
      }
      else if ( zscore > squash_th_u ) {
        arr[i] = median + isign(diff) * squash_th_u * sumAbsDeviation;
        /*
        const float seg = 0.2f;
        short s = fsign(zscore);
        arr[i] = median + (s*squash_th_u + s*(abs_z-squash_th_u)*seg) * sumAbsDeviation;
        
        short d = arr[i-1] - median;
        s = isign(d);
        if ( i>0 ) arr[i-1] = median + (s*squash_th_u +s*(iabs(d)-squash_th_u)*seg);

        d = arr[i+1] - median;
        s = isign(d);
        if ( i<size-1 ) arr[i+1] = median + (s*squash_th_u +s*(iabs(d)-squash_th_u)*seg);
        */
      } else if ( zscore < squash_th_l ) {
        arr[i] = median + squash_th_l * sumAbsDeviation;
        /*
        const float seg = 0.2f;
        arr[i] = median + (squash_th_l + (zscore-squash_th_l)*seg) * sumAbsDeviation;
        
        short d = arr[i-1] - median;
        if ( i>0 ) arr[i-1] = median + (squash_th_l +(d-squash_th_l)*seg);

        d = arr[i+1] - median;
        if ( i<size-1 ) arr[i+1] = median + (squash_th_l +(d-squash_th_l)*seg);
        */
      }
      /*
      else if ( zscore > 1 ) {
        //squash tall outliers
        arr[i] = median + (0.6 * zscore) * sumAbsDeviation;
      } else if ( zscore < -3 ) {
        //raise super low areas.
        //arr[i] = median + (0.05 * zscore) * sumAbsDeviation;
        printS("Fixed...");
        print(arr[i]);
        arr[i] = median;
        print(arr[i]);
      }
      */

      /*
      if ( abs_z > 2) {
        bool found = false;
        for ( int j = i; j >=0;j++ ) {
          float zz =fabs((arr[j] - median)/sumAbsDeviation);
          if ( zz < abs_z ) {
            arr[i] = arr[j];
            found = true;
            break;
          }
        }
        if ( !found ) arr[i] = median;
      }
      */
    }
}


const short HRMVALUE_MIN = -32768;
const short HRMVALUE_MAX = 32767;

typedef union {
    float f;
    int i;
} floatint;

  //5sec
  const unsigned char windowSize = 50;
  short minDeri = 32767;
  short maxDeri = -32768;

  short cutoffUp = 10000;
  short cutfoffLow = -10000;
  float thresh = 0.12;

  typedef struct inputs {
    short * vals;
    short * avgs;
    short * beats;
    short * deris;
    short * diffs;
  } inputs_t;

  int rAverage;
  int dAverage;
  int ddAverage;
  void onInit(void) {
    rAverage = 0;
  }
  int lastBeatTick = 0;
  char beats[250];
  short totalBeats = 0;

  void onHRM(unsigned int &tick,int val,inputs_t *argss, JsVar *fn){
    if (val<HRMVALUE_MIN) val=HRMVALUE_MIN;
    if (val>HRMVALUE_MAX) val=HRMVALUE_MAX;
    int h = val;

    unsigned int nowTick = tick % windowSize;

    argss->vals[nowTick] = h;

    //=========================Running Average

    const short avg_wdw = 8;

    short * avgfrom = argss->vals;
    //rAverage += avgfrom[nowTick];
    //rAverage -= avgfrom[(tick - (avg_wdw - 1)) % windowSize];

    
    rAverage = ((rAverage * avg_wdw) + avgfrom[nowTick]) / (avg_wdw + 1);

    //argss->avgs[nowTick] = h - argss->vals[(tick-1) % windowSize];
    argss->avgs[nowTick] = rAverage;


    //==========================Differencing
    short * diffFrom = argss->avgs;
    short diff = diffFrom[nowTick] - diffFrom[(tick-1) % windowSize];
/*
    dAverage = ((dAverage * avg_wdw) + diff) / (avg_wdw + 1);

    argss->diffs[nowTick] = dAverage;
*/
    argss->diffs[nowTick] = diff;

    //=============================Derivatives

    short * use = argss->diffs;

    unsigned int cc = tick-2;
    //2nd deri approx
    //short deriV = use[(cc-1) % windowSize] - 2 * use[cc % windowSize] + use[(cc+1) % windowSize];

    //short deriV = (use[nowTick] - use[(tick-2) % windowSize] ) / 2;
    //ddAverage = ((ddAverage * avg_wdw) + deriV) / (avg_wdw + 1);

    short deriV = use[nowTick];

    //=====================Limit deriV===================

    //if ( deriV > cutoffUp ) deriV = cutoffUp;
    //if ( deriV < cutfoffLow ) deriV = cutfoffLow;

    argss->deris[nowTick] = deriV;

    //===============Calc Min in window=============
    //2sec
    const short wdw = 40;
    if ( minDeri == argss->deris[(tick-wdw) % windowSize] )
    {
      minDeri = 32767; //recalculate min
      for ( int i = 0; i < wdw; i++ ) {
        if ( argss->deris[(tick-i) % windowSize] < minDeri )
          minDeri = argss->deris[(tick-i) % windowSize];
      }
    } else {
      if ( deriV < minDeri ) minDeri = deriV;
    }

    if ( maxDeri == argss->deris[(tick-wdw) % windowSize] )
    {
      maxDeri = -32768; //recalculate min
      for ( int i = 0; i < wdw; i++ ) {
        if ( argss->deris[(tick-i) % windowSize] > maxDeri )
          maxDeri = argss->deris[(tick-i) % windowSize];
      }
    } else {
      if ( deriV > maxDeri ) maxDeri = deriV;
    }

    // fastBeatLimit = TimeBeat - TimeLastBeat >= 330
    // slowBeatLimit = TimeBeat - TimeLastBeat <= 1400

    if ( deriV < maxDeri*thresh && tick - lastBeatTick >= 8 ) {
      lastBeatTick = tick;
      argss->beats[nowTick] = 1;
      beats[tick % 250] = 1;
      totalBeats += 1;
    }
    else {
      argss->beats[nowTick] = 0;
      beats[tick % 250] = 0;
    }


    //1500 = 60 seconds.
    totalBeats -= beats[(tick + 1) % 250];
    if ( tick % 25 == 0 ) {
      short bpm = totalBeats*6;
      print(bpm);
    }

    if ( tick % windowSize == windowSize-1 ) {
      //calculate beats
      //short crop = 10;
      //identifyAndModifyExtremeValues(argss->diffs,windowSize,crop,100-crop,0.8);

      normalizeValues(argss->deris,windowSize);

      jsvUnLock(jspeFunctionCall(fn,0,0,false,0,0));
    }
    
    tick++;
  }

  void setCutOffUp(int c) {
    cutoffUp = c;
  }
  void setCutOffLow(int c) {
    cutfoffLow = c;
  }
  void setThreshold(int t) {
    floatint tt;
    tt.i = t;
    thresh = tt.f;
  }
`);

let cFloatArray = (val,len)=>{
  val = typeof val !== 'undefined' ? val : 0.0;
  len = typeof len !== 'undefined' ? len : 1;

  let fs = E.toFlatString((new Float32Array(len)).buffer);
  if ( !fs ) throw new Error("Required to be a flatstring");
  let f = new Float32Array(E.toArrayBuffer(fs));

  let i = new Int32Array(f.buffer);
  let r = E.getAddressOf(f.buffer,true);
  if ( r % 4 != 0 ) throw new Error("Required to be a aligned");

  f.fill(val);
  return {
    f: f,
    i: i,
    ref: r,
    store: s=>{
      i[0]=s;
    },
    get: ()=>f[0]
  };
};

let newUint32FlatArray = len => {
  let fs = E.toFlatString((new Uint32Array(len)).buffer);
  if ( !fs ) throw new Error("Required to be a flatstring");
  let obj = new Uint32Array(E.toArrayBuffer(fs));
  return {"obj" : obj, "ref" : E.getAddressOf(obj,true) };
};
let newInt16FlatArray = len => {
  let fs = E.toFlatString((new Int16Array(len)).buffer);
  if ( !fs ) throw new Error("Required to be a flatstring");
  let obj = new Int16Array(E.toArrayBuffer(fs));
  return {"obj" :  obj, "ref" : E.getAddressOf(obj,true) };
};

let setThreshold = (t) => {
  c.setThreshold(cFloatArray(t).i[0]);
};

Bangle.setBacklight(0);
Bangle.setLCDTimeout(0);
Bangle.setOptions({hrmSportMode:0});
Bangle.setHRMPower(1);//greenadjust+weardetect set by hrmPowerOn
Bangle.setOptions({
  powerSave:false,
  //hrmGreenAdjust: false
});
Bangle.setPollInterval(80);
//Bangle.hrmWr(0x17,5 ); //| 0x80
//9 == best , 8?
//Bangle.hrmRd(0x17);
let windowSize = 50;

var vals = newInt16FlatArray(windowSize);
vals.obj.fill(0);

var avgs = newInt16FlatArray(windowSize);
avgs.obj.fill(0);

var beats = newInt16FlatArray(windowSize);
beats.obj.fill(0);

var deris = newInt16FlatArray(windowSize);
deris.obj.fill(0);

var diffs = newInt16FlatArray(windowSize);
diffs.obj.fill(0);


var inputs = newUint32FlatArray(6);
inputs.obj[0] = vals.ref;
inputs.obj[1] = avgs.ref;
inputs.obj[2] = beats.ref;
inputs.obj[3] = deris.ref;
inputs.obj[4] = diffs.ref;

let tickFlat = newUint32FlatArray(1);
tickFlat.obj.fill(0);

c.onInit();

let savedBeats = Array(12).fill(0);
let intervalTick = 0;
//25Hz, Bangle.js 2
//50Hz, Bangle.js 1
let lastTick = 0;

let graphIt = ()=>{
    intervalTick += 1;
    let tick = tickFlat.obj[0];
    let tt = tick-lastTick;
    print(`${tt} ticks have passed ${tt/5} ${tick % windowSize}`); 
    lastTick = tick;
    g.clear(false);
  
    let nonCircVals = [];
    let nonCircAvgs = [];
    let nonCircBeats = [];
    let nonCircDeris = [];
    let nonCircDiffs = [];
  
    let min = 40000;
    let max = -40000;
    let minA = 40000;
    let maxA = -40000;
    let minD = 40000;
    let maxD = -40000;
  
    let minDi = 40000;
    let maxDi = -40000;
    
    let bpI = 0;
    //circular to non-circular.
    for ( let i = 0; i < windowSize; i++ ) {
      let v = beats.obj[(tick+1+i) % windowSize];
      nonCircBeats.push(v);
      if ( v == 1 ) bpI +=1;
      
      
      v = deris.obj[(tick + 1 + i) % windowSize];
      nonCircDeris.push(v);
      if ( v < minD ) minD = v;
      if ( v > maxD ) maxD = v;
      
      v = avgs.obj[(tick + 1 + i) % windowSize];
      if ( v < minA ) minA = v;
      if ( v > maxA ) maxA = v;
      nonCircAvgs.push(v);
      
      v = vals.obj[(tick + 1 + i) % windowSize];
      if ( v < min ) min = v;
      if ( v > max ) max = v;
      nonCircVals.push(v);
      
      v = diffs.obj[(tick+1+i) % windowSize];
      if ( v < minDi ) minDi = v;
      if ( v > maxDi ) maxDi = v;
      nonCircDiffs.push(v);
    }
    let idx = intervalTick % 12;
    savedBeats[idx] = bpI;
  
    let print_bpm = 0;
    for ( let i = 0 ; i < 12; i++ ) {
       print_bpm += savedBeats[(intervalTick+1+i) % 12];
    }
    print_bpm = intervalTick<12 ? print_bpm*(12/intervalTick) : print_bpm;
    print(`min is ${min}, max is ${max}`);
    print(`minD is ${minD}, maxD is ${maxD}`);
    //black
    g.setColor(0,0,0).setFont("Vector:16");
    //require("graph").drawLine(g, nonCircVals,{axes:false,miny:min,maxy:max,gridy:(Math.abs(max-min))/8});
    g.setColor(0,0,1);
    //require("graph").drawLine(g, nonCircAvgs,{axes:false,miny:minA,maxy:maxA,gridy:(Math.abs(maxA-minA))/8});
    g.setColor(0,1,0);
    //require("graph").drawLine(g, nonCircBeats,{axes:false,miny:0,maxy:1});
    g.setColor(1,0,0);
    require("graph").drawLine(g, nonCircDeris,{axes:false,miny:minD,maxy:maxD});
    
    g.setColor(0,0,0);
    //require("graph").drawLine(g, nonCircDiffs,{axes:false,miny:minDi,maxy:maxDi});
    
    //g.setColor(0,0,0).setFont("Vector:48").drawString(print_bpm.toString(),48,0);

};

//This is called by idleLoop. Not an interrupt.
Bangle.on('HRM-raw', function(hrm) {
  let val = hrm.raw;
  //print(`HRM-Raw : ${val}`);
  c.onHRM(tickFlat.ref,val,inputs.ref,graphIt);
});