/**
 * Copyright (c) 2015 - 2018, Nordic Semiconductor ASA
 * 
 * All rights reserved.
 * 
 * Redistribution and use in source and binary forms, with or without modification,
 * are permitted provided that the following conditions are met:
 * 
 * 1. Redistributions of source code must retain the above copyright notice, this
 *    list of conditions and the following disclaimer.
 * 
 * 2. Redistributions in binary form, except as embedded into a Nordic
 *    Semiconductor ASA integrated circuit in a product or a software update for
 *    such product, 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.
 * 
 * 3. Neither the name of Nordic Semiconductor ASA nor the names of its
 *    contributors may be used to endorse or promote products derived from this
 *    software without specific prior written permission.
 * 
 * 4. This software, with or without modification, must only be used with a
 *    Nordic Semiconductor ASA integrated circuit.
 * 
 * 5. Any software provided in binary form under this license must not be reverse
 *    engineered, decompiled, modified and/or disassembled.
 * 
 * THIS SOFTWARE IS PROVIDED BY NORDIC SEMICONDUCTOR ASA "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 NORDIC SEMICONDUCTOR ASA 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.
 * 
 */
/** @file
 * @defgroup tw_sensor_example main.c
 * @{
 * @ingroup nrf_twi_example
 * @brief TWI Sensor Example main file.
 *
 * This file contains the source code for a sample application using TWI.
 *
 */

#include <stdio.h>
#include "boards.h"
#include "app_util_platform.h"
#include "app_error.h"
#include "nrf_delay.h"
#include "nrf_log.h"
#include "nrf_log_ctrl.h"
#include "nrf_log_default_backends.h"
#include "SEGGER_RTT.h"
#include "GPIO.h"
#include "inv_mpu.h"
#include "tdk_ERmini.h"
#include "nrf_drv_rtc.h"

#include "inv_mpu.h"
#include "inv_mpu_dmp_motion_driver.h"
#include "invensense.h"
#include "invensense_adv.h"
#include "eMPL_outputs.h"
#include "mltypes.h"
#include "mpu.h"
#include "log.h"
#include "packet.h"
#include "timestamping.h"

#define UART_TX_BUF_SIZE 256                         /**< UART TX buffer size. */
#define UART_RX_BUF_SIZE 256                         /**< UART RX buffer size. */

#define COMPASS_ENABLED     1
#define NRF52               1

static nrf_drv_rtc_t const m_rtc = NRF_DRV_RTC_INSTANCE(0);

// RTC tick events generation.
static void rtc_handler(nrf_drv_rtc_int_type_t int_type)
{
}

static inline void run_self_test(void)
{
    int result;
    long gyro[3], accel[3];

#if defined (MPU6500) || defined (MPU9250)
    result = mpu_run_6500_self_test(gyro, accel, 0);
    NRF_LOG_INFO("MU9250");
#elif defined (MPU6050) || defined (MPU9150)
    result = mpu_run_self_test(gyro, accel);
#endif
    if (result == 0x7) {
    NRF_LOG_INFO("Passed!\n");
        NRF_LOG_INFO("accel:" NRF_LOG_FLOAT_MARKER  " ",NRF_LOG_FLOAT(accel[0]/65536.f));
        NRF_LOG_INFO("" NRF_LOG_FLOAT_MARKER  " ",NRF_LOG_FLOAT(accel[1]/65536.f));
        NRF_LOG_INFO("" NRF_LOG_FLOAT_MARKER  "\n",NRF_LOG_FLOAT(accel[2]/65536.f));

        NRF_LOG_INFO("gyro:" NRF_LOG_FLOAT_MARKER  " ",NRF_LOG_FLOAT(gyro[0]/65536.f));
        NRF_LOG_INFO("" NRF_LOG_FLOAT_MARKER  " ",NRF_LOG_FLOAT(gyro[1]/65536.f));
        NRF_LOG_INFO("" NRF_LOG_FLOAT_MARKER  "\n",NRF_LOG_FLOAT(gyro[2]/65536.f));

        /* Test passed. We can trust the gyro data here, so now we need to update calibrated data*/

#ifdef USE_CAL_HW_REGISTERS
        /*
         * This portion of the code uses the HW offset registers that are in the MPUxxxx devices
         * instead of pushing the cal data to the MPL software library
         */
        unsigned char i = 0;

        for(i = 0; i<3; i++) {
            gyro[i] = (long)(gyro[i] * 32.8f); //convert to +-1000dps
            accel[i] *=  2048.f; //convert to +-16G
            accel[i] = accel[i] >> 16;
            gyro[i] = (long)(gyro[i] >> 16);
        }

        mpu_set_gyro_bias_reg(gyro);

#if defined (MPU6500) || defined (MPU9250)
        mpu_set_accel_bias_6500_reg(accel);
#elif defined (MPU6050) || defined (MPU9150)
        mpu_set_accel_bias_6050_reg(accel);
#endif
#else
        /* Push the calibrated data to the MPL library.
         *
         * MPL expects biases in hardware units << 16, but self test returns
         * biases in g's << 16.
         */
        unsigned short accel_sens;
        float gyro_sens;

        mpu_get_accel_sens(&accel_sens);
        accel[0] *= accel_sens;
        accel[1] *= accel_sens;
        accel[2] *= accel_sens;
        inv_set_accel_bias(accel, 3);
        mpu_get_gyro_sens(&gyro_sens);
        gyro[0] = (long) (gyro[0] * gyro_sens);
        gyro[1] = (long) (gyro[1] * gyro_sens);
        gyro[2] = (long) (gyro[2] * gyro_sens);
        inv_set_gyro_bias(gyro, 3);

#endif
    }
    else {
            if (!(result & 0x1))
                NRF_LOG_ERROR("Gyro failed.\n");
            if (!(result & 0x2))
                NRF_LOG_ERROR("Accel failed.\n");
            if (!(result & 0x4))
                NRF_LOG_ERROR("Compass failed.\n");
     }

}
/**
 * @brief Function for main application entry.
 */
int main(void)
{
    unsigned char accel_fsr,  new_temp = 0;
    unsigned short gyro_rate, gyro_fsr;
    unsigned long timestamp;
    struct int_param_s int_param;
    int result;

#ifdef COMPASS_ENABLED
    unsigned char new_compass = 0;
    unsigned short compass_fsr;
#endif

    uint8_t  product_id;
    uint8_t  data[4];
            
    gpio_init();
    
    APP_ERROR_CHECK(NRF_LOG_INIT(NULL));
    NRF_LOG_DEFAULT_BACKENDS_INIT();
    NRF_LOG_INFO("TWI example.");

    ERmini_init();
    
    result = mpu_init(&int_param);
    if (result) {
        NRF_LOG_INFO("Could not initialize gyro.\n");
    }
    
    run_self_test();
    
    mpu_set_accel_fsr(16);

    //mpu_lp_motion_interrupt(8000, 255, 640);
   
    
    result = mpu_get_accel_fsr(&accel_fsr);
    if(!result)
        NRF_LOG_INFO("accel_fsr: %d",accel_fsr);


    result = mpu_get_gyro_fsr(&gyro_fsr);
    if(!result)
        NRF_LOG_INFO("gyro_fsr_fsr: %d",gyro_fsr);
    
    result = mpu_get_compass_fsr(&compass_fsr);
    if(!result)
        NRF_LOG_INFO("compass_fsr: %d",compass_fsr);
    NRF_LOG_FLUSH();

    while (1)
     {
        
        short gyro[3], accel[3], compass[3];
        long temp;

        get_ms(&timestamp);

        result = mpu_get_gyro_reg(gyro, &timestamp);

        NRF_LOG_INFO("Timestamp: %d" ,timestamp);
        NRF_LOG_INFO("gyro:" NRF_LOG_FLOAT_MARKER  " ",NRF_LOG_FLOAT(gyro[0]/16.4f));
         NRF_LOG_INFO("" NRF_LOG_FLOAT_MARKER  " ",NRF_LOG_FLOAT(gyro[1]/16.4f));
        NRF_LOG_INFO("" NRF_LOG_FLOAT_MARKER  "\n",NRF_LOG_FLOAT(gyro[2]/16.4f));

        result = mpu_get_accel_reg(accel, &timestamp);

        NRF_LOG_INFO("Timestamp: %d" ,timestamp);
        NRF_LOG_INFO("accel:" NRF_LOG_FLOAT_MARKER  " ",NRF_LOG_FLOAT(accel[0]/2048.f));
        NRF_LOG_INFO("" NRF_LOG_FLOAT_MARKER  " ",NRF_LOG_FLOAT(accel[1]/2048.f));
        NRF_LOG_INFO("" NRF_LOG_FLOAT_MARKER  "\n",NRF_LOG_FLOAT(accel[2]/2048.f));

        result = mpu_get_compass_reg(compass, &timestamp);

        NRF_LOG_INFO("Timestamp: %d" ,timestamp);
        NRF_LOG_INFO("compass:" NRF_LOG_FLOAT_MARKER  " ",NRF_LOG_FLOAT(compass[0]*.6f));
        NRF_LOG_INFO("" NRF_LOG_FLOAT_MARKER  " ",NRF_LOG_FLOAT(compass[1]*.6f));
        NRF_LOG_INFO("" NRF_LOG_FLOAT_MARKER  "\n",NRF_LOG_FLOAT(compass[2]*.6f));

        result = mpu_get_temperature(&temp, null);

        NRF_LOG_INFO("Timestamp: %d" ,timestamp);
        NRF_LOG_INFO("temperature:" NRF_LOG_FLOAT_MARKER  "\n",NRF_LOG_FLOAT(temp*0.002995177763f));
                
        NRF_LOG_FLUSH();
        nrf_delay_ms(100);
     }
}


