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

int echoPin = 4;
int trigPin = 3;

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

RF24 radio(9, 10);

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;

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

uint64_t getPipe(int id = __ID__)
{
    return pipes[id];
}

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

    radio.openReadingPipe(1, getPipe());

    radio.begin();
    radio.setRetries(15, 15);
    radio.startListening();
    radio.printDetails();
}

int cm1 = 0;

void loop(void)
{
    if (Serial.available()) {
        radio.openWritingPipe(getPipe(0));
    }

    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 = 0; i < mount; i++) {
            if (i != __ID__) {
              radio.openWritingPipe(getPipe(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();
        }
    }
}