#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 LED_PIN 2

#define MPU6050_ADDRESS 0x68  // MPU6050 I2C address

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 turnOnLED() {
    gpio_set_level(LED_PIN, 1);  // Set GPIO level high to turn on the LED
}
void app_main() {
    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]);
        
            turnOnLED();
        
        vTaskDelay(1000 / 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];
}