/**
 * MPU6050 (加速度・ジャイロセンサ) を Raspberry Pi Pico から I2C 経由で読み取るサンプルコード
 * pico-sdk を使用
 *
 * 配線:
 *   MPU6050 VCC  -> Pico 3V3(OUT)  (物理ピン36)
 *   MPU6050 GND  -> Pico GND       (どこでもよい)
 *   MPU6050 SCL  -> Pico GPIO5     (I2C0 SCL)
 *   MPU6050 SDA  -> Pico GPIO4     (I2C0 SDA)
 *
 * CMakeLists.txt の target_link_libraries に
 *   pico_stdlib hardware_i2c
 * を追加すること
 */

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

// ---- 設定項目 ----
#define I2C_PORT         i2c0       // 使用するI2Cペリフェラル (i2c0 か i2c1)
#define I2C_SDA_PIN      4          // SDAに使うGPIO番号
#define I2C_SCL_PIN      5          // SCLに使うGPIO番号
#define I2C_BAUDRATE     400000     // I2C通信速度 400kHz (Fast-mode)

#define MPU6050_ADDR     0x68       // MPU6050のI2Cスレーブアドレス (AD0ピンがGNDの場合。3.3Vなら0x69)

// MPU6050の内部レジスタアドレス(データシートに載っている番地)
#define REG_PWR_MGMT_1   0x6B       // 電源管理レジスタ(起動直後はスリープ状態なのでこれで解除する)
#define REG_ACCEL_XOUT_H 0x3B       // 加速度・ジャイロデータが並んでいる先頭レジスタ

// ---- 関数プロトタイプ ----
static void mpu6050_init(void);
static void mpu6050_read_raw(int16_t accel[3], int16_t gyro[3]);

int main(void) {
    stdio_init_all();

    // --- I2Cペリフェラルの初期化 ---
    // i2c_init()はクロック(ボーレート)を設定する。戻り値は実際に設定された値。
    i2c_init(I2C_PORT, I2C_BAUDRATE);

    // GPIOピンを「I2C機能」として使うよう割り当てる
    // (普通のGPIOのままだとI2Cのハードウェアからは制御できない)
    gpio_set_function(I2C_SDA_PIN, GPIO_FUNC_I2C);
    gpio_set_function(I2C_SCL_PIN, GPIO_FUNC_I2C);

    // I2Cはオープンドレイン方式の通信なので、線を "H" に持ち上げるプルアップ抵抗が必須
    // 外付けの抵抗(大体2.2k〜10kΩ)があれば不要だが、なければPico内蔵のプルアップを有効化しておく
    gpio_pull_up(I2C_SDA_PIN);
    gpio_pull_up(I2C_SCL_PIN);

    sleep_ms(100); // センサの電源が安定するまで少し待つ

    mpu6050_init();

    while (true) {
        int16_t accel[3], gyro[3];
        mpu6050_read_raw(accel, gyro);

        printf("Accel: X=%6d Y=%6d Z=%6d | Gyro: X=%6d Y=%6d Z=%6d\n",
               accel[0], accel[1], accel[2],
               gyro[0], gyro[1], gyro[2]);

        sleep_ms(200);
    }
}

/**
 * MPU6050は電源投入直後、省電力のためスリープモードで止まっている。
 * PWR_MGMT_1レジスタ(0x6B)に0x00を書き込むとスリープが解除され、センサが動き出す。
 */
static void mpu6050_init(void) {
    // I2Cで1バイトのレジスタに書き込むときは
    // [レジスタアドレス, 書き込む値] の2バイトを続けて送るのが決まった作法
    uint8_t buf[2] = {REG_PWR_MGMT_1, 0x00};

    // i2c_write_blocking(I2Cポート, スレーブアドレス, 送信データ, バイト数, ストップ条件を送らないか)
    // 最後の引数 false = 送信後に普通にSTOP条件を出す(この通信はこれで完結)
    i2c_write_blocking(I2C_PORT, MPU6050_ADDR, buf, 2, false);
}

/**
 * 加速度3軸(X,Y,Z)とジャイロ3軸(X,Y,Z)の生データ(それぞれ16bit)を読み取る。
 *
 * MPU6050では 0x3B番地から0x48番地まで14バイトが連続して並んでいる:
 *   0x3B-0x40 : ACCEL_XOUT_H/L, ACCEL_YOUT_H/L, ACCEL_ZOUT_H/L (加速度 X/Y/Z、各2バイト)
 *   0x41-0x42 : TEMP_OUT_H/L                                   (温度、今回は使わない)
 *   0x43-0x48 : GYRO_XOUT_H/L, GYRO_YOUT_H/L, GYRO_ZOUT_H/L    (ジャイロ X/Y/Z、各2バイト)
 */
static void mpu6050_read_raw(int16_t accel[3], int16_t gyro[3]) {
    uint8_t buf[14];
    uint8_t reg = REG_ACCEL_XOUT_H;

    // 手順1: 「これから読みたいレジスタのアドレス」だけを先にセンサへ送る
    // 最後の引数を true にすると、送信後にSTOP条件を出さずバスを保持したままにする
    // → 次のi2c_read_blockingが「リピートスタート」として発行され、
    //    他のデバイスに横取りされずに読み出しへ移行できる
    i2c_write_blocking(I2C_PORT, MPU6050_ADDR, &reg, 1, true);

    // 手順2: 14バイトをまとめて読み出す
    // MPU6050はレジスタ読み出し中、アドレスを自動でインクリメントしてくれる仕様なので
    // 1回の通信で14バイト分ごっそり取得できる
    i2c_read_blocking(I2C_PORT, MPU6050_ADDR, buf, 14, false);

    // 各軸は「上位バイト(H)・下位バイト(L)」の2バイトで1つの16bit値になっている(ビッグエンディアン)
    // 例: buf[0]がACCEL_XOUT_H、buf[1]がACCEL_XOUT_L → (H << 8) | L で結合
    accel[0] = (int16_t)((buf[0]  << 8) | buf[1]);   // ACCEL_XOUT
    accel[1] = (int16_t)((buf[2]  << 8) | buf[3]);   // ACCEL_YOUT
    accel[2] = (int16_t)((buf[4]  << 8) | buf[5]);   // ACCEL_ZOUT
    // buf[6], buf[7] はTEMP_OUT(温度データ)なので今回は読み飛ばす
    gyro[0]  = (int16_t)((buf[8]  << 8) | buf[9]);   // GYRO_XOUT
    gyro[1]  = (int16_t)((buf[10] << 8) | buf[11]);  // GYRO_YOUT
    gyro[2]  = (int16_t)((buf[12] << 8) | buf[13]);  // GYRO_ZOUT
}