#include <SPI.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <stdio.h>
#include <math.h>

#define SCREEN_WIDTH 128 // OLED display width, in pixels
#define SCREEN_HEIGHT 64 // OLED display height, in pixels

#define OLED_RESET     -1 // Reset pin # (or -1 if sharing Arduino reset pin)
#define SCREEN_ADDRESS 0x3C ///< See datasheet for Address; 0x3D for 128x64, 0x3C for 128x32
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

int indx;
int indx2 = 0;
int tmp; 
float samples;
float oup;
float cor;
int rng = 10; // 0 -- 9
float angle;
float v_angle;
float y0;
float y1;
float y2;
float duration     = 1.00;
float frequency    = 1000; // 1 kHz
float a[2] = {0.95, 0.25}; // amplitude
float s[2] = {0.60, 1.20}; // sigma
float m[2] = {-M_PI_2, M_PI_4}; // mu
float d[100][10] ; // dataset 

int CirX = 96; // (128 / 4) * 3
int CirY = 32; // 64 / 2
int CirR = 21; // Radius               
const float Pi = 3.14159265359;

void setup() {
  Serial.begin(9600);

  if(!display.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS)) {
    Serial.println(F("SSD1306 allocation failed"));
    for(;;); // Don't proceed, loop forever
  }

  samples = frequency / duration;
  v_angle =  2 * M_PI / samples;

}

void loop() { 

  display.clearDisplay();
  display.drawRect(0, 0, 128, 64, WHITE); 
  // display.drawCircle(CirX, CirY, CirR, WHITE);
  // display.drawLine(86,48,106,48,WHITE);    
  //display.drawLine(30,0,30,64,WHITE);     
  display.drawLine(64,0,64,64,WHITE);     
  // display.drawPixel(CirX, CirY, WHITE);     

  for (indx=0;indx<samples;indx++) 
  {
    angle = - M_PI + (indx * v_angle);
    y1 = (angle - m[0]) / s[0];
    y1 = pow(y1, 2);
    y1 = exp(- y1 / 2);
    y1 = a[0] * y1;
    y2 = (angle - m[1]) / s[1];
    y2 = pow(y2, 2);
    y2 = exp(- y2 / 2);
    y2 = a[1] * y2;
    y0 = y1 + y2;
    oup = y0 * rng;  // scale to output
    if (indx == 0){cor = oup; };
    oup = oup - cor; // correction
    if (((indx+1) % 10) == 0) {
        // Serial.print("\n");
        tmp = int((indx+1)/10)-1;
        d[tmp][indx2] = oup;
        // Serial.print(tmp); 
        // Serial.print(", "); 
        // Serial.print(indx2);
        // Serial.print(", ");
        // Serial.print(d[tmp][indx2]); 
        // Serial.print(", ");
    };
    if (((indx+1) % 1000) == 0) {
      indx2 = indx2+1;
      if (indx2 >= 10) { 
        // call it now!
        for (int i=0; i<50; i++)
        {                                                 
          int a = i+7;
          int b = 55 - d[2*i][0] * 5;
        
          display.drawPixel(a, b, WHITE);                              
          display.display();                                         
        }
        // new beginning 
        indx2 = 0; 
      } // circular index
    };

    int z = 5;

    display.drawPixel(90, 44, WHITE); // 0
    display.drawPixel(90, 44 + z, WHITE); 
    display.drawPixel(88, 42, WHITE); // 1
    display.drawPixel(88, 46 + z, WHITE);
    display.drawPixel(86, 40, WHITE); // 2
    display.drawPixel(86, 46 + z, WHITE);
    display.drawPixel(84, 36, WHITE); // 3
    display.drawPixel(84, 47 + z, WHITE);
    display.drawPixel(82, 30, WHITE); // 4
    display.drawPixel(82, 47 + z, WHITE);
    display.drawPixel(81, 20, WHITE);
    
    display.drawPixel(80, 8, WHITE);
    
    display.drawPixel(81, 4, WHITE);
    
    display.drawPixel(83, 8, WHITE);
    
    display.drawPixel(84, 12, WHITE);
    
    display.drawPixel(86, 20, WHITE);
    
    display.drawPixel(88, 26, WHITE);
    
    display.drawPixel(89, 30, WHITE);
    
    display.drawPixel(91, 34, WHITE);
    
    display.drawPixel(92, 38, WHITE);
    
    display.drawPixel(94, 40, WHITE);
    
    display.drawPixel(96, 43, WHITE);
    
    display.drawPixel(97, 46, WHITE);
    
    display.drawPixel(98, 47, WHITE);
    
    display.drawPixel(100, 46, WHITE);
    
    display.drawPixel(102, 46, WHITE);
    
    display.drawPixel(106, 46, WHITE);
    
    display.drawPixel(111, 44, WHITE);
    
    display.drawPixel(112, 43, WHITE);
    
    display.drawPixel(110, 42, WHITE);
    
    display.drawPixel(102, 44, WHITE);
    
    display.drawPixel(98, 46, WHITE);
    
    display.drawPixel(92, 46, WHITE);
    

    delay(1);
  };
}