#define DEBUG true
#include<SoftwareSerial.h>
#include <Servo.h>

SoftwareSerial mySerial(9,10);
//192.168.4.1
//parola123
//netGratis
// Pinii motor 1
#define mpin00 5
#define mpin01 6
// Pinii motor 2
#define mpin10 3
#define mpin11 11
Servo srv;
void setup() {
 Serial.begin(115200);
 mySerial.begin(115200);
 pinMode(LED_BUILTIN, OUTPUT);
 digitalWrite(LED_BUILTIN, LOW);
 sendData("AT+RST\r\n", 2000, false); // resetare modul
 sendData("AT+CWMODE=2\r\n", 1000, false); // configurare ca access point
 sendData("AT+CIFSR\r\n", 1000, DEBUG); // citeste adresa IP
 sendData("AT+CWSAP?\r\n", 2000, DEBUG); // citeste informatia SSID (nume retea)
 sendData("AT+CIPMUX=1\r\n", 1000, false); // configurare conexiuni multiple
 sendData("AT+CIPSERVER=1,80\r\n", 1000, false); // pornire server pe port 80
  digitalWrite(mpin00, 0);
 digitalWrite(mpin01, 0);
 digitalWrite(mpin10, 0);
 digitalWrite(mpin11, 0);
 pinMode (mpin00, OUTPUT);
 pinMode (mpin01, OUTPUT);
 pinMode (mpin10, OUTPUT);
 pinMode (mpin11, OUTPUT);
 // pin LED
}
void delayStopped(int ms)
{
 StartMotor (mpin00, mpin01, 0, 0);
 StartMotor (mpin10, mpin11, 0, 0);
 delay(ms);
}
void StartMotor (int m1, int m2, int forward, int speed)
{

 if (speed==0) // oprire
 {
 digitalWrite(m1, 0);
 digitalWrite(m2, 0);
 }
 else
 {
 if (forward)
 {
 digitalWrite(m2, 0);
 analogWrite (m1, speed); // folosire PWM
 }
 else
 {
 digitalWrite(m1, 0);
 analogWrite(m2, speed);
 }
 }
}
void loop() {
 if (mySerial.available()) {
 if (mySerial.find("+IPD,")) {
 delay(500);
 int connectionId = mySerial.read() - 48; // functia read() returneaza valori zecimale ASCII
 // si caracterul ‘0’ are codul ASCII 48
 String webpage = "<h1>Robot Control!</h1><a href=\"/l0\"><button>Forward</button></a></br>";
 String cipSend = "AT+CIPSEND=";
 cipSend += connectionId;
 cipSend += ",";
 webpage += "<a href=\"/l3\"><button>Left</button></a>";
 webpage += "<a href=\"/l1\"><button>Stop</button></a>";
 webpage += "<a href=\"/l4\"><button>Right</button></a></br>";
 webpage += "<a href=\"/l2\"><button>Backward</button></a>";

 if (readSensor() > 0) {
 webpage += "<h2>Millis:</h2>";
 webpage += readSensor();
 }
 cipSend += webpage.length();
 cipSend += "\r\n";
 sendData(cipSend, 100, DEBUG);
 sendData(webpage, 150, DEBUG);

 String closeCommand = "AT+CIPCLOSE=";
 closeCommand += connectionId; //se adauga identificatorul conexiunii
 closeCommand += "\r\n";
 sendData(closeCommand, 300, DEBUG);
 }
 }
}
String sendData(String command, const int timeout, boolean debug) {
 String response = "";
 mySerial.print(command); // trimite comanda la esp8266
 long int time = millis();
 while ((time + timeout) > millis()) {
 while (mySerial.available()) {
 char c = mySerial.read(); // citeste caracter urmator
 response += c;
 }
 }
 if (response.indexOf("/l0") != -1) {
 digitalWrite(LED_BUILTIN, HIGH);
 StartMotor (mpin00, mpin01, 1, 255);
 StartMotor (mpin10, mpin11, 1, 255);
 }
 if (response.indexOf("/l2") != -1) {
 digitalWrite(LED_BUILTIN, HIGH);
 StartMotor (mpin00, mpin01, 0, 255);
 StartMotor (mpin10, mpin11, 0, 255);
 }
 if (response.indexOf("/l3") != -1) {
 digitalWrite(LED_BUILTIN, HIGH);
StartMotor (mpin00, mpin01, 0, 90);
 StartMotor (mpin10, mpin11, 1, 90);

 delay (500);
 delayStopped(500);
 }
 if (response.indexOf("/l4") != -1) {
 digitalWrite(LED_BUILTIN, HIGH);
StartMotor (mpin00, mpin01, 1, 90);
 StartMotor (mpin10, mpin11, 0, 90);

 delay (500);
 delayStopped(500);
 }
 if (response.indexOf("/l1") != -1) {
 digitalWrite(LED_BUILTIN, LOW);
 delayStopped(500);
 }
 if (debug) {
 Serial.print(response);
 }
 return response;
}
unsigned long readSensor() {
 return millis();
}