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

// Create an RF24 object to manage the nRF24L01 module, specifying the CE and CSN pins
RF24 radio(7, 8);
// Define the address for the radio pipe. This address should match on the transmitter and receiver
const byte address[6] = "00001";

// Create servo objects
Servo servo1;
Servo servo2;

// Initialize Sabertooth motor driver on address 128
Sabertooth ST(128);

// Define servo connected pins
const int servoPin1 = 9;
const int servoPin2 = 10;

void setup() {
  // Initialize serial communication at 9600 bits per second
  Serial.begin(9600);
  // Initialize the nRF24L01 module
  radio.begin();
  // Open a reading pipe on address "00001", using pipe 0
  radio.openReadingPipe(0, address);
  // Set the power amplifier level to minimum to save power
  radio.setPALevel(RF24_PA_MIN);
  // Start listening for incoming messages
  radio.startListening();

  // Attach servos to their pins
  servo1.attach(servoPin1);
  servo2.attach(servoPin2);

  // Begin serial communication with Sabertooth
  SabertoothTXPinSerial.begin(9600);
  ST.autobaud(); // Autobaud for Sabertooth controller
}

void loop() {
  if (radio.available()) {
    struct {
      int j1X; // Joystick 1 X value
      int j1Y; // Joystick 1 Y value
      int j2X; // Joystick 2 X value
      int j2Y; // Joystick 2 Y value
    } joystickValues;

    radio.read(&joystickValues, sizeof(joystickValues));

    // Control the servos based on joystick values
    servo1.write(map(joystickValues.j1Y, -512, 512, 0, 180));
    servo2.write(map(joystickValues.j1X, -512, 512, 40, 140));

    // Control the Sabertooth motor driver based on Joystick 2
    ST.drive(map(joystickValues.j2Y, -512, 512, -127, 127));  // Map Joystick 2 Y for speed control
    ST.turn(map(joystickValues.j2X, -512, 512, -127, 127));   // Map Joystick 2 X for turning control
  }
}