#include <Arduino.h>
#include <U8glib.h>

#include <Microduino_Motor.h>
#include <SoftwareSerial.h>

SoftwareSerial mySerial(4,5);
#define BLESerial mySerial

int var_black;
U8GLIB_SSD1306_128X64 u8g(U8G_I2C_OPT_NONE);

String solution(String _sta, char *c)
{
    String data;
    if (strstr(_sta.c_str(), c) == NULL)
    return "";
    else
    {
        if (_sta.startsWith("{") && _sta.endsWith("}"))
        {
            _sta = _sta.substring(1, _sta.length() - 1);
            _sta.replace("\"", "");
            uint8_t _length = _sta.length();
            char buf[_length];
            char c_all[30] = "";
            char data1[] = ":%s";
            strcat(c_all, c);
            strcat(c_all, data1);
            sscanf(_sta.c_str(), c_all, &buf);
            data = String(buf);
        }
        if (data != NULL)
        return data;
    }
}

Motor MotorRight(MOTOR1_PINA, MOTOR1_PINB);
Motor MotorLeft(MOTOR0_PINA, MOTOR0_PINB);
String uartMsg;
uint8_t uartStep = 0;
String readserail()
{
    char inByte = BLESerial.read();
    switch (uartStep)
    {
        case 0:
        uartMsg = "";
        if (inByte == '{')
        {
            uartMsg += inByte;
            uartStep = 1;
        }
        break;
        case 1:
        uartMsg += inByte;
        if (inByte == '}')
        {
            uartStep = 0;
            return uartMsg;
        }
        break;
        default:
        break;
    }
    return "";
}

void setup()
{
    Serial.begin(9600);
    MotorRight.begin();
    MotorLeft.begin();
    BLESerial.begin(9600);


}

void loop()
{
    String  BLE_Receive = readserail();
    if (BLE_Receive != "")
    {
        delay(0.1*1000);
        var_black=analogRead(A6);
        u8g.setFont(u8g_font_9x15);
        u8g.firstPage();
        do
        {

            u8g.setPrintPos(40, 20);
            u8g.print(String("阈值")+String((var_black)));


            u8g.setPrintPos(40, 30);
            u8g.print(String("A0")+String((analogRead(A0))));


            u8g.setPrintPos(40, 40);
            u8g.print(String("A2")+String((analogRead(A2))));


        }
        while (u8g.nextPage());

        Serial.println(solution(BLE_Receive, "show_S1"));
        if(((solution(BLE_Receive, "show_S1")) == "begin"))
        {
            if(((((analogRead(A0)) < (var_black))) && (((analogRead(A2)) < (var_black)))))
            {
                MotorRight.setSpeed(-50);
                MotorLeft.setSpeed(40);

            }
            else
            {
                if(((analogRead(A0)) > (var_black)))
                {
                    MotorRight.setSpeed(0);
                    MotorLeft.setSpeed(40);

                }
                else
                {
                    MotorRight.setSpeed(-50);
                    MotorLeft.setSpeed(0);

                }

            }

        }
        if(((solution(BLE_Receive, "show_S1")) == "stop"))
        {
            MotorRight.Brake();
            MotorLeft.Brake();

        }
        if(((solution(BLE_Receive, "show_S1")) == "up"))
        {
            MotorRight.setSpeed(-50);
            MotorLeft.setSpeed(40);

        }
        if(((solution(BLE_Receive, "show_S1")) == "left"))
        {
            MotorRight.setSpeed(0);
            MotorLeft.setSpeed(40);
            delay(1.5*1000);
            MotorRight.setSpeed(-50);
            MotorLeft.setSpeed(40);

        }
        if(((solution(BLE_Receive, "show_S1")) == "right"))
        {
            MotorRight.setSpeed(-50);
            MotorLeft.setSpeed(0);
            delay(1.5*1000);
            MotorRight.setSpeed(-50);
            MotorLeft.setSpeed(40);

        }

    }
    delay(10);

}