#include <stdio.h>
#include "pico/stdlib.h"
#include "hardware/i2c.h"

// MPU6050 Registers
#define MPU6050_ADDR 0x68
#define MPU6050_ACCEL_XOUT_H 0x3B
#define MPU6050_GYRO_XOUT_H 0x43

// LED Pins
#define LED_RIGHT 25
#define LED_LEFT 24

// I2C Pins
#define I2C_SDA 4
#define I2C_SCL 5

// Function to read data from MPU6050 registers
void read_mpu6050(uint8_t reg, uint8_t *data, uint8_t len) {
    i2c_write_blocking(i2c0, MPU6050_ADDR, &reg, 1, true);
    i2c_read_blocking(i2c0, MPU6050_ADDR, data, len, false);
}

int main() {
    stdio_init_all();

    // Initialize I2C
    i2c_init(i2c0, 400000);
    gpio_set_function(I2C_SDA, GPIO_FUNC_I2C);
    gpio_set_function(I2C_SCL, GPIO_FUNC_I2C);
    gpio_pull_up(I2C_SDA);
    gpio_pull_up(I2C_SCL);

    // Initialize LED pins
    gpio_init(LED_RIGHT);
    gpio_init(LED_LEFT);
    gpio_set_dir(LED_RIGHT, GPIO_OUT);
    gpio_set_dir(LED_LEFT, GPIO_OUT);

    while (1) {
        uint8_t accel_data[6];
        uint8_t gyro_data[6];

        // Read accelerometer data
        read_mpu6050(MPU6050_ACCEL_XOUT_H, accel_data, 6);

        // Read gyroscope data
        read_mpu6050(MPU6050_GYRO_XOUT_H, gyro_data, 6);

        // Calculate roll angle (adjust the calculations if needed)
        int16_t ax = (accel_data[0] << 8) | accel_data[1];
        int16_t ay = (accel_data[2] << 8) | accel_data[3];
        int16_t az = (accel_data[4] << 8) | accel_data[5];
        float roll = atan2f(ay, az) * 180.0 / M_PI;

        // Detect motion turning right or left
        if (roll > 30.0) {
            gpio_put(LED_RIGHT, 1);
            gpio_put(LED_LEFT, 0);
            // You can add code to send data to HC-05 Bluetooth module here
        } else if (roll < -30.0) {
            gpio_put(LED_RIGHT, 0);
            gpio_put(LED_LEFT, 1);
            // You can add code to send data to HC-05 Bluetooth module here
        } else {
            gpio_put(LED_RIGHT, 0);
            gpio_put(LED_LEFT, 0);
        }

        sleep_ms(100); // Adjust the delay as needed
    }

    return 0;
}