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

void BLESenddata(String _st, String _data)
{
    String send_data;
    int8_t number;
    send_data = "{\"";
    send_data += _st;
    send_data += "\":\"";
    send_data += _data;
    send_data += "\"}";
    number = send_data.length() / 17;
    if (number == 0)
    {
        BLESerial.println(send_data);
        delay(30);
    }
    else
    {
        while (number >= 0)
        {
            BLESerial.print(send_data.substring(0, 17));
            send_data = send_data.substring(17, send_data.length());
            delay(30);
            number--;
        }
        BLESerial.print("\n");
    }
}

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()
{
    var_black=analogRead(A6);
    u8g.setFont(u8g_font_9x15);
    u8g.firstPage();
    do
    {

        u8g.setPrintPos(40, 20);
        u8g.print(String("A6:")+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());

    String  BLE_Receive = readserail();
    if (BLE_Receive != "")
    {
        delay(0.1*1000);
        Serial.println(solution(BLE_Receive, "show_S1"));
        if(((solution(BLE_Receive, "show_S1")) == "begin"))
        {
            BLESenddata("control_C1", String("begin"));
            delay(30);
            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
                {
                    if(((analogRead(A2)) > (var_black)))
                    {
                        MotorRight.setSpeed(-50);
                        MotorLeft.setSpeed(0);

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

                    }

                }

            }

        }
        if(((solution(BLE_Receive, "show_S1")) == "stop"))
        {
            BLESenddata("control_C1", String("stop"));
            delay(30);
            MotorRight.Brake();
            MotorLeft.Brake();

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

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

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

        }

    }
    delay(10);

}