////////////////////////////////////////////////////////////////////////////
//
//  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.


#include "RTIMULSM9DS0.h"
#include "RTIMUSettings.h"

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

#define COMPASS_ALPHA 0.2f

RTIMULSM9DS0::RTIMULSM9DS0(RTIMUSettings *settings) : RTIMU(settings)
{
    m_sampleRate = 100;
}

RTIMULSM9DS0::~RTIMULSM9DS0()
{
}

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

#ifdef LSM9DS0_CACHE_MODE
    m_firstTime = true;
    m_cacheIn = m_cacheOut = m_cacheCount = 0;
#endif
    // 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;

    // work out accelmag address

    if (m_settings->HALRead(LSM9DS0_ACCELMAG_ADDRESS0, LSM9DS0_WHO_AM_I, 1, &result, "")) {
        if (result == LSM9DS0_ACCELMAG_ID) {
            m_accelCompassSlaveAddr = LSM9DS0_ACCELMAG_ADDRESS0;
        }
    } else {
        m_accelCompassSlaveAddr = LSM9DS0_ACCELMAG_ADDRESS1;
    }

    setCalibrationData();

    //  enable the I2C bus

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

    //  Set up the gyro

    if (!m_settings->HALWrite(m_gyroSlaveAddr, LSM9DS0_GYRO_CTRL5, 0x80, "Failed to boot LSM9DS0"))
        return false;

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

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

    if (!setGyroSampleRate())
            return false;

    if (!setGyroCTRL2())
            return false;

    if (!setGyroCTRL4())
            return false;

    //  Set up the accel

    if (!m_settings->HALRead(m_accelCompassSlaveAddr, LSM9DS0_WHO_AM_I, 1, &result, "Failed to read LSM9DS0 accel/mag id"))
        return false;

    if (result != LSM9DS0_ACCELMAG_ID) {
        HAL_ERROR1("Incorrect LSM9DS0 accel/mag id %d\n", result);
        return false;
    }

    if (!setAccelCTRL1())
        return false;

    if (!setAccelCTRL2())
        return false;

    if (!setCompassCTRL5())
        return false;

    if (!setCompassCTRL6())
        return false;

    if (!setCompassCTRL7())
        return false;

#ifdef LSM9DS0_CACHE_MODE

    //  turn on gyro fifo

    if (!m_settings->HALWrite(m_gyroSlaveAddr, LSM9DS0_GYRO_FIFO_CTRL, 0x3f, "Failed to set LSM9DS0 FIFO mode"))
        return false;
#endif

    if (!setGyroCTRL5())
            return false;

    gyroBiasInit();

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

bool RTIMULSM9DS0::setGyroSampleRate()
{
    unsigned char ctrl1;

    switch (m_settings->m_LSM9DS0GyroSampleRate) {
    case LSM9DS0_GYRO_SAMPLERATE_95:
        ctrl1 = 0x0f;
        m_sampleRate = 95;
        break;

    case LSM9DS0_GYRO_SAMPLERATE_190:
        ctrl1 = 0x4f;
        m_sampleRate = 190;
        break;

    case LSM9DS0_GYRO_SAMPLERATE_380:
        ctrl1 = 0x8f;
        m_sampleRate = 380;
        break;

    case LSM9DS0_GYRO_SAMPLERATE_760:
        ctrl1 = 0xcf;
        m_sampleRate = 760;
        break;

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

    m_sampleInterval = (uint64_t)1000000 / m_sampleRate;

    switch (m_settings->m_LSM9DS0GyroBW) {
    case LSM9DS0_GYRO_BANDWIDTH_0:
        ctrl1 |= 0x00;
        break;

    case LSM9DS0_GYRO_BANDWIDTH_1:
        ctrl1 |= 0x10;
        break;

    case LSM9DS0_GYRO_BANDWIDTH_2:
        ctrl1 |= 0x20;
        break;

    case LSM9DS0_GYRO_BANDWIDTH_3:
        ctrl1 |= 0x30;
        break;

    }

    return (m_settings->HALWrite(m_gyroSlaveAddr, LSM9DS0_GYRO_CTRL1, ctrl1, "Failed to set LSM9DS0 gyro CTRL1"));
}

bool RTIMULSM9DS0::setGyroCTRL2()
{
    if ((m_settings->m_LSM9DS0GyroHpf < LSM9DS0_GYRO_HPF_0) || (m_settings->m_LSM9DS0GyroHpf > LSM9DS0_GYRO_HPF_9)) {
        HAL_ERROR1("Illegal LSM9DS0 gyro high pass filter code %d\n", m_settings->m_LSM9DS0GyroHpf);
        return false;
    }
    return m_settings->HALWrite(m_gyroSlaveAddr,  LSM9DS0_GYRO_CTRL2, m_settings->m_LSM9DS0GyroHpf, "Failed to set LSM9DS0 gyro CTRL2");
}

bool RTIMULSM9DS0::setGyroCTRL4()
{
    unsigned char ctrl4;

    switch (m_settings->m_LSM9DS0GyroFsr) {
    case LSM9DS0_GYRO_FSR_250:
        ctrl4 = 0x00;
        m_gyroScale = (RTFLOAT)0.00875 * RTMATH_DEGREE_TO_RAD;
        break;

    case LSM9DS0_GYRO_FSR_500:
        ctrl4 = 0x10;
        m_gyroScale = (RTFLOAT)0.0175 * RTMATH_DEGREE_TO_RAD;
        break;

    case LSM9DS0_GYRO_FSR_2000:
        ctrl4 = 0x20;
        m_gyroScale = (RTFLOAT)0.07 * RTMATH_DEGREE_TO_RAD;
        break;

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

    return m_settings->HALWrite(m_gyroSlaveAddr,  LSM9DS0_GYRO_CTRL4, ctrl4, "Failed to set LSM9DS0 gyro CTRL4");
}


bool RTIMULSM9DS0::setGyroCTRL5()
{
    unsigned char ctrl5;

    //  Turn on hpf

    ctrl5 = 0x10;

#ifdef LSM9DS0_CACHE_MODE
    //  turn on fifo

    ctrl5 |= 0x40;
#endif

    return m_settings->HALWrite(m_gyroSlaveAddr,  LSM9DS0_GYRO_CTRL5, ctrl5, "Failed to set LSM9DS0 gyro CTRL5");
}


bool RTIMULSM9DS0::setAccelCTRL1()
{
    unsigned char ctrl1;

    if ((m_settings->m_LSM9DS0AccelSampleRate < 0) || (m_settings->m_LSM9DS0AccelSampleRate > 10)) {
        HAL_ERROR1("Illegal LSM9DS0 accel sample rate code %d\n", m_settings->m_LSM9DS0AccelSampleRate);
        return false;
    }

    ctrl1 = (m_settings->m_LSM9DS0AccelSampleRate << 4) | 0x07;

    return m_settings->HALWrite(m_accelCompassSlaveAddr,  LSM9DS0_CTRL1, ctrl1, "Failed to set LSM9DS0 accell CTRL1");
}

bool RTIMULSM9DS0::setAccelCTRL2()
{
    unsigned char ctrl2;

    if ((m_settings->m_LSM9DS0AccelLpf < 0) || (m_settings->m_LSM9DS0AccelLpf > 3)) {
        HAL_ERROR1("Illegal LSM9DS0 accel low pass fiter code %d\n", m_settings->m_LSM9DS0AccelLpf);
        return false;
    }

    switch (m_settings->m_LSM9DS0AccelFsr) {
    case LSM9DS0_ACCEL_FSR_2:
        m_accelScale = (RTFLOAT)0.000061;
        break;

    case LSM9DS0_ACCEL_FSR_4:
        m_accelScale = (RTFLOAT)0.000122;
        break;

    case LSM9DS0_ACCEL_FSR_6:
        m_accelScale = (RTFLOAT)0.000183;
        break;

    case LSM9DS0_ACCEL_FSR_8:
        m_accelScale = (RTFLOAT)0.000244;
        break;

    case LSM9DS0_ACCEL_FSR_16:
        m_accelScale = (RTFLOAT)0.000732;
        break;

    default:
        HAL_ERROR1("Illegal LSM9DS0 accel FSR code %d\n", m_settings->m_LSM9DS0AccelFsr);
        return false;
    }

    ctrl2 = (m_settings->m_LSM9DS0AccelLpf << 6) | (m_settings->m_LSM9DS0AccelFsr << 3);

    return m_settings->HALWrite(m_accelCompassSlaveAddr,  LSM9DS0_CTRL2, ctrl2, "Failed to set LSM9DS0 accel CTRL2");
}


bool RTIMULSM9DS0::setCompassCTRL5()
{
    unsigned char ctrl5;

    if ((m_settings->m_LSM9DS0CompassSampleRate < 0) || (m_settings->m_LSM9DS0CompassSampleRate > 5)) {
        HAL_ERROR1("Illegal LSM9DS0 compass sample rate code %d\n", m_settings->m_LSM9DS0CompassSampleRate);
        return false;
    }

    ctrl5 = (m_settings->m_LSM9DS0CompassSampleRate << 2);

#ifdef LSM9DS0_CACHE_MODE
    //  enable fifo

    ctrl5 |= 0x40;
#endif

    return m_settings->HALWrite(m_accelCompassSlaveAddr,  LSM9DS0_CTRL5, ctrl5, "Failed to set LSM9DS0 compass CTRL5");
}

bool RTIMULSM9DS0::setCompassCTRL6()
{
    unsigned char ctrl6;

    //  convert FSR to uT

    switch (m_settings->m_LSM9DS0CompassFsr) {
    case LSM9DS0_COMPASS_FSR_2:
        ctrl6 = 0;
        m_compassScale = (RTFLOAT)0.008;
        break;

    case LSM9DS0_COMPASS_FSR_4:
        ctrl6 = 0x20;
        m_compassScale = (RTFLOAT)0.016;
        break;

    case LSM9DS0_COMPASS_FSR_8:
        ctrl6 = 0x40;
        m_compassScale = (RTFLOAT)0.032;
        break;

    case LSM9DS0_COMPASS_FSR_12:
        ctrl6 = 0x60;
        m_compassScale = (RTFLOAT)0.0479;
        break;

    default:
        HAL_ERROR1("Illegal LSM9DS0 compass FSR code %d\n", m_settings->m_LSM9DS0CompassFsr);
        return false;
    }

    return m_settings->HALWrite(m_accelCompassSlaveAddr,  LSM9DS0_CTRL6, ctrl6, "Failed to set LSM9DS0 compass CTRL6");
}

bool RTIMULSM9DS0::setCompassCTRL7()
{
     return m_settings->HALWrite(m_accelCompassSlaveAddr,  LSM9DS0_CTRL7, 0x60, "Failed to set LSM9DS0CTRL7");
}



int RTIMULSM9DS0::IMUGetPollInterval()
{
    return (400 / m_sampleRate);
}

bool RTIMULSM9DS0::IMURead()
{
    unsigned char status;
    unsigned char gyroData[6];
    unsigned char accelData[6];
    unsigned char compassData[6];


#ifdef LSM9DS0_CACHE_MODE
    int count;

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

    if ((status & 0x40) != 0) {
        HAL_INFO("LSM9DS0 gyro fifo overrun\n");
        if (!m_settings->HALWrite(m_gyroSlaveAddr, LSM9DS0_GYRO_CTRL5, 0x10, "Failed to set LSM9DS0 gyro CTRL5"))
            return false;

        if (!m_settings->HALWrite(m_gyroSlaveAddr, LSM9DS0_GYRO_FIFO_CTRL, 0x0, "Failed to set LSM9DS0 gyro FIFO mode"))
            return false;

        if (!m_settings->HALWrite(m_gyroSlaveAddr, LSM9DS0_GYRO_FIFO_CTRL, 0x3f, "Failed to set LSM9DS0 gyro FIFO mode"))
            return false;

        if (!setGyroCTRL5())
            return false;

        m_imuData.timestamp += m_sampleInterval * 32;
        return false;
    }

    // get count of samples in fifo
    count = status & 0x1f;

    if ((m_cacheCount == 0) && (count > 0) && (count < LSM9DS0_FIFO_THRESH)) {
        // special case of a small fifo and nothing cached - just handle as simple read

        if (!m_settings->HALRead(m_gyroSlaveAddr, 0x80 | LSM9DS0_GYRO_OUT_X_L, 6, gyroData, "Failed to read LSM9DS0 gyro data"))
            return false;

        if (!m_settings->HALRead(m_accelCompassSlaveAddr, 0x80 | LSM9DS0_OUT_X_L_A, 6, accelData, "Failed to read LSM9DS0 accel data"))
            return false;

        if (!m_settings->HALRead(m_accelCompassSlaveAddr, 0x80 | LSM9DS0_OUT_X_L_M, 6, compassData, "Failed to read LSM9DS0 compass data"))
            return false;

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

        m_firstTime = false;
   } else {
        if (count >=  LSM9DS0_FIFO_THRESH) {
            // need to create a cache block

            if (m_cacheCount == LSM9DS0_CACHE_BLOCK_COUNT) {
                // all cache blocks are full - discard oldest and update timestamp to account for lost samples
                m_imuData.timestamp += m_sampleInterval * m_cache[m_cacheOut].count;
                if (++m_cacheOut == LSM9DS0_CACHE_BLOCK_COUNT)
                    m_cacheOut = 0;
                m_cacheCount--;
            }

            if (!m_settings->HALRead(m_gyroSlaveAddr, 0x80 | LSM9DS0_GYRO_OUT_X_L, LSM9DS0_FIFO_CHUNK_SIZE * LSM9DS0_FIFO_THRESH,
                         m_cache[m_cacheIn].data, "Failed to read LSM9DS0 fifo data"))
                return false;

            if (!m_settings->HALRead(m_accelCompassSlaveAddr, 0x80 | LSM9DS0_OUT_X_L_A, 6,
                         m_cache[m_cacheIn].accel, "Failed to read LSM9DS0 accel data"))
                return false;

            if (!m_settings->HALRead(m_accelCompassSlaveAddr, 0x80 | LSM9DS0_OUT_X_L_M, 6,
                         m_cache[m_cacheIn].compass, "Failed to read LSM9DS0 compass data"))
                return false;

            m_cache[m_cacheIn].count = LSM9DS0_FIFO_THRESH;
            m_cache[m_cacheIn].index = 0;

            m_cacheCount++;
            if (++m_cacheIn == LSM9DS0_CACHE_BLOCK_COUNT)
                m_cacheIn = 0;

        }

        //  now fifo has been read if necessary, get something to process

        if (m_cacheCount == 0)
            return false;

        memcpy(gyroData, m_cache[m_cacheOut].data + m_cache[m_cacheOut].index, LSM9DS0_FIFO_CHUNK_SIZE);
        memcpy(accelData, m_cache[m_cacheOut].accel, 6);
        memcpy(compassData, m_cache[m_cacheOut].compass, 6);

        m_cache[m_cacheOut].index += LSM9DS0_FIFO_CHUNK_SIZE;

        if (--m_cache[m_cacheOut].count == 0) {
            //  this cache block is now empty

            if (++m_cacheOut == LSM9DS0_CACHE_BLOCK_COUNT)
                m_cacheOut = 0;
            m_cacheCount--;
        }
        if (m_firstTime)
            m_imuData.timestamp = RTMath::currentUSecsSinceEpoch();
        else
            m_imuData.timestamp += m_sampleInterval;

        m_firstTime = false;
    }

#else
    if (!m_settings->HALRead(m_gyroSlaveAddr, LSM9DS0_GYRO_STATUS, 1, &status, "Failed to read LSM9DS0 status"))
        return false;

    if ((status & 0x8) == 0)
        return false;

    if (!m_settings->HALRead(m_gyroSlaveAddr, 0x80 | LSM9DS0_GYRO_OUT_X_L, 6, gyroData, "Failed to read LSM9DS0 gyro data"))
        return false;

    m_imuData.timestamp = RTMath::currentUSecsSinceEpoch();

    if (!m_settings->HALRead(m_accelCompassSlaveAddr, 0x80 | LSM9DS0_OUT_X_L_A, 6, accelData, "Failed to read LSM9DS0 accel data"))
        return false;

    if (!m_settings->HALRead(m_accelCompassSlaveAddr, 0x80 | LSM9DS0_OUT_X_L_M, 6, compassData, "Failed to read LSM9DS0 compass data"))
        return false;

#endif

    RTMath::convertToVector(gyroData, m_imuData.gyro, m_gyroScale, false);
    RTMath::convertToVector(accelData, m_imuData.accel, m_accelScale, false);
    RTMath::convertToVector(compassData, m_imuData.compass, m_compassScale, false);

    //  sort out gyro axes and correct for bias

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

    //  sort out accel data;

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

    //  sort out compass axes

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

    //  now do standard processing

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

    //  now update the filter

    updateFusion();

    return true;
}
