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

#define CE_PIN   9
#define CSN_PIN 10

const uint64_t pipes[2] = { 0xF0F0F0F0E1LL, 0xF0F0F0F0D2LL };
int joystick[2];

RF24 radio(CE_PIN, CSN_PIN);

void turn_ahead(){
  turn_stop();
  delay(100);
  digitalWrite(2, HIGH);
  digitalWrite(3, LOW);
  digitalWrite(4, HIGH);
  digitalWrite(5, LOW);
}
void turn_back(){
  turn_stop();
  delay(100);
  digitalWrite(2, LOW);
  digitalWrite(3, HIGH);
  digitalWrite(4, LOW);
  digitalWrite(5, HIGH);
}
void turn_stop(){
  digitalWrite(2, LOW);
  digitalWrite(3, LOW);
  digitalWrite(4, LOW);
  digitalWrite(5, LOW);
}
void turn_right(){
  turn_stop();
  delay(100);
  digitalWrite(2, HIGH);
  digitalWrite(3, LOW);
  digitalWrite(4, LOW);
  digitalWrite(5, HIGH);
}
void turn_left(){
  turn_stop();
  delay(100);
  digitalWrite(2, LOW);
  digitalWrite(3, HIGH);
  digitalWrite(4, HIGH);
  digitalWrite(5, LOW);
}
void performAction(int action){
   switch(action)
    {
      case 10:
        Serial.write("Do przodu\n");
        turn_ahead();
        break;
      case 20:
        Serial.write("Do tylu\n");
        turn_back();
        break;
      case 30:
        Serial.write("Stop\n");
        turn_stop();
        break;
      case 40:
        Serial.write("W lewo\n");
        turn_left();
        break;
      case 50:
        Serial.write("w prawo\n");
        turn_right();
        break;
    }
}
void setup() {
  Serial.begin(9600);
  delay(1000);
  Serial.println("Starting");
  radio.begin();
  radio.openReadingPipe(1,pipes[0]);
  radio.openWritingPipe(pipes[1]);
  radio.startListening();
  radio.printDetails();
}

void loop() {
  if(radio.available()){
    bool done = false;
    while(radio.available())
    {
      radio.read(joystick, sizeof(joystick));
      Serial.print("Command:");
      Serial.print(joystick[0]);
      radio.stopListening();
      int response[2] = {1, 0};
      radio.write(response, sizeof(response));
      radio.startListening();
      performAction(joystick[0]);
    }
  }
}