#include <Wire.h>
#include <MPU6050.h>

#define SDA_PIN 19
#define SCL_PIN 18
#define BUFFER_SIZE 500 // Buffertstorlek för snabbare Serial
#define SAMPLE_RATE 1000 // 1000 Hz

MPU6050 mpu;
char buffer[BUFFER_SIZE * 16]; // Buffer för Serial
int bufferIndex = 0;
unsigned long lastTime = 0;
unsigned long startTime;

void setup() {
  Serial.begin(2000000);  // Öka Serial-hastigheten till 2 Mbaud för snabbare överföring
  Wire.begin(SDA_PIN, SCL_PIN);
  Wire.setClock(1000000);  // Sätter I2C-hastigheten till 1 MHz för snabbare läsningar

  Serial.println("Initialiserar MPU6050...");
  mpu.initialize();
  
  if (mpu.testConnection()) Serial.println("MPU6050 ansluten!");
  else Serial.println("Misslyckades med att ansluta till MPU6050");

  // Ställ in MPU6050 på 1 kHz utan DLPF
  mpu.setDLPFMode(0);
  mpu.setRate(0); // Sätter sample rate till max (1 kHz)

  startTime = micros();
}

void logData(int16_t ax, unsigned long timeTracker) {
  bufferIndex += snprintf(buffer + bufferIndex, sizeof(buffer) - bufferIndex, "%lu; %d\n", timeTracker, ax);
  
  if (bufferIndex >= sizeof(buffer) - 16) {
    Serial.write(buffer, bufferIndex);
    bufferIndex = 0;
  }
}

void loop() {
  if (micros() - lastTime >= (1000000 / SAMPLE_RATE)) {
    lastTime = micros();

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

    logData(ax, lastTime - startTime);
  }
}