#include <SPI.h>
#include "nRF24L01.h"
#include "RF24.h"
#include "printf.h"

RF24 radio(9,10);
// Pipes for reading/writing { Controller, Head, Body }
const uint64_t pipes[3] = { 0xF0F0F0F0D2LL, 0xF0F0F0F0C3LL, 0xF0F0F0F0B4LL };

typedef struct{
  int forward;
  int lean;
  int swivel;
  int nod;
  int look;
  int sound;
  long timestamp;
}
tD;
tD transmit_data;


void setup(void)
{
    Serial.begin(115200);
    Serial.println("BaBy-8 Controller");

    radio.begin();
    radio.setPALevel(RF24_PA_LOW);
    radio.setRetries(15, 15);

    radio.openWritingPipe(pipes[0]);
    radio.openReadingPipe(1, pipes[1]); // Head
    //radio.openReadingPipe(2, pipes[2]); // Body
    radio.startListening();
    //radio.printDetails();
}

void loop(void)
{
    // First, stop listening so we can talk.
    radio.stopListening();

    // Take the time, and send it.  This will block until complete
    unsigned long time = millis();
    Serial.print("Now sending ");
    transmit_data.forward = 10;
    transmit_data.lean = 20;
    transmit_data.swivel = 30;
    transmit_data.nod = 40;
    transmit_data.look = 50;
    transmit_data.sound = 60;
    transmit_data.timestamp = millis();
    bool ok = radio.write( &transmit_data, sizeof(transmit_data) );

    if (ok)
        Serial.print("ok...");
    else
        Serial.println("failed!");

    // Now, continue listening
    radio.startListening();

    bool timeout = false;
    uint8_t pipe_num;
    while ( ! radio.available(&pipe_num) && ! timeout )
        if (millis() - transmit_data.timestamp > 250 )
            timeout = true;

    // Wait here until we get a response, or timeout (250ms)
    unsigned long started_waiting_at = millis();
    unsigned long got_time;

    if ( timeout ) {
        Serial.println("Failed, response timed out.");
    } else {
        radio.read( &got_time, sizeof(unsigned long) );
        Serial.print(pipe_num);
        Serial.print(" responded in ");
        Serial.print(millis()-got_time);
    }
    Serial.println("");

    delay(250);
}