#include <stdio.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "driver/i2c.h"
#include "driver/gpio.h"

#define I2C_SCL_PIN 22
#define I2C_SDA_PIN 21
#define I2C_FREQ_HZ 100000  // I2C clock frequency
#define MPU6050_ADDRESS 0x68  // MPU6050 I2C address

#define LED_PIN GPIO_NUM_2  // Change this to the GPIO pin connected to the LED
#define BUZZER_PIN GPIO_NUM_4  // Change this to the GPIO pin connected to the Buzzer

void MPU6050_init(i2c_port_t i2c_port);
void MPU6050_read_data(i2c_port_t i2c_port, int16_t *accel_data, int16_t *gyro_data);

void app_main() {
    // Setup LED and Buzzer
    gpio_config_t io_conf;
    io_conf.mode = GPIO_MODE_OUTPUT;
    io_conf.pin_bit_mask = (1ULL << LED_PIN) | (1ULL << BUZZER_PIN);
    io_conf.intr_type = GPIO_INTR_DISABLE;
    gpio_config(&io_conf);

    i2c_config_t i2c_config = {
        .mode = I2C_MODE_MASTER,
        .sda_io_num = I2C_SDA_PIN,
        .sda_pullup_en = GPIO_PULLUP_ENABLE,
        .scl_io_num = I2C_SCL_PIN,
        .scl_pullup_en = GPIO_PULLUP_ENABLE,
        .master.clk_speed = I2C_FREQ_HZ,
    };

    i2c_param_config(I2C_NUM_0, &i2c_config);
    i2c_driver_install(I2C_NUM_0, I2C_MODE_MASTER, 0, 0, 0);

    MPU6050_init(I2C_NUM_0);

    while (1) {
        int16_t accel_data[3];
        int16_t gyro_data[3];

        MPU6050_read_data(I2C_NUM_0, accel_data, gyro_data);

        printf("Accelerometer Data: X=%d, Y=%d, Z=%d\n", accel_data[0], accel_data[1], accel_data[2]);
        printf("Gyroscope Data: X=%d, Y=%d, Z=%d\n", gyro_data[0], gyro_data[1], gyro_data[2]);

        // Check for high acceleration (greater than 0.5g)
        if (accel_data[0] > 1638 || accel_data[1] > 1638 || accel_data[2] > 1638) {
            // Activate LED
            gpio_set_level(LED_PIN, 1);

            // Activate Buzzer
            gpio_set_level(BUZZER_PIN, 1);

            vTaskDelay(500 / portTICK_PERIOD_MS);  // Wait for 500ms
        } else {
            // Deactivate LED
            gpio_set_level(LED_PIN, 0);

            // Deactivate Buzzer
            gpio_set_level(BUZZER_PIN, 0);
        }

        vTaskDelay(100 / portTICK_PERIOD_MS);
    }
}

void MPU6050_init(i2c_port_t i2c_port) {
    // Initialize MPU6050
    // You can send configuration commands if needed
}

void MPU6050_read_data(i2c_port_t i2c_port, int16_t *accel_data, int16_t *gyro_data) {
    uint8_t buffer[14];

    // Read raw data from MPU6050
    i2c_cmd_handle_t cmd = i2c_cmd_link_create();
    i2c_master_start(cmd);
    i2c_master_write_byte(cmd, (MPU6050_ADDRESS << 1) | I2C_MASTER_WRITE, true);
    i2c_master_write_byte(cmd, 0x3B, true);
    i2c_master_start(cmd);
    i2c_master_write_byte(cmd, (MPU6050_ADDRESS << 1) | I2C_MASTER_READ, true);
    i2c_master_read(cmd, buffer, 14, I2C_MASTER_LAST_NACK);
    i2c_master_stop(cmd);
    i2c_master_cmd_begin(i2c_port, cmd, 1000 / portTICK_PERIOD_MS);
    i2c_cmd_link_delete(cmd);

    // Extract accelerometer data
    accel_data[0] = (buffer[0] << 8) | buffer[1];
    accel_data[1] = (buffer[2] << 8) | buffer[3];
    accel_data[2] = (buffer[4] << 8) | buffer[5];

    // Extract gyroscope data
    gyro_data[0] = (buffer[8] << 8) | buffer[9];
    gyro_data[1] = (buffer[10] << 8) | buffer[11];
    gyro_data[2] = (buffer[12] << 8) | buffer[13];
}