#include <M5Core2.h>
#include <driver/i2s.h>

// ========== スピーカー(I2S)設定 ==========
#define SPK_I2S_NUM        I2S_NUM_0
#define SPK_BCK_PIN        12
#define SPK_LRCK_PIN       0
#define SPK_DATA_PIN       2
#define SPK_DATA_IN_PIN    34
const int SAMPLE_RATE = 44100;

bool speakerReady   = false;
bool ngBeepActive   = false;
int  ngHalfPeriod   = 0;
int16_t ngLevel     = 22000;
int  ngCounter      = 0;

// I2S初期化
bool initSpeaker() {
  i2s_driver_uninstall(SPK_I2S_NUM);

  i2s_config_t cfg = {};
  cfg.mode = (i2s_mode_t)(I2S_MODE_MASTER | I2S_MODE_TX);
  cfg.sample_rate = SAMPLE_RATE;
  cfg.bits_per_sample = I2S_BITS_PER_SAMPLE_16BIT;
  cfg.channel_format = I2S_CHANNEL_FMT_ONLY_RIGHT;
  cfg.communication_format = I2S_COMM_FORMAT_I2S;
  cfg.intr_alloc_flags = ESP_INTR_FLAG_LEVEL1;
  cfg.dma_buf_count = 2;
  cfg.dma_buf_len = 128;
  cfg.use_apll = false;
  cfg.tx_desc_auto_clear = true;

  if (i2s_driver_install(SPK_I2S_NUM, &cfg, 0, NULL) != ESP_OK) return false;

  i2s_pin_config_t pins = {};
  pins.bck_io_num   = SPK_BCK_PIN;
  pins.ws_io_num    = SPK_LRCK_PIN;
  pins.data_out_num = SPK_DATA_PIN;
  pins.data_in_num  = SPK_DATA_IN_PIN;

  if (i2s_set_pin(SPK_I2S_NUM, &pins) != ESP_OK) return false;
  if (i2s_set_clk(SPK_I2S_NUM, SAMPLE_RATE, I2S_BITS_PER_SAMPLE_16BIT, I2S_CHANNEL_MONO) != ESP_OK) return false;

  return true;
}

// OK用 単発ビープ
void beepOnce(int freq, int ms) {
  if (!speakerReady) speakerReady = initSpeaker();
  if (!speakerReady) return;

  const int BUF = 256;
  int16_t buf[BUF];

  int total = SAMPLE_RATE * ms / 1000;
  int halfP = SAMPLE_RATE / (freq * 2);
  if (halfP < 1) halfP = 1;

  int16_t level = 22000;
  int cnt = 0;

  while (total > 0) {
    int n = (total > BUF) ? BUF : total;
    for (int i = 0; i < n; ++i) {
      buf[i] = level;
      cnt++;
      if (cnt >= halfP) {
        cnt = 0;
        level = -level;
      }
    }
    size_t written = 0;
    i2s_write(SPK_I2S_NUM, buf, n * sizeof(int16_t), &written, portMAX_DELAY);
    total -= n;
  }
}

// NG用 連続トーン
void feedNgTone(int freq) {
  if (!ngBeepActive) return;
  if (!speakerReady) speakerReady = initSpeaker();
  if (!speakerReady) return;

  if (ngHalfPeriod == 0) {
    ngHalfPeriod = SAMPLE_RATE / (freq * 2);
    if (ngHalfPeriod < 1) ngHalfPeriod = 1;
    ngCounter = 0;
    ngLevel   = 22000;
  }

  const int BUF = 256;
  int16_t buf[BUF];

  for (int i = 0; i < BUF; ++i) {
    buf[i] = ngLevel;
    ngCounter++;
    if (ngCounter >= ngHalfPeriod) {
      ngCounter = 0;
      ngLevel = -ngLevel;
    }
  }

  size_t written = 0;
  i2s_write(SPK_I2S_NUM, buf, BUF * sizeof(int16_t), &written, portMAX_DELAY);
}

// ========== GPIO割り当て ==========
const int PIN_SOL_RELEASE = 32;  // 復帰
const int PIN_SOL_PULLIN  = 33;  // 吸引
const int PIN_RESET_SW    = 13;  // リセット
const int PIN_START_SW    = 22;  // スタート
const int PIN_STATE_SW    = 27;  // 状態検出

// ========== テストタイミング ==========
const unsigned long RELEASE_MS = 1000;
const unsigned long PULL_MS    = 1000;

// ========== 状態管理 ==========
bool testing      = false;
unsigned long testStartMs = 0;
bool initialOn    = false;
bool sawOff       = false;
bool sawOnAgain   = false;
bool prevStateOn  = false;
bool prevStartOn  = false;
int  lastResult   = 0;   // 0:未判定  1:OK  2:NG
int  currentMode  = -1;  // 0:READY 1:TEST 2:OK 3:NG

// ========== ヘルパ ==========
void setSolenoid(bool rel, bool pull) {
  digitalWrite(PIN_SOL_RELEASE, rel ? HIGH : LOW);
  digitalWrite(PIN_SOL_PULLIN,  pull ? HIGH : LOW);
}
void allOff() { setSolenoid(false, false); }

uint16_t modeColor(int mode) {
  switch (mode) {
    case 0: return DARKGREY; // READY
    case 1: return BLUE;     // TEST
    case 2: return GREEN;    // OK
    case 3: return RED;      // NG
  }
  return BLACK;
}

// 画面描画（モードが変わったときだけ）
void drawScreen(int mode) {
  M5.Lcd.fillScreen(modeColor(mode));
  M5.Lcd.setTextColor(WHITE);
  M5.Lcd.setTextSize(7);

  const char* txt =
    (mode == 0) ? "READY" :
    (mode == 1) ? "TEST"  :
    (mode == 2) ? "OK"    :
                  "NG";

  int16_t w = M5.Lcd.textWidth(txt);
  int16_t h = 7 * 8;
  int16_t x = (320 - w) / 2;
  int16_t y = (240 - h) / 2;

  M5.Lcd.setCursor(x, y);
  M5.Lcd.print(txt);

  currentMode = mode;
}

// リセット共通処理
void resetAll() {
  testing      = false;
  lastResult   = 0;
  initialOn    = false;
  sawOff       = false;
  sawOnAgain   = false;
  prevStateOn  = false;
  ngBeepActive = false;
  allOff();
  currentMode = -1;  // 次のループで必ず再描画
}

// ========== setup ==========
void setup() {
  M5.begin();
  M5.Axp.SetSpkEnable(true);

  pinMode(PIN_SOL_RELEASE, OUTPUT);
  pinMode(PIN_SOL_PULLIN,  OUTPUT);
  pinMode(PIN_RESET_SW,    INPUT_PULLUP);
  pinMode(PIN_START_SW,    INPUT_PULLUP);
  pinMode(PIN_STATE_SW,    INPUT_PULLUP);

  resetAll();
}

// ========== loop ==========
void loop() {
  M5.update();

  bool resetOn = (digitalRead(PIN_RESET_SW) == LOW);
  bool startOn = (digitalRead(PIN_START_SW) == LOW);
  bool stateOn = (digitalRead(PIN_STATE_SW) == HIGH);

  // RESET 最優先
  if (resetOn) {
    resetAll();
    drawScreen(0);   // READY
    delay(50);
    return;
  }

  // START 立ち上がりでテスト開始
  if (!testing && startOn && !prevStartOn) {
    testing      = true;
    testStartMs  = millis();
    lastResult   = 0;

    initialOn    = stateOn;
    sawOff       = false;
    sawOnAgain   = false;
    prevStateOn  = stateOn;

    setSolenoid(true, false);  // まず復帰側ON
  }
  prevStartOn = startOn;

  int mode = 0; // 0:READY

  if (testing) {
    unsigned long el = millis() - testStartMs;

    if (el < RELEASE_MS) {
      mode = 1;  // TEST
      setSolenoid(true, false);   // 復帰
    } else if (el < RELEASE_MS + PULL_MS) {
      mode = 1;  // TEST
      setSolenoid(false, true);   // 吸引
    } else {
      // テスト終了
      testing = false;
      allOff();

      if (initialOn && sawOff && sawOnAgain) {
        lastResult = 1;
        beepOnce(3000, 120);   // OKピッ
      } else {
        lastResult = 2;
        ngBeepActive = true;   // NGピーーー開始
        ngHalfPeriod = 0;      // 初期化
      }
    }

    // ON→OFF→ON の検出
    if (stateOn != prevStateOn) {
      if (!stateOn && prevStateOn)           sawOff = true;
      if (stateOn && !prevStateOn && sawOff) sawOnAgain = true;
      prevStateOn = stateOn;
    }
  }

  if (!testing && lastResult == 1) mode = 2; // OK
  if (!testing && lastResult == 2) mode = 3; // NG

  // モードが変わったら画面更新
  if (mode != currentMode) {
    drawScreen(mode);
  }

  // NG中は連続トーンを送り続ける
  if (ngBeepActive) {
    feedNgTone(3000);   // 3kHz ピーーー
  } else {
    delay(10);          // 通常時だけ軽くウェイト
  }
}
