#include <Wire.h>
#include <Adafruit_GFX.h>     // https://github.com/adafruit/Adafruit-GFX-Library
#include <Adafruit_SSD1306.h> // https://github.com/adafruit/Adafruit_SSD1306
#include <Arduino.h>
#include <Keypad.h>     // https://github.com/Chris--A/Keypad
#include <BleGamepad.h> // https://github.com/lemmingDev/ESP32-BLE-Gamepad

//////////////////////////////////////////////////
// Adjustment Setup
//////////////////////////////////////////////////
bool debounceAdjustMode = false;

//////////////////////////////////////////////////
// Battery Polling Intervals
//////////////////////////////////////////////////
long batteryUpdateInterval = 1000; // every 5 minutes
// long batteryUpdateInterval = 1000; // 1 Hz (testing/fun)
long prevBatteryUpdate = 700; // 10 seconds from goal

//////////////////////////////////////////////////
// OLED Screen Setup
//////////////////////////////////////////////////
#define SCREEN_WIDTH 128 // OLED display width, in pixels
#define SCREEN_HEIGHT 32 // OLED display height, in pixels
#define OLED_RESET    -1 // Reset pin # (or -1 if sharing Arduino reset pin)
#define SCREEN_ADDRESS 0x3C
#define CUSTOM_I2C_SDA 21
#define CUSTOM_I2C_SCL 18
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);


BleGamepad bleGamepad("ESP32 Keypad", "lemmingDev", 100); // Shows how you can customise the device name, manufacturer name and initial battery level

#define ROWS 4
#define COLS 4
uint8_t rowPins[ROWS] = {27, 12, 13, 14}; // ESP32 pins used for rows      --> adjust to suit --> Pinout on board: R1, R2, R3, R4
uint8_t colPins[COLS] = {16, 4, 0, 2}; // ESP32 pins used for columns   --> adjust to suit --> Pinout on board: Q1, Q2, Q3, Q4
uint8_t keymap[ROWS][COLS] =
    {
        {1, 2, 3, 4},    // Buttons  1,  2,  3,  4      --> Used for calulating the bitmask for sending to the library
        {5, 6, 7, 8},    // Buttons  5,  6,  7,  8      --> Adjust to suit which buttons you want the library to send
        {9, 10, 11, 12}, // Buttons  9, 10, 11, 12      -->
        {13, 14, 15, 16} // Buttons 13, 14, 15, 16      --> Eg. The value 12 in the array refers to button 12
};

Keypad customKeypad = Keypad(makeKeymap(keymap), rowPins, colPins, ROWS, COLS);

void setup()  
{
  BleGamepadConfiguration bleGamepadConfig;
  bleGamepadConfig.setAutoReport(false); // Disable auto reports --> You then need to force HID updates with bleGamepad.sendReport()
  bleGamepad.begin();                    // Begin library with default buttons/hats/axes

  Serial.begin(115200);
  
  // Custom i2c pins and address for Adafruit OLED library
  Wire.begin(CUSTOM_I2C_SDA, CUSTOM_I2C_SCL, SCREEN_ADDRESS);

  // Initialize the OLED display
  display.begin();
  Serial.println("Booted!");

  // Cheesy OLED boot message
  display.clearDisplay();
  display.setTextSize(2);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(0, 0);
  display.println("Ignition");
  display.println("Race Mode");
  display.display();
  delay(3000);
  display.clearDisplay();
}    // changing bleGamepadConfig after the begin function has no effect, unless you call the begin function again


void loop()
{
    KeypadUpdate();
    delay(10);
    unsigned long now = millis();

 // Send battery level to host periodically
  if(now - prevBatteryUpdate > batteryUpdateInterval) {
    prevBatteryUpdate = now;
    int batteryLevel = getBatteryPercent();
    float batteryVoltage = getBatteryVoltage();

    Serial.print("battery: \t");
    Serial.println((String)batteryLevel);
    
    if(bleGamepad.isConnected()) {
      bleGamepad.setBatteryLevel(batteryLevel);
    }

    if(!debounceAdjustMode) {
      displayBatteryStats();
    }
  }
  // Send battery level to host periodically
  if(now - prevBatteryUpdate > batteryUpdateInterval) {
    prevBatteryUpdate = now;
    int batteryLevel = getBatteryPercent();
    float batteryVoltage = getBatteryVoltage();

    Serial.print("battery: \t");
    Serial.println((String)batteryLevel);
    
    if(bleGamepad.isConnected()) {
      bleGamepad.setBatteryLevel(batteryLevel);
    }

    if(!debounceAdjustMode) {
      displayBatteryStats();
    }
  }


}

void KeypadUpdate()
{
    customKeypad.getKeys();

    for (int i = 0; i < LIST_MAX; i++) // Scan the whole key list.      //LIST_MAX is provided by the Keypad library and gives the number of buttons of the Keypad instance
    {
        if (customKeypad.key[i].stateChanged) // Only find keys that have changed state.
        {
            uint8_t keystate = customKeypad.key[i].kstate;

            if (bleGamepad.isConnected())
            {
                if (keystate == PRESSED)
                {
                    bleGamepad.press(customKeypad.key[i].kchar);
                } // Press or release button based on the current state
                if (keystate == RELEASED)
                {
                    bleGamepad.release(customKeypad.key[i].kchar);
                }

                bleGamepad.sendReport(); // Send the HID report after values for all button states are updated, and at least one button state had changed
            }
        }
    }
}

// battery 
int getBatteryPercent() {
  const int maxDisplayed = 100; // 100% (higher values are constrained)
  const int minDisplayed = 0; // 0% or flat (we'll probably never get there)
  const float maxBatteryVoltage = 4.2; // Max LiPoly voltage of a 3.7 battery is 4.2
  const float minBatteryVoltage = 3.3; // cutoff voltage (3.2, 3.3 to be safer)
  float voltageLevel = getBatteryVoltage();
  float usablePercent = ((voltageLevel - minBatteryVoltage) / (maxBatteryVoltage - minBatteryVoltage)) * ((maxDisplayed - minDisplayed) + minDisplayed);
  return constrain((int)usablePercent, minDisplayed, maxDisplayed);
}

// If you read voltage values higher than 4.2, and are seeing a difference
// between the value on your multimeter, adjust the vRef. Something between
// 1.0 and 1.1 should be where you're aiming for.
// You can go as deep as you want here:
// https://esp32.com/viewtopic.php?f=19&t=2881&start=30;
// the simple fix below works for me though.
float getBatteryVoltage() {
  const float vrefCalibration = 0.9417;
  const float vRef = 1.1; // should be 1.1V but may need to be calibrated above
  const float maxAnalogVal = 4095.0; // defines the range of the ADC calculation
  return (analogRead(35) / maxAnalogVal) * 2 * (vRef * vrefCalibration) * 3.3; // calculate voltage level
}

void displayBatteryStats() {
  int batteryLevel = getBatteryPercent();
  float batteryVoltage = getBatteryVoltage();

  display.clearDisplay();
  display.setTextSize(2);
  display.setTextColor(SSD1306_WHITE);
  // display.setCursor(30, 2);
  // display.println((String)batteryVoltage + "V");
  display.setCursor(30, 17);
  display.println((String)batteryLevel + "%");
  display.display();
}

// void displayDebounceMessage(unsigned short holdoff) {
//  display.clearDisplay();
//  display.setTextSize(2);
//  display.setTextColor(SSD1306_WHITE);
//  display.setCursor(0, 0);
//  display.println("Debounce");
//  display.println((String)holdoff + "ms");
//  display.display();
// }