#include "SPI.h"
#include "Adafruit_GFX.h"
#include "Adafruit_ST7735.h"
#include "esp_dsp.h"

#define TFT_MOSI 10
#define TFT_SCLK 8
#define TFT_CS 21 // Chip select control pin
#define TFT_DC 6  // Data Command control pin
#define TFT_RST 7

Adafruit_ST7735 tft = Adafruit_ST7735(TFT_CS, TFT_DC, TFT_MOSI, TFT_SCLK, TFT_RST);

#define VERBOSE false

#define N_SAMPLES 1024

int N = N_SAMPLES;

int pointer = 0;
__attribute__((aligned(16))) float values[N_SAMPLES];

int notes[] = {4186, 4435, 4699, 4978, 5274, 5588, 5920, 6272, 6645, 7040, 7459, 7902};
char letters[] = {'C', '#', 'D', '#', 'E', 'F', '#', 'G', '#', 'A', '#', 'B'};
float totals[12];
int int_totals[12];
uint16_t colors[12] = {0xfc14, 0xf800, 0xfc00, 0xff80, 0xcff0, 0x07e0, 0x0520, 0x06ba, 0x039a, 0x001f, 0xa45f, 0xd67f};

// Window coefficients
__attribute__((aligned(16))) float wind[N_SAMPLES];
// working complex array
__attribute__((aligned(16))) float y_cf[N_SAMPLES * 2];
// Pointers to result arrays
float *y1_cf = &y_cf[0];

uint32_t timer = micros();

void setup()
{
  // put your setup code here, to run once:
  Serial.begin(115200);

  esp_err_t ret;

  ret = dsps_fft2r_init_fc32(NULL, CONFIG_DSP_MAX_FFT_SIZE);

  dsps_wind_hann_f32(wind, N);

  for (int n = 0; n < 12; n++)
  {
    int_totals[n] = 0;
  }

  tft.initR(INITR_BLACKTAB);
  tft.setRotation(2);
  tft.fillScreen(0x0000);
}

void fft(float *x)
{
  for (int i = 0; i < N; i++)
  {
    y_cf[i * 2 + 0] = x[i] * wind[i];
    y_cf[i * 2 + 1] = 0;
  }

  dsps_fft2r_fc32(y_cf, N);

  dsps_bit_rev_fc32(y_cf, N);

  // Convert one complex vector to two complex vectors
  dsps_cplx2reC_fc32(y_cf, N);

  if (VERBOSE)
  {
    dsps_view(y1_cf, N, 64, 10, 0, 10, '-');
  }
}

int argmax(float *input, int length)
{
  int max_i = -1;
  float max_val = 0;
  for (int i = 0; i < length; i++)
  {
    float val = abs(input[i]);
    if (i == 0 || val > max_val)
    {
      max_i = i;
      max_val = val;
    }
  }
  return max_i;
}

void loop()
{
  // put your main code here, to run repeatedly:
  // delay(1); // this speeds up the simulation
  delayMicroseconds(10);

  float val = (float)(analogRead(2) - 2048) / (2048.0);
  values[pointer] = (float)val;
  pointer = (pointer + 1) % N;

  if (pointer == 0)
  {
    uint32_t T = micros() - timer;

    // int Fs = N*1000000/T;
    fft(values);
    int i = argmax(y1_cf, N / 2);
    Serial.println((float)i * 500000.0 / (float)T);
    float freq = (float)i * 500000.0 / (float)T;

    int octave = 8;

    for (int expn = 0; expn < 8; expn++)
    {
      if (freq > 4000)
      {
        break;
      }
      freq *= 2;
      octave -= 1;
    }

    for (int n = 0; n < 12; n++)
    {

      if (freq < notes[n])
      {
        if (n != 0 && freq - notes[n - 1] < notes[n] - freq)
        {
          n = n - 1;
        }

        if (letters[n] == '#')
        {
          Serial.print(letters[n - 1]);
        }
        Serial.print(letters[n]);
        Serial.println(octave);
        break;
      }
    }

    if (true)
    {
      float ratio = ((float)T / 500000.0);
      for (int n = 0; n < 12; n++)
      {
        int old_total = int_totals[n];
        totals[n] = 0;
        uint32_t start = notes[n] >> 8;
        // ignore octave 0 and 1 since that might be noise.
        for (uint32_t expn = 2; expn <= 8; expn++)
        {
          // in octave 1, notes are spaced about 2 apart so +-1 is about the separation.
          int start_i = (((start << 1) - 1) << (expn - 1)) * ratio;
          int end_i = ((((start << 1) + 1) << (expn - 1))) * ratio;
          for (int i = start_i; i < end_i; i++)
          {

            totals[n] += fabs(y1_cf[i]);
          }
        }

        int_totals[n] = ((int)totals[n]) >> 3;

        if (old_total > int_totals[n])
        {

          tft.fillRect(int_totals[n], 6 + 13 * n, old_total - int_totals[n], 3, 0x0000);
        }
        else
        {
          tft.fillRect(old_total, 6 + 13 * n, int_totals[n] - old_total, 3, colors[n]);
        }
      }
    }

    timer = micros();
  }
}