#include <SPI.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

#define ENCODER_CLK 2
#define ENCODER_DT  4
#define BUTTON_PIN 16

#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64

Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire);

float freq = 146.55;
int selectedDigit = 0; // 0 for hundreds, 1 for tens, 2 for ones, 3 for tenths, 4 for hundredths

static const unsigned char PROGMEM image_MHz_bits[] = {
  0xc3, 0x61, 0x80, 0x00, 0xe7, 0x61, 0x80, 0x00, 0xff, 0x61, 0x80, 0x00, 
  0xff, 0x61, 0xbf, 0x80, 0xdb, 0x7f, 0xbf, 0x80, 0xdb, 0x7f, 0x83, 0x00, 
  0xdb, 0x61, 0x86, 0x00, 0xc3, 0x61, 0x8c, 0x00, 0xc3, 0x61, 0x98, 0x00, 
  0xc3, 0x61, 0xbf, 0x80, 0xc3, 0x61, 0xbf, 0x80
};

void setup() {
  Serial.begin(115200);
  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println(F("SSD1306 allocation failed"));
    for (;;);
  }
  display.clearDisplay();
  
  pinMode(ENCODER_CLK, INPUT);
  pinMode(ENCODER_DT, INPUT);
  pinMode(BUTTON_PIN, INPUT_PULLUP);

  attachInterrupt(digitalPinToInterrupt(ENCODER_CLK), readEncoder, FALLING);
}

void readEncoder() {
  int dtValue = digitalRead(ENCODER_DT);
  float step = 0.0;

  // Determine the step size based on the selected digit
  switch (selectedDigit) {
    case 0: step = 100; break;
    case 1: step = 10; break;
    case 2: step = 1; break;
    case 3: step = 0.1; break;
    case 4: step = 0.01; break;
    case 5: step = 0.001; break;
  }

  // Adjust the frequency based on the encoder direction
  if (dtValue == HIGH) {
    freq += step;
  } else {
    freq -= step;
  }

  // Constrain the frequency to valid range
  freq = constrain(freq, 144.0, 148.0);
}

void loop() {
  // Check if the button is pressed
  if (digitalRead(BUTTON_PIN) == LOW) {
    delay(200); // Debounce
    selectedDigit = (selectedDigit + 1) % 6; // Cycle through 6 digits
  }

  display.clearDisplay();
  display.drawBitmap(101, 14, image_MHz_bits, 25, 11, 1);
  display.setTextColor(WHITE);

  // Display the main part of the frequency
  display.setTextSize(3);
  display.setCursor(1, 4);
  int mainPart = (int)freq;
  display.print(mainPart);

  // Display the decimal part with exactly three digits
  display.setTextSize(2);
  display.setCursor(52, 11);
  
  // Calculate decimal part ensuring exactly 3 digits
  int decimalPart = (int)((freq - mainPart) * 1000 + 0.5);
  // Ensure we don't exceed 999
  decimalPart = decimalPart % 1000;
  
  char decimalStr[8];
  sprintf(decimalStr, ".%03d", decimalPart);
  display.print(decimalStr);

  // Calculate cursor positions for each digit
  int cursorX;
  int cursorL;
  switch(selectedDigit) {
    case 0:  // Hundred
      cursorX = 1;
      cursorL = 15;
      break;
    case 1:  // Ten
      cursorX = 19;
      cursorL = 15;
      break;
    case 2:  // One
      cursorX = 37;
      cursorL = 15;
      break;
    case 3:  // Tenth
      cursorX = 64;
      cursorL = 10;
      break;
    case 4:  // Hundredth
      cursorX = 76;
      cursorL = 10;
      break;
    case 5:  // Thousandth
      cursorX = 88;
      cursorL = 10;
      break;
  }
  
  // Draw cursor under the selected digit
  display.drawFastHLine(cursorX, 27, cursorL, WHITE);

  display.display();
  delay(100);
}