/* Spectrum display of audio signal on 128x64 OLED 20220808_PiPicoFftAnalyzerV062.ino
  Changed pin assignment to make implementation easier
  Board: RP pico (RP2040). Library ArduinoFFT.h is 1.5.6, Arduino: 1.8.16
  Use ADC in DMA mode to increase speed and sensitivity to -80dB
  2022/08/08 Radio pliers 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 CAPTURE_CHANNEL0 0 // ADC port number

dma_channel_config cfg;
uint dma_chan;
int dmaAdcSet = 0;

#define RT6150_PS 23 // Pico power converter Power Save Pin
#define BOARD_LED 25 // Board built-in LED
#define CHECK_PIN 16 // Operation timing check pin
#define SIG_IN 26 // Signal input pin
#define OF_LED 6 // Excessive input display LED pin (changed from 15 to 6)
#define UP_SW 3 // Range Up button (changed from 9 to 3)
#define DN_SW 2 // Range Down button (changed from 10 to 2)
#define ADC_COMPE 14 // ADC errata patch selection (no correction at LOW)

#define PX2 0 // Screen (R) origin
#define PY1 16 // Bottom edge of waveform screen
#define PY2 52 // Lower end of spectrum screen (-50db)
#define NNN 256 // Number of samples for FFT

arduinoFFT FFT = arduinoFFT(); // Create FFT object
//U8G2_SSD1306_128X64_NONAME_F_HW_I2C u8g2(U8G2_R0, /* reset=*/ U8X8_PIN_NONE);; // Select when using 0.96inch OLED SSD1306
U8G2_SH1106_128X64_NONAME_F_HW_I2C u8g2(U8G2_R0, /* reset=*/ U8X8_PIN_NONE); // Select when using 1.3inch OLED SH1106

double vReal[NNN]; // FFT calculation area (actually maybe 32-bit floating point?)
double vImag[NNN]; // Imaginary value
uint16_t wave[NNN]; // Raw waveform data
int16_t peak[NNN / 2]; // Spectrum peak
int16_t adcOffset = 40; // ADC offset correction amount Automatically adjusted, but convergence will be faster if you specify the initial value
uint16_t range = 8; // Range number (3:100Hz, 4:200Hz, 5:50Hz, 6:1k, 7:2k, 8:5k, 9:10k, 10:20k, 11:50k, 12:100k, 13:180k)
uint16_t dR = 1; // Values ​​that vary depending on peak hold attenuation, decay rate, and range

void setup() {
  pinMode(RT6150_PS, OUTPUT); // Power mode specification pin
  pinMode(CHECK_PIN, OUTPUT); // For measuring execution time
  pinMode(BOARD_LED, OUTPUT); // pico built-in LED
  pinMode(OF_LED, OUTPUT); // Overflow indication LED
  pinMode(UP_SW, INPUT_PULLUP); // UP button
  pinMode(DN_SW, INPUT_PULLUP); // Down button
  pinMode(ADC_COMPE, INPUT_PULLUP); // ADC correction specification pin (corrected when HIGH)

  digitalWrite(RT6150_PS, HIGH); // Set the power converter to PWM mode (noise prevention)
  Serial.begin(115200); // Rp pico may fail to start serial
  // while (!Serial) {
  // }

  analogReadResolution(12); // Set ADC full scale to 12 bits

  u8g2.begin(); // (starts with I2C bus 400kbps)
  u8g2.setFont(u8g2_font_6x10_tf);
  u8g2.setDrawColor(1);
  u8g2.setFontPosTop(); // Set top left as character position
  u8g2.clearBuffer();
  u8g2.drawStr(0, 0, "FFT Analyzer v1.0");
  u8g2.sendBuffer();
  clearPeak(); // Initialize peak value memory
  delay(1000);
}
printf("start")
void loop() {
  int olFlag; // Excessive input flag
  rangeSet(); // Set the frequency range if the button is pressed

  // read waveform
  digitalWrite(BOARD_LED, HIGH); // Lights up the board's built-in LED
  digitalWrite(CHECK_PIN, HIGH); // Timing measurement pin High
  readWave(); // Read the waveform (behavior changes when excessive input occurs with DMA)
  digitalWrite(CHECK_PIN, LOW); // Timing measurement pin Low
  digitalWrite(BOARD_LED, LOW); // Turn off the board's built-in LED

  // FFT calculation data preparation (ms)
  olFlag = 0;
  for (int i = 0; i < NNN; i++) {
    if ((wave[i] > 4050) || (wave[i] < 50)) { // Check value for overflow warning
      olFlag = 1; // Set a flag if the check fails
    }
    if (digitalRead(ADC_COMPE) == HIGH) { // If there is no correction jumper for AD converter (no GND connection)
      wave[i] = adcCompe(wave[i]); // Correct ADC nonlinearity (RP2040-E11)
    }
    wave[i] += adcOffset; // Perform offset correction
    vReal[i] = (wave[i] - 2048) * 3.3 / 4096.0; // Convert to voltage value (full scale has increased by 32 due to ADC correction, but ignore it for now)
    vImag[i] = 0; // Imaginary part is zero
  }
  digitalWrite(OF_LED, olFlag); // Lights the overflow LED according to the flag

  // FFT calculation
// FFT.Windowing(vReal, NNN, FFT_WIN_TYP_HAMMING, FFT_FORWARD); // Window function (hamming), sharp peak but poor SN (9ms)
  FFT.Windowing(vReal, NNN, FFT_WIN_TYP_HANN, FFT_FORWARD); // Window function (Han window), good SN (9ms)
  FFT.Compute(vReal, vImag, NNN, FFT_FORWARD); // FFT calculation (26ms)
  offsetAdj(); // Calculate offset correction amount
  FFT.ComplexToMagnitude(vReal, vImag, NNN); // Calculate absolute value (4.3ms)

  u8g2.clearBuffer(); // Clear screen buffer (22us)
  showWaveform(); // Waveform display (0.72ms)
  showSpectrum(); // Spectrum display (11~12ms)
  showBackGround(); // Display background such as grid lines (1.4ms)
  u8g2.sendBuffer(); // Transfer data and update display (31ms)
  delay(1); // Magic to prevent write failures
} // loop (83~1000ms)

void rangeSet() { // Switch range if range switch is pressed
  if (digitalRead(UP_SW) == LOW) { // If the UP button is pressed
    digitalWrite(OF_LED, HIGH); // Turn on the LED to confirm operation
    range++;
    clearPeak(); // Since the range has changed, clear the peak memory
    delay(30);
    if (range > 13) range = 13; // upper limit of range setting
    dR = decayRate(range); // Set the peak value decay amount according to the range
  }
  while (digitalRead(UP_SW) == LOW) { // Wait until button is released
  }
  digitalWrite(OF_LED, LOW);

  if (digitalRead(DN_SW) == LOW) { // If the DOWN button is pressed
    digitalWrite(OF_LED, HIGH);
    range--;
    clearPeak();
    delay(30);
    if (range < 3) range = 3;
    dR = decayRate(range); // Set the peak value decay amount according to the range
  }
  while (digitalRead(DN_SW) == LOW) { // Wait until button is released
  }
  digitalWrite(OF_LED, LOW);
}

void clearPeak() { // Clear peak value memory to zero
  for (int i = 0; i < (NNN / 2 ); i++) {
    peak[i] = 0;
  }
}

uint16_t decayRate(uint16_t d) { // Set decay rate according to range
  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() { // Read the waveform under the applicable range conditions
  switch (range) {
    case 3: // 100Hz range (execution time: 1000ms)
      dmaAdcSet = 0; // Clear DMA ready flag (using analogRead)
      for (int i = 0; i < NNN; i++) {
        wave[i] = analogRead(SIG_IN); // Get waveform data
        if (digitalRead(UP_SW) == LOW || digitalRead(DN_SW) == LOW) { // Interrupt processing if the range button is pressed
          break;
        }
        delayMicroseconds(3886); // Sampling period adjustment
      }
      break;
    case 4: // 200Hz range (execution time: 500ms)
      dmaAdcSet = 0;
      for (int i = 0; i < NNN; i++) {
        wave[i] = analogRead(SIG_IN); // Get waveform data
        if (digitalRead(UP_SW) == LOW || digitalRead(DN_SW) == LOW) { // Interrupt processing if the range button is pressed
          break;
        }
        delayMicroseconds(1942); // Sampling period adjustment
      }
      break;
    case 5: // 500Hz range (execution time: 200ms)
      dmaAdcSet = 0;
      for (int i = 0; i < NNN; i++) {
        wave[i] = analogRead(SIG_IN); // Get waveform data
        if (digitalRead(UP_SW) == LOW || digitalRead(DN_SW) == LOW) { // Interrupt processing if the range button is pressed
          break;
        }
        delayMicroseconds(772); // Sampling period adjustment
      }
      break;
    case 6: // 1k range (execution time: 100ms)
      dmaAdcSet = 0;
      for (int i = 0; i < NNN; i++) {
        wave[i] = analogRead(SIG_IN); // Get waveform data
        delayMicroseconds(383); // Sampling period adjustment
      }
      break;
    case 7: // 2k range (execution time: 50ms)
      dmaPrepare();
      adcDma(9375.0); // DMA, sampled at 5.12kHz 48M/9375=5.12k
      break;
    case 8: // 5k range (execution time: 20ms)
      dmaPrepare();
      adcDma(3750.0); // DMA, sampled at 12.8kHz 48M/3750=12.8k
      break;
    case 9: // 10k range (execution time: 10ms)
      dmaPrepare();
      adcDma(1875.0); // DMA, sampled at 25.6kHz 48M/1875=25.6k
      break;
    case 10: // 20k range (execution time: 5ms)
      dmaPrepare();
      adcDma(937.5); // DMA, sampled at 51.2kHz 48M/937.5=51.2k
      break;
    case 11: // 50k range (execution time: 2ms)
      dmaPrepare();
      adcDma(375.0); // DMA, sampled at 128kHz 48M/375=128k
      break;
    case 12: // 100k range (execution time: 1ms)
      dmaPrepare();
      adcDma(187.5); // DMA, sampled at 256kHz 48M/187.5=256kHz
      break;
    case 13: // 180k range (execution time: us)
      dmaPrepare();
      adcDma(104.1667); // DMA, sampled at 460.8kHz 48M/104.1667=460.8kkHz
      break;
    default:
      break;
  }
}

void dmaPrepare() { // Prepare DMA
  if (dmaAdcSet == 0) { // If DMA has not been initialized,
    dmaadc_setup(0); // Initialization
    dmaAdcSet = 1; // Initialization flag set
  }
}

/*
void delay80ns(int t) { // Wait for about argument*80ns+400ns
  volatile uint32_t x; // Time-buying variable (declared as volatile to avoid being removed by optimization)
  for (int i = 0; i < t; i++) {
    x++; // Since it is a 32-bit increment, it should take more time
  }
}
*/

void offsetAdj() { // ADC offset correction
  if (vReal[0] > 0) { // If element 0 (DC component) of the FFT result is positive
    adcOffset -= 1; // Offset minus
  } else {
    adcOffset += 1; // plus
  }
  Serial.println(adcOffset); // Current offset value (setting the value displayed here as the initial value of adcOffset will speed up convergence)
}

int adcCompe(int x) { // RP2040 ADC nonlinearity correction (RP2040-E11 countermeasure)
  int y;
  if (x >= 3584) y = x + 32; // Offset correction for 7/8 point of full scale
  else if (x == 3583) y = x + 29; // Fill in the transition steps a little
  else if (x == 3582) y = x + 27; // Same as above

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

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

  else if (x >= 512) y = x + 8; // 1/8 of fs
  else if (x == 511) y = x + 5;
  else if (x == 510) y = x + 3;
  else y = x; // If not applicable, leave as is
  return y;
}

void showWaveform() { // Display input waveform
  int last_y, new_y;
  u8g2.setDrawColor(1); // Draw in white
  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); // Waveform plot
    last_y = new_y;
  }
}

void showSpectrum() { // Spectrum display
  int d;
  u8g2.setDrawColor(1); // Draw in white
  for (int xi = 0; xi < 128; xi++) { // Spectrum display
    d = barLength_80(vReal[xi]);
    u8g2.drawVLine(xi + PX2, PY2 - d, d); // Spectrum bar display
    u8g2.drawPixel(xi + PX2, PY2 - peak[xi]); // Peak value plot
    if (peak[xi] < d) { // If the current value is greater than or equal to the peak value
      peak[xi] = d; // update peak value
    }
    peak[xi] -= dR; // Attenuate the peak value by the specified amount
    if (peak[xi] < 0) {
      peak[xi] = 0;
    }
  }
}

int barLength_80(double d) { // Calculate the length of the spectrum of the graph 80dB version
  float fy;
  int y;
  fy = 8.0 * (log10(d) + 2.46344); // Adjustment required 8 pixels at 20dB When 2Vpp was FFTed, it was 68.889, so it was converted to the Log10 value of 1mV
  y = fy;
  y = constrain(y, 0, 56);
  return y;
}

/*
int barLength_60(double d) { // Calculate the length of the spectrum of the graph 60dB version
  float fy;
  int y;
  fy = 10.0 * (log10(d) + 1.46344); // 10 pixels at 20dB When 2Vpp was FFTed, it was 68.889, so it was converted to the Log10 value of 0.1mV
  y = fy;
  y = constrain(y, 0, 56);
  return y;
}
*/

void showBackGround() { // Graph decoration (drawing scales, etc.)
  // area dividing line
  u8g2.setDrawColor(1); // Draw in white
  u8g2.drawVLine( 0, 7, 4); // Vertical line at left end of time axis
  u8g2.drawVLine( 63, 7, 4); // Time axis 1/2
  u8g2.drawVLine(127, 7, 4); // Right end of time axis
  u8g2.drawHLine(PX2, PY2, 128); // Lower spectrum line

  // Frequency scale (bottom horizontal axis)
  for (int xp = PX2; xp < 127; xp += 10) { // Evenly spaced scale
    u8g2.drawVLine(xp, PY2 + 1, 2);
  }
  u8g2.drawBox(PX2, PY2 + 2, 2, 2); // 0k thick scale (2 pixels)
  u8g2.drawBox(PX2 + 49, PY2 + 2, 3, 2); // 10k thick scale (3 pixels)
  u8g2.drawBox(PX2 + 99, PY2 + 2, 3, 2); // 20k thick scale

  freqScale(); // Display the frequency scale corresponding to the range

  // Spectral level scale (vertical axis)
  u8g2.setDrawColor(2); // Write using XOR so that it can be seen even if it overlaps
  for (int y = PY2 - 7; y > 16; y -= 8) { // dB scale line (horizontal dotted line)
    u8g2.drawHLine(0, y, 2); // Tickmark near zero
    for (int x = 9; x < 110; x += 10) {
      u8g2.drawHLine(x, y, 3);
    }
  }

  for (int y = PY2 - 7; y > 16; y -= 8) { // (-60dB compatible) Vertical line with intersection mark (+)
    for (int x = 0; x < 110; x += 50) {
      u8g2.drawPixel(x, y - 1); // For vertical lines, the intersection points disappear, so draw them as points
      u8g2.drawPixel(x, y + 1);
    }
  }
  u8g2.setFont(u8g2_font_micro_tr); // Small 3x5 dot font,
  u8g2.setFontMode(0);
  u8g2.setDrawColor(1);
  u8g2.drawStr(117, 18, "0dB"); // Spectral sensitivity
  // u8g2.drawStr(117, 26, "-20");
  u8g2.drawStr(117, 34, "-40");
  u8g2.drawStr(117, 45, "-80");
}

void freqScale() { // Frequency scale display
  u8g2.setFont(u8g2_font_mozart_nbp_tr); // 5x7 dot font
  u8g2.drawStr( 0, 56, "0"); // Display 0 at the origin
  switch (range) { // Display the frequency according to the range number
    case 3:
      u8g2.drawStr(116, -1, "1s"); // Sampling period
      u8g2.drawStr(45, 56, "50"); // 1/2 frequency
      u8g2.drawStr(92, 56, "100"); // Full scale frequency
      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, "556us");
      u8g2.drawStr(42, 56, "90k");
      u8g2.drawStr(89, 56, "180k"); // (460.8/2)*100/128=180
      break;
    default:
      break;
  }
}

void adcDma(float dv) { // ADC in DMA mode at specified speed
  adc_select_input(0); // Input from GPIO26(A0)
  adc_set_clkdiv(dv); // Set sampling rate
  sample_dma(wave); // Sample in DMA mode and write the result to wave
}

void sample_dma(uint16_t *capture_buf) { // Execute sampling with DMA
  dma_channel_configure(dma_chan, &cfg,
                        capture_buf, // dst
                        &adc_hw->fifo, // src
                        NNN, // 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) { // Settings for using ADC with 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); // Sample rate setting (temporary setting)

  // sleep_ms(1000); // I'm investigating the meaning. Isn't there a need to wait this long?
  sleep_ms(50); // Temporarily set it to 50ms and see how it goes (10ms is not good)
  // 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);
}