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

// LED's
const int GREEN_LED = 2;
const int BLUE_LED = 16;

// joystick 1
const int STICKX1 = 36;
const int STICKY1 = 39;
const int STICKB1 = 19;

// joystick 2
const int STICKX2 = 32;
const int STICKY2 = 33;
const int STICKB2 = 18;

// joystick 3
const int STICKX3 = 34;
const int STICKY3 = 35;
const int STICKB3 = 17;

// Replace with your slave's MAC address
uint8_t slaveAddress[] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};

// Structure example to send data
// Must match the receiver structure
typedef struct struct_message {
  int stickX1;
  int stickY1;
  int stickB1;
  
  int stickX2;
  int stickY2;
  int stickB2;
  
  int stickX3;
  int stickY3;
  int stickB3;    
} struct_message;

// Create a struct_message called myData
struct_message myData;

esp_now_peer_info_t peerInfo;

LiquidCrystal_I2C lcd(0x27, 16, 2);

void setup() {
  pinMode(GREEN_LED, OUTPUT);
  pinMode(BLUE_LED, OUTPUT);

  pinMode(STICKX1, INPUT);
  pinMode(STICKY1, INPUT);
  pinMode(STICKB1, INPUT_PULLUP);
  
  pinMode(STICKY2, INPUT);
  pinMode(STICKX2, INPUT);
  pinMode(STICKB2, INPUT_PULLUP);
  
  pinMode(STICKX3, INPUT);
  pinMode(STICKY3, INPUT);
  pinMode(STICKB3, INPUT_PULLUP);
   
  Wire.begin();  
  Serial.begin(115200);
  lcd.init();
  lcd.backlight();
  
  // Set device as a Wi-Fi Station
  WiFi.mode(WIFI_STA);

    // Init ESP-NOW
  if (esp_now_init() != ESP_OK) {
    Serial.println("Error initializing ESP-NOW");
    return;
  }
  
  // Register peer
  memcpy(peerInfo.peer_addr, slaveAddress, 6);
  peerInfo.channel = 0;
  peerInfo.encrypt = false;
  
  if (esp_now_add_peer(&peerInfo) != ESP_OK) {
    Serial.println("Failed to add peer");
    digitalWrite(BLUE_LED, LOW);
    return;
  } else {
    digitalWrite(BLUE_LED, HIGH);
    return;
  }
}

void loop() {

  // joystick 1
  int stickX1Val = analogRead(STICKX1);
  int stickY1Val = analogRead(STICKY1);
  int stickX1Map = map(stickX1Val, 0, 4095, -100, 100);
  int stickY1Map = map(stickY1Val, 0, 4095, -100, 100);
  int stickB1Val = digitalRead(STICKB1);

  // joystick 2
  int stickX2Val = analogRead(STICKX2);
  int stickY2Val = analogRead(STICKY2);
  int stickX2Map = map(stickX2Val, 0, 4095, -100, 100);
  int stickY2Map = map(stickY2Val, 0, 4095, -100, 100);
  int stickB2Val = digitalRead(STICKB2);

  // joystick 3
  int stickX3Val = analogRead(STICKX3);
  int stickY3Val = analogRead(STICKY3);
  int stickX3Map = map(stickX3Val, 0, 4095, 100, -100);
  int stickY3Map = map(stickY3Val, 0, 4095, 100, -100);
  int stickB3Val = digitalRead(STICKB3);
  
  // Set values to send
  myData.stickX1 = stickX1Map;
  myData.stickY1 = stickY1Map;
  myData.stickB1 = stickB1Val;

  myData.stickX2 = stickX2Map;
  myData.stickY2 = stickY2Map;
  myData.stickB2 = stickB2Val;

  myData.stickX3 = stickX3Map;
  myData.stickY3 = stickY3Map;
  myData.stickB3 = stickB3Val;
  
  esp_err_t result = esp_now_send(slaveAddress, (uint8_t *) &myData, sizeof(myData));
 
  if (result == ESP_OK) {
    Serial.println("Sent!");
    digitalWrite(BLUE_LED, HIGH);
  } else {
    Serial.println("Failed to send data");
    digitalWrite(BLUE_LED, LOW);
  }

  lcd.setCursor(0, 0);
  lcd.print("Hello World!");
  
  delay(100); // Send data every 100 miliseconds
}