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

//MAC address of the car
uint8_t broadcastAddress[] = {0x7C, 0x9E, 0xBD, 0x02, 0x61, 0xA4};

//Variables storing forward, left, right, backward commands to be sent
int forward;
int left;
int right;
int backward;

//We will use these commands to see if the commands have been received
int incomingForward;
int incomingLeft;
int incomingRight;
int incomingBackward;

//Variable to store if the sent data was succesful
String success;

//Structure to send data
//Must match the receiver structure
typedef struct struct_message {
  int forw;
  int lft;
  int rght;
  int rev;
} struct_message;

//Create a struct_message called drivePath
struct_message drivePath;

//Create a struct_message to hold incoming data
struct_message incomingReadings;

// Callback 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");
  if (status ==0){
    success = "Delivery Success :)";
  }
  else{
    success = "Delivery Fail :(";
  }
}

// Callback when data is received
void OnDataRecv(const uint8_t * mac, const uint8_t *incomingData, int len) {
  memcpy(&incomingReadings, incomingData, sizeof(incomingReadings));
  Serial.print("Bytes received: ");
  Serial.println(len);
  incomingForward = incomingReadings.forw;
  incomingLeft = incomingReadings.lft;
  incomingRight = incomingReadings.rght;
  incomingBackward = incomingReadings.rev;
}

int thumbIndex = 23;
int indexMiddle = 22;
int middleRing = 21;
int ringPinkie = 19;
int LED1 = 16;
int LED2 = 4;
int other = 13;

void setup() {

   pinMode(thumbIndex, INPUT);
   pinMode(indexMiddle, INPUT); 
   pinMode(middleRing, INPUT);
   pinMode(ringPinkie, INPUT); 
   pinMode(LED1, OUTPUT);
   pinMode(LED2, OUTPUT);
   pinMode(other, OUTPUT);
   

//   digitalWrite(LED1, HIGH);
//   digitalWrite(LED2, HIGH);
//   digitalWrite(other, HIGH);

   Serial.begin(115200);
   
   WiFi.mode(WIFI_STA);
   
   // Init ESP-NOW
   if (esp_now_init() != ESP_OK) {
     Serial.println("Error initializing ESP-NOW");
     return;
   }

   // Once ESPNow is successfully Init, we will register for Send CB to
   // get the status of Trasnmitted packet
   esp_now_register_send_cb(OnDataSent);
  
   // Register peer
   esp_now_peer_info_t peerInfo;
   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;
   }
   // Register for a callback function that will be called when data is received
   esp_now_register_recv_cb(OnDataRecv);

   }

void loop() {

   if (!(digitalRead(indexMiddle)) && !(digitalRead(middleRing)) && !(digitalRead(ringPinkie))) {
       if (digitalRead(thumbIndex)) {
        digitalWrite(LED1, HIGH);
        digitalWrite(LED2, HIGH);
        drivePath.forw = 1;
        drivePath.lft = 0;
        drivePath.rght = 0;
        drivePath.rev = 0;
       }
       else {
        digitalWrite(LED1, LOW);
        digitalWrite(LED2, LOW);
        drivePath.forw = 0;
        drivePath.lft = 0;
        drivePath.rght = 0;
        drivePath.rev = 0;
       }
   }
   else if (!(digitalRead(thumbIndex)) && !(digitalRead(middleRing)) && !(digitalRead(ringPinkie))) {
       if (digitalRead(indexMiddle)) {
        digitalWrite(LED1, LOW);
        digitalWrite(LED2, HIGH);
        drivePath.forw = 0;
        drivePath.lft = 0;
        drivePath.rght = 0;
        drivePath.rev = 1;
       }
       else {
        digitalWrite(LED1, LOW);
        digitalWrite(LED2, LOW);
        drivePath.forw = 0;
        drivePath.lft = 0;
        drivePath.rght = 0;
        drivePath.rev = 0;
       }
   }
   else if (!(digitalRead(thumbIndex)) && !(digitalRead(indexMiddle)) && !(digitalRead(ringPinkie))) {
       if (digitalRead(middleRing)) {
        digitalWrite(LED1, HIGH);
        digitalWrite(LED2, LOW);
        drivePath.forw = 0;
        drivePath.lft = 0;
        drivePath.rght = 1;
        drivePath.rev = 0;
       }
       else {
        digitalWrite(LED1, LOW);
        digitalWrite(LED2, LOW);
        drivePath.forw = 0;
        drivePath.lft = 0;
        drivePath.rght = 0;
        drivePath.rev = 0;
       }
   }
   else if (!(digitalRead(thumbIndex)) && !(digitalRead(indexMiddle)) && !(digitalRead(middleRing))) {
       if (digitalRead(ringPinkie)) {
        digitalWrite(LED1, HIGH);
        digitalWrite(LED2, HIGH);
        drivePath.forw = 0;
        drivePath.lft = 1;
        drivePath.rght = 0;
        drivePath.rev = 0;
       }
       else {
        digitalWrite(LED1, LOW);
        digitalWrite(LED2, LOW);
        drivePath.forw = 0;
        drivePath.lft = 0;
        drivePath.rght = 0;
        drivePath.rev = 0;
       }
  }

  // Send message via ESP-NOW
  esp_err_t result = esp_now_send(broadcastAddress, (uint8_t *) &drivePath, sizeof(drivePath));
  
  if (result == ESP_OK) {
    Serial.println("Sent with success");
  }
  else {
    Serial.println("Error sending the data");
  }

   }