#define NUMREADSENS 16 //Кол-во считываний с датчика для фильтра

struct SensDATA {
  uint16_t Raws[NUMREADSENS];
  uint16_t Raw;
};

SensDATA Sens[2];

#include <LiquidCrystal_I2C.h>
LiquidCrystal_I2C lcd(0x27, 20, 4);

void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  lcd.init(); lcd.backlight();
}

//int currraw;

void loop() {
  ReadSens();
  UpdLCD();
}

uint8_t tmr1;
void UpdLCD() {
  uint8_t ms = millis();
  if ((uint8_t)(ms - tmr1) >= 200) {
    //lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("1Sens: "); lcd.print(Sens[0].Raw); lcd.print(" ");
    int Bar = ((Sens[0].Raw - 103) / 817.0) * 800;
    int Atm = ((Sens[0].Raw - 103) / 817.0) * 790;
    lcd.setCursor(0, 1);
    lcd.print("Bar: "); lcd.print(Bar / 100.0); lcd.print(" ");
    lcd.print("Atm: "); lcd.print(Atm / 100.0); lcd.print(" ");
    lcd.setCursor(0, 2);
    lcd.print("2Sens: "); lcd.print(Sens[1].Raw); lcd.print(" ");
    Bar = ((Sens[1].Raw - 103) / 817.0) * 800;
    Atm = ((Sens[1].Raw - 103) / 817.0) * 790;
    lcd.setCursor(0, 3);
    lcd.print("Bar: "); lcd.print(Bar / 100.0); lcd.print(" ");
    lcd.print("Atm: "); lcd.print(Atm / 100.0); lcd.print(" ");
    tmr1 = ms;
  }
}

uint8_t tmr;
uint8_t n = 0;
void ReadSens() {
  uint8_t ms = millis();
  if ((uint8_t)(ms - tmr) >= 5) {
    Sens[0].Raws[n] = analogRead(A0) + random(-2, 3);
    Sens[1].Raws[n] = analogRead(A1) + random(-2, 3);
    n++;
    if (n >= NUMREADSENS) {
      FilterRaw(0);
      FilterRaw(1);
      n = 0;
    }
    tmr = ms;
  }
}

void FilterRaw(byte _sens) {
  uint16_t FilterArr[NUMREADSENS];
  byte     RepArr[NUMREADSENS];
  unsigned long time = micros();
  Serial.println("---------------");
  Serial.println(time);
  for (byte n = 0; n < NUMREADSENS; n++) {
    FilterArr[n] = 0; RepArr[n] = 0;
  }
  byte maxRep = 0, indRep = 0;
  for (byte n = 0; n < NUMREADSENS; n++) {
    for (byte k = 0; k < NUMREADSENS; k++) {
      if (FilterArr[k] == 0) {
        FilterArr[k] = Sens[_sens].Raws[n];
        ++RepArr[k];
        if (RepArr[k] > maxRep) {
          indRep = k;
          maxRep = RepArr[k];
        }
        break;
      }
      if (FilterArr[k] == Sens[_sens].Raws[n]) {
        ++RepArr[k];
        if (RepArr[k] > maxRep) {
          indRep = k;
          maxRep = RepArr[k];
        }
        break;
      }
    }
  }

  Serial.print("[");
  for (byte n = 0; n < NUMREADSENS; n++) {
    Serial.print(Sens[_sens].Raws[n]); Serial.print(", ");
  }
  Serial.println("]");

  Serial.print("[");
  for (byte n = 0; n < NUMREADSENS; n++) {
    if (FilterArr[n] == 0) break;
    Serial.print(FilterArr[n]); Serial.print(", ");
  }
  Serial.println("]");

  for (byte n = 0; n < NUMREADSENS; n++) {
    if (FilterArr[n] == 0) break;
    Serial.print(FilterArr[n]); Serial.print(" = "); Serial.println(RepArr[n]);
  }
  Serial.print("indRep: "); Serial.println(indRep);
  Serial.print("Num: "); Serial.println(FilterArr[indRep]);
  Serial.print("maxRep: "); Serial.println(maxRep);
  Serial.println(micros() - time);
  Sens[_sens].Raw = FilterArr[indRep];
}