#include <Wire.h>
#include <Adafruit_GFX.h>      
#include <Adafruit_SSD1306.h>  
#include <Adafruit_NeoPixel.h>
#include <MPU6050.h>
#include <math.h>       
#include <Fonts/FreeSansOblique9pt7b.h>

#include <Fonts/FreeMonoBold12pt7b.h>  // Include the desired font

#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_RESET -1
#define OLED_ADDRESS 0x3C 

Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

#define NEOPIXEL_PIN 15
#define NEOPIXEL_COUNT 1
Adafruit_NeoPixel strip = Adafruit_NeoPixel(NEOPIXEL_COUNT, NEOPIXEL_PIN, NEO_GRB + NEO_KHZ800);

MPU6050 mpu;

float speed = 0.0; // Speed in m/s
unsigned long lastTime = 0;

void setup() {
  Serial.begin(115200);
  
  if (!display.begin(SSD1306_SWITCHCAPVCC, OLED_ADDRESS)) {
    Serial.println(F("SSD1306 allocation failed"));
    while (1);
  }
  
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(0, 0);
  display.println("Initializing...");
  display.display();
  delay(2000);

  strip.begin();
  strip.show();

  Wire.begin();
  mpu.initialize();

  if (!mpu.testConnection()) {
    Serial.println("MPU6050 connection failed");
    display.clearDisplay();
    display.setCursor(0, 0);
    display.println("MPU6050 failed!");
    display.display();
    while (1);
  } else {
    display.clearDisplay();
    display.setCursor(0, 0);
    display.println("MPU6050 OK!");
    display.display();
    delay(2000);
  }
}

void loop() {
  int16_t ax, ay, az;
  mpu.getAcceleration(&ax, &ay, &az);

  // Disable the Y-axis acceleration by setting it to 0
  ay = 0;

  // Calculate acceleration based on the X and Z axes only
  float acceleration = ax / 16384.0;
  unsigned long currentTime = millis();
  float dt = (currentTime - lastTime) / 1000.0;

  speed += acceleration * dt; 
  lastTime = currentTime;

  float speed_kmh = fabs(speed * 3.6);

  // Display speed on OLED with a different font
  display.clearDisplay();
  display.setFont(&FreeSansOblique9pt7b);  // Set the new font
  display.setCursor(0, 40);
  display.print(speed_kmh, 2); 
  display.print(" km/h");
  display.setFont(NULL); // Reset to default font for other text
  display.setCursor(0, 0);
  display.print("Happy Journey :D");
  display.display();

  if (speed_kmh < 10) {
    strip.setPixelColor(0, strip.Color(0, 0, 255));
  } else if (speed_kmh < 20) {
    strip.setPixelColor(0, strip.Color(0, 255, 0));
  } else {
    strip.setPixelColor(0, strip.Color(255, 0, 0));
  }
  strip.show();
  
  delay(100); 
}