#ifndef myClasses_h
#define myClasses_h

#include <Arduino.h> 
#include <esp_now.h>
#include <WiFi.h>
#include <NewPing.h>

typedef void (*CallbackFunction)(const esp_now_recv_info *, const unsigned char *, int);
typedef struct struct_message {
  char text[32];
} struct_message;


class esp {
private:
  uint8_t myMAC[6];


public:
  esp(uint8_t *);
  bool init(CallbackFunction, esp);
  bool send(char *, esp);
};

#endif


#include "myClasses.h"


esp::esp(uint8_t *p_mac) {
  for (int i = 0; i < 6; i++)
    myMAC[i] = p_mac[i];
}

//bool esp::init(esp receiver, CallbackFunction func) {
bool esp::init(CallbackFunction func, esp receiver) {

  esp_now_peer_info_t peerInfo;

  WiFi.mode(WIFI_STA);  // ESP-NOW works in station mode

  if (esp_now_init() != ESP_OK) {
    Serial.println("ESP-NOW initialization failed!");
    return (false);
  }

  esp_now_register_recv_cb(func);

  memset(&peerInfo, 0, sizeof(peerInfo));
  memcpy(peerInfo.peer_addr, receiver.myMAC, 6);
  peerInfo.channel = 0;
  peerInfo.encrypt = false;

  if (esp_now_add_peer(&peerInfo) != ESP_OK) {
    Serial.println("Failed to add peer");
    return (false);
  }

  Serial.println("Wireless Communication ready!");
  return (true);
}

bool esp::send(char *data, esp receiver) {

  struct_message message;
 
  strcpy(message.text, data);

  esp_err_t result = esp_now_send(receiver.myMAC, (uint8_t *)&message, sizeof(message));

  if (result == ESP_OK) {
    Serial.println("Message sent successfully");
    return (true);
  } else {
    Serial.println("Message send failed");
    return (false);
  }
}



// Κύκλωμα με δύο ultrasonic
//24:ec:4a:2c:4f:7c   ESP με ultrasonic
//e8:06:90:95:76:f0   ESP με servo

#include <myClasses.h>

int dataReceived = 0;

void OnDataRecv(const esp_now_recv_info *mac, const unsigned char *incomingData, int len) {
  struct_message message;

  memcpy(&message, incomingData, sizeof(message));

  Serial.println(message.text);

  dataReceived = 0;

  if (message.text[0] == '1') {
    Serial.println("Left");
    dataReceived = 1;
  }

  if (message.text[0] == '2') {

    Serial.println("Middle");
    dataReceived = 2;
  }

  if (message.text[0] == '3') {

    Serial.println("Right");
    dataReceived = 3;
  }
};



// Ορίζουμε τις MAC Addresses των δύο ESP. Είναι αριθμοί σε δεκαεξαδική μορφή.
uint8_t myMAC[] = { 0x24, 0xEC, 0x4A, 0x2C, 0x4F, 0x7C };
uint8_t receiverMAC[] = { 0xE8, 0x06, 0x90, 0x95, 0x76, 0xF0 };

// Δημιουργούμε δύο αντικείμενα esp. Το πρώτο δημιουργείται με τη δική μας MAC
// και το δεύτερο με την MAC του παραλήπτη.
esp me(myMAC);
esp receiver(receiverMAC);

#define SONAR_NUM 2       // Number of sensors.
#define MAX_DISTANCE 400  // Maximum distance (in cm) to ping.

// Εδώ δημιουργούνται τα αντικείμενα sonar1 και sonar2.
NewPing sonar1 = NewPing(6, 5, MAX_DISTANCE);
NewPing sonar2 = NewPing(7, 8, MAX_DISTANCE);

// Οι μεταβλητές dist1 και dist2 είναι για την απόσταση που
// θα διαβάζουν το sonar1 και το sonar2 αντίστοιχα.
float dist1, dist2;


void setup() {
  Serial.begin(9600);  // Open serial monitor at 9600 baud to see ping results.
  me.init(OnDataRecv, receiver);  // το αντικείμενο me, εκτελεί την ικανότητά του init()
}


void loop() {
  delay(185);  // Wait 50ms between pings (about 20 pings/sec). 29ms should be the shortest delay between pings.

  // Το αντικείμενο sonar1 εκτελεί την ικανότητά του που λέγεται ping_cm()
  // και η οποία μετράει την απόσταση από το κοντινότερο εμπόδιο.
  // Την απόσταση αυτή μας την επιστρέφει (μας την λέει). Οπότε, εμείς την
  // αποθηκεύουμε στη μεταβλητή dist1.

  dist1 = sonar1.ping_cm();

  // Το αντικείμενο sonar2 εκτελεί την ικανότητά του που λέγεται ping_cm()
  // και η οποία μετράει την απόσταση από το κοντινότερο εμπόδιο.
  // Την απόσταση αυτή μας την επιστρέφει (μας την λέει). Οπότε, εμείς την
  // αποθηκεύουμε στη μεταβλητή dist2.
  dist2 = sonar2.ping_cm();

  //Tυπώνουμε στο Serial monitor τις τιμές, για να τις δούμε.
  Serial.print(dist1);
  Serial.println("cm ");

  Serial.print(dist2);
  Serial.println("cm ");


  // Ελέγχουμε διάφορες περιπτώσεις ώστε να αποφασίσουμε αν τα μάτια πρέπει
  // να κοιτάξουν δεξιά, αριστερά ή ευθεία μπροστά.
  // Το αποτέλεσμα του ελέγχου το τυπώνουμε στο Serial monitor για να το δούμε.
  // Θα πρεπει όμως να το στέλνουμε και σαν πληροφορία στο άλλο ESP, ώστε να κινηθούν τα μάτια.
  if (dist1 > dist2 + 10) 
  {
    Serial.println("right");
    me.send("3", receiver);
  } 
  else if (dist1 + 10 < dist2) 
  {
    Serial.println("left");
    me.send("1", receiver);
  } 
  else
  {
    Serial.println("middle");
    me.send("2", receiver);
  }
}