#include <Wire.h> 

const uint8_t MPU6050_ADDRESS = 0x68; 

const uint8_t REG_SMPLRT_DIV   = 0x19; 
const uint8_t REG_CONFIG       = 0x1A; 
const uint8_t REG_GYRO_CONFIG  = 0x1B; 
const uint8_t REG_ACCEL_CONFIG = 0x1C; 
const uint8_t REG_ACCEL_XOUT_H = 0x3B; 
const uint8_t REG_PWR_MGMT_1   = 0x6B; 
const uint8_t REG_WHO_AM_I     = 0x75; 
const uint32_t SAMPLE_PERIOD_US = 10000; 
uint32_t nextSampleTime = 0;
float rollEstimate = 0.0f;
float pitchEstimate = 0.0f; 
bool filterInitialized = false;

bool writeRegister(uint8_t reg, uint8_t value) {  
  Wire.beginTransmission(MPU6050_ADDRESS);  
  Wire.write(reg);  
  Wire.write(value);   

  return Wire.endTransmission() == 0;
}

bool readRegisters(uint8_t firstRegister, 
                   uint8_t* buffer,
                   uint8_t length) { 
  Wire.beginTransmission(MPU6050_ADDRESS);  
  Wire.write(firstRegister); 

  if (Wire.endTransmission(false) != 0) {
    return false; 
  }

  uint8_t received = Wire.requestFrom(MPU6050_ADDRESS, length, static_cast<uint8_t>(true));

  if (received != length) {    
    return false;
  }
  for (uint8_t i = 0; i < length; i++) {    
    buffer[i] = Wire.read();
   }  
  return true; 
}

int16_t combineBytes(uint8_t highByte, uint8_t lowByte){
  return (highByte << 8) | lowByte;
}

bool initializeMPU6050() { 
  if (!writeRegister(REG_PWR_MGMT_1, 0x00)) {
    return false;  
  }
  delay(100);
  writeRegister(REG_CONFIG, 0x03);  
  writeRegister(REG_SMPLRT_DIV, 9);  
  writeRegister(REG_GYRO_CONFIG, 0x00);  
  writeRegister(REG_ACCEL_CONFIG, 0x00);

  return true; 
}

void setup() {
    Serial.begin(115200);  
  delay(200);
  Serial.println("HELLO BRO");

  Wire.begin();
  Wire.setClock(400000);

  Serial.println("Start");  
  Serial.println("Checking I2C communication...");

  if (!initializeMPU6050()) {
    Serial.println("INIT ERR");
    while (1);
  }

  if (!initializeMPU6050()) {    
    Serial.println("INIT ERR");

    while (1);   
  }

  Serial.println("READY");
 
  nextSampleTime = micros();
}

void loop() {
  const uint32_t currentTime = micros();
  if (static_cast<int32_t>(          
          currentTime - nextSampleTime) < 0) {
                return;  
  }
  nextSampleTime += SAMPLE_PERIOD_US;

  uint8_t rawData[14];

  if (!readRegisters(REG_ACCEL_XOUT_H, rawData, sizeof(rawData))) {    
    Serial.println("ERROR: IMU read failed.");    
    return;  
  }

  const int16_t rawAx = combineBytes(rawData[0], rawData[1]);  
  const int16_t rawAy = combineBytes(rawData[2], rawData[3]);  
  const int16_t rawAz = combineBytes(rawData[4], rawData[5]);  
  const int16_t rawTemp = combineBytes(rawData[6], rawData[7]);  
  const int16_t rawGx = combineBytes(rawData[8], rawData[9]);
  const int16_t rawGy = combineBytes(rawData[10], rawData[11]);
  const int16_t rawGz =      combineBytes(rawData[12], rawData[13]);

  const float ax = rawAx / 16384.0f;  
  const float ay = rawAy / 16384.0f;  
  const float az = rawAz / 16384.0f;   
  const float gx = rawGx / 131.0f;  
  const float gy = rawGy / 131.0f;  
  const float gz = rawGz / 131.0f;

  const float temperature = rawTemp / 340.0f + 36.53f;
  const float accelerometerRoll = atan2(ay, az) * 180.0f / PI;
  const float accelerometerPitch = atan2(-ax, sqrt(ay * ay + az * az)) * 180.0f / PI;
  const float dt = 0.01f;
  const float alpha = 0.98f;

  if (!filterInitialized) {    
    rollEstimate = accelerometerRoll;    
    pitchEstimate = accelerometerPitch;    
    filterInitialized = true;
  } else {    
    rollEstimate = alpha * (rollEstimate + gx * dt) + (1.0f - alpha) * accelerometerRoll;
    pitchEstimate = alpha * (pitchEstimate + gy * dt) + (1.0f - alpha) * accelerometerPitch;  
  }

  Serial.println("OK");
}