#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include "queue.h"
#define SCREEN_WIDTH 128 // OLED width,  in pixels
#define SCREEN_HEIGHT 64 // OLED height, in pixels
Adafruit_SSD1306 oled(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire);

const double sigma = 10.0;
const double beta = 8.0 / 3.0;
const double rho = 28.0;
double x = 0.1;
double y = 0.0;
double z = 0.0;
double dt = 0.02; // Time step

void setup() {
  //init serial
  Serial.begin(115200);
  //init oled
  if (!oled.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println(F("failed to start SSD1306 OLED"));
    while (1);
  }
  delay(2000);         
  oled.clearDisplay();
  oled.display();  
}

void loop() {
  double dx = sigma * (y - x);
  double dy = (x*(rho-z)) - y;
  double dz = (x * y) - (beta * z);

  x += dx * dt;
  y += dy * dt;
  z += dz * dt;

  Serial.print(x);
  Serial.print(',');
  Serial.print(y);
  Serial.print(',');
  Serial.println(z);
  
  //oled.clearDisplay();
  //plotGraph(z, x); // Plot z vs x
  oled.drawPixel(z, x, WHITE);
  oled.display();
  //delay(1); // Adjust delay according to the plotter's refresh rate
}

void plotGraph(double z, double x) {
  int xPos = map(x, -20, 20, 0, SCREEN_WIDTH/2);
  int yPos = map(z, -20, 20, SCREEN_HEIGHT/2, 0);

  oled.drawPixel(xPos+32, yPos+40, WHITE);
}