#include <Wire.h>
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <TFT_eSPI.h> // Include the TFT_eSPI library

#define MPU6050_ADDRESS 0x68  // MPU6050 I2C address

Adafruit_MPU6050 mpu;
TFT_eSPI tft = TFT_eSPI(); // Create a TFT object

void setup() {
  Serial.begin(115200);
  while (!Serial);

  if (!mpu.begin(MPU6050_ADDRESS)) {
    Serial.println("Could not find a valid MPU6050 sensor, check wiring!");
    while (1);
  }

  // Initialize the MPU6050
  mpu.setAccelerometerRange(MPU6050_RANGE_2_G);
  mpu.setGyroRange(MPU6050_RANGE_250_DEG);
  mpu.setFilterBandwidth(MPU6050_BAND_21_HZ);

  tft.init(); // Initialize the TFT display
  tft.setRotation(3); // Adjust the rotation as needed (0, 1, 2, or 3)
  tft.fillScreen(TFT_BLACK); // Set the background color to black
  tft.setTextColor(TFT_WHITE); // Set the text color to white
  tft.setTextSize(2); // Set the text size

  // Display the text "C00253514" at the top left corner
  tft.setCursor(0, 0); // Set the cursor position
  tft.print("C00253514");
}

void loop() {
  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);

  float ax = a.acceleration.x;
  float ay = a.acceleration.y;
  float az = a.acceleration.z;

  // Print values to Serial for reference
  Serial.print(ax);
  Serial.print("\t");
  Serial.print(ay);
  Serial.print("\t");
  Serial.print(az);
  Serial.println();

  // Draw the graph on the TFT display with thicker pixels
  drawGraph(ax, ay, az);

  delay(100);  // Adjust the delay as needed
}

void drawGraph(float ax, float ay, float az) {
  static int yOffset = 120; // Vertical offset for the graph
  static int x = 0;

  int yAx = yOffset + (int)(ax * 30); // Scale the values for display
  int yAy = yOffset + (int)(ay * 30);
  int yAz = yOffset + (int)(az * 30);

  // Draw a thicker point at the current position
  tft.fillCircle(x, yAx, 1.5, TFT_MAGENTA); // Adjust the thickness as needed
  tft.fillCircle(x, yAy, 1.5, TFT_WHITE);
  tft.fillCircle(x, yAz, 1.5, TFT_BLUE);

  // Increment the x position
  x++;

  // Clear the screen and reset the x position when it reaches the edge
  if (x >= 320) {
    x = 0;
    tft.fillScreen(TFT_BLACK);
  }
}