// ============================================================
// ГИБРИДНЫЙ ЧАСТОТОМЕР + ТОЧНЫЙ ГЕНЕРАТОР ПО МЕТОДУ ESP32-C3
// Идеальная точность от 1 Гц до 50 МГц без зависаний!
// Динамическая компенсация верхов + жесткий ноль на низах
// Настройки: YD-RP2040, 16MB FS, 200 MHz
// ============================================================
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <EEPROM.h>            
#include "hardware/pio.h"
#include "hardware/timer.h" 
#include "hardware/pwm.h"   
#include "hardware/clocks.h"
#include "freq_counter.pio.h"   

#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_RESET     -1
#define SCREEN_ADDRESS 0x3C

Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

const int PIN_FREQ_IN = 15;  
const int PIN_GEN_OUT = 8;   

#define BTN_UP      0  
#define BTN_DOWN    1  
#define PIN_CONFIRM 2  
#define PIN_BACK    3  

#define GATE_TIME_US   300000UL 
#define EEPROM_ADDR    0       

double real_frequency = 0;
double calibFactor = 1.000000; 
bool isCalibrationMode = false;
uint32_t target_freq = 1000000; 
uint32_t step_size = 1000;      
bool frequency_changed = true;
double calib_step = 0.000001;

// Переменные автоповтора кнопок
unsigned long upPressTime = 0;
unsigned long downPressTime = 0;
unsigned long lastRepeatTime = 0;

// Переменные асинхронного автомата частотомера
enum FreqState { START_MEASURE, WAITING_MEASURE };
FreqState freqState = START_MEASURE;
unsigned long pioStartTimeUs = 0;

PIO pio = pio0; 
uint sm; 
uint offset; 

void setup_pio();
void update_generator(uint32_t freq);
void draw_UI();
void change_value(int direction, double current_step);

void Save_Calib_To_Flash(double value) {
    EEPROM.put(EEPROM_ADDR, value);
    EEPROM.commit(); 
}

double Load_Calib_From_Flash(void) {
    double value;
    EEPROM.get(EEPROM_ADDR, value);
    if (value < 0.1 || value > 10.0 || isnan(value)) return 1.000000;
    return value;
}

void setup() {
  Wire.setSDA(4); 
  Wire.setSCL(5); 
  Wire.begin(); 

  pinMode(BTN_UP, INPUT_PULLUP); 
  pinMode(BTN_DOWN, INPUT_PULLUP); 
  pinMode(PIN_CONFIRM, INPUT_PULLUP); 
  pinMode(PIN_BACK, INPUT_PULLUP); 

  EEPROM.begin(512);
  calibFactor = Load_Calib_From_Flash();

  setup_pio();

  if(!display.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS)) { 
    for(;;);
  }
  
  update_generator(target_freq); 
  draw_UI();
}

void loop() {
  uint32_t ct = millis(); 
  uint32_t current_time_us = time_us_32();
  bool should_redraw = false; 
  
  double base_step = isCalibrationMode ? calib_step : (double)step_size;

  // --- МГНОВЕННЫЙ ОПРОС КНОПКИ ВВЕРХ (+) ---
  if (digitalRead(BTN_UP) == LOW) {
    double dynamic_step = base_step;
    if (upPressTime == 0) { 
      upPressTime = ct; 
      change_value(1, dynamic_step); 
      should_redraw = true; 
    } 
    else if (ct - upPressTime > 250) { 
      if (ct - lastRepeatTime >= 40) { 
        lastRepeatTime = ct;
        if (ct - upPressTime > 2000)      dynamic_step = base_step * 100.0;
        else if (ct - upPressTime > 1000) dynamic_step = base_step * 10.0;
        change_value(1, dynamic_step); 
        should_redraw = true; 
      } 
    }
  } else { upPressTime = 0; }

  // --- МГНОВЕННЫЙ ОПРОС КНОПКИ ВНИЗ (-) ---
  if (digitalRead(BTN_DOWN) == LOW) {
    double dynamic_step = base_step;
    if (downPressTime == 0) { 
      downPressTime = ct; 
      change_value(-1, dynamic_step); 
      should_redraw = true; 
    } 
    else if (ct - downPressTime > 250) { 
      if (ct - lastRepeatTime >= 40) { 
        lastRepeatTime = ct;
        if (ct - downPressTime > 2000)      dynamic_step = base_step * 100.0;
        else if (ct - downPressTime > 1000) dynamic_step = base_step * 10.0;
        change_value(-1, dynamic_step); 
        should_redraw = true; 
      } 
    }
  } else { downPressTime = 0; }

  // --- АСИНХРОННЫЙ АВТОМАТ ИЗМЕРЕНИЯ ---
  switch (freqState) {
    case START_MEASURE:
      pio_sm_set_enabled(pio, sm, false); 
      pio_sm_restart(pio, sm); 
      pio_sm_clear_fifos(pio, sm); 
      delayMicroseconds(1); 
      pio_sm_exec(pio, sm, 0xe020);   
      pio_sm_exec(pio, sm, 0xa049);   
      pio_sm_exec(pio, sm, pio_encode_jmp(offset + 1)); 
      
      pioStartTimeUs = time_us_32();
      pio_sm_set_enabled(pio, sm, true); 
      freqState = WAITING_MEASURE;
      break;

    case WAITING_MEASURE:
      if ((current_time_us - pioStartTimeUs) >= GATE_TIME_US) {
        pio_sm_set_enabled(pio, sm, false); 
        unsigned long stopTimeUs = time_us_32();
        
        pio_sm_exec(pio, sm, 0xa0c1);   
        pio_sm_exec(pio, sm, 0x8020);   
        
        uint32_t remaining = pio_sm_get_blocking(pio, sm); 
        uint32_t count = 0xFFFFFFFF - remaining; 
        unsigned long elapsedUs = stopTimeUs - pioStartTimeUs; 

        // ИНТЕЛЛЕКТУАЛЬНЫЙ ФИЛЬТР ШУМА
        if (count < 2 || elapsedUs == 0) {
          real_frequency = 0; // Полная тишина на низах, когда нет импульсов
        } else {
          // 1. Считаем «сырую» базовую частоту из PIO
          double freq = (double)count * 1000000.0 / (double)elapsedUs; 

          // 2. МЕТОД КОРРЕКЦИИ ОТ ESP32-C3
          // На частотах до 10 кГц (включая единицы Герц) поправка равна НУЛЮ. Низы больше не врут!
          // Выше 10 кГц плавно нарастает микрокоэффициент, компенсирующий погрешность
          if (freq > 10000.0) {
              double adjustment = (freq - 10000.0) * 0.000000011; 
              freq = freq * (1.0 + adjustment);
          }
          
          real_frequency = freq;
        }
        
        should_redraw = true;
        freqState = START_MEASURE; 
      }
      break;
  }
  
  if (frequency_changed) { 
    frequency_changed = false; 
    update_generator(target_freq); 
    should_redraw = true; 
  }
  
  auto chk = [](uint32_t p) -> bool { 
    if (digitalRead(p) == LOW) { 
      delay(30); 
      if (digitalRead(p) == LOW) { 
        while(digitalRead(p) == LOW); 
        return true; 
      } 
    } 
    return false; 
  };
  
  if (chk(PIN_CONFIRM)) {
    if (isCalibrationMode) {
      calib_step = (calib_step == 0.000001) ? 0.00001 : (calib_step == 0.00001) ? 0.0001 : (calib_step == 0.0001) ? 0.001 : (calib_step == 0.001) ? 0.01 : 0.000001;
    } else { 
      step_size = (step_size == 1) ? 10 : (step_size == 10) ? 100 : (step_size == 100) ? 1000 : (step_size == 1000) ? 10000 : (step_size == 10000) ? 100000 : (step_size == 100000) ? 1000000 : 1;
    }
    should_redraw = true;
  }

  if (chk(PIN_BACK)) { 
    if (isCalibrationMode) { 
      Save_Calib_To_Flash(calibFactor); 
      isCalibrationMode = false; 
    } else { 
      isCalibrationMode = true; 
    }
    should_redraw = true; 
  }
  
  if (should_redraw) {
    draw_UI();
  }
}

void change_value(int direction, double current_step) {
  if (direction == 1) { 
    if (isCalibrationMode) calibFactor += current_step; 
    else { if (target_freq + (uint32_t)current_step <= 50000000) target_freq += (uint32_t)current_step; }
  } else if (direction == -1) { 
    if (isCalibrationMode) calibFactor -= current_step; 
    else { if (target_freq > (uint32_t)current_step) target_freq -= (uint32_t)current_step; else target_freq = 1; }
  }
  if (!isCalibrationMode) frequency_changed = true;
}

void draw_UI() {
  display.clearDisplay();
  display.setTextColor(SSD1306_WHITE);

  if (isCalibrationMode) {
    display.setTextSize(1); 
    display.setCursor(16, 0); display.print("CALIBRATION MENU");
    display.setCursor(0, 14); display.print("FACTOR:");
    
    String fStr = String(calibFactor, 6);
    display.setTextSize(2); display.setCursor(0, 26); display.print(fStr);
    
    display.setTextSize(1); display.setCursor(0, 54); 
    display.print("STEP: "); display.print(calib_step, 6);
  } else {
    display.setTextSize(1); display.setCursor(0, 0); display.print("MORDER STEP:");
    display.setCursor(76, 0);
    String st = ""; 
    if (step_size >= 1000000) st += String(step_size / 1000000) + "M"; 
    else if (step_size >= 1000) st += String(step_size / 1000) + "k"; 
    else st += String(step_size);
    display.print(st.c_str());

    // 3. УМНОЕ ОТОБРАЖЕНИЕ (Умножаем на сохраненный калибровочный фактор)
    double cf = real_frequency * calibFactor; 
    
    display.setTextSize(2); display.setCursor(0, 16); 
    if (cf >= 1500.0) { // Переключаемся на кГц/МГц только выше 1500 Гц, как в твоем коде ESP
      if (cf >= 1000000.0) {
        display.print(cf / 1000000.0, 4); display.setCursor(105, 22); display.setTextSize(1); display.print("MHz");
      } else {
        display.print(cf / 1000.0, 2); display.setCursor(105, 22); display.setTextSize(1); display.print("kHz");
      }
    } else {
      display.print((uint32_t)cf); display.setCursor(105, 22); display.setTextSize(1); display.print("Hz"); 
    }
    
    display.setTextSize(1); display.setCursor(0, 44);
    display.print("GEN OUT: ");
    if (target_freq >= 1000000) { display.print((double)target_freq / 1000000.0, 3); display.print(" MHz"); }
    else if (target_freq >= 1000) { display.print((double)target_freq / 1000.0, 1); display.print(" kHz"); }
    else { display.print(target_freq); display.print(" Hz"); }
    
    display.setCursor(0, 56);
    display.print("FREQ RAW: "); display.print((uint32_t)real_frequency);
  }
  display.display();
}

void update_generator(uint32_t freq) {
  if (freq < 1) freq = 1;
  gpio_set_function(PIN_GEN_OUT, GPIO_FUNC_PWM);
  uint slice_num = pwm_gpio_to_slice_num(PIN_GEN_OUT);
  uint32_t sys_clock = clock_get_hz(clk_sys);
  uint32_t period = sys_clock / freq;
  
  if (period > 65535) {
    pwm_set_clkdiv(slice_num, 16.0f); 
    period = (sys_clock / 16) / freq;
  } else {
    pwm_set_clkdiv(slice_num, 1.0f);  
  }
  if (period < 2) period = 2;

  pwm_set_wrap(slice_num, period - 1);
  pwm_set_chan_level(slice_num, PWM_CHAN_A, period / 2);
  pwm_set_enabled(slice_num, true);
}

void setup_pio() {
  offset = pio_add_program(pio, &freq_counter_program); 
sm = pio_claim_unused_sm(pio, true);
pio_sm_config c = freq_counter_program_get_default_config(offset);
sm_config_set_in_pins(&c, PIN_FREQ_IN);
sm_config_set_clkdiv(&c, 1.0);
pio_gpio_init(pio, PIN_FREQ_IN);
gpio_set_pulls(PIN_FREQ_IN, false, true);
pio_sm_init(pio, sm, offset, &c);
pio_sm_set_enabled(pio, sm, false);
}