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

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

// create an OLED display object connected to I2C
Adafruit_SSD1306 oled(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);

const int ANALOG_INPUT_PIN = A0;
const int MIN_ANALOG_INPUT = 0;
const int MAX_ANALOG_INPUT = 1023;
const int DELAY_LOOP_MS = 5; // change to slow down how often to read and graph value

int _circularBuffer[SCREEN_WIDTH]; //fast way to store values 
int _curWriteIndex = 0; // tracks where we are in the circular buffer

// for tracking fps
float _fps = 0;
unsigned long _frameCount = 0;
unsigned long _fpsStartTimeStamp = 0;

// status bar
boolean _drawStatusBar = true; // change to show/hide status bar
int _graphHeight = SCREEN_HEIGHT;

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

  // initialize OLED display with I2C address 0x3C
   (!oled.begin(SSD1306_SWITCHCAPVCC, 0x3C));
  

  delay(2000);         // wait two seconds for initializing
  oled.clearDisplay(); // clear display

  oled.setTextSize(2);         // set text size
  oled.setTextColor(WHITE);    // set text color
  oled.setCursor(0, 2);       // set position to display (x,y)
  oled.println("KISAKA"); // set text
  oled.display(); 
  delay(3000);             // display on OLED

 // Clear the buffer
  oled.clearDisplay();

  oled.setTextSize(1);
  oled.setCursor(0, 0);
  oled.setTextColor(WHITE, BLACK);
  oled.println("Screen initialized!");
  oled.display();
  delay(500);
  
  oled.clearDisplay();
  if(_drawStatusBar){
    _graphHeight = SCREEN_HEIGHT - 10;
  }

  _fpsStartTimeStamp = millis();
}
  void loop() {
  // read the input on analog pin 0:
  int sensorValue = analogRead(A0);
  // print out the value you read:
  Serial.println(sensorValue);
  delay(500);  // delay in between reads for stability

   oled.clearDisplay(); // clear display

  oled.setTextSize(1.9);         // set text size
  oled.setTextColor(WHITE);    // set text color
  oled.setCursor(0, 2);       // set position to display (x,y)
  oled.println("SENSOR_V:"); // set text
  oled.println(sensorValue);
  oled.display();              // display on OLED

  // Clear the display on each frame. We draw from the _circularBuffer
  oled.clearDisplay();

  // Read and store the analog data into a circular buffer
  int analogVal = analogRead(ANALOG_INPUT_PIN);
  Serial.println(analogVal);
  _circularBuffer[_curWriteIndex++] = analogVal;

  // Set the circular buffer index back to zero when it reaches the 
  // right of the screen
  if(_curWriteIndex >=  oled.width()){
    _curWriteIndex = 0;
  }
  
  if(_drawStatusBar){
    drawStatusBar(analogVal);
  }
  
  // Draw the line graph based on data in _circularBuffer
  int xPos = 0; 
  for (int i = _curWriteIndex; i < oled.width(); i++){
    int analogVal = _circularBuffer[i];
    drawLine(xPos, analogVal);
    xPos++;
  }
  
  for(int i = 0; i < _curWriteIndex; i++){
    int analogVal = _circularBuffer[i];
    drawLine(xPos, analogVal);
    xPos++;;
  }
  
   oled.display();
  
  calcFrameRate();
  
  delay(DELAY_LOOP_MS);
}


void drawLine(int xPos, int analogVal){
  int lineHeight = map(analogVal, MIN_ANALOG_INPUT, MAX_ANALOG_INPUT, 0, _graphHeight);
  int yPos =   oled.height() - lineHeight;
    oled.drawFastVLine(xPos, yPos, lineHeight, SSD1306_WHITE);
}

/**
 * Call this every frame to calculate frame rate
 */
void calcFrameRate() {
    
  unsigned long elapsedTime = millis() - _fpsStartTimeStamp;
  _frameCount++;
  if (elapsedTime > 1000) {
    _fps = _frameCount / (elapsedTime / 1000.0);
    _fpsStartTimeStamp = millis();
    _frameCount = 0;
  }
}

/**
 * Draws the status bar at top of screen with fps and analog value
 */
void drawStatusBar(int analogVal) {

   // erase status bar by drawing all black
    oled.fillRect(0, 0, oled.width(), 8, SSD1306_BLACK); 
  
  // Draw current val
   oled.setCursor(0, 0);
    oled.print(analogVal);

  // Draw frame count
  int16_t x1, y1;
  uint16_t w, h;
   oled.getTextBounds("XX.XX fps", 0, 0, &x1, &y1, &w, &h);
    oled.setCursor(oled.width() - w, 0);
    oled.print(_fps);
    oled.print(" fps");
}