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

#include <OSCBundle.h>
#include <OSCBoards.h>

//#define DEBUG

#ifdef BOARD_HAS_USB_SERIAL
#include <SLIPEncodedUSBSerial.h>
SLIPEncodedUSBSerial SLIPSerial( thisBoardsSerialUSB );
#else
#include <SLIPEncodedSerial.h>
SLIPEncodedSerial SLIPSerial(Serial); // Change to Serial1 or Serial2 etc. for boards with multiple serial ports that don’t have Serial
#endif

// Set your new MAC Address
// MAC naslov sprejemnika: 08:3A:F2:50:EF:6C
uint8_t newMACAddress[] = {0x08, 0x3A, 0xF2, 0x50, 0xEF, 0x6C};

typedef struct sensor_msg {
  uint8_t id;
  float aX;
  float aY;
  float aZ;
  float qX;
  float qY;
  float qZ;
  float qW;
} sensor_msg;

//sensor_msg odcitek;

// Maksimalno stevilo
#define ST_KEGLOV 10

int odcitekId;
sensor_msg  odcitki[ST_KEGLOV];
bool poslji[ST_KEGLOV];

void prejemPodatkov(const uint8_t * mac_addr, const uint8_t * noviPodatki, int len) {
  
  char macNaslov[18];
  snprintf(macNaslov, sizeof(macNaslov), "%02x:%02x:%02x:%02x:%02x:%02x",
           mac_addr[0], mac_addr[1], mac_addr[2], mac_addr[3], mac_addr[4], mac_addr[5]);
  
  #ifdef DEBUG
  Serial.print("Prejel podatke od ");
  Serial.println(macNaslov);
  #endif

  // ID odcitka je na prvem mestu!
  memcpy(&odcitekId, noviPodatki, sizeof(uint8_t));
  
  #ifdef DEBUG
  Serial.print("RAZBIRAM ID ");
  Serial.println(odcitekId);
  #endif
  
  memcpy(&odcitki[odcitekId], noviPodatki, sizeof(sensor_msg));

  #ifdef DEBUG
  Serial.printf("aX: %i \n", odcitki[odcitekId].aX);
  Serial.printf("aY: %i \n", odcitki[odcitekId].aY);
  Serial.printf("aZ: %i \n", odcitki[odcitekId].aZ);
  Serial.printf("qX: %f \n", odcitki[odcitekId].qX);
  Serial.printf("qY: %f \n", odcitki[odcitekId].qY);
  Serial.printf("qZ: %f \n", odcitki[odcitekId].qZ);
  Serial.printf("qW: %f \n", odcitki[odcitekId].qW);
  Serial.println();
  #endif  

  poslji[odcitekId] = true;
}

void setup() {
  SLIPSerial.begin(115200);

  // Ne posiljaj preden se podatki napolnijo
  for (int i = 0; i < ST_KEGLOV; i++) {
    poslji[0] = false;
  }

  Serial.println("Inicializiram WIFI...");
  WiFi.mode(WIFI_STA);

  esp_wifi_set_mac(WIFI_IF_STA, &newMACAddress[0]);
  
  esp_err_t result = esp_now_init();
  if (result != ESP_OK) {
    Serial.println("Error initializing ESP-NOW");
    Serial.println(result);
    return;
  }
  Serial.print("MAC naslov: ");
  Serial.println(WiFi.macAddress());
  esp_now_register_recv_cb(prejemPodatkov);
}

void loop() {
  /* OSC MSG channels */
  OSCBundle bundle;
  char glava[32];

  for (int i = 0; i < ST_KEGLOV; i++) {
    if (poslji[i]) {
      sprintf(glava, "/ww/%d/accel", i); 
      /*
      Serial.print("Posiljam ");
      Serial.println(glava);
      */
      bundle.add(glava)
        .add(odcitki[i].aX)
        .add(odcitki[i].aY)
        .add(odcitki[i].aZ);

      sprintf(glava, "/ww/%d/quaternion", i);
      /*
      Serial.print("Posiljam ");
      Serial.println(glava);
      */
      bundle.add(glava)
        .add(odcitki[i].qW)
        .add(odcitki[i].qX)
        .add(odcitki[i].qY)
        .add(odcitki[i].qZ);

      /*
      Serial.printf("XaX: %i \n", odcitki[i].aX);
      Serial.printf("XaY: %i \n", odcitki[i].aY);
      Serial.printf("XaZ: %i \n", odcitki[i].aZ);
      Serial.printf("XqX: %f \n", odcitki[i].qX);
      Serial.printf("XqY: %f \n", odcitki[i].qY);
      Serial.printf("XqZ: %f \n", odcitki[i].qZ);
      Serial.printf("XqW: %f \n", odcitki[i].qW);
      Serial.println();
      */

      SLIPSerial.beginPacket();
      bundle.send(SLIPSerial);
      SLIPSerial.endPacket();
      
      bundle.empty();
      poslji[i] = false;
    }
  }
  //delay(10);
}