#ifndef myClasses_h
#define myClasses_h

#include <Arduino.h> 
#include <esp_now.h>
#include <WiFi.h>
#include <ESP32Servo.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);
  }
}

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

#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[] = { 0xE8, 0x06, 0x90, 0x95, 0x76, 0xF0 };
uint8_t receiverMAC[] = { 0x24, 0xEC, 0x4A, 0x2C, 0x4F, 0x7C };

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


Servo myServo1, myServo2;

void setup() {
  
  Serial.begin(9600); 

  me.init(OnDataRecv, receiver);  // το αντικείμενο me, εκτελεί την ικανότητά του init()

  myServo1.attach(6);  
  myServo1.write(0);

  myServo2.attach(7);
  myServo2.write(0);

  delay(15);
   
}

void loop() {
  
  if (dataReceived == 1) {
    myServo1.write(170);    // υποθετικά, οι 170 μοίρες είναι προς αριστερά
    myServo2.write(170);
    delay(15);
    dataReceived = 0;
  }

  if (dataReceived == 2) {
    myServo1.write(90);      // υποθετικά, οι 90 μοίρες είναι ευθεία μπροστά
    myServo2.write(90);
    delay(15);
    dataReceived = 0;
  }

  if (dataReceived == 3) {   
    myServo1.write(0);     // υποθετικά, οι 0 μοίρες είναι προς δεξιά
    myServo2.write(0);
    delay(15);
    dataReceived = 0;
  }


    delay(50);
}