#include <Arduino.h>
#include <U8g2lib.h>
//#include <Adafruit_ADS1X15.h>
#include <math.h>


const int8_t ANALOG_PIN = A6;
const int8_t CS = 11;
const int8_t SD = 12;
const int8_t SCLK = 13;

const int8_t UPDATE_PERIOD = 50;
const int16_t DWELL_TIME = 500;
#warning  Проверь максимальное значение напряжения
const int16_t MAX_ADC_VAL = 1023;  // 2^15-1
const float MAX_VOLTAGE = 1.2;     // Максимально допустимое значение напряжение
const uint16_t MAX_VOLTAGE_BITS = 1024*MAX_VOLTAGE/5;   // битовое значение, относящееся к максимальному допустимому напряжению

const int8_t HEIGHT =  128;
const int8_t WIDTH  = 64;

const float VOLTS_PER_BIT = 0.0000078125; //0.0078125 mV

U8G2_SSD1306_128X64_NONAME_F_HW_I2C u8g2(U8G2_R0, /* reset=*/ U8X8_PIN_NONE);
//Adafruit_ADS1115 ads;  /* Use this for the 16-bit version */

void setup() {
  // put your setup code here, to run once:
  u8g2.begin();
  u8g2.setContrast(0);
  u8g2.enableUTF8Print();
  u8g2.setFont(u8g2_font_logisoso28_tn);
  u8g2.setCursor(25,40);
  u8g2.print("hello");
  pinMode(ANALOG_PIN, INPUT);
  
  Serial.begin(9600);
  Serial.println("Indicator has been initialised");



  for (int i = 0; i <= 20; i++) {
    if (i%2 == 0)
      u8g2.drawBox(6, 4+6*i,3,1);
    else
      u8g2.drawBox(7, 4+6*i,1,1);
  }
}



#define DWELL 1
#define FALL 2
#define MAX 3

#define ACCEL MAX_ADC_VAL/300/5

int16_t getBarZ(int16_t analog_val_raw) {
  static int16_t dwell_timer;
  static int16_t speed;
  static int8_t state;
  static int16_t bar_Z;

  if (bar_Z<=analog_val_raw) {
    bar_Z = analog_val_raw;
    state = MAX;
    speed = 0;
    }
  else {
    //bar_peak_Z = ALPHA*analog_val_raw + (1-ALPHA)*bar_peak_Z;
    if ((state!= FALL) and (state!=DWELL)) {
      //если зарегистрировали впервые (фактически, state == MAX)
      state = DWELL;
      dwell_timer = (int16_t)millis();
    }
    
    switch(state) {
      case DWELL:
        if ((int16_t)millis() - dwell_timer > DWELL_TIME) {
          speed = 0;
          state = FALL;
          }
        break;
      case FALL:
        speed =speed + ACCEL;
        if (speed > bar_Z - analog_val_raw)
          bar_Z = analog_val_raw;
        else 
          bar_Z -= speed;
        break;
    }
    //bar_peak_Z -= min(MAX_ADC_VAL/10, bar_peak_Z);
    
  }
  //Serial.print(analog_val_raw), Serial.print(" "), Serial.println(bar_Z);
  //Serial.println(map(bar_Z, 0, MAX_ADC_VAL, 60,4));
  return min(map(bar_Z, 0, MAX_VOLTAGE_BITS, 126,4),120);
}

uint8_t getOrder(uint8_t voltage) {
  uint8_t _voltage = voltage;
  uint8_t order = 0;
  while (_voltage) {
    order++;
    _voltage = _voltage/10;
  }
  return order;
}

void loop() {
  static int16_t prev_val;
  static int16_t bar_peak_Z;
  static bool need_to_update;
  static int16_t timer;
  static float need_to_refresh;
  
  
  
  //int16_t analog_val_raw = ads.readADC_SingleEnded(0);
  int16_t analog_val_raw = analogRead(ANALOG_PIN);
  //float voltage = ads.computeVolts(analog_val_raw);
  float voltage = (float)(analog_val_raw*4.95)/1024;
  uint16_t voltage_percent = round(voltage/MAX_VOLTAGE*100);
  Serial.println(voltage);
  

  if ((int16_t)millis() - timer > UPDATE_PERIOD) {
    u8g2.setCursor(8,40);
    if (voltage_percent <= 100) {

      if (need_to_refresh) {
        need_to_refresh = false;
        u8g2.clear();
        for (int i = 0; i <= 20; i++) {
          if (i%2 == 0)
            u8g2.drawBox(6, 4+6*i,3,1);
          else
            u8g2.drawBox(7, 4+6*i,1,1);
        }
      }
      u8g2.setDrawColor(0);
      u8g2.drawBox(2, 4, 3,126);
      u8g2.drawBox(9, 8, 55, 32);
      u8g2.setDrawColor(1);
      
      for (uint8_t i = 0; i < 3 - getOrder(voltage); i++)
        u8g2.print(" ");
      u8g2.print(voltage_percent);
    }
    else {
      u8g2.setFont(u8g2_font_haxrcorp4089_t_cyrillic);
      u8g2.print("Перегруз");
      need_to_refresh = true;
      u8g2.setFont(u8g2_font_logisoso28_tn);
    }
    // вычисляем координату последнего макс значения (4-120 пикселей). 120 пикселям соответствует цифровая величина >= MAX__VOLTAGE_BITS
    bar_peak_Z = getBarZ(analog_val_raw);   

    u8g2.drawBox(2,bar_peak_Z+1, 3,2);
    int16_t analog_z = constrain(map(analog_val_raw, 0, MAX_ADC_VAL, 126, 4),0,124);
    Serial.print(bar_peak_Z); Serial.print(" "); Serial.println(analog_z);
    u8g2.drawBox(2, analog_z, 3, max(126-analog_z-1,0));
    u8g2.setDrawColor(0);
    u8g2.drawBox(2,bar_peak_Z+1, 3,1);
    u8g2.updateDisplay();
    need_to_update = false;
    timer = (int16_t)millis();
  }
  //delay(20);


}