Slow I2C communication

Question

To collect temperature and BPM, I connected a max30101 in my Heltec WiFi kit V3 with ESP32. The data collected are stored in arrays to extract statistical features from signals which an algorithm will process to predict anxiety or baseline (with two other additional libraries). The problem is that I see a very slow communication on the serial which increase if I add u8x8 library to print values on my builtin OLED. Thus, BPM are always very slow reaching a maximum of 20 BPM.

This is my code:

#include <PlaquetteLib.h> // --> Smoothing and scaling operations [https://sofapirate.github.io/Plaquette/]
using namespace pq; // --> use plaquette library as "pq"
Smoother smoother3(3.0f); // --> third moving mean of 3 seconds for ST values
Smoother smoother2(3.0f); // --> second moving mean of 3 seconds for HR values
#include <Wire.h> // --> I2C connection
#include "MAX30105.h" // --> Sparkfun library for MAX30101 sensor [https://github.com/sparkfun/SparkFun_MAX3010x_Sensor_Library]
MAX30105 particleSensor; // --> used for BPM and ST
#include "Scaler.h" // --> Feature extraction library [https://github.com/eloquentarduino/everywhereml]
#include "Classifier.h" // --> Classification library containing Logistic Regression algorithm [https://github.com/eloquentarduino/micromlgen]
Eloquent::ML::Port::LogisticRegression clf; // --> declare classifier library
#include "heartRate.h" // --> Optical Heart Rate Detection (PBA Algorithm) [https://github.com/sparkfun/SparkFun_MAX3010x_Sensor_Library/blob/master/src/heartRate.h]
#include <Arduino.h>
#include <U8x8lib.h>
U8X8_SSD1306_128X64_NONAME_SW_I2C u8x8(18, 17, 21);

//____________________________________________________________________________________________________________________________________________________________________________
//DEFINING GLOBAL VARIABLES
//hr
const byte RATE_SIZE = 3; //Averaging
byte rates[RATE_SIZE]; //Array of heart rates
byte rateSpot = 0;
long lastBeat = 0; //Time at which the last beat occurred
float beatsPerMinute;
int beatAvg;
//st
float degree; // --> raw peripheral skin temperature in °C
float st; // averaged peripheral skin temperature in °C
unsigned long time_printing = 0; // --> variables for millis delay (1Hz)
const byte WINDOW_SIZE = 60;
byte signal1[WINDOW_SIZE];
byte signal2[WINDOW_SIZE];
byte signal1Index = 0;
byte signal2Index = 0;

//_________________________________________________________________________________________________________________________________________________________________________
void setup() {

  //instrumental initialization
  //Serial.begin(9600);
  Plaquette.begin();
  Wire.begin(41, 42);
  u8x8.begin();

  //hr sensor
  particleSensor.begin(Wire, I2C_SPEED_FAST);
  byte ledBrightness = 0x1F;
  byte sampleAverage = 1; 
  byte ledMode = 3; 
  byte sampleRate = 100;
  int pulseWidth = 411;
  int adcRange = 4096; 
  particleSensor.setup(ledBrightness, sampleAverage, ledMode, sampleRate, pulseWidth, adcRange); 
  particleSensor.setPulseAmplitudeRed(0); //0 mA
  particleSensor.setPulseAmplitudeIR(0);  // 0 mA
  particleSensor.setPulseAmplitudeGreen(0x19); // 1 mA

  //st sensor
  particleSensor.enableDIETEMPRDY(); 

  //u8x8.setFont(u8x8_font_8x13B_1x2_r);
  //u8x8.drawString(0, 4, "Initializing...");
  //delay(2000);
  //u8x8.clearDisplay();
  //u8x8.setFont(u8x8_font_open_iconic_check_4x4);
  //u8x8.drawGlyph(6, 2, 0x40);
  //delay(2000);
  //u8x8.clearDisplay();
  
  }

//_________________________________________________________________________________________________________________________________________________________________________
// BPM values with MAX30101 sensor 1Hz
void hrCollection() {
/**
* The particle sensor capture the green led emission from the inner wrist. When a blood pulse happen, the photodiode capture a pulse of light which corresponds to a heart beat.
* In order to be able to capture peaks throw the wrist, the green led has been accurately set up in the setup() section, following the instructions published by the authors
* on page 22 of the paper. [https://pdfserv.maximintegrated.com/en/an/AN6409.pdf]
* The peaks, corresponding to the BPM, are computed throw a PBA algorithm, then, they are averaged with a moving window of size 3. 
*/
  long irValue = particleSensor.getGreen();

  if (checkForBeat(irValue) == true)
  {
    //We sensed a beat!
    long delta = millis() - lastBeat;
    lastBeat = millis();

    beatsPerMinute = 60 / (delta / 1000.0);

    if (beatsPerMinute < 255 && beatsPerMinute > 20)
    {
      rates[rateSpot++] = (byte)beatsPerMinute; //Store this reading in the array
      rateSpot %= RATE_SIZE; //Wrap variable

      //Take average of readings
      beatAvg = 0;
      for (byte x = 0 ; x < RATE_SIZE ; x++)
        beatAvg += rates[x];
      beatAvg /= RATE_SIZE;

    }
  }

  // Add beatAvg to signal1
  signal1[signal1Index] = beatAvg;
  signal1Index = (signal1Index + 1) % WINDOW_SIZE;

}

//_________________________________________________________________________________________________________________________________________________________________-
// ST values with the particle sensor 1Hz.
void stCollection() {
/**
* Peripheral skin Temperature has been measured in degree Celsius thanks to an InfraRed thermophile that capture the emissivity of the human skin.
*/
  
  degree = particleSensor.readTemperature();

  st = degree >> smoother3;

  // Add st to signal2
  signal2[signal2Index] = st;
  signal2Index = (signal2Index + 1) % WINDOW_SIZE;

}
//____________________________________________________________________________________________________________________________________________________________________________
// Feature Extraction
void extraction(float* features) {
  // Calculate minimum, maximum, mean, median, standard deviation, variance, skewness, and kurtosis for signal1 (beatAvg)
  int beatMin = INT_MAX;
  int beatMax = INT_MIN;
  float beatSum = 0.0;
  float beatSumSquares = 0.0;
  float beatSkewness = 0.0;
  float beatKurtosis = 0.0;
  byte sortedSignal1[WINDOW_SIZE];

  for (byte i = 0; i < WINDOW_SIZE; i++) {
    if (signal1[i] < beatMin) {
      beatMin = signal1[i];
    }
    if (signal1[i] > beatMax) {
      beatMax = signal1[i];
    }
    beatSum += signal1[i];
    beatSumSquares += pow(signal1[i], 2);
    sortedSignal1[i] = signal1[i];
  }

  float beatMean = beatSum / WINDOW_SIZE;

  std::sort(sortedSignal1, sortedSignal1 + WINDOW_SIZE);

  byte middle = WINDOW_SIZE / 2;
  byte beatMedian;
  if (WINDOW_SIZE % 2 == 0) {
    beatMedian = (sortedSignal1[middle - 1] + sortedSignal1[middle]) / 2;
  } else {
    beatMedian = sortedSignal1[middle];
  }

  float beatStdDeviation = sqrt((beatSumSquares / WINDOW_SIZE) - pow(beatMean, 2));
  float beatVariance = pow(beatStdDeviation, 2);

  for (byte i = 0; i < WINDOW_SIZE; i++) {
    float deviation = signal1[i] - beatMean;
    beatSkewness += pow(deviation, 3);
    beatKurtosis += pow(deviation, 4);
  }
  beatSkewness /= (WINDOW_SIZE * pow(beatStdDeviation, 3));
  beatKurtosis /= (WINDOW_SIZE * pow(beatStdDeviation, 4));

  // Calculate minimum, maximum, mean, median, standard deviation, variance, skewness, and kurtosis for signal2 (st)
  float stMin = FLT_MAX;
  float stMax = FLT_MIN;
  float stSum = 0.0;
  float stSumSquares = 0.0;
  float stSkewness = 0.0;
  float stKurtosis = 0.0;
  float sortedSignal2[WINDOW_SIZE];

  for (byte i = 0; i < WINDOW_SIZE; i++) {
    if (signal2[i] < stMin) {
      stMin = signal2[i];
    }
    if (signal2[i] > stMax) {
      stMax = signal2[i];
    }
    stSum += signal2[i];
    stSumSquares += pow(signal2[i], 2);
    sortedSignal2[i] = signal2[i];
  }

  float stMean = stSum / WINDOW_SIZE;

  std::sort(sortedSignal2, sortedSignal2 + WINDOW_SIZE);

  middle = WINDOW_SIZE / 2;
  byte stMedian;
  if (WINDOW_SIZE % 2 == 0) {
    stMedian = (sortedSignal2[middle - 1] + sortedSignal2[middle]) / 2;
  } else {
    stMedian = sortedSignal2[middle];
  }

  float stStdDeviation = sqrt((stSumSquares / WINDOW_SIZE) - pow(stMean, 2));
  float stVariance = pow(stStdDeviation, 2);

  for (byte i = 0; i < WINDOW_SIZE; i++) {
    float deviation = signal2[i] - stMean;
    stSkewness += pow(deviation, 3);
    stKurtosis += pow(deviation, 4);
  }
  stSkewness /= (WINDOW_SIZE * pow(stStdDeviation, 3));
  stKurtosis /= (WINDOW_SIZE * pow(stStdDeviation, 4));

  // Store the calculated features in the features array
  features[0] = beatMin;
  features[1] = beatMax;
  features[2] = beatMean;
  features[3] = beatMedian;
  features[4] = beatStdDeviation;
  features[5] = beatVariance;
  features[6] = beatSkewness;
  features[7] = beatKurtosis;
  features[8] = stMin;
  features[9] = stMax;
  features[10] = stMean;
  features[11] = stMedian;
  features[12] = stStdDeviation;
  features[13] = stVariance;
  features[14] = stSkewness;
  features[15] = stKurtosis;
}
//_____________________________________________________________________________________________________________________________________________________________________________

//_____________________________________________________________________________________________________________________________________________________________________________
void loop() {

  Plaquette.step();

  hrCollection();
  stCollection();

  float features[16]; // Create the features array
  extraction(features); 

  if (!processor.transform(features))
  return;

  //Serial.print(beatsPerMinute); Serial.print(","); Serial.print(st); Serial.print(","); Serial.println(clf.predictLabel(processor.X));

  char heart[3];
  char temperature[2];
  u8x8.setFont(u8x8_font_open_iconic_embedded_2x2);
  u8x8.drawGlyph(4, 1, 0x46);
  u8x8.setFont(u8x8_font_lucasarts_scumm_subtitle_r_2x2_f);
  itoa(beatsPerMinute, heart, 10);
  u8x8.drawString(7, 1, heart);
  itoa(st, temperature, 10);
  u8x8.drawString(7, 3, temperature);
  u8x8.drawString(0, 5, clf.predictLabel(processor.X));
}

When I print on 9600 without u8x8 the printing is normal even if the problem with BPM persists, when I add u8x8 I see on the serial a very slow printing. 115200 baud rate doesn't show anything but empty squares. I am wondering if the problem could be the bad quality wires or I am making some mistake inside the code.