// ============================================================
// ПРЕЦИЗИОННЫЙ ЛАБОРАТОРНЫЙ ЧАСТОТОМЕР ДО 50 МГц (HARD DIVIDER)
// 1 вход строго на GP15. Аппаратный делитель на 8 устраняет пропуски.
// Настройки в IDE: YD-RP2040, 16MB FS, 133 MHz
// ============================================================
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <EEPROM.h>            
#include "hardware/pwm.h"   
#include "hardware/timer.h" 
#include "hardware/clocks.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 EEPROM_ADDR    0       
#define GATE_TIME_US   400000UL // Ворота измерения 400 мс

uint pwm_slice_num;
uint32_t pwm_overflow_counter = 0;
uint32_t last_hw_raw_count = 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;
unsigned long lastDisplayUpdateMs = 0;

uint64_t last_gate_time_us = 0;
uint32_t last_total_pulses = 0;

void setup_hardware_counter();
uint32_t get_full_32bit_pulses();
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(); 
}

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

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

  EEPROM.begin(512);
  EEPROM.get(EEPROM_ADDR, calibFactor);
  if (calibFactor < 0.1 || calibFactor > 10.0 || isnan(calibFactor)) calibFactor = 1.000000;

  setup_hardware_counter();

  if(!display.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS)) { 
    for(;;);
  }
  
  last_gate_time_us = time_us_64();
  last_total_pulses = get_full_32bit_pulses();
  
  update_generator(target_freq); 
  draw_UI();
}

void loop() {
  uint32_t ct = millis(); 
  uint64_t current_time_us = time_us_64();
  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; }

  uint32_t current_hw_raw = pwm_get_counter(pwm_slice_num);
  if (current_hw_raw < last_hw_raw_count) {
      pwm_overflow_counter++; 
  }
  last_hw_raw_count = current_hw_raw;

  if ((current_time_us - last_gate_time_us) >= GATE_TIME_US) {
    uint32_t current_total_pulses = get_full_32bit_pulses();
    uint64_t elapsed_us = current_time_us - last_gate_time_us;

    uint32_t delta_pulses = current_total_pulses - last_total_pulses;
    
    last_gate_time_us = current_time_us;
    last_total_pulses = current_total_pulses;

    if (delta_pulses == 0 || elapsed_us == 0) {
      real_frequency = 0;
    } else {
      // ИСПРАВЛЕНО: Умножаем импульсы на 8.0, так как включен хардверный делитель на входе
      real_frequency = ((double)delta_pulses * 8.0 * 1000000.0) / (double)elapsed_us;
    }
    should_redraw = true;
  }

  if (ct - lastDisplayUpdateMs >= 250) {
    lastDisplayUpdateMs = ct;
    should_redraw = true;
  }
  
  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();
  }
}

uint32_t get_full_32bit_pulses() {
    uint32_t current_hw_raw = pwm_get_counter(pwm_slice_num);
    if (current_hw_raw < last_hw_raw_count) {
        pwm_overflow_counter++;
        last_hw_raw_count = current_hw_raw;
    }
    return (pwm_overflow_counter << 16) | current_hw_raw;
}

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());

    double cf = real_frequency * calibFactor; 
    
    display.setTextSize(2); display.setCursor(0, 16); 
    if (cf >= 1000.0) { 
      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 {
      if (cf < 100.0 && cf > 0.05) { display.print(cf, 2); } 
      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 setup_hardware_counter() {
    gpio_set_function(PIN_FREQ_IN, GPIO_FUNC_PWM); 
    pwm_slice_num = pwm_gpio_to_slice_num(PIN_FREQ_IN); 
    
    pwm_config config = pwm_get_default_config();
    
    // ИСПРАВЛЕНО: Включаем аппаратный делитель входной частоты на 8.0f
    pwm_config_set_clkdiv_mode(&config, PWM_DIV_B_RISING); 
    pwm_config_set_clkdiv(&config, 8.0f); 
    
    pwm_init(pwm_slice_num, &config, true);
    
    gpio_set_input_hysteresis_enabled(PIN_FREQ_IN, true);
    
    last_hw_raw_count = pwm_get_counter(pwm_slice_num);
}

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); 
  float clkdiv = (float)sys_clock / (freq * 65536.0f);
  if (clkdiv < 1.0f) clkdiv = 1.0f;
  
  uint32_t period = (uint32_t)((float)sys_clock / (clkdiv * freq));
  if (period < 2) period = 2;
  if (period > 65536) period = 65536;

  uint8_t div_int = (uint8_t)clkdiv;
  uint8_t div_frac = (uint8_t)((clkdiv - div_int) * 16.0f);
  pwm_set_clkdiv_int_frac(slice_num, div_int, div_frac);

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