// ESPNOWSkate Reciever by Lukas Bachschwell this device SLAVE =D
#include <Servo.h>
#include "Arduino.h"
#include "SSD1306.h"
#include <esp_now.h>
#include <WiFi.h>

static const int esc1pin = 15;
static const int esc2pin = 13;

Servo esc1;
Servo esc2;
SSD1306 display(0x3c, 5, 4);


//#define pairingMode
#define FAILSAFE 90
#define CONNECTION_TIMEOUT 200
#define CHANNEL 1
long lastPacket = 0;

bool isConnected = false;

// ESPNOW Functions ############################
//
// 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"));
  }
}

void writeServos(uint8_t firstServo, uint8_t secondServo) {
  isConnected = true;
  esc1.write(firstServo);
  esc2.write(secondServo);
}

// callback when data is recv from Master
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 Packet Recv from: "); Serial.println(macStr);
  uint8_t recData[2];
  memcpy(recData, data, data_len);
  Serial.print("Last Packet Recv Data: "); Serial.println(recData[0]); Serial.print(" "); Serial.print(recData[1]); Serial.print(" len:");  Serial.println(data_len);

  lastPacket = millis();
  isConnected = true;
  // Could check mac here for some security
  writeServos(recData[0], recData[1]);

  display.clear();
  char buf[25];
  sprintf(buf, "1: %i | 2: %i", recData[0], recData[1]);
  display.drawString(2, 0, buf);
  display.display();
}

// end ESPNOW functions

void setup() {
  Serial.begin(115200);
  Serial.println("ESPNowSkate Reciever");

  display.init();
  display.flipScreenVertically();
  display.setFont(ArialMT_Plain_16);

  esc1.attach(esc1pin);
  esc2.attach(esc2pin);

  //Set device in AP mode to begin with
  WiFi.mode(WIFI_AP);
  // configure device AP mode
  #ifdef pairingMode
  configDeviceAP(false);
  #else
  configDeviceAP(true);
  #endif

  Serial.print("AP MAC: "); Serial.println(WiFi.softAPmacAddress());

  // Init ESPNow
  if (esp_now_init() == ESP_OK) {
    Serial.println("ESPNow Init Success");
  }
  else {
    Serial.println("ESPNow Init Failed");
    ESP.restart();
  }
  // Once ESPNow is successfully Init, we will register for recv CB to
  // get recv packer info.
  esp_now_register_recv_cb(OnDataRecv);
}


void loop() {
  if(isConnected) {
    if(millis() - lastPacket > CONNECTION_TIMEOUT ) {
      isConnected = false;
      writeServos(FAILSAFE, FAILSAFE);
    }
  }
}