/*
 * Connect
 * pin 7 = enable on right side
 * pin 13 = reverse on left side
 * pin 12 = forward on right side
 * pin 6 = enable on left side
 * pin 11 = forward right side
 * pin 10 = reverse right side
 * 
 */



#include <Scheduler.h>
#include <FastLED.h>
#include <SPI.h>
#include <nRF24L01.h>
//#include <Printf.h>
#include <RF24.h>
#include <RF24_config.h>
RF24 radio(9,8); //wireless module Ce, CSN pin (in order)
byte adresy[][6] = {"1Node", "2Node"};
struct dataStruct{
  int Vry;
  int Vry1;
  bool prepinac;
} data;
int cyklus;
String prev_state;
int const echoPinp = 25; //right sided Hc-Sr04 sensor
int const trigPinp = 24; //right sided Hc-Sr04 sensor
int const echoPinl = 27; //left sided Hc-Sr04 sensor
int const trigPinl = 26; //left sided Hc-Sr04 sensor
int const echoPin = 23;  //front Hc-Sr04 sensor
int const trigPin = 22;  //front Hc-Sr04 sensor
long durationp;
long durationl;
int i = 0;
int front_sensor;
int right_sensor;
int left_sensor;
int c = 0;
char course;
int const dist = 40; //stands for alloved distance in autonomous mode
int const min_dist = 10;
int const rychlost = 70; // speed in autonomous mode
struct datastruct {
  int d;
  int dp;
  int dl;
}odoslat;
#define NUM_LEDS_boky 9 // number of leds on edges
#define NUM_LEDS_stred 15 // number of leds in front
#define NUM_LEDS_zadok 7 // number of leds in rear

#define DATA_PIN_s 2 // data pin of front leds, s - generaly stands for front
#define DATA_PIN_pp 4 //data pin of front right leds, pp - means right front
#define DATA_PIN_lp 28 // data pin of front left leds, lp - means left front
#define DATA_PIN_z 3 //data pin of rear leds, z - means rear

CRGB led_s[NUM_LEDS_stred];
CRGB led_pp[NUM_LEDS_boky];
CRGB led_lp[NUM_LEDS_boky];
CRGB led_z[NUM_LEDS_zadok];
void zastav () { //stop
  digitalWrite(13, LOW);
  digitalWrite(12, LOW);
  digitalWrite(11, LOW);
  digitalWrite(10, LOW);
  digitalWrite(6, LOW);
  digitalWrite(7, LOW);
}
void dopredu () { //forward
  digitalWrite(13, LOW);
  digitalWrite(12, HIGH);
  digitalWrite(11, HIGH);
  digitalWrite(10, LOW);
  analogWrite(6, rychlost);
  analogWrite(7, rychlost);
}
void dolava () { //to left
  digitalWrite(13, HIGH);
  digitalWrite(12, LOW);
  digitalWrite(11, HIGH);
  digitalWrite(10, LOW);
  analogWrite(6, rychlost);
  analogWrite(7, rychlost*2);
}
void doprava () { // to right
  digitalWrite(13, LOW);
  digitalWrite(12, HIGH);
  digitalWrite(11, LOW);
  digitalWrite(10, HIGH);
  analogWrite(6, rychlost*2);
  analogWrite(7, rychlost);
}
void mierne_dolava () { //only a little to left
  digitalWrite(13, LOW);
  digitalWrite(12, HIGH);
  digitalWrite(11, HIGH);
  digitalWrite(10, LOW);
  analogWrite(6, rychlost);
  analogWrite(7, rychlost*2);
}
void mierne_doprava () { //only a little to right
  digitalWrite(13, LOW);
  digitalWrite(12, HIGH);
  digitalWrite(11, HIGH);
  digitalWrite(10, LOW);
  analogWrite(6, rychlost*2);
  analogWrite(7, rychlost);
}
void cuvaj () { // go reverse
  digitalWrite(13, HIGH);
  digitalWrite(12, LOW);
  digitalWrite(11, LOW);
  digitalWrite(10, HIGH);
  analogWrite(6, rychlost);
  analogWrite(7, rychlost);
}
void lights_off () {
  for (int a = 0; a < NUM_LEDS_zadok; a++){
    led_z[a] = CRGB(0 ,0, 0);
  }
  for (int a = 0; a < NUM_LEDS_stred; a++) {
    led_s[a] = CRGB(0, 0, 0);
  }
  for (int a = 0; a < NUM_LEDS_boky; a++) {
    led_pp[a] = CRGB(0, 0, 0);
    led_lp[a] = CRGB(0, 0, 0);
  }
  FastLED.show();
}
void right_blinker () {
  for (int a = 0; a < NUM_LEDS_boky; a++) {
    led_lp[a] = CRGB(0, 0, 0);
  }
  for (int b = 0; b < NUM_LEDS_boky; b++) {
    led_pp[b] = CRGB::Orange;
  }
  for (int a = 2; a < NUM_LEDS_zadok; a++){
    led_z[a] = CRGB(0 , 255, 0);
  }
  led_z[0] = CRGB::Orange;
  led_z[1] = CRGB::Orange;

  led_s[1] = CRGB::Orange;
  for (int b = 5; b < 10; b ++) {
    led_s[b] = CRGB::Orange;
  }
  led_s[11] = CRGB::Orange;
  led_s[0, 2, 3, 4, 10, 12, 13, 14] = CRGB(0, 0, 0);
  FastLED.show();
  FastLED.delay(300);
   for (int b = 0; b < NUM_LEDS_boky; b++) {
    led_pp[b] = CRGB::Black;
  }
  led_z[0] = CRGB::Black;
  led_z[1] = CRGB::Black;
  FastLED.show();
  FastLED.delay(200);
}
void left_blinker () {
  for (int a = 0; a < NUM_LEDS_boky; a++) {
    led_pp[a] = CRGB(0, 0, 0);
  }
  for (int b = 0; b < NUM_LEDS_boky; b++) {
    led_lp[b] = CRGB::Orange;
  }
  for (int a = 0; a < 6; a++){
    led_z[a] = CRGB(0 , 255, 0);
  }
  led_z[6] = CRGB::Orange;
  led_z[7] = CRGB::Orange;

  led_s[3] = CRGB::Orange;
  for (int b = 5; b < 10; b ++) {
    led_s[b] = CRGB::Orange;
  }
  led_s[13] = CRGB::Orange;
  led_s[0, 1, 2, 4, 10, 11, 12, 14] = CRGB(0, 0, 0);
  FastLED.show();
  FastLED.delay(300);
   for (int b = 0; b < NUM_LEDS_boky; b++) {
    led_lp[b] = CRGB::Black;
  }
  led_z[6] = CRGB::Black;
  led_z[7] = CRGB::Black;
  FastLED.show();
  FastLED.delay(200);
}
void forward_lights () {
  
  for (int a = 0; a < NUM_LEDS_stred; a++) {
    led_s[a] = CRGB(255, 255, 255);
  } 
  led_pp[0, 1, 2, 4, 7] = CRGB(255, 255, 255);
  led_pp[3, 5, 6, 8] = CRGB(0, 0, 0);
  led_lp[0, 1, 2, 4, 7] = CRGB(255, 255, 255);
  led_lp[3, 5, 6, 8] = CRGB(0, 0, 0);
  for (int a = 0; a < NUM_LEDS_zadok; a++){
    led_z[a] = CRGB(0 ,255, 0);
  }
  
  FastLED.show();
  
}
void reverse_lights () {
  for (int a = 10; a < NUM_LEDS_stred; a++) {
    led_s[a] = CRGB(255, 0, 0);
  } 
  led_s[2, 7] = CRGB(255, 0, 0);
  led_s[0, 1, 3, 4, 5, 6, 8, 9] = CRGB(0, 0, 0);
  led_pp[1, 4, 6, 7, 8] = CRGB(255, 0, 0);
  led_pp[0, 2, 3, 5] = CRGB(0, 0, 0);
  led_lp[1, 4, 6, 7, 8] = CRGB(255, 0, 0);
  led_lp[0, 2, 3, 5] = CRGB(0, 0, 0);
  for (int a = 0; a < NUM_LEDS_zadok; a++){
    led_z[a] = CRGB(255 ,255, 255);
  }
  led_z[0, 6] = CRGB::Orange;
  FastLED.show();
  FastLED.delay(400);
  for (int a = 1; a < 6; a++){
    led_z[a] = CRGB(0, 0, 0);
  }
  FastLED.show();
  FastLED.delay(400);
}
void setup() {
  radio.begin();
  radio.setChannel(103);
  radio.setPayloadSize(32);
  radio.setPALevel(RF24_PA_MAX);
  radio.setDataRate(RF24_250KBPS);
  radio.openWritingPipe(adresy[1]);
  radio.openReadingPipe(1, adresy[0]);
  //radio.printDetails();
  pinMode(6, OUTPUT);
  pinMode(7, OUTPUT);
  pinMode(11, OUTPUT);
  pinMode(12, OUTPUT);
  pinMode(13, OUTPUT);
  pinMode(10, OUTPUT);
  pinMode(5, OUTPUT);
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  pinMode(trigPinp, OUTPUT);
  pinMode(echoPinp, INPUT);
  pinMode(trigPinl, OUTPUT);
  pinMode(echoPinl, INPUT);
  radio.startListening();
  Scheduler.startLoop(svetla);
  FastLED.addLeds<WS2812B, DATA_PIN_s, GRB>(led_s, NUM_LEDS_stred);
  FastLED.addLeds<WS2812B, DATA_PIN_pp, GRB>(led_pp, NUM_LEDS_boky);
  FastLED.addLeds<WS2812B, DATA_PIN_lp, GRB>(led_lp, NUM_LEDS_boky);
  FastLED.addLeds<WS2812B, DATA_PIN_z, GRB>(led_z, NUM_LEDS_zadok);
  FastLED.setBrightness(170);
  cyklus = 3;
}

void loop() {
  c = 0;
  //lights_off();
  odoslat.dp = vzdialenost(1); //stores the value of right distance sensor
  delay(10); 
  odoslat.d = vzdialenost(2); //stores the value of front distance sensor
  delay(10);
  odoslat.dl = vzdialenost(3); //stores the value of left distance sensor

  if(radio.available()) {

    while(radio.available() and c < 100) {
      radio.read(&data, sizeof(data));
      c += 1;
    }
    radio.stopListening();
    radio.write(&odoslat, sizeof(odoslat));
    radio.startListening();
    
    if (c == 99) cyklus += 1;
    else {
      cyklus = 0;
    }
    digitalWrite(5, HIGH);
  }
  else {
    if (cyklus > 2) {
      zastav();
      digitalWrite(5, LOW);
    }
    cyklus += 1;
      
   
  }
  if (cyklus < 2) {
    zastav();
    if (data.prepinac == false) {
      data.Vry = map(data.Vry, 1023, 0, 200, -200);
      data.Vry1 = map(data.Vry1, 1023,0, 200, -200);
      if (data.Vry1 > 10) digitalWrite(12, HIGH);
      if (data.Vry1 < -10) digitalWrite(13, HIGH);
      if (data.Vry > 10) digitalWrite(10, HIGH);
      if (data.Vry < -10) digitalWrite(11, HIGH);
      data.Vry = abs(data.Vry);
      data.Vry1 = abs(data.Vry1);
      analogWrite(6, data.Vry1);
      analogWrite(7, data.Vry);
    }
    else {
      digitalWrite(5, LOW);
      if (odoslat.d > dist and odoslat.dl < dist or odoslat.dp < dist) {
        if (odoslat.dl < dist) {
          doprava();
          course = 'r';
          delay(40);
        }
        else {
          dolava();
          course = 'l';
          delay(40);
        }
      }
      else if (odoslat.d > dist and odoslat.dl < dist and odoslat.dp < dist) {
        if (odoslat.dp > odoslat.dl) {
          mierne_doprava();
          course = 'r';
          delay(50);
        }
        else if (odoslat.dl > odoslat.dp) {
          mierne_dolava();
          course = 'l';
          delay(50);
        }
        else {
          dopredu();
          course = 'f';
          delay(50);
        }
      }
      else if (odoslat.d > dist and odoslat.dl > dist and odoslat.dp > dist) {
        dopredu();
        course = 'f';
        delay(100);
      }
      else {
        if (odoslat.d > min_dist and odoslat.dp > dist and odoslat.dl > dist) {
          if (odoslat.dp > odoslat.dl) {
            doprava();
            course = 'r';
            delay(100);
          }
          else {
            dolava();
            course = 'l';
            delay(100);
          }
        }
        else {
          if ( odoslat.d > min_dist and odoslat.dp > dist) {
            doprava();
            course = 'r';
            delay(100);
          }
          else if (odoslat.d > min_dist and odoslat.dl > dist) {
            dolava();
            course = 'l';
            delay(100);
          }
          else {
            if (odoslat.d > min_dist){
              if ( odoslat.dp > odoslat.dl){
                doprava();
                course = 'r';
              }
              else {
                dolava();
                course = 'l';
              }
              delay(500);
            }
            else {
              cuvaj();
              course = 'b';
              delay(800);
              odoslat.dp = vzdialenost(1);
              odoslat.dl = vzdialenost(3);
              if (odoslat.dp > odoslat.dl) {
                doprava();
                course = 'r';
              }
              else {
                dolava();
                course = 'l';
              }
              delay(200);
            }
          }
        }
      }
      
      digitalWrite(5,HIGH);
    }
  }
  delay(50);
  

}
void svetla() {
  
  if (cyklus > 2) {
    for (int i = 0; i < NUM_LEDS_zadok; i++){
      led_z[i] = CRGB::Red;
    }
    for (int i = 0; i < 5; i++) {
      led_s[i] = CRGB::Red;
    }
    for (int i = 0; i < 3; i++) {
      led_pp[i] = CRGB::Red;
      led_lp[i] = CRGB::Red;
    }
    led_s[5] = CRGB::Red;
    led_s[9] = CRGB::Red;
    led_s[10] = CRGB::Red;
    led_s[14] = CRGB::Red;
    
    led_pp[3] = CRGB::Red;
    led_pp[6] = CRGB::Red;
    led_lp[5] = CRGB::Red;
    led_lp[8] = CRGB::Red;
    FastLED.show();
    FastLED.delay(100);
  
  }
  else {
    
    right_sensor = constrain(odoslat.dp, 5, 150);
    right_sensor = map(right_sensor, 5, 150, 0, 255);
    front_sensor = constrain(odoslat.d, 5, 150);
    front_sensor = map(front_sensor, 5, 150, 0, 255);
    left_sensor = constrain(odoslat.dl, 5, 150);
    left_sensor = map(left_sensor, 5, 150, 0, 255);
    for (int i = 0; i < NUM_LEDS_stred; i++) {
      led_s[i] = CRGB(abs(front_sensor-255), front_sensor, 0);
    }
    for (int i = 0; i < NUM_LEDS_boky; i++) {
      led_pp[i] = CRGB(abs(right_sensor-255), right_sensor, 0);
    }
    for (int i = 0; i < NUM_LEDS_boky; i++) {
      led_lp[i] = CRGB(abs(left_sensor-255), left_sensor, 0);
    }
    FastLED.show();
    FastLED.delay(50);
  }
  
  yield();
}

int vzdialenost (int vyber) {
  int const echoPinp = 25;
  int const trigPinp = 24;
  int const echoPinl = 27;
  int const trigPinl = 26;
  int const echoPin = 23; 
  int const trigPin = 22;
  int akttrig = 0;
  int aktecho = 0;
  int distance = 0;
  int priemer = 0;
  long duration = 0;
  int priemer_c = 0;
  switch (vyber) {
    case 1:
    {
      akttrig = trigPinp;
      aktecho = echoPinp;
    }
    break;
    case 2:
    {
      akttrig = trigPin;
      aktecho = echoPin;
    }
    break;
    case 3:
    {
      akttrig = trigPinl;
      aktecho = echoPinl;
    }
    break;
  }
  while (priemer_c < 3) {
    digitalWrite(akttrig, LOW); 
    delayMicroseconds(2); 
  
    digitalWrite(akttrig, HIGH);
    delayMicroseconds(10); 
   
    digitalWrite(akttrig, LOW);
    duration = pulseIn(aktecho, HIGH, 10000);
   
    //Calculate the distance (in cm) based on the speed of sound.
    distance = duration/58.2;
    if (distance == 0) distance = 200 ;
    priemer = priemer + distance;
    
    priemer_c += 1;
    duration = 0;
    delay(20);
  }
  distance = priemer/3;
 
  return distance;
}