#include "BluetoothSerial.h"

#define MOTOR11 25
#define MOTOR12 26
#define MOTOR21 32
#define MOTOR22 33
#define LED 17

BluetoothSerial SerialBT;
int mode = 4;
char recieve[5];
char send[128];

void setup() {
  // Bluetooth device name
  SerialBT.begin("ESP32track");
  pinMode(MOTOR11, OUTPUT);
  pinMode(MOTOR12, OUTPUT);
  pinMode(MOTOR21, OUTPUT);
  pinMode(MOTOR22, OUTPUT);
  pinMode(LED,OUTPUT);
}

void loop() {
  if (SerialBT.available()) {
    char c = SerialBT.read();
    memcpy(recieve, recieve + 1, 4);
    recieve[4] = c;

    if (c == '\n') {
      if (memcmp(recieve, "$RDY", 4) == 0) {
        SerialBT.println("\nINIT 100,75,#FFFFFF,0");
        SerialBT.println("FONT 1,SERIF,BOLD,8");
        SerialBT.println("FONT 2,SERIF,BOLD,12");
        SerialBT.println("DRAWTEXT TRACK CONTROLLER,50,0,1,1,#0000FF");
        char* moji[] = {"","↑","","⟲","-","⟳","","↓",""};
        for (int pos = 0; pos < 9; pos++) {
          if (pos != 0 && pos != 2 && pos != 6 && pos != 8) {
            sprintf(send, "DRAWTEXT %s,%d,%d,4,2,#0000FF", moji[pos], (pos % 3) * 20 + 29, (pos / 3) * 20 + 24);
            SerialBT.println(send);
            
            sprintf(send, "P%d:+DRAWRECT %d,%d,18,18,#0000FF,1", pos, (pos % 3) * 20 + 20, (pos / 3) * 20 + 15);
            SerialBT.println(send);
          }
        }
      }
      if (memcmp(recieve, "$+P", 3) == 0 && recieve[3] >= '0' && recieve[3] <= '8') {
        mode = recieve[3] - '0';
        sprintf(send, "CUR:DRAWRECT %d,%d,18,18,#FF0000,1", (mode % 3) * 20 + 20, (mode / 3) * 20 + 15);
        SerialBT.println(send);
        digitalWrite(LED, HIGH);
      }
    }
  }
  switch (mode) {
    case 1:   // 前進
      digitalWrite(MOTOR11, LOW);
      digitalWrite(MOTOR12, HIGH);
      digitalWrite(MOTOR21, LOW);
      digitalWrite(MOTOR22, HIGH);
      break;
    case 3:   // 左回転
      digitalWrite(MOTOR11, HIGH);
      digitalWrite(MOTOR12, LOW);
      digitalWrite(MOTOR21, LOW);
      digitalWrite(MOTOR22, HIGH);
      break;
    case 4:  // 停止
      digitalWrite(MOTOR11, LOW);
      digitalWrite(MOTOR12, LOW);
      digitalWrite(MOTOR21, LOW);
      digitalWrite(MOTOR22, LOW);
      digitalWrite(LED, LOW);
      break;  
    case 5:   // 右回転
      digitalWrite(MOTOR11, LOW);
      digitalWrite(MOTOR12, HIGH);
      digitalWrite(MOTOR21, HIGH);
      digitalWrite(MOTOR22, LOW);
      break;
    case 7:   // 後退
      digitalWrite(MOTOR11, HIGH);
      digitalWrite(MOTOR12, LOW);
      digitalWrite(MOTOR21, HIGH);
      digitalWrite(MOTOR22, LOW);
      break;
  }
}