#include <Servo.h>   // biblioteka do obsługi serwomechanizmów
#include <SoftwareSerial.h>
#include "NewPing.h" // biblioteka do obsługi sonaru
#include "ESP8266.h"

SoftwareSerial mySerial(A1, A2); // RX, TX
String inputString = "";         // a string to hold incoming data
boolean stringComplete = false;

ESP8266 wifi(Serial);

Servo myservo1;
Servo myservo2;

unsigned int odleglosc;
#define TRIG A3
#define ECHO A4

void przod()
{
  digitalWrite(7,HIGH);
  myservo1.writeMicroseconds(1700);
  myservo2.writeMicroseconds(1300);
}

void zatrzymaj()
{
  digitalWrite(7,LOW);
}

void prawo()
{
  digitalWrite(7,HIGH);
  myservo1.writeMicroseconds(1300);
  myservo2.writeMicroseconds(1300);
}

void lewo()
{
  digitalWrite(7,HIGH);
  myservo1.writeMicroseconds(1700);
  myservo2.writeMicroseconds(1700);
}

void tyl()
{
  digitalWrite(7,HIGH);
  myservo1.writeMicroseconds(1300);
  myservo2.writeMicroseconds(1700);
}


void sonar()
{
  NewPing sonar( TRIG, ECHO, 150 );
  odleglosc = sonar.ping_cm();
  //Serial.print( "Odl : " );    // w celach debugowania
  //Serial.println( odleglosc );
}

//funkcja inicjalizująca ESP
void espInit(){
 Serial.begin(9600);

  while (!Serial) {
     // czeka na podłączenie portu szeregowego, nie wykorzystywane w głównym programie
  }
   inputString.reserve(200);  //rezerwuje pamiec dla zmiennej inputString


  //mySerial.begin(9600);   //w celach debugowania, ustawia prędkość transmisji
  //dla programowego UARTa
  Serial.write("AT+RST\r\n");		  // restartuje moduł ESP

  delay(4000);
  Serial.write("AT+CIPMUX=1\r\n");    //konfiguracja ESP- ustawia tryb pracy
									  //1- obsługa wielu połączeń na raz

  delay(1500);
  Serial.write("AT+CWMODE=3\r\n");    //konfiguracja ESP - ustawia tryb pracy klient+ AP

  delay(1500);
  Serial.write("AT+CWSAP=\"ESP_Biter\",\"1234abcd\",6,3\r\n");
  //ustawia nazwę sieci wifi na "ESP_BITER", hasło do połączenia na "1234abcd"

  delay(1500);
  Serial.write("AT+CIPSERVER=1,1315\r\n");  // serwer na porcie 1315

  delay(1500);
  Serial.write("AT+CIPSTO=10000\r\n");      //ustawia limit czasu oczekiwania na 10000s

  delay(1500);
// mySerial.println(wifi.getLocalIP().c_str());  //w celach debugowania, zwraca IP

}


void setup() {
  // inicjalizacja układu
 espInit();       //wywołanie funkcji inicjalizującej ESP
 myservo1.attach(3);  // dołącza silnik do wyjścia 3 //lewy
 myservo2.attach(5);  // dołącza silnik do wyjścia 5  //prawy
  pinMode(7,OUTPUT);  //ustawia PIN "enable" jako wyjście
  //digitalWrite(7,HIGH);
 // delay(2000);
}

void loop() {

   if (stringComplete)  {		//jeśli ciąg znaków jest kompletny

    if(inputString.charAt(9)=='1'){		//na 9 znaku jest informacja przesyłana
      //przod
      sonar();					//funkcja zwracająca odległość czujnika od przeszkody
      if(odleglosc >=15 || (odleglosc==0)){
      przod();
      delay(500);
      }
    }else if(inputString.charAt(9)=='2'){
     //zatrzymaj
     zatrzymaj();
     delay(500);
   }else if(inputString.charAt(9)=='3'){
      //lewo
       sonar();
      if(odleglosc >=15 || (odleglosc==0)){
      lewo();
      delay(500);}
    }else if(inputString.charAt(9)=='4){
      //prawo
       sonar();
      if(odleglosc >=15 || (odleglosc==0)){
      prawo();
      delay(500);}
    }else if(inputString.charAt(9)=='5'){
      //do tylu
      tyl();
      delay(500);
    }else if(inputString.charAt(9)=='6'){
      //łuk w lewo
       sonar();
      if(odleglosc >=15 || (odleglosc==0)){
      luklewo();
      delay(500);}}


   // mySerial.println("input" + inputString);
    // clear the string:
    inputString = "";		//czyszczenie zmiennej
    stringComplete = false;	// resetowanie flagi
    digitalWrite(7,LOW);	// wyłączenie napędu, zatrzymanie robota
}
}





void serialEvent() {		//obsługa przerwania
  while (Serial.available()) {
    // get the new byte:
    char inChar = (char)Serial.read();
   // mySerial.println(inChar);		//debugowanie
    // kompletowanie wysłanego ciągu znaków
    inputString += inChar;
    // ustawienie flagi jeśli znak jest kompletny
    if (inChar == '\n') {
      stringComplete = true;
    }
  }  }