#include "M5Atom.h"
#include "BluetoothSerial.h"
BluetoothSerial TJ9_ATOM_BT;

int rxValue;
int F_LedState;
int B_LedState;
int L_LedState;
int R_LedState;
int LedState;
int F_LedPin = 21; //前進方向LED
int B_LedPin = 25; //後進方向LED
int L_LedPin = 26; //左方向LED
int R_LedPin = 32; //右方向LED
int M_IN1 = 22; //R_Motor IN1pin
int M_IN2 = 19; //R_Motor IN2pin
int M_IN3 = 23; //L_Motor IN3pin
int M_IN4 = 33; //L_Motor IN4pin
int Turn;

unsigned long previousMillis = 0; //方向指示LED
unsigned long interval = 200; //方向指示LED点滅時間(mSec)
extern const unsigned char AtomImageData[375 + 2];
uint8_t DisBuff[2 + 5 * 5 * 3]; //RGB LED初期設定

void setup() {
  Serial.begin(115200);
  M5.begin(true, false, true);
  delay(10);
  setBuff(0xff, 0x00, 0x00); //RGB LED初期設定：赤色
  M5.dis.displaybuff(DisBuff);
  TJ9_ATOM_BT.begin("TJ9_ATOM_BT");
  Serial.println("Bluetooth JT9 Control");
  pinMode(F_LedPin, OUTPUT); digitalWrite(F_LedPin, LOW);
  pinMode(B_LedPin, OUTPUT); digitalWrite(B_LedPin, LOW);
  pinMode(L_LedPin, OUTPUT); digitalWrite(L_LedPin, LOW);
  pinMode(R_LedPin, OUTPUT); digitalWrite(R_LedPin, LOW);
  ledcSetup(0, 500, 8); //R_Motor IN1(ch、frequency(Hz)、duty(Bit)）
  ledcSetup(1, 500, 8); //R_Motor IN2(ch、frequency(Hz)、duty(Bit)）
  ledcSetup(2, 500, 8); //L_Motor IN3(ch、frequency(Hz)、duty(Bit)）
  ledcSetup(3, 500, 8); //L_Motor IN4(ch、frequency(Hz)、duty(Bit)）
  ledcAttachPin(M_IN1, 0); //R_Motor M_IN1(GPIO-22)を0chに定義
  ledcAttachPin(M_IN2, 1); //R_Motor M_IN2(GPIO-19)を1chに定義
  ledcAttachPin(M_IN3, 2); //L_Motor M_IN3(GPIO-23)を2chに定義
  ledcAttachPin(M_IN4, 3); //L_Motor M_IN4(GPIO-33)を3chに定義
  Turn = 0;
}

void Forward() { //前進
  ledcWrite(0, 255); //ch0のPWM値を100%の255に設定
  ledcWrite(1, 0); //ch1のPWM値は必ず0とする。
  ledcWrite(2, 190); //左モータの回転が早く右にカーブするのでch2のPWM値を
                     //190に下げて進路を補正。（カットアンドトライ）
                     //左右モータに回転差が無い場合は左右のPWM値を揃えます。
  ledcWrite(3, 0); //ch3のPWM値は必ず0とする。
  F_LedState = 1; //前進方向LED点滅表示ON
  B_LedState = 0;
  L_LedState = 0;
  R_LedState = 0;
}

void Backward() { //後進
  ledcWrite(0, 0);
  ledcWrite(1, 255);
  ledcWrite(2, 0);
  ledcWrite(3, 210); //左モーターの回転が早く右にカーブするのでch3のPWM値を
                     //210に下げて進路を補正。（カットアンドトライ）
                     //左右モータに回転差が無い場合は左右のPWM値を揃えます。
  F_LedState = 0;
  B_LedState = 1; //後進方向LED点滅表示ON
  L_LedState = 0;
  R_LedState = 0;
}

void Turnleft() {
  if (Turn == 0) { //左超信地旋回　左右のモータを反対方向に回転させてその場で旋回
    ledcWrite(0, 255);
    ledcWrite(1, 0);
    ledcWrite(2, 0);
    ledcWrite(3, 255);
    F_LedState = 0;
    B_LedState = 0;
    L_LedState = 1; //左方向LED点滅表示ON
    R_LedState = 0;
  } else{ //左円弧旋回　左モータのPWM値を70に下げて左へ円弧旋回
    ledcWrite(0, 255);
    ledcWrite(1, 0);
    ledcWrite(2, 70);
    ledcWrite(3, 0);
    F_LedState = 0;
    B_LedState = 0;
    L_LedState = 1; //左方向LED点滅表示ON
    R_LedState = 0;
  }
}

void Turnright() {
  if (Turn == 0) { //右超信地旋回　左右のモータを反対方向に回転させてその場で旋回
    ledcWrite(0, 0);
    ledcWrite(1, 255);
    ledcWrite(2, 255);
    ledcWrite(3, 0);
    F_LedState = 0;
    B_LedState = 0;
    L_LedState = 0;
    R_LedState = 1; //右方向LED点滅表示ON
  } else { //右円弧旋回　右モータのPWM値を70に下げて左へ円弧旋回
    ledcWrite(0, 70);
    ledcWrite(1, 0);
    ledcWrite(2, 255);
    ledcWrite(3, 0);
    F_LedState = 0;
    B_LedState = 0;
    L_LedState = 0;
    R_LedState = 1; //右方向LED点滅表示ON
  }
}

void Stop() { //停止　全てのPWM値が0にて停止
  ledcWrite(0, 0);
  ledcWrite(1, 0);
  ledcWrite(2, 0);
  ledcWrite(3, 0);
  F_LedState = 0;
  B_LedState = 0;
  L_LedState = 0;
  R_LedState = 0;
}

void setBuff(uint8_t Rdata, uint8_t Gdata, uint8_t Bdata){ //RGB LEDの設定
  DisBuff[0] = 0x05;
  DisBuff[1] = 0x05;
  for (int i = 0; i < 25; i++)
  {
    DisBuff[2 + i * 3 + 0] = Rdata;
    DisBuff[2 + i * 3 + 1] = Gdata;
    DisBuff[2 + i * 3 + 2] = Bdata;
  }
}

uint8_t FSM = 0;

void loop() {
  if (TJ9_ATOM_BT.available()) {
    rxValue = TJ9_ATOM_BT.read(); //スマホアプリからのBluetooth信号を受信
    //Serial.print("Received:"); Serial.println(rxValue);
    switch (rxValue) {
      case 'F': //Forward
        Forward();
        setBuff(0x40, 0x40, 0x40); //M5 Atom LiteのRGB LEDを白色に設定
        break;
      case 'B': //Backward
        Backward();
        setBuff(0x40, 0x00, 0x00);//M5 Atom LiteのRGB LEDを赤色に設定
        break;
      case 'L': //Turnleft
        Turnleft();
        setBuff(0x00, 0x00, 0x40);//M5 Atom LiteのRGB LEDを青色に設定
        break;
      case 'R': //Turnleft
        Turnright();
        setBuff(0x40, 0x00, 0x40);//M5 Atom LiteのRGB LEDをマゼンタに設定
        break;
      case '1': //HardTurning　スマホアプリMOVICのAボタン選択時　超信地旋回
        Turn = 0;
        break;
      case '2': //GentleTurning　スマホアプリMOVICのBボタン選択時　円弧旋回
        Turn = 1;
        break;
      case 'S': //Stop
        Stop();
        setBuff(0x00, 0x40, 0x00);//M5 Atom LiteのRGB LEDを緑色に設定
        break;
      default:
        break;
    }
    M5.dis.displaybuff(DisBuff);
  }

  unsigned long currentMillis = millis();
  if (currentMillis - previousMillis > interval) { //方向指示LEDの点滅信号作成
    if (LedState == 0) {
      LedState = 1;
    }
    else {
      LedState = 0;
    }
    previousMillis = currentMillis;
  }

  if (F_LedState == 1) { //前進方向LED点滅表示ON
    if (LedState == 0) {
      digitalWrite(F_LedPin, HIGH);
    } else {
      digitalWrite(F_LedPin, LOW);
    }
  } else {
    digitalWrite(F_LedPin, LOW);
  }

  if (B_LedState == 1) { //後進方向LED点滅表示ON
    if (LedState == 0) {
      digitalWrite(B_LedPin, HIGH);
    } else {
      digitalWrite(B_LedPin, LOW);
    }
  } else {
    digitalWrite(B_LedPin, LOW);
  }

  if (L_LedState == 1) { //左方向LED点滅表示ON
    if (LedState == 0) {
      digitalWrite(L_LedPin, HIGH);
    } else {
      digitalWrite(L_LedPin, LOW);
    }
  } else {
    digitalWrite(L_LedPin, LOW);
  }

  if (R_LedState == 1) { //右方向LED点滅表示ON
    if (LedState == 0) {
      digitalWrite(R_LedPin, HIGH);
    } else {
      digitalWrite(R_LedPin, LOW);
    }
  } else {
    digitalWrite(R_LedPin, LOW);
  }
  delay(20);
  M5.update();
}
}