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

// -------------------------
// I²C Configuration (MPU6050)
// -------------------------
#define I2C_MASTER_SCL_IO      8       
#define I2C_MASTER_SDA_IO      9       
#define I2C_MASTER_NUM         I2C_NUM_0
#define I2C_MASTER_FREQ_HZ     100000  
#define I2C_TIMEOUT_MS         1000
#define MPU6050_ADDR           0x68    

// MPU6050 Registers
#define MPU6050_WHO_AM_I       0x75
#define MPU6050_PWR_MGMT_1     0x6B
#define MPU6050_ACCEL_XOUT_H   0x3B
#define MPU6050_GYRO_XOUT_H    0x43

static const char *TAG_MPU = "MPU6050";
static const char *TAG_TEMP = "TempSensor";
static const char *TAG_MAIN = "Main";

// Temperature sensor handle
static temperature_sensor_handle_t temp_sensor = NULL;



// -------------------------
// Main Application
// -------------------------
void app_main(void)
{

    printf("  ESP32-S3 Multi-Sensor System\n");
    printf("  - MPU6050 (Accelerometer & Gyroscope)\n");
    printf("  - Built-in Temperature Sensor\n");
  
    
    // Initialize MPU6050
    esp_err_t ret = init_mpu6050();
    if (ret != ESP_OK) {
        ESP_LOGE(TAG_MAIN, "MPU6050 initialization failed!");
        return;
    }
    
    // Initialize built-in temperature sensor
    ret = init_temp_sensor();
    if (ret != ESP_OK) {
        ESP_LOGE(TAG_MAIN, "Temperature sensor initialization failed!");
        return;
    }

    printf("  All sensors initialized successfully!\n");
    printf("  Starting continuous readings...\n");
    
    // Create tasks for reading sensors
    xTaskCreate(mpu6050_task, "MPU6050", 4096, NULL, 5, NULL);
    xTaskCreate(temp_sensor_task, "TempSensor", 2048, NULL, 5, NULL);
    
    ESP_LOGI(TAG_MAIN, "Tasks created. System running...");
}

esp_err_t init_mpu6050(void)
{
    ESP_LOGI(TAG_MPU, "Initializing I2C and MPU6050...");
    
    // Configure I2C
    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_NUM, &conf);
    if (ret != ESP_OK) {
        ESP_LOGE(TAG_MPU, "I2C config failed: %s", esp_err_to_name(ret));
        return ret;
    }

    ret = i2c_driver_install(I2C_MASTER_NUM, conf.mode, 0, 0, 0);
    if (ret != ESP_OK) {
        ESP_LOGE(TAG_MPU, "I2C driver install failed: %s", esp_err_to_name(ret));
        return ret;
    }

    vTaskDelay(pdMS_TO_TICKS(100));

    // Verify device
    uint8_t who_am_i_reg = MPU6050_WHO_AM_I;
    uint8_t who_am_i_data;
    ret = i2c_master_write_read_device(I2C_MASTER_NUM, MPU6050_ADDR,
                                       &who_am_i_reg, 1,
                                       &who_am_i_data, 1,
                                       pdMS_TO_TICKS(I2C_TIMEOUT_MS));
    
    if (ret == ESP_OK) {
        ESP_LOGI(TAG_MPU, "✓ MPU6050 detected! WHO_AM_I = 0x%02X", who_am_i_data);
    } else {
        ESP_LOGE(TAG_MPU, "✗ Failed to detect MPU6050: %s", esp_err_to_name(ret));
        return ret;
    }

    // Wake up MPU6050
    uint8_t wakeup_cmd[2] = { MPU6050_PWR_MGMT_1, 0x00 };
    ret = i2c_master_write_to_device(I2C_MASTER_NUM, MPU6050_ADDR,
                                     wakeup_cmd, 2,
                                     pdMS_TO_TICKS(I2C_TIMEOUT_MS));
    if (ret == ESP_OK) {
        ESP_LOGI(TAG_MPU, "✓ MPU6050 is awake and ready");
    } else {
        ESP_LOGE(TAG_MPU, "✗ Failed to wake MPU6050");
        return ret;
    }

    return ESP_OK;
}

void temp_sensor_task(void *param)
{
    ESP_LOGI(TAG_TEMP, "Starting temperature sensor reading task...");
    
    while(1) {
        float temperature;
        esp_err_t ret = temperature_sensor_get_celsius(temp_sensor, &temperature);
        
        if (ret == ESP_OK) {
            float fahrenheit = temperature * 9.0 / 5.0 + 32.0;
            ESP_LOGI(TAG_TEMP, "ESP32-S3 Die Temperature: %.2f °C (%.2f °F)", 
                     temperature, fahrenheit);
        } else {
            ESP_LOGE(TAG_TEMP, "Failed to read temperature: %s", esp_err_to_name(ret));
        }
        
        vTaskDelay(pdMS_TO_TICKS(2000));  // Read every 2 seconds
    }
}

void mpu6050_task(void *param)
{
    ESP_LOGI(TAG_MPU, "Starting MPU6050 reading task...");
    
    int count = 0;
    while(1) {
        printf("\n========== Reading #%d ==========\n", count++);
        
        // Read accelerometer
        uint8_t accel_reg = MPU6050_ACCEL_XOUT_H;
        uint8_t accel_data[6];
        
        esp_err_t ret = i2c_master_write_read_device(I2C_MASTER_NUM, MPU6050_ADDR,
                                                     &accel_reg, 1,
                                                     accel_data, 6,
                                                     pdMS_TO_TICKS(I2C_TIMEOUT_MS));
        
        if (ret == ESP_OK) {
            int16_t accel_x = (accel_data[0] << 8) | accel_data[1];
            int16_t accel_y = (accel_data[2] << 8) | accel_data[3];
            int16_t accel_z = (accel_data[4] << 8) | accel_data[5];
            
            float accel_x_g = accel_x / 16384.0;
            float accel_y_g = accel_y / 16384.0;
            float accel_z_g = accel_z / 16384.0;
            
            ESP_LOGI(TAG_MPU, "Accel (g): X=%.3f  Y=%.3f  Z=%.3f", 
                     accel_x_g, accel_y_g, accel_z_g);
        } else {
            ESP_LOGE(TAG_MPU, "Failed to read accelerometer");
        }
        
        // Read gyroscope
        uint8_t gyro_reg = MPU6050_GYRO_XOUT_H;
        uint8_t gyro_data[6];
        
        ret = i2c_master_write_read_device(I2C_MASTER_NUM, MPU6050_ADDR,
                                          &gyro_reg, 1,
                                          gyro_data, 6,
                                          pdMS_TO_TICKS(I2C_TIMEOUT_MS));
        
        if (ret == ESP_OK) {
            int16_t gyro_x = (gyro_data[0] << 8) | gyro_data[1];
            int16_t gyro_y = (gyro_data[2] << 8) | gyro_data[3];
            int16_t gyro_z = (gyro_data[4] << 8) | gyro_data[5];
            
            float gyro_x_dps = gyro_x / 131.0;
            float gyro_y_dps = gyro_y / 131.0;
            float gyro_z_dps = gyro_z / 131.0;
            
            ESP_LOGI(TAG_MPU, "Gyro (°/s): X=%.2f  Y=%.2f  Z=%.2f", 
                     gyro_x_dps, gyro_y_dps, gyro_z_dps);
        } else {
            ESP_LOGE(TAG_MPU, "Failed to read gyroscope");
        }
        
        vTaskDelay(pdMS_TO_TICKS(2000));  // Read every 2 seconds
    }
}

esp_err_t init_temp_sensor(void)
{
    ESP_LOGI(TAG_TEMP, "Initializing built-in temperature sensor...");
    
    // Configure temperature sensor
    temperature_sensor_config_t temp_sensor_config = TEMPERATURE_SENSOR_CONFIG_DEFAULT(-10, 80);
    
    esp_err_t ret = temperature_sensor_install(&temp_sensor_config, &temp_sensor);
    if (ret != ESP_OK) {
        ESP_LOGE(TAG_TEMP, "Failed to install temperature sensor: %s", esp_err_to_name(ret));
        return ret;
    }
    
    // Enable the sensor
    ret = temperature_sensor_enable(temp_sensor);
    if (ret != ESP_OK) {
        ESP_LOGE(TAG_TEMP, "Failed to enable temperature sensor: %s", esp_err_to_name(ret));
        return ret;
    }
    
    ESP_LOGI(TAG_TEMP, "✓ Built-in temperature sensor ready");
    return ESP_OK;
}