#include <nRF905.h>
#include <SPI.h>

#define RXADDR {0xFE, 0x4C, 0xA6, 0xE5} // Address of this device (4 bytes)
#define TXADDR {0x58, 0x6F, 0x2E, 0x10} // Address of device to send to (4 bytes)

String ENQ = "0x05";
String ACK = "0x06";
String SOH = "0x01";
String SA = "_EH1";
String DA = "_EC1";
String PW = "_tenAaie";
String data = "PB URGENT!!!";
String CRC = "CRC";
String EOT = "0x04";
String okF = "0x120";
String reply;
String frame;

void setup() {
  Serial.begin(38400);
  nRF905_init();
  byte addr[] = RXADDR;
  nRF905_setRXAddress(addr);
  byte addr1[] = TXADDR;
  nRF905_setTXAddress(addr1);
  nRF905_receive();
}

void loop() {

  //SEND ENQ
  byte replyH[50] = {0};
  ENQ.getBytes(replyH, 50);
  nRF905_setData(replyH, sizeof(replyH));
  while (!nRF905_send());
  nRF905_receive();
  Serial.println("ENQ SENT");
  Serial.write(replyH, sizeof(replyH));
  Serial.println();

  ///WAIT FOR reply (receive)
  reply = "\0";
  Serial.println("WAITING FOR REPLY");
  byte buffer[50] = {0};
  while (!nRF905_getData(buffer, sizeof(buffer)));
  nRF905_setData(buffer, sizeof(buffer));
  while (!nRF905_send());
  nRF905_receive();
  Serial.print(F("Data: "));
  Serial.write(buffer, sizeof(buffer));
  Serial.println();

  for (int i = 0; i < sizeof(buffer); i++) {
    if ((buffer[i] > 0) && (buffer[i] < 127) && (buffer[i] != 32))
      reply = reply + char(buffer[i]);
  }

  Serial.println(reply);

  if (reply == ACK) {
    Serial.println("YEY ACKNOWLEDGE!");
    frame = SOH + DA + SA + PW + data  + CRC + EOT;

frameLoop:

    //SEND ENQ
    delay(1000);
    byte replyH[50] = {0};
    frame.getBytes(replyH, 50);
    nRF905_setData(replyH, sizeof(replyH));
    while (!nRF905_send());
    nRF905_receive();
    Serial.println("FRAME SENT");
    Serial.write(replyH, sizeof(replyH));
    Serial.println();

    ///WAIT FOR REPLY (receive)
    reply = "\0";
    Serial.println("WAITING FOR REPLY");
    byte buffer[50] = {0};
    while (!nRF905_getData(buffer, sizeof(buffer)));
    nRF905_setData(buffer, sizeof(buffer));
    while (!nRF905_send());
    nRF905_receive();
    Serial.print(F("Data: "));
    Serial.write(buffer, sizeof(buffer));
    Serial.println();

    for (int i = 0; i < sizeof(buffer); i++) {
      if ((buffer[i] > 0) && (buffer[i] < 127) && (buffer[i] != 32))
        reply = reply + char(buffer[i]);
    }

    Serial.println(reply);

    if (reply == okF) {
      Serial.println("YEY THE FRAME HAS BEEN RECEIVED!");
    }
    else {
      Serial.println("UGHHH NEED TO RESEND FRAME");
      goto frameLoop;
    }
  }
  else {
    Serial.println("ugh");
  }

  delay(1000);
}