#include <Wire.h>
#include <Adafruit_SSD1306.h>
#include <fix_fft.h>

// configuracion display
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_RESET -1   //   QT-PY / XIAO

//#define SIGNAL_FREQUENCY 3000
#define SAMPLING_FREQUENCY 15000
#define SAMPLES 128

char membar[SAMPLES];
char datai[SAMPLES];
char dataq[SAMPLES];

int SIGNAL_FREQUENCY1 = 3000;
int SIGNAL_FREQUENCY2 = 3000;

const unsigned int sampling_period_us = round(1000000 * (2.0 / SAMPLING_FREQUENCY)); // Sampling period (doubled to account for overclock)
unsigned long microseconds;

/* declaro la libreria para controlar el display*/ 
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);

void setup() {
  
  /* inicio el display */
  delay(250); // "importante" esperar antes de iniciar para darle tiempo a arrancar
  display.begin(SSD1306_SWITCHCAPVCC, 0x3C); // inicializo el display
  display.clearDisplay();
  delay(250);
  
  /* dibujo las barras en el display */
  display.fillRect(0,0,128,64,WHITE); // Dibujo un rectangulo nego para borrar la muestra anterior

  display.display(); // Actualizo la pantalla


}

void loop() {
  /* declaro variables */
  int i, j;
  int prom, outp, avgi, avgq, val, maxp, summi = 0, summq = 0, maxpant, cont;
  int8_t s1, s2;

  /* genera una onda senoidal */
  SIGNAL_FREQUENCY1 = 5000;
  SIGNAL_FREQUENCY2 = 5500;
  double amplitude = 200;
  double freq1 = 6.283185 * SIGNAL_FREQUENCY1 / SAMPLING_FREQUENCY; // Fraction of a complete cycle stored at each sample (in radians)
  double freq2 = 6.283185 * SIGNAL_FREQUENCY2 / SAMPLING_FREQUENCY; // Fraction of a complete cycle stored at each sample (in radians)
  
  for (uint16_t i = 0; i < SAMPLES; i++)
  {
    s1 = int8_t(amplitude * sin(i * freq1) / 2.0);
    s2 = int8_t(amplitude * sin(i * freq2) / 2.0);
    datai[i] = int8_t(s1*s2);/* Build data with positive and negative values*/
    //vReal[i] = uint8_t((amplitude * (sin(i * ratio) + 1.0)) / 2.0);/* Build data displaced on the Y axis to include only positive values*/
    dataq[i] = 0.0; //Imaginary part must be zeroed in case of looping to avoid wrong calculations and overflows
  }
  
  // run FFT
  fix_fft(datai, dataq, 7, 0);
 
  /* dibujo las barras en el display */
  display.fillRect(0,0,128,64,BLACK); // Dibujo un rectangulo nego para borrar la muestra anterior

  /* calculo el promedio para eliminar parte del ruido */
  prom = 0;
  for(i = 0; i < SAMPLES; i++)
  {
    maxp = int( sqrt(datai[i] * datai[i] + dataq[i] * dataq[i]));
    prom = prom + maxp;
  }
  prom = prom / SAMPLES;
  
  maxpant = 0;
  for(i = 0; i < SAMPLES; i++)
  {
    /* calculo la magnitud */
    maxp = int( sqrt(datai[i] * datai[i] + dataq[i] * dataq[i]));

    outp = maxp - prom; // le resto el promedio a la salida para eliminar el ruido

    //membar[i] = membar[i] + outp + 1;
    //if(outp < 4){outp = ;}

    

    //if(outp > 1){
    //  membar[i] = outp;
    //}else{
    //  membar[i] = membar[i] + outp;  
    //  membar[i] = -1;
    //  if(membar[i] < 1){membar[i] = 1;}
    //}
    membar[i] = outp * 2;
    if(membar[i] > 40){membar[i] = 40;}
    
  }

  int xant = 0, yant = 52;

  for(i = 0; i < SAMPLES; i++)
  {
    /* 
     dibujo la barra vertical que indica la potencia 
     como la primera mitad del total de las muestras contiene
     las frecuencias positivas y la segunda mitad las negativas
     para que me quede la frecuencia 0 al centro de la pantalla
     dibujo las frecuencias positivas a partir del centro de la 
     pantalla hacia la derecha y las negativas del centro hacia 
     la izquierda 
    */
    
    //outp = (membar[i] * 4);
    outp = membar[i];
    if(outp > 50){outp = 50;}
    if(outp < 1){outp = 1;}
    
    if(i < 62){
      display.drawLine(63 - xant, yant, 63 - i, 52 -(outp), WHITE);
      //display.fillRect(i*1 + 63, 52-(outp), 1, outp, WHITE);
    }else{
      display.drawLine(128 - (xant - 63) , yant, 128 - (i - 63), 52-(outp), WHITE);
      //display.fillRect(i*1 - 62, 52-(outp), 1, outp, WHITE);
    }

    xant = i;
    yant = 52-(outp);
  }
  
  display.fillRect(64, 52-(42), 1, 42, WHITE);

  display.setTextSize(1);
  display.setTextColor(WHITE);
  display.setCursor(62,53);
  display.print("0");
  display.setCursor(0,53);
  display.print("-6k");
  display.setCursor(110,53);
  display.print("+6k");
  display.setCursor(64,0);
  display.print(SIGNAL_FREQUENCY1);
  display.display(); // Actualizo la pantalla

  delay(5000);

}