/*
 $License:
    Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
    See included License.txt for License information.
 $
 */
 
/**
 *   @defgroup  HAL_Outputs hal_outputs
 *   @brief     Motion Library - HAL Outputs
 *              Sets up common outputs for HAL
 *
 *   @{
 *       @file hal_outputs.c
 *       @brief HAL Outputs.
 */
#include "hal_outputs.h"
#include "log.h"
#include "ml_math_func.h"
#include "mlmath.h"
#include "start_manager.h"
#include "data_builder.h"
#include "results_holder.h"

struct hal_output_t {
    int accuracy_mag; /**< Compass accuracy */
    int accuracy_gyro; /**< Gyro Accuracy */
    int accuracy_accel;	/**< Accel Accuracy */
    inv_time_t nav_timestamp;
    inv_time_t gam_timestamp;
    inv_time_t accel_timestamp;
    long nav_quat[4];
    int gyro_status;
    int accel_status;
    int compass_status;
    int nine_axis_status;
};

static struct hal_output_t hal_out;

/** Acceleration (m/s^2) in body frame.
* @param[out] values Acceleration in m/s^2 includes gravity. So while not in motion, it
*             should return a vector of magnitude near 9.81 m/s^2
* @param[out] accuracy Accuracy of the measurment, 0 is least accurate, while 3 is most accurate.
* @param[out] timestamp The timestamp for this sensor. Derived from the timestamp sent to
*             inv_build_accel().
* @return     Returns 1 if the data was updated or 0 if it was not updated.
*/
int inv_get_sensor_type_accelerometer(float *values, int8_t *accuracy,
                                       inv_time_t * timestamp)
{
    int status;
    /* Converts fixed point to m/s^2. Fixed point has 1g = 2^16.
     * So this 9.80665 / 2^16 */
#define ACCEL_CONVERSION 0.000149637603759766f
    long accel[3];
    inv_get_accel_set(accel, accuracy, timestamp);
    values[0] = accel[0] * ACCEL_CONVERSION;
    values[1] = accel[1] * ACCEL_CONVERSION;
    values[2] = accel[2] * ACCEL_CONVERSION;
    if (hal_out.accel_status & INV_NEW_DATA)
        status = 1;
    else
        status = 0;
    return status;
}

/** Linear Acceleration (m/s^2) in Body Frame.
* @param[out] values Linear Acceleration in body frame, length 3, (m/s^2). May show
*             accel biases while at rest.
* @param[out] accuracy Accuracy of the measurment, 0 is least accurate, while 3 is most accurate.
* @param[out] timestamp The timestamp for this sensor. Derived from the timestamp sent to
*             inv_build_accel().
* @return     Returns 1 if the data was updated or 0 if it was not updated.
*/
int inv_get_sensor_type_linear_acceleration(float *values, int8_t *accuracy,
        inv_time_t * timestamp)
{
    long gravity[3], accel[3];

    inv_get_accel_set(accel, accuracy, timestamp);
    inv_get_gravity(gravity);
    accel[0] -= gravity[0] >> 14;
    accel[1] -= gravity[1] >> 14;
    accel[2] -= gravity[2] >> 14;
    values[0] = accel[0] * ACCEL_CONVERSION;
    values[1] = accel[1] * ACCEL_CONVERSION;
    values[2] = accel[2] * ACCEL_CONVERSION;

    return hal_out.nine_axis_status;
}

/** Gravity vector (m/s^2) in Body Frame.
* @param[out] values Gravity vector in body frame, length 3, (m/s^2)
* @param[out] accuracy Accuracy of the measurment, 0 is least accurate, while 3 is most accurate.
* @param[out] timestamp The timestamp for this sensor. Derived from the timestamp sent to
*             inv_build_accel().
* @return     Returns 1 if the data was updated or 0 if it was not updated.
*/
int inv_get_sensor_type_gravity(float *values, int8_t *accuracy,
                                 inv_time_t * timestamp)
{
    long gravity[3];
    int status;

    *accuracy = hal_out.accuracy_mag;
    *timestamp = hal_out.nav_timestamp;
    inv_get_gravity(gravity);
    values[0] = (gravity[0] >> 14) * ACCEL_CONVERSION;
    values[1] = (gravity[1] >> 14) * ACCEL_CONVERSION;
    values[2] = (gravity[2] >> 14) * ACCEL_CONVERSION;
    if ((hal_out.accel_status & INV_NEW_DATA) || (hal_out.gyro_status & INV_NEW_DATA))
        status = 1;
    else
        status = 0;
    return status;
}

/** Gyroscope data (rad/s) in body frame.
* @param[out] values Rotation Rate in rad/sec.
* @param[out] accuracy Accuracy of the measurment, 0 is least accurate, while 3 is most accurate.
* @param[out] timestamp The timestamp for this sensor. Derived from the timestamp sent to
*             inv_build_gyro().
* @return     Returns 1 if the data was updated or 0 if it was not updated.
*/
int inv_get_sensor_type_gyroscope(float *values, int8_t *accuracy,
                                   inv_time_t * timestamp)
{
    /* Converts fixed point to rad/sec. Fixed point has 1 dps = 2^16.
     * So this is: pi / 2^16 / 180 */
#define GYRO_CONVERSION 2.66316109007924e-007f
    long gyro[3];
    int status;

    inv_get_gyro_set(gyro, accuracy, timestamp);
    values[0] = gyro[0] * GYRO_CONVERSION;
    values[1] = gyro[1] * GYRO_CONVERSION;
    values[2] = gyro[2] * GYRO_CONVERSION;
    if (hal_out.gyro_status & INV_NEW_DATA)
        status = 1;
    else
        status = 0;
    return status;
}

/**
* This corresponds to Sensor.TYPE_ROTATION_VECTOR.
* The rotation vector represents the orientation of the device as a combination
* of an angle and an axis, in which the device has rotated through an angle @f$\theta@f$
* around an axis {x, y, z}. <br>
* The three elements of the rotation vector are
* {x*sin(@f$\theta@f$/2), y*sin(@f$\theta@f$/2), z*sin(@f$\theta@f$/2)}, such that the magnitude of the rotation
* vector is equal to sin(@f$\theta@f$/2), and the direction of the rotation vector is
* equal to the direction of the axis of rotation.
*
* The three elements of the rotation vector are equal to the last three components of a unit quaternion
* {x*sin(@f$\theta@f$/2), y*sin(@f$\theta@f$/2), z*sin(@f$\theta@f$/2)>. The 4th element is cos(@f$\theta@f$/2).
*
* Elements of the rotation vector are unitless. The x,y and z axis are defined in the same way as the acceleration sensor.
* The reference coordinate system is defined as a direct orthonormal basis, where:

    -X is defined as the vector product Y.Z (It is tangential to the ground at the device's current location and roughly points East).
    -Y is tangential to the ground at the device's current location and points towards the magnetic North Pole.
    -Z points towards the sky and is perpendicular to the ground.
* @param[out] values Length 4.
* @param[out] accuracy Accuracy 0 to 3, 3 = most accurate
* @param[out] timestamp Timestamp. In (ns) for Android.
* @return     Returns 1 if the data was updated or 0 if it was not updated.
*/
int inv_get_sensor_type_rotation_vector(float *values, int8_t *accuracy,
        inv_time_t * timestamp)
{
    *accuracy = hal_out.accuracy_mag;
    *timestamp = hal_out.nav_timestamp;

    if (hal_out.nav_quat[0] >= 0) {
        values[0] = hal_out.nav_quat[1] * INV_TWO_POWER_NEG_30;
        values[1] = hal_out.nav_quat[2] * INV_TWO_POWER_NEG_30;
        values[2] = hal_out.nav_quat[3] * INV_TWO_POWER_NEG_30;
        values[3] = hal_out.nav_quat[0] * INV_TWO_POWER_NEG_30;
    } else {
        values[0] = -hal_out.nav_quat[1] * INV_TWO_POWER_NEG_30;
        values[1] = -hal_out.nav_quat[2] * INV_TWO_POWER_NEG_30;
        values[2] = -hal_out.nav_quat[3] * INV_TWO_POWER_NEG_30;
        values[3] = -hal_out.nav_quat[0] * INV_TWO_POWER_NEG_30;
    }
    values[4] = inv_get_heading_confidence_interval();

    return hal_out.nine_axis_status;
}


/** Compass data (uT) in body frame.
* @param[out] values Compass data in (uT), length 3. May be calibrated by having
*             biases removed and sensitivity adjusted
* @param[out] accuracy Accuracy 0 to 3, 3 = most accurate
* @param[out] timestamp Timestamp. In (ns) for Android.
* @return     Returns 1 if the data was updated or 0 if it was not updated.
*/
int inv_get_sensor_type_magnetic_field(float *values, int8_t *accuracy,
                                        inv_time_t * timestamp)
{
    int status;
    /* Converts fixed point to uT. Fixed point has 1 uT = 2^16.
     * So this is: 1 / 2^16*/
#define COMPASS_CONVERSION 1.52587890625e-005f
    long compass[3];
    inv_get_compass_set(compass, accuracy, timestamp);
    values[0] = compass[0] * COMPASS_CONVERSION;
    values[1] = compass[1] * COMPASS_CONVERSION;
    values[2] = compass[2] * COMPASS_CONVERSION;
    if (hal_out.compass_status & INV_NEW_DATA)
        status = 1;
    else
        status = 0;
    return status;
}


/** This corresponds to Sensor.TYPE_ORIENTATION. All values are angles in degrees.
* @param[out] values Length 3, Degrees.<br>
*        - values[0]: Azimuth, angle between the magnetic north direction
*         and the y-axis, around the z-axis (0 to 359). 0=North, 90=East, 180=South, 270=West<br>
*        - values[1]: Pitch, rotation around x-axis (-180 to 180), with positive values
*         when the z-axis moves toward the y-axis.<br>
*        - values[2]: Roll, rotation around y-axis (-90 to 90), with positive
*          values when the x-axis moves toward the z-axis.<br>
*
* @note  This definition is different from yaw, pitch and roll used in aviation
*        where the X axis is along the long side of the plane (tail to nose).
*        Note: This sensor type exists for legacy reasons, please use getRotationMatrix()
*        in conjunction with remapCoordinateSystem() and getOrientation() to compute
*        these values instead.
*        Important note: For historical reasons the roll angle is positive in the
*        clockwise direction (mathematically speaking, it should be positive in
*        the counter-clockwise direction).
* @param[out] accuracy Accuracy of the measurment, 0 is least accurate, while 3 is most accurate.
* @param[out] timestamp The timestamp for this sensor.
* @return     Returns 1 if the data was updated or 0 if it was not updated.
*/
int inv_get_sensor_type_orientation(float *values, int8_t *accuracy,
                                     inv_time_t * timestamp)
{
    long t1, t2, t3;
    long q00, q01, q02, q03, q11, q12, q13, q22, q23, q33;

    *accuracy = hal_out.accuracy_mag;
    *timestamp = hal_out.nav_timestamp;

    q00 = inv_q29_mult(hal_out.nav_quat[0], hal_out.nav_quat[0]);
    q01 = inv_q29_mult(hal_out.nav_quat[0], hal_out.nav_quat[1]);
    q02 = inv_q29_mult(hal_out.nav_quat[0], hal_out.nav_quat[2]);
    q03 = inv_q29_mult(hal_out.nav_quat[0], hal_out.nav_quat[3]);
    q11 = inv_q29_mult(hal_out.nav_quat[1], hal_out.nav_quat[1]);
    q12 = inv_q29_mult(hal_out.nav_quat[1], hal_out.nav_quat[2]);
    q13 = inv_q29_mult(hal_out.nav_quat[1], hal_out.nav_quat[3]);
    q22 = inv_q29_mult(hal_out.nav_quat[2], hal_out.nav_quat[2]);
    q23 = inv_q29_mult(hal_out.nav_quat[2], hal_out.nav_quat[3]);
    q33 = inv_q29_mult(hal_out.nav_quat[3], hal_out.nav_quat[3]);

    /* X component of the Ybody axis in World frame */
    t1 = q12 - q03;

    /* Y component of the Ybody axis in World frame */
    t2 = q22 + q00 - (1L << 30);
    values[0] = atan2f((float) t1, (float) t2) * 180.f / (float) M_PI;
    if (values[0] < 0)
        values[0] += 360;

    /* Z component of the Ybody axis in World frame */
    t3 = q23 + q01;
    values[1] =
        -atan2f((float) t3,
                sqrtf((float) t1 * t1 +
                      (float) t2 * t2)) * 180.f / (float) M_PI;
    /* Z component of the Zbody axis in World frame */
    t2 = q33 + q00 - (1L << 30);
    if (t2 < 0) {
        if (values[1] >= 0)
            values[1] = 180.f - values[1];
        else
            values[1] = -180.f - values[1];
    }

    /* X component of the Xbody axis in World frame */
    t1 = q11 + q00 - (1L << 30);
    /* Y component of the Xbody axis in World frame */
    t2 = q12 + q03;
    /* Z component of the Xbody axis in World frame */
    t3 = q13 - q02;
    //values[2] = atan2f((float)t3,sqrtf((float)t1*t1+(float)t2*t2))*180.f/(float)M_PI;

    values[2] =
        -(atan2f((float)(q33 + q00 - (1L << 30)), (float)(q13 - q02)) *
          180.f / (float) M_PI - 90);
    if (values[2] >= 90)
        values[2] = 180 - values[2];

    if (values[2] < -90)
        values[2] = -180 - values[2];

    return hal_out.nine_axis_status;
}

/** Main callback to generate HAL outputs. Typically not called by library users.
* @param[in] sensor_cal Input variable to take sensor data whenever there is new 
* sensor data.
* @return Returns INV_SUCCESS if successful or an error code if not.
*/
inv_error_t inv_generate_hal_outputs(struct inv_sensor_cal_t *sensor_cal)
{
    int use_sensor = 0;
    long sr;
    (void) sensor_cal;
    inv_get_quaternion_set(hal_out.nav_quat, &hal_out.accuracy_mag,
                           &hal_out.nav_timestamp);
    hal_out.gyro_status = sensor_cal->gyro.status;
    hal_out.accel_status = sensor_cal->accel.status;
    hal_out.compass_status = sensor_cal->compass.status;

    // Find the highest sample rate and tie generating 9-axis to that one.
    if (sensor_cal->gyro.status & INV_SENSOR_ON) {
        sr = sensor_cal->gyro.sample_rate_ms;
        use_sensor = 0;
    }
    if ((sensor_cal->accel.status & INV_SENSOR_ON) && (sr > sensor_cal->accel.sample_rate_ms)) {
        sr = sensor_cal->accel.sample_rate_ms;
        use_sensor = 1;
    }
    if ((sensor_cal->compass.status & INV_SENSOR_ON) && (sr > sensor_cal->compass.sample_rate_ms)) {
        sr = sensor_cal->compass.sample_rate_ms;
        use_sensor = 2;
    }
    if ((sensor_cal->quat.status & INV_SENSOR_ON) && (sr > sensor_cal->quat.sample_rate_ms)) {
        sr = sensor_cal->quat.sample_rate_ms;
        use_sensor = 3;
    }

    switch (use_sensor) {
    default:
    case 0:
        hal_out.nine_axis_status = (sensor_cal->gyro.status & INV_NEW_DATA) ? 1 : 0;
        hal_out.nav_timestamp = sensor_cal->gyro.timestamp; 
        break;
    case 1:
        hal_out.nine_axis_status = (sensor_cal->accel.status & INV_NEW_DATA) ? 1 : 0;
        hal_out.nav_timestamp = sensor_cal->accel.timestamp; 
        break;
    case 2:
        hal_out.nine_axis_status = (sensor_cal->compass.status & INV_NEW_DATA) ? 1 : 0;
        hal_out.nav_timestamp = sensor_cal->compass.timestamp; 
        break;
    case 3:
        hal_out.nine_axis_status = (sensor_cal->quat.status & INV_NEW_DATA) ? 1 : 0;
        hal_out.nav_timestamp = sensor_cal->quat.timestamp; 
        break;
    }

    return INV_SUCCESS;
}

/** Turns off generation of HAL outputs.
* @return Returns INV_SUCCESS if successful or an error code if not.
 */
inv_error_t inv_stop_hal_outputs(void)
{
    inv_error_t result;
    result = inv_unregister_data_cb(inv_generate_hal_outputs);
    return result;
}

/** Turns on generation of HAL outputs. This should be called after inv_stop_hal_outputs()
* to turn generation of HAL outputs back on. It is automatically called by inv_enable_hal_outputs().
* @return Returns INV_SUCCESS if successful or an error code if not.
*/
inv_error_t inv_start_hal_outputs(void)
{
    inv_error_t result;
    result =
        inv_register_data_cb(inv_generate_hal_outputs,
                             INV_PRIORITY_HAL_OUTPUTS,
                             INV_GYRO_NEW | INV_ACCEL_NEW | INV_MAG_NEW);
    return result;
}

/** Initializes hal outputs class. This is called automatically by the
* enable function. It may be called any time the feature is enabled, but
* is typically not needed to be called by outside callers.
* @return Returns INV_SUCCESS if successful or an error code if not.
*/
inv_error_t inv_init_hal_outputs(void)
{
    memset(&hal_out, 0, sizeof(hal_out));
    return INV_SUCCESS;
}

/** Turns on creation and storage of HAL type results.
* @return Returns INV_SUCCESS if successful or an error code if not.
*/
inv_error_t inv_enable_hal_outputs(void)
{
    inv_error_t result;
    
	// don't need to check the result for inv_init_hal_outputs 
	// since it's always INV_SUCCESS
	inv_init_hal_outputs();

    result = inv_register_mpl_start_notification(inv_start_hal_outputs);
    return result;
}

/** Turns off creation and storage of HAL type results.
*/
inv_error_t inv_disable_hal_outputs(void)
{
    inv_error_t result;

    inv_stop_hal_outputs(); // Ignore error if we have already stopped this    
    result = inv_unregister_mpl_start_notification(inv_start_hal_outputs);
    return result;
}

/**
 * @}
 */