#include <RHReliableDatagram.h>
#include <RH_NRF24.h>
#include <SPI.h>

#define joyV A0
#define joyH A1
#define CLIENT_ADDRESS 1
#define SERVER_ADDRESS 2

int joyposV = 512;
int joyposH = 512;

RH_NRF24 RadioDriver;
RHReliableDatagram RadioManager(RadioDriver, CLIENT_ADDRESS);

uint8_t motorcontrol[3];
uint8_t buf[RH_NRF24_MAX_MESSAGE_LEN];

void setup() {
  // put your setup code here, to run once:
  pinMode(3,INPUT);
  if(!RadioManager.init())
  motorcontrol[2] = 0;
}

void loop() {
  // put your main code here, to run repeatedly:
  joyposV = analogRead(joyV);
  joyposH = analogRead(joyH);

  if(joyposV < 460){
    motorcontrol[2] = 1;

    motorcontrol[0] = map(joyposV, 460, 0, 0, 255);
    motorcontrol[1] = map(joyposV, 460, 0, 0, 255);
  }
  else if(joyposV > 564){
    motorcontrol[2] = 0;

    motorcontrol[0] = map(joyposV, 564, 1023, 0, 255);
    motorcontrol[1] = map(joyposV, 564, 1023, 0, 255);
  }
  else{
    motorcontrol[0] = 0;
    motorcontrol[1] = 0;
    motorcontrol[2] = 0;
  }

  if(joyposH < 460){
    joyposH = map(joyposH, 460, 0, 0, 255);

    motorcontrol[0] = motorcontrol[0] - joyposH;
    motorcontrol[1] = motorcontrol[1] + joyposH;

    if(motorcontrol[0] < 0)motorcontrol[0] = 0;
    if(motorcontrol[1] > 255)motorcontrol[1] = 255;
  }
  else if(joyposH > 564){
    joyposH = map(joyposH, 564, 1023, 0, 255);

    motorcontrol[0] = motorcontrol[0] + joyposH;
    motorcontrol[1] = motorcontrol[1] - joyposH;

    if(motorcontrol[0] > 255)motorcontrol[0] = 255;
    if(motorcontrol[1] < 0)motorcontrol[1] = 0;
  }

  if(motorcontrol[0] < 8)motorcontrol[0] = 0;
  if(motorcontrol[1] < 8)motorcontrol[1] = 0;

  if(RadioManager.sendtoWait(motorcontrol, sizeof(motorcontrol), SERVER_ADDRESS)){
    uint8_t len = sizeof(buf);
    uint8_t from;
  }
  delay(50);
}