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

#define I2C_MASTER_SCL_IO           22      // GPIO номер для SCL
#define I2C_MASTER_SDA_IO           21      // GPIO номер для SDA
#define I2C_MASTER_FREQ_HZ          100000  // Частота I2C
#define I2C_MASTER_PORT             I2C_NUM_0

#define MPU6050_ADDR                 0x68   // Адрес MPU6050
#define MPU6050_PWR_MGMT_1           0x6B
#define MPU6050_TEMP_OUT_H           0x41

static const char *TAG = "MPU6050";

// Функция инициализации I2C
esp_err_t i2c_master_init(void) {
    i2c_config_t conf = {
        .mode = I2C_MODE_MASTER,
        .sda_io_num = I2C_MASTER_SDA_IO,
        .scl_io_num = I2C_MASTER_SCL_IO,
        .sda_pullup_en = GPIO_PULLUP_ENABLE,
        .scl_pullup_en = GPIO_PULLUP_ENABLE,
        .master.clk_speed = I2C_MASTER_FREQ_HZ,
    };
    
    esp_err_t ret = i2c_param_config(I2C_MASTER_PORT, &conf);
    if (ret != ESP_OK) {
        return ret;
    }
    
    return i2c_driver_install(I2C_MASTER_PORT, conf.mode, 0, 0, 0);
}

// Функция записи в регистр MPU6050
esp_err_t mpu6050_write_byte(uint8_t reg_addr, uint8_t data) {
    i2c_cmd_handle_t cmd = i2c_cmd_link_create();
    i2c_master_start(cmd);
    i2c_master_write_byte(cmd, (MPU6050_ADDR << 1) | I2C_MASTER_WRITE, true);
    i2c_master_write_byte(cmd, reg_addr, true);
    i2c_master_write_byte(cmd, data, true);
    i2c_master_stop(cmd);
    esp_err_t ret = i2c_master_cmd_begin(I2C_MASTER_PORT, cmd, 1000 / portTICK_PERIOD_MS);
    i2c_cmd_link_delete(cmd);
    return ret;
}

// Функция чтения температуры
float read_mpu6050_temperature(void) {
    uint8_t data[2];
    
    // Устанавливаем указатель на регистр температуры
    i2c_cmd_handle_t cmd = i2c_cmd_link_create();
    i2c_master_start(cmd);
    i2c_master_write_byte(cmd, (MPU6050_ADDR << 1) | I2C_MASTER_WRITE, true);
    i2c_master_write_byte(cmd, MPU6050_TEMP_OUT_H, true);
    i2c_master_stop(cmd);
    esp_err_t ret = i2c_master_cmd_begin(I2C_MASTER_PORT, cmd, 1000 / portTICK_PERIOD_MS);
    i2c_cmd_link_delete(cmd);
    
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "Failed to set register address");
        return -1000.0;
    }
    
    // Читаем данные температуры
    cmd = i2c_cmd_link_create();
    i2c_master_start(cmd);
    i2c_master_write_byte(cmd, (MPU6050_ADDR << 1) | I2C_MASTER_READ, true);
    i2c_master_read_byte(cmd, &data[0], I2C_MASTER_ACK);
    i2c_master_read_byte(cmd, &data[1], I2C_MASTER_NACK);
    i2c_master_stop(cmd);
    ret = i2c_master_cmd_begin(I2C_MASTER_PORT, cmd, 1000 / portTICK_PERIOD_MS);
    i2c_cmd_link_delete(cmd);
    
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "Failed to read temperature data");
        return -1000.0;
    }
    
    int16_t raw_temp = (data[0] << 8) | data[1];
    return (raw_temp / 340.0) + 36.53;
}

void app_main(void) {
    ESP_LOGI(TAG, "Initializing I2C...");
    
    // Инициализация I2C
    esp_err_t ret = i2c_master_init();
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "I2C initialization failed");
        return;
    }
    
    // Активация MPU6050
    ret = mpu6050_write_byte(MPU6050_PWR_MGMT_1, 0x00);
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "MPU6050 activation failed");
        return;
    }
    
    ESP_LOGI(TAG, "MPU6050 Temperature Sensor Ready");
    
    while (1) {
        float temperature = read_mpu6050_temperature();
        if (temperature > -100.0) {
            printf("Temperature: %.2f °C\n", temperature);
        } else {
            printf("Error reading temperature\n");
        }
        
        vTaskDelay(pdMS_TO_TICKS(1000));
    }
}