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

#define TRANSMIT_DELAY 200 // to allow for more discrete scoop control

RF24 radio(9, 10); // CE, CSN

const byte Address[6] = "00001";

int X_pin = A0;
int Y_pin = A2;

int X_pin2 = A3;
int Y_pin2 = A4;

void setup()
{
  Serial.begin(9600);

  if (!radio.begin())
  {
    Serial.println("Radio setup failed...");
    if (!radio.isChipConnected())
    {
      Serial.println("Did you wire something wrong? Radio did not boot.");
    }
    while (1)
      ;
  }

  radio.stopListening();
  delay(200);

  radio.openWritingPipe(Address);
  printf_begin();

  radio.printPrettyDetails();
}

void loop()
{
  int Xpot = analogRead(X_pin);

  int Ypot = analogRead(Y_pin);

  int Xpot2 = analogRead(X_pin2);

  int servo_direction = map(Xpot2, 2, 1023, 0, 2);

#ifdef DEBUG
  Serial.print("X1 Analog value: " + String(Xpot));
  Serial.println(" Y1 Analog value: " + String(Ypot + 15));
  Serial.print("X2 Analog value: " + String(Xpot2));
  Serial.println("Servo direction " + String(servo_direction));
#endif

  int values[] = {Xpot, Ypot + 15, servo_direction}; // the +15 is just because our joystick needed a little adjustment, yours may not!

  delay(TRANSMIT_DELAY);

  if (radio.write(values, sizeof(values)))
  {
    Serial.println("Ack packet received");
  }
  else
  {
    Serial.println("No ack, controller desynced");
  }
}