////////////////////////////////////////////////////////////////////////////
//
//  This file is part of RTIMULib
//
//  Copyright (c) 2014-2015, richards-tech
//
//  Permission is hereby granted, free of charge, to any person obtaining a copy of
//  this software and associated documentation files (the "Software"), to deal in
//  the Software without restriction, including without limitation the rights to use,
//  copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the
//  Software, and to permit persons to whom the Software is furnished to do so,
//  subject to the following conditions:
//
//  The above copyright notice and this permission notice shall be included in all
//  copies or substantial portions of the Software.
//
//  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
//  INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
//  PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
//  HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
//  OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
//  SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.

//  Some code from Bosch BMM150 API driver:

/*
****************************************************************************
* Copyright (C) 2011 - 2014 Bosch Sensortec GmbH
*
* bmm050.c
* Date: 2014/12/12
* Revision: 2.0.3 $
*
* Usage: Sensor Driver for  BMM050 and BMM150 sensor
*
****************************************************************************
* License:
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
*   Redistributions of source code must retain the above copyright
*   notice, this list of conditions and the following disclaimer.
*
*   Redistributions in binary form must reproduce the above copyright
*   notice, this list of conditions and the following disclaimer in the
*   documentation and/or other materials provided with the distribution.
*
*   Neither the name of the copyright holder nor the names of the
*   contributors may be used to endorse or promote products derived from
*   this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
* CONTRIBUTORS "AS IS" AND ANY EXPRESS OR
* IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDER
* OR CONTRIBUTORS BE LIABLE FOR ANY
* DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY,
* OR CONSEQUENTIAL DAMAGES(INCLUDING, BUT NOT LIMITED TO,
* PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
* WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS
* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE
*
* The information provided is believed to be accurate and reliable.
* The copyright holder assumes no responsibility
* for the consequences of use
* of such information nor for any infringement of patents or
* other rights of third parties which may result from its use.
* No license is granted by implication or otherwise under any patent or
* patent rights of the copyright holder.
**************************************************************************/
/****************************************************************************/

#include "RTIMUBMX055.h"
#include "RTIMUSettings.h"

//  this sets the learning rate for compass running average calculation

#define COMPASS_ALPHA 0.2f

RTIMUBMX055::RTIMUBMX055(RTIMUSettings *settings) : RTIMU(settings)
{
    m_sampleRate = 100;
    m_sampleInterval = (uint64_t)1000000 / m_sampleRate;
}

RTIMUBMX055::~RTIMUBMX055()
{
}

bool RTIMUBMX055::IMUInit()
{
    unsigned char result;

    m_firstTime = true;

    // set validity flags

    m_imuData.fusionPoseValid = false;
    m_imuData.fusionQPoseValid = false;
    m_imuData.gyroValid = true;
    m_imuData.accelValid = true;
    m_imuData.compassValid = true;
    m_imuData.pressureValid = false;
    m_imuData.temperatureValid = false;
    m_imuData.humidityValid = false;

    //  configure IMU

    m_gyroSlaveAddr = m_settings->m_I2CSlaveAddress;

    if (!m_settings->HALRead(m_gyroSlaveAddr, BMX055_GYRO_WHO_AM_I, 1, &result, "Failed to read BMX055 gyro id"))
        return false;

    if (result !=  BMX055_GYRO_ID) {
        HAL_ERROR1("Incorrect BMX055 id %d\n", result);
        return false;
    }

    // work out accel address

    if (m_settings->HALRead(BMX055_ACCEL_ADDRESS0, BMX055_ACCEL_WHO_AM_I, 1, &result, "")) {
        if (result == BMX055_ACCEL_ID) {
            m_accelSlaveAddr = BMX055_ACCEL_ADDRESS0;
        } else {
            m_accelSlaveAddr = BMX055_ACCEL_ADDRESS1;
        }
    }

    // work out mag address

    int magAddr;

    for (magAddr = BMX055_MAG_ADDRESS0; magAddr <= BMX055_MAG_ADDRESS3; magAddr++) {

        // have to enable chip to get id...

        m_settings->HALWrite(magAddr, BMX055_MAG_POWER, 1, "");

        m_settings->delayMs(50);

        if (m_settings->HALRead(magAddr, BMX055_MAG_WHO_AM_I, 1, &result, "")) {
            if (result == BMX055_MAG_ID) {
                m_magSlaveAddr = magAddr;
                break;
            }
        }
    }

    if (magAddr > BMX055_MAG_ADDRESS3) {
        HAL_ERROR("Failed to find BMX055 mag\n");
        return false;
    }

    setCalibrationData();

    //  enable the I2C bus

    if (!m_settings->HALOpen())
        return false;

    //  Set up the gyro

    if (!m_settings->HALWrite(m_gyroSlaveAddr, BMX055_GYRO_FIFO_CONFIG_1, 0x40, "Failed to set BMX055 FIFO config"))
        return false;

    if (!setGyroSampleRate())
            return false;

    if (!setGyroFSR())
            return false;

    gyroBiasInit();

    //  set up the accel

    if (!setAccelSampleRate())
            return false;

    if (!setAccelFSR())
            return false;


    //  set up the mag

    magInitTrimRegisters();
    setMagPreset();

    HAL_INFO("BMX055 init complete\n");
    return true;
}

bool RTIMUBMX055::setGyroSampleRate()
{
    unsigned char bw;

    switch (m_settings->m_BMX055GyroSampleRate) {
    case BMX055_GYRO_SAMPLERATE_2000_523:
        bw = 0;
        m_sampleRate = 2000;
        break;

    case BMX055_GYRO_SAMPLERATE_2000_230:
        bw = 1;
        m_sampleRate = 2000;
        break;

    case BMX055_GYRO_SAMPLERATE_1000_116:
        bw = 2;
        m_sampleRate = 1000;
        break;

    case BMX055_GYRO_SAMPLERATE_400_47:
        bw = 3;
        m_sampleRate = 400;
        break;

    case BMX055_GYRO_SAMPLERATE_200_23:
        bw = 4;
        m_sampleRate = 200;
        break;

    case BMX055_GYRO_SAMPLERATE_100_12:
        bw = 5;
        m_sampleRate = 100;
        break;

    case BMX055_GYRO_SAMPLERATE_200_64:
        bw = 6;
        m_sampleRate = 200;
        break;

    case BMX055_GYRO_SAMPLERATE_100_32:
        bw = 7;
        m_sampleRate = 100;
        break;

    default:
        HAL_ERROR1("Illegal BMX055 gyro sample rate code %d\n", m_settings->m_BMX055GyroSampleRate);
        return false;
    }

    m_sampleInterval = (uint64_t)1000000 / m_sampleRate;
    return (m_settings->HALWrite(m_gyroSlaveAddr, BMX055_GYRO_BW, bw, "Failed to set BMX055 gyro rate"));
}

bool RTIMUBMX055::setGyroFSR()
{
    switch(m_settings->m_BMX055GyroFsr) {
    case BMX055_GYRO_FSR_2000:
        m_gyroScale = 0.061 * RTMATH_DEGREE_TO_RAD;
        break;

    case BMX055_GYRO_FSR_1000:
        m_gyroScale = 0.0305 * RTMATH_DEGREE_TO_RAD;
        break;

    case BMX055_GYRO_FSR_500:
        m_gyroScale = 0.0153 * RTMATH_DEGREE_TO_RAD;
        break;

    case BMX055_GYRO_FSR_250:
        m_gyroScale = 0.0076 * RTMATH_DEGREE_TO_RAD;
        break;

    case BMX055_GYRO_FSR_125:
        m_gyroScale = 0.0038 * RTMATH_DEGREE_TO_RAD;
        break;

    default:
        HAL_ERROR1("Illegal BMX055 gyro FSR code %d\n", m_settings->m_BMX055GyroFsr);
        return false;

    }
    return (m_settings->HALWrite(m_gyroSlaveAddr, BMX055_GYRO_RANGE, m_settings->m_BMX055GyroFsr, "Failed to set BMX055 gyro rate"));
}


bool RTIMUBMX055::setAccelSampleRate()
{
    unsigned char reg;

    switch(m_settings->m_BMX055AccelSampleRate) {
    case BMX055_ACCEL_SAMPLERATE_15:
        reg = 0x08;
        break;

    case BMX055_ACCEL_SAMPLERATE_31:
        reg = 0x09;
        break;

    case BMX055_ACCEL_SAMPLERATE_62:
        reg = 0x0a;
        break;

    case BMX055_ACCEL_SAMPLERATE_125:
        reg = 0x0b;
        break;

    case BMX055_ACCEL_SAMPLERATE_250:
        reg = 0x0c;
        break;

    case BMX055_ACCEL_SAMPLERATE_500:
        reg = 0x0d;
        break;

    case BMX055_ACCEL_SAMPLERATE_1000:
        reg = 0x0e;
        break;

    case BMX055_ACCEL_SAMPLERATE_2000:
        reg = 0x0f;
        break;

    default:
        HAL_ERROR1("Illegal BMX055 accel FSR code %d\n", m_settings->m_BMX055AccelSampleRate);
        return false;
    }
    return (m_settings->HALWrite(m_accelSlaveAddr, BMX055_ACCEL_PMU_BW, reg, "Failed to set BMX055 accel rate"));
}

bool RTIMUBMX055::setAccelFSR()
{
    unsigned char reg;

    switch(m_settings->m_BMX055AccelFsr) {
    case BMX055_ACCEL_FSR_2:
        reg = 0x03;
        m_accelScale = 0.00098 / 16.0;
        break;

    case BMX055_ACCEL_FSR_4:
        reg = 0x05;
        m_accelScale = 0.00195 / 16.0;
        break;

    case BMX055_ACCEL_FSR_8:
        reg = 0x08;
        m_accelScale = 0.00391 / 16.0;
        break;

    case BMX055_ACCEL_FSR_16:
        reg = 0x0c;
        m_accelScale = 0.00781 / 16.0;
        break;

    default:
        HAL_ERROR1("Illegal BMX055 accel FSR code %d\n", m_settings->m_BMX055AccelFsr);
        return false;
    }
    return (m_settings->HALWrite(m_accelSlaveAddr, BMX055_ACCEL_PMU_RANGE, reg, "Failed to set BMX055 accel rate"));
}

bool RTIMUBMX055::setMagPreset()
{
    unsigned char mode, repXY, repZ;

    switch (m_settings->m_BMX055MagPreset) {
    case BMX055_MAG_LOW_POWER:                              // ODR=10, RepXY=3, RepZ=3
        mode = 0;
        repXY = 1;
        repZ = 2;
        break;

    case BMX055_MAG_REGULAR:                                // ODR=10, RepXY=9, RepZ=15
        mode = 0;
        repXY = 4;
        repZ = 14;
        break;

    case BMX055_MAG_ENHANCED:                               // ODR=10, RepXY=15, RepZ=27
        mode = 0;
        repXY = 7;
        repZ = 26;
        break;

    case BMX055_MAG_HIGH_ACCURACY:                          // ODR=10, RepXY=47, RepZ=83
        mode = 0;
        repXY = 23;
        repZ = 82;
        break;

    default:
        HAL_ERROR1("Illegal BMX055 mag preset code %d\n", m_settings->m_BMX055MagPreset);
        return false;
    }

    if (!m_settings->HALWrite(m_magSlaveAddr, BMX055_MAG_MODE, mode, "Failed to set BMX055 mag mode"))
        return false;
    if (!m_settings->HALWrite(m_magSlaveAddr, BMX055_MAG_REPXY, repXY, "Failed to set BMX055 mag repXY"))
        return false;
    if (!m_settings->HALWrite(m_magSlaveAddr, BMX055_MAG_REPZ, repZ, "Failed to set BMX055 mag repZ"))
        return false;
    return true;
}

int RTIMUBMX055::IMUGetPollInterval()
{
    if (m_sampleRate > 400)
        return 1;
    else
        return (400 / m_sampleRate);
}

bool RTIMUBMX055::IMURead()
{
    unsigned char status;
    unsigned char gyroData[6];
    unsigned char accelData[6];
    unsigned char magData[8];

    if (!m_settings->HALRead(m_gyroSlaveAddr, BMX055_GYRO_FIFO_STATUS, 1, &status, "Failed to read BMX055 gyro fifo status"))
        return false;

    if (status & 0x80) {
        // fifo overflowed
        HAL_ERROR("BMX055 fifo overflow\n");

        // this should clear it
        if (!m_settings->HALWrite(m_gyroSlaveAddr, BMX055_GYRO_FIFO_CONFIG_1, 0x40, "Failed to set BMX055 FIFO config"))
            return false;

        m_imuData.timestamp = RTMath::currentUSecsSinceEpoch();  // try to fix timestamp
        return false;
    }

    if (status == 0)
        return false;

    if (!m_settings->HALRead(m_gyroSlaveAddr, BMX055_GYRO_FIFO_DATA, 6, gyroData, "Failed to read BMX055 gyro data"))
            return false;

    if (!m_settings->HALRead(m_accelSlaveAddr, BMX055_ACCEL_X_LSB, 6, accelData, "Failed to read BMX055 accel data"))
        return false;

    if (!m_settings->HALRead(m_magSlaveAddr, BMX055_MAG_X_LSB, 8, magData, "Failed to read BMX055 mag data"))
        return false;

    RTMath::convertToVector(gyroData, m_imuData.gyro, m_gyroScale, false);

    //  need to prepare accel data

    accelData[0] &= 0xf0;
    accelData[2] &= 0xf0;
    accelData[4] &= 0xf0;

    RTMath::convertToVector(accelData, m_imuData.accel, m_accelScale, false);

    float mx, my, mz;

    processMagData(magData, mx, my, mz);
    m_imuData.compass.setX(mx);
    m_imuData.compass.setY(my);
    m_imuData.compass.setZ(mz);

    //  sort out gyro axes

    m_imuData.gyro.setY(-m_imuData.gyro.y());
    m_imuData.gyro.setZ(-m_imuData.gyro.z());

    //  sort out accel axes

    m_imuData.accel.setX(-m_imuData.accel.x());

    //  sort out mag axes

    m_imuData.compass.setY(-m_imuData.compass.y());
    m_imuData.compass.setZ(-m_imuData.compass.z());

    //  now do standard processing

    handleGyroBias();
    calibrateAverageCompass();
    calibrateAccel();

    if (m_firstTime)
        m_imuData.timestamp = RTMath::currentUSecsSinceEpoch();
    else
        m_imuData.timestamp += m_sampleInterval;

    m_firstTime = false;

    //  now update the filter

    updateFusion();

    return true;
}

/*!
 *	@brief This API reads remapped compensated Magnetometer
 *	data of X,Y,Z values
 *	@note The output value of compensated X, Y, Z as float
 *
 *	@note In this function X and Y axis is remapped
 *	@note X is read from the address 0x44 & 0x45
 *	@note Y is read from the address 0x42 & 0x43
 *	@note this API is only applicable for BMX055 sensor
 * *
 *
*/

/****************************************************/
/**\name	ARRAY PARAMETERS      */
/***************************************************/
#define LSB_ZERO	0
#define MSB_ONE		1
#define LSB_TWO		2
#define MSB_THREE	3
#define LSB_FOUR	4
#define MSB_FIVE	5
#define LSB_SIX		6
#define MSB_SEVEN	7

/********************************************/
/**\name BIT MASK, LENGTH AND POSITION OF REMAPPED DATA REGISTERS   */
/********************************************/
/* Data X LSB Remapped Register only applicable for BMX055 */
#define BMM050_BMX055_REMAPPED_DATAX_LSB_VALUEX__POS        3
#define BMM050_BMX055_REMAPPED_DATAX_LSB_VALUEX__LEN        5
#define BMM050_BMX055_REMAPPED_DATAX_LSB_VALUEX__MSK        0xF8
#define BMM050_BMX055_REMAPPED_DATAX_LSB_VALUEX__REG\
BMM050_BMX055_REMAPPED_DATAX_LSB

/* Data Y LSB Remapped Register only applicable for BMX055  */
#define BMM050_BMX055_REMAPPED_DATAY_LSB_VALUEY__POS        3
#define BMM050_BMX055_REMAPPED_DATAY_LSB_VALUEY__LEN        5
#define BMM050_BMX055_REMAPPED_DATAY_LSB_VALUEY__MSK        0xF8
#define BMM050_BMX055_REMAPPED_DATAY_LSB_VALUEY__REG\
BMM050_BMX055_REMAPPED_DATAY_LSB

/********************************************/
/**\name BIT MASK, LENGTH AND POSITION OF DATA REGISTERS   */
/********************************************/
/* Data X LSB Register */
#define BMM050_DATAX_LSB_VALUEX__POS        3
#define BMM050_DATAX_LSB_VALUEX__LEN        5
#define BMM050_DATAX_LSB_VALUEX__MSK        0xF8
#define BMM050_DATAX_LSB_VALUEX__REG        BMM050_DATAX_LSB

/* Data X SelfTest Register */
#define BMM050_DATAX_LSB_TESTX__POS         0
#define BMM050_DATAX_LSB_TESTX__LEN         1
#define BMM050_DATAX_LSB_TESTX__MSK         0x01
#define BMM050_DATAX_LSB_TESTX__REG         BMM050_DATAX_LSB

/* Data Y LSB Register */
#define BMM050_DATAY_LSB_VALUEY__POS        3
#define BMM050_DATAY_LSB_VALUEY__LEN        5
#define BMM050_DATAY_LSB_VALUEY__MSK        0xF8
#define BMM050_DATAY_LSB_VALUEY__REG        BMM050_DATAY_LSB

/* Data Y SelfTest Register */
#define BMM050_DATAY_LSB_TESTY__POS         0
#define BMM050_DATAY_LSB_TESTY__LEN         1
#define BMM050_DATAY_LSB_TESTY__MSK         0x01
#define BMM050_DATAY_LSB_TESTY__REG         BMM050_DATAY_LSB

/* Data Z LSB Register */
#define BMM050_DATAZ_LSB_VALUEZ__POS        1
#define BMM050_DATAZ_LSB_VALUEZ__LEN        7
#define BMM050_DATAZ_LSB_VALUEZ__MSK        0xFE
#define BMM050_DATAZ_LSB_VALUEZ__REG        BMM050_DATAZ_LSB

/* Data Z SelfTest Register */
#define BMM050_DATAZ_LSB_TESTZ__POS         0
#define BMM050_DATAZ_LSB_TESTZ__LEN         1
#define BMM050_DATAZ_LSB_TESTZ__MSK         0x01
#define BMM050_DATAZ_LSB_TESTZ__REG         BMM050_DATAZ_LSB

/* Hall Resistance LSB Register */
#define BMM050_R_LSB_VALUE__POS             2
#define BMM050_R_LSB_VALUE__LEN             6
#define BMM050_R_LSB_VALUE__MSK             0xFC
#define BMM050_R_LSB_VALUE__REG             BMM050_R_LSB

#define BMM050_DATA_RDYSTAT__POS            0
#define BMM050_DATA_RDYSTAT__LEN            1
#define BMM050_DATA_RDYSTAT__MSK            0x01
#define BMM050_DATA_RDYSTAT__REG            BMM050_R_LSB

/********************************************/
/**\name GET AND SET BITSLICE FUNCTIONS  */
/********************************************/
/* get bit slice  */
#define BMM050_GET_BITSLICE(regvar, bitname)\
    ((regvar & bitname##__MSK) >> bitname##__POS)

/* Set bit slice */
#define BMM050_SET_BITSLICE(regvar, bitname, val)\
    ((regvar & ~bitname##__MSK) | ((val<<bitname##__POS)&bitname##__MSK))

/********************************************/
/**\name BIT SHIFTING DEFINITIONS  */
/********************************************/
/*Shifting Constants*/
#define SHIFT_RIGHT_1_POSITION                  1
#define SHIFT_RIGHT_2_POSITION                  2
#define SHIFT_RIGHT_3_POSITION                  3
#define SHIFT_RIGHT_4_POSITION                  4
#define SHIFT_RIGHT_5_POSITION                  5
#define SHIFT_RIGHT_6_POSITION                  6
#define SHIFT_RIGHT_7_POSITION                  7
#define SHIFT_RIGHT_8_POSITION                  8
#define SHIFT_RIGHT_9_POSITION                  9
#define SHIFT_RIGHT_12_POSITION                 12
#define SHIFT_RIGHT_13_POSITION                 13
#define SHIFT_RIGHT_16_POSITION                 16

#define SHIFT_LEFT_1_POSITION                   1
#define SHIFT_LEFT_2_POSITION                   2
#define SHIFT_LEFT_3_POSITION                   3
#define SHIFT_LEFT_4_POSITION                   4
#define SHIFT_LEFT_5_POSITION                   5
#define SHIFT_LEFT_6_POSITION                   6
#define SHIFT_LEFT_7_POSITION                   7
#define SHIFT_LEFT_8_POSITION                   8
#define SHIFT_LEFT_14_POSITION                  14
#define SHIFT_LEFT_15_POSITION                  15
#define SHIFT_LEFT_16_POSITION                  16

/****************************************************/
/**\name	COMPENSATED FORMULA DEFINITIONS      */
/***************************************************/
#define BMM050_HEX_FOUR_THOUSAND			0x4000
#define BMM050_HEX_ONE_LACK					0x100000
#define BMM050_HEX_A_ZERO					0xA0

#define	BMM050_FLOAT_ONE_SIX_THREE_EIGHT_FOUR		16384.0f
#define	BMM050_FLOAT_2_6_8_4_3_5_4_5_6_DATA			268435456.0f
#define	BMM050_FLOAT_1_6_3_8_4_DATA					16384.0f
#define	BMM050_FLOAT_2_5_6_DATA						256.0f
#define	BMM050_FLOAT_1_6_0_DATA						160.0f
#define	BMM050_FLOAT_8_1_9_2_DATA					8192.0f
#define	BMM050_FLOAT_EIGHT_DATA						8.0f
#define	BMM050_FLOAT_SIXTEEN_DATA					16.0f
#define	BMM050_FLOAT_1_3_1_0_7_2_DATA				131072.0f
#define	BMM050_FLOAT_3_2_7_6_8_DATA					32768.0
#define	BMM050_FLOAT_4_DATA                         4.0

/********************************************/
/**\name ENABLE/DISABLE DEFINITIONS  */
/********************************************/
#define BMM050_ZERO_U8X                         0
#define BMM050_DISABLE                          0
#define BMM050_ENABLE                           1
#define BMM050_CHANNEL_DISABLE                  1
#define BMM050_CHANNEL_ENABLE                   0
#define BMM050_INTPIN_LATCH_ENABLE              1
#define BMM050_INTPIN_LATCH_DISABLE             0
#define BMM050_OFF                              0
#define BMM050_ON                               1

/********************************************/
/**\name OVERFLOW DEFINITIONS  */
/********************************************/
/* compensated output value returned if sensor had overflow */
#define BMM050_OVERFLOW_OUTPUT			-32768
#define BMM050_OVERFLOW_OUTPUT_S32		((int32_t)(-2147483647-1))
#define BMM050_OVERFLOW_OUTPUT_FLOAT	0.0f
#define BMM050_FLIP_OVERFLOW_ADCVAL		-4096
#define BMM050_HALL_OVERFLOW_ADCVAL		-16384

/********************************************/
/**\name NUMERIC DEFINITIONS  */
/********************************************/
#define         C_BMM050_ZERO_U8X				((uint8_t)0)
#define         C_BMM050_ONE_U8X				((uint8_t)1)
#define         C_BMM050_TWO_U8X				((uint8_t)2)
#define         C_BMM050_FOUR_U8X				((uint8_t)4)
#define         C_BMM050_FIVE_U8X				((uint8_t)5)
#define         C_BMM050_EIGHT_U8X				((uint8_t)8)

#define BMM0505_HEX_ZERO_ZERO	0x00
/* Conversion factors*/
#define BMM050_CONVFACTOR_LSB_UT                6

/********************************************/
/**\name BIT MASK, LENGTH AND POSITION OF TRIM REGISTER   */
/********************************************/
/* Register 6D */
#define BMM050_DIG_XYZ1_MSB__POS         0
#define BMM050_DIG_XYZ1_MSB__LEN         7
#define BMM050_DIG_XYZ1_MSB__MSK         0x7F
#define BMM050_DIG_XYZ1_MSB__REG         BMM050_DIG_XYZ1_MSB


void RTIMUBMX055::processMagData(unsigned char *v_data_uint8_t, float& magX, float& magY, float& magZ)
{
    /* structure used to store the mag raw xyz and r data */
    struct {
        int16_t raw_data_x;
        int16_t raw_data_y;
        int16_t raw_data_z;
        uint16_t raw_data_r;
    } raw_data_xyz_t;

    /* Reading data for Y axis */
    v_data_uint8_t[LSB_ZERO] = BMM050_GET_BITSLICE(v_data_uint8_t[LSB_ZERO],
    BMM050_BMX055_REMAPPED_DATAY_LSB_VALUEY);
    raw_data_xyz_t.raw_data_y = (int16_t)((((int32_t)
    ((int8_t)v_data_uint8_t[MSB_ONE])) <<
    SHIFT_LEFT_5_POSITION) | v_data_uint8_t[LSB_ZERO]);

    /* Reading data for X axis */
    v_data_uint8_t[LSB_TWO] = BMM050_GET_BITSLICE(v_data_uint8_t[LSB_TWO],
    BMM050_BMX055_REMAPPED_DATAX_LSB_VALUEX);
    raw_data_xyz_t.raw_data_x = (int16_t)((((int32_t)
    ((int8_t)v_data_uint8_t[MSB_THREE])) <<
    SHIFT_LEFT_5_POSITION) | v_data_uint8_t[LSB_TWO]);
    raw_data_xyz_t.raw_data_x = -raw_data_xyz_t.raw_data_x;

    /* Reading data for Z axis */
    v_data_uint8_t[LSB_FOUR] = BMM050_GET_BITSLICE(v_data_uint8_t[LSB_FOUR],
    BMM050_DATAZ_LSB_VALUEZ);
    raw_data_xyz_t.raw_data_z = (int16_t)((((int32_t)
    ((int8_t)v_data_uint8_t[MSB_FIVE])) <<
    SHIFT_LEFT_7_POSITION) | v_data_uint8_t[LSB_FOUR]);

    /* Reading data for Resistance*/
    v_data_uint8_t[LSB_SIX] = BMM050_GET_BITSLICE(v_data_uint8_t[LSB_SIX],
    BMM050_R_LSB_VALUE);
    raw_data_xyz_t.raw_data_r = (uint16_t)((((uint32_t)
    v_data_uint8_t[MSB_SEVEN]) <<
    SHIFT_LEFT_6_POSITION) | v_data_uint8_t[LSB_SIX]);

    /* Compensation for X axis */
    magX = bmm050_compensate_X_float(
    raw_data_xyz_t.raw_data_x,
    raw_data_xyz_t.raw_data_r);

    /* Compensation for Y axis */
    magY = bmm050_compensate_Y_float(
    raw_data_xyz_t.raw_data_y,
    raw_data_xyz_t.raw_data_r);

    /* Compensation for Z axis */
    magZ = bmm050_compensate_Z_float(
    raw_data_xyz_t.raw_data_z,
    raw_data_xyz_t.raw_data_r);

    /* Output raw resistance value */
//    mag_data->resistance = raw_data_xyz_t.raw_data_r;

}

float RTIMUBMX055::bmm050_compensate_X_float(int16_t mag_data_x, uint16_t data_r)
{
    float inter_retval = BMM050_ZERO_U8X;
    if (mag_data_x != BMM050_FLIP_OVERFLOW_ADCVAL	/* no overflow */
       ) {
        if (data_r != C_BMM050_ZERO_U8X) {
            inter_retval = ((((float)m_dig_xyz1)
            * BMM050_FLOAT_ONE_SIX_THREE_EIGHT_FOUR
                /data_r)
                - BMM050_FLOAT_ONE_SIX_THREE_EIGHT_FOUR);
        } else {
            inter_retval = BMM050_OVERFLOW_OUTPUT_FLOAT;
            return inter_retval;
        }
        inter_retval = (((mag_data_x * ((((((float)m_dig_xy2) *
            (inter_retval*inter_retval /
            BMM050_FLOAT_2_6_8_4_3_5_4_5_6_DATA) +
            inter_retval * ((float)m_dig_xy1)
            / BMM050_FLOAT_1_6_3_8_4_DATA))
            + BMM050_FLOAT_2_5_6_DATA) *
            (((float)m_dig_x2) + BMM050_FLOAT_1_6_0_DATA)))
            / BMM050_FLOAT_8_1_9_2_DATA)
            + (((float)m_dig_x1) *
            BMM050_FLOAT_EIGHT_DATA))/
            BMM050_FLOAT_SIXTEEN_DATA;
    } else {
        inter_retval = BMM050_OVERFLOW_OUTPUT_FLOAT;
    }
    return inter_retval;
}

float RTIMUBMX055::bmm050_compensate_Y_float(int16_t mag_data_y, uint16_t data_r)
{
    float inter_retval = BMM050_ZERO_U8X;
    if (mag_data_y != BMM050_FLIP_OVERFLOW_ADCVAL /* no overflow */
       ) {
        if (data_r != C_BMM050_ZERO_U8X) {
            inter_retval = ((((float)m_dig_xyz1)
            * BMM050_FLOAT_ONE_SIX_THREE_EIGHT_FOUR
            /data_r) - BMM050_FLOAT_ONE_SIX_THREE_EIGHT_FOUR);
        } else {
            inter_retval = BMM050_OVERFLOW_OUTPUT_FLOAT;
            return inter_retval;
        }
        inter_retval = (((mag_data_y * ((((((float)m_dig_xy2) *
            (inter_retval*inter_retval
            / BMM050_FLOAT_2_6_8_4_3_5_4_5_6_DATA) +
            inter_retval * ((float)m_dig_xy1)
            / BMM050_FLOAT_1_6_3_8_4_DATA)) +
            BMM050_FLOAT_2_5_6_DATA) *
            (((float)m_dig_y2) + BMM050_FLOAT_1_6_0_DATA)))
            / BMM050_FLOAT_8_1_9_2_DATA) +
            (((float)m_dig_y1) * BMM050_FLOAT_EIGHT_DATA))
            / BMM050_FLOAT_SIXTEEN_DATA;
    } else {
        /* overflow, set output to 0.0f */
        inter_retval = BMM050_OVERFLOW_OUTPUT_FLOAT;
    }
    return inter_retval;
}

float RTIMUBMX055::bmm050_compensate_Z_float (int16_t mag_data_z, uint16_t data_r)
{
    float inter_retval = BMM050_ZERO_U8X;
     /* no overflow */
    if (mag_data_z != BMM050_HALL_OVERFLOW_ADCVAL) {
        if ((m_dig_z2 != BMM050_ZERO_U8X)
        && (m_dig_z1 != BMM050_ZERO_U8X)
        && (data_r != BMM050_ZERO_U8X)) {
            inter_retval = ((((((float)mag_data_z)-
            ((float)m_dig_z4))*
            BMM050_FLOAT_1_3_1_0_7_2_DATA)-
            (((float)m_dig_z3)*(((float)data_r)
            -((float)m_dig_xyz1))))
            /((((float)m_dig_z2)+
            ((float)m_dig_z1)*((float)data_r) /
            BMM050_FLOAT_3_2_7_6_8_DATA)
            * BMM050_FLOAT_4_DATA))
            / BMM050_FLOAT_SIXTEEN_DATA;
        }
    } else {
        /* overflow, set output to 0.0f */
        inter_retval = BMM050_OVERFLOW_OUTPUT_FLOAT;
    }
    return inter_retval;
}

bool RTIMUBMX055::magInitTrimRegisters()
{
    unsigned char data[2];
    data[0] = 0;
    data[1] = 0;

    if (!m_settings->HALRead(m_magSlaveAddr, BMX055_MAG_DIG_X1, 1, (uint8_t *)&m_dig_x1, "Failed to read BMX055 mag trim x1"))
        return false;

    if (!m_settings->HALRead(m_magSlaveAddr, BMX055_MAG_DIG_Y1, 1, (uint8_t *)&m_dig_y1, "Failed to read BMX055 mag trim y1"))
        return false;

    if (!m_settings->HALRead(m_magSlaveAddr, BMX055_MAG_DIG_X2, 1, (uint8_t *)&m_dig_x2, "Failed to read BMX055 mag trim x2"))
        return false;

    if (!m_settings->HALRead(m_magSlaveAddr, BMX055_MAG_DIG_Y2, 1, (uint8_t *)&m_dig_y2, "Failed to read BMX055 mag trim y2"))
        return false;

    if (!m_settings->HALRead(m_magSlaveAddr, BMX055_MAG_DIG_XY1, 1, &m_dig_xy1, "Failed to read BMX055 mag trim xy1"))
        return false;

    if (!m_settings->HALRead(m_magSlaveAddr, BMX055_MAG_DIG_XY2, 1, (uint8_t *)&m_dig_xy2, "Failed to read BMX055 mag trim xy2"))
        return false;

    /* shorts can not be recast into (uint8_t*)
    * due to possible mix up between trim data
    * arrangement and memory arrangement */

    if (!m_settings->HALRead(m_magSlaveAddr, BMX055_MAG_DIG_Z1_LSB, 2, data, "Failed to read BMX055 mag trim z1"))
        return false;

    m_dig_z1 = (uint16_t)((((uint32_t)((uint8_t)
    data[MSB_ONE])) <<
    SHIFT_LEFT_8_POSITION) | data[LSB_ZERO]);

    if (!m_settings->HALRead(m_magSlaveAddr, BMX055_MAG_DIG_Z2_LSB, 2, data, "Failed to read BMX055 mag trim z2"))
        return false;

    m_dig_z2 = (int16_t)((((int32_t)(
    (int8_t)data[MSB_ONE])) <<
    SHIFT_LEFT_8_POSITION) | data[LSB_ZERO]);

    if (!m_settings->HALRead(m_magSlaveAddr, BMX055_MAG_DIG_Z3_LSB, 2, data, "Failed to read BMX055 mag trim z3"))
        return false;

    m_dig_z3 = (int16_t)((((int32_t)(
    (int8_t)data[MSB_ONE])) <<
    SHIFT_LEFT_8_POSITION) | data[LSB_ZERO]);

    if (!m_settings->HALRead(m_magSlaveAddr, BMX055_MAG_DIG_Z4_LSB, 2, data, "Failed to read BMX055 mag trim z4"))
        return false;

    m_dig_z4 = (int16_t)((((int32_t)(
    (int8_t)data[MSB_ONE])) <<
    SHIFT_LEFT_8_POSITION) | data[LSB_ZERO]);

    if (!m_settings->HALRead(m_magSlaveAddr, BMX055_MAG_DIG_XYZ1_LSB, 2, data, "Failed to read BMX055 mag trim xyz1"))
        return false;

    data[MSB_ONE] = BMM050_GET_BITSLICE(data[MSB_ONE],
    BMM050_DIG_XYZ1_MSB);
    m_dig_xyz1 = (uint16_t)((((uint32_t)
    ((uint8_t)data[MSB_ONE])) <<
    SHIFT_LEFT_8_POSITION) | data[LSB_ZERO]);

    return true;
}