#include "main.h"
#include "BMP280.h"
#include "BH1750.h"

// I2C handle
I2C_HandleTypeDef hi2c1;

// MPU6050 address
#define MPU_ADDR 0x68

// Function prototypes
void kalmanFilter(float AcX, float AcY, float GyX, float GyY, float dt);
void soundAlert(int frequency);
void checkWarnings();

int16_t AcX, AcY, AcZ, GyX, GyY, GyZ;

float temperature, pressure, lux;
float angleX = 0.0, angleY = 0.0;
float gyroX = 0.0, gyroY = 0.0;
float accelX = 0.0, accelY = 0.0;

BMP280 bmp(0x76);  // BMP280 I2C address
BH1750 lightMeter(0x23);  // BH1750 I2C address

// Main function
int main(void) {
  // Initialize the HAL Library
  HAL_Init();

  // Configure the system clock
  SystemClock_Config();

  // Initialize GPIO, I2C, and UART
  MX_GPIO_Init();
  MX_I2C1_Init();
  MX_USART1_UART_Init();

  // Initialize BMP280 sensor
  if (bmp.begin()) {
    HAL_UART_Transmit(&huart1, (uint8_t*)"BMP280 sensor initialized.\r\n", 26, 1000);
  } else {
    HAL_UART_Transmit(&huart1, (uint8_t*)"Could not find BMP280, check wiring!\r\n", 36, 1000);
    while (1);
  }

  // Initialize BH1750 sensor
  if (lightMeter.begin()) {
    HAL_UART_Transmit(&huart1, (uint8_t*)"BH1750 sensor initialized.\r\n", 27, 1000);
  } else {
    HAL_UART_Transmit(&huart1, (uint8_t*)"Error initializing BH1750 sensor.\r\n", 34, 1000);
    while (1);
  }

  // Initialize MPU6050 sensor
  uint8_t data = 0;
  HAL_I2C_Mem_Write(&hi2c1, MPU_ADDR << 1, 0x6B, 1, &data, 1, 1000);  // Wake up MPU6050

  while (1) {
    // Read data from MPU6050 (accelerometer and gyroscope)
    uint8_t data[14];
    HAL_I2C_Mem_Read(&hi2c1, MPU_ADDR << 1, 0x3B, 1, data, 14, 1000);

    // Extract accelerometer and gyroscope data
    AcX = (int16_t)(data[0] << 8 | data[1]);
    AcY = (int16_t)(data[2] << 8 | data[3]);
    AcZ = (int16_t)(data[4] << 8 | data[5]);
    GyX = (int16_t)(data[8] << 8 | data[9]);
    GyY = (int16_t)(data[10] << 8 | data[11]);
    GyZ = (int16_t)(data[12] << 8 | data[13]);

    // Read BMP280 sensor data (temperature and pressure)
    temperature = bmp.getTemperature();
    pressure = bmp.getPressure();  // Pressure in hPa
    pressure = pressure * ((float)1000 / 95000); // Convert to desired pressure scale
    float alt = bmp.calAltitude(pressure, 1);  // Calculate altitude based on pressure

    // Read BH1750 sensor data (lux)
    lux = lightMeter.readLightLevel();

    // Output sensor values via UART (for debugging purposes)
    char buffer[100];
    snprintf(buffer, sizeof(buffer), "Temp: %.2f C, Pressure: %.2f hPa, Lux: %.2f\n", temperature, pressure, lux);
    HAL_UART_Transmit(&huart1, (uint8_t*)buffer, strlen(buffer), 1000);

    HAL_Delay(1000);  // Adjust the loop delay as needed
  }
}

// System Clock configuration
void SystemClock_Config(void) {
  // System Clock configuration code (same as your provided code)
}

// GPIO initialization
static void MX_GPIO_Init(void) {
  // GPIO initialization (same as your provided code)
}

// I2C1 initialization
static void MX_I2C1_Init(void) {
  hi2c1.Instance = I2C1;
  hi2c1.Init.Timing = 0x20303E5D;  // I2C Timing configuration, adjust this based on your clock setup
  hi2c1.Init.OwnAddress1 = 0;
  hi2c1.Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT;
  hi2c1.Init.DualAddressMode = I2C_DUALADDRESS_DISABLE;
  hi2c1.Init.OwnAddress2 = 0;
  hi2c1.Init.OwnAddress2Masks = I2C_OA2_NOMASK;
  hi2c1.Init.GeneralCallMode = I2C_GENERALCALL_DISABLE;
  hi2c1.Init.NoStretchMode = I2C_NOSTRETCH_DISABLE;
  if (HAL_I2C_Init(&hi2c1) != HAL_OK) {
    Error_Handler();
  }
}

// USART1 initialization for UART communication
static void MX_USART1_UART_Init(void) {
  // USART1 initialization (same as your provided code)
}

// Error handler (for debugging)
void Error_Handler(void) {
  while (1);
}