#include "driver/i2c.h"
#include "driver/gpio.h"
#include "esp_log.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"

#define I2C_MASTER_SCL_IO 22          // SCL pin
#define I2C_MASTER_SDA_IO 21          // SDA pin
#define I2C_MASTER_NUM I2C_NUM_0      // I2C port number
#define I2C_MASTER_FREQ_HZ 100000     // I2C clock frequency
#define MPU6050_SENSOR_ADDR 0x68      // MPU6050 I2C address
#define LED_PIN 2                     // LED GPIO pin
#define MPU6050_WHO_AM_I 0x75         // WHO_AM_I register address
#define MPU6050_ACCEL_XOUT_H 0x3B     // Accelerometer data register

static const char *TAG = "MPU6050";

// I2C master initialization
void i2c_master_init() {
    i2c_config_t conf = {
        .mode = I2C_MODE_MASTER,
        .sda_io_num = I2C_MASTER_SDA_IO,
        .sda_pullup_en = GPIO_PULLUP_ENABLE,
        .scl_io_num = I2C_MASTER_SCL_IO,
        .scl_pullup_en = GPIO_PULLUP_ENABLE,
        .master.clk_speed = I2C_MASTER_FREQ_HZ,
    };
    i2c_param_config(I2C_MASTER_NUM, &conf);
    i2c_driver_install(I2C_MASTER_NUM, conf.mode, 0, 0, 0);
}

// MPU6050 initialization
void mpu6050_init() {
    uint8_t data = 0;
    i2c_master_write_to_device(I2C_MASTER_NUM, MPU6050_SENSOR_ADDR, &data, 1, 1000 /  portTICK_PERIOD_MS);
}

// Read data from MPU6050
int16_t read_mpu6050_accel_x() {
    uint8_t data[2];
    i2c_master_read_from_device(I2C_MASTER_NUM, MPU6050_SENSOR_ADDR, &data[0], 2, 1000 /  portTICK_PERIOD_MS);
    return (int16_t)((data[0] << 8) | data[1]);
}

void app_main(void) {
    // Initialize I2C
    i2c_master_init();
    // Initialize MPU6050
    mpu6050_init();
    // Configure LED pin
    gpio_set_direction(LED_PIN, GPIO_MODE_OUTPUT);

    while (1) {
        int16_t accel_x = read_mpu6050_accel_x();
        ESP_LOGI(TAG, "Accel X: %d", accel_x);

        if (accel_x > 10000 || accel_x < -10000) {
            gpio_set_level(LED_PIN, 1);  // Turn on LED
        } else {
            gpio_set_level(LED_PIN, 0);  // Turn off LED
        }

        vTaskDelay(100 / portTICK_PERIOD_MS);  // Wait for 100 ms
    }
}