//PS3コントローラ対応
//2020/8/22 メカナムホイール車用に改修
//http://gijin77.blog.jp/archives/26057910.html

#include <Wire.h>
#include <M5StickC.h>
#include <Ps3Controller.h>
#include "WEMOS_Motor.h"
/*
#define _CCW  1
#define _BK   2
#define _STOP 3
#define _STANDBY 4
*/
#define _FD  1
#define _BK  2

#define BTN_A 37
#define BTN_B 39
#define LED 10

// Global Variables

float vBatt, voltAve=3.7;
int bx=10;
int by=80;
int bw=20;
int bh=64;
int stop_flg=0;
int mrx=0;
int mry=0;
int mlx=0;
int mly=0;
int demoflg=0;
int pwr=50;
int xyoff=50;

int ps3bat; //PS3バッテリー残量

//Motor shield default I2C Address: 0x2F,0x30
//PWM frequency: 1000Hz(1kHz)
Motor MFR(0x2F, _MOTOR_A, 1000); //Motor A FR
Motor MFL(0x2F, _MOTOR_B, 1000); //Motor B FL
Motor MBR(0x30, _MOTOR_A, 1000); //Motor A BR
Motor MBL(0x30, _MOTOR_B, 1000); //Motor B BL

void setup() {
  pinMode(BTN_A, INPUT);
  pinMode(BTN_B, INPUT);
  pinMode(LED, OUTPUT);
  digitalWrite(LED,HIGH);
  Serial.begin(115200);
  M5.begin();
  Wire.begin(0, 26); //SDA,SCL

  Serial.println("プログラム　開始");
  Ps3.begin("D8:A0:1D:58:4E:C6");

  for (int i=0;i<3;i++){
    M5.Lcd.fillScreen(GREEN);
    digitalWrite(LED,LOW);delay(250);
    M5.Lcd.fillScreen(BLACK);
    digitalWrite(LED,HIGH);delay(250);
  }

  M5.Lcd.setRotation( 0 ); //Mycar = 0
  M5.Lcd.setTextFont(1);
  M5.Lcd.setTextSize(2);
  M5.Lcd.setTextColor(GREEN);
  M5.Lcd.setCursor(5,0);M5.Lcd.print("Manual");
  dispvbatt();
  face();
}

void loop() {
int rx,ry;
int lx,ly;

  M5.update();
  if ( M5.Axp.GetBtnPress() ) {
    // 電源ボタンを押すとパワーオフ
//      M5.Axp.pwrerOff();
  }
  if ( M5.BtnB.wasReleased() ) {
    // Bボタンを押すとリスタート
      esp_restart();
  }
  if ( M5.BtnA.wasReleased() ) {
    // Aボタンを押すとデモスタート
      demo_a();
  }

  if  (Ps3.isConnected()){
    dispvbatt();
    if (Ps3.data.button.l1){
//      Serial.println("l1 ボタン圧下");
      ps3bat=Ps3.data.status.battery;
      disp3batt(ps3bat);
      lcdout(GREEN,"Manual");
      delay(200);demoflg=0;
    } else if (Ps3.data.button.l2){ //バッテリ表示デモ
//      Serial.println("l2 ボタン圧下");
      for (int i=1;i<6;i++){
        disp3batt(i);
      }
      disp3batt(0xee);
      lcdout(GREEN,"Manual");
      delay(200);demoflg=0;
    } else  if (Ps3.data.button.select ){
//      Serial.println("select ボタン圧下");
      i2cck();
    } else if (Ps3.data.button.triangle){
//      Serial.println("triangle ボタン圧下");
      lcdout(WHITE,"DEMO A");delay(200);demoflg=1;
    } else if (Ps3.data.button.circle){
//      Serial.println("circle ボタン圧下");
      lcdout(WHITE,"DEMO B");delay(200);demoflg=2;
    } else if (Ps3.data.button.cross){
//    Serial.println("cross ボタン圧下");
      lcdout(WHITE,"DEMO C");delay(200);demoflg=3;
    } else if (Ps3.data.button.square){
      lcdout(WHITE,"DEMO D");delay(200);demoflg=4;
//      Serial.println("square ボタン圧下");
    } else if (Ps3.data.button.r3){
//      Serial.println("r3 ボタン圧下"); //デモリセット
      lcdout(GREEN,"Manual");
      delay(200);demoflg=0;
    } else if (Ps3.data.button.start){
//      Serial.println("start ボタン圧下"); //デモスタート
      if      (demoflg==1) demo_a();
      else if (demoflg==2) demo_b();
      else if (demoflg==3) demo_c();
      else if (demoflg==4) demo_d();
    } else if (Ps3.data.button.r1){
//      Serial.println("r1 ボタン圧下");
      lcdout(YELLOW,"pwr 50");
      pwr=50;delay(200);
    } else if (Ps3.data.button.r2){
//      Serial.println("r2 ボタン圧下");
      lcdout(YELLOW,"pwr100");
      pwr=100;delay(200);
    }
    rx=Ps3.data.analog.stick.rx;
    ry=Ps3.data.analog.stick.ry;
    lx=Ps3.data.analog.stick.lx;
    ly=Ps3.data.analog.stick.ly;

    if (abs(rx)<xyoff) rx=0;
    if (abs(ry)<xyoff) ry=0;
    if (abs(lx)<xyoff) lx=0;
    if (abs(ly)<xyoff) ly=0;
    
    rx = map(rx,-128,128,100, -100); // R-JOYSTICK X
    ry = map(ry,-128,128,100, -100); // R-JOYSTICK Y
    lx = map(lx,-128,128,100, -100); // L-JOYSTICK X
    ly = map(ly,-128,128,100, -100); // L-JOYSTICK Y

    if ((rx==0)&&(ry==0)&&(lx==0)&&(ly==0)&&(stop_flg==0)) {
        stop_flg=1;
        drvFR(_STOP,0);
        drvBR(_STOP,0);
        drvFL(_STOP,0);
        drvBL(_STOP,0);
        face();
    } else if ((mrx != rx)||(mry != ry)||(mlx != lx)||(mly != ly)){
      Serial.printf("1 RX=%4d RY=%4d LX=%4d LY=%4d pwrer=%d",rx,ry,lx,ly,pwr);
      Serial.println();
      stop_flg=0;
      BLE_4car(rx,ry,lx,ly);
    }
    mrx=rx;mry=ry;mlx=lx;mly=ly;
  }
  delay(100);
}

void i2cck(){
  int address;
  int error;

  M5.Lcd.fillScreen(BLACK);
  M5.Lcd.setTextSize(1);
  M5.Lcd.setCursor(0, 0);
  M5.Lcd.println("I2C scanning");
  Serial.println("I2C address scanning");
  for(address = 1; address < 127; address++ ) 
  {
    Wire.beginTransmission(address);
    error = Wire.endTransmission();
    if(error==0)
    {
      M5.Lcd.print(address,HEX);M5.Lcd.print(" ");
      Serial.println(address,HEX);
    }
    else {
      M5.Lcd.print(".");
 //     Serial.println(".");
    }

    delay(10);
  }
  Serial.println("End");
  delay(3000); 
  M5.Lcd.setTextSize(2); 
  M5.Lcd.fillScreen(BLACK);
  lcdout(GREEN,"Manual");
}

void BLE_4car(int rx,int ry,int lx,int ly) {
//int pwr=50;
/*
  +Y
+X  -X
  -Y
*/
  if ((ry> xyoff)&&(rx==0)) { ff(pwr);Serial.print("ff");}//前進
  if ((ry<-xyoff)&&(rx==0)) { bb(pwr);Serial.print("bb");}//後進
  if ((rx<-xyoff)&&(ry==0)) { ll(pwr);Serial.print("ll");}//左横移動
  if ((rx> xyoff)&&(ry==0)) { rr(pwr);Serial.print("rr");}//右横移動
  if ((ry> xyoff)&&(rx> xyoff)) { fr(pwr);Serial.print("fr");}//右前斜め
  if ((ry> xyoff)&&(rx<-xyoff)) { fl(pwr);Serial.print("fl");}//左前斜め
  if ((ry<-xyoff)&&(rx> xyoff)) { br(pwr);Serial.print("br");}//右後斜め
  if ((ry<-xyoff)&&(rx<-xyoff)) { bl(pwr);Serial.print("bl");}//左後斜め
 
  if ((ly> xyoff)&&(lx==0)) { ff(pwr);Serial.print("ff");}//前進
  if ((ly<-xyoff)&&(lx==0)) { bb(pwr);Serial.print("bb");}//後進
  if ((lx<-xyoff)&&(ly==0)) { rcir(pwr);Serial.print("rcir"); }//右旋回
  if ((lx> xyoff)&&(ly==0)) { lcir(pwr);Serial.print("lcir"); }//左旋回
  if ((lx<-xyoff)&&(ly> xyoff)) { ftr(pwr);Serial.print("ftr");} //前右折 Turn right
  if ((lx> xyoff)&&(ly> xyoff)) { ftl(pwr);Serial.print("ftl"); }//前左折 Turn left
  if ((lx<-xyoff)&&(ly<-xyoff)) { btr(pwr);Serial.print("btr"); }//後右折
  if ((lx> xyoff)&&(ly<-xyoff)) { btl(pwr);Serial.print("btl"); }//後左折
  Serial.println(" --> end ");
}

void drvFR(int fb,int po){
int h;
  if (po>bh) h=bh; else h=po;
  MFR.setmotor( fb,po);
  M5.Lcd.fillRect(bx+30, by-bh-2, 40, bh, BLACK);
  if (fb== _BK) {
    M5.Lcd.fillRect(bx+50, by-h-2, bw, h, RED);
  } else {
    M5.Lcd.fillRect(bx+50, by-h-2, bw, h, GREEN);
  }
}
void drvFL(int fb,int po){
int h;
  if (po>bh) h=bh; else h=po;
  MFL.setmotor( fb, po);
  M5.Lcd.fillRect(bx-10, by-bh-2, 40, bh, BLACK);
  if (fb==_BK) {
    M5.Lcd.fillRect(bx, by-h-2, bw, h, RED);
  } else {
    M5.Lcd.fillRect(bx, by-h-2, bw, h, GREEN);
  }
}
void drvBR(int fb,int po){
int h;
  if (po>bh) h=bh; else h=po;
  MBR.setmotor( fb,po);
  M5.Lcd.fillRect(bx+30, by+2, 40, bh, BLACK);
  if (fb== _BK) {
    M5.Lcd.fillRect(bx+50, by+2, bw, h, RED);
  } else {
    M5.Lcd.fillRect(bx+50, by+2, bw, h, GREEN);
  }
}
void drvBL(int fb,int po){
int h;
  if (po>bh) h=bh; else h=po;
  MBL.setmotor( fb, po);
  M5.Lcd.fillRect(bx-10, by+2, 40, bh, BLACK);
  if (fb==_BK) {
    M5.Lcd.fillRect(bx, by+2, bw, h, RED);
  } else {
    M5.Lcd.fillRect(bx, by+2, bw, h, GREEN);
  }
}

void ff(int pwr){//前進
  drvFR( _FD, pwr);
  drvFL( _FD, pwr);
  drvBR( _FD, pwr);
  drvBL( _FD, pwr);
}

void bb(int pwr){//後進
  drvFR( _BK, pwr);
  drvFL( _BK, pwr);
  drvBR( _BK, pwr);
  drvBL( _BK, pwr);
}

void rr(int pwr){//右横移動
  drvFR( _BK, pwr);
  drvFL( _FD, pwr);
  drvBR( _FD, pwr);
  drvBL( _BK, pwr);
}

void ll(int pwr){//左横移動
  drvFR( _FD, pwr);
  drvFL( _BK, pwr);
  drvBR( _BK, pwr);
  drvBL( _FD, pwr);
}

void fr(int pwr){//右前斜め
  drvFR(_STOP,0);
  drvFL( _FD, pwr);
  drvBR( _FD, pwr);
  drvBL(_STOP,0);
}

void fl(int pwr){//左前斜め
  drvFR( _FD, pwr);
  drvFL(_STOP,0);
  drvBR(_STOP,0);
  drvBL( _FD, pwr);
}

void br(int pwr){//右後斜め
  drvFR( _BK, pwr);
  drvFL(_STOP,0);
  drvBR(_STOP,0);
  drvBL( _BK, pwr);
}

void bl(int pwr){//左後斜め
  drvFR(_STOP,0);
  drvFL( _BK, pwr);
  drvBR( _BK, pwr);
  drvBL(_STOP,0);
}

void rcir(int pwr){
  drvFR( _BK, pwr);//右旋回
  drvFL( _FD, pwr);
  drvBR( _BK, pwr);
  drvBL( _FD, pwr);
}

void lcir(int pwr){//左旋回
  drvFR( _FD, pwr);
  drvFL( _BK, pwr);
  drvBR( _FD, pwr);
  drvBL( _BK, pwr);
}

void ftr(int pwr){//前右折
  drvFR(_STOP,0);
  drvFL( _FD, pwr);
  drvBR(_STOP,0);
  drvBL( _FD, pwr);
}

void ftl(int pwr){//前左折
  drvFR( _FD, pwr);
  drvFL(_STOP,0);
  drvBR( _FD, pwr);
  drvBL(_STOP,0);
}

void btr(int pwr){//後右折
  drvFR(_STOP,0);
  drvFL( _BK, pwr);
  drvBR(_STOP,0);
  drvBL( _BK, pwr);
}

void btl(int pwr){//後左折
  drvFR( _BK, pwr);
  drvFL(_STOP,0);
  drvBR( _BK, pwr);
  drvBL(_STOP,0);

}

void mstop(){
  drvFR(_STOP,0);
  drvFL(_STOP,0);
  drvBR(_STOP,0);
  drvBL(_STOP,0);
  face();
  delay(1000);
}

void demo_a(){
int pwr=50;
int wt=1000;

  lcdout(GREEN,"DEMO A");
//前進
  ff(pwr);delay(wt/2);mstop();
//後進
  bb(pwr);delay(wt);mstop(); 
//前進
  ff(pwr);delay(wt/2);mstop();

//右横移動
  rr(pwr);delay(wt/2);mstop();  
//左横移動
  ll(pwr);delay(wt);mstop();
//右横移動
  rr(pwr);delay(wt/2);mstop();  

//右旋回
  rcir(pwr);delay(wt*3);mstop();  
//左旋回
  lcir(pwr);delay(wt*3);mstop();

//右前斜め
  fr(pwr);delay(wt/2);mstop();
//左後斜め
  bl(pwr);delay(wt/2);mstop();
//左前斜め
  fl(pwr);delay(wt/2);mstop();
//右後斜め
  br(pwr);delay(wt/2);mstop();
  demoend();
}

void demo_b() {
int pwr=50;
int wt=1000;
  lcdout(GREEN,"DEMO B");
//前進
  ff(pwr);delay(wt/2);mstop();
//右横移動
  rr(pwr);delay(wt/2);mstop();  
//後進
  bb(pwr);delay(wt);mstop(); 
//左横移動
  ll(pwr);delay(wt);mstop();
//前進
  ff(pwr);delay(wt);mstop();
//右横移動
  rr(pwr);delay(wt/2);mstop();  
//後進
  bb(pwr);delay(wt/2);mstop();      
  demoend();
}

void demo_c(){
int pwr=75;
int wt=700;
  lcdout(GREEN,"DEMO C");
//前進
  ff(pwr);delay(wt);
//右横移動
  rr(pwr);delay(wt);  
//後進
  bb(pwr);delay(wt);
//左横移動
  ll(pwr);delay(wt);
//右横移動
  rr(pwr);delay(wt); 
//前進
  ff(pwr);delay(wt);
//左横移動
  ll(pwr);delay(wt);
//後進
  bb(pwr);delay(wt);

//右前斜め
  fr(pwr);delay(wt/2);
//右旋回
  rcir(pwr);delay(wt*3);
//左旋回
  lcir(pwr);delay(wt*3);
//左後斜め
  bl(pwr);delay(wt/2);
  demoend();
}

void demo_d(){
int pwr=75;
int wt=500;
  lcdout(GREEN,"DEMO D");
 //前進
  ff(pwr);delay(wt);
//右横移動
  rr(pwr);delay(wt);  
//後進
  bb(pwr);delay(wt);
//左横移動
  ll(pwr);delay(wt);
//右横移動
  rr(pwr);delay(wt); 
//前進
  ff(pwr);delay(wt);
//左横移動
  ll(pwr);delay(wt);
//後進
  bb(pwr);delay(wt);

//右前斜め
  fr(pwr);delay(wt/2);
//右旋回
  rcir(pwr);delay(wt*3);
//左旋回
  lcir(pwr);delay(wt*3);
//左後斜め
  bl(pwr);delay(wt/2);
  demoend();
}

void demoend(){
  mstop();
  lcdout(GREEN,"Manual");
  demoflg=0;
}

void lcdout(int col,String msg){
  M5.Lcd.fillRect(0,0,79,32, BLACK);
  M5.Lcd.setCursor(5, 0);
  M5.Lcd.setTextColor(col);
  M5.Lcd.print(msg);
}

void face(){
 int x0=10;
 int y0=40;
 int w0=25;
 int h0=10;
 int col=TFT_ORANGE;
 M5.Lcd.fillRect(x0,y0,w0,h0,col);
 M5.Lcd.fillRect(x0+35,y0,w0,h0,col);
 M5.Lcd.fillCircle(x0+12,y0+25, 7,col);
 M5.Lcd.fillCircle(x0+12,y0+25, 5,BLACK);
 M5.Lcd.fillCircle(x0+48,y0+25, 7,col);
 M5.Lcd.fillCircle(x0+48,y0+25, 5,BLACK);
 M5.Lcd.fillTriangle(x0+30,y0+25, 30,90,50,90,col);
 M5.Lcd.fillRect(20,y0+60,40,15,col);
}

void dispvbatt(){
  vBatt= M5.Axp.GetVbatData() * 1.1 / 1000;
  M5.Lcd.fillRect(0,145,79,32, BLACK);
  M5.Lcd.setCursor(10,145);
  if (vBatt > voltAve) {
     M5.Lcd.setTextColor(GREEN);
  } else{
     M5.Lcd.setTextColor(RED);
  }
  M5.Lcd.printf("%4.2fv ", vBatt);
}

void disp3batt(int p3bt){
 int x0=20;
 int y0=30;
 int w0=40;
 int h0=120;
 int y1=22;
 int x1,w1;
 int i;

// M5.Lcd.fillRect(0,y0,79,h0,BLACK);
 M5.Lcd.fillScreen(BLACK);
 dispvbatt();
 lcdout(GREEN,"PS3Bat");

 x1=x0+1;w1=w0-2;
 if (p3bt==0xEE) {
   for(int j=0;j<6;j++){
     M5.Lcd.fillRect(x1,y0,w1,y1*5,GREEN);
     for(i=0;i<5;i++){
       M5.Lcd.drawRect(x0,y0+y1*i,w0,20,WHITE);
     }
     delay(500);
     M5.Lcd.fillRect(x1,y0,w1,y1*5,BLACK);
     for(i=0;i<5;i++){
       M5.Lcd.drawRect(x0,y0+y1*i,w0,20,WHITE);
     }
     delay(500);
   }
   M5.Lcd.fillScreen(BLACK);
   return;
 }
 i=5-p3bt;
 switch(p3bt){
   case 1:
        M5.Lcd.fillRect(x1,y0+y1*i,w1,20,RED);
        break;
   case 2:
        M5.Lcd.fillRect(x1,y0+y1*i,w1,y1*p3bt,YELLOW);
        break;
   case 3:
        M5.Lcd.fillRect(x1,y0+y1*i,w1,y1*p3bt,YELLOW);
        break;
   case 4:
        M5.Lcd.fillRect(x1,y0+y1*i,w1,y1*p3bt,GREEN);
        break;
   case 5:
        M5.Lcd.fillRect(x1,y0+y1*i,w1,y1*p3bt,GREEN);
        break;
 }
 for(i=0;i<5;i++){
   M5.Lcd.drawRect(x0,y0+y1*i,w0,20,WHITE);
 }
 delay(3000);
// M5.Lcd.fillRect(0,y0,79,h0,BLACK);
 M5.Lcd.fillScreen(BLACK);
}