#include <ESP8266WiFi.h>
#include <ESP8266mDNS.h>
#include <WiFiUdp.h>
#include <ArduinoOTA.h>
#include<SoftwareSerial.h>
 

int bttx = 1;  //tx of bluetooth module is connected to pin 9 of arduino
int btrx = 3;  //rx of bluetooth module is connected to pin 10 of arduino

int servo1 = 5; // connect servo to pin 10
int servo2 = 4;
int pwmVal = 0; // declare pulse width modulation value
int pwmVal2 = 0;

// Replace with your network credentials
const char* ssid = "BUBUKA";
const char* password = "19601003";

const int ESP_BUILTIN_LED = 2;
SoftwareSerial bluetooth(bttx, btrx);

void setup() {
 // analogWriteFreq(5000);
  pinMode(servo1, OUTPUT); //set up the servoPin as an output pin
  pinMode(servo2, OUTPUT);
  Serial.begin(115200);
  bluetooth.begin(9600);
  Serial.println("Booting");
  WiFi.mode(WIFI_STA);
  WiFi.begin(ssid, password);
  while (WiFi.waitForConnectResult() != WL_CONNECTED) {
    Serial.println("Connection Failed! Rebooting...");
    delay(5000);
    ESP.restart();
  }

  // Port defaults to 8266
  // ArduinoOTA.setPort(8266);

  // Hostname defaults to esp8266-[ChipID]
  // ArduinoOTA.setHostname("myesp8266");

  // No authentication by default
  // ArduinoOTA.setPassword((const char *)"123");

  ArduinoOTA.onStart([]() {
    Serial.println("Start");
  });
  ArduinoOTA.onEnd([]() {
    Serial.println("\nEnd");
  });
  ArduinoOTA.onProgress([](unsigned int progress, unsigned int total) {
    Serial.printf("Progress: %u%%\r", (progress / (total / 100)));
  });
  ArduinoOTA.onError([](ota_error_t error) {
    Serial.printf("Error[%u]: ", error);
    if (error == OTA_AUTH_ERROR) Serial.println("Auth Failed");
    else if (error == OTA_BEGIN_ERROR) Serial.println("Begin Failed");
    else if (error == OTA_CONNECT_ERROR) Serial.println("Connect Failed");
    else if (error == OTA_RECEIVE_ERROR) Serial.println("Receive Failed");
    else if (error == OTA_END_ERROR) Serial.println("End Failed");
  });
  ArduinoOTA.begin();
  Serial.println("Ready");
  Serial.print("IP address: ");
  Serial.println(WiFi.localIP());
  pinMode(ESP_BUILTIN_LED, OUTPUT);
}

void loop() {
  ArduinoOTA.handle();

  
  
Eleje:

  ArduinoOTA.handle();
  digitalWrite(ESP_BUILTIN_LED, LOW);
  delay(200);
  digitalWrite(ESP_BUILTIN_LED, HIGH);
  delay(200);

  if (bluetooth.available() > 0 ) {
    Serial.println(bluetooth.parseInt());
  }

  if (bluetooth.parseInt() == 0)
  {
    goto Eleje;
  }


  if (bluetooth.parseInt() == 40)
  {
    goto Stop;
  }

  if (bluetooth.parseInt() == 10)
  {
    goto Motor1E;
  }


  if (bluetooth.parseInt() == 20)
  {
    goto Motor2E;

  }

  if (bluetooth.parseInt() == 30)
  {
    goto ME;

  }

  if (bluetooth.parseInt() == 70)
  {
    goto MH;

  }

  if (bluetooth.parseInt() == 50)
  {
    goto Motor1H;

  }

  if (bluetooth.parseInt() == 60)
  {
    goto Motor2H;

  }

Motor1E:

  if (bluetooth.available() > 0 ) {
    // int pwmVal = bluetooth.parseInt();
    analogWrite(servo1, 1023);
    Serial.println("Mootor1E");
    //  Serial.println(pwmVal);
    goto Eleje;
  }


Motor2E:

  if (bluetooth.available() > 0 ) {
    // int pwmVal2 = bluetooth.parseInt() - 250;
    analogWrite(servo2, 1);
    Serial.println("Motor2E");
    //  Serial.println(pwmVal2);
    goto Eleje;

  }


Motor1H:

  if (bluetooth.available() > 0 ) {
    //  int pwmVal = bluetooth.parseInt();
    analogWrite(servo1, 1);
    Serial.println("Motor1H");
    //  Serial.println(pwmVal);
    goto Eleje;
  }


Motor2H:

  if (bluetooth.available() > 0 ) {
    //  int pwmVal2 = bluetooth.parseInt() - 250;
    analogWrite(servo2, 1023);
    Serial.println("Motor2H");
    //  Serial.println(pwmVal2);
    goto Eleje;

  }


ME:

  if (bluetooth.available() > 0 ) {
    //  int pwmVal2 = bluetooth.parseInt() - 250;
    analogWrite(servo2, 1);
    analogWrite(servo1, 1023);
    Serial.println("MindkettoE");
    //  Serial.println(pwmVal2);
    goto Eleje;
  }

MH:

  if (bluetooth.available() > 0 ) {
    //  int pwmVal2 = bluetooth.parseInt() - 250;
    analogWrite(servo2, 1023);
    analogWrite(servo1, 1);
    Serial.println("MindkettoH");
    //  Serial.println(pwmVal2);
    goto Eleje;

  }



Stop:

  if (bluetooth.available() > 0 ) {
    analogWrite(servo2, 0);
    analogWrite(servo1, 0);
    Serial.println("STOP");
    Serial.println("0");
    goto Eleje;

  }

}