//230213-24　DEMO_mode
//緑加速　赤減速　どちらかの最中に逆のボタンで惰行
//黄色 停車中は方向反転　走行中は緊急停止　VRで常時出力を調整（車両灯用）
//count^(zouka1+zouka2)*0.9
/*画面
［速度値］［常時出力値］［トータル出力値(=maxspeed）］［F/R］［急停止/転換］［ノッチ］
［動作モード］［車両状態］　
*/

//ライブラリ
#include <LiquidCrystal_I2C.h>
LiquidCrystal_I2C lcd(0x27, 16, 2);

//定義
#define kasoku_button 2 //加速ボタン
#define gensoku_button 6 //減速ボタン
#define direction_button 4 //方向切替ボタン
#define volume A0
#define maxspeed 120  //最大PWM値　255　最大速度はここで調整。こんなもんで十分。
#define ENABLE 3 //L293 PWM入力pin
#define ch1 7 //L293前ch
#define ch2 8 //L293後ch


int volume_val; //0~1023 VRのanalogRead値
unsigned int joutou; //0~255 VR読みのmap結果 出力に加算

int kasoku_count = 0; //ボタン押す回数で増減
int kasoku_state; //=kasoku_count
int kasoku_Amount = 0; //=kasoku_count ループ毎にこの値が加算される
int current_kasoku = 0; //kasoku_Amountの加算合計値 ノッチごとのスピードリミットをつける
boolean kasoku_button_state = false;
boolean gensoku_button_state = false;
boolean kasoku_press = HIGH;
boolean kasoku_latch = HIGH;
boolean gensoku_press = HIGH;
boolean gensoku_latch = HIGH;
boolean kasoku_accelaration_limit=false;
boolean ones = false;

unsigned int cul_speed = 0; // = current_speed/10 ゆっくりカウントのための一時的な変数
float current_speed = 0; // maxspeed - joutou unsignedだとダメ
unsigned int speed = 0;  // speed = cul_speed + joutou 最終出力PWM値 3pin に analogWriteされる

boolean teisha = false;
boolean speedismax = false;

boolean direction_button_press = HIGH;
boolean direction_button_latch = HIGH;
boolean direction_button_state = false;
boolean direction_state = false;

//DEMO
unsigned long press_start = 0;
unsigned long press_time=0;  //赤長押し検出用
boolean long_press = false;
boolean Auto_RUN=false;
boolean DEMO = false;
unsigned long current_time=0;
unsigned long previous_time=0;
unsigned long sec=0;



//関数

//PWM周波数設定 20kHz？（これを呼び出すと勝手に3pin analogWriteされる）
//呼び　analogWrite_ENABLE(speed);
void analogWrite_ENABLE(int _frq)//speed=current_kasoku+kasoku_Amount
{
  TCCR2A = 0b00100011;
  TCCR2B = 0b00001010;
  OCR2A = maxspeed;
  OCR2B = _frq;
}

//進行方向切替と緊急停止　黄色ボタン
/*
・ボタン4が押されたらdirection_state=true、分岐が2つ
  ☆speedが0以外の時  緊急停止
  ☆speedが0の時　方向反転
  ・current_speedを0にする
  ・1ms待つ
  ・dirを切り替える
  ・Nの状態にするteisha
  ・その後のボタン状態リセット関数でボタン状態リセット

*/
void get_direction_button() {
  direction_button_press = digitalRead(direction_button);

  if(direction_button_press==LOW && direction_button_latch==HIGH){
      direction_button_state=true;
        //  direction_button_latch=direction_button_press;//LL

      if(direction_button_state==true){ // 押された
        if (current_speed == 0) { //speed 0なら方向反転
          kasoku_count = 0;
          joutou = 0;
          current_speed=0;
          lcd.setCursor(12, 0);
          lcd.print("- ");
          delay(1500);
          direction_state = !direction_state; //方向反転
        } else if(current_speed > 0){  //緊急停止
          kasoku_count = 0;
          current_speed=0;
          lcd.setCursor(12, 0);
          lcd.print("! ");
          digitalWrite(ch1, LOW);
          digitalWrite(ch2, LOW);
          delay(1500);
        }
      }
  }
}

//進行方向ごとの出力設定
void get_direction_state(){
    switch (direction_state) {
      case false: //前進
        digitalWrite(ch1, HIGH);
        digitalWrite(ch2, LOW);
      break;

      case true:  //後退
        digitalWrite(ch1, LOW);
        digitalWrite(ch2, HIGH);
      break;
    }
}



//前照灯用常時出力の調整　volumeリードmap変換　呼び出し read_volume();
void get_map_joutou() {
  volume_val = analogRead(volume);
  joutou = map(volume_val, 0, 1024, 0, 256);
  if (joutou >= maxspeed) {
    joutou = maxspeed;
  }
}

//赤、緑 ボタン押し
void get_button_state() {
  //kasoku
  kasoku_press = digitalRead(kasoku_button);
  if (kasoku_press == LOW && kasoku_latch == HIGH) { //HH->LH
    kasoku_button_state = true;
    
  }
  kasoku_latch = kasoku_press; //LH->LL

  gensoku_press = digitalRead(gensoku_button);
  if (gensoku_press == LOW && gensoku_latch == HIGH) {
    gensoku_button_state = true;
    ones = true;
  }
  gensoku_latch = gensoku_press;
}

//赤ボタン離し
void get_button_release(){
  if (ones == true && gensoku_press == HIGH) {    
    ones=false;
    press_time = 0;
    press_start = 0;
  }
} 

//赤長押し
void get_long_press(){
  if(ones == true && gensoku_press == LOW){
    press_start=millis();
    gensoku_button_state = false;
  }
  press_time = millis()-press_start;
  
  if(press_time > 1500){
    long_press = true;
    gensoku_button_state = false;
    DEMO =! DEMO;
  }else{
    long_press=false;
  }
  

}


//ボタンリセット
void button_state_reset() {
//  gensoku_latch = gensoku_press;
  kasoku_button_state = false;
  gensoku_button_state = false;
  direction_button_latch=direction_button_press;
}

void DEMO_mode(){
  if(DEMO == true && ones==true){    
    kasoku_count = 0; //出力OFF
    current_speed=0;
    digitalWrite(ch1, LOW);
    digitalWrite(ch2, LOW);
    lcd.clear();

    lcd.clear();
    direction_state=false;  //前進を選択
    gensoku_button_state=false;
    press_time=0;
  }
    
  if(DEMO==true && long_press==true){
    lcd.setCursor(0, 0);
    lcd.print("    DEMO OFF    ");
    delay(2000);
    Auto_RUN=false;
  }
  if(DEMO==false && long_press==true){
    lcd.setCursor(0, 0);
    lcd.print("    DEMO ON     ");
    delay(2000);
    Auto_RUN=true;
  }
    

  //DEMOの内容
  if(Auto_RUN==true){

    if(kasoku_count == 0 && current_speed <= 0){
      kasoku_state=true;
      kasoku_count=1;
    }else if(kasoku_count >= 1 && kasoku_accelaration_limit==false){
      kasoku_count++;
    }
    
    if(kasoku_count > 2 ){
      kasoku_count=-4;  
    }
    if(kasoku_count==-4 && current_speed==0){
        kasoku_count=0;
      }

  }
}
//DEMOここまで




//ノッチ反応 ボタン押し回数カウントで増減 kasoku_count-4~+4
void get_kasoku_count() {
  if (kasoku_button_state == true) {
    if (kasoku_count < 0) {
      kasoku_count = 0; //減速中ならNに
    } else {
      kasoku_count++; //Nか加速中なら加速++
    }
  }

  //gonsoku
  if (gensoku_button_state == true) {
    if (kasoku_count > 0) {
      kasoku_count = 0; //加速中ならNに
    } else {
      kasoku_count--;//Nか減速中なら減速++
    }
  }

  //kasoku_count-4~+4で縛り
  if (kasoku_count > 4) {
    kasoku_count = 4;
  } else if (kasoku_count < -4) {
    kasoku_count = -4;
  }
}

//kasoku_Amount取得 (基本=kasoku_count)
int get_kasoku_Amount(int _kasoku_count) {
  return kasoku_Amount = _kasoku_count;
}

//ノッチ毎の加速上限設定
//上限到達で加速感停止
void kasoku_Amount_limit() {
  if(kasoku_count <= 0){
    kasoku_accelaration_limit=false;
  }
  switch (kasoku_count) {
    case 4:
       if (cul_speed >= (maxspeed - joutou) / 4 * 4) {
       kasoku_Amount = 0;
      }else{
        kasoku_accelaration_limit=true;
      }
      break;

    case 3:
      if (cul_speed >= (maxspeed - joutou) / 4 * 3) {
        kasoku_Amount = 0;
        kasoku_accelaration_limit=false;
      }else{
        kasoku_accelaration_limit=true;
      }
      break;

    case 2:
      if (cul_speed >= (maxspeed - joutou) / 4 * 2) {
        kasoku_Amount = 0;
        kasoku_accelaration_limit=false;
      }else{
        kasoku_accelaration_limit=true;
      }
      break;

    case 1:
      if (cul_speed >= (maxspeed - joutou) / 4 * 1) {
        kasoku_Amount = 0;
        kasoku_accelaration_limit=false;
      }else{
        kasoku_accelaration_limit=true;
      }
      break;
  }
}

//kasoku_state取得 戻り値=kasoku_count
int get_kasoku_state(int _kasoku_count) {
  return kasoku_state = _kasoku_count;
}

//current_speed(0~255)取得 kasoku_Amount値をループ毎に加算する
//加速感を出す //count=count^(zouka1+zouka2)*0.25みたいな?
//べき乗 ^の書き方　a = pow(10, 1.5); //pow（ベース,乗）;
void get_current_speed() {
  if(kasoku_accelaration_limit==false){
  current_speed = current_speed + kasoku_Amount;  //powなし
  }else{
    current_speed = pow(current_speed,(1+1*0.001));  //powあり
    current_speed = current_speed+ kasoku_Amount;
  }
}


//speed計算
void calculate_speed() {
  if (current_speed < 1 ) { //重要 速度0以下にしない
      current_speed = 0;
    }

  //maxspeedまでの範囲に固定する maxspeed（～255）- joutou（～255）がcurrent_speedという変動値
  if (current_speed > (maxspeed * 10) - (joutou * 10 )) {  //ゆっくりカウントアップのため全体を10倍
    current_speed = (maxspeed * 10) - (joutou * 10);  //10倍を元に戻す
  }
  cul_speed = (current_speed / 10);
  //if(cul_speed < 1){  //なくて平気
  //  cul_speed=0;
  //}
  //cul_speed++;
  speed = cul_speed + joutou;  //出力名
  //if(speed<1){  //なくて平気
  //  speed=0;
  //}
}


//関数  LCD表示　呼びfunc_lcd_print();
void lcd_print() {

    //cul_speed表示
    lcd.setCursor(0, 0);
    lcd.print(cul_speed);
    if (cul_speed < 10) {
      lcd.setCursor(1, 0);
      lcd.print("  ");
    } else if (cul_speed < 100) {
      lcd.setCursor(2, 0);
      lcd.print(" ");
    }

    //joutou表示
    lcd.setCursor(4, 0);
    lcd.print(joutou);
    if (joutou < 10) {
      lcd.setCursor(5, 0);
      lcd.print("  ");
    } else if (joutou < 100) {
      lcd.setCursor(6, 0);
      lcd.print(" ");
    }

    //speed表示
    lcd.setCursor(8, 0);
    lcd.print(speed);
    if (speed < 10) {
      lcd.setCursor(9, 0);
      lcd.print("  ");
    } else if (speed < 100) {
      lcd.setCursor(10, 0);
      lcd.print(" ");
    }

    //kasoku_count表示
    lcd.setCursor(14, 0);
    lcd.print(kasoku_count);
    if (kasoku_count >= 0) {
      lcd.setCursor(15, 0);
      lcd.print(" ");
    }

    //進行方向切替
    if (direction_state == true) {
      lcd.setCursor(12, 0);
      lcd.print("R ");
    } else {
      lcd.setCursor(12, 0);
      lcd.print("F ");
    }

    //kasoku_stateの文字表示

    if (kasoku_state > 0) {
      lcd.setCursor(0, 1);
      lcd.print("kasoku ");
    } else if (kasoku_state == 0 && current_speed > 0) {
      lcd.setCursor(0, 1);
      lcd.print("dakou  ");
    } else if (kasoku_state == 0 && current_speed <= 0) {
      lcd.setCursor(0, 1);
      lcd.print("N      ");
    } else if (kasoku_state < 0) {
      lcd.setCursor(0, 1);
      lcd.print("gensoku");
    }

    //speed
    if (current_speed <= 0) {
      lcd.setCursor(8, 1);
      lcd.print("teisha  ");
    } else if (speed >= maxspeed) {
      lcd.setCursor(8, 1);
      lcd.print("maxspeed");
    } else {
      lcd.setCursor(8, 1);
      lcd.print("shinkou  ");
    }

}


void setup() {
  Serial.begin(9600);
  pinMode(ENABLE, OUTPUT); //出力pin
  pinMode(ch1, OUTPUT);
  pinMode(ch2, OUTPUT);

  pinMode(direction_button, INPUT_PULLUP);
  pinMode(kasoku_button, INPUT_PULLUP);
  pinMode(gensoku_button, INPUT_PULLUP);

  lcd.init();
  lcd.backlight();
  lcd.clear();

}

//メイン　関数だけになった・・
void loop() {
  get_map_joutou();   //VR読み 常時出力の調整
  get_direction_button();   //黄色ボタン読み
  get_direction_state();    //進行方向セット
  get_button_state();   //緑赤ボタン読み
  get_kasoku_count();   //ノッチ変更
  get_kasoku_Amount(kasoku_count);    //加速加算値を設定
  get_kasoku_state(kasoku_count);   //加速状態を設定
  kasoku_Amount_limit();  //加速制限を設定
  get_current_speed();  //加速した速度とかの計算
  calculate_speed();  //ゆっくり加速、出力計算
  analogWrite_ENABLE(speed);  //3pin 出力 20kHz？
  lcd_print();  //LCD表示まとめ
get_long_press();
get_button_release();
DEMO_mode();

//DEMO mode
/*
kasoku_pressが2000msでDEMOmode
millis-kasoku_press>=2000
*/



  


  button_state_reset();   //ボタンリセット

  //メインここまで
    Serial.print(" millis:");
    Serial.print(millis());
    Serial.print(" START:");
    Serial.print(press_start);

    Serial.print(" press_T:");
    Serial.print(press_time);

    Serial.print(" long_press:");
    Serial.print(long_press);
    
    Serial.print(" DEMO:");
    Serial.print(DEMO);
    Serial.print(" Auto:");
    Serial.print(Auto_RUN);
    
/*
    Serial.print(" kasoku_press:");
    Serial.print(kasoku_press);
    Serial.print(" kasoku_latch:");
    Serial.print(kasoku_latch);
*/
/*
    Serial.print(" direction_button_state:");
    Serial.print(direction_button_state);

    Serial.print(" kasoku_count:");
    Serial.print(kasoku_count);
    Serial.print(" kasoku_state:");
    Serial.print(kasoku_state);
    Serial.print(" kasoku_Amount:");
    Serial.print(kasoku_Amount);
    Serial.print(" current_speed:");
    Serial.print(current_speed);
    Serial.print(" cul_speed:");
    Serial.print(cul_speed);
    Serial.print(" speed:");
    Serial.print(speed);
    Serial.print(" joutou:");
    Serial.print(joutou);
*/

Serial.println("");
}