////////////////////////////////////////////////////////////////////////////
//
//  This file is part of RTIMULib
//
//  Copyright (c) 2014-2015, richards-tech, LLC
//
//  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 "RTIMUAccelCal.h"

//  ACCEL_ALPHA control the smoothing - the lower it is, the smoother it is

#define ACCEL_ALPHA                     0.1f

RTIMUAccelCal::RTIMUAccelCal(RTIMUSettings *settings)
{
    m_settings = settings;
    for (int i = 0; i < 3; i++)
        m_accelCalEnable[i] = false;
}

RTIMUAccelCal::~RTIMUAccelCal()
{

}

void RTIMUAccelCal::accelCalInit()
{
    if (m_settings->m_accelCalValid) {
        m_accelMin = m_settings->m_accelCalMin;
        m_accelMax = m_settings->m_accelCalMax;
    } else {
        m_accelMin = RTVector3(RTIMUCALDEFS_DEFAULT_MIN, RTIMUCALDEFS_DEFAULT_MIN, RTIMUCALDEFS_DEFAULT_MIN);
        m_accelMax = RTVector3(RTIMUCALDEFS_DEFAULT_MAX, RTIMUCALDEFS_DEFAULT_MAX, RTIMUCALDEFS_DEFAULT_MAX);
    }
}

void RTIMUAccelCal::accelCalReset()
{
    for (int i = 0; i < 3; i++) {
        if (m_accelCalEnable[i]) {
            m_accelMin.setData(i, RTIMUCALDEFS_DEFAULT_MIN);
            m_accelMax.setData(i, RTIMUCALDEFS_DEFAULT_MAX);
        }
    }
}

void RTIMUAccelCal::accelCalEnable(int axis, bool enable)
{
    m_accelCalEnable[axis] = enable;
}

void RTIMUAccelCal::newAccelCalData(const RTVector3& data)
{

    for (int i = 0; i < 3; i++) {
        if (m_accelCalEnable[i]) {
            m_averageValue.setData(i, (data.data(i) * ACCEL_ALPHA + m_averageValue.data(i) * (1.0 - ACCEL_ALPHA)));
            if (m_accelMin.data(i) > m_averageValue.data(i))
                m_accelMin.setData(i, m_averageValue.data(i));
            if (m_accelMax.data(i) < m_averageValue.data(i))
                m_accelMax.setData(i, m_averageValue.data(i));
        }
    }
}

bool RTIMUAccelCal::accelCalValid()
{
    bool valid = true;

    for (int i = 0; i < 3; i++) {
        if (m_accelMax.data(i) < m_accelMin.data(i))
            valid = false;
    }
    return valid;
}

bool RTIMUAccelCal::accelCalSave()
{
    if (!accelCalValid())
        return false;

    m_settings->m_accelCalValid = true;
    m_settings->m_accelCalMin = m_accelMin;
    m_settings->m_accelCalMax = m_accelMax;
    m_settings->saveSettings();
    return true;
}