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

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

#include <Microduino_AudioPro.h>
#include <utility/MIDI_file.h>
#include <Microduino_Matrix.h>
#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 "";
}
AudioPro midiPlayer;

uint8_t Addr[MatrixPix_X][MatrixPix_Y] =
{
    {
        64
    }
}
;
Matrix display = Matrix(Addr, TYPE_COLOR);

uint8_t rgbArray_64_3216_5[8][8][3]=
{
    {
        {51,255,51},{255,255,51},{255,102,0},{153,153,153},{0,153,0},{51,51,255},{153,255,255},{204,102,204}},{{51,255,51},{255,255,51},{255,102,0},{153,153,153},{0,153,0},{51,51,255},{153,255,255},{204,102,204}},{{51,255,51},{255,255,51},{255,102,0},{153,153,153},{0,153,0},{51,51,255},{153,255,255},{204,102,204}},{{51,255,51},{255,255,51},{255,102,0},{153,153,153},{0,153,0},{51,51,255},{153,255,255},{204,102,204}},{{51,255,51},{255,255,51},{255,102,0},{153,153,153},{0,153,0},{51,51,255},{153,255,255},{204,102,204}},{{51,255,51},{255,255,51},{255,102,0},{153,153,153},{0,153,0},{51,51,255},{153,255,255},{204,102,204}},{{51,255,51},{255,255,51},{255,102,0},{153,153,153},{0,153,0},{51,51,255},{153,255,255},{204,102,204}},{{51,255,51},{255,255,51},{255,102,0},{153,153,153},{0,153,0},{51,51,255},{153,255,255},{204,102,204}
    }

}
;

uint8_t rgbArray_64_0_6[8][8][3]=
{
    {
        {0,0,0},{0,0,0},{0,0,0},{0,0,0},{0,0,0},{0,0,0},{0,0,0},{0,0,0}},{{0,0,0},{0,0,0},{0,0,0},{0,0,0},{0,0,0},{0,0,0},{0,0,0},{0,0,0}},{{0,0,0},{0,0,0},{0,0,0},{0,0,0},{0,0,0},{0,0,0},{0,0,0},{0,0,0}},{{0,0,0},{0,0,0},{0,0,0},{0,0,0},{0,0,0},{0,0,0},{0,0,0},{0,0,0}},{{0,0,0},{0,0,0},{0,0,0},{0,0,0},{0,0,0},{0,0,0},{0,0,0},{0,0,0}},{{0,0,0},{0,0,0},{0,0,0},{0,0,0},{0,0,0},{0,0,0},{0,0,0},{0,0,0}},{{0,0,0},{0,0,0},{0,0,0},{0,0,0},{0,0,0},{0,0,0},{0,0,0},{0,0,0}},{{0,0,0},{0,0,0},{0,0,0},{0,0,0},{0,0,0},{0,0,0},{0,0,0},{0,0,0}
    }

}
;

Motor MotorLeft(MOTOR0_PINA, MOTOR0_PINB);
Motor MotorRight(MOTOR1_PINA, MOTOR1_PINB);

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

    if (! midiPlayer.begin())
    {
        while (1);
    }
    midiPlayer.setVolume(127);
    midiPlayer.useInterrupt(VS1053_PIN_DREQ);

    Wire.begin();

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

}

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

    }
    delay(10);
    midiPlayer.playROM(Jay, sizeof(Jay));
    for (int y = 0; y < 8; y++)
    {
        for (int x = 0; x <8; x++)
        {
            display.setLedColor(x, y , rgbArray_64_3216_5[y][x][0], rgbArray_64_3216_5[y][x][1],rgbArray_64_3216_5[y][x][2]);
        }
    }
    for (int y = 0; y < 8; y++)
    {
        for (int x = 0; x <8; x++)
        {
            display.setLedColor(x, y , rgbArray_64_0_6[y][x][0], rgbArray_64_0_6[y][x][1],rgbArray_64_0_6[y][x][2]);
        }
    }
    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);

    }

}