#include <stdio.h>
#include <freertos/FreeRTOS.h>
#include <freertos/task.h>
#include <driver/i2c.h>
#include <driver/gpio.h>
#include <driver/adc.h>
#include <esp_log.h>
#include <inttypes.h>
#include <math.h>

#define I2C_MASTER_SCL_IO 22
#define I2C_MASTER_SDA_IO 21
#define I2C_PORT I2C_NUM_0
#define MPU6050_I2C_ADDRESS 0x68
#define MPU6050_REG_WHO_AM_I 0x75
#define MPU6050_REG_PWR_MGMT_1 0x6B
#define MPU6050_REG_SMPLRT_DIV 0x19
#define MPU6050_REG_ACCEL_CONFIG 0x1C
#define MPU6050_REG_GYRO_CONFIG 0x1B
#define MPU6050_REG_ACCEL_XOUT_H 0x3B
#define MPU6050_REG_GYRO_XOUT_H 0x43
#define LED_GPIO 2 // Built-in LED
#define LDR_GPIO 34 // LDR sensor connected to GPIO 34 (ADC1 channel 6)

static const char *TAG = "MPU6050";

static esp_err_t i2c_scan(i2c_port_t i2c_num) {
    printf("Scanning I2C bus...\n");
    for (uint8_t addr = 0x08; addr <= 0x77; addr++) {
        i2c_cmd_handle_t cmd = i2c_cmd_link_create();
        i2c_master_start(cmd);
        i2c_master_write_byte(cmd, (addr << 1) | I2C_MASTER_WRITE, true);
        i2c_master_stop(cmd);
        esp_err_t ret = i2c_master_cmd_begin(i2c_num, cmd, 100 / portTICK_PERIOD_MS);
        i2c_cmd_link_delete(cmd);
        if (ret == ESP_OK) {
            printf("Device found at address 0x%02x\n", addr);
        }
    }
    return ESP_OK;
}

static esp_err_t mpu6050_write_reg(i2c_port_t i2c_num, uint8_t reg, uint8_t data) {
    i2c_cmd_handle_t cmd = i2c_cmd_link_create();
    i2c_master_start(cmd);
    i2c_master_write_byte(cmd, (MPU6050_I2C_ADDRESS << 1) | I2C_MASTER_WRITE, true);
    i2c_master_write_byte(cmd, reg, true);
    i2c_master_write_byte(cmd, data, true);
    i2c_master_stop(cmd);
    esp_err_t ret = i2c_master_cmd_begin(i2c_num, cmd, 1000 / portTICK_PERIOD_MS);
    i2c_cmd_link_delete(cmd);
    if (ret != ESP_OK) {
        printf("Failed to write reg 0x%02x: %s\n", reg, esp_err_to_name(ret));
    }
    return ret;
}

static esp_err_t mpu6050_read_regs(i2c_port_t i2c_num, uint8_t reg, uint8_t *data, size_t len) {
    i2c_cmd_handle_t cmd = i2c_cmd_link_create();
    i2c_master_start(cmd);
    i2c_master_write_byte(cmd, (MPU6050_I2C_ADDRESS << 1) | I2C_MASTER_WRITE, true);
    i2c_master_write_byte(cmd, reg, true);
    i2c_master_start(cmd);
    i2c_master_write_byte(cmd, (MPU6050_I2C_ADDRESS << 1) | I2C_MASTER_READ, true);
    i2c_master_read(cmd, data, len, I2C_MASTER_LAST_NACK);
    i2c_master_stop(cmd);
    esp_err_t ret = i2c_master_cmd_begin(i2c_num, cmd, 1000 / portTICK_PERIOD_MS);
    i2c_cmd_link_delete(cmd);
    if (ret != ESP_OK) {
        printf("Failed to read reg 0x%02x: %s\n", reg, esp_err_to_name(ret));
    }
    return ret;
}

static esp_err_t mpu6050_init(i2c_port_t i2c_num) {
    printf("Initializing MPU6050\n");

    // Check WHO_AM_I register
    uint8_t who_am_i;
    esp_err_t ret = mpu6050_read_regs(i2c_num, MPU6050_REG_WHO_AM_I, &who_am_i, 1);
    if (ret != ESP_OK || who_am_i != 0x68) {
        printf("MPU6050 not found: WHO_AM_I=0x%02x, error=%s\n", who_am_i, esp_err_to_name(ret));
        return ESP_FAIL;
    }
    printf("MPU6050 detected: WHO_AM_I=0x%02x\n", who_am_i);

    // Wake up sensor
    ret = mpu6050_write_reg(i2c_num, MPU6050_REG_PWR_MGMT_1, 0x00);
    if (ret != ESP_OK) return ret;

    // Set sample rate to 1 kHz (SMPLRT_DIV = 7)
    ret = mpu6050_write_reg(i2c_num, MPU6050_REG_SMPLRT_DIV, 0x07);
    if (ret != ESP_OK) return ret;

    // Set accelerometer range to ±2g
    ret = mpu6050_write_reg(i2c_num, MPU6050_REG_ACCEL_CONFIG, 0x00);
    if (ret != ESP_OK) return ret;

    // Set gyroscope range to ±250°/s
    ret = mpu6050_write_reg(i2c_num, MPU6050_REG_GYRO_CONFIG, 0x00);
    if (ret != ESP_OK) return ret;

    printf("MPU6050 initialized successfully\n");
    return ESP_OK;
}

static esp_err_t mpu6050_get_acceleration(i2c_port_t i2c_num, float *ax, float *ay, float *az) {
    uint8_t data[6];
    esp_err_t ret = mpu6050_read_regs(i2c_num, MPU6050_REG_ACCEL_XOUT_H, data, 6);
    if (ret != ESP_OK) {
        printf("Failed to read acceleration: %s\n", esp_err_to_name(ret));
        return ret;
    }

    int16_t raw_x = (data[0] << 8) | data[1];
    int16_t raw_y = (data[2] << 8) | data[3];
    int16_t raw_z = (data[4] << 8) | data[5];
    *ax = raw_x / 16384.0f * 9.81f; // ±2g = 16384 LSB/g
    *ay = raw_y / 16384.0f * 9.81f;
    *az = raw_z / 16384.0f * 9.81f;

    if (fabs(*ax) > 50.0f || fabs(*ay) > 50.0f || fabs(*az) > 50.0f) {
        printf("Suspicious acceleration values: X=%.2f, Y=%.2f, Z=%.2f\n", *ax, *ay, *az);
    }

    printf("Acceleration: X=%.2f, Y=%.2f, Z=%.2f m/s^2\n", *ax, *ay, *az);
    return ESP_OK;
}

static esp_err_t mpu6050_get_gyro(i2c_port_t i2c_num, float *gx, float *gy, float *gz) {
    uint8_t data[6];
    esp_err_t ret = mpu6050_read_regs(i2c_num, MPU6050_REG_GYRO_XOUT_H, data, 6);
    if (ret != ESP_OK) {
        printf("Failed to read gyro: %s\n", esp_err_to_name(ret));
        return ret;
    }

    int16_t raw_x = (data[0] << 8) | data[1];
    int16_t raw_y = (data[2] << 8) | data[3];
    int16_t raw_z = (data[4] << 8) | data[5];
    *gx = raw_x / 131.0f; // ±250°/s = 131 LSB/°/s
    *gy = raw_y / 131.0f;
    *gz = raw_z / 131.0f;

    if (fabs(*gx) > 1000.0f || fabs(*gy) > 1000.0f || fabs(*gz) > 1000.0f) {
        printf("Suspicious gyro values: X=%.2f, Y=%.2f, Z=%.2f\n", *gx, *gy, *gz);
    }

    printf("Gyro: X=%.2f, Y=%.2f, Z=%.2f deg/s\n", *gx, *gy, *gz);
    return ESP_OK;
}

void app_main(void) {
    printf("Starting application\n");

    // Initialize LED for visual debug
    gpio_reset_pin(LED_GPIO);
    gpio_set_direction(LED_GPIO, GPIO_MODE_OUTPUT);

    // Initialize ADC for LDR sensor
    adc1_config_width(ADC_WIDTH_BIT_12);
    adc1_config_channel_atten(ADC1_CHANNEL_6, ADC_ATTEN_DB_11);

    // Initialize I2C
    printf("Configuring I2C on SDA=%d, SCL=%d\n", I2C_MASTER_SDA_IO, I2C_MASTER_SCL_IO);
    i2c_config_t i2c_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 = 100000
    };
    esp_err_t ret = i2c_param_config(I2C_PORT, &i2c_conf);
    if (ret != ESP_OK) {
        printf("I2C config failed: %s\n", esp_err_to_name(ret));
        return;
    }
    ret = i2c_driver_install(I2C_PORT, i2c_conf.mode, 0, 0, 0);
    if (ret != ESP_OK) {
        printf("I2C driver install failed: %s\n", esp_err_to_name(ret));
        return;
    }
    printf("I2C initialized\n");

    // Scan I2C bus
    i2c_scan(I2C_PORT);

    // Initialize MPU6050
    ret = mpu6050_init(I2C_PORT);
    if (ret != ESP_OK) {
        printf("MPU6050 init failed, halting\n");
        return;
    }

    while (1) {
        // Blink LED to indicate loop is running
        gpio_set_level(LED_GPIO, 1);
        vTaskDelay(100 / portTICK_PERIOD_MS);
        gpio_set_level(LED_GPIO, 0);

        float ax, ay, az, gx, gy, gz;
        esp_err_t accel_ret = mpu6050_get_acceleration(I2C_PORT, &ax, &ay, &az);
        esp_err_t gyro_ret = mpu6050_get_gyro(I2C_PORT, &gx, &gy, &gz);

        // Read LDR sensor value
        int ldr_value = adc1_get_raw(ADC1_CHANNEL_6);
        printf("LDR value: %d\n", ldr_value);

        if (accel_ret == ESP_OK && gyro_ret == ESP_OK) {
            printf("[%" PRIu32 "] Acc (m/s^2): X=%.2f, Y=%.2f, Z=%.2f | Gyro (deg/s): X=%.2f, Y=%.2f, Z=%.2f | LDR: %d\n",
                   esp_log_timestamp(), ax, ay, az, gx, gy, gz, ldr_value);
        } else {
            printf("MPU6050 read failed: Acc=%s, Gyro=%s\n",
                   esp_err_to_name(accel_ret), esp_err_to_name(gyro_ret));
        }
        vTaskDelay(200 / portTICK_PERIOD_MS); // Delay 2 seconds
    }
}