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

int echoPin = 4;
int trigPin = 3;

int __ID__ = 1;
int mount = 2;

int mydata[3];
int hisdata[3];

RF24 radio(9,10);

const uint64_t pipes[mount] = { 0xF0F0F0F0E1LL, 0xF0F0F0F0D2LL };


typedef enum { role_ping_out = 1, role_pong_back } role_e;


const char* role_friendly_name[] = { "invalid", "Ping out", "Pong back"};


role_e role = role_pong_back;

void setup(void)
{


  Serial.begin(9600);
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  printf_begin();
  printf("\n\rRF24/examples/GettingStarted/\n\r");
  printf("ROLE: %s\n\r",role_friendly_name[role]);
  printf("*** PRESS 'T' to begin transmitting to the other node\n\r");



  radio.begin();


  radio.setRetries(15,15);

  radio.openReadingPipe(1,pipes[__ID__-1]);

  radio.startListening();

  radio.printDetails();
}

int cm1=0;

void loop(void)
{
  radio.startListening();
  int duration, cm;
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  duration = pulseIn(echoPin, HIGH);
  cm = duration / 58;

 if((cm<=0.8*cm1) || (cm>=1.2*cm1))
 {
    role=role_ping_out;
 }
 else
 {
    role=role_pong_back;
 }

 if (role == role_ping_out)
 {
    cm1=cm;

    radio.stopListening();

    mydata[0] = __ID__;
    mydata[1] = millis();
    mydata[2] = cm;

    for(int i = __ID__, i <= mount, i++)
    {
      radio.openWritingPipe(pipes[i]);

    unsigned long time = millis();
    printf("Now sending %u...",cm);
    bool ok = radio.write( &mydata, sizeof(mydata) );

    if (ok)
      printf("ok...");
    else
      printf("failed.\n\r");


    radio.startListening();

    unsigned long started_waiting_at = millis();
    bool timeout = false;
    while ( ! radio.available() && ! timeout )
      if (millis() - started_waiting_at > 200 )
        timeout = true;

    if ( timeout )
    {
      printf("Failed, response timed out.\n\r");
    }
    else
    {
      int got_time;
      radio.read( &mydata[2], sizeof(int) );
      printf("Got response %u, round-trip delay: %u\n\r",mydata[2],mydata[2]);
    }
    }
}


delay(1000);




if ( role == role_pong_back )
{
    if ( radio.available() )
    {
      int got_time;
      bool done = false;
      while (!done)
      {
        done = radio.read( &hisdata, sizeof(hisdata) );
        printf("Got payload %u...",hisdata[2]);
        delay(20);
      }

      radio.stopListening();


      radio.write( &hisdata[2], sizeof(int) );
      printf("Sent response.\n\r");


      radio.startListening();
    }
}

  if ( Serial.available() )
  {
  }
}