/*
 * CE D7
 * CSN D8
 * SCK D13
 * MO D11
 * MI D12
*/

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

#define CE_PIN   7
#define CSN_PIN 8


#define go 9
#define L 2
#define P 4

//int32_t frequency = 2500;

// NOTE: the "LL" at the end of the constant is "LongLong" type
const uint64_t pipe = 0xE8E8F0F0E1LL; // Define the transmit pipe

//---( Declare objects )---/
RF24 radio(CE_PIN, CSN_PIN); // Create a Radio

Servo Turn;

//---( Declare Variables )---/
int joystick[3];
int forward = 0;
int reverse = 0;
int sterering = 90;
int fail = 0;

void setup()
{
  delay(1000);
//  InitTimersSafe();
//  bool success = SetPinFrequencySafe(9, frequency);
  radio.begin();
  radio.setDataRate(RF24_250KBPS);
  radio.openReadingPipe(1, pipe);
  radio.startListening();
  Turn.attach(6);
  pinMode(P, OUTPUT);
  digitalWrite(P, 0);
  pinMode(L, OUTPUT);
  digitalWrite(L, 0);
}

void loop(){
  if (radio.available()){
    {
      radio.read(joystick, sizeof(joystick));
      sterering = map(joystick[1], 286, 757, 35, 145);
      Turn.write(sterering);
      if(joystick[2] == 0){
      if(460 < joystick[0] < 550){
        forward = 0;
      }
      if(550 < joystick[0]){
        forward = map(joystick[0], 550, 800, 0, 255);
        digitalWrite(L, LOW);
        digitalWrite(P, HIGH);
      }
      else if(450 > joystick[0]){
        forward = map(joystick[0], 460, 200, 0, 255);
        digitalWrite(P, LOW);
        digitalWrite(L, HIGH);
      }
      analogWrite(go, forward);
      fail = 0;
    }
    if(joystick[2] == 1){
      digitalWrite(P, LOW);
      digitalWrite(L, LOW);
      analogWrite(go, 0);
    }
   }
  }
/*  else
  {
    fail++;
    if (fail > 200){
    analogWrite(go, 0);
    analogWrite(L, 0);
    sterering = 90;
    }
   ]
*/
}