#include "screen.h"
#include "Arduino.h"
#include "esp32-hal.h"
#include <cstdint>

namespace Screen {

bool initialized_wire = false;
bool initialized_screen = false;

Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);

// attempt to start up screen; will lock program if fails
void setup_screen() {
  if (!initialized_wire) {
    Wire.begin(I2C_SDA_PIN, I2C_SCL_PIN);
    initialized_wire = true;
  }
  for (;;) {
    if (!display.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS)) {
      Serial.println("SSD1306 failed initiation");
      delay(500);
    } else {
      initialized_screen = true;
      return;
    }
  }
}

// example code to draw to screen (test working)
void test_fillrect() {
  display.clearDisplay();

  for (int16_t i = 0; i < display.height() / 2; i += 3) {
    display.fillRect(i, i, display.width() - i * 2, display.height() - i * 2,
                     SSD1306_INVERSE);
    display.display();
    delay(1);
  }
}

void wire_search(uint8_t loops = 5) {
  if (!initialized_wire) {
    Wire.begin(I2C_SDA_PIN, I2C_SCL_PIN);
    initialized_wire = true;
  }
  for (uint8_t i = 0; i < loops; i++) {
    byte error, address;
    int nDevices;
    Serial.println("Scanning...");

    nDevices = 0;
    for (address = 1; address < 127; address++) {
      Wire.beginTransmission(address);
      error = Wire.endTransmission();

      if (error == 0) {
        Serial.print("I2C device found at address 0x");
        if (address < 16)
          Serial.print("0");
        Serial.print(address, HEX);
        Serial.println("  !");

        nDevices++;
      } else if (error == 4) {
        Serial.print("Unknown error at address 0x");
        if (address < 16)
          Serial.print("0");
        Serial.println(address, HEX);
      }
    }
    if (nDevices == 0)
      Serial.println("No I2C devices found.");
    else
      Serial.println("Done.");
    delay(5000);
  }
}

} // namespace Screen