#include <ESP8266WiFi.h>
#include <WiFiUdp.h>
#include <Servo.h>

//L9110S motor drive input pin
// L9110S B-2A motors Right       GPIO14(D5)
#define IN_1  14     
// L9110S B-1A motors Right       GPIO4(D4)     
#define IN_2  4     
// L9110S A-1B motors Left        GPIO13(D7)      
#define IN_3  13   
// L9110S A-1A motors Left        GPIO12(D6)       
#define IN_4  12     
// ServoPin Input pin             GPIO0(D8)     
#define ServoPin  0      

// Define the ultrasonic sensor pin
//GPIO16(D2)
#define  Trig 16    
//GPIO5(D3)
#define  Echo 5 

//Button to send data Flash BTN on NodeMCU
#define SendKey 0  

const char* ssid = "wifissid";
const char* password = "wifipassword";

WiFiUDP Udp;
unsigned int localUdpPort = 1987;  // local port to listen on
char incomingPacket[255];  // buffer for incoming packets
char  replyPacket[] = "Hi there! Got the message :-)";  // a reply string to send back

Servo myservo;  // create servo object to control a servo

int speedCar = 200;       
unsigned long BrakeTime = 0;  
String COMMAND = "";

void setup()
{
   
  
  Serial.begin(9600);
  //Serial.println();

  //Serial.printf("Connecting to %s ", ssid);
  WiFi.begin(ssid, password);
  while (WiFi.status() != WL_CONNECTED)
  {
    delay(500);
    //Serial.print(".");
  }
  //Serial.println(" connected");

  Udp.begin(localUdpPort);
  //Serial.printf("Now listening at IP %s, UDP port %d\n", WiFi.localIP().toString().c_str(), localUdpPort);

    //Set the pin mode
   pinMode(IN_1, OUTPUT);
   pinMode(IN_2, OUTPUT);
   pinMode(IN_3, OUTPUT);
   pinMode(IN_4, OUTPUT); 
   pinMode(Trig,OUTPUT);
   pinMode(Echo,INPUT);

    //Btn to send data
  pinMode(SendKey,INPUT_PULLUP);
  
  // attaches the servo on GPIO0 to the servo object
  myservo.attach(ServoPin); 
  myservo.write(100);
}

void goAhead(){ 

      analogWrite(IN_1, 0);
      analogWrite(IN_2, speedCar);
      
      analogWrite(IN_3, 0);
      analogWrite(IN_4, speedCar);
}

void goBack(){ 
      analogWrite(IN_1, speedCar);
      analogWrite(IN_2, 0);
     
      analogWrite(IN_3, speedCar);
      analogWrite(IN_4, 0);   
}

void goRight(){ 

      analogWrite(IN_1, speedCar);
      analogWrite(IN_2, 0);
      
      analogWrite(IN_3, 0);
      analogWrite(IN_4, speedCar);  
}

void goLeft(){

      analogWrite(IN_1, 0);
      analogWrite(IN_2, speedCar);
     
      analogWrite(IN_3, speedCar);
      analogWrite(IN_4, 0);   
}

void stopRobot(){  

      BrakeTime = millis();  
      digitalWrite(IN_1, LOW);
      digitalWrite(IN_2, LOW);
      digitalWrite(IN_3, LOW);
      digitalWrite(IN_4, LOW);        
   
}

/*
Function: obtain ultrasonic sensor ranging data
Parameter description: sensor is connected to the motherboard pin port A1,A2
*/
float GetDistance()
{
    float distance;
  
    digitalWrite(Trig, LOW); 
    delayMicroseconds(2); 
    digitalWrite(Trig, HIGH); 
    delayMicroseconds(10);
    digitalWrite(Trig, LOW);
  
     distance = pulseIn(Echo, HIGH) / 58.00;
     //Serial.print("distance = ");
     //Serial.println(distance);
     
     return distance;
}

/*
* Function: Obstacle avoidance
* Parameter: set_dis sets the obstacle avoidance distance
*/
void avoidance(int set_dis)
{
    int angle;
    
    // distance
    int dis[3];
  
     goAhead();
     //Steering engine back to center
     myservo.write(100); 
    //Obtain the distance between the cart and the obstacle and store it in dis[1]
    dis[1] = GetDistance(); 

    if(dis[1] < set_dis )
    {
        stopRobot();  //Stop the car

        //If the left and right infrared obstacle avoidance sensors do not encounter obstacles when the steering gear rotates
        
            for (angle = 100; angle <= 180; angle++) 
            {
                 myservo.write(angle);
                //delay(1);         
            }
             delay(5); 
           // Measure the distance between the left obstacle and the cart, and store the measurement data in dis[2]
            dis[2]=GetDistance(); 

            for (angle = 100; angle >= 0; angle--) 
            {
                 myservo.write(angle);  
                          
                if(angle == 100)  
                {   // Stores measurement data in dis[1]
                      delay(5); 
                    dis[1] = GetDistance(); 
                } 
            }
           // Record the range data on the right side of the trolley
            dis[0] = GetDistance(); 
            delay(5);
           
            for (angle = 0; angle <= 100; angle++) 
            {
                 myservo.write(angle);
            }
        //The right is more distant from the obstacle than the Left
        if(dis[0] < dis[2] ) 
        {
          if(dis[0] < 10)
          {
            goBack();
            delay(300);
          }
            goLeft();
            delay(150);
        }
        //The right is more distant from the obstacle than the Right
        else if (dis[0] > dis[2] )
        {
             if(dis[0] < 10)
            {
              goBack();
              delay(300);
            }
              
            goRight(); 
            delay(150);
        } 
    }
}


void loop()
{

   //Отправляем IP адрес устройства
   while(Serial.available())
    {
       delay(10);
       char c = Serial.read();
       COMMAND+=c;    
    }
    if(COMMAND.length()>0)
    {     
      if(COMMAND.compareTo("ip")==0)
      {
         Serial.println(WiFi.localIP());
         COMMAND = "";
      }
    }

      if (strcmp(incomingPacket, "F") == 0) goAhead();        
      else if (strcmp(incomingPacket, "B") == 0) goBack();        
      else if (strcmp(incomingPacket, "L") == 0) goLeft();
      else if (strcmp(incomingPacket, "R") == 0) goRight();
      else if (strcmp(incomingPacket, "W") == 0) avoidance(20);
      else if (strcmp(incomingPacket, "0") == 0) speedCar = 130;
      else if (strcmp(incomingPacket, "1") == 0) speedCar = 145;
      else if (strcmp(incomingPacket, "2") == 0) speedCar = 160;
      else if (strcmp(incomingPacket, "3") == 0) speedCar = 175;
      else if (strcmp(incomingPacket, "4") == 0) speedCar = 190;
      else if (strcmp(incomingPacket, "5") == 0) speedCar = 200;
      else if (strcmp(incomingPacket, "6") == 0) speedCar = 215;
      else if (strcmp(incomingPacket, "7") == 0) speedCar = 230;
      else if (strcmp(incomingPacket, "8") == 0) speedCar = 245;
      else if (strcmp(incomingPacket, "9") == 0) speedCar = 255;
      else if (strcmp(incomingPacket, "S") == 0) stopRobot();

  
  int packetSize = Udp.parsePacket();
  if (packetSize)
  {
    // receive incoming UDP packets
    //Serial.printf("Received %d bytes from %s, port %d\n", packetSize, Udp.remoteIP().toString().c_str(), Udp.remotePort());
    int len = Udp.read(incomingPacket, 255);
    if (len > 0)
    {
      incomingPacket[len] = 0;
    }
    //Serial.printf("UDP packet contents: %s\n", incomingPacket);   

      // send back a reply, to the IP address and port we got the packet from
      /*Udp.beginPacket(Udp.remoteIP(), Udp.remotePort());
      Udp.write(replyPacket);
      Udp.endPacket();*/
    
    
  }
}