 /******************************************************************************
 * @file    drv_motion.c
 * @author  Insight SiP
 * @version V1.0.0
 * @date    06-november-2018
 * @brief   motion driver implementation file.
 *
 * @attention
 *	THIS SOFTWARE IS PROVIDED BY INSIGHT SIP "AS IS" AND ANY EXPRESS
 *	OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
 *	OF MERCHANTABILITY, NONINFRINGEMENT, AND FITNESS FOR A PARTICULAR PURPOSE ARE
 *	DISCLAIMED. IN NO EVENT SHALL INSIGHT SIP 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.
 *
 *****************************************************************************/

#include "nrf_delay.h"
#include "nrf_error.h"

#include "drv_motion.h"
#include "drv_lsm9ds1.h"

#include "orientation.h"

#include "nrf_log.h"
#include "macros_common.h"


#define RETURN_IF_INV_ERROR(PARAM)              \
        if ((PARAM) != INV_SUCCESS)             \
        {                              		\
            return NRF_ERROR_INTERNAL;		\
        }
		
// calibration data
static bool m_is_magnetometer_calibrated     = false;
static int16_t m_mag_offset[3] = {0};
		
/**@brief Motion configuration struct.
 */
typedef struct
{
    bool                      enabled;       ///< Driver enabled.
    drv_lsm9ds1_twi_cfg_t     cfg;           ///< TWI configuraion.
    bool                      running;
    drv_motion_feature_mask_t features;
    drv_motion_evt_handler_t  evt_handler;
} drv_motion_t;

/**@brief Stored configuration.
 */
static drv_motion_t m_drv_motion;

uint32_t drv_motion_init(drv_motion_init_t * p_params)
{
    uint32_t            err_code;
	
    NULL_PARAM_CHECK(p_params);
    NULL_PARAM_CHECK(p_params->p_twi_instance);
    NULL_PARAM_CHECK(p_params->p_twi_cfg);
    NULL_PARAM_CHECK(p_params->evt_handler);

    m_drv_motion.evt_handler        	= p_params->evt_handler;
    m_drv_motion.cfg.acc_twi_addr       = LSM9DS1_ACC_GYRO_ADDR;
    m_drv_motion.cfg.mag_twi_addr       = LSM9DS1_MAG_ADDR;
    m_drv_motion.cfg.acc_pin_int        = p_params->acc_pin_int;
    m_drv_motion.cfg.mag_pin_int        = p_params->mag_pin_int;
    m_drv_motion.cfg.p_twi_instance 	= p_params->p_twi_instance;
    m_drv_motion.cfg.p_twi_cfg      	= p_params->p_twi_cfg;
    m_drv_motion.enabled                = false;
    m_drv_motion.features               = 0;

    // Request twi bus
    drv_lsm9ds1_open(&m_drv_motion.cfg);

    err_code = drv_lsm9ds1_verify();
    drv_lsm9ds1_reboot();
    drv_lsm9ds1_offset_mag_set(0, 0, 0);

    // Release twi bus
    drv_lsm9ds1_close();
	
    reset_mag_calibration();

    return err_code;
}

uint32_t drv_motion_enable(drv_motion_feature_mask_t feature_mask)
{
    uint32_t err_code;

    if ( (feature_mask & ~(DRV_MOTION_FEATURE_MASK)) || (feature_mask == 0) )
    {
        return NRF_ERROR_INVALID_PARAM;
    }

    // Set features bits.
    m_drv_motion.features |= feature_mask;
	
    if (m_drv_motion.enabled)
    {
        return NRF_SUCCESS;
    }
    m_drv_motion.enabled = true;
	
    static const drv_lsm9ds1_cfg_t lsm9ds1_cfg =
    {
        .ctrl_reg1_g = BITS_ODR_59_5HZ_G | BITS_FS_2000_G,
        .ctrl_reg3_g = BITS_LP_EN,
        .ctrl_reg4   = BITS_XEN_G | BITS_YEN_G | BITS_ZEN_G,
        .ctrl_reg5_xl= BITS_XEN_XL | BITS_YEN_XL | BITS_ZEN_XL,
        .ctrl_reg6_xl= BITS_ODR_119HZ_XL,
        .ctrl_reg1_m = BITS_TEMP_COMP | BITS_OM_HIGH | BITS_ODR_M_80HZ,
        .ctrl_reg2_m = BITS_FS_M_4Gs,
        .ctrl_reg3_m = BITS_MD_CONTINUOUS,
	.ctrl_reg4_m = BITS_OMZ_HIGH,
        .ctrl_reg5_m = 0x00,	
        .ctrl_reg9   = CTRL_REG9_DEFAULT
    };	

    // Request twi bus
    drv_lsm9ds1_open(&m_drv_motion.cfg);
	
    drv_lsm9ds1_cfg_set(&lsm9ds1_cfg);
	
    // Release twi bus
    drv_lsm9ds1_close();

    return NRF_SUCCESS;
}

uint32_t drv_motion_disable(drv_motion_feature_mask_t feature_mask)
{
    uint32_t err_code = NRF_SUCCESS;

    if ( (feature_mask & ~(DRV_MOTION_FEATURE_MASK)) || (feature_mask == 0) )
    {
        return NRF_ERROR_INVALID_PARAM;
    }

    // Clear feature bits
    m_drv_motion.features &= ~feature_mask;

    if (!m_drv_motion.features)
    {
        m_drv_motion.enabled = false;
		
        static const drv_lsm9ds1_cfg_t lsm9ds1_cfg =
        {
            .ctrl_reg1_g = CTRL_REG1_G_DEFAULT,
            .ctrl_reg3_g = BITS_LP_EN,
            .ctrl_reg4 	 = CTRL_REG4_DEFAULT,
            .ctrl_reg5_xl= CTRL_REG5_XL_DEFAULT,
            .ctrl_reg6_xl= CTRL_REG6_XL_DEFAULT,
            .ctrl_reg1_m = CTRL_REG1_M_DEFAULT,
            .ctrl_reg2_m = CTRL_REG2_M_DEFAULT,
            .ctrl_reg3_m = CTRL_REG3_M_DEFAULT,
            .ctrl_reg4_m = CTRL_REG4_M_DEFAULT,
            .ctrl_reg5_m = 0x00,
            .ctrl_reg9	 = BITS_SLEEP_G	
        };	

        // Request twi bus
        drv_lsm9ds1_open(&m_drv_motion.cfg);
		
        err_code = drv_lsm9ds1_cfg_set(&lsm9ds1_cfg);
	
        // Release twi bus
        drv_lsm9ds1_close();
    }

    return err_code;
}

//TODO useless ??
uint32_t drv_motion_config(drv_motion_cfg_t * p_cfg)
{
    uint32_t err_code;

    NULL_PARAM_CHECK(p_cfg);


    if (m_drv_motion.enabled)
    {
        drv_motion_feature_mask_t enabled_features = m_drv_motion.features;

        err_code = drv_motion_disable(enabled_features);
        RETURN_IF_ERROR(err_code);
		
        return drv_motion_enable(enabled_features);
    }
    else
    {
        return NRF_SUCCESS;
    }
}


uint32_t drv_motion_raw_acc_get(drv_lsm9ds1_acc_data_t * acc_value)
{
    uint32_t err_code = NRF_SUCCESS;	
	
    // Request twi bus
    drv_lsm9ds1_open(&m_drv_motion.cfg);
	
    err_code = drv_lsm9ds1_accelerometer_get(acc_value);
	
    // Release twi bus
    drv_lsm9ds1_close();

    return err_code;	
}

uint32_t drv_motion_raw_mag_get(drv_lsm9ds1_mag_data_t * mag_value)
{
    uint32_t err_code = NRF_SUCCESS;	
	
    // Request twi bus
    drv_lsm9ds1_open(&m_drv_motion.cfg);
	
    err_code = drv_lsm9ds1_magnetometer_get(mag_value);
	
    // Release twi bus
    drv_lsm9ds1_close();

    return err_code;		
}

uint32_t drv_motion_raw_gyro_get(drv_lsm9ds1_gyro_data_t * gyro_value)
{
    uint32_t err_code = NRF_SUCCESS;	
	
    // Request twi bus
    drv_lsm9ds1_open(&m_drv_motion.cfg);

    err_code = drv_lsm9ds1_gyroscope_get(gyro_value);

    // Release twi bus
    drv_lsm9ds1_close();

    return err_code;			
}

uint32_t drv_motion_offset_mag_set(int16_t offset_x, int16_t offset_y, int16_t offset_z)
{
    uint32_t err_code = NRF_SUCCESS;	
	
    // Request twi bus
    drv_lsm9ds1_open(&m_drv_motion.cfg);
	
    err_code = drv_lsm9ds1_offset_mag_set(offset_x, offset_y, offset_z);
	
    // Release twi bus
    drv_lsm9ds1_close();

    return err_code;	
}

uint32_t drv_motion_offset_mag_get(int16_t *offset_x, int16_t *offset_y ,int16_t *offset_z)
{
    uint32_t err_code = NRF_SUCCESS;
	
    // Request twi bus
    drv_lsm9ds1_open(&m_drv_motion.cfg);
	
    err_code = drv_lsm9ds1_offset_mag_get(offset_x, offset_y, offset_z);
	
    // Release twi bus
    drv_lsm9ds1_close();
	
    return err_code;	
}

uint32_t drv_motion_data_get(void)
{
    uint32_t err_code;
    drv_motion_evt_t evt;
    drv_lsm9ds1_acc_data_t acc_value;
    drv_lsm9ds1_gyro_data_t gyro_value;
    drv_lsm9ds1_mag_data_t mag_value;
    float rotation_matrix[3][3];
    float quaternion[4];
    float euler[3];
	
    // Fetch All data from sensor
    err_code = drv_motion_raw_acc_get(&acc_value);
    err_code |= drv_motion_raw_gyro_get(&gyro_value);
    err_code |= drv_motion_raw_mag_get(&mag_value);
    RETURN_IF_ERROR(err_code);
	
    if(!m_is_magnetometer_calibrated)
    {
        update_magnetometer_buffer( MAG_SCALE_4Gs_IN_UT*mag_value.x, 	\
                                    MAG_SCALE_4Gs_IN_UT*mag_value.y, 	\
                                    MAG_SCALE_4Gs_IN_UT*mag_value.z,	\
                                    ACC_SCALE_2G*acc_value.x,           \
                                    ACC_SCALE_2G*acc_value.y,           \
                                    ACC_SCALE_2G*acc_value.z); 		
									
        if (m_mag_buffer_count_get() > MIN_EQUATIONS)
        {
            m_is_magnetometer_calibrated = true;
            float mag_offset[3];
            hard_iron_calibration(&mag_offset[0], &mag_offset[1], &mag_offset[2]);
            m_mag_offset[0] = mag_offset[0]*INV_MAG_SCALE_4Gs_IN_UT;
            m_mag_offset[1] = mag_offset[1]*INV_MAG_SCALE_4Gs_IN_UT;
            m_mag_offset[2] = mag_offset[2]*INV_MAG_SCALE_4Gs_IN_UT;
			
                //drv_motion_offset_mag_set(mag_offset_x, mag_offset_y, mag_offset_z);
            NRF_LOG_INFO("drv_motion_data_get: calibration results: %d, %d, %d\r\n", m_mag_offset[0], m_mag_offset[1], m_mag_offset[2]);
        }
    }
    else
    {
        mag_value.x -= m_mag_offset[0];
        mag_value.y -= m_mag_offset[1];
        mag_value.z -= m_mag_offset[2];
    }

    if (m_drv_motion.features & DRV_MOTION_FEATURE_MASK_GRAVITY_VECTOR)
    {
        float gravity[3];
        // From the board point of view:
        // The X device axis is the X axis (inverted)
        // The Y device axis is the Y axis (inverted)
        // The Z device axis is the Z axis 
        gravity[0] = ACC_SCALE_2G*acc_value.x;
        gravity[1] = ACC_SCALE_2G*acc_value.y;
        gravity[2] = ACC_SCALE_2G*acc_value.z;
			
        evt = DRV_MOTION_EVT_GRAVITY;
        // x, y and z
        m_drv_motion.evt_handler(&evt, gravity, sizeof(float) * 3);
    }
	
    if (m_drv_motion.features & DRV_MOTION_FEATURE_MASK_ANGULAR_VELOCITY_VECTOR)
    {
        float angular_rotation[3];

        angular_rotation [0] = GYRO_SCALE_2000DPS*gyro_value.x;
        angular_rotation [1] = GYRO_SCALE_2000DPS*gyro_value.y;
        angular_rotation [2] = GYRO_SCALE_2000DPS*gyro_value.z;
			
        evt = DRV_MOTION_EVT_ANGULAR_ROTATION;
        // x, y and z
        m_drv_motion.evt_handler(&evt, angular_rotation, sizeof(float) * 3);
    }
	
    if (m_drv_motion.features & DRV_MOTION_FEATURE_MASK_MAGNETIZATION_VECTOR)
    {
        float magnetization[3];
        // From the board point of view:
        // The X device axis is the X axis (inverted)
        // The Y device axis is the Y axis 
        // The Z device axis is the Z axis (inverted)
        magnetization [0] = MAG_SCALE_4Gs_IN_UT*mag_value.x;
        magnetization [1] = MAG_SCALE_4Gs_IN_UT*mag_value.y;
        magnetization [2] = MAG_SCALE_4Gs_IN_UT*mag_value.z;
			
        evt = DRV_MOTION_EVT_MAGNETIZATION;
        // x, y and z
        m_drv_motion.evt_handler(&evt, magnetization, sizeof(float) * 3);
    }
	
    if (m_drv_motion.features & DRV_MOTION_FEATURE_MASK_ROT_MAT)
    {
        if (!orient_calculate_rotation_matrix(	rotation_matrix, 			\
                                                MAG_SCALE_4Gs_IN_UT*mag_value.y,	\
                                                MAG_SCALE_4Gs_IN_UT*mag_value.x,	\
						-MAG_SCALE_4Gs_IN_UT*mag_value.z,	\
						ACC_SCALE_2G*acc_value.y,		\
						-ACC_SCALE_2G*acc_value.x,		\
						-ACC_SCALE_2G*acc_value.z	))
        {
            evt = DRV_MOTION_EVT_ROT_MAT;
            m_drv_motion.evt_handler(&evt, rotation_matrix, sizeof(float) * 9);
        }
    }

    if (m_drv_motion.features & DRV_MOTION_FEATURE_MASK_QUAT)
    {
	//Check if ratation matrix is already computed
	if (!(m_drv_motion.features & DRV_MOTION_FEATURE_MASK_ROT_MAT))
	{
            orient_calculate_rotation_matrix(	rotation_matrix, 			\
						MAG_SCALE_4Gs_IN_UT*mag_value.y,	\
						MAG_SCALE_4Gs_IN_UT*mag_value.x,	\
						-MAG_SCALE_4Gs_IN_UT*mag_value.z,	\
						ACC_SCALE_2G*acc_value.y,		\
						-ACC_SCALE_2G*acc_value.x,		\
						-ACC_SCALE_2G*acc_value.z	);
	}
        if (orient_calculate_quaternions(quaternion, rotation_matrix) == NRF_SUCCESS)
        {
            evt = DRV_MOTION_EVT_QUAT;
            // W, X, Y, and Z
            m_drv_motion.evt_handler(&evt, quaternion, sizeof(float) * 4);
        }
    }

    if (m_drv_motion.features & DRV_MOTION_FEATURE_MASK_EULER)
    {
		//Check if ratation matrix is already computed
		if (!(m_drv_motion.features & DRV_MOTION_FEATURE_MASK_ROT_MAT))
		{
			orient_calculate_rotation_matrix(	rotation_matrix, 					\
												MAG_SCALE_4Gs_IN_UT*mag_value.y,	\
												MAG_SCALE_4Gs_IN_UT*mag_value.x,	\
												-MAG_SCALE_4Gs_IN_UT*mag_value.z,	\
												ACC_SCALE_2G*acc_value.y,			\
												-ACC_SCALE_2G*acc_value.x,			\
												-ACC_SCALE_2G*acc_value.z	);
		}
        if (orient_calculate_euler_angles(euler, rotation_matrix) == NRF_SUCCESS)
        {
            evt = DRV_MOTION_EVT_EULER;
            // Pitch, roll and yaw
            m_drv_motion.evt_handler(&evt, euler, sizeof(float) * 3);
        }
    }

    if (m_drv_motion.features & DRV_MOTION_FEATURE_MASK_HEADING)
    {
		//Check if rotation matrix is already computed
		if (!(m_drv_motion.features & DRV_MOTION_FEATURE_MASK_ROT_MAT))
		{
			orient_calculate_rotation_matrix(	rotation_matrix, 					\
												MAG_SCALE_4Gs_IN_UT*mag_value.y,	\
												MAG_SCALE_4Gs_IN_UT*mag_value.x,	\
												-MAG_SCALE_4Gs_IN_UT*mag_value.z,	\
												ACC_SCALE_2G*acc_value.y,			\
												-ACC_SCALE_2G*acc_value.x,			\
												-ACC_SCALE_2G*acc_value.z);
		}
		//Check if euler anlges are already computed
		if (!(m_drv_motion.features & DRV_MOTION_FEATURE_MASK_EULER))
		{
			orient_calculate_euler_angles(euler, rotation_matrix);
		}
		evt = DRV_MOTION_EVT_HEADING;
		m_drv_motion.evt_handler(&evt, &euler[2], sizeof(float));
	}

   
	
	return NRF_SUCCESS;
}



