#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include "WiFi.h"
#include <esp_now.h>

const String version = "v0.0.1";
#define SERIAL_SPEED 115200

#define JOY_A 32
#define JOY_B 33

#define ENCODER_CLK 34
#define ENCODER_DT 35
#define ENCODER_SW 25

#define LED_A 23

#define SDA 21
#define SCL 22

#define DISPLAY_I2C_ADDR 0x3c
#define SCREEN_WIDTH 128 // OLED display width, in pixels
#define SCREEN_HEIGHT 64 // OLED display height, in pixels

// create an OLED display object connected to I2C
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);

// REPLACE WITH YOUR RECEIVER MAC Address
uint8_t broadcastAddress[] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};
unsigned long nextUpdateMillis;
int millisBetweenUpdates = 100;

int YAW = 0;
int THROTTLE = 0;

void showWelcome(Adafruit_SSD1306 *display) {
  display->clearDisplay();
  display->setTextSize(2);
  display->setTextColor(WHITE); // Draw white text
  display->setCursor(0, 0);
  display->print("Welcome");
  display->setTextSize(1);
  display->setCursor(0, 16);
  display->print(version);
  display->display();
}

void setup() {
  pinMode(LED_A, OUTPUT);
  digitalWrite(LED_A, HIGH);

  Serial.begin(SERIAL_SPEED);
  Serial.print("Starting display at ");
  Serial.println(DISPLAY_I2C_ADDR);

  // initialize OLED display with I2C address DISPLAY_I2C_ADDR
  if (!display.begin(SSD1306_SWITCHCAPVCC, DISPLAY_I2C_ADDR)) {
    Serial.println(F("failed to start display. HALT"));
    while (1);
  }

  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(WHITE); // Draw white text
  display.setCursor(0, 0);

  // Log the MAC address
  WiFi.mode(WIFI_MODE_STA);
  Serial.println(WiFi.macAddress());
  display.println(WiFi.macAddress());
  display.display();
  
  // Set device as a Wi-Fi Station
  WiFi.mode(WIFI_STA);

  // Enable interrupt for the Rotary Encoder.
  attachInterrupt(digitalPinToInterrupt(ENCODER_CLK), readEncoder, FALLING);

  pinMode(JOY_A, INPUT);  
  pinMode(JOY_B, INPUT);  

  
  // Init ESP-NOW
  if (esp_now_init() != ESP_OK) {
    Serial.println("Error initializing ESP-NOW. HALT");
    display.println("ESPNOW ERROR, HALT");
    display.display();
    while(1);
  } 

  display.setCursor(0, 10);
  display.println("Ok, running.");
  display.display();
}


void loop() {
  if (millis() > nextUpdateMillis) { 
    updateDisplay();
    nextUpdateMillis = millis() + millisBetweenUpdates;
  }

  if (digitalRead(ENCODER_SW)){
    Serial.print('BUTTON');
  }
}


void updateDisplay() {
  Serial.print(analogRead(JOY_A));
  Serial.print(' ');
  Serial.println(analogRead(JOY_B));
}


void readEncoder() {
  int dtValue = digitalRead(ENCODER_DT);
  if (dtValue == HIGH) {
    Serial.println("Rotated clockwise ⏩");
  }
  if (dtValue == LOW) {
    Serial.println("Rotated counterclockwise ⏪");
  }
}