/* オーディオ信号を128x64のOLEDにスペクトル表示 20220719_PiPicoFftAnalyzerV06.ino
  ボード:RP pico(RP2040)。ライブラリは ArduinoFFT.hは 1.5.6、Arduino:は 1.8.16
  ADCをDMAモードで使用し高速化、感度を-80dBに拡大
  
  2022/07/19 ラジオペンチ http://radiopench.blog96.fc2.com/
*/
#include <Arduino.h>
#include <U8g2lib.h>
#include <Wire.h>
#include "arduinoFFT.h"

#include "hardware/adc.h"
#include "hardware/dma.h"

// #define CLOCK_DIV 0
#define CAPTURE_CHANNEL0 0   // ADCポート番号
#define NSAMP 256            // サンプル数

dma_channel_config cfg;
uint dma_chan;
int dmaAdcSet = 0;

uint16_t cap_buf[NSAMP];   // データキャプチャバッファ

#define RT6150_PS   23  // Pico電源コンバーターPower Save Pin
#define BOARD_LED   25  // 基板内蔵LED
#define CHECK_PIN   16  // 動作タイミングチェックピン
#define SIG_IN      26  // 信号入力ピン
#define OF_LED      15  // 過大入力表示LEDピン
#define UP_SW        9  // レンジUpボタン
#define DN_SW       10  // レンジDownボタン
#define ADC_COMPE   14  // ADCエラッタパッチ選択（LOWなら補正無し）
#define PX2          0  // 画面(R)原点
#define PY1         16  // 波形画面の下端
#define PY2         52  // スペクトル画面の下端(-50db)
#define NNN        256  // FFTのサンプル数

arduinoFFT FFT = arduinoFFT();          // Create FFT object
//U8G2_SSD1306_128X64_NONAME_F_HW_I2C u8g2(U8G2_R0, /* reset=*/ U8X8_PIN_NONE);;  // 0.96inch OLED SSD1306 使用時に選択
U8G2_SH1106_128X64_NONAME_F_HW_I2C u8g2(U8G2_R0, /* reset=*/ U8X8_PIN_NONE);     // 1.3inch OLED SH1106 使用時に選択

double   vReal[NNN];       // FFTの計算領域（実際にはたぶん32ビット浮動小数点？）
double   vImag[NNN];       // イマジナリ値
uint16_t  wave[NNN];       // 波形の生データー
int16_t  peak[NNN / 2];    // スペクトルピーク
int16_t  adcOffset = 40;   // ADCのオフセット補正量 自動調整されるが初期値を指定すると収束が速くなる
uint16_t range = 8;        // レンジ番号(3:100Hz, 4:200Hz, 5:50Hz, 6:1k, 7:2k, 8:5k, 9:10k, 10:20k, 11:50k)
uint16_t dR = 1;           // ピークホールド減衰量

void setup() {
  pinMode(RT6150_PS, OUTPUT);    // 電源モード指定ピン
  pinMode(CHECK_PIN, OUTPUT);    // 実行時間測定用
  pinMode(BOARD_LED, OUTPUT);    // pico内蔵LED
  pinMode(OF_LED, OUTPUT);       // オーバーフロー表示LED
  pinMode(UP_SW, INPUT_PULLUP);  // UPボタン
  pinMode(DN_SW, INPUT_PULLUP);  // Downボタン
  pinMode(ADC_COMPE, INPUT_PULLUP); // ADC補正指定ピン（HIGHで補正有り）

  digitalWrite(RT6150_PS, HIGH); // 電源コンバーターをPWMモードに設定（ノイズ対策）
  Serial.begin(115200);          // Rp pico はシリアルの起動が失敗することがある
  //  while (!Serial) {
  //  }

  analogReadResolution(12);      // ADCのフルスケールを12ビットに設定

  u8g2.begin();                  // (I2Cバス400kbpsで開始される)
  u8g2.setFont(u8g2_font_6x10_tf);
  u8g2.setDrawColor(1);
  u8g2.setFontPosTop();          // 左上を文字位置とする
  u8g2.clearBuffer();
  u8g2.drawStr(0, 0, "Multi Range FFT v0.6");
  u8g2.sendBuffer();
  clearPeak();                   // ピーク値メモリーを初期化
  delay(1000);
}

void loop() {
  int olFlag;                       // 過大入力フラグ
  rangeSet();                       // ボタンが押されていたら周波数レンジを設定

  // 波形の読み取り
  digitalWrite(BOARD_LED, HIGH);    // ボード内蔵LED点灯
  digitalWrite(CHECK_PIN, HIGH);    // タイミング測定ピンHigh
  readWave();                       // 波形を読み取り
  digitalWrite(CHECK_PIN, LOW);     // タイミング測定ピンLow
  digitalWrite(BOARD_LED, LOW);     // ボード内蔵LED消灯

  // FFT計算データ準備(  ms)
  olFlag = 0;
  for (int i = 0; i < NNN; i++) {
    if ((wave[i] > 4050) || (wave[i] < 50)) {     // オーバーフロー警告のために値をチェック
      olFlag = 1;                                 // チェックに引っ掛かったらフラグ立てる
    }
    if (digitalRead(ADC_COMPE) == HIGH) {         // ADコンバーターの補正ジャンパー無し(GND接続無し)だったら
      wave[i] = adcCompe(wave[i]);                // ADCの非直線性(RP2040-E11)を補正
    }
    wave[i] += adcOffset;                         // オフセット補正を実施
    vReal[i] = (wave[i] - 2048) * 3.3 / 4096.0;   // 電圧の値に変換（ADCの補正でフルスケールが32増えているが、とりあえず無視）
    vImag[i] = 0;                                 // 虚数部はゼロ
  }
  digitalWrite(OF_LED, olFlag);                   // フラグに応じオーバーフローLEDを点灯

  // FFTの計算
//  FFT.Windowing(vReal, NNN, FFT_WIN_TYP_HAMMING, FFT_FORWARD); // 窓関数（ハミング）、ピークが鋭いがSN悪い(9ms)
  FFT.Windowing(vReal, NNN, FFT_WIN_TYP_HANN, FFT_FORWARD); // 窓関数（ハン窓）、SNが良い(9ms)
  FFT.Compute(vReal, vImag, NNN, FFT_FORWARD);    // FFTの計算(26ms)
  offsetAdj();                                    // オフセット補正量を計算
  FFT.ComplexToMagnitude(vReal, vImag, NNN);      // 絶対値の算出 (4.3ms)

  u8g2.clearBuffer();           // 画面バッファクリア       (22us)
  showWaveform();               // 波形表示                (0.72ms)
  showSpectrum();               // スペクトラム表示         (11～12ms)
  showBackGround();             // 目盛線等の背景を表示           (1.4ms)
  u8g2.sendBuffer();            // データーを転送して表示更新 (31ms)
  delay(1);                     // 書き込み不良対策のおまじない
}   // loop  (83～1000ms)

void rangeSet() {                     // レンジスイッチが押されていたらレンジ切り替え
  if (digitalRead(UP_SW) == LOW) {    // UPボタンが押されていたら
    digitalWrite(OF_LED, HIGH);       // 動作確認用にLED点灯
    range++;
    clearPeak();                      // レンジが変わったのでピークメモリーをクリア
    delay(30);
    if ( range > 13) range = 13;      // レンジ設定の上限
    dR = decayRate(range);            // レンジに応じたピーク値デケイ量を設定
  }
  while (digitalRead(UP_SW) == LOW) { // ボタンが離されるまで待つ
  }
  digitalWrite(OF_LED, LOW);

  if (digitalRead(DN_SW) == LOW) {    // DOWNボタンが押されていたら
    digitalWrite(OF_LED, HIGH);
    range--;
    clearPeak();
    delay(30);
    if (range < 3) range = 3;
    dR = decayRate(range);            // レンジに応じたピーク値デケイ量を設定
  }
  while (digitalRead(DN_SW) == LOW) { // ボタンが離されるまで待つ
  }
  digitalWrite(OF_LED, LOW);
}

void clearPeak() {                       // ピーク値メモリをゼロクリア
  for (int i = 0; i < (NNN / 2 ); i++) {
    peak[i] = 0;
  }
}

uint16_t decayRate(uint16_t d) {      // レンジに応じたディケイレートを設定
  uint16_t x;
  if (d >= 8)      x = 1;
  else if (d >= 6) x = 2;
  else if (d >= 4) x = 3;
  else             x = 4;
  return x;
}

void readWave() {                         // 該当するレンジの条件で波形を読み込む
  switch (range) {
    case 3:                               // 100Hzレンジ（実行時間：1000ms)
      dmaAdcSet = 0;                      // DMA準備完了フラグをクリア（analogReadを使用）
      for (int i = 0; i < NNN; i++) {
        wave[i] = analogRead(SIG_IN);     // 波形データー取得
        if (digitalRead(UP_SW) == LOW || digitalRead(DN_SW) == LOW) { // レンジボタンが押されていたら処理中断
          break;
        }
        delayMicroseconds(3886);          // サンプリング周期調整
      }
      break;
    case 4:                               // 200Hzレンジ（実行時間：500ms)
      dmaAdcSet = 0;
      for (int i = 0; i < NNN; i++) {
        wave[i] = analogRead(SIG_IN);     // 波形データー取得
        if (digitalRead(UP_SW) == LOW || digitalRead(DN_SW) == LOW) { // レンジボタンが押されていたら処理中断
          break;
        }
        delayMicroseconds(1942);          // サンプリング周期調整
      }
      break;
    case 5:                               // 500Hzレンジ（実行時間：200ms)
      dmaAdcSet = 0;
      for (int i = 0; i < NNN; i++) {
        wave[i] = analogRead(SIG_IN);     // 波形データー取得
        if (digitalRead(UP_SW) == LOW || digitalRead(DN_SW) == LOW) { // レンジボタンが押されていたら処理中断
          break;
        }
        delayMicroseconds(772);           // サンプリング周期調整
      }
      break;
    case 6:                               // 1kレンジ（実行時間：100ms)
      dmaAdcSet = 0;
      for (int i = 0; i < NNN; i++) {
        wave[i] = analogRead(SIG_IN);     // 波形データー取得
        delayMicroseconds(383);           // サンプリング周期調整
      }
      break;
    case 7:                               // 2kレンジ（実行時間：50ms)
      dmaPrepare();
      adcDma(9375.0);                     // DMA, 5.12kHzでサンプリング　48M/9375=5.12k
      break;
    case 8:                               // 5kレンジ（実行時間：20ms)
      dmaPrepare();
      adcDma(3750.0);                     // DMA, 12.8kHzでサンプリング　48M/3750=12.8k
      break;
    case 9:                               // 10kレンジ（実行時間：10ms)
      dmaPrepare();
      adcDma(1875.0);                     // DMA, 25.6kHzでサンプリング　48M/1875=25.6k
      break;
    case 10:                              // 20kレンジ（実行時間：5ms)
      dmaPrepare();
      adcDma(937.5);                      // DMA, 51.2kHzでサンプリング　48M/937.5=51.2k
      break;
    case 11:                              // 50kレンジ（実行時間：2ms)
      dmaPrepare();
      adcDma(375.0);                      // DMA, 128kHzでサンプリング　48M/375=128k
      break;
    case 12:                              // 100kレンジ（実行時間：1ms)
      dmaPrepare();
      adcDma(187.5);                      // DMA, 256kHzでサンプリング　48M/187.5=256kHz
      break;
    case 13:                              // 180kレンジ（実行時間：us)
      dmaPrepare();
      adcDma(104.1667);                   // DMA, 460.8kHzでサンプリング　48M/104.1667=460.8kkHz
      break;
    default:
      break;
  }
}

void dmaPrepare() {                   // DMAの準備
  if (dmaAdcSet == 0) {               // DMA初期化してなかったら、
    dmaadc_setup(0);                  // 初期化
    dmaAdcSet = 1;                    // 初期化フラグセット
  }
}

/*
void delay80ns(int t) {         // 引数*80ns+400nsくらい待つ
  volatile uint32_t x;          // 時間稼ぎ変数（最適化で削除されないようにvolatileで宣言）
  for (int i = 0; i < t; i++) {
    x++;                        // 32bitのインクリメントなので時間が余計にかかるはず
  }
}
*/

void offsetAdj() {              // ADCのオフセット補正
  if (vReal[0] > 0) {           // FFT結果の要素0(DC成分)が正なら
    adcOffset -= 1;             // オフセットマイナス
  } else {
    adcOffset += 1;             // プラス
  }
  Serial.println(adcOffset);    // 現在のオフセット値（ここで表示される値をadcOffsetの初期値に設定すると収束が速くなる）
}

int adcCompe(int x) {              // RP2040のADCの非直線性補正(RP2040-E11対策）
  int y;
  if      (x >= 3584) y = x + 32;  // フルスケールの7/8のポイントのオフセット補正
  else if (x == 3583) y = x + 29;  // 遷移のステップを少し埋める
  else if (x == 3582) y = x + 27;  // 同上

  else if (x >= 2560) y = x + 24;  // fsの5/8
  else if (x == 2559) y = x + 21;
  else if (x == 2558) y = x + 19;

  else if (x >= 1536) y = x + 16;  // fsの3/8
  else if (x == 1535) y = x + 13;
  else if (x == 1534) y = x + 11;

  else if (x >=  512) y = x +  8;  // fsの1/8
  else if (x ==  511) y = x +  5;
  else if (x ==  510) y = x +  3;
  else                y = x;       // 該当しなければそのままの値
  return y;
}

void showWaveform() {              // 入力波形を表示
  int last_y, new_y;
  u8g2.setDrawColor(1);            // 白で書く
  last_y = PY1 - (wave[0]) / 256;
  for (int i = 0; i < 254; i += 2) {
    new_y = PY1 - (wave[i + 2] / 256);
    u8g2.drawLine(PX2 + i / 2, last_y, PX2 + i / 2 + 1, new_y); // 波形プロット
    last_y = new_y;
  }
}

void showSpectrum() {                         // スペクトラム表示
  int d;
  u8g2.setDrawColor(1);                       // 白で書く
  for (int xi = 0; xi < 128; xi++) {          // スペクトラム表示
    d = barLength_80(vReal[xi]);
    u8g2.drawVLine(xi + PX2, PY2 - d, d);     // スペクトラムバー表示
    u8g2.drawPixel(xi + PX2, PY2 - peak[xi]); // ピーク値プロット
    if (peak[xi] < d) {                       // 現在値がピーク値以上だったら
      peak[xi] = d;                           // ピーク値を更新
    }
    peak[xi] -= dR;                           // ピーク値を指定量減衰させる
    if (peak[xi] < 0) {
      peak[xi] = 0;
    }
  }
}

int barLength_80(double d) {            // グラフのスペクトルの長さを計算 80dB版
  float fy;
  int y;
  fy = 8.0 * (log10(d) + 2.46344);      // 要調整　20dBで8画素 2VppをFFTすると68.889だったので、1ｍVのLog10の値で換算
  y = fy;
  y = constrain(y, 0, 56);
  return y;
}

/*
int barLength_60(double d) {            // グラフのスペクトルの長さを計算　60dB版
  float fy;
  int y;
  fy = 10.0 * (log10(d) + 1.46344);      // 20dBで10画素 2VppをFFTすると68.889だったので、0.1ｍVのLog10の値で換算
  y = fy;
  y = constrain(y, 0, 56);
  return y;
}
*/

void showBackGround() {                    // グラフの修飾（目盛他の作画）
  //　領域区分線
  u8g2.setDrawColor(1);                    // 白で書く
  u8g2.drawVLine(  0,  7, 4);              // 時間軸左端 縦線
  u8g2.drawVLine( 63,  7, 4);              // 時間軸1/2
  u8g2.drawVLine(127,  7, 4);              // 時間軸右端
  u8g2.drawHLine(PX2, PY2, 128);           // スペクトル下端線

  // 周波数目盛（下の横軸）
  for (int xp = PX2; xp < 127; xp += 10) { // 等間隔目盛
    u8g2.drawVLine(xp, PY2 + 1, 2);
  }
  u8g2.drawBox(PX2     , PY2 + 2, 2, 2);   // 0k太い目盛(2画素）
  u8g2.drawBox(PX2 + 49, PY2 + 2, 3, 2);   // 10k太い目盛(3画素）
  u8g2.drawBox(PX2 + 99, PY2 + 2, 3, 2);   // 20k太い目盛

  freqScale();                             // レンジに対応した周波数目盛を表示

  //　スペクトルレベル目盛（縦軸）
  u8g2.setDrawColor(2);                    // 重なっても見えるようにXORで書く
  for (int y = PY2 - 7; y > 16; y -= 8) {  // dB目盛線（横の点線）
    u8g2.drawHLine(0, y, 2);               // ゼロの傍の目盛
    for (int x = 9; x < 110; x += 10) {
      u8g2.drawHLine(x, y, 3);
    }
  }

  for (int y = PY2 - 7; y > 16; y -= 8) {  // (-60dB対応）交点マーク（+）の縦の線
    for (int x = 0; x < 110; x += 50) {
      u8g2.drawPixel(x, y - 1);            // 縦線では交点が消えるので点で描く
      u8g2.drawPixel(x, y + 1);
    }
  }
  u8g2.setFont(u8g2_font_micro_tr);        // 小さな3x5ドットフォントで、
  u8g2.setFontMode(0);
  u8g2.setDrawColor(1);
  u8g2.drawStr(117, 18, "0dB");            // スペクトル感度
  //  u8g2.drawStr(117, 26, "-20");
  u8g2.drawStr(117, 34, "-40");
  u8g2.drawStr(117, 45, "-80");
}

void freqScale() {                       // 周波数目盛表示
  u8g2.setFont(u8g2_font_mozart_nbp_tr); // 5x7ドットフォント
  u8g2.drawStr( 0, 56, "0");             // 原点の0を表示
  switch (range) {                       // レンジ番号に応じた周波数を表示
    case 3:
      u8g2.drawStr(116, -1,   "1s");     // サンプリング期間
      u8g2.drawStr(45, 56,    "50");     // 1/2周波数
      u8g2.drawStr(92, 56,   "100");     // フルスケール周波数
      break;
    case 4:
      u8g2.drawStr(104, -1, "0.5s");
      u8g2.drawStr(42, 56,   "100");
      u8g2.drawStr(92, 56,   "200");
      break;
    case 5:
      u8g2.drawStr(98, -1, "200ms");
      u8g2.drawStr(42, 56,   "250");
      u8g2.drawStr(92, 56,   "500");
      break;
    case 6:
      u8g2.drawStr(98, -1, "100ms");
      u8g2.drawStr(42, 56,   "500");
      u8g2.drawStr(95, 56,    "1k");
      break;
    case 7:
      u8g2.drawStr(104, -1, "50ms");
      u8g2.drawStr(45, 56,    "1k");
      u8g2.drawStr(95, 56,    "2k");
      break;
    case 8:
      u8g2.drawStr(104, -1, "20ms");
      u8g2.drawStr(41, 56,  "2.5k");
      u8g2.drawStr(95, 56,    "5k");
      break;
    case 9:
      u8g2.drawStr(104, -1, "10ms");
      u8g2.drawStr(45, 56,    "5k");
      u8g2.drawStr(92, 56,   "10k");
      break;
    case 10:
      u8g2.drawStr(110, -1, "5ms");
      u8g2.drawStr(42, 56,  "10k");
      u8g2.drawStr(92, 56,  "20k");
      break;
    case 11:
      u8g2.drawStr(110, -1, "2ms");
      u8g2.drawStr(42, 56,  "25k");
      u8g2.drawStr(92, 56,  "50k");
      break;
    case 12:
      u8g2.drawStr(110, -1, "1ms");
      u8g2.drawStr(42, 56,  "50k");
      u8g2.drawStr(89, 56, "100k");
      break;
    case 13:
      u8g2.drawStr(98, -1, "461us");
      u8g2.drawStr(42, 56,   "90k");
      u8g2.drawStr(89, 56,  "180k");  // (460.8/2)*100/128=180
      break;
    default:
      break;
  }
}

void adcDma(float dv) {      // 指定速度でDMAモードでADC
  adc_select_input(0);       // GPIO26(A0)から入力
  adc_set_clkdiv(dv);        // サンプリング速度を設定
  sample_dma(wave);          // DMAモードでサンプリング、結果をwaveに書き込み
}

void sample_dma(uint16_t *capture_buf) { // DMAでサンプリング実行
  dma_channel_configure(dma_chan, &cfg,
                        capture_buf,     // dst
                        &adc_hw->fifo,   // src
                        NSAMP,           // transfer count
                        true             // start immediately
                       );
  adc_run(true);
  dma_channel_wait_for_finish_blocking(dma_chan);
  adc_run(false);
  adc_fifo_drain();
}

void dmaadc_setup(float clock_div) {        // ADCをDMAで使うための設定
  adc_gpio_init(26 + CAPTURE_CHANNEL0);
  adc_init();
  adc_select_input(CAPTURE_CHANNEL0);
  adc_fifo_setup(
    true,    // Write each completed conversion to the sample FIFO
    true,    // Enable DMA data request (DREQ)
    1,       // DREQ (and IRQ) asserted when at least 1 sample present
    false,   // We won't see the ERR bit because of 8 bit reads; disable.
    false    // do not Shift each sample to 8 bits when pushing to FIFO
  );

  adc_set_clkdiv(clock_div);    // サンプルレート設定(仮設定）

  //    sleep_ms(1000);     // 意味を調査中 こんなに待たなくても良いのでは？
  sleep_ms(50);             // 仮に50msに設定して様子を見る（10msはダメ）
  // Set up the DMA to start transferring data as soon as it appears in FIFO
  uint dma_chan = dma_claim_unused_channel(true);
  cfg = dma_channel_get_default_config(dma_chan);

  // Reading from constant address, writing to incrementing byte addresses
  channel_config_set_transfer_data_size(&cfg, DMA_SIZE_16);
  channel_config_set_read_increment(&cfg, false);
  channel_config_set_write_increment(&cfg, true);

  // Pace transfers based on availability of ADC samples
  channel_config_set_dreq(&cfg, DREQ_ADC);
}