// Alexandre Kaspar <akaspar@mit.edu>

#include "Arduino.h"
#include "RF24.h"
#include "serial.h"
// #include "util.h"

// radio number (two different roles)
#ifndef RADIO_NUM
#define RADIO_NUM 0
#endif

// ATMEL ATTINY84 / ARDUINO
//
//                           +-\/-+
//                     VCC  1|    |14  GND
//     SCK <-> (D 10)  PB0  2|    |13  AREF (D  0) PA <-> MISO
//    MOSI <-> (D  9)  PB1  3|    |12  PA1  (D  1) 
//                     PB3  4|    |11  PA2  (D  2) 
//  PWM  INT0  (D  8)  PB2  5|    |10  PA3  (D  3) 
//  PWM CE <-> (D  7)  PA7  6|    |9   PA4  (D  4) 
//  PWM CS <-> (D  6)  PA6  7|    |8   PA5  (D  5)        PWM
//                           +----+

/* Hardware configuration: Set up nRF24L01 radio on SoftSPI bus plus pins CE=8 and CS=7 */

//
// main
//
int main(void) {

    RF24 radio(8, 7);

    const unsigned char addresses[][6] = {"1Node","2Node"};

    //
    // set clock divider to /1
    //
    CLKPR = (1 << CLKPCE);
    CLKPR = (0 << CLKPS3) | (0 << CLKPS2) | (0 << CLKPS1) | (0 << CLKPS0);

    serial_begin();
    
    init(); // from wiring.c to enable interrupts and clock timing
    
    serial_write("RF24 / Radio ");
    serial_putd(RADIO_NUM);
    serial_write("\r\n");
    
    // SETUP
    while(!radio.begin()){
        serial_writeln("RF24 INI ERR");
    }
            
    // 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
    radio.openWritingPipe(addresses[RADIO_NUM]);
    radio.openReadingPipe(1, addresses[1 - RADIO_NUM]);
    
    // Start the radio listening for data
    radio.startListening();
    
    typedef unsigned long payload_t;
    
    // LOOP
    while(1){
            // serial_writeln("loop");
#if RADIO_NUM == 1
            radio.stopListening(); // stop listening so that we can talk
            serial_writeln("Sending");
            
            payload_t start_time = micros();                             // Take the time, and send it.  This will block until complete
            if (!radio.write( &start_time, sizeof(payload_t) )){
                serial_writeln("failed");
                // serial_write("F");
            }
            
            radio.startListening();
            
            payload_t started_waiting_at = micros();               // Set up a timeout period, get the current microseconds
            bool 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 > 200 ){            // If waited longer than 200ms, indicate timeout and exit while loop
                  timeout = true;
                  break;
              }      
            }
                
            if ( timeout ){                                             // Describe the results
                serial_writeln("Failed, response timed out.");
            }else{
                payload_t got_time;                                 // Grab the response, compare, and send to debugging spew
                radio.read( &got_time, sizeof(payload_t) );
                payload_t end_time = micros();
                
                // Spew it
                serial_writeln("=");
                serial_putd(start_time);
                serial_write(" Got response ");
                serial_putd(got_time);
                serial_write(" in ");
                serial_putd(end_time-start_time);
                serial_writeln(" us");
            }

            // Try again 1s later
            _delay_ms(1000);
#else
            payload_t got_time;
            
            if( radio.available()){
                                                                            // Variable for the received timestamp
              while (radio.available()) {                                   // While there is data ready
                radio.read( &got_time, sizeof(payload_t) );             // Get the payload
              }
             
              radio.stopListening();                                        // First, stop listening so we can talk   
              radio.write( &got_time, sizeof(payload_t) );              // Send the final one back.      
              radio.startListening();                                       // Now, resume listening so we catch the next packets.     
              serial_write("Sent response ");
              serial_putd(got_time);
              serial_write("\r\n");
           }
        
#endif   
        // potentially switch role given input
        // TODO implement that reading in a non-blocking way
    }
    
    return 0;
}