// ESPNOWSkate by Lukas Bachschwell this device MASTER =D

#include <Arduino.h>
#include <esp_now.h>
#include <WiFi.h>
#include <U8g2lib.h>
#include <EEPROM.h> // ESP32 ?

#include "mac_config.h"
#include "graphics.h"

#include "accel.h"

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

// Defining variables for Settings menu
bool changeSettings = false;
bool changeSelectedSetting = false;

bool settingsLoopFlag = false;
bool settingsChangeFlag = false;
bool settingsChangeValueFlag = false;

// Defining variables for Hall Effect throttle.
short hallMeasurement;
int throttle = 127;
int sendThrottle = 127;
byte hallCenterMargin = 5;

byte currentSetting = 0;
const byte numOfSettings = 11;

const float minVoltage = 3.2;
const float maxVoltage = 4.1;
const float refVoltage = 3.3;


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

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


// ESPNOW functions ##############################
// Scan for boards in AP mode

void configDeviceAP(bool hidden) {
  bool result = WiFi.softAP("ESK8Remote", "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));

  Serial.println("");
  if (scanResults == 0) {
    Serial.println("No WiFi devices in AP Mode found");
  } else {
    Serial.print("Found "); Serial.print(scanResults); Serial.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) {
        Serial.print(i + 1);
        Serial.print(": ");
        Serial.print(SSID);
        Serial.print(" (");
        Serial.print(RSSI);
        Serial.print(")");
        Serial.println("");
      }

      delay(10);
      // Check if the current device starts with `board`
      if (SSID.indexOf("ESK8") == 0) {
        // SSID of interest
        Serial.println("Found a board.");
        Serial.print(i + 1); Serial.print(": "); Serial.print(SSID); Serial.print(" ["); Serial.print(BSSIDstr); Serial.print("]"); Serial.print(" ("); Serial.print(RSSI); Serial.print(")"); Serial.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

        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) {
    Serial.println("board Found, processing..");
  } else {
    Serial.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);
  Serial.print("board Delete Status: ");
  if (delStatus == ESP_OK) {
    // Delete success
    Serial.println("Success");
  } else if (delStatus == ESP_ERR_ESPNOW_NOT_INIT) {
    // How did we get so far!!
    Serial.println("ESPNOW Not Init");
  } else if (delStatus == ESP_ERR_ESPNOW_ARG) {
    Serial.println("Invalid Argument");
  } else if (delStatus == ESP_ERR_ESPNOW_NOT_FOUND) {
    Serial.println("Peer not found.");
  } else {
    Serial.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();
    }

    Serial.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.
      Serial.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
        Serial.println("Pair success");
        return true;
      } else if (addStatus == ESP_ERR_ESPNOW_NOT_INIT) {
        // How did we get so far!!
        Serial.println("ESPNOW Not Init");
        return false;
      } else if (addStatus == ESP_ERR_ESPNOW_ARG) {
        Serial.println("Invalid Argument");
        return false;
      } else if (addStatus == ESP_ERR_ESPNOW_FULL) {
        Serial.println("Peer list full");
        return false;
      } else if (addStatus == ESP_ERR_ESPNOW_NO_MEM) {
        Serial.println("Out of memory");
        return false;
      } else if (addStatus == ESP_ERR_ESPNOW_EXIST) {
        Serial.println("Peer Exists");
        return true;
      } else {
        Serial.println("Not sure what happened");
        return false;
      }
    }
  } else {
    // No board found to process
    Serial.println("No board found to process");
    return false;
  }
}

// send data
void sendData() {
  uint8_t esc1 = sendThrottle;
  uint8_t esc2 = esc1;

  const uint8_t data[] = { esc1, esc2 }; // no mixture for the normal mode

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

// 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]);
  Serial.print("Last Packet Sent to: "); Serial.println(macStr);
  Serial.print("Last Packet Send Status: "); Serial.println(status == ESP_NOW_SEND_SUCCESS ? "Delivery Success" : "Delivery Fail");
}

// 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]);
  Serial.print("#######################Last Response Recv from: "); Serial.println(macStr);
  uint8_t recData[3];
  memcpy(recData, data, data_len);
  Serial.print("Last Response Recv Data: "); Serial.println(recData[0]); Serial.print(" "); Serial.print(recData[1]); Serial.print(" len:");  Serial.println(data_len);
}

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

//############ Hardware Helpers
// Check if an integer is within a min and max value
bool inRange(int val, int minimum, int maximum) {
  return ((minimum <= val) && (val <= maximum));
}

// Return true if trigger is activated, false otherwice
bool triggerActive() {
  if (digitalRead(triggerPin) == LOW)
    return true;
  else
    return 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;
  Serial.println(hallMeasurement);
  //DEBUG_PRINT( (String)hallMeasurement );

  if (hallMeasurement >= HAL_CENTER) {
    throttle = constrain(map(hallMeasurement, HAL_CENTER, HAL_MAX, 127, 255), 127, 255);
  } else {
    throttle = constrain(map(hallMeasurement, HAL_MIN, HAL_CENTER, 0, 127), 0, 127);
  }
  // removeing center noise
  if (abs(throttle - 127) < hallCenterMargin) {
    throttle = 127;
  }
}

// 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 / 4096.0) * ((float)total / 10.0);
  Serial.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;

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

  averageRSSI=rssi/points;
  return averageRSSI;
}

//############ 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 * i), y + 2, 2, 5);
    }
  }
}

void drawThrottle() {
  int x = 0;
  int y = 18;

  // 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 (throttle >= 127) {
    int width = map(throttle, 127, 255, 0, 49);

    for (int i = 0; i < width; i++) {
      u8g2.drawVLine(x + i + 2, y + 2, 7);
    }
  } else {
    int width = map(throttle, 0, 126, 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.firstPage();
  do {
    title.toCharArray(displayBuffer, 20);
    u8g2.setFont(u8g2_font_helvR10_tr  );
    u8g2.drawStr(12, 20, displayBuffer);
  } while ( u8g2.nextPage() );
  delay(1500);
}

void drawBoardVoltage() {

  int x = 0;
  int y = 16;
  String suffix = "V";
  String prefix = "BATTERY";
  float value = 0.0; // TODO: No info this yet, measure in the board

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

  // Split up the float value: a number, b decimals.
  int first = abs(floor(value));
  int last = value * pow(10, 3) - first * pow(10, 3);

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

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

  // Display decimals
  displayString = "." + (String)last;
  displayString.toCharArray(displayBuffer, 3);
  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 drawSettingsMenu() {
  /*
     // Position on OLED
     int x = 0; int y = 10;

     // Draw setting title
     displayString = settingPages[currentSetting][0];
     displayString.toCharArray(displayBuffer, displayString.length() + 1);

     u8g2.setFont(u8g2_font_profont12_tr);
     u8g2.drawStr(x, y, displayBuffer);

     int val = getSettingValue(currentSetting);

     displayString = (String)val + "" + settingPages[currentSetting][1];
     displayString.toCharArray(displayBuffer, displayString.length() + 1);
     u8g2.setFont(u8g2_font_10x20_tr  );

     if (changeSelectedSetting == true) {
     u8g2.drawStr(x + 10, y + 20, displayBuffer);
     } else {
     u8g2.drawStr(x, y + 20, displayBuffer);
     }
   */
}

void drawSettingNumber() {
  // Position on OLED
  int x = 2; int y = 10;

  // Draw current setting number box
  u8g2.drawRFrame(x + 102, y - 10, 22, 32, 4);

  // Draw current setting number
  displayString = (String)(currentSetting + 1);
  displayString.toCharArray(displayBuffer, displayString.length() + 1);

  u8g2.setFont(u8g2_font_profont22_tn);
  u8g2.drawStr(x + 108, 22, displayBuffer);
}

void updateMainDisplay() {

  u8g2.firstPage();
  do {

    if (changeSettings == true) {
      drawSettingsMenu();
      drawSettingNumber();
    } else {
      drawThrottle();
      drawBoardVoltage();
      drawBatteryLevel();
      drawSignal();
    }

  } while ( u8g2.nextPage() );
}

void drawStartScreen() {
  u8g2.firstPage();
  do {
    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);
  } while ( u8g2.nextPage() );
  delay(1500);
}

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

void setup() {
  Serial.begin(115200);
  //Set device in STA mode to begin with
  WiFi.mode(WIFI_STA);
  Serial.println("ESPNowSkate");

  // 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);

  initAccel();

  Serial.println("ESPNowSkate Sender");
  u8g2.begin();
  drawStartScreen();

  if (triggerActive()) {
    changeSettings = true;
    drawTitleScreen("Remote Settings");
  }

  // This is the mac address of the Master in Station Mode
  Serial.print("STA MAC: "); Serial.println(WiFi.macAddress());

  if (esp_now_init() == ESP_OK) {
    Serial.println("ESPNow Init Success");
  }
  else {
    Serial.println("ESPNow Init Failed");
    ESP.restart();
  }

  //configDeviceAP(true);

  // 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
}

void loop() {
  // Call function to update display and LED
  updateMainDisplay();

  calculateThrottlePosition();

  if (changeSettings == true) {
    // Use throttle and trigger to change settings
    //controlSettingsMenu();
  } else {
    // Use throttle and trigger to drive motors
    if (triggerActive()) {
      sendThrottle = throttle;
    } else {
      // 127 is the middle position - no throttle and no brake/reverse
      sendThrottle = 127;
    }

    // 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) {
        // pair success or already paired
        // Send data to device

        sendData();
        /* update Value
           char buf[10];
           sprintf(buf, "%i", map(analogRead(leverPin), HAL_MIN, HAL_MAX, TRIM_LOW, TRIM_HIGH));
           u8g2.firstPage();
           do {
           u8g2.setFont(u8g2_font_10x20_tr );
           u8g2.drawStr(0, 20, buf);
           } while ( u8g2.nextPage() );
         */
      } else {
        // board pair failed

        Serial.println("board not found / paired!");
      }
    }
    else {
      // No board found to process
    }

    delay(20);
  }

  readAccel();
  Serial.print(AcX);
  Serial.print(" ");
  Serial.println(GyX);
}