#include <WiFi.h>
#include <esp_now.h>
#include <ESP32Servo.h>

Servo servoHorz;
Servo servoVert;

//Definitions
#define VERT_PIN  7
#define HORZ_PIN  8
#define SERVO_HORZ_PIN 9
#define SERVO_VERT_PIN 10

#define J1_THRESHOLD_UP    10
#define J1_THRESHOLD_DOWN  10
#define J1_THRESHOLD_LEFT  10
#define J1_THRESHOLD_RIGHT 10

//Variables
int mapHorz = 0;
int mapVert = 0;

//MAC Address of responder
uint8_t broadcastAddress[] = {0xBC, 0xFF, 0x4D, 0x49, 0x69, 0x10};

//Message Structure
  typedef struct struct_message {
  int mapHorz;
  int mapVert;
} struct_message;

struct_message myData;

//Peer info
esp_now_peer_info_t peerInfo;

//Callback function called when data is sent
void OnDataSent(const uint8_t *mac_addr, esp_now_send_status_t status) {
  Serial.print("\r\nLast Packet Send Status:\t");
  Serial.println(status == ESP_NOW_SEND_SUCCESS ? "Delivery Success" : "Delivery Fail");
}


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

// Set ESP32 as a Wi-Fi Station
  WiFi.mode(WIFI_STA);

// Initilize ESP-NOW
  if (esp_now_init() != ESP_OK) {
    Serial.println("Error initializing ESP-NOW");
    return;
  }

// Register the send callback
  esp_now_register_send_cb(OnDataSent);

// Register peer
  memcpy(peerInfo.peer_addr, broadcastAddress, 6);
  peerInfo.channel = 0;  
  peerInfo.encrypt = false;

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

  servoHorz.attach(SERVO_HORZ_PIN);
  servoVert.attach(SERVO_VERT_PIN);

  pinMode(VERT_PIN, INPUT);
  pinMode(HORZ_PIN, INPUT);
}

void loop() {
//Read Joystick 1
mapHorz = map(analogRead(HORZ_PIN), 0, 8191, 0, 180);
mapVert = map(analogRead(VERT_PIN), 0, 8191, 0, 180);

// Format structured data
myData.mapHorz = mapHorz;
myData.mapVert = mapVert;

// Send message via ESP-NOW
  esp_err_t result = esp_now_send(broadcastAddress, (uint8_t *) &myData, sizeof(myData));

    if (result == ESP_OK) {
    Serial.println("Sending confirmed");
  }
  else {
    Serial.println("Sending error");
  } 

servoHorz.write(mapHorz);
servoVert.write(mapVert);

  Serial.println("Horz:");
  Serial.print(myData.mapHorz);
  Serial.println("Vert:");
  Serial.print(myData.mapVert);

  delay(1000);
};