ESPNowESK8/src/remote.cpp

// ESPNOWSkate by Lukas Bachschwell this device MASTER =D

#include <Arduino.h>
#include <esp_now.h>
#include <WiFi.h>
#include <U8g2lib.h>
#include <Preferences.h>

#include "valuehelpers.h"
#include "mac_config.h"
#include "graphics.h"
#include "accel.h"

#define VARIANT_HOVER 1

TaskHandle_t clickTaskHandle;
Preferences preferences;

#define B_VOLT 0
#define B_VOLT_D 1
#define B_TEMP 2
#define B_TEMP_D 3
#define B_SPEED 4
#define B_SPEED_D 5

uint8_t boardData[6] = {0, 0, 0, 0, 0, 0};

#define M_NORMAL 0 // Limit is a sub of normal
#define M_SELECT 1
#define M_SETTINGS 2
#define M_CRUISE 3
#define M_STEERING 4

#ifdef VARIANT_HOVER

#define THROTTLE_MIN -1000
#define THROTTLE_CENTER 0
#define THROTTLE_MAX 1000
#define THROTTLE_LIMITED 600

#else

#define THROTTLE_MIN 0
#define THROTTLE_CENTER 127
#define THROTTLE_MAX 255
#define THROTTLE_LIMITED 180

#endif

// #define steeringInfluential

uint8_t currentMode = M_NORMAL;
uint8_t selectedIndex = 2;

bool lightActive = false;
bool shouldUpdateSettings = false; // Needed to update limitmode on core 1 instead of 0

bool crusing = false;
int16_t crusingSpeed = THROTTLE_CENTER;

#define DEBUG

#ifdef DEBUG
#define DEBUG_PRINTLN(x) Serial.println(x)
#define DEBUG_PRINT(x) Serial.print(x)
#else
#define DEBUG_PRINTLN(x)
#define DEBUG_PRINT(x)
#endif

bool connected = false;

uint8_t clickCounter = 0;
bool lastTriggerState = false;
long lastClick = 0;
#define clickDiff 250

// Defining variables for OLED display
U8G2_SSD1306_128X64_NONAME_F_HW_I2C u8g2(U8G2_R2, /* reset=*/16, /* clock=*/15, /* data=*/4);
char displayBuffer[20];
String displayString;
short displayData = 0;
unsigned long lastSignalBlink;
bool signalBlink = false;
unsigned long lastDataRotation;

// Defining variables for Hall Effect throttle.
short hallMeasurement;
int throttle = THROTTLE_CENTER;

int16_t esc1 = THROTTLE_CENTER;
int16_t esc2 = THROTTLE_CENTER;

byte hallCenterMargin = 5;

const float minVoltage = 3.4;
const float maxVoltage = 4.2;
const float refVoltage = 3.3;
// Resistors in Ohms
const float deviderR1 = 22000;
const float deviderR2 = 11200;

// Global copy of board
esp_now_peer_info_t board;
#define CHANNEL 1
#define PRINTSCANRESULTS 0
#define DELETEBEFOREPAIR 0
#define HAL_MIN 1390    // defaults
#define HAL_MAX 2230    // defaults
#define HAL_CENTER 1880 // defaults

// #define pairingMode
#define leverPin 36
#define triggerPin 17
#define batteryMeasurePin 39

bool triggerActive();

#include "settings.h"

uint8_t *buf;

void setCrusing(int16_t speed)
{
  if (speed < THROTTLE_CENTER)
  { // no backward cruse!
    crusing = false;
    crusingSpeed = THROTTLE_CENTER;
  }
  else
  {
    crusing = true;
    if (speed > crusingSpeed)
      crusingSpeed = speed;
  }
}
// ESPNOW functions ##############################
// Scan for boards in AP mode
// config AP
void configDeviceAP(bool hidden)
{
  bool result = WiFi.softAP("ESK8", "ESK8_Password+vD8z2YAvoDBW?Zx", CHANNEL, hidden);
  if (!result)
  {
    Serial.println("AP Config failed.");
  }
  else
  {
    Serial.println("AP Config Success. Broadcasting with AP: " + String("ESK8"));
  }
}

#ifdef pairingMode
void ScanForBoard()
{
  int8_t scanResults = WiFi.scanNetworks();
  // reset on each scan
  bool boardFound = 0;
  memset(&board, 0, sizeof(board));

  DEBUG_PRINTLN("");
  if (scanResults == 0)
  {
    DEBUG_PRINTLN("No WiFi devices in AP Mode found");
  }
  else
  {
    DEBUG_PRINT("Found ");
    DEBUG_PRINT(scanResults);
    DEBUG_PRINTLN(" devices ");
    for (int i = 0; i < scanResults; ++i)
    {
      // Print SSID and RSSI for each device found
      String SSID = WiFi.SSID(i);
      int32_t RSSI = WiFi.RSSI(i);
      String BSSIDstr = WiFi.BSSIDstr(i);

      if (PRINTSCANRESULTS)
      {
        DEBUG_PRINT(i + 1);
        DEBUG_PRINT(": ");
        DEBUG_PRINT(SSID);
        DEBUG_PRINT(" (");
        DEBUG_PRINT(RSSI);
        DEBUG_PRINT(")");
        DEBUG_PRINTLN("");
      }

      delay(10);
      // Check if the current device starts with `board`
      if (SSID.indexOf("ESK8") == 0)
      {
        // SSID of interest
        DEBUG_PRINTLN("Found a board.");
        DEBUG_PRINT(i + 1);
        DEBUG_PRINT(": ");
        DEBUG_PRINT(SSID);
        DEBUG_PRINT(" [");
        DEBUG_PRINT(BSSIDstr);
        DEBUG_PRINT("]");
        DEBUG_PRINT(" (");
        DEBUG_PRINT(RSSI);
        DEBUG_PRINT(")");
        DEBUG_PRINTLN("");
        // Get BSSID => Mac Address of the board
        int mac[6];
        if (6 == sscanf(BSSIDstr.c_str(), "%x:%x:%x:%x:%x:%x%c", &mac[0], &mac[1], &mac[2], &mac[3], &mac[4], &mac[5]))
        {
          for (int ii = 0; ii < 6; ++ii)
          {
            board.peer_addr[ii] = (uint8_t)mac[ii];
          }
        }

        board.channel = CHANNEL; // pick a channel
        board.encrypt = 0;       // no encryption
        board.ifidx = WIFI_IF_STA;

        boardFound = 1;
        // we are planning to have only one board in this example;
        // Hence, break after we find one, to be a bit efficient
        break;
      }
    }
  }

  if (boardFound)
  {
    DEBUG_PRINTLN("board Found, processing..");
  }
  else
  {
    DEBUG_PRINTLN("board Not Found, trying again.");
  }

  // clean up ram
  WiFi.scanDelete();
}
#endif

void deletePeer()
{
  const esp_now_peer_info_t *peer = &board;

  const uint8_t *peer_addr = board.peer_addr;
  esp_err_t delStatus = esp_now_del_peer(peer_addr);
  DEBUG_PRINT("board Delete Status: ");
  if (delStatus == ESP_OK)
  {
    // Delete success
    DEBUG_PRINTLN("Success");
  }
  else if (delStatus == ESP_ERR_ESPNOW_NOT_INIT)
  {
    // How did we get so far!!
    DEBUG_PRINTLN("ESPNOW Not Init");
  }
  else if (delStatus == ESP_ERR_ESPNOW_ARG)
  {
    DEBUG_PRINTLN("Invalid Argument");
  }
  else if (delStatus == ESP_ERR_ESPNOW_NOT_FOUND)
  {
    DEBUG_PRINTLN("Peer not found.");
  }
  else
  {
    DEBUG_PRINTLN("Not sure what happened");
  }
}

// Check if the board is already paired with the master.
// If not, pair the board with master
bool manageBoard()
{
  if (board.channel == CHANNEL)
  {
    if (DELETEBEFOREPAIR)
    {
      deletePeer();
    }

    DEBUG_PRINT("board Status: ");
    const esp_now_peer_info_t *peer = &board;
    const uint8_t *peer_addr = board.peer_addr;
    // check if the peer exists
    bool exists = esp_now_is_peer_exist(peer_addr);
    if (exists)
    {
      // board already paired.
      DEBUG_PRINTLN("Already Paired");
      return true;
    }
    else
    {
      // board not paired, attempt pair
      esp_err_t addStatus = esp_now_add_peer(peer);
      if (addStatus == ESP_OK)
      {
        // Pair success
        DEBUG_PRINTLN("Pair success");
        return true;
      }
      else if (addStatus == ESP_ERR_ESPNOW_NOT_INIT)
      {
        // How did we get so far!!
        DEBUG_PRINTLN("ESPNOW Not Init");
        return false;
      }
      else if (addStatus == ESP_ERR_ESPNOW_ARG)
      {
        DEBUG_PRINTLN("Invalid Argument");
        return false;
      }
      else if (addStatus == ESP_ERR_ESPNOW_FULL)
      {
        DEBUG_PRINTLN("Peer list full");
        return false;
      }
      else if (addStatus == ESP_ERR_ESPNOW_NO_MEM)
      {
        DEBUG_PRINTLN("Out of memory");
        return false;
      }
      else if (addStatus == ESP_ERR_ESPNOW_EXIST)
      {
        DEBUG_PRINTLN("Peer Exists");
        return true;
      }
      else
      {
        DEBUG_PRINTLN("Not sure what happened");
        return false;
      }
    }
  }
  else
  {
    // No board found to process
    DEBUG_PRINTLN("No board found to process");
    return false;
  }
}

// send data
void sendData()
{
  const uint8_t data[] = {split16(esc1, 0), split16(esc1, 1), split16(esc2, 0), split16(esc2, 1), boardOptions};

  const uint8_t *peer_addr = board.peer_addr;
  DEBUG_PRINT("Sending: ");
  DEBUG_PRINTLN(esc1);
  esp_err_t result = esp_now_send(peer_addr, data, sizeof(data));
  DEBUG_PRINT("Send Status: ");
  if (result != ESP_OK)
    setCrusing(0);
  if (result == ESP_OK)
  {
    DEBUG_PRINTLN("Success");
  }
  else if (result == ESP_ERR_ESPNOW_NOT_INIT)
  {
    // How did we get so far!!
    DEBUG_PRINTLN("ESPNOW not Init.");
  }
  else if (result == ESP_ERR_ESPNOW_ARG)
  {
    DEBUG_PRINTLN("Invalid Argument");
  }
  else if (result == ESP_ERR_ESPNOW_INTERNAL)
  {
    DEBUG_PRINTLN("Internal Error");
  }
  else if (result == ESP_ERR_ESPNOW_NO_MEM)
  {
    DEBUG_PRINTLN("ESP_ERR_ESPNOW_NO_MEM");
  }
  else if (result == ESP_ERR_ESPNOW_NOT_FOUND)
  {
    DEBUG_PRINTLN("Peer not found.");
  }
  else if (result == ESP_ERR_ESPNOW_IF)
  {
    DEBUG_PRINTLN("Interface error.");
  }
  else
  {
    DEBUG_PRINT("Not sure what happened ");
    DEBUG_PRINTLN(result);
  }
}

// callback when data is sent from Master to board
void OnDataSent(const uint8_t *mac_addr, esp_now_send_status_t status)
{
  char macStr[18];
  snprintf(macStr, sizeof(macStr), "%02x:%02x:%02x:%02x:%02x:%02x", mac_addr[0], mac_addr[1], mac_addr[2], mac_addr[3], mac_addr[4], mac_addr[5]);
  DEBUG_PRINT("Last Packet Sent to: ");
  DEBUG_PRINTLN(macStr);
  DEBUG_PRINT("Last Packet Send Status: ");
  if (status == ESP_NOW_SEND_SUCCESS)
  {
    connected = true;
    DEBUG_PRINTLN("Delivery Success");
    return;
  }

  connected = false;
  DEBUG_PRINTLN("Delivery Fail");
  setCrusing(0);
}

// callback when data is recv from board
void OnDataRecv(const uint8_t *mac_addr, const uint8_t *data, int data_len)
{
  char macStr[18];
  snprintf(macStr, sizeof(macStr), "%02x:%02x:%02x:%02x:%02x:%02x", mac_addr[0], mac_addr[1], mac_addr[2], mac_addr[3], mac_addr[4], mac_addr[5]);
  DEBUG_PRINT("Last Response Recv from: ");
  DEBUG_PRINTLN(macStr);
  memcpy(boardData, data, data_len);
  DEBUG_PRINT("Recieved data! len: ");
  DEBUG_PRINTLN(data_len);
}

// ############ End ESP Now

// ############ Hardware Helpers

int c_map(int value, int inMin, int inMax, int outMin, int outMax)
{
  return constrain(map(value, inMin, inMax, outMin, outMax), outMin, outMax);
}

// Return true if trigger is activated, false otherwice
bool triggerActive()
{
  if (digitalRead(triggerPin) == LOW)
  {
    vTaskDelay(pdMS_TO_TICKS(0.1));
    if (digitalRead(triggerPin) == LOW)
      return true;
  }
  else
  {
    vTaskDelay(pdMS_TO_TICKS(0.1));
    if (digitalRead(triggerPin) == HIGH)
      return false;
  }

  return false; // biased to false
}

void calculateThrottlePosition()
{
  // Hall sensor reading can be noisy, lets make an average reading.
  int total = 0;
  for (int i = 0; i < 10; i++)
  {
    total += analogRead(leverPin);
  }

  hallMeasurement = total / 10;
  DEBUG_PRINT("HAL: ");
  DEBUG_PRINTLN(hallMeasurement);

  int maxSpeed = THROTTLE_MAX;
  if (settings[limitMode] == 1)
  {
    maxSpeed = THROTTLE_LIMITED;
  }

  if (hallMeasurement >= settings[centerHallValue])
  {
    throttle = c_map(hallMeasurement, settings[centerHallValue], settings[maxHallValue], THROTTLE_CENTER, maxSpeed);
  }
  else
  {
    throttle = c_map(hallMeasurement, settings[minHallValue], settings[centerHallValue], THROTTLE_MIN, THROTTLE_CENTER);
  }
  // removeing center noise
  if (abs(throttle - THROTTLE_CENTER) < hallCenterMargin)
  {
    throttle = THROTTLE_CENTER;
  }
}

// Function to calculate and return the remotes battery voltage.
float batteryVoltage()
{
  float batteryVoltage = 0.0;
  int total = 0;

  for (int i = 0; i < 10; i++)
  {
    total += analogRead(batteryMeasurePin);
  }
  batteryVoltage = (refVoltage / 4095.0) * ((float)total / 10.0);
  // Now we have the actual Voltage, lets calculate the value befor the devider
  batteryVoltage = batteryVoltage / (deviderR1 / (deviderR1 + deviderR2));
  DEBUG_PRINT("Batt: ");
  DEBUG_PRINTLN(batteryVoltage);
  return batteryVoltage;
}

// Function used to indicate the remotes battery level.
int batteryLevel()
{
  float voltage = batteryVoltage();

  if (voltage <= minVoltage)
  {
    return 0;
  }
  else if (voltage >= maxVoltage)
  {
    return 100;
  }
  else
  {
    return (voltage - minVoltage) * 100 / (maxVoltage - minVoltage);
  }
}

// Take a number of measurements of the WiFi strength and return the average result.
int getStrength(int points)
{
  long rssi = 0;
  long averageRSSI = 0;
  if (points == 1)
    return WiFi.RSSI();

  for (int i = 0; i < points; i++)
  {
    rssi += WiFi.RSSI();
    delay(20);
  }

  averageRSSI = rssi / points;
  DEBUG_PRINT("RSSI: ");
  DEBUG_PRINTLN(averageRSSI);
  return averageRSSI;
}

void checkClicks(void *parameter)
{
  for (;;)
  {
    // DEBUG_PRINT("Trig: "); DEBUG_PRINT(triggerActive()); DEBUG_PRINT(" LAST: "); DEBUG_PRINTLN(lastTriggerState);
    if (millis() - lastClick > clickDiff && clickCounter != 0)
    {
      DEBUG_PRINTLN("reset");
      // timeout, check amount
      if (clickCounter == 2)
      {
        // cycle page if active
        DEBUG_PRINTLN("Double, rotate page");
        if (settings[pageDisplay] != 0)
        {
          displayData++;
          if (displayData > 2)
          {
            displayData = 0;
          }
          // write and save
          settings[pageDisplay] = displayData + 1;
          shouldUpdateSettings = true;
        }
      }
      else if (clickCounter == 3)
      {
        DEBUG_PRINTLN("YEAH TRIPPLE");

        if (currentMode == M_NORMAL)
        {
          currentMode = M_SELECT;
          selectedIndex = 2;
        }
        else
        {
          currentMode = M_NORMAL;
        }
      }
      clickCounter = 0;
    }

    if (!triggerActive() && lastTriggerState)
    {
      DEBUG_PRINTLN("CLICK ##################### ");
      int timeSinceLastClick = millis() - lastClick;
      lastClick = millis();

      if (currentMode == M_SELECT)
      {
        switch (selectedIndex)
        {
        case 0:
          currentMode = M_SETTINGS;
          break;
        case 1:
          if (settings[limitMode] == 0)
            settings[limitMode] = 1;
          else
            settings[limitMode] = 0;
          shouldUpdateSettings = true;
          currentMode = M_NORMAL;
          break;
        case 2:
          lightActive = !lightActive;
          updateBoardOptions();
          currentMode = M_NORMAL;
          break;
        case 3:
          currentMode = M_STEERING;
          break;
        case 4:
          currentMode = M_CRUISE;
          break;
        }
      }

      if (timeSinceLastClick < clickDiff)
      {
        clickCounter++;
      }
      else
      {
        clickCounter = 1;
      }

      lastClick = millis();
      lastTriggerState = false;
    }
    else if (triggerActive())
    {
      lastTriggerState = true;
    }
    vTaskDelay(pdMS_TO_TICKS(20));
  }
}
// ############ End Hardware Helpers

// ############ Drawing Functions

void drawBatteryLevel()
{
  int level = batteryLevel();

  // Position on OLED
  int x = 108;
  int y = 4;

  u8g2.drawFrame(x + 2, y, 18, 9);
  u8g2.drawBox(x, y + 2, 2, 5);

  for (int i = 0; i < 5; i++)
  {
    int p = round((100 / 5) * i);
    if (p <= level)
    {
      u8g2.drawBox(x + 4 + (3 * (4 - i)), y + 2, 2, 5);
    }
  }
}

void drawThrottle()
{
  int x = 0;
  int y = 18;
  uint8_t displayThrottle = throttle;
  if (crusing)
    displayThrottle = crusingSpeed;
  // Draw throttle
  u8g2.drawHLine(x, y, 52);
  u8g2.drawVLine(x, y, 10);
  u8g2.drawVLine(x + 52, y, 10);
  u8g2.drawHLine(x, y + 10, 5);
  u8g2.drawHLine(x + 52 - 4, y + 10, 5);

  if (displayThrottle >= THROTTLE_CENTER)
  {
    int width = map(displayThrottle, THROTTLE_CENTER, THROTTLE_MAX, 0, 49);

    for (int i = 0; i < width; i++)
    {
      u8g2.drawVLine(x + i + 2, y + 2, 7);
    }
  }
  else
  {
    int width = map(displayThrottle, THROTTLE_MIN, THROTTLE_CENTER - 1, 49, 0);
    for (int i = 0; i < width; i++)
    {
      u8g2.drawVLine(x + 50 - i, y + 2, 7);
    }
  }
}

void drawSignal()
{
  // Position on OLED
  int x = 114;
  int y = 17;
  if (connected == true)
  {
    if (triggerActive())
    {
      u8g2.drawXBM(x, y, 12, 12, signal_transmitting_bits);
    }
    else
    {
      u8g2.drawXBM(x, y, 12, 12, signal_connected_bits);
    }
  }
  else
  {
    if (millis() - lastSignalBlink > 500)
    {
      signalBlink = !signalBlink;
      lastSignalBlink = millis();
    }

    if (signalBlink == true)
    {
      u8g2.drawXBM(x, y, 12, 12, signal_connected_bits);
    }
    else
    {
      u8g2.drawXBM(x, y, 12, 12, signal_noconnection_bits);
    }
  }
}

void drawTitleScreen(String title)
{

  u8g2.clearBuffer();
  title.toCharArray(displayBuffer, 20);
  u8g2.setFont(u8g2_font_helvR10_tr);
  u8g2.drawStr(12, 20, displayBuffer);

  u8g2.sendBuffer();
  delay(1500);
}

void drawPage()
{
  int decimals;
  String suffix;
  String prefix;

  int first, last;

  int x = 0;
  int y = 16;

  // Rotate the realtime data each 4s.
  if (settings[pageDisplay] == 0)
  {
    if ((millis() - lastDataRotation) >= 4000)
    {

      lastDataRotation = millis();
      displayData++;

      if (displayData > 2)
      {
        displayData = 0;
      }
    }
  }
  else
  {
    displayData = settings[pageDisplay] - 1;
  }

  switch (displayData)
  {
  case 0:
    first = boardData[B_TEMP];
    last = boardData[B_TEMP_D];
    suffix = "C";
    prefix = "BOX TEMP";
    decimals = 2;
    break;
  case 1:
    first = boardData[B_SPEED];
    last = boardData[B_SPEED_D];
    suffix = "KmH";
    prefix = "SPEED";
    decimals = 2;
    break;
  case 2:
    first = boardData[B_VOLT];
    last = boardData[B_VOLT_D];
    suffix = "V";
    prefix = "BATTERY";
    decimals = 2;
    break;
  }

  // Display prefix (title)
  displayString = prefix;
  displayString.toCharArray(displayBuffer, 10);
  u8g2.setFont(u8g2_font_profont12_tr);
  u8g2.drawStr(x, y - 1, displayBuffer);

  // Add leading zero
  if (first <= 9)
  {
    if (connected)
      displayString = "0" + (String)first;
    else
      displayString = "--";
  }
  else
  {
    if (connected)
      displayString = (String)first;
    else
      displayString = "--";
  }

  // Display numbers
  displayString.toCharArray(displayBuffer, 4);
  u8g2.setFont(u8g2_font_logisoso22_tn);
  u8g2.drawStr(x + 55, y + 13, displayBuffer);

  // Display decimals
  // Add leading zero
  if (last <= 9)
  {
    if (connected)
      displayString = ".0" + (String)last;
    else
      displayString = ".--";
  }
  else
  {
    if (connected)
      displayString = "." + (String)last;
    else
      displayString = ".--";
  }

  // Display decimals
  displayString.toCharArray(displayBuffer, decimals + 2);
  u8g2.setFont(u8g2_font_profont12_tr);
  u8g2.drawStr(x + 86, y - 1, displayBuffer);

  // Display suffix
  displayString = suffix;
  displayString.toCharArray(displayBuffer, 10);
  u8g2.setFont(u8g2_font_profont12_tr);
  u8g2.drawStr(x + 86 + 2, y + 13, displayBuffer);
}

void controlSelect()
{
  if (hallMeasurement >= (settings[maxHallValue] - 250) && settingsLoopFlag == false)
  { // settings[maxHallValue]
    // Up
    if (selectedIndex != 0)
    {
      selectedIndex--;
      settingsLoopFlag = true;
    }
  }
  else if (hallMeasurement <= (settings[minHallValue] + 250) && settingsLoopFlag == false)
  {
    // Down
    if (selectedIndex < 4)
    {
      selectedIndex++;
      settingsLoopFlag = true;
    }
  }
  else if (inRange(hallMeasurement, settings[centerHallValue] - 50, settings[centerHallValue] + 50))
  { // settings[centerHallValue]
    settingsLoopFlag = false;
  }
}

String selectionTitles[5] = {
    "Settings", "Limit", "Light", "Steering", "Cruise"};

uint16_t selectionGlyphs[5] = {
    0x0081, 0x008d, 0x0103, 0x00f6, 0x0088};

void drawSelectionMenu()
{
  const uint8_t y = 35;
  const uint8_t xStart = 10;
  const uint8_t xSpace = 20;

  u8g2.setFontMode(0);
  u8g2.setDrawColor(1);

  for (int i = 0; i < 5; i++)
  {
    if (selectedIndex == i)
    {
      String title = selectionTitles[i];
      title.toCharArray(displayBuffer, 20);
      u8g2.setFont(u8g2_font_helvR10_tr);
      u8g2.drawStr(30, 12, displayBuffer);

      u8g2.setFontMode(0);
      // u8g2.drawBox(xStart-2 + xSpace * i, y-12, 15, 15);
      u8g2.drawBox(xStart + xSpace * i, y - 12, 15, 15);
      u8g2.setDrawColor(0);
    }

    // selectionItems[i].toCharArray(displayBuffer, 10);
    // u8g2.setFont(u8g2_font_profont12_tr);
    // u8g2.drawStr(xStart + xSpace * i, y, displayBuffer);

    u8g2.setFont(u8g2_font_open_iconic_all_2x_t);
    u8g2.drawGlyph(xStart + xSpace * i, y, selectionGlyphs[i]);

    u8g2.setFontMode(0);
    u8g2.setDrawColor(1);
  }
}

void drawLight()
{
  if (lightActive)
  {
    displayString = "Light";
    displayString.toCharArray(displayBuffer, 12);
    u8g2.setFont(u8g2_font_profont10_tr);
    u8g2.drawStr(100, 38, displayBuffer);
  }
}

void drawMode()
{
  if (currentMode == M_STEERING)
  {
    displayString = (String)esc1 + " |S| " + (String)esc2;
    displayString.toCharArray(displayBuffer, 12);
    u8g2.setFont(u8g2_font_profont12_tr);
    u8g2.drawStr(25, 50, displayBuffer);
  }
  else if (currentMode == M_NORMAL && settings[limitMode] == 1)
  {
    displayString = "LIM";
    displayString.toCharArray(displayBuffer, 12);
    u8g2.setFont(u8g2_font_profont12_tr);
    u8g2.drawStr(105, 50, displayBuffer);
  }
  else if (currentMode == M_CRUISE)
  {
    if (crusing)
    {
      displayString = "ACTIVE: " + (String)crusingSpeed;
      displayString.toCharArray(displayBuffer, 12);
      u8g2.setFont(u8g2_font_profont12_tr);
      u8g2.drawStr(0, 50, displayBuffer);
    }
    else
    {
      displayString = "Inactive";
      displayString.toCharArray(displayBuffer, 12);
      u8g2.setFont(u8g2_font_profont12_tr);
      u8g2.drawStr(0, 50, displayBuffer);
    }

    displayString = "CRU";
    displayString.toCharArray(displayBuffer, 12);
    u8g2.setFont(u8g2_font_profont12_tr);
    u8g2.drawStr(105, 50, displayBuffer);
  }
}

void updateMainDisplay()
{
  long start = millis();
  u8g2.clearBuffer();
  if (currentMode == M_SELECT)
  {
    drawSelectionMenu();
  }
  else if (currentMode == M_SETTINGS)
  {
    drawSettingsMenu();
    drawSettingNumber();
  }
  else
  {
    drawThrottle();
    drawPage();
    drawBatteryLevel();
    drawSignal();
    drawLight();
    drawMode();
  }
  u8g2.sendBuffer();
  Serial.printf("Took %dms\n", millis() - start);
}

void drawStartScreen()
{
  u8g2.clearBuffer(); // clear the internal memory

  u8g2.drawXBM(4, 4, 24, 24, logo_bits);

  displayString = "Esk8 remote";
  displayString.toCharArray(displayBuffer, 12);
  u8g2.setFont(u8g2_font_helvR10_tr);
  u8g2.drawStr(34, 22, displayBuffer);

  u8g2.sendBuffer(); // transfer internal memory to the display
  delay(800);
}

// ############ End Drawing Functions

void setup()
{
  Serial.begin(115200);
  // Set device in STA mode to begin with
  // WiFi.mode(WIFI_STA);
  // Set device in AP mode to begin with
  WiFi.mode(WIFI_AP_STA);
  // configure device AP mode
  configDeviceAP(true);
  DEBUG_PRINTLN("ESPNowSkate");

  loadSettings();

  // reset the screen
  pinMode(16, OUTPUT);
  digitalWrite(16, LOW); // set GPIO16 low to reset OLED
  delay(50);
  digitalWrite(16, HIGH);

  // setup other pins
  pinMode(triggerPin, INPUT_PULLUP);

  xTaskCreatePinnedToCore(
      checkClicks,
      "click task",
      1000,
      NULL,
      1,
      &clickTaskHandle,
      0);

  initAccel();

  DEBUG_PRINTLN("ESPNowSkate Sender");
  u8g2.setBusClock(400000);
  u8g2.begin();
  u8g2.setBusClock(400000);

  // buf = (uint8_t *)malloc(u8g2.getBufferSize());
  // u8g2.setBufferPtr(buf);
  // u8g2.initDisplay();
  // u8g2.clearDisplay();
  // u8g2.setPowerSave(0);

  drawStartScreen();

  if (triggerActive())
  {
    currentMode = M_SETTINGS;
    drawTitleScreen("Remote Settings");
  }

  // This is the mac address of the Master in Station Mode

  DEBUG_PRINT("STA MAC: ");
  DEBUG_PRINTLN(WiFi.macAddress());
  if (esp_now_init() == ESP_OK)
  {
    DEBUG_PRINTLN("ESPNow Init Success");
  }
  else
  {
    DEBUG_PRINTLN("ESPNow Init Failed");
    ESP.restart();
  }

  // Once ESPNow is successfully Init, we will register for Send CB to
  // get the status of Trasnmitted packet
  esp_now_register_send_cb(OnDataSent);
  // ScanForBoard();

  // Once ESPNow is successfully Init, we will register for recv CB to
  // get recv packer info.
  esp_now_register_recv_cb(OnDataRecv);

  // Retrieve board from config:
  for (int i = 0; i < 6; ++i)
  {
    board.peer_addr[i] = (uint8_t)mac_receiver[i];
  }
  board.channel = CHANNEL; // pick a channel
  board.encrypt = 0;       // no encryption
  // board.ifidx = ESP_IF_WIFI_STA;
}

void loop()
{
  updateMainDisplay();

  // if(currentMode == M_STEERING) readAccel();
  readAccel();
  DEBUG_PRINT("Accel Value Left Right (Y): ");
  DEBUG_PRINT(map(AcY, -15000, 15000, -100, 100));
  // Serial.print("Help(Y): ");
  // Serial.println(map(GyY, -15000, 15000, -100, 100));

  DEBUG_PRINT(" (X): ");
  DEBUG_PRINT(map(AcX, -15000, 15000, -100, 100));

  DEBUG_PRINT(" (Z): ");
  DEBUG_PRINTLN(map(AcZ, -15000, 15000, -100, 100));

  // left rigt : Z forward Back : Y
  calculateThrottlePosition();

  if (currentMode == M_SELECT)
  {
    controlSelect();
    esc1 = THROTTLE_CENTER;
    esc2 = THROTTLE_CENTER;
  }
  else if (currentMode == M_SETTINGS)
  {
    // Use throttle and trigger to change settings
    controlSettingsMenu();
  }
  else
  {
    // Use throttle and trigger to drive motors
    if (triggerActive())
    {
      if (currentMode == M_STEERING)
      {
        DEBUG_PRINT("Value: ");
        int map = c_map(AcZ, -15000, 15000, -60, 60);
        DEBUG_PRINTLN(map);

#ifdef steeringInfluential
        esc1 = (uint8_t)constrain(throttle - map, THROTTLE_MIN, THROTTLE_MAX);
        esc2 = (uint8_t)constrain(throttle + map, THROTTLE_MIN, THROTTLE_MAX);
#else
        esc1 = (uint8_t)constrain(THROTTLE_CENTER - map, THROTTLE_MIN, THROTTLE_MAX);
        esc2 = (uint8_t)constrain(THROTTLE_CENTER + map, THROTTLE_MIN, THROTTLE_MAX);
        if (-10 < map && map < 10)
        {
          esc1 = throttle;
          esc2 = throttle;
        }

        // ignore reverse
        if (throttle < THROTTLE_CENTER)
        {
          esc1 = throttle;
          esc2 = throttle;
        }
#endif
        if (throttle == THROTTLE_CENTER)
        {
          esc1 = THROTTLE_CENTER;
          esc2 = THROTTLE_CENTER;
        }

        DEBUG_PRINT("SteeringMode: ESC1: ");
        DEBUG_PRINT(esc1);
        DEBUG_PRINT(" ESC2: ");
        DEBUG_PRINTLN(esc2);
      }
      else if (currentMode == M_CRUISE)
      {

        setCrusing(throttle);

        esc1 = throttle;
        esc2 = throttle;

        if (crusing)
        {
          esc1 = crusingSpeed;
          esc2 = crusingSpeed;
        }
      }
      else
      {
        esc1 = throttle;
        esc2 = throttle;
      }
    }
    else
    {
      // THROTTLE_CENTER is the middle position - no throttle and no brake/reverse
      esc1 = THROTTLE_CENTER;
      esc2 = THROTTLE_CENTER;
      if (crusing)
        setCrusing(0);
    }
  }

  // If board is found, it would be populate in `board` variable
  // We will check if `board` is defined and then we proceed further
  if (board.channel == CHANNEL)
  { // check if board channel is defined
    // `board` is defined
    // Add board as peer if it has not been added already
    bool isPaired = manageBoard();
    if (isPaired)
    {
      sendData();
    }
    else
    {
      // board pair failed
      DEBUG_PRINTLN("board not found / paired!");
    }
  }
  else
  {
    // No board found to process
  }

  if (shouldUpdateSettings)
  {
    // updateSettings();
    // shouldUpdateSettings = false;
  }
}