/*
 * See documentation at https://nRF24.github.io/RF24
 * See License information at root directory of this library
 * Author: Brendan Doherty (2bndy5)
 */

/**
 * A simple example of sending data from 1 nRF24L01 transceiver to another.
 *
 * This example was written to be used on 2 devices acting as "nodes".
 * Use the Serial Terminal to change each node's behavior.
 */
#include "pico/stdlib.h"  // printf(), sleep_ms(), getchar_timeout_us(), to_us_since_boot(), get_absolute_time()
#include "pico/bootrom.h" // reset_usb_boot()
#include <RF24.h>         // RF24 radio object
#include "nRF24L01.h"

// instantiate an object for the nRF24L01 transceiver
RF24 radio(2, 1);

// Used to control whether this node is sending or receiving
bool transmit = false; // true = TX role, false = RX role

// For this example, we'll be using a payload containing
// a single float number that will be incremented
// on every successful transmission
float payload = 5.6;

char getCommand(char* msg, char* validCmd){
  char input[2];
  while(true){
    printf(msg);
    input[0] = getchar();
    input[1] = getchar();
    if(input[1] == '\n'){
      putchar(input[0]);
      putchar('\n');
      for(int i=strlen(validCmd)-1; i >= 0; i--){
        if(input[0] == validCmd[i]){
          return input[0];
        }
      }
      printf("invalid input\n");
      return getCommand(msg, validCmd);
    }
    while(input[0] != '\n'){
      input[0] = getchar();
      putchar(input[0]);
    }
  }
}

bool setup()
{
    // Let these addresses be used for the pair
    uint8_t address[5][6] = {"0Node", "1Node"};
    // It is very helpful to think of an address as a path instead of as
    // an identifying device destination

    // to use different addresses on a pair of radios, we need a variable to
    // uniquely identify which address this radio will use to transmit
    bool radioNumber = 1; // 0 uses address[0] to transmit, 1 uses address[1] to transmit

    spi.begin(spi0, 6, 7, 4); // spi0 or spi1 bus, SCK, MOSI, MISO
    sleep_ms(1000);
    // initialize the transceiver on the SPI bus
    if (!radio.begin(&spi)) {
        printf("radio hardware is not responding!!\n");
        return false;
    }

    printf("radio transiver v0.0.1\n");

    // To set the radioNumber via the Serial terminal on startup
    char input = getCommand("Which radio is this? [0, 1]# ", "01");
    radioNumber = input == '1';
    printf("radioNumber = %d\n", (int)radioNumber);

    // Set the PA Level low to try preventing power supply related problems
    // because these examples are likely run with nodes in close proximity to
    // each other.
    radio.setPALevel(RF24_PA_MIN); // RF24_PA_MAX is default.

    // save on transmission time by setting the radio to only transmit the
    // number of bytes we need to transmit a float
    radio.setPayloadSize(sizeof(payload)); // float datatype occupies 4 bytes

    // set the TX address of the RX node into the TX pipe
    radio.openWritingPipe(address[radioNumber]); // always uses pipe 0

    // set the RX address of the TX node into a RX pipe
    radio.openReadingPipe(1, address[!radioNumber]); // using pipe 1

    // additional setup specific to the node's role
    if (transmit) {
        radio.stopListening(); // put radio in TX mode
        printf("*** PRESS 'R' to begin reciveing from the other node\n");
    }
    else {
        radio.startListening(); // put radio in RX mode
        printf("*** PRESS 'T' to begin transmitting to the other node\n");
    }

    // For debugging info
    // radio.printDetails();       // (smaller) function that prints raw register values
    // radio.printPrettyDetails(); // (larger) function that prints human readable data

    return true;
} // setup

void loop()
{
    if (transmit) {
        // This device is a TX node

        uint64_t start_timer = to_us_since_boot(get_absolute_time()); // start the timer

        bool report = true;
        //Start Writing
        radio.startFastWrite(&payload, sizeof(payload), false);

        //Wait until complete or failed
        uint32_t timer = millis();
        while (!(radio.get_status() & (_BV(TX_DS) | _BV(MAX_RT)))) {
          if (millis() - timer > 95) {
            printf("transmission problebly faild. It took more then 95 ms\n");
            report = false;
            break;
          }
        }

        radio.ce(LOW);
        radio.write_register(NRF_STATUS, _BV(RX_DR) | _BV(TX_DS) | _BV(MAX_RT));

        if (radio.status & _BV(MAX_RT)){
          radio.flush_tx(); // Only going to be 1 packet in the FIFO at a time using this method, so just flush
          printf("transmission failed!! :( reached max retries\n");
          report = false;
        }

        // bool report = radio.write(&payload, sizeof(payload));         // transmit & save the report
        uint64_t end_timer = to_us_since_boot(get_absolute_time());   // end the timer

        if (report) {
            // payload was delivered; print the payload sent & the timer result
            printf("Transmission successful! Time to transmit = %llu us. Sent: %f\n", end_timer - start_timer, payload);

            // increment float payload
            payload += 0.01;
        }
        // else {
        //     // payload was not delivered
        //     printf("Transmission failed or timed out\n");
        // }

        // to make this example readable in the serial terminal
        sleep_ms(1000); // slow transmissions down by 1 second
    } else {
        // This device is a RX node

        uint8_t pipe;
        if (radio.available(&pipe)) {               // is there a payload? get the pipe number that recieved it
            uint8_t bytes = radio.getPayloadSize(); // get the size of the payload
            radio.read(&payload, bytes);            // fetch payload from FIFO

            // print the size of the payload, the pipe number, payload's value
            printf("Received %d bytes on pipe %d: %f\n", bytes, pipe, payload);
        }
    } // end if (transmit)

    char input = getchar_timeout_us(0); // get char from buffer for user input
    if (input != PICO_ERROR_TIMEOUT) {
        // change the role via the serial terminal

        if ((input == 'T' || input == 't') && !transmit) {
            // Become the TX node

            transmit = true;
            printf("*** CHANGING TO TRANSMIT ROLE -- PRESS 'R' TO SWITCH BACK\n");
            radio.stopListening();
        }
        else if ((input == 'R' || input == 'r') && transmit) {
            // Become the RX node

            transmit = false;
            printf("*** CHANGING TO RECEIVE ROLE -- PRESS 'T' TO SWITCH BACK\n");
            radio.startListening();
        }
        else if (input == 'b' || input == 'B') {
            // reset to bootloader
            radio.powerDown();
            reset_usb_boot(0, 0);
        }
        else if (input == 'i' || input == 'I'){
          radio.printPrettyDetails();
        }
    }
} // loop

int main()
{
    stdio_init_all(); // init necessary IO for the RP2040
    sleep_ms(5000);
    printf("Radio thingy\n");

    while (!setup()) { // if radio.begin() failed
        // hold program in infinite attempts to initialize radio
    }
    while (true) {
        loop();
    }
    return 0; // we will never reach this
}