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

//MAC address of the car
uint8_t broadcastAddress[] = {0xC4, 0x4F, 0x33, 0x68, 0xE3, 0xAD};

//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 leftMotor_left = 22;
int leftMotor_right = 19;
int rightMotor_left = 18;
int rightMotor_right = 17;
int extra1 = 21;
int extra2 = 23;

void setup() {
   Serial.begin(115200);
   pinMode(leftMotor_left, OUTPUT);
   pinMode(leftMotor_right, OUTPUT); 
   pinMode(rightMotor_left, OUTPUT);
   pinMode(rightMotor_right, OUTPUT); 
   pinMode(extra1, OUTPUT);
   pinMode(extra2, OUTPUT);
   
   //digitalWrite(leftMotor_left, HIGH);
   //digitalWrite(leftMotor_right, LOW); 
   //digitalWrite(rightMotor_left, HIGH);
   //digitalWrite(rightMotor_right, LOW); 
   //digitalWrite(extra1, LOW);
   //digitalWrite(extra2, LOW);

   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() {
    // 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");
    }

    if (!incomingForward && !incomingBackward && !incomingLeft && !incomingRight) {
      digitalWrite(leftMotor_left, LOW);
      digitalWrite(leftMotor_right, LOW); 
      digitalWrite(rightMotor_left, LOW);
      digitalWrite(rightMotor_right, LOW); 
    }
    else if (incomingForward && !incomingLeft && !incomingRight && !incomingBackward) {         //Going forward gets priority
      digitalWrite(leftMotor_left, HIGH);
      digitalWrite(leftMotor_right, LOW); 
      digitalWrite(rightMotor_left, HIGH);
      digitalWrite(rightMotor_right, LOW); 
    }
    else if (incomingBackward && !incomingForward && !incomingLeft && !incomingRight) {        //Reverse gets second priority
      digitalWrite(leftMotor_left, LOW);
      digitalWrite(leftMotor_right, HIGH); 
      digitalWrite(rightMotor_left, LOW);
      digitalWrite(rightMotor_right, HIGH); 
    }
    else if (incomingLeft && !incomingForward && !incomingBackward && !incomingRight) {    //Going left gets third priority
      digitalWrite(leftMotor_left, LOW);
      digitalWrite(leftMotor_right, HIGH); 
      digitalWrite(rightMotor_left, HIGH);
      digitalWrite(rightMotor_right, LOW); 
    }
    else if (incomingRight && !incomingForward && !incomingBackward && !incomingLeft) {    //Going right gets fourth priority
      digitalWrite(leftMotor_left, HIGH);
      digitalWrite(leftMotor_right, LOW); 
      digitalWrite(rightMotor_left, LOW);
      digitalWrite(rightMotor_right, HIGH); 
    }

   }