#include <ESP8266WiFi.h>
#include <WebSocketsClient.h>

const String ssid = "AutoWiFiManager";
const String passwd = "123456789";
String remoteHost = "192.168.4.1";

// 超声波传感器引脚
#define trigPin 14 // D5 引脚 蓝色 trig
#define echoPin 12 // D6 引脚 褐色 echo

#define LED1 4 // D2
#define LED2 0 // D3

#define leftPin 13 // 左边红外 传感器 D7
#define rightPin 5 // 右边红外 传感器 D1

bool isAuto = false;
bool isHand = false;

WebSocketsClient wsClient;

//静态地址、网关、子网掩码
IPAddress local_IP(192, 168, 4, 2);
IPAddress gateway(192, 168, 4, 1);
IPAddress subnet(255, 255, 255, 0);

void setup()
{
    Serial.begin(9600);
    WiFi.config(local_IP, gateway, subnet); //设置静态IP
    WiFi.mode(WIFI_STA);
    WiFi.begin(ssid, passwd);

    // 判断是否连接成功
    while (WiFi.status() != WL_CONNECTED)
    {
        Serial.print('.');
        delay(100);
    }
    Serial.println("");
    Serial.println("Connected to " + ssid);
    Serial.print("Current ip address is ");
    Serial.println(WiFi.localIP());

    pinMode(leftPin, INPUT);
    pinMode(rightPin, INPUT);

    pinMode(LED_BUILTIN, OUTPUT);
    digitalWrite(LED_BUILTIN, HIGH);

    pinMode(LED1, OUTPUT);
    pinMode(LED2, OUTPUT);

    analogWrite(LED1, 0);
    analogWrite(LED2, 0);

    pinMode(trigPin, OUTPUT); // 将trigPin设置为输出
    pinMode(echoPin, INPUT);  // 将echoPin设置为输入

    // 启动WebSocket客户端
    wsClient.begin(remoteHost, 81);

    // 指定事件处理函数
    wsClient.onEvent([](WStype_t type, uint8_t *payload, size_t length)
                     {
                         if (type == WStype_TEXT)
                         {
                             // 接收来自服务端的信息（服务端FLASH按键状态），并控制LED的工作
                             String data = (char *)payload;
                             Serial.print("wsClient  ======== ：");
                             Serial.println(data);

                             String type = getValue(data, '|', 0);
                             if (type == "client")
                             {
                                 String status = getValue(data, '|', 1);
                                 if (status == "Automatic")
                                 {
                                     String val = getValue(data, '|', 2);
                                     val == "1" ? isAuto = true : isAuto = false;
                                 }
                                 else if (status == "fronLamp")
                                 {
                                     String LampStatus = getValue(data, '|', 2);
                                     LampStatus == "1" ? oftenOpen() : close();
                                 }
                                 else if (status == "doubleFlash")
                                 {
                                     String flashStatus = getValue(data, '|', 2);
                                     flashStatus == "1" ? open() : close();
                                 }
                                 else if (status == "AutoHand")
                                 {
                                     String handStatus = getValue(data, '|', 2);
                                     handStatus == "1" ? isHand = true : isHand = false;
                                 }
                             }
                         }
                     });
}

void loop()
{

    // Serial.println("loop...");
    if (isAuto)
    {
        Serial.println("Automatic...");
        Automatic();
        delay(800);
    }
    if (isHand)
    {
        Serial.println("follow...");
        follow();
        delay(300);
    }
    wsClient.loop();
}

// 自动跟随的代码
void follow()
{
    int Right_Value = digitalRead(rightPin);
    int Left_Value = digitalRead(leftPin);

    Serial.print("RIGHT：");
    Serial.println(Right_Value);
    Serial.print("LEFT：");
    Serial.println(Left_Value);

    // Serial.print("getDistance():  ========");
    // Serial.println(getDistance());
    // Serial.println(getDistance() <= 30);


    Serial.println(
        (Right_Value == 1 && Left_Value == 1) && (getDistance() <= 30));

    // 1 没有检测到障碍物，0 检测到障碍物

    // 左右都没有检测到障碍物   直行
    if ((Right_Value == 1 && Left_Value == 1) && (getDistance() <= 30))
    {
        Serial.println("straight..");
        // 直行
        digitalWrite(LED_BUILTIN, LOW); // 开
        String straightForwardPos = String("sensor|motor|straight");
        wsClient.sendTXT(straightForwardPos);
    }
    // 右边 有 障碍物，   左边 没有 障碍物    右行
    else if ((Right_Value == 0) && (Left_Value == 1))
    {
        Serial.println("right..");
        String rightPos = String("sensor|servo|140");
        wsClient.sendTXT(rightPos);
        delay(200);
        String rightForwardPos = String("sensor|motor|right");
        wsClient.sendTXT(rightForwardPos);
    }
    // 右边 没有 障碍物，  左边 有 障碍物    左行
    else if ((Right_Value == 1) && (Left_Value == 0))
    {
        Serial.println("left..");
        String leftPos = String("sensor|servo|40");
        wsClient.sendTXT(leftPos);
        delay(200);
        String leftForwardPos = String("sensor|motor|left");
        wsClient.sendTXT(leftForwardPos);
    }
    // 左右 没有 障碍物  停车
    else if ((Right_Value == 1) && (Left_Value == 1))
    {
        Serial.println("stop1..");
        // 停车
        digitalWrite(LED_BUILTIN, HIGH); // 关
        String instructions = String("sensor|ultrasonic|stop");
        wsClient.sendTXT(instructions);
        delay(1000);
    }
}

// 自动寻路
void Automatic()
{

    Serial.print("getDistance():  ========");
    Serial.println(getDistance());
    Serial.println(getDistance() <= 50);

    // 如果距离小于30cm那么停车
    if (getDistance() <= 50)
    {
        Serial.println("stop car ========");
        digitalWrite(LED_BUILTIN, LOW);

        // 停车
        String instructions = String("sensor|ultrasonic|stop");
        wsClient.sendTXT(instructions);
        delay(500);

        Serial.println("left car ========");
        // 左转舵机
        String leftPos = String("sensor|servo|40");
        wsClient.sendTXT(leftPos);
        delay(800);

        if (getDistance() <= 50)
        {
            
            Serial.println("right car ========");
            // 右转舵机
            String rightPos = String("sensor|servo|140");
            wsClient.sendTXT(rightPos);
            delay(1200);
            if (getDistance() <= 50)
            {

                Serial.println("center car ========");
                // 向中间转动舵机
                String centerPos = String("sensor|servo|90");
                wsClient.sendTXT(centerPos);
                delay(800);

                Serial.println("back car ========");
                // 后退
                String backForwardPos = String("sensor|motor|back");
                wsClient.sendTXT(backForwardPos);

                // 如果右边没有障碍物
            }
            else
            {
                Serial.println("right car run ========");
                String rightForwardPos = String("sensor|motor|right");
                wsClient.sendTXT(rightForwardPos);
            }
        // 如果左转没有障碍物
        }
        else
        {
            Serial.println("left car run ========");
            String leftForwardPos = String("sensor|motor|left");
            wsClient.sendTXT(leftForwardPos);
        }
        // 如果前面没有障碍物
    }
    else
    {
        Serial.println("run car ========");
        digitalWrite(LED_BUILTIN, HIGH);
        String straightForwardPos = String("sensor|motor|straight");
        wsClient.sendTXT(straightForwardPos);
    }
}

// 一直打开大灯
void oftenOpen()
{
    analogWrite(LED1, 255);
    analogWrite(LED2, 255);
}

void open()
{
    analogWrite(LED1, 255);
    analogWrite(LED2, 255);
    delay(300);
    analogWrite(LED1, 0);
    analogWrite(LED2, 0);
    delay(300);
}

void close()
{
    analogWrite(LED1, 0);
    analogWrite(LED2, 0);
}

// 返回距离
int getDistance()
{
    long duration;
    int distance;

    // Clears the trigPin
    digitalWrite(trigPin, LOW);
    delayMicroseconds(2);

    //将trigPin设置为HIGH状态10微秒
    digitalWrite(trigPin, HIGH);
    delayMicroseconds(10);
    digitalWrite(trigPin, LOW);

    // 读取echoPin，以微秒为单位返回声波传播时间
    duration = pulseIn(echoPin, HIGH);

    // 计算距离
    distance = duration * 0.034 / 2;

    return distance;
}

String getValue(String data, char separator, int index)
{
    int found = 0;
    int strIndex[] = {0, -1};
    int maxIndex = data.length() - 1;

    for (int i = 0; i <= maxIndex && found <= index; i++)
    {
        if (data.charAt(i) == separator || i == maxIndex)
        {
            found++;
            strIndex[0] = strIndex[1] + 1;
            strIndex[1] = (i == maxIndex) ? i + 1 : i;
        }
    }
    return found > index ? data.substring(strIndex[0], strIndex[1]) : "";
}