/*
 $License:
   Copyright 2011 InvenSense, Inc.

 Licensed under the Apache License, Version 2.0 (the "License");
 you may not use this file except in compliance with the License.
 You may obtain a copy of the License at

 http://www.apache.org/licenses/LICENSE-2.0

 Unless required by applicable law or agreed to in writing, software
 distributed under the License is distributed on an "AS IS" BASIS,
 WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 See the License for the specific language governing permissions and
 limitations under the License.
  $
 */
/******************************************************************************
 *
 * $Id: mlarray_legacy.c $
 *
 *****************************************************************************/

/** 
 *  @defgroup MLArray_Legacy 
 *  @brief  Legacy Motion Library Array APIs.
 *          The Motion Library Array APIs provide the user access to the
 *          Motion Library state. These Legacy APIs provide access to
 *          individual state arrays using a data set name as the first
 *          argument to the API. This format has been replaced by unique
 *          named APIs for each data set, found in the MLArray group.
 *
 *  @{
 *      @file   mlarray_legacy.c
 *      @brief  The Legacy Motion Library Array APIs.
 */

#include "ml.h"
#include "mltypes.h"
#include "mlinclude.h"
#include "mlFIFO.h"
#include "mldl_cfg.h"

/**
 *  @brief  inv_get_array is used to get an array of processed motion sensor data.
 *          inv_get_array can be used to retrieve various data sets. Certain data
 *          sets require functions to be enabled using MLEnable in order to be
 *          valid.
 *
 *          The available data sets are:
 *
 *          - INV_ROTATION_MATRIX
 *          - INV_QUATERNION
 *          - INV_EULER_ANGLES_X
 *          - INV_EULER_ANGLES_Y
 *          - INV_EULER_ANGLES_Z
 *          - INV_EULER_ANGLES
 *          - INV_LINEAR_ACCELERATION
 *          - INV_LINEAR_ACCELERATION_WORLD
 *          - INV_GRAVITY
 *          - INV_ANGULAR_VELOCITY
 *          - INV_RAW_DATA
 *          - INV_GYROS
 *          - INV_ACCELS
 *          - INV_MAGNETOMETER
 *          - INV_GYRO_BIAS
 *          - INV_ACCEL_BIAS
 *          - INV_MAG_BIAS 
 *          - INV_HEADING
 *          - INV_MAG_BIAS_ERROR
 *          - INV_PRESSURE
 *
 *          Please refer to the documentation of inv_get_float_array() for a 
 *          description of these data sets.
 *
 *  @pre    MLDmpOpen() or MLDmpPedometerStandAloneOpen()
 *          must have been called.
 *
 *  @param  dataSet     
 *              A constant specifying an array of data processed by the
 *              motion processor.
 *  @param  data        
 *              A pointer to an array to be passed back to the user.
 *              <b>Must be 9 cells long at least</b>.
 *
 *  @return Zero if the command is successful; an ML error code otherwise.
 */
inv_error_t inv_get_array(int dataSet, long *data)
{
    inv_error_t result;
    switch (dataSet) {
    case INV_GYROS:
        result = inv_get_gyro(data);
        break;
    case INV_ACCELS:
        result = inv_get_accel(data);
        break;
    case INV_TEMPERATURE:
        result = inv_get_temperature(data);
        break;
    case INV_ROTATION_MATRIX:
        result = inv_get_rot_mat(data);
        break;
    case INV_QUATERNION:
        result = inv_get_quaternion(data);
        break;
    case INV_LINEAR_ACCELERATION:
        result = inv_get_linear_accel(data);
        break;
    case INV_LINEAR_ACCELERATION_WORLD:
        result = inv_get_linear_accel_in_world(data);
        break;
    case INV_GRAVITY:
        result = inv_get_gravity(data);
        break;
    case INV_ANGULAR_VELOCITY:
        result = inv_get_angular_velocity(data);
        break;
    case INV_EULER_ANGLES:
        result = inv_get_euler_angles(data);
        break;
    case INV_EULER_ANGLES_X:
        result = inv_get_euler_angles_x(data);
        break;
    case INV_EULER_ANGLES_Y:
        result = inv_get_euler_angles_y(data);
        break;
    case INV_EULER_ANGLES_Z:
        result = inv_get_euler_angles_z(data);
        break;
    case INV_GYRO_TEMP_SLOPE:
        result = inv_get_gyro_temp_slope(data);
        break;
    case INV_GYRO_BIAS:
        result = inv_get_gyro_bias(data);
        break;
    case INV_ACCEL_BIAS:
        result = inv_get_accel_bias(data);
        break;
    case INV_MAG_BIAS:
        result = inv_get_mag_bias(data);
        break;
    case INV_RAW_DATA:
        result = inv_get_gyro_and_accel_sensor(data);
        break;
    case INV_MAG_RAW_DATA:
        result = inv_get_mag_raw_data(data);
        break;
    case INV_MAGNETOMETER:
        result = inv_get_magnetometer(data);
        break;
    case INV_PRESSURE:
        result = inv_get_pressure(data);
        break;
    case INV_HEADING:
        result = inv_get_heading(data);
        break;
    case INV_GYRO_CALIBRATION_MATRIX:
        result = inv_get_gyro_cal_matrix(data);
        break;
    case INV_ACCEL_CALIBRATION_MATRIX:
        result = inv_get_accel_cal_matrix(data);
        break;
    case INV_MAG_CALIBRATION_MATRIX:
        result = inv_get_mag_cal_matrix(data);
        break;
    case INV_MAG_BIAS_ERROR:
        result = inv_get_mag_bias_error(data);
        break;
    case INV_MAG_SCALE:
        result = inv_get_mag_scale(data);
        break;
    case INV_LOCAL_FIELD:
        result = inv_get_local_field(data);
        break;
    case INV_RELATIVE_QUATERNION:
        result = inv_get_relative_quaternion(data);
        break;
    default:
        return INV_ERROR_INVALID_PARAMETER;
        break;
    }
    return result;
}

/**
 *  @brief  inv_get_float_array is used to get an array of processed motion sensor 
 *          data. inv_get_array can be used to retrieve various data sets. 
 *          Certain data sets require functions to be enabled using MLEnable 
 *          in order to be valid.
 *
 *          The available data sets are:
 *
 *          - INV_ROTATION_MATRIX :
 *          Returns an array of nine data points representing the rotation 
 *          matrix generated from all available sensors. 
 *          This requires that ML_SENSOR_FUSION be enabled.
 *          The array format will be R11, R12, R13, R21, R22, R23, R31, R32, 
 *          R33, representing the matrix:
 *          <center>R11 R12 R13</center>
 *          <center>R21 R22 R23</center>
 *          <center>R31 R32 R33</center>
 *          <b>Please refer to the "9-Axis Sensor Fusion Application Note" document, 
 *          section 7 "Sensor Fusion Output", for details regarding rotation 
 *          matrix output</b>.
 *
 *          - INV_QUATERNION :
 *          Returns an array of four data points representing the quaternion 
 *          generated from all available sensors. 
 *          This requires that ML_SENSOR_FUSION be enabled.
 *
 *          - INV_EULER_ANGLES_X :
 *          Returns an array of three data points representing roll, pitch, and 
 *          yaw using the X axis of the gyroscope, accelerometer, and compass as 
 *          reference axis. 
 *          This is typically the convention used for mobile devices where the X
 *          axis is the width of the screen, Y axis is the height, and Z the 
 *          depth. In this case roll is defined as the rotation around the X 
 *          axis of the device.
 *          The euler angles convention for this output is the following:
 *          <TABLE>
 *          <TR><TD><b>EULER ANGLE</b></TD><TD><b>ROTATION AROUND</b></TD></TR>
 *          <TR><TD>roll              </TD><TD>X axis                </TD></TR>
 *          <TR><TD>pitch             </TD><TD>Y axis                </TD></TR>
 *          <TR><TD>yaw               </TD><TD>Z axis                </TD></TR>
 *          </TABLE>
 *          INV_EULER_ANGLES_X corresponds to the INV_EULER_ANGLES output and is 
 *          therefore the default convention.
 *
 *          - INV_EULER_ANGLES_Y :
 *          Returns an array of three data points representing roll, pitch, and 
 *          yaw using the Y axis of the gyroscope, accelerometer, and compass as 
 *          reference axis. 
 *          This convention is typically used in augmented reality applications, 
 *          where roll is defined as the rotation around the axis along the 
 *          height of the screen of a mobile device, namely the Y axis.
 *          The euler angles convention for this output is the following:
 *          <TABLE>
 *          <TR><TD><b>EULER ANGLE</b></TD><TD><b>ROTATION AROUND</b></TD></TR>
 *          <TR><TD>roll              </TD><TD>Y axis                </TD></TR>
 *          <TR><TD>pitch             </TD><TD>X axis                </TD></TR>
 *          <TR><TD>yaw               </TD><TD>Z axis                </TD></TR>
 *          </TABLE>
 *
 *          - INV_EULER_ANGLES_Z :
 *          Returns an array of three data points representing roll, pitch, and 
 *          yaw using the Z axis of the gyroscope, accelerometer, and compass as 
 *          reference axis. 
 *          This convention is mostly used in application involving the use 
 *          of a camera, typically placed on the back of a mobile device, that 
 *          is along the Z axis.  In this convention roll is defined as the 
 *          rotation around the Z axis.
 *          The euler angles convention for this output is the following:
 *          <TABLE>
 *          <TR><TD><b>EULER ANGLE</b></TD><TD><b>ROTATION AROUND</b></TD></TR>
 *          <TR><TD>roll              </TD><TD>Z axis                </TD></TR>
 *          <TR><TD>pitch             </TD><TD>X axis                </TD></TR>
 *          <TR><TD>yaw               </TD><TD>Y axis                </TD></TR>
 *          </TABLE>
 *
 *          - INV_EULER_ANGLES :
 *          Returns an array of three data points representing roll, pitch, and 
 *          yaw corresponding to the INV_EULER_ANGLES_X output and it is 
 *          therefore the default convention for Euler angles.
 *          Please refer to the INV_EULER_ANGLES_X for a detailed description.
 *
 *          - INV_LINEAR_ACCELERATION :
 *          Returns an array of three data points representing the linear 
 *          acceleration as derived from both gyroscopes and accelerometers. 
 *          This requires that ML_SENSOR_FUSION be enabled.
 *
 *          - INV_LINEAR_ACCELERATION_WORLD :
 *          Returns an array of three data points representing the linear 
 *          acceleration in world coordinates, as derived from both gyroscopes 
 *          and accelerometers.
 *          This requires that ML_SENSOR_FUSION be enabled.
 *
 *          - INV_GRAVITY :
 *          Returns an array of three data points representing the direction 
 *          of gravity in body coordinates, as derived from both gyroscopes 
 *          and accelerometers.
 *          This requires that ML_SENSOR_FUSION be enabled.
 *
 *          - INV_ANGULAR_VELOCITY :
 *          Returns an array of three data points representing the angular 
 *          velocity as derived from <b>both</b> gyroscopes and accelerometers.
 *          This requires that ML_SENSOR_FUSION be enabled, to fuse data from
 *          the gyroscope and accelerometer device, appropriately scaled and 
 *          oriented according to the respective mounting matrices.
 *
 *          - INV_RAW_DATA :
 *          Returns an array of nine data points representing raw sensor data 
 *          of the gyroscope X, Y, Z, accelerometer X, Y, Z, and 
 *          compass X, Y, Z values.
 *          These values are not scaled and come out directly from the devices'
 *          sensor data output. In case of accelerometers with lower output 
 *          resolution, e.g 8-bit, the sensor data is scaled up to match the 
 *          2^14 = 1 gee typical representation for a +/- 2 gee full scale 
 *          range.
 *
 *          - INV_GYROS :
 *          Returns an array of three data points representing the X gyroscope,
 *          Y gyroscope, and Z gyroscope values.
 *          The values are not sensor fused with other sensor types data but
 *          reflect the orientation from the mounting matrices in use.
 *          The INV_GYROS values are scaled to ensure 1 dps corresponds to 2^16 
 *          codes.
 *
 *          - INV_ACCELS :
 *          Returns an array of three data points representing the X 
 *          accelerometer, Y accelerometer, and Z accelerometer values.
 *          The values are not sensor fused with other sensor types data but
 *          reflect the orientation from the mounting matrices in use.
 *          The INV_ACCELS values are scaled to ensure 1 gee corresponds to 2^16
 *          codes.
 *
 *          - INV_MAGNETOMETER :
 *          Returns an array of three data points representing the compass
 *          X, Y, and Z values.
 *          The values are not sensor fused with other sensor types data but
 *          reflect the orientation from the mounting matrices in use.
 *          The INV_MAGNETOMETER values are scaled to ensure 1 micro Tesla (uT) 
 *          corresponds to 2^16 codes.
 *
 *          - INV_GYRO_BIAS :
 *          Returns an array of three data points representing the gyroscope 
 *          biases.
 *
 *          - INV_ACCEL_BIAS :
 *          Returns an array of three data points representing the 
 *          accelerometer biases.
 *
 *          - INV_MAG_BIAS :
 *          Returns an array of three data points representing the compass 
 *          biases.
 *
 *          - INV_GYRO_CALIBRATION_MATRIX :
 *          Returns an array of nine data points representing the calibration 
 *          matrix for the gyroscopes:
 *          <center>C11 C12 C13</center>
 *          <center>C21 C22 C23</center>
 *          <center>C31 C32 C33</center>
 *
 *          - INV_ACCEL_CALIBRATION_MATRIX :
 *          Returns an array of nine data points representing the calibration 
 *          matrix for the accelerometers:
 *          <center>C11 C12 C13</center>
 *          <center>C21 C22 C23</center>
 *          <center>C31 C32 C33</center>
 *
 *          - INV_MAG_CALIBRATION_MATRIX :
 *          Returns an array of nine data points representing the calibration 
 *          matrix for the compass:
 *          <center>C11 C12 C13</center>
 *          <center>C21 C22 C23</center>
 *          <center>C31 C32 C33</center>
 *
 *          - INV_PRESSURE :
 *          Returns a single value representing the pressure in Pascal
 *
 *          - INV_HEADING : 
 *          Returns a single number representing the heading of the device 
 *          relative to the Earth, in which 0 represents North, 90 degrees 
 *          represents East, and so on. 
 *          The heading is defined as the direction of the +Y axis if the Y 
 *          axis is horizontal, and otherwise the direction of the -Z axis.
 *
 *          - INV_MAG_BIAS_ERROR :
 *          Returns an array of three numbers representing the current estimated
 *          error in the compass biases. These numbers are unitless and serve
 *          as rough estimates in which numbers less than 100 typically represent
 *          reasonably well calibrated compass axes.
 *
 *  @pre    MLDmpOpen() or MLDmpPedometerStandAloneOpen()
 *          must have been called.
 *
 *  @param  dataSet     
 *              A constant specifying an array of data processed by 
 *              the motion processor.
 *  @param  data        
 *              A pointer to an array to be passed back to the user.
 *              <b>Must be 9 cells long at least</b>.
 *
 *  @return INV_SUCCESS if the command is successful; an error code otherwise.
 */
inv_error_t inv_get_float_array(int dataSet, float *data)
{
    inv_error_t result;
    switch (dataSet) {
    case INV_GYROS:
        result = inv_get_gyro_float(data);
        break;
    case INV_ACCELS:
        result = inv_get_accel_float(data);
        break;
    case INV_TEMPERATURE:
        result = inv_get_temperature_float(data);
        break;
    case INV_ROTATION_MATRIX:
        result = inv_get_rot_mat_float(data);
        break;
    case INV_QUATERNION:
        result = inv_get_quaternion_float(data);
        break;
    case INV_LINEAR_ACCELERATION:
        result = inv_get_linear_accel_float(data);
        break;
    case INV_LINEAR_ACCELERATION_WORLD:
        result = inv_get_linear_accel_in_world_float(data);
        break;
    case INV_GRAVITY:
        result = inv_get_gravity_float(data);
        break;
    case INV_ANGULAR_VELOCITY:
        result = inv_get_angular_velocity_float(data);
        break;
    case INV_EULER_ANGLES:
        result = inv_get_euler_angles_float(data);
        break;
    case INV_EULER_ANGLES_X:
        result = inv_get_euler_angles_x_float(data);
        break;
    case INV_EULER_ANGLES_Y:
        result = inv_get_euler_angles_y_float(data);
        break;
    case INV_EULER_ANGLES_Z:
        result = inv_get_euler_angles_z_float(data);
        break;
    case INV_GYRO_TEMP_SLOPE:
        result = inv_get_gyro_temp_slope_float(data);
        break;
    case INV_GYRO_BIAS:
        result = inv_get_gyro_bias_float(data);
        break;
    case INV_ACCEL_BIAS:
        result = inv_get_accel_bias_float(data);
        break;
    case INV_MAG_BIAS:
        result = inv_get_mag_bias_float(data);
        break;
    case INV_RAW_DATA:
        result = inv_get_gyro_and_accel_sensor_float(data);
        break;
    case INV_MAG_RAW_DATA:
        result = inv_get_mag_raw_data_float(data);
        break;
    case INV_MAGNETOMETER:
        result = inv_get_magnetometer_float(data);
        break;
    case INV_PRESSURE:
        result = inv_get_pressure_float(data);
        break;
    case INV_HEADING:
        result = inv_get_heading_float(data);
        break;
    case INV_GYRO_CALIBRATION_MATRIX:
        result = inv_get_gyro_cal_matrix_float(data);
        break;
    case INV_ACCEL_CALIBRATION_MATRIX:
        result = inv_get_accel_cal_matrix_float(data);
        break;
    case INV_MAG_CALIBRATION_MATRIX:
        result = inv_get_mag_cal_matrix_float(data);
        break;
    case INV_MAG_BIAS_ERROR:
        result = inv_get_mag_bias_error_float(data);
        break;
    case INV_MAG_SCALE:
        result = inv_get_mag_scale_float(data);
        break;
    case INV_LOCAL_FIELD:
        result = inv_get_local_field_float(data);
        break;
    case INV_RELATIVE_QUATERNION:
        result = inv_get_relative_quaternion_float(data);
        break;
    default:
        return INV_ERROR_INVALID_PARAMETER;
        break;
    }
    return result;
}

/**
 *  @brief  used to set an array of motion sensor data.
 *          Handles the following data sets:
 *          - INV_GYRO_BIAS
 *          - INV_ACCEL_BIAS
 *          - INV_MAG_BIAS
 *          - INV_GYRO_TEMP_SLOPE
 *
 *          For more details about the use of the data sets
 *          please refer to the documentation of inv_set_float_array().
 *
 *          Please also refer to the provided "9-Axis Sensor Fusion 
 *          Application Note" document provided.
 *
 *  @pre    MLDmpOpen() or 
 *          MLDmpPedometerStandAloneOpen() 
 *  @pre    MLDmpStart() must <b>NOT</b> have been called.
 *
 *  @param  dataSet     A constant specifying an array of data.
 *  @param  data        A pointer to an array to be copied from the user.
 *
 *  @return INV_SUCCESS if successful; a non-zero error code otherwise.
 */
inv_error_t inv_set_array(int dataSet, long *data)
{
    INVENSENSE_FUNC_START;
    inv_error_t result;
    switch (dataSet) {
    case INV_GYRO_BIAS:
        result = inv_set_gyro_bias(data);
        break;
    case INV_ACCEL_BIAS:
        result = inv_set_accel_bias(data);
        break;
    case INV_MAG_BIAS:
        result = inv_set_mag_bias(data);
        break;
    case INV_GYRO_TEMP_SLOPE:
        result = inv_set_gyro_temp_slope(data);
        break;
    case INV_LOCAL_FIELD:
        result = inv_set_local_field(data);
        break;
    case INV_MAG_SCALE:
        result = inv_set_mag_scale(data);
        break;
    default:
        return INV_ERROR_INVALID_PARAMETER;
        break;
    }
    return result;
}

/**
 *  @brief  used to set an array of motion sensor data.
 *          Handles various data sets:
 *          - INV_GYRO_BIAS
 *          - INV_ACCEL_BIAS
 *          - INV_MAG_BIAS
 *          - INV_GYRO_TEMP_SLOPE
 *
 *          Please refer to the provided "9-Axis Sensor Fusion Application
 *          Note" document provided.
 *
 *  @pre    MLDmpOpen() or 
 *          MLDmpPedometerStandAloneOpen() 
 *  @pre    MLDmpStart() must <b>NOT</b> have been called.
 *
 *  @param  dataSet     A constant specifying an array of data.
 *  @param  data        A pointer to an array to be copied from the user.
 *
 *  @return INV_SUCCESS if successful; a non-zero error code otherwise.
 */
inv_error_t inv_set_float_array(int dataSet, float *data)
{
    INVENSENSE_FUNC_START;
    inv_error_t result;

    switch (dataSet) {
    case INV_GYRO_TEMP_SLOPE:  // internal
        result = inv_set_gyro_temp_slope_float(data);
        break;
    case INV_GYRO_BIAS:        // internal
        result = inv_set_gyro_bias_float(data);
        break;
    case INV_ACCEL_BIAS:       // internal
        result = inv_set_accel_bias_float(data);
        break;
    case INV_MAG_BIAS:         // internal            
        result = inv_set_mag_bias_float(data);
        break;
    case INV_LOCAL_FIELD:      // internal     
        result = inv_set_local_field_float(data);
        break;
    case INV_MAG_SCALE:        // internal            
        result = inv_set_mag_scale_float(data);
        break;
    default:
        result = INV_ERROR_INVALID_PARAMETER;
        break;
    }

    return result;
}