#include <Arduino.h>
#include <stdio.h>
#include <esp_now.h>
#include <WiFi.h>
#include "configuration.h"
#include "crv2_esp_now.h"
#include "neighbors.h"

// Our broadcast peer
esp_now_peer_info_t bcast;
bool waiting_on_send = false;

/* Send a message in an MBOX header 

  Header:
    Bytes 0-3: "MBOX"
    Byte 4: "Message Type"
    Byte 5: Length, not including header.
*/
bool send_encapsulated(const uint8_t type, const uint8_t *data, const size_t len) {
  static uint8_t buffer[256] = "MBOX";
  buffer[4] = PROTOCOL_VERSION;
  buffer[5] = type;
  buffer[6] = len;
  //Serial.print("send_encapsulated len = "); Serial.println(buffer[6]);
  memcpy(buffer+7, data, len);
  return send_broadcast(buffer, len + 7);
}

/* Decapsulate a message and route it appropriately */
void process_message(receive_buffer_t& incoming) {
  uint8_t *buffer = incoming.data;
  if(buffer[0] != 77 || buffer[1] != 66 || buffer[2] != 79 || buffer[3] != 88) {
    Serial.print("Received non MBOX message from "); Serial.print((char *)incoming.mac_addr); Serial.println(". Ignoring.");
    return;
  }
  if(buffer[4] < PROTOCOL_VERSION) {
    Serial.print("Warning. Protocol version "); Serial.print((int) buffer[4]); Serial.print(" is earlier than expected version ("); Serial.print((int) PROTOCOL_VERSION); Serial.println("). Results may be unexpected.");
  }
  if(buffer[4] > PROTOCOL_VERSION) {
    Serial.print("Warning. Protocol version "); Serial.print((int) buffer[4]); Serial.print(" is greater than expected version ("); Serial.print((int) PROTOCOL_VERSION); Serial.println("). Results may be unexpected.");
  }
  uint8_t type = buffer[5];
  uint8_t length = buffer[6];
  //Serial.print("Processing message; Type = "); Serial.print(type); Serial.print("; Length = "); Serial.println(length);
  switch(type) {
    case 1:
      //Serial.print("Processing neighbor discovery from peer "); Serial.println((char *)incoming.mac_addr);
      NList.process_discovery(incoming.mac_addr, incoming.data+7, length);
      break;
    default:
      Serial.print("ERROR: Unsupported transmission type "); 
      Serial.print((int) type);
      Serial.print(". Continuing, but results may be unexpected.");
      break;
  }
}

void initBroadcast() {
  memset(&bcast, 0, sizeof(bcast));
  for (int i = 0; i < 6; ++i) {
    bcast.peer_addr[i] = (uint8_t)0xff;
  }
  bcast.channel = CHANNEL; // pick a channel
  bcast.encrypt = 0; // no encryption
  manageBroadcast();
}

bool manageBroadcast() {
  if (bcast.channel == CHANNEL) {
    Serial.print("Bcast Status: ");
    const esp_now_peer_info_t *peer = &bcast;
    const uint8_t *peer_addr = bcast.peer_addr;
    // check if the peer exists
    bool exists = esp_now_is_peer_exist(peer_addr);
    if (exists) {
      // Slave already paired.
      Serial.println("Already Paired");
      return true;
    }
    else {
      // Slave not paired, attempt pair
      esp_err_t addStatus = esp_now_add_peer(peer);
      if (addStatus == ESP_OK) {
        // Pair success
        Serial.println("Pair success");
        return true;
      }
      else if (addStatus == ESP_ERR_ESPNOW_NOT_INIT) {
        // How did we get so far!!
        Serial.println("ESPNOW Not Init");
        return false;
      }
      else if (addStatus == ESP_ERR_ESPNOW_ARG) {
        Serial.println("Invalid Argument");
        return false;
      }
      else if (addStatus == ESP_ERR_ESPNOW_FULL) {
        Serial.println("Peer list full");
        return false;
      }
      else if (addStatus == ESP_ERR_ESPNOW_NO_MEM) {
        Serial.println("Out of memory");
        return false;
      }
      else if (addStatus == ESP_ERR_ESPNOW_EXIST) {
        Serial.println("Peer Exists");
        return true;
      }
      else {
        Serial.println("Not sure what happened");
        return false;
      }
    }
  }
  else {
    // No slave found to process
    Serial.println("No Slave found to process");
    return false;
  }
}

// callback when data is received
receive_buffer_t rbuf[RECEIVE_BUFFER_SIZE];
int rbuf_cur=0;
int rbuf_next=0;

receive_buffer_t *get_message() {
  static receive_buffer_t msg;

  if(rbuf_cur == rbuf_next) {
    return NULL;
  }
  //if(DEBUG) {
  //  Serial.print("Debugging get_message. Message available. cur = ");
  //  Serial.print(rbuf_cur);
  //  Serial.print("; next = ");
  //  Serial.println(rbuf_next);
  //}
  memcpy(&msg, &rbuf[rbuf_cur], sizeof(receive_buffer_t));
  rbuf_cur++;
  if(rbuf_cur >= RECEIVE_BUFFER_SIZE) {
    rbuf_cur = 0;
  }

  return &msg;
}

void OnDataRecv(const uint8_t *mac_addr, const uint8_t *data, int data_len) {
  // Quickly copy to a buffer, then return
  int dest = rbuf_next;
  if(dest >= RECEIVE_BUFFER_SIZE) {
    dest = 0;
    if(rbuf_cur == 0) {
      Serial.print("WARNING: Buffer overflow. rbuf_cur="); Serial.print(rbuf_cur); Serial.print("; rbuf_next="); Serial.println(rbuf_next);
      return;
    }
  }
  if(dest == rbuf_cur - 1) {
      Serial.print("WARNING: Buffer overflow. rbuf_cur="); Serial.print(rbuf_cur); Serial.print("; rbuf_next="); Serial.println(rbuf_next);
      return;    
  }
  
  // No buffer overflow, copy everything to our buffer
  snprintf((char *) rbuf[dest].mac_addr, sizeof(rbuf[dest].mac_addr), "%02x:%02x:%02x:%02x:%02x:%02x",
           mac_addr[0], mac_addr[1], mac_addr[2], mac_addr[3], mac_addr[4], mac_addr[5]);
  memcpy(rbuf[dest].data, data, data_len);
  rbuf[dest].data[data_len] = 0x0;
  rbuf[dest].data_len = data_len;
  if(DEBUG) {
    //Serial.print("Data received from: "); Serial.println((char*) rbuf[dest].mac_addr);
  }
  rbuf_next = dest + 1;
  if(rbuf_next >= RECEIVE_BUFFER_SIZE) {
    rbuf_next = 0;
  }
  return;
}

// callback when data is sent
void OnDataSent(const uint8_t *mac_addr, esp_now_send_status_t status) {
  char macStr[18];
  
  snprintf(macStr, sizeof(macStr), "%02x:%02x:%02x:%02x:%02x:%02x",
    mac_addr[0], mac_addr[1], mac_addr[2], mac_addr[3], mac_addr[4], mac_addr[5]);
  //Serial.print("Last Packet Sent to: "); Serial.println(macStr);
  //Serial.print("Last Packet Send Status: "); Serial.println(status == ESP_NOW_SEND_SUCCESS ? "Delivery Success" : "Delivery Fail");
  
  waiting_on_send = false;
}

bool send_broadcast(const uint8_t *data, const size_t len) {
  assert(len > 0);
  assert(len < ESP_NOW_MAX_DATA_LEN);
  while(waiting_on_send) {
    // an interrupt will clear this once the last send message has been sent
  }
  //Serial.print("Sending "); Serial.print(len); Serial.println(" bytes of data.");
  waiting_on_send = true;
  esp_err_t result = esp_now_send(bcast.peer_addr, data, len);
  if (result == ESP_OK) {
    //Serial.println("Send success.");
    return true;
  }
  else if (result == ESP_ERR_ESPNOW_NOT_INIT) {
    // How did we get so far!!
    Serial.println("ERROR: During send ESPNOW not Init.");
  }
  else if (result == ESP_ERR_ESPNOW_ARG) {
    Serial.println("ERROR: During send Invalid Argument");
  }
  else if (result == ESP_ERR_ESPNOW_INTERNAL) {
    Serial.println("ERROR: During send Internal Error");
  }
  else if (result == ESP_ERR_ESPNOW_NO_MEM) {
    Serial.println("ERROR: During send ESP_ERR_ESPNOW_NO_MEM");
  }
  else if (result == ESP_ERR_ESPNOW_NOT_FOUND) {
    Serial.println("ERROR: During send Peer not found.");
  }
  else {
    Serial.println("ERROR: During send Not sure what happened");
  }
  return false;
}