/** 
 * FFTで周波数特性
 * https://101010.fun/iot/m5stickc-plus-accel.html
 * を改変
 */

#include <M5StickCPlus.h>
#include <arduinoFFT.h>

#define SAMPLE_PERIOD 10    // サンプリング間隔(mS)

const uint16_t FFTsamples = 64; //This value MUST ALWAYS be a power of 2
double vReal[FFTsamples];
double vImag[FFTsamples];
const double samplingFrequency = 1000.0 / (double)SAMPLE_PERIOD;
ArduinoFFT<double> FFT = ArduinoFFT<double>(vReal, vImag, FFTsamples, samplingFrequency);

int Y0 = 15;
int _height = 135 - Y0;
int _width = 240;
float dmax = 1000.0; // Sensitive

void drawChart(int nsamples) {
    int band_width = floor(_width / nsamples);
    int band_pad = band_width - 1;

    for (int band = 0; band < nsamples; band++) {
        int hpos = band * band_width;
        float d = vReal[band];
        if (d > dmax) d = dmax;
        int h = (int)((d / dmax) * (_height) * 0.9);
        M5.Lcd.fillRect(hpos, _height - h, band_pad, h, BLACK);
        if ((band % 16) == 0) {
            M5.Lcd.setCursor(hpos, _height);
            M5.Lcd.printf("%dHz", (int)((band * 1.0 * samplingFrequency) / FFTsamples));
        }
    }
}

void setup() {
  M5.begin();
//   M5.Axp.ScreenBreath(24);
  M5.Lcd.setRotation(3);
  M5.Lcd.fillScreen(WHITE);
  M5.Lcd.setTextColor(BLACK);
  M5.Lcd.setTextSize(2);
  M5.IMU.Init();
  M5.IMU.SetAccelFsr(M5.IMU.AFS_4G);
}

void DCRemoval(double *vData, uint16_t samples) {
    double mean = 0;
    for (uint16_t i = 0; i < samples; i++) {
        mean += vData[i];
    }
    mean /= samples;
    for (uint16_t i = 0; i < samples; i++) {
        vData[i] -= mean;
    }
}

void loop() {
  for (int i = 0; i < FFTsamples; i++) {
    float ax, ay, az; // 画面短軸方向がx 長軸方向がy 画面に対して垂直がz
    long t = micros();
    M5.IMU.getAccelData(&ax,&ay,&az);  // MPU6886から加速度を取得
    vReal[i] = az * 1000;  // mGに変換
    vImag[i] = 0;
    delayMicroseconds(SAMPLE_PERIOD * 1000 - (micros() - t));
  }

  DCRemoval(vReal, FFTsamples);  // 直流分を除去
  FFT.windowing(FFT_WIN_TYP_HAMMING, FFT_FORWARD);  // 窓関数
  FFT.compute(FFT_FORWARD); // FFT処理(複素数で計算)
  FFT.complexToMagnitude(); // 複素数を実数に変換
  double x = FFT.majorPeak();

  M5.Lcd.fillScreen(WHITE);  // 画面をクリア
  drawChart(FFTsamples / 2);
  M5.Lcd.setCursor(40, 0);
  M5.Lcd.printf("Peak: %3.1fHz", x);
}