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

#include "mac_config.h"

#define esc1pin 15
#define esc2pin 13
#define voltageEnable 12
#define voltagePin 36
#define failsafeValue 127
#define ONE_WIRE_BUS 14
#define fanRelais 16

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

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


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(uint8_t firstServo, uint8_t secondServo) {
  esc1.write(firstServo);
  esc2.write(secondServo);
}

// 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[3];
  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);

// 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;
  // Could check mac here for some security
  writeServos(recData[0], recData[1]);
  setBoardOptions(recData[2]);
  Serial.print("recieved: ");
  Serial.println(recData[2], BIN);
  display.clear();
  char buf[25];
  sprintf(buf, "1: %i | 2: %i", recData[0], recData[1]);
  display.drawString(2, 0, buf);
  display.display();
}

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

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

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

  initRPMPins();

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

  setupLights();
  lightOff();

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


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

  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 = ESP_IF_WIFI_AP;
  manageRemote();
}


void loop() {
  if(millis() - lastPacket > CONNECTION_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);
    display.clear();
    char buf[25];
    sprintf(buf, "FAIL: %i", failsafeValue);
    display.drawString(2, 0, buf);
    display.display();
  }

  checkTemperature();
  checkVoltage();
  if(shouldUpdateLights) updateLights();
}