#include <esp_now.h>
#include <WiFi.h>
#include <Arduino.h>
#include <ESP32CAN.h>
#include <CAN_config.h>


// CAN device
CAN_device_t CAN_cfg;               // CAN Config
const int rx_queue_size = 10;       // Receive Queue size
CAN_frame_t rx_frame;
CAN_frame_t tx_frame;


// Comment only the board to which code is uploaded right not
uint8_t broadcastAddress1[] = {0xFC, 0xF5, 0xC4, 0x2F, 0x71, 0x24}; //ALPHA
uint8_t broadcastAddress2[] = {0xFC, 0xF5, 0xC4, 0x2F, 0xE2, 0xF0}; //BETA
uint8_t broadcastAddress3[] = {0xFC, 0xF5, 0xC4, 0x31, 0x7E, 0x08}; //THETA
//uint8_t broadcastAddress3[] = {0xFC, 0xF5, 0xC4, 0x31, 0x9E, 0xB4}; //GAMMA


// Variable to store if sending data was successful
String success;

// Create a struct_message to hold incoming frame
CAN_frame_t incomingFrame;

// CAN frame
CAN_frame_t frame2send;


// 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(&incomingFrame, incomingData, sizeof(incomingFrame));
  tx_frame = incomingFrame;
  ESP32Can.CANWriteFrame(&tx_frame);
}
 
void setup() {
  // Init Serial Monitor
  Serial.begin(115200);

  // External antenna ON
  pinMode(GPIO_NUM_21, OUTPUT);           
  digitalWrite(GPIO_NUM_21, HIGH); 

  // Init CAN Module
  CAN_cfg.speed = CAN_SPEED_250KBPS;
  CAN_cfg.tx_pin_id = GPIO_NUM_25;
  CAN_cfg.rx_pin_id = GPIO_NUM_26;
  CAN_cfg.rx_queue = xQueueCreate(rx_queue_size, sizeof(CAN_frame_t));
  ESP32Can.CANInit();
  Serial.println("\nCAN init done");

  // 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;
  }
  esp_now_register_send_cb(OnDataSent);
  
  // Register peer
  esp_now_peer_info_t peerInfo;
  peerInfo.channel = 0;  
  peerInfo.encrypt = false;
  
  // Add peer 1
  memcpy(peerInfo.peer_addr, broadcastAddress1, 6);       
  if (esp_now_add_peer(&peerInfo) != ESP_OK){
    Serial.println("Failed to add peer");
    return;
  }
  // Add peer 2 
  memcpy(peerInfo.peer_addr, broadcastAddress2, 6);        
  if (esp_now_add_peer(&peerInfo) != ESP_OK){
    Serial.println("Failed to add peer");
    return;
  }
    // Add peer 3 
  memcpy(peerInfo.peer_addr, broadcastAddress3, 6);        
  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 (xQueueReceive(CAN_cfg.rx_queue, &rx_frame, 3 * portTICK_PERIOD_MS) == pdTRUE) 
  {
    frame2send = rx_frame;
    esp_err_t result = esp_now_send(0, (uint8_t *) &frame2send, sizeof(frame2send));
    if (result == ESP_OK) {
      Serial.println("Sent with success");
    }
    else {
      Serial.println("Error sending the data");
    }
  }
}
