/* Board */
#include "board.h"
#include "clock_config.h"
#include "pin_mux.h"

/* Debug Console */
#include "fsl_debug_console.h"

/* Private FSPI */
#include "app_fspi.h"
#include "app_mpu_impl.h"

#define RAW16_TO_ACTUAL(raw, fs) ((float)raw * (float)fs / 32768.0)

mpu6500_t s_mpu = {
    .ops =
        {
            .read_register  = app_mpu_impl_read_reg,
            .write_register = app_mpu_impl_write_reg,
            .delay          = app_mpu_impl_delay,
        },
    .user_data = NULL,
};

int main(void) {
    /* Init board hardware. */
    BOARD_ConfigMPU();
    BOARD_InitBootPins();
    BOARD_InitBootClocks();
    BOARD_InitDebugConsole();

    PRINTF("FSPI example start:\r\n");

    status_t ret = fspi_init();
    if (ret != kStatus_Success) {
        PRINTF("FSPI init failed.\r\n");
        goto dead_loop;
    }

    if (mpu6500_init(&s_mpu) != MPU6500_RET_SUCCESS) {
        PRINTF("MPU6500 initialization failed.\r\n");
        goto dead_loop;
    }

    mpu6500_result_t sensor_result;

    uint16_t accel_fs;
    uint16_t gyro_fs;

    if (mpu6500_get_accel_fullscale(&s_mpu, &accel_fs) != MPU6500_RET_SUCCESS) {
        PRINTF("Failed to get accel range.\r\n");
        goto dead_loop;
    }

    if (mpu6500_get_gyro_fullscale(&s_mpu, &gyro_fs) != MPU6500_RET_SUCCESS) {
        PRINTF("Failed to get gyro range.\r\n");
        goto dead_loop;
    }

    for (;;) {
        SDK_DelayAtLeastUs(10 * 1000, CLOCK_GetCoreSysClkFreq());
        if (mpu6500_read_result(&s_mpu, &sensor_result) != MPU6500_RET_SUCCESS) {
            PRINTF("Sensor read failed.\r\n");
            continue;
        }

        float accel_x = RAW16_TO_ACTUAL(sensor_result.accel_x_raw, accel_fs) * 9.80;
        float accel_y = RAW16_TO_ACTUAL(sensor_result.accel_y_raw, accel_fs) * 9.80;
        float accel_z = RAW16_TO_ACTUAL(sensor_result.accel_z_raw, accel_fs) * 9.80;

        float gyro_x = RAW16_TO_ACTUAL(sensor_result.gyro_x_raw, gyro_fs);
        float gyro_y = RAW16_TO_ACTUAL(sensor_result.gyro_y_raw, gyro_fs);
        float gyro_z = RAW16_TO_ACTUAL(sensor_result.gyro_z_raw, gyro_fs);

        PRINTF(
            "/* %6.3f, %6.3f, %6.3f, %6.3f, %6.3f, %6.3f, %6d */\r\n",
            accel_x, accel_y, accel_z, gyro_x, gyro_y, gyro_z, sensor_result.temp_raw);
    }

dead_loop:
    for (;;) {
        /* -- */
    }
}