// ESPNOWSkate Receiver by Lukas Bachschwell this device SLAVE =D
#include "Arduino.h"
#include <esp_now.h>
#include <WiFi.h>
// #include <OneWire.h>
// #include <DallasTemperature.h>
#include "valuehelpers.h"

#include "mac_config.h"

#define esc1pin 15
#define esc2pin 13
#define voltageEnable 12
#define voltagePin 36
#define ONE_WIRE_BUS 14
#define fanRelais 18 // Changed from originally 16 to help hover usart

#define DELETEBEFOREPAIR 0

// Resistors in Ohms
const float deviderR1 = 1275; // needs to be calibrated carefully using a multimeter because of tolerances of the resistors
const float deviderR2 = 22000;
const float refVoltage = 3.3;

#define VARIANT_HOVER 1
// #define ENABLE_DISPLAY 1

#ifdef VARIANT_HOVER
#define failsafeValue 0
#include "hoverusart.h"
#else
#define failsafeValue 127
#include <Servo.h>
Servo esc1;
Servo esc2;
#endif

#ifdef ENABLE_DISPLAY
#include "SSD1306.h"
SSD1306 display(0x3c, 5, 4);
#endif

OneWire oneWire(ONE_WIRE_BUS);
// DallasTemperature sensors(&oneWire); // one instance for all sensrs

esp_now_peer_info_t remote;

float temperature = 0;
float voltage = 0;
float kmh = 0;

uint8_t sendTemperature = 0;
uint8_t sendTemperatureDecimals = 0;
uint8_t sendVoltage = 0;
uint8_t sendVoltageDecimals = 0;
uint8_t sendSpeed = 0;
uint8_t sendSpeedDecimals = 0;

#include "rpm.h"

// #define pairingMode
#define CONNECTION_TIMEOUT 300
#define CHANNEL 1
long lastPacket = 0;

bool isConnected = false;
uint8_t oldOptions = 0;
#define FANS_AUTO 0
#define FANS_ON 1
#define FANS_OFF 2

bool lightActive = false;
int fanMode = FANS_AUTO;

#include "lights.h"
void setBoardOptions(uint8_t options)
{
  if (options != oldOptions)
  {
    oldOptions = options;
    shouldUpdateLights = true;
    // Serial.println("true2");
  }

  lightMode = options & 3;
  lightActive = (options >> 3) & 1;
  fanMode = (options >> 2) & 3;
}

// 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(uint16_t firstServo, uint16_t secondServo)
{
#ifdef VARIANT_HOVER
  Send(firstServo, secondServo);
#else
  esc1.write(firstServo);
  esc2.write(secondServo);
#endif
}

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

  // Answer with response

  const uint8_t respData[] = {sendVoltage, sendVoltageDecimals, sendTemperature, sendTemperatureDecimals, sendSpeed, sendSpeedDecimals};
  Serial.print("Sending RESPONSE.... ");
  esp_err_t result = esp_now_send(mac_addr, respData, sizeof(respData));
  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 if (result == ESP_ERR_ESPNOW_IF)
  {
    Serial.println("ESP_ERR_ESPNOW_IF");
  }
  else
  {
    Serial.println("Not sure what happened");
  }

  lastPacket = millis();
  isConnected = true;
  // TODO: Could check mac here for some minimal security
  writeServos(make16(recData[0], recData[1]), make16(recData[2], recData[3]));

  Serial.print("recieved Options: ");
  setBoardOptions(recData[4]);
  Serial.println(recData[4], BIN);
#ifdef ENABLE_DISPLAY
  display.clear();
  char buf[25];
  sprintf(buf, "1: %i | 2: %i", make16(recData[0], recData[1]), make16(recData[2], recData[3]));
  display.drawString(2, 0, buf);
  display.display();
#endif
}

void deletePeer()
{
  const esp_now_peer_info_t *peer = &remote;
  const uint8_t *peer_addr = remote.peer_addr;
  esp_err_t delStatus = esp_now_del_peer(peer_addr);
  Serial.print("Slave 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");
  }
}

bool manageRemote()
{
  if (remote.channel == CHANNEL)
  {
    if (DELETEBEFOREPAIR)
    {
      deletePeer();
    }

    Serial.print("Remote Status: ");
    const esp_now_peer_info_t *peer = &remote;
    const uint8_t *peer_addr = remote.peer_addr;
    // check if the peer exists
    bool exists = esp_now_is_peer_exist(peer_addr);
    if (exists)
    {
      // Slave already paired.
      Serial.println("Already Paired");
      return true;
    }
    else
    {
      // Slave 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 slave found to process
    Serial.println("No Slave found to process");
    return false;
  }
}

// end ESPNOW functions

void checkTemperature()
{
  // sensors.requestTemperatures(); // Send the command to get temperatures
  // temperature = sensors.getTempCByIndex(0);
  // Serial.print("Temp: ");
  // Serial.println(temperature);
  switch (fanMode)
  {
  case FANS_AUTO:
    if (temperature < 35)
      digitalWrite(fanRelais, LOW);
    if (temperature > 40)
      digitalWrite(fanRelais, HIGH);
    break;
  case FANS_ON:
    digitalWrite(fanRelais, HIGH);
    break;
  case FANS_OFF:
    digitalWrite(fanRelais, LOW);
    break;
  }

  sendTemperature = abs(floor(temperature));
  sendTemperatureDecimals = (temperature - sendTemperature) * 100;
}

void checkVoltage()
{
  digitalWrite(voltageEnable, HIGH);
  // Serial.print("Voltage: ");
  int value = analogRead(voltagePin);
  // Serial.println(value);

  float batteryVoltage = 0.0;
  int total = 0;

  for (int i = 0; i < 10; i++)
  {
    total += analogRead(voltagePin);
  }

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

  sendVoltage = abs(floor(batteryVoltage));
  sendVoltageDecimals = (batteryVoltage - sendVoltage) * 100;
  // Serial.print("Voltage: ");
  // Serial.println(batteryVoltage);
  digitalWrite(voltageEnable, LOW); // change to low
}

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

#ifdef ENABLE_DISPLAY
  display.init();
  display.flipScreenVertically();
  display.setFont(ArialMT_Plain_16);
#endif

  pinMode(voltageEnable, OUTPUT);
  pinMode(fanRelais, OUTPUT);

#ifdef VARIANT_HOVER
  initHoverSerial();
#else
  // Init escs, min and max value similar as Traxxas TQI 1100, 1900
  // chanel, minAngel, maxAngel, minPulseWidth, maxPulseWidth
  esc1.attach(esc1pin, 0, 0, 255, 1100, 1900);
  esc2.attach(esc2pin, 1, 0, 255, 1100, 1900);
  sensors.begin();
  initRPMPins();
#endif

  // setupLights();
  // lightOff();

  // xTaskCreatePinnedToCore(
  //     measureRpm,
  //     "rpm task",
  //     1000,
  //     NULL,
  //     1,
  //     &rpmTaskHandle,
  //     0);

  // Set device in AP mode to begin with
  Serial.println("INIT Wifi");

  WiFi.mode(WIFI_AP);
  Serial.println("INIT Pair");
// 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);

  for (int i = 0; i < 6; ++i)
  {
    remote.peer_addr[i] = (uint8_t)mac_remote[i];
  }

  remote.channel = CHANNEL; // pick a channel
  remote.encrypt = 0;       // no encryption
  remote.ifidx = WIFI_IF_AP;
  manageRemote();
}

void loop()
{
  if (millis() - lastPacket > CONNECTION_TIMEOUT)
  {
    Serial.println("Con timeout!!");
    isConnected = false;
    // int failsafeValue = map(analogRead(fallbackpin), 0, 4095, 0, 180);
    //  Taking 127 because it should be the center value anyway
    writeServos(failsafeValue, failsafeValue);
#ifdef ENABLE_DISPLAY
    display.clear();
    char buf[25];
    sprintf(buf, "FAIL: %i", failsafeValue);
    display.drawString(2, 0, buf);
    display.display();
#endif
  }

  // checkTemperature();
  // checkVoltage();
#ifdef VARIANT_HOVER
  Receive();
#endif
  // if (shouldUpdateLights)
  //   updateLights();
}