// Load up the libraries
#include <SPI.h>
#include <nRF24L01.h>
#include <RF24.h>
#include <Servo.h>

// define the pins
#define CE_PIN   9
#define CSN_PIN 8
#define steerPin A5
#define actuator_pin A0

// define TT motor
#define In1 2
#define In2 4
#define EnTTpwm 3
#define lights_pin A4

bool LED_ON = false;
int prev_state = 0;
// Servo instance

Servo steer_ser;
Servo actu_ser;

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

// The tx/rx address
const byte rxAddr[6] = "00001";

void setup()
{
  
  // Start the serial
  Serial.begin(9600);
  while(!Serial);
  Serial.println("NRF24L01P Receiver Starting...");

  // Configure Servo
  steer_ser.attach(steerPin);
  actu_ser.attach(actuator_pin);
  //configure TT motor
  pinMode(In1,OUTPUT);
  pinMode(In2,OUTPUT);
  pinMode(EnTTpwm,OUTPUT);
  pinMode(lights_pin,OUTPUT);
  
  digitalWrite(In1, LOW);
  digitalWrite(In2, LOW);
  
  
  // Start the radio, again set to min & slow as I'm guessing while testing theire really close to each other
  radio.begin();
  radio.setPALevel(RF24_PA_MIN);   // RF24_PA_MIN ,RF24_PA_LOW, RF24_PA_HIGH, RF24_PA_MAX
  radio.setDataRate( RF24_250KBPS ); // RF24_250KBPS, RF24_1MBPS, RF24_2MBPS
  
  // Set the reading pipe and start listening
  radio.openReadingPipe(0, rxAddr);
  radio.startListening();
  
}

void loop()
{
  
  if (radio.available())
  {
    // the buffer to store the received message in
    char data[100] = {0};
    int steerServoAngle = "";
    int pwmMotor = ""; 
    int Abutton = 0;
    int led_switch = 0;
    
    // Now read the message, old examples have done = radio.read(), that doesn't work anymore!!!
    radio.read(&data, sizeof(data));
    steerServoAngle = data[0];
    pwmMotor = data[1];
    Abutton = data[2];
  else if(LED_ON == false && led_switch == 0){
      prev_state = 0;
    }
    // A Button
    if(Abutton == 0 ){
      actu_ser.write(90);
    }else
    {
    actu_ser.write(10);
    }
    
    //map angle
    Serial.println(steerServoAngle);
    steerServoAngle = map(steerServoAngle,-127,127,90,30);
    if(steerServoAngle > 55 && steerServoAngle < 65){
      steerServoAngle = 60;
    }
    steer_ser.write(steerServoAngle);
    
    // Motor direction
   if(pwmMotor < -10){
    digitalWrite(In1, HIGH);
    digitalWrite(In2, LOW);
   }
   else if(pwmMotor > 0){
    digitalWrite(In1, LOW);
    digitalWrite(In2, HIGH);
   }else{
     digitalWrite(In1, LOW);
    digitalWrite(In2, LOW);
   }
   int motor_speed = map(pwmMotor,-127,127,-255,255);
   
   if(motor_speed < 0){
    motor_speed = -motor_speed;
   }
    analogWrite(EnTTpwm, motor_speed);

    
    // Print the message out to the COM window
    steer_ser.write(steerServoAngle);
    Serial.println("Received Message: ");
    Serial.println(motor_speed);
    Serial.println(steerServoAngle);
    Serial.println(Abutton);
    Serial.println("********");
  } 
}