#include <GyverFilters.h>

#include <EEPROM.h>
#include <GyverHX711.h>
#define tmp102Address 0x49
GyverHX711 sensor(2, 3, HX_GAIN64_A);
// HX_GAIN128_A - канал А усиление 128
// HX_GAIN32_B - канал B усиление 32
// HX_GAIN64_A - канал А усиление 64



#define Stable 0x30      //1 Weight signal
#define Not_Stable 0x31  //1 Weight signal
// “kg” 0x6B 0x67
// “a” 0x61
//  “A” 0x41 1
// “-“ 0x2D 1
// “.” 0x2E 1
//-------------------------------------
//======================наборы строк=============
const byte Tail[] = { 0x0D, 0x0A };

//-------------------------------------
float byfer[] = { 1.0, 2.0, 3.0, 1.0, 2.0, 3.0 };   //буфер датчика
float byfer1[] = { 1.0, 2.0, 3.0, 4.0, 5.0, 6.0 };  //
byte SEND[24] = { 0x06, 0x0D, 0x4C, 0x30, 0x0D, 0x47, 0, 0, 0, 0, 0, 0, 0, 0x0D, 0x4C, 0, 0, 0, 0, 0, 0, 0, 0x0D, 0x0A };
struct {
  bool grafik = 1;       // график (1 -включено 0 -выкл) команда k1
  float Koof = 0.01650;  // коофицыент поправки датчика 0.01650
  int per = 200;         // период вывода данных
} coefficient;
long f = 0;
// калмана======================================================
float _err_measure = 1;  // примерный шум измерений
float _q = 0.5;          // скорость изменения значений 0.001-1, варьировать самому
//================================================================
// GLinear<float> ZameD;
long time_Milis;
void setup() {
  Serial.begin(9600);


  if (EEPROM.read(0) != 50) {
    EEPROM.put(10, coefficient);
    EEPROM.write(0, 50);
  } else {
    EEPROM.get(10, coefficient);
    Serial.println();
    delay(100);
    // Serial.println("получили");
  }

  Serial.setTimeout(200);
  Serial.flush();
  // если тарирование при первом запуске -
  // нужно выждать готовность датчика
  delay(500);
  sensor.tare();  // калибровка нуля
  //sensor.sleepMode(true);    // выключить датчик
  //sensor.sleepMode(false);  // включить датчик
  delay(500);
  time_Milis = millis();
}
void loop() {
  ObrKomand();    // проверяем не поступило ли команд
  updateArray();  //обновляем буфер датчика
}
//============o,обновляем буфер весов=================
void updateArray() {
  float q = 1000;
  int sostoyanie;
  static int stab;

  for (byte i = 0; i < 5; i++) {
    byfer[i] = byfer[i + 1];
  }
  if (sensor.available()) {
    // пишем новое значение в последний элемент
    f = sensor.read();
    byfer[5] = simpleKalman(findMedianN_optim((float)f * coefficient.Koof));
  }
  // ZameD.compute(byfer1, byfer, 6);
  //-------------------------------------------------
  // проверка стабильности сигнала==============================
  if (((fabs(fabs(byfer[5]) - fabs(f * coefficient.Koof))) < 3) and (byfer[5] > -2)) {
    sostoyanie = 1;  // стабилизацыя
    stab++;


  } else {
    sostoyanie = -1;  // изминение
    stab -= 2;
    Stabil(false);
    // Serial.println(byfer[5]);
  }
  if (stab < 0) {  // ждем 20 кратного подтверждения стабильности
    Stabil(false);
    stab = 0;
    if ((byfer[5]) < (-1)) {
      sensor.tare();
      // Serial.println("tara-======================================================F   ");
    }
  }
  if (stab > 5) {  // ждем 20 кратного подтверждения стабильности
    Stabil(true);
    // Serial.print("*");
    stab = 6;

  } else {
    Stabil(false);
    //
  }
  vesSend(byfer[5]);
  if (coefficient.grafik) {
    // Serial.print(vesSend(byfer[5]), 3);
    // Serial.print(",");
    // Serial.print(byfer[5] / q, 3);
    // Serial.print(",");
    // Serial.println((f * coefficient.Koof) / q, 3);
  }
  //----------------------------------------------------------
  if (!coefficient.grafik) {

    // Serial.print(vesSend(byfer[5]) * q, 3);
    // Serial.print(",");
    // Serial.println(sostoyanie);
  }
}

#define NUM_READ 3  // порядок медианы
// медиана на N значений со своим буфером, ускоренный вариант
float findMedianN_optim(float newVal) {
  static float buffer[NUM_READ];  // статический буфер
  static byte count = 0;
  buffer[count] = newVal;
  if ((count < NUM_READ - 1) and (buffer[count] > buffer[count + 1])) {
    for (int i = count; i < NUM_READ - 1; i++) {
      if (buffer[i] > buffer[i + 1]) {
        float buff = buffer[i];
        buffer[i] = buffer[i + 1];
        buffer[i + 1] = buff;
      }
    }
  } else {
    if ((count > 0) and (buffer[count - 1] > buffer[count])) {
      for (int i = count; i > 0; i--) {
        if (buffer[i] < buffer[i - 1]) {
          float buff = buffer[i];
          buffer[i] = buffer[i - 1];
          buffer[i - 1] = buff;
        }
      }
    }
  }
  if (++count >= NUM_READ) count = 0;
  return buffer[(int)NUM_READ / 2];
}
float simpleKalman(float newVal) {  // ===============================калмана==============================
  float _kalman_gain, _current_estimate;
  static float _err_estimate = _err_measure;
  static float _last_estimate;
  _kalman_gain = (float)_err_estimate / (_err_estimate + _err_measure);
  _current_estimate = _last_estimate + (float)_kalman_gain * (newVal - _last_estimate);
  _err_estimate = (1.0 - _kalman_gain) * _err_estimate + fabs(_last_estimate - _current_estimate) * _q;
  _last_estimate = _current_estimate;
  return _current_estimate;
}

float vesSend(float ves) {
  byte kratnost;

  if (ves > 15000) {
    kratnost = 10;
  } else {
    kratnost = 5;
  }
  float q = 1000;
  float rez = (lrint(ves / kratnost) * kratnost) / q;
  // Serial.println(rez);
  byte str[7] = { 0x30, 0x30, 0x30, 0x30, 0x30, 0x30, 0x30 };
  // Serial.println(str);
  dtostrf(rez, 7, 3, str);
  for (byte i = 0; i < 7; i++) {
    if (str[i] == 0x20) { str[i] = 0x30; }
  }
  // Serial.println(str);
  strncpy(SEND + 6, str, 7);
  strncpy(SEND + 15, str, 7);
  // Serial.println();
  // Serial.println(sizeof(SEND));
  for (int i = 0; i < sizeof(SEND); i++) {
    // Serial.print(SEND[i]); Serial.print("="); //коды ascii в десятичном формате
  }
  // Serial.println();
  // Serial.write(SEND, 24); //вывод символов ascii (передаются коды)
  return rez;
}
void Stabil(bool w) {
  if (w) {
    SEND[3] = 0x30;
  } else {
    SEND[3] = 0x31;
  }
}
void sending() {
  //  Serial.print("в буфере приема байт - "+String (Serial.available()));
  Serial.write(SEND, 24);
  //  Serial.print("d отправлено");
  Serial.flush();
}
//float getLocalTemperature()
//{
//  OneWire.requestFrom(tmp102Address, 2);
//
//  byte MSB = Wire.read();
//  byte LSB = Wire.read();
//
//  //It's a 12bit int, using two's compliment for negative
//  int TemperatureSum = ((MSB << 8) | LSB) >> 4;
//
//  float celsius = TemperatureSum * 0.0625;
//  return celsius;
//}