////////////////////////////////////////////////////////////////////////////////////////////////////
//ANDRUINO LIBRARY
//A.Scavuzzo
//www.andruino.it
////////////////////////////////////////////////////////////////////////////////////////////////////

#if NRF24L_ENABLE == 1

#include <RF24Network.h>
#include <RF24.h>

//PIN USED FOR NRF CE and CS (you can move as you want)
//#define PIN_NRF_CE 49
//#define PIN_NRF_CS 48



RF24 radio(PIN_NRF_CE, PIN_NRF_CS);                                   // CE & CS pins to use (Using 7,8 on Uno,Nano)
RF24Network network(radio);


const short max_active_nodes = NRF24LMaxModules;                    // Array of nodes we are aware of
//uint16_t active_nodes[max_active_nodes];
short num_active_nodes = 0;
short next_ping_node_index = 0;

//functions
bool nrf_send_T(uint16_t to);                              // Prototypes for functions to send & handle messages
bool nrf_send_N(uint16_t to);
bool nrf_sendCommand (uint16_t to, unsigned char cmd, uint8_t *states, uint8_t num);
void nrf_handle_T(RF24NetworkHeader& header);
void nrf_handle_N(RF24NetworkHeader& header);
void nrf_handle_SYSTEM(RF24NetworkHeader& header, short index_node);
void nrf_handle_ANALOG(RF24NetworkHeader& header, short index_node, byte mode);
void nrf_handle_DIGITAL(RF24NetworkHeader& header, short index_node, byte mode);
void nrf_handle_VARIABLE(RF24NetworkHeader& header, short index_node, byte mode);
//void send_active_nodes(RF24NetworkHeader& header);
short nrf_search_node(uint16_t node);
short nrf_add_node(uint16_t node);
short nrf_search_nodeFlash(uint16_t node);
short nrf_add_nodeFlash(uint16_t node);
void nrf_PrintVector (uint16_t *states, int i);
void nrf_PrintVector (uint8_t *states, int i);
void nrf_PrintVector (float *states, int i);
void nrf_copyArray (uint8_t *source, uint8_t *destination, uint8_t start_byte, uint8_t num_byte);
short nrf_search_nodeFlash(uint16_t node);
short nrf_add_nodeFlash(uint16_t node);
void nrf_receive_nrf();
void nrf_loop();

//16 bits system_data
//system_data[0]=vcc
//system_data[1]=counter
//system_data[2]=default sleep interval
//system_data[3]=firmware version
//system_data[4]=PA level
//system_data[5]=router(0), leaf(1)
//
uint16_t system_data[] = {1, 0, 8, 255, 255, 255};
const uint8_t max_num_system_data = 6;



// Digital input Pins

const uint8_t max_num_digital_pins = NRF24LMaxDigitalPin;
//uint8_t digital_pins[max_num_digital_pins];                                    // {2, 3, 4, 5, 9};
//uint8_t digital_states[max_num_digital_pins];                                  // pin values
//bool digital_pins_mode[max_num_digital_pins];                                  //{INPUT, INPUT, INPUT, INPUT, OUTPUT};


// Analog input Pins
const uint8_t num_analog_pins = NRF24LMaxAnalogPin;
//uint8_t analog_pins[num_analog_pins];                         //{2, 3};
//uint16_t analog_states[num_analog_pins];


// Variables input
float variable_states[NRF24LMaxVariable];


bool nrf_setup_nrf() {

  bool status_nrf = 0;

  //  SPI.begin();                                           // Bring up the RF network
  status_nrf = radio.begin();
  radio.setPALevel(RF24_PA_HIGH);
  //radio.setDataRate(RF24_250KBPS);      //speed  RF24_250KBPS for 250kbs, RF24_1MBPS for 1Mbps, or RF24_2MBPS for 2Mbps
  //already in begin radio.setRetries(15, 15);               //How long to wait between each retry, in multiples of 250us, max is 15. 0 means 250us, 15 means 4000us.

  //  network.begin(/*channel*/ NRF_CHANNEL_NUM, /*node address*/ 00 );



  network.choose_pipe_address(nrf_pipeline_id);
  network.begin(/*channel*/ NRF_CHANNEL_NUM, /*node address*/ 00 );

  radio.startListening();
  radio.printDetails();            // Dump the configuration of the rf unit for debugging
  //IF_SERIAL_DEBUG_NRF(radio.printDetails());            // Dump the configuration of the rf unit for debugging



  //if the signature 0x73 is not present means that the flash storage is not used by NRF and so I will erase it
  if (eeprom_read_byteNEW (FLA_NRF_NODE_ADDRESS_OK) != 0x73) {
    //erase all the NRF data index
    for (int i = FLA_NRF_NODE_ADDRESS; i < (FLA_NRF_NODE_ADDRESS + NRF24LMaxModules * 2); i++) {
      eeprom_write_byteNEW (i, 255);
    }
    eeprom_write_byteNEW ((uint8_t *)(FLA_NRF_NODE_ADDRESS_OK), 0x73);
    IF_SERIAL_DEBUG_NRF(printf_P(PSTR("NRF flash storage has been erased\r\n")));
  }

  return status_nrf;

}

void powerdown_nrf() {
  radio.stopListening();
  radio.powerDown();
}


void nrf_loop() {
  if (nrf_radio_enable_rq) {
    Serial.println(F("\r\nNRF enabling"));
    if (!nrf_setup_nrf()) {
      Serial.println(F("ERROR, NRF module doesn't respond"));
      nrf_radio_enable = false;
      nrf_radio_enable_rq = false;
      nrf_radio_error_setup = true;
    }
    else {
      Serial.println(F("\r\nNRF is alive"));
      nrf_radio_error_setup = false;
      nrf_radio_enable = true;
      nrf_radio_enable_rq = false;
    }
  } else if (nrf_radio_disable_rq) {
    Serial.println(F("\r\nNRF disabling"));
    ///    powerdown_nrf();                                       //SI BLOCCA
    nrf_radio_enable = false;
    nrf_radio_disable_rq = false;
    nrf_radio_error_setup = false;
  }
  if (nrf_radio_enable)
    nrf_receive_nrf();
}




void nrf_receive_nrf() {


  network.update();                                      // Pump the network regularly

  while ( network.available() )  {                      // Is there anything ready for us?

    RF24NetworkHeader header;                            // If so, take a look at it
    network.peek(header);

    IF_SERIAL_DEBUG_NRF(printf_P(PSTR("---------------------------------\r\n")));
    IF_SERIAL_DEBUG_NRF(printf_P(PSTR("----RECEIVED--FROM--NODE: 0%o\r\n"), header.from_node));
    IF_SERIAL_DEBUG_NRF(printf_P(PSTR("---------------------------------\r\n")));


    //check if the received node is already inthe table (index_node>=0) or is not present (index_node = -1)
    short index_node =  nrf_search_nodeFlash(header.from_node);


    switch (header.type) {                             // Dispatch the message to the correct handler.
      //T and N has to be at the beginning, don't move them!
      case 'T': nrf_handle_T(header); break;
      case 'N': nrf_handle_N(header); break;
      case 'Y': nrf_handle_SYSTEM(header, index_node); break;
      case 'D': nrf_handle_DIGITAL(header, index_node, 0); break;  //DIGITAL PIN status
      case 'A': nrf_handle_ANALOG(header, index_node, 0); break;   //ANALOG PIN status

      case 'a': nrf_handle_ANALOG(header, index_node, 1); break;   //ANALOG PIN position
      case '1': nrf_handle_DIGITAL(header, index_node, 1); break;  //DIGITAL PIN mode (INPUT, OUTPUT)
      case 'd': nrf_handle_DIGITAL(header, index_node, 2); break;  //DIGITAL PIN position

      case 'V': nrf_handle_VARIABLE(header, index_node, 0); break;  //VARIABLES PIN status

      //     case 'R': send_active_nodes(header); break;              //send active node


      case 'S': /*This is a sleep payload, do nothing*/ break;

      default:
        IF_SERIAL_DEBUG_NRF(printf_P(PSTR("*** WARNING *** Unknown message type %c\r\n"), header.type));
        network.read(header, 0, 0);
        break;
    };
  }
}


bool nrf_sendCommand (uint16_t to, unsigned char cmd, uint8_t *states, uint8_t num) {
  uint8_t tx_buffer[50];
  radio.powerUp();
  tx_buffer[0] = cmd;
  tx_buffer[1] = num;
  nrf_copyArray(states, tx_buffer + 2, 0, num);

  delay(100);

  RF24NetworkHeader header(/*to node*/ to, /*type*/ cmd /*Time*/);

  IF_SERIAL_DEBUG_NRF(printf_P(PSTR("---------------------------------\r\n")));
  IF_SERIAL_DEBUG_NRF(printf_P(PSTR("SENSOR is sending to node 0%o...\r\n"), to));

  bool ok = network.write(header, tx_buffer, num + 2);

  IF_SERIAL_DEBUG_NRF(nrf_PrintVector(tx_buffer, num + 2));

#if DEBUG_SERIAL_NRF24L == 1
  if (ok)
    printf("Command ok\r\n");
  else
    printf("Command failed\r\n");
  // radio.powerDown();
#endif




  return (ok);
}



/**
   Send a 'T' message, the current time
*/
bool nrf_send_T(uint16_t to)
{
  RF24NetworkHeader header(/*to node*/ to, /*type*/ 'T' /*Time*/);

  // The 'T' message that we send is just a ulong, containing the time
  unsigned long message = millis();
  IF_SERIAL_DEBUG_NRF(printf_P(PSTR("---------------------------------\r\n")));
  IF_SERIAL_DEBUG_NRF(printf_P(PSTR("%lu: APP Sending %lu to 0%o...\r\n"), millis(), message, to));
  return network.write(header, &message, sizeof(unsigned long));
}




void nrf_handle_SYSTEM(RF24NetworkHeader& header, short index_node) {
  uint8_t Ndata_received;
  uint8_t command_rx[34];
  uint16_t system_data[max_num_system_data] = {1, 0, 0, 300, 0, 255};


  if (header.from_node == 00)                      //nothing if base
    return;
  if (index_node == -1)
    index_node = nrf_add_nodeFlash(header.from_node);                   //if index = -1, then add the node in the table and extract the new index

  network.read(header, command_rx, 32);
  // printf_P(PSTR("BASE has received SYSTEM INFO from 0%o\r\n"), header.from_node);


  //command_rx[1]  -- number of received data
  Ndata_received = command_rx[1];


  if ((Ndata_received / 2) < sizeof(system_data))  {  //base

    nrf_copyArray(command_rx, (uint8_t *)system_data, 2, Ndata_received);

    IF_SERIAL_DEBUG_NRF(printf_P(PSTR("SYSTEM COMMAND DECODED, node: 0%o, index: %d, data:"), header.from_node, index_node));
    IF_SERIAL_DEBUG_NRF(nrf_PrintVector(system_data, Ndata_received / 2));


    //send back SYSTEM to sensors
    if (SystemNRF24LPins[index_node].system_back_wr) {
      system_data[2] = SystemNRF24LPins[index_node].sleep_time_value;
      bool ok = nrf_sendCommand(header.from_node, 'Y', (uint8_t *)system_data, Ndata_received);
      if (ok) {
        SystemNRF24LPins[index_node].system_back_wr = false;             //no more write request
        IF_SERIAL_DEBUG_NRF(printf_P(PSTR("System written on remote sensor successfully\r\n")));
      }
    } else {

      //////////////////////
      //UPDATE: if no write request
      //REMOTE SUPPLY
      //REMOTE ADC STEP
      //REMOTE NRF ADDRESS
      //////////////////////
      SystemNRF24LPins[index_node].Supply = system_data[0];                     //int16  supply *1000
      SystemNRF24LPins[index_node].AdcStep = (((float)system_data[0]) / 1000) / 1024; //float  adc step
      SystemNRF24LPins[index_node].RNF24LAddr = header.from_node;
      SystemNRF24LPins[index_node].samples = system_data[1];
      SystemNRF24LPins[index_node].sleep_time_value = system_data[2];
      SystemNRF24LPins[index_node].fw_version = system_data[3];
      SystemNRF24LPins[index_node].pa_level = system_data[4];
      SystemNRF24LPins[index_node].leaf = system_data[5];
      SystemNRF24LPins[index_node].alive_counter = system_data[2] * 4;    //copy the sleep time * 4. This value will be decremented by one. If it will reach zero, means that the sensor doesn't respond
    }

#if DEBUG_SERIAL_NRF24L == 1
    Serial.print(F("Supply: ")); Serial.print(SystemNRF24LPins[index_node].Supply); Serial.println(F("V"));
    Serial.print(F("AdcStep: ")); Serial.print(SystemNRF24LPins[index_node].AdcStep, 6); Serial.println(F("V"));
    printf_P(PSTR("RNF24LAddr: 0%o\r\n"), SystemNRF24LPins[index_node].RNF24LAddr);
    printf_P(PSTR("samples: %d\r\n"), SystemNRF24LPins[index_node].samples);
    printf_P(PSTR("pa level: %d\r\n"), SystemNRF24LPins[index_node].pa_level);
    printf_P(PSTR("alive_counter: %d\r\n"), SystemNRF24LPins[index_node].alive_counter);
    printf_P(PSTR("leaf: %d\r\n"), SystemNRF24LPins[index_node].leaf);
    printf_P(PSTR("firmware version: %d\r\n"), SystemNRF24LPins[index_node].fw_version);
#endif

  }


}




void nrf_handle_DIGITAL(RF24NetworkHeader& header, short index_node, byte mode) {

  uint8_t command_rx[34];
  uint8_t j, Ndata_received;
  uint8_t digital_pins[max_num_digital_pins];                                    // {2, 3, 4, 5, 9};
  uint8_t digital_states[max_num_digital_pins];                                  // pin values
  bool digital_pins_mode[max_num_digital_pins];                                  //{INPUT, INPUT, INPUT, INPUT, OUTPUT};
  // The 'T' message is just a ulong, containing the time
  bool transmit_OutPin = false;

  IF_SERIAL_DEBUG_NRF(printf_P(PSTR("PRE index: %d, num_active_nodes:%d\r\n"), index_node, num_active_nodes));
  if (header.from_node == 00)                      //nothing if base
    return;
  if (index_node == -1)
    index_node = nrf_add_nodeFlash(header.from_node);                   //if index = -1, then add the node in the table and extract the new index
  IF_SERIAL_DEBUG_NRF(printf_P(PSTR("AFTER index: %d, num_active_nodes:%d\r\n"), index_node, num_active_nodes));




  network.read(header, command_rx, 32);


  //command_rx[1]  -- number of received data
  Ndata_received = command_rx[1];

  SystemNRF24LPins[index_node].dig_num = Ndata_received;

  // pin values
  if (mode == 0) {
    nrf_copyArray(command_rx, digital_states, 2, Ndata_received);

    IF_SERIAL_DEBUG_NRF(printf_P(PSTR("DIGITAL PIN STATE, node: 0%o, index: %d, data:"), header.from_node, index_node));
    IF_SERIAL_DEBUG_NRF(nrf_PrintVector(digital_states, Ndata_received));


    //update base vectors according to remote sensor data
    //update also output on remote sensor
    for (j = 0; j < Ndata_received; j++)  {
      //only if is OUTPUT and W=true (write request)
      if (SystemNRF24LPins[index_node].DigPin[j].W == true && SystemNRF24LPins[index_node].DigPin[j].mode == OUTPUT) {
        IF_SERIAL_DEBUG_NRF(printf_P(PSTR("DIGITAL OUT PIN: %d, has to be updated\r\n"), SystemNRF24LPins[index_node].DigPin[j].pin));
        digital_states[j] = SystemNRF24LPins[index_node].DigPin[j].Wstate;                                               //update the vector with the new data to be updated on sensor node
        SystemNRF24LPins[index_node].DigPin[j].state = digital_states[j];         //used to update the output immediately
        transmit_OutPin = true;
      }
      //update only input pin or the output when no write operation has been done
      else {
        SystemNRF24LPins[index_node].DigPin[j].state = digital_states[j];
      }
    }


    //transmit back OUTPUT pin values if the user has to change the values
    if (transmit_OutPin) {
      IF_SERIAL_DEBUG_NRF(printf_P(PSTR("-------------------- DIGITAL OUT PIN sending back to remote sensor(D)\r\n")));
      bool ok = nrf_sendCommand(header.from_node, 'D', digital_states, Ndata_received);
      //      if (ok) {
      IF_SERIAL_DEBUG_NRF(printf_P(PSTR("DIGITAL OUT PIN written on remote sensor successfully\r\n")));
      //clearing all the w flag because the command has been transimmted correctly
      for (j = 0; j < Ndata_received; j++)
        SystemNRF24LPins[index_node].DigPin[j].W = false;
      //      }
    }


    check_limits_NRF = true;        //allow limits check for NRF when the new data are loaded

  }

  //read the pin position for each field
  //{2, 3, 4, 5, 9};
  else if (mode == 2) {
    nrf_copyArray(command_rx, digital_pins, 2, Ndata_received);

    IF_SERIAL_DEBUG_NRF(printf_P(PSTR("DIGITAL PIN POSITION, node: 0%o, index: %d, data:"), header.from_node, index_node));
    IF_SERIAL_DEBUG_NRF(nrf_PrintVector(digital_pins, Ndata_received));

    for (j = 0; j < Ndata_received; j++)  {
      SystemNRF24LPins[index_node].DigPin[j].pin = digital_pins[j];
      SystemNRF24LPins[index_node].DigPin[j].used = true;
    }

  }
  //{INPUT, INPUT, INPUT, INPUT, OUTPUT};
  else if (mode == 1) {
    nrf_copyArray(command_rx, (uint8_t *)digital_pins_mode, 2, Ndata_received);

    IF_SERIAL_DEBUG_NRF(printf_P(PSTR("DIGITAL PIN MODE, node: 0%o, index: %d, data:"), header.from_node, index_node));
    IF_SERIAL_DEBUG_NRF(nrf_PrintVector((uint8_t *)digital_pins_mode, Ndata_received));

    for (j = 0; j < Ndata_received; j++)
      SystemNRF24LPins[index_node].DigPin[j].mode = digital_pins_mode[j];
  }

}

//nrf_send_digital_out(SystemNRF24LPins[indexModule].RNF24LAddr, OUTPUT, SystemNRF24LPins[indexModule].DigPin[j].pin, value, SystemNRF24LPins[indexModule].DigPin[j].inv);
bool nrf_send_digital_out(uint16_t tx_to_node, byte mode, byte channel, byte new_state, short inv, uint16_t duration) {

  uint8_t digital_vector[7];

  digital_vector[0] = 3;
  digital_vector[1] = mode;
  digital_vector[2] = channel;
  digital_vector[3] = new_state;

  //for new Node 1100

  if (inv)
    digital_vector[4] = 0xAA;         //pin inversion TRUE
  else if (inv == 0)
    digital_vector[4] = 0x55;        //pin inversion FALSE
  else
    digital_vector[4] = 0;           //no INV changes


  //Fix duration, compatible with node 1.21
  //Low
  digital_vector[5] = duration & 0xFF;   //pin duration low (8 bit low)
  //high
  digital_vector[6] = (duration >> 8) & 0xFF; //pin duration high (8 bit high)


  IF_SERIAL_DEBUG_NRF(printf_P(PSTR("-------------------- DIGITAL OUT PIN single write(Q)\r\n")));
  bool ok = nrf_sendCommand(tx_to_node, 'Q', digital_vector, sizeof(digital_vector));
  if (ok)
    IF_SERIAL_DEBUG_NRF(printf_P(PSTR("DIGITAL OUT PIN written on remote sensor successfully\r\n")));
  else
    IF_SERIAL_DEBUG_NRF(printf_P(PSTR("DIGITAL OUT PIN error\r\n")));

  return ok;
}

/*
  bool send_digital_vector(uint16_t tx_to_node, byte index_node, byte type, byte channel, byte new_state) {

  uint8_t digital_states[max_num_digital_pins];
  uint8_t Ndata_received;

  Ndata_received = SystemNRF24LPins[index_node].dig_num;

  //update base vectors according to remote sensor data
  //update also output on remote sensor
  for (uint8_t j = 0; j < Ndata_received; j++)  {
    //only if is OUTPUT and W=true (write request)
    if (SystemNRF24LPins[index_node].DigPin[j].W == true && SystemNRF24LPins[index_node].DigPin[j].mode == OUTPUT) {
      IF_SERIAL_DEBUG_NRF(printf_P(PSTR("DIGITAL OUT PIN: %d, has to be updated\r\n"), SystemNRF24LPins[index_node].DigPin[j].pin));
      digital_states[j] = SystemNRF24LPins[index_node].DigPin[j].Wstate;                                               //update the vector with the new data to be updated on sensor node
      SystemNRF24LPins[index_node].DigPin[j].state = digital_states[j];         //used to update the output immediately
    }
  }
  //transmit back OUTPUT pin values if the user has to change the values
  IF_SERIAL_DEBUG_NRF(printf_P(PSTR("-------------------- DIGITAL OUT PIN sending to remote sensor\r\n")));
  bool ok = nrf_sendCommand(tx_to_node, 'D', digital_states, Ndata_received);
  IF_SERIAL_DEBUG_NRF(printf_P(PSTR("DIGITAL OUT PIN written on remote sensor successfully\r\n")));
  //clearing all the w flag because the command has been transimmted correctly
  for (uint8_t j = 0; j < Ndata_received; j++)
    SystemNRF24LPins[index_node].DigPin[j].W = false;

  }
*/



void nrf_handle_ANALOG(RF24NetworkHeader& header, short index_node, byte mode) {

  uint8_t command_rx[34];
  uint8_t j, Ndata_received;
  uint8_t analog_pins[num_analog_pins];                         //{2, 3};
  uint16_t analog_states[num_analog_pins];


  if (header.from_node == 00)                      //nothing if base
    return;
  if (index_node == -1)
    index_node = nrf_add_nodeFlash(header.from_node);                   //if index = -1, then add the node in the table and extract the new index


  // The 'T' message is just a ulong, containing the time
  network.read(header, command_rx, 32);
  //  printf_P(PSTR("BASE has received ANALOG DATA from 0%o\r\n"), header.from_node);

  //command_rx[1]  -- number of received data (1 data = 8 bit)
  Ndata_received = command_rx[1];

  SystemNRF24LPins[index_node].ana_num = Ndata_received / 2;  //16 bits

  if (mode == 0) {
    nrf_copyArray(command_rx, (uint8_t *)analog_states, 2, Ndata_received);    //analog_states 16 bits

    IF_SERIAL_DEBUG_NRF(printf_P(PSTR("ANALOG VALUES, node: 0%o, index: %d, data:"), header.from_node, index_node));
    IF_SERIAL_DEBUG_NRF(nrf_PrintVector(analog_states, Ndata_received / 2));


#if DEBUG_SERIAL_NRF24L == 1
    float vcc_remote = ((float) SystemNRF24LPins[index_node].Supply) / 1000.0;
    Serial.print(F("Remote batt voltage: "));
    Serial.print(vcc_remote, 2);
    Serial.println(F("V"));
#endif

    float step_adc = SystemNRF24LPins[index_node].AdcStep;

    //update vectors
    for (j = 0; j < Ndata_received / 2; j++) {
      SystemNRF24LPins[index_node].AnaPin[j].state = analog_states[j];
      SystemNRF24LPins[index_node].AnaPin[j].value = (float)analog_states[j] * step_adc;
    }
    check_limits_NRF = true;        //allow limits check for NRF when the new data are loaded


  }
  else if (mode == 1) {
    nrf_copyArray(command_rx, analog_pins, 2, Ndata_received);    //analog_pins 8 bits

    IF_SERIAL_DEBUG_NRF(printf_P(PSTR("ANALOG PIN POSITION, node: 0%o, index: %d, data:"), header.from_node, index_node));
    IF_SERIAL_DEBUG_NRF(nrf_PrintVector(analog_pins, Ndata_received));


    SystemNRF24LPins[index_node].RNF24LAddr = header.from_node;
    for (j = 0; j < Ndata_received; j++)  {
      SystemNRF24LPins[index_node].AnaPin[j].pin = analog_pins[j];
      SystemNRF24LPins[index_node].AnaPin[j].used = true;
    }
  }
}

//One Variable is 32bit
void nrf_handle_VARIABLE(RF24NetworkHeader& header, short index_node, byte mode) {

  uint8_t command_rx[34];
  uint8_t j, Ndata_received;
  float variable_states[NRF24LMaxVariable];


  if (header.from_node == 00)                      //nothing if base
    return;
  if (index_node == -1)
    index_node = nrf_add_nodeFlash(header.from_node);                   //if index = -1, then add the node in the table and extract the new index

  // IF_SERIAL_DEBUG_NRF(printf_P(PSTR("Payload: %d \r\n"), radio.getPayloadSize()));

  network.read(header, command_rx, 32);
  Ndata_received = command_rx[1];


  //IF_SERIAL_DEBUG_NRF(nrf_PrintVector(command_rx, Ndata_received+2));


  SystemNRF24LPins[index_node].var_num = Ndata_received / 4;  //32 bits
  nrf_copyArray(command_rx, (uint8_t *)variable_states, 2, Ndata_received);    //analog_states 16 bits

  IF_SERIAL_DEBUG_NRF(printf_P(PSTR("VARIABLE VALUES, node: 0%o, index: %d, data:"), header.from_node, index_node));
  IF_SERIAL_DEBUG_NRF(nrf_PrintVector(variable_states, Ndata_received / 4));

  for (j = 0; j < Ndata_received / 4; j++)
    SystemNRF24LPins[index_node].VarPin[j].value = variable_states[j];


  check_limits_NRF = true;        //allow limits check for NRF when the new data are loaded
}

/**
   Handle a 'T' message
   Add the node to the list of active nodes
*/
void nrf_handle_T(RF24NetworkHeader& header) {

  unsigned long message;
  network.read(header, &message, sizeof(unsigned long));
  IF_SERIAL_DEBUG_NRF(printf_P(PSTR("nrf_handle_T: %lu from 0%o\r\n"), message, header.from_node));

  //Add if it is from a node (not base)
  if (header.from_node > 00 )
    nrf_add_nodeFlash(header.from_node);
}

/**
   Handle an 'N' message, the active node list
*/
void nrf_handle_N(RF24NetworkHeader& header)
{
  static uint16_t incoming_nodes[max_active_nodes];

  network.read(header, &incoming_nodes, sizeof(incoming_nodes));
  IF_SERIAL_DEBUG_NRF(printf_P(PSTR("nrf_handle_N: Received active nodes from 0%o\r\n"), header.from_node));

  int i = 0;
  while ( i < max_active_nodes && incoming_nodes[i] > 00 )
    nrf_add_nodeFlash(incoming_nodes[i++]);
}




//search the node in the flash list
//if it is not present, add it
//refresh num_active_nodes number var

short nrf_add_nodeFlash(uint16_t node) {
  short i = num_active_nodes;

  //is there the node in the list?
  while (i--)  {
    //check if it is already present
    if (eeprom_read_wordNEW ((FLA_NRF_NODE_ADDRESS + i * 2)) == node)
      break;
  }
  //the current node is not in the flash list -- store it
  if ( i == -1 && num_active_nodes < max_active_nodes ) {        // If not, add it to the table
    i = num_active_nodes;
    eeprom_write_wordNEW ((FLA_NRF_NODE_ADDRESS + i * 2), node); //write active node to flash
    num_active_nodes++;
    IF_SERIAL_DEBUG_NRF(printf_P(PSTR("Added 0%o to list of active nodes, index: %d, num_active_nodes:%d\r\n"), node, i, num_active_nodes));
  }
  return (i);     //return the index of the new node
}


//search the node in the whole flash list
//-1 if it is not present
//refresh num_active_nodes number var

//2 bytes for each node
short nrf_search_nodeFlash(uint16_t node) {

  IF_SERIAL_DEBUG_NRF(printf_P(PSTR("Search node 0%o, num_active_nodes: %d\r\n"), node, num_active_nodes));
  short i = NRF24LMaxModules;                                    // Do we already know about this node?
  bool found_max_node = false;
  uint16_t data;

  while (i--)  {
    data = eeprom_read_wordNEW ((FLA_NRF_NODE_ADDRESS + i * 2));
    IF_SERIAL_DEBUG_NRF(printf_P(PSTR("Module index:%d - node: 0%o\r\n"), i, data));
    //find the last element of the node list
    if (data != 65535  && found_max_node == false) {
      num_active_nodes = i + 1;
      found_max_node = true;
    }
    if (data == node) {
      IF_SERIAL_DEBUG_NRF(printf_P(PSTR("Node already in the flash list, node 0%o, index: %d\r\n"), node, i));
      break;
    }
  }
#if DEBUG_SERIAL_NRF24L == 1
  if (found_max_node)
    printf_P(PSTR("num_active_nodes:%d\r\n"), num_active_nodes);
#endif

  if ( i == -1 && num_active_nodes < max_active_nodes ) {        // If not, add it to the table
    IF_SERIAL_DEBUG_NRF(printf_P(PSTR("Node 0%o not found in the flash list\r\n"), node));
    return (i);  //not found
  }

  return (i);  //inded of node found
}



//nrf_copyArray(pippo, tano, 1, 8);
void nrf_copyArray (uint8_t *source, uint8_t *destination, uint8_t start_byte, uint8_t num_byte) {

  //(destination, source, Ndata)
  memcpy(destination, source + start_byte, num_byte );
  /*  for (int i = 0; i < num_byte; i++) {
      destination[i] = source[i + start_byte];
    }*/
}

void nrf_PrintVector (uint8_t *states, int i) {
  for (int k = 0; k < i; k++)
  {
    Serial.print(states[k]);
    if (k < i - 1) {
      Serial.print(F(", "));
    } else
      Serial.println();
  }
}
void nrf_PrintVector (uint16_t *states, int i) {
  for (int k = 0; k < i; k++)
  {
    Serial.print(states[k]);
    if (k < i - 1) {
      Serial.print(F(", "));
    } else
      Serial.println();
  }
}
void nrf_PrintVector (float *states, int i) {
  for (int k = 0; k < i; k++)
  {
    Serial.print(states[k], 2);
    if (k < i - 1) {
      Serial.print(F(", "));
    } else
      Serial.println();
  }
}

#endif
