/** 
  * @file    motion.c
  * @brief   Motion Module
  * @author  Kenny Tran
  * @date    Jan 27 2023
  * @version 0.0.1
  *
  * @section LICENSES
  * Copyright (C) Morgan Advanced Programmable Systems, Inc.
 */


/*--------------------------------------------------------------------------------------------------
 * INCLUDE FILES
 *------------------------------------------------------------------------------------------------*/
#include <stdint.h>
#include <stdbool.h>
#include <stdlib.h>
#include <math.h>

#include <zephyr/kernel.h>

#include "../lib/fs.h"
#include "../lib/errno.h"
#include "../lib/print.h"

#include "../bsp/imu.h"
#include "../bsp/wdt.h"

#include "../wrappers/gpio.h"

#include "motion.h"


/*--------------------------------------------------------------------------------------------------
 * MACROS & CONSTANTS
 *------------------------------------------------------------------------------------------------*/
#define _MOTION_PI                      ((float) 3.14159265359)
/* Exponential moving average filter constant (0.0 to 1.0, higher = more smoothing).
 * Also used as scalar for sensor fusion. */
#define _MOTION_ALPHA                   ((float) 0.5)

#define _MOTION_GYRO_MAG_WINDOW_SIZE    (10)


/*--------------------------------------------------------------------------------------------------
 * THREAD DEFINITIONS
 *------------------------------------------------------------------------------------------------*/
K_THREAD_STACK_DEFINE(_motion_stack, MOTION_STACK_SIZE);


/*--------------------------------------------------------------------------------------------------
 * TYPE DEFINITIONS & ENUMERATIONS
 *------------------------------------------------------------------------------------------------*/


/*--------------------------------------------------------------------------------------------------
 * PRIVATE CONSTANTS & VARIABLES
 *------------------------------------------------------------------------------------------------*/
static struct k_thread  _motion_thread;
static FS_file_t        _motion_file;
static MOTION_config_s  _motion_config = {
    .accel_cal = {
        .offset = { .x = 0.0, .y = 0.0, .z = 0.0 },
        .scalar = { .x = 1.0, .y = 1.0, .z = 1.0 }
    },
    .gyro_cal = {
        .offset = { .x = 0.0, .y = 0.0, .z = 0.0 },
        .scalar = { .x = 1.0, .y = 1.0, .z = 1.0 }
    },
    .angular_threshold = MOTION_ANG_THRESHOLD_DEFAULT,
    .back_cross_axis_factor  = 0.0,
    .front_cross_axis_factor = 0.0,
};
static MOTION_vector_s           _motion_orientation = { .x = 0.0, .y = 0.0, .z = 0.0 };
static IMU_data_s                _motion_accel_data;
static IMU_data_s                _motion_gyro_data;
static float                     _motion_dt;
static uint32_t                  _motion_recal_readings   = 0;
static bool                      _motion_gyro_recalibrate = false;
static bool                      _motion_imu_detected     = false;
static MOTION_calibration_data_s _motion_recal_gyro = { .offset = { .x = 0.0, .y = 0.0, .z = 0.0 },
                                                        .scalar = { .x = 1.0, .y = 1.0, .z = 1.0 } };

static uint32_t                  _motion_cal_readings[MOTION_CAL_ORIENTATION_COUNT] = { 0, 0, 0 };
static bool                      _motion_cal[MOTION_CAL_ORIENTATION_COUNT]          = { false, false, false };

/* Variables to dump measured X and Y angles to terminal. */
static uint32_t         _motion_timeout_cnt  = 0;
static bool             _motion_data_dump    = false;
static uint32_t         _motion_data_dump_ms = 0;

/* Variables to to calculate angular magnitude for motion from gyro. */
static float            _motion_angular_mag = 0.0;


/*--------------------------------------------------------------------------------------------------
 * PRIVATE FUNCTION PROTOTYPES
 *------------------------------------------------------------------------------------------------*/
static void            _motion_task (void *p1, void *p2, void *p3);
static void            _motion_config_read (void);
static void            _motion_config_write (void);
static void            _motion_data_process (MOTION_vector_s accel_raw, MOTION_vector_s gyro_raw, float dt);
static MOTION_vector_s _motion_apply_cal (MOTION_vector_s raw, MOTION_calibration_data_s cal);


/*--------------------------------------------------------------------------------------------------
 * PUBLIC FUNCTION DEFINITIONS
 *------------------------------------------------------------------------------------------------*/
void MOTION_init (void)
{
    PRINT (INFO, "Initializing MOTION...\r\n");

    /* Initialize IMU. */   
    if (IMU_init () < ERRNO_SUCCESS)
    {
        PRINT (ERROR, "Failure to initialize IMU.\r\n");
    }

    /* Set delta time, / by 1000.0 to get seconds. */
    _motion_dt = (float) MOTION_TASK_PERIOD_MS / (float) 1000.0;

    /* Get configuration settings. */
    _motion_config_read ();
}

bool MOTION_linear_detected (void)
{
    bool detected = _motion_imu_detected;

    /* Reset motion detected flag. */
    _motion_imu_detected = false;

    return detected;
}

bool MOTION_angular_detected (void)
{
    static float   mag_history[_MOTION_GYRO_MAG_WINDOW_SIZE] = { 0 };
    static float   sum    = 0.0f;       /* Running sum for average.             */
    static float   sum_sq = 0.0f;       /* Running sum of squares for variance. */
    static uint8_t idx    = 0;

    /* Remove old value from sum. */
    float old_mag = mag_history[idx];
    sum          -= old_mag;
    sum_sq       -= (old_mag * old_mag);
    
    /* Add new value to sums */
    mag_history[idx]  = _motion_angular_mag;
    sum              += _motion_angular_mag;
    sum_sq           += (_motion_angular_mag * _motion_angular_mag);
    
    /* Update index */
    idx = (idx + 1) % _MOTION_GYRO_MAG_WINDOW_SIZE;
    
    /* Calculate variance: Var(X) = E[X²] - E[X]² */
    float mean     = sum / _MOTION_GYRO_MAG_WINDOW_SIZE;
    float mean_sq  = sum_sq / _MOTION_GYRO_MAG_WINDOW_SIZE;
    float variance = mean_sq - (mean * mean);
    
    /* Detect motion when variance exceeds threshold */
    return (variance >= _motion_config.angular_threshold);
}

void MOTION_angular_threshold_set (float variance)
{
    _motion_config.angular_threshold = variance;

    /* Write new threshold to flash. */
    _motion_config_write ();
}

float MOTION_angle_get (MOTION_axis_e axis)
{
    float comp;

    switch (axis)
    {
        case MOTION_AXIS_X:
            /* y relative to IMU, x relative to board */
            return (_motion_orientation.y);

        case MOTION_AXIS_Y:
            /* x relative to IMU, y relative to board */
            if (_motion_orientation.y > 0)
            {
                comp = (_motion_orientation.y * _motion_config.back_cross_axis_factor);
            }
            else
            {
                comp = (_motion_orientation.y * _motion_config.front_cross_axis_factor);
            }
            
            return (_motion_orientation.x - comp);

        default:
            PRINT (ERROR, "Invalid axis.\r\n");
            return 1234.567;
    }
}

void MOTION_calibrate (MOTION_cal_orientation_e orientation)
{
    if (orientation >= MOTION_CAL_ORIENTATION_INVALID)
    {
        PRINT (ERROR, "Invalid calibration orientation.\r\n");
        return;
    }

    /* Reset motion calibration readings. */
    _motion_cal[(uint32_t) orientation]          = true;
    _motion_cal_readings[(uint32_t) orientation] = 0;
}

float MOTION_calibrate_status_get (MOTION_cal_orientation_e orientation)
{
    if (orientation >= MOTION_CAL_ORIENTATION_INVALID)
    {
        PRINT (ERROR, "Invalid calibration orientation.\r\n");
        return 1.0;
    }

    return ((float) _motion_cal_readings[(uint32_t) orientation] / (float) MOTION_CAL_READINGS);
}

void MOTION_gyro_recalibrate (void)
{
    /* Reset motion recalibration readings. */
    _motion_recal_readings = 0;

    _motion_recal_gyro = (MOTION_calibration_data_s) { .offset = { .x = 0.0, .y = 0.0, .z = 0.0 },
                                                       .scalar = { .x = 1.0, .y = 1.0, .z = 1.0 } };

    _motion_gyro_recalibrate = true;
}

void MOTION_gyro_recalibrate_abort (void)
{
    _motion_gyro_recalibrate = false;
}

float MOTION_gyro_recalibrate_status_get (void)
{
    return ((float) _motion_recal_readings / (float) MOTION_GYRO_RECAL_READINGS);
}

MOTION_config_s *MOTION_config_get (void)
{
    return &_motion_config;
}

void MOTION_config_set (MOTION_config_s cfg)
{
    _motion_config = cfg;
    
    _motion_config_write ();
}

void MOTION_task_suspend (void)
{
    k_thread_suspend (&_motion_thread);
}

void MOTION_task_resume (void)
{
    k_thread_resume (&_motion_thread);
}

void MOTION_task_install (void)
{
    static k_tid_t id;

    id = k_thread_create (&_motion_thread,
                          _motion_stack,
                          MOTION_STACK_SIZE,
                          _motion_task,
                          NULL,
                          NULL,
                          NULL,
                          MOTION_THREAD_PRIORITY,
                          0,
                          K_NO_WAIT);
    k_thread_name_set(&_motion_thread, "MOTION_task");                      
}

void MOTION_cli_cmd (int32_t argc, char **argv)
{
    if (argc == 0)
    {
        float x_adjusted = MOTION_angle_get (MOTION_AXIS_X);
        float y_adjusted = MOTION_angle_get (MOTION_AXIS_Y);

        PRINT (INFO, "MOTION\r\n");
        PRINT (INFO, "-------------------------------------------\r\n");
        PRINT (INFO, "X (Corrected)     : %.3f\r\n", (double) x_adjusted);
        PRINT (INFO, "Y (Corrected)     : %.3f\r\n", (double) y_adjusted);
        PRINT (INFO, "Angular Threshold : %.3f\r\n", (double) _motion_config.angular_threshold);
        PRINT (INFO, "Back Cal Factor   : %.3f\r\n", (double) _motion_config.back_cross_axis_factor);
        PRINT (INFO, "Front Cal Factor  : %.3f\r\n", (double) _motion_config.front_cross_axis_factor);
        PRINT (INFO, "Calibration Data\r\n");
        PRINT (INFO, "-------------------------------------------\r\n");
        PRINT (INFO, "Accelerometer\r\n");
        PRINT (INFO, "  Offset X        : %.3f\r\n", (double) _motion_config.accel_cal.offset.x);
        PRINT (INFO, "  Offset Y        : %.3f\r\n", (double) _motion_config.accel_cal.offset.y);
        PRINT (INFO, "  Offset Z        : %.3f\r\n", (double) _motion_config.accel_cal.offset.z);
        PRINT (INFO, "  Scalar X        : %.3f\r\n", (double) _motion_config.accel_cal.scalar.x);
        PRINT (INFO, "  Scalar Y        : %.3f\r\n", (double) _motion_config.accel_cal.scalar.y);
        PRINT (INFO, "  Scalar Z        : %.3f\r\n", (double) _motion_config.accel_cal.scalar.z);
        PRINT (INFO, "Gyroscope\r\n");
        PRINT (INFO, "  Offset X        : %.3f\r\n", (double) _motion_config.gyro_cal.offset.x);
        PRINT (INFO, "  Offset Y        : %.3f\r\n", (double) _motion_config.gyro_cal.offset.y);
        PRINT (INFO, "  Offset Z        : %.3f\r\n", (double) _motion_config.gyro_cal.offset.z);
        PRINT (INFO, "  Scalar X        : %.3f\r\n", (double) _motion_config.gyro_cal.scalar.x);
        PRINT (INFO, "  Scalar Y        : %.3f\r\n", (double) _motion_config.gyro_cal.scalar.y);
        PRINT (INFO, "  Scalar Z        : %.3f\r\n", (double) _motion_config.gyro_cal.scalar.z);
    }
    else if (argc == 2)
    {
        if (!strcmp (argv[0], "--cal"))
        {
            MOTION_cal_orientation_e orientation = (MOTION_cal_orientation_e) strtoul (argv[1], NULL, 0);
            
            PRINT (INFO, "Calibrating with %d readings...\r\n", MOTION_CAL_READINGS);

            MOTION_calibrate (orientation);
        }
        else if (!strcmp (argv[0], "--dump"))
        {
            _motion_data_dump_ms = strtoul (argv[1], NULL, 0);
            _motion_data_dump = true;
        }
        else if (!strcmp (argv[0], "--theta"))
        {
            float deg = strtof (argv[1], NULL);

            MOTION_angular_threshold_set (deg);
        }
        else
        {
            goto USAGE;
        }
    }
    else
    {
USAGE:
        PRINT (INFO, "Usage: motion [OPTION...]\r\n");
        PRINT (INFO, "  --cal       Calibrates IMU (offsets).\r\n");
        PRINT (INFO, "    <%%d>      0 = flat, 1, = back, 2 = front.\r\n");
        PRINT (INFO, "  --dump      Dumps calculated angles to terminal.\r\n");
        PRINT (INFO, "    <%%d>      Duration in milliseconds.\r\n");
        PRINT (INFO, "  --theta     Sets new angular threshold.\r\n");
        PRINT (INFO, "    <%%f>      Value to set.\r\n");
    }
}

void MOTION_detected_flag_set (void)
{
    _motion_imu_detected = true;
}


/*--------------------------------------------------------------------------------------------------
 * PRIVATE FUNCTION DEFINITIONS
 *------------------------------------------------------------------------------------------------*/
static void _motion_task (void *p1, void *p2, void *p3)
{
    while (1)
    {
        /* Read accelerometer data. */
        if (IMU_xl_read (&_motion_accel_data) != ERRNO_SUCCESS)
        {
            PRINT (ERROR, "Failure to read IMU accel data.\r\n");
        }

        /* Read gyroscope data. */
        if (IMU_gyro_read (&_motion_gyro_data) != ERRNO_SUCCESS)
        {
            PRINT (ERROR, "Failure to read IMU gyro data.\r\n");
        }

        _motion_data_process ((MOTION_vector_s) _motion_accel_data, (MOTION_vector_s) _motion_gyro_data, _motion_dt);

        if (_motion_cal[MOTION_CAL_ORIENTATION_FLAT])
        {
            static MOTION_calibration_data_s accel = { .offset = { .x = 0.0, .y = 0.0, .z = 0.0 },
                                                       .scalar = { .x = 0.0, .y = 0.0, .z = 0.0 } };
            static MOTION_calibration_data_s gyro  = { .offset = { .x = 0.0, .y = 0.0, .z = 0.0 },
                                                       .scalar = { .x = 0.0, .y = 0.0, .z = 0.0 } };

            accel.offset.x += _motion_accel_data.x;
            accel.offset.y += _motion_accel_data.y;
            accel.offset.z += _motion_accel_data.z;

            gyro.offset.x += _motion_gyro_data.x;
            gyro.offset.y += _motion_gyro_data.y;
            gyro.offset.z += _motion_gyro_data.z;

            if (++_motion_cal_readings[MOTION_CAL_ORIENTATION_FLAT] == MOTION_CAL_READINGS)
            {
                _motion_config.accel_cal.offset.x = accel.offset.x / (float) MOTION_CAL_READINGS;
                _motion_config.accel_cal.offset.y = accel.offset.y / (float) MOTION_CAL_READINGS;
                /* Add 1G (gravity) for z, since IMU is pointing down. */
                _motion_config.accel_cal.offset.z = (accel.offset.z / (float) MOTION_CAL_READINGS) + 1.0f;
                
                _motion_config.gyro_cal.offset.x = gyro.offset.x / (float) MOTION_CAL_READINGS;
                _motion_config.gyro_cal.offset.y = gyro.offset.y / (float) MOTION_CAL_READINGS;
                _motion_config.gyro_cal.offset.z = gyro.offset.z / (float) MOTION_CAL_READINGS;

                /* Reset static values for next calibration. */
                _motion_cal[MOTION_CAL_ORIENTATION_FLAT] = false;

                PRINT (INFO, "\r\nCalibration complete!\r\n");

                /* Write new settings to file. */
                _motion_config_write ();

                /* Reset static values in case calibration is ran again.*/
                accel = (MOTION_calibration_data_s) { .offset = { .x = 0.0, .y = 0.0, .z = 0.0 },
                                                      .scalar = { .x = 0.0, .y = 0.0, .z = 0.0 } };
                gyro  = (MOTION_calibration_data_s) { .offset = { .x = 0.0, .y = 0.0, .z = 0.0 },
                                                      .scalar = { .x = 0.0, .y = 0.0, .z = 0.0 } };
            }
            else if (_motion_cal_readings[MOTION_CAL_ORIENTATION_FLAT] % 10 == 0)
            {
                PRINT (INFO, ".\r\n");
            }
        }
        else if (_motion_cal[MOTION_CAL_ORIENTATION_BACK])
        {
            static float x_sum = 0.0;
            static float y_sum = 0.0;

            x_sum += _motion_orientation.x;
            y_sum += _motion_orientation.y;

            if (++_motion_cal_readings[MOTION_CAL_ORIENTATION_BACK] == MOTION_CAL_READINGS)
            {
                x_sum /= (float) MOTION_CAL_READINGS;
                y_sum /= (float) MOTION_CAL_READINGS;

                _motion_config.back_cross_axis_factor = x_sum / y_sum;

                /* Reset static values for next calibration. */
                _motion_cal[MOTION_CAL_ORIENTATION_BACK] = false;

                PRINT (INFO, "\r\nCalibration complete!\r\n");

                /* Write new settings to file. */
                _motion_config_write ();

                /* Reset sums for next calibration. */
                x_sum = 0.0;
                y_sum = 0.0;
            }
            else if (_motion_cal_readings[MOTION_CAL_ORIENTATION_BACK] % 10 == 0)
            {
                PRINT (INFO, ".\r\n");
            }
        }
        else if (_motion_cal[MOTION_CAL_ORIENTATION_FRONT])
        {
            static float x_sum = 0.0;
            static float y_sum = 0.0;

            x_sum += _motion_orientation.x;
            y_sum += _motion_orientation.y;

            if (++_motion_cal_readings[MOTION_CAL_ORIENTATION_FRONT] == MOTION_CAL_READINGS)
            {
                x_sum /= (float) MOTION_CAL_READINGS;
                y_sum /= (float) MOTION_CAL_READINGS;

                _motion_config.front_cross_axis_factor = x_sum / y_sum;

                /* Reset static values for next calibration. */
                _motion_cal[MOTION_CAL_ORIENTATION_FRONT] = false;

                PRINT (INFO, "\r\nCalibration complete!\r\n");

                /* Write new settings to file. */
                _motion_config_write ();

                /* Reset sums for next calibration. */
                x_sum = 0.0;
                y_sum = 0.0;
            }
            else if (_motion_cal_readings[MOTION_CAL_ORIENTATION_FRONT] % 10 == 0)
            {
                PRINT (INFO, ".\r\n");
            }
        }
        else if (_motion_gyro_recalibrate)
        {
            _motion_recal_gyro.offset.x += _motion_gyro_data.x;
            _motion_recal_gyro.offset.y += _motion_gyro_data.y;
            _motion_recal_gyro.offset.z += _motion_gyro_data.z;

            if (++_motion_recal_readings == MOTION_GYRO_RECAL_READINGS)
            {   
                _motion_config.gyro_cal.offset.x = _motion_recal_gyro.offset.x / (float) MOTION_GYRO_RECAL_READINGS;
                _motion_config.gyro_cal.offset.y = _motion_recal_gyro.offset.y / (float) MOTION_GYRO_RECAL_READINGS;
                _motion_config.gyro_cal.offset.z = _motion_recal_gyro.offset.z / (float) MOTION_GYRO_RECAL_READINGS;

                /* Reset static values for next calibration. */
                _motion_gyro_recalibrate = false;

                /* Write new settings to file. */
                _motion_config_write ();
            }
        }

        if (_motion_data_dump)
        {
            if (_motion_timeout_cnt >= _motion_data_dump_ms)
            {
                /* Reset timeout count. */
                _motion_timeout_cnt = 0;
                /* Reset data dump flag. */
                _motion_data_dump = false;
            }
            else
            {
                /* Increment timeout counter. */
                _motion_timeout_cnt += MOTION_TASK_PERIOD_MS;
            }

            // PRINT (INFO, "x = %.3f, y = %.3f\r\n", (double) _motion_orientation.y,
            //                                        (double) _motion_orientation.x);
        }

        /* Feed watchdog at end of loop. */
        WDT_feed ();

        k_msleep (MOTION_TASK_PERIOD_MS);
    }
}

static void _motion_config_read (void)
{
    ERRNO_e         err;
    MOTION_config_s cfg;

    err = FS_open_or_create (MOTION_FILENAME, &_motion_file, sizeof (MOTION_config_s), (FS_FILE_DEPTH_DEFAULT * 8));
    if (err == ERRNO_SUCCESS)
    {
        /* Read file. */
        err = FS_read (_motion_file, (uint8_t *) &cfg, 0, sizeof (MOTION_config_s));

        /* Check file entry is valid. */
        if (err != ERRNO_SUCCESS)
        { 
            /* Write defaults to file. */
            _motion_config_write ();
            return;
        }

        /* Validate read file. */
        // FIXME: find better way to validate
        if (isnan (cfg.accel_cal.offset.x) || isnan (cfg.accel_cal.offset.y) || isnan (cfg.accel_cal.offset.z))
        {
            cfg.accel_cal.offset = (MOTION_vector_s) { .x = 0.0, .y = 0.0, .z = 0.0 };
        }
        if (isnan (cfg.accel_cal.scalar.x) || isnan (cfg.accel_cal.scalar.y) || isnan (cfg.accel_cal.scalar.z))
        {
            cfg.accel_cal.scalar = (MOTION_vector_s) { .x = 1.0, .y = 1.0, .z = 1.0 };
        }
        if (isnan (cfg.gyro_cal.offset.x) || isnan (cfg.gyro_cal.offset.y) || isnan (cfg.gyro_cal.offset.z))
        {
            cfg.gyro_cal.offset = (MOTION_vector_s) { .x = 0.0, .y = 0.0, .z = 0.0 };
        }
        if (isnan (cfg.gyro_cal.scalar.x) || isnan (cfg.gyro_cal.scalar.y) || isnan (cfg.gyro_cal.scalar.z))
        {
            cfg.gyro_cal.scalar = (MOTION_vector_s) { .x = 1.0, .y = 1.0, .z = 1.0 };
        }
        if (isnan (cfg.angular_threshold))
        {
            cfg.angular_threshold = MOTION_ANG_THRESHOLD_DEFAULT;
        }

        /* Update motion configuration to what was read from flash and validated. */
        _motion_config = cfg;
    }
    else if (err == ERRNO_SIZE_MISMATCH)
    {
        /* Config has been modified, need to delete old file and write new configuration. */
        err = FS_delete (MOTION_FILENAME);

        if (err != ERRNO_SUCCESS)
        {
            /* Something else is going on... */
            PRINT (ERROR, "Failure to delete old motion file.\r\n");
            return;
        }

        /* Create new file. */
        err = FS_open_or_create (MOTION_FILENAME, &_motion_file, sizeof (MOTION_config_s), FS_FILE_DEPTH_DEFAULT);
        if (err != ERRNO_SUCCESS)
        {
            PRINT (ERROR, "Failure to create motion configuration file.\r\n");
        }
        else
        {
            PRINT (INFO, "New motion file created successfully\r\n");
        }

        /* Write default config to file. */
        _motion_config_write ();
    }
    else
    {
        /* Write defaults to file. */
        _motion_config_write ();
    }
}

static void _motion_config_write (void)
{
    if (FS_write (_motion_file, (uint8_t *) &_motion_config) != ERRNO_SUCCESS)
    {
        PRINT (ERROR, "Failed to write motion configuration to file.\r\n");
    }
}

static void _motion_data_process (MOTION_vector_s accel_raw, MOTION_vector_s gyro_raw, float dt)
{
    /* Apply calibration parameters to raw data. */
    MOTION_vector_s accel_corrected = _motion_apply_cal (accel_raw, _motion_config.accel_cal);
    MOTION_vector_s gyro_corrected  = _motion_apply_cal (gyro_raw, _motion_config.gyro_cal);

    if (_motion_data_dump)
    {
        PRINT (INFO, "%.3f, %.3f, %.3f, %.3f, %.3f, %.3f\r\n", accel_corrected.x, accel_corrected.y, accel_corrected.z,
                                                               gyro_corrected.x, gyro_corrected.y, gyro_corrected.z);
    }

    /* Integrate gyro rates. */
    float gyro_x  = _motion_orientation.x + gyro_corrected.x * dt;
    float gyro_y  = _motion_orientation.y + gyro_corrected.y * dt;

    float accel_x = atan2f (accel_corrected.y, sqrtf(accel_corrected.x * accel_corrected.x + accel_corrected.z * accel_corrected.z)) * 180.0f / _MOTION_PI;
    float accel_y = atan2f(-accel_corrected.x, sqrtf(accel_corrected.y * accel_corrected.y + accel_corrected.z * accel_corrected.z)) * 180.0f / _MOTION_PI;

    /* Smoothing filter. */
    _motion_orientation.x = _MOTION_ALPHA * gyro_x + ((1.0f - _MOTION_ALPHA) * accel_x);
    _motion_orientation.y = _MOTION_ALPHA * gyro_y + ((1.0f - _MOTION_ALPHA) * accel_y);
    /* Z angle cannot be determined from accel/gyro alone (need magnetometer). */
    _motion_orientation.z += (gyro_corrected.z * dt);
    
    /* Compute magnitude of angular velocity (deg/s) */
    _motion_angular_mag = sqrtf ((gyro_corrected.x * gyro_corrected.x) + 
                                 (gyro_corrected.y * gyro_corrected.y) + 
                                 (gyro_corrected.z * gyro_corrected.z));
}

static MOTION_vector_s _motion_apply_cal (MOTION_vector_s raw, MOTION_calibration_data_s cal)
{
    return (MOTION_vector_s) {
        .x = (raw.x - cal.offset.x) * cal.scalar.x,
        .y = (raw.y - cal.offset.y) * cal.scalar.y,
        .z = (raw.z - cal.offset.z) * cal.scalar.z
    };
}

