#include <Arduino.h>
#include <SoftwareSerial.h>

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

#include <Microduino_Motor.h>
String var_variable;

int var_black;

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;
    }
}

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 "";
}
Motor MotorLeft(MOTOR0_PINA, MOTOR0_PINB);
Motor MotorRight(MOTOR1_PINA, MOTOR1_PINB);

void setup()
{
    BLESerial.begin(9600);

    MotorLeft.begin();
    MotorRight.begin();

}

void loop()
{
    String  BLE_Receive = readserail();
    if (BLE_Receive != "")
    {
        var_variable=solution(BLE_Receive, "show_S1");

    }
    delay(10);
    if(((var_variable) == "begin"))
    {
        while(!(!(((var_variable) == "begin"))))
        {
            String  BLE_Receive = readserail();
            if (BLE_Receive != "")
            {
                var_variable=solution(BLE_Receive, "show_S1");

            }
            delay(10);
            var_black=analogRead(A6);
        }
        if(((((analogRead(A0)) < (var_black))) && (((analogRead(A2)) < (var_black)))))
        {
            MotorLeft.setSpeed(40);
            MotorRight.setSpeed(-40);

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

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

            }

        }

    }
    if(((var_variable) == "stop"))
    {
        MotorLeft.Brake();
        MotorRight.Brake();

    }
    if(((var_variable) == "up"))
    {
        MotorLeft.setSpeed(60);
        MotorRight.setSpeed(-60);

    }
    if(((var_variable) == "left"))
    {
        MotorRight.setSpeed(0);
        MotorLeft.setSpeed(40);

    }
    if(((var_variable) == "right"))
    {
        MotorRight.setSpeed(-40);
        MotorLeft.setSpeed(0);

    }

}