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

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

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

int var_variable;

int var_black;

float var_speed;

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

U8GLIB_SSD1306_128X64 u8g(U8G_I2C_OPT_NONE);

Motor MotorLeft(MOTOR0_PINA, MOTOR0_PINB);
Motor MotorRight(MOTOR1_PINA, MOTOR1_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);
    MotorLeft.begin();
    MotorRight.begin();
    BLESerial.begin(9600);

    var_speed=40;

}

void loop()
{
    String  BLE_Receive = readserail();
    if (BLE_Receive != "")
    {
        Serial.println(solution(BLE_Receive, "show_S1"));
        if(((solution(BLE_Receive, "show_S1")) == "begin"))
        {
            while(1)
            {
                var_black=analogRead(A6);
                u8g.setFont(u8g_font_9x15);
                u8g.firstPage();
                do
                {

                    u8g.setPrintPos(10, 10);
                    u8g.print(String("B:")+String((var_black)));


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


                    u8g.setPrintPos(10, 50);
                    u8g.print(String("A2:")+String((analogRead(A2))));


                }
                 while ( u8g.nextPage() );
                if(((((var_black) > (analogRead(A2)))) && (((var_black) > (analogRead(A0))))))
                {
                    MotorLeft.setSpeed(var_speed);
                    MotorRight.setSpeed(0-(var_speed));

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

                    }
                    else
                    {
                        MotorLeft.setSpeed(var_speed);
                        MotorRight.setSpeed(0);

                    }

                }

            }

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

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

        }
        if(((solution(BLE_Receive, "show_S1")) == "left"))
        {
            MotorRight.setSpeed(0);
            MotorLeft.setSpeed(40);

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

        }

    }
    delay(10);

}