/*
* This node sends the magnetic direction the pole is facing. 
*/

#include <SPI.h>
#include "DigitalIO.h"
#include "RF24.h"

/****************** User Config ***************************/
/***      Set this radio as radio number 0 or 1         ***/
bool radioNumber = 0;

/* Hardware configuration: Set up nRF24L01 radio on SPI bus plus pins 6 & 5 */
/* Currently using Software SPI */
RF24 radio(6,5);
/**********************************************************/

byte addresses[][6] = {"1Node","2Node"};

// the most recent direction in degrees
int degrees_ = 0;

void setup() {
  Serial.begin(115200);
  Serial.println(F("GPS Pole Serial Interface"));
  
  radio.begin();

  // Set the PA Level low to prevent power supply related issues since this is a
 // getting_started sketch, and the likelihood of close proximity of the devices. RF24_PA_MAX is default.
  radio.setPALevel(RF24_PA_LOW);
  
  // Open a writing and reading pipe on each radio, with opposite addresses
  if(radioNumber){
    radio.openWritingPipe(addresses[1]);
    radio.openReadingPipe(1,addresses[0]);
  }else{
    radio.openWritingPipe(addresses[0]);
    radio.openReadingPipe(1,addresses[1]);
  }
  
  // Start the radio listening for data
  radio.startListening();
}

void loop() {
  
/****************** Sending Data Role ***************************/      
    radio.stopListening();                                    // First, stop listening so we can talk.
    
    Serial.println(F("Now sending"));

    degrees_ += 15;                                      // Increment Degrees - then send to the screen receiver
    degrees_ %= 360; 
    
     if (!radio.write( &degrees_, sizeof(int) )){
       Serial.println(F("failed"));
     }
        
    radio.startListening();                                    // Now, continue listening
    
    unsigned long started_waiting_at = micros();               // Set up a timeout period, get the current microseconds
    boolean timeout = false;                                   // Set up a variable to indicate if a response was received or not
    
    while ( ! radio.available() ){                             // While nothing is received
      if (micros() - started_waiting_at > 200000 ){            // If waited longer than 200ms, indicate timeout and exit while loop
          timeout = true;
          break;
      }      
    }
        
    if ( timeout ){                                             // Describe the results
        Serial.println(F("Failed, response timed out."));
    }else{
        unsigned long got_time;                                 // Grab the response, compare, and send to debugging spew
        radio.read( &got_time, sizeof(unsigned long) );
        unsigned long time = micros();
        
        // Spew it
        Serial.print(F("Sent "));
        Serial.print(degrees_);
        Serial.print(F(", Got response "));
        Serial.print(got_time);
        Serial.print(F(", Response delay "));
        Serial.print(started_waiting_at-got_time);
        Serial.println(F(" microseconds"));
    }

    // Try again 1s later
    delay(1000);

} // Loop