//NRF24
#include "RF24.h"
#include "RF24Network.h"
#include "RF24Mesh.h"
#include <SPI.h>
#include <EEPROM.h>

//BNO 055
#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_BNO055.h>
#include <utility/imumaths.h>
//#include <printf.h>

RF24 radio(7, 8);
RF24Network network(radio);
RF24Mesh mesh(radio, network);

#define nodeID 1
#define BNO055_SAMPLERATE_DELAY_MS (100)

Adafruit_BNO055 bno = Adafruit_BNO055();

uint32_t displayTimer = 0;
uint32_t rotary = 0;
int updateRate = 200;

void setup() {

  Serial.begin(115200);
  pinMode(9, OUTPUT);
  digitalWrite(9, HIGH);
  if (!bno.begin())
  {
    Serial.print("Ooops, no BNO055 detected ... Check your wiring or I2C ADDR!");
    while (1);
  }

  delay(1000);
  digitalWrite(9, LOW);

  /* Display the current temperature */
  int8_t temp = bno.getTemp();
  Serial.print("Current Temperature: ");
  Serial.print(temp);
  Serial.println(" C");
  Serial.println("");

  bno.setExtCrystalUse(true);

  mesh.setNodeID(nodeID);
  // Connect to the mesh
  Serial.println(F("Connecting to the mesh..."));
  mesh.begin();


}



void loop() {

  mesh.update();

  // Send to the master node every second
  if (millis() - displayTimer >= updateRate) {
    displayTimer = millis();
    //sendData(displayTimer, 'M');
   // sendData(rotary++, 'R');
    imu::Quaternion quat = bno.getQuat();
    sendData((quat.w()), 'W');
    sendData((quat.x()), 'X');
    sendData((quat.y()), 'Y');
    sendData((quat.z()), 'Z');


  }

}

bool sendData(double value, char type) {
  bool messageSend;
  digitalWrite(9, HIGH);
  if (!mesh.write(&value, type, sizeof(value))) {

    if ( ! mesh.checkConnection() ) {
      Serial.println("Renewing Address");
      mesh.renewAddress();
    } else {
      Serial.println("Send fail, Test OK");
      messageSend = false;
    }
  } else {
    Serial.print("Send OK: "); Serial.println(value);
    messageSend = true;
    digitalWrite(9, LOW);

  }

  /*while (network.available()) {
    RF24NetworkHeader header;
    payload_t payload;
    network.read(header, &payload, sizeof(payload));
    Serial.print("Received packet #");
    Serial.print(payload.counter);
    Serial.print(" at ");
    Serial.println(payload.ms);
    }*/
  return messageSend;
}