#include <Servo.h>
#include <SPI.h>
#include <nRF24L01.h>
#include <RF24.h>

RF24 radio(9, 10); // CE, CSN
const byte address[6] = "00001";
//The following are the variables for the numbers we will receive
int xVal = 1000;
int xValDefault = 1000;
int yVal = 1400;
int yValDefault = 1400;
int switchVal = 1000;
int potVal = 1000;
int potValDefault = 1000;
byte joystick[4];
int ledPin = 5;
Servo rudder;         //Declaring the servo
Servo thrust;         //Declaring the servo
Servo hover;         //Declaring the servo
int counter = 0;


//Setup initialises everything we need to start receiving (serial is not necessary, but helps debugging)
void setup() 
{
  pinMode(ledPin, OUTPUT);
  thrust.attach(2);
  rudder.attach(3);
  hover.attach(4);
  Serial.begin(57600);
  radio.begin();
  radio.openReadingPipe(0, address);   //Setting the address at which we will receive the data
  radio.setPALevel(RF24_PA_MAX);       //You can set this as minimum or maximum depending on the distance between the transmitter and receiver.
  radio.setDataRate(RF24_250KBPS);  //Need to check this out!!! (also 1MBPS or 2MBPS)
  radio.startListening();              //This sets the module as receiver
}

void loop()
{
  //First, check if anything to receive!
  if (radio.available())              //Looking for the data.
  {
    digitalWrite(ledPin, LOW);
    counter = 0;
    //Read in data (our array)
    radio.read(&joystick, sizeof(joystick));    //Reading the data
    xVal = map(joystick[0],0,255,1000,2000);
    byte temp = 255-joystick[1];
    yVal = map(temp,0,255,0,3000);
    switchVal = map(joystick[2],0,1,1000,2000);
    potVal = map(joystick[3],0,255,1000,2000);
    //Output array to serial for checking!
    Serial.print("SIGNAL OK! ");
    Serial.print(" xVal = ");
    Serial.print(xVal);
    Serial.print(" , y Val =");
    Serial.print(yVal);
    Serial.print(" , switchVal = ");
    Serial.print(switchVal);
    Serial.print(" , potVal = ");
    Serial.println(potVal);
    //Allocate data from array to individual variable
    //This isn't strictly necessary as we could display variables by reference to the array 
    //eg: instead if i, we would reference joystick[0].
    thrust.writeMicroseconds(xVal);         //Value written out to the servo
    rudder.writeMicroseconds(yVal);         //Value written out to the servo
    hover.writeMicroseconds(potVal);         //Value written out to the servo
    }
    else
    {
      digitalWrite(ledPin, HIGH);
      counter = counter + 1;
      if(counter > 500)
      {
        if(xVal < xValDefault-100)
        {
          xVal=xVal + 100;
        }
        else
        {
          xVal++;
        }
        if(xVal > xValDefault+100)
        {
          xVal=xVal - 100;
        }
        else
        {
          xVal--;
        }
         if(yVal < yValDefault - 100)
        {
          yVal=yVal + 100;
        }
        else
        {
           yVal++;
        }
        if(yVal > yValDefault + 100)
        {
          yVal=yVal - 100;
        }
        else
        {
          yVal--;
        }
        if(switchVal == 1000)
        {
            if(potVal < potValDefault-100)
            {
              potVal=potVal + 100;
            }
            else
            {
              potVal++;
            }
            if(potVal > potValDefault+100)
            {
              potVal=potVal - 100;
            }
            else
            {
              potVal--;
            }
        }
        Serial.print("NO SIGNAL! ");
        Serial.print(" xVal = ");
        Serial.print(xVal);
        Serial.print(" , y Val =");
        Serial.print(yVal);
        Serial.print(" , switchVal = ");
        Serial.print(switchVal);
        Serial.print(" , potVal = ");
        Serial.println(potVal);
        thrust.writeMicroseconds(xVal);         //Value written out to the servo
        rudder.writeMicroseconds(yVal);         //Value written out to the servo
        hover.writeMicroseconds(potVal);         //Value written out to the servo
      }
    }
//Delay added.  This is approx 1/2 of the send delay, but very much depends on what else you want to do in your code.
delay(10);

}