#include <SPI.h>
#include <nRF24L01.h>             //Downlaod it here: https://www.electronoobs.com/eng_arduino_NRF24.php
#include <RF24.h>

#include <SPI.h>
#include <Wire.h>
#include <Adafruit_GFX.h>         //download here: https://www.electronoobs.com/eng_arduino_Adafruit_GFX.php
#include <Adafruit_SSD1306.h>     //downlaod here: https://www.electronoobs.com/eng_arduino_Adafruit_SSD1306.php
#define OLED_RESET 6
Adafruit_SSD1306 display(OLED_RESET);

const uint64_t pipeIn = 0xE8E8F0F0E1LL; //Remember that this code is the same as in the transmitter
RF24 radio(9, 10);                      //CE and CSN pins (Arduino NANO)

int max_received = 4096;
int prev_throttle;
int prev_yaw;
int prev_roll;
int prev_pitch;

//We could use up to 32 channels
struct MyData {
  long throttle; 
  long yaw; 
  long pitch;  
  long roll; 
  bool AUX1;            //This is a digital channel (1000 or 2000), so we don't need precision
  bool AUX2;
};

MyData data;

void resetData()
{
//We define the inicial value of each data input
//3 potenciometers will be in the middle position so 127 is the middle from 254
  data.throttle = 0;  
  data.yaw = 2048;  
  data.pitch = 2048;  
  data.roll = 2048;  
  data.AUX1 = 0;
  data.AUX2 = 0;

}


void setup()
{
  pinMode(5,OUTPUT);
display.begin(SSD1306_SWITCHCAPVCC, 0x3C);  // initialize with the I2C addr 0x3C (for the 128x32 or 64 from eBay)
delay(100);
display.clearDisplay();
display.setTextSize(2);   
display.setTextColor(WHITE);
display.display();
delay(100);
  
Serial.begin(9600); //Set the speed to 9600 bauds if you want.
//You should always have the same speed selected in the serial monitor
resetData();
radio.begin();

radio.setAutoAck(false);                    // Ensure autoACK is enabled
radio.setChannel(0x2F);
radio.setDataRate(RF24_2MBPS);

radio.openReadingPipe(1,pipeIn);
//we start the radio comunication
radio.startListening();
}

/**************************************************/

unsigned long lastRecvTime = 0;

void recvData()
{
  while ( radio.available() ) {
    radio.read(&data, sizeof(MyData));  
    lastRecvTime = millis(); //here we receive the data
  }
}

/**************************************************/

void loop()
{
recvData();
unsigned long now = millis();
//Here we check if we've lost signal, if we did we reset the values 
if ( now - lastRecvTime > 1000 ) {
// Signal lost?
resetData();
}

int throttle = data.throttle;
int yaw = data.yaw;
int roll = data.roll;
int pitch = data.pitch;
bool aux_1 = data.AUX1;
bool aux_2 = data.AUX2;

if(throttle > max_received){
  throttle = prev_throttle;
}

if(yaw > max_received){
  yaw = prev_yaw;
}

if(roll > max_received){
  roll = prev_roll;
}

if(pitch > max_received){
  pitch = prev_pitch;
}

Serial.print("t");
Serial.print(throttle);   Serial.print("     y "); 
Serial.print(yaw);         Serial.print("     r "); 
Serial.print(roll);         Serial.print("     p "); 
Serial.print(pitch);      Serial.print("     A1 "); 
Serial.print(aux_1);      Serial.print("     A2 "); 
Serial.println(aux_2);             
delay(1);


if(pitch>0 && roll>0){
  int val = map(pitch,0,4096,255,0);
  analogWrite(5,val);
  
  display.clearDisplay();
  display.setCursor(0,0);
  display.print("THR: "); 
  display.println(4096-pitch);  

  display.print("YAW: "); 
  display.println(4096-roll);  
  display.display();//Finally display the created image
}



prev_throttle = throttle;
prev_yaw = yaw;
prev_roll = roll;
prev_pitch = pitch;
}
/**************************************************/