/*******************************************************************************

  Robot Toolkit ++ (RTK++)

  Copyright (c) 2007-2014 Shuhui Bu <bushuhui@nwpu.edu.cn>
    http://www.adv-ci.com

  ----------------------------------------------------------------------------

    This program is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program. If not, see <http://www.gnu.org/licenses/>.

*******************************************************************************/

#include "imu_tracking.h"
#include "rtk_dsp.h"

using namespace std;

void IMU_Tracking::integrateVel(void)
{
    return;
}

void IMU_Tracking::integrateDisp(void)
{
    typedef deque<IMU_TrackingData>::iterator   it_td;

    it_td       it;

    double      vel[3], acc[3], dis[3],
                dt;
    double      *vel_x, *vel_y, *vel_z;
    double      *dis_x, *dis_y, *dis_z;

    int         i, j;
    int         frame_n;

    IMU_TrackingData    td1, td2;

    m_mux.lock();

    frame_n = m_trackingDataQueue.size();

    if( frame_n < 10 ) {
        m_mux.unlock();
        return;
    }

    vel_x = new double[frame_n];
    vel_y = new double[frame_n];
    vel_z = new double[frame_n];
    dis_x = new double[frame_n];
    dis_y = new double[frame_n];
    dis_z = new double[frame_n];

    for(i=0; i<3; i++) {
        vel[i] = m_initialVel[i];
        dis[i] = m_initialDisp[i];
    }

    // integrate velocity
    vel_x[0] = vel[0];
    vel_y[0] = vel[1];
    vel_z[0] = vel[2];

    for(j=0, it=m_trackingDataQueue.begin(); it!=m_trackingDataQueue.end(); it++, j++) {
        td2 = *it;

        // integrate acc to vel
        if( j > 0 ) {
            dt = 1.0*(td2.m_af.tm_s - td1.m_af.tm_s)/1000000.0;

            for(i=0; i<3; i++) {
                acc[i] = (td1.m_acc[i] + td2.m_acc[i])/2.0;

                vel[i] = vel[i] + dt*acc[i];
            }

            vel_x[j] = vel[0];
            vel_y[j] = vel[1];
            vel_z[j] = vel[2];
        }

        // set current frame to next time's prev frame
        td1 = td2;
    }

    // do filtfilt
    rtk::filtfilt(1, m_filt_a, m_filt_b, frame_n, vel_x);
    rtk::filtfilt(1, m_filt_a, m_filt_b, frame_n, vel_y);
    rtk::filtfilt(1, m_filt_a, m_filt_b, frame_n, vel_z);

    // integrate displacement
    dis_x[0] = dis[0];
    dis_y[0] = dis[1];
    dis_z[0] = dis[2];

    for(j=0, it=m_trackingDataQueue.begin(); it!=m_trackingDataQueue.end(); it++, j++) {
        td2 = *it;

        // integrate vel to disp
        if( j > 0 ) {
            dt = 1.0*(td2.m_af.tm_s - td1.m_af.tm_s)/1000000.0;

            vel[0] = (vel_x[j] + vel_x[j-1])/2.0;
            vel[1] = (vel_y[j] + vel_y[j-1])/2.0;
            vel[2] = (vel_z[j] + vel_z[j-1])/2.0;

            for(i=0; i<3; i++) {
                dis[i] = dis[i] + dt*vel[i];
            }

            dis_x[j] = dis[0];
            dis_y[j] = dis[1];
            dis_z[j] = dis[2];
        }

        // set current frame to next time's prev frame
        td1 = td2;
    }

    // do filtfilt
    rtk::filtfilt(1, m_filt_a, m_filt_b, frame_n, dis_x);
    rtk::filtfilt(1, m_filt_a, m_filt_b, frame_n, dis_y);
    rtk::filtfilt(1, m_filt_a, m_filt_b, frame_n, dis_z);

    // copy calculated data back
    for(j=0, it=m_trackingDataQueue.begin(); it!=m_trackingDataQueue.end(); it++, j++) {
        IMU_TrackingData &td = *it;

        td.m_vel[0] = vel_x[j];
        td.m_vel[1] = vel_y[j];
        td.m_vel[2] = vel_z[j];

        td.m_disp[0] = dis_x[j];
        td.m_disp[1] = dis_y[j];
        td.m_disp[2] = dis_z[j];

        td.m_calculated = 1;
    }

    m_mux.unlock();

    m_disp[0] = dis_x[frame_n-1];
    m_disp[1] = dis_y[frame_n-1];
    m_disp[2] = dis_z[frame_n-1];

    delete [] vel_x;
    delete [] vel_y;
    delete [] vel_z;
    delete [] dis_x;
    delete [] dis_y;
    delete [] dis_z;

    return;
}

int IMU_Tracking::frame_num(void)
{
    return m_trackingDataQueue.size();
}

void IMU_Tracking::frame_clear(void)
{
    m_mux.lock();
    m_trackingDataQueue.clear();
    m_mux.unlock();
}

void IMU_Tracking::frame_insert(AHRS_Frame &f)
{
    float       R[9], v;
    int         i, j;

    IMU_TrackingData    td;

    // transform body accelation to global coordinate
    f.q.toRotMat(R);

    for(j=0; j<3; j++) {
        v = 0.0;
        for(i=0; i<3; i++) v += R[j*3+i]*f.fA[i];

        td.m_acc[j] = v*m_g;
    }

    // convert to linear acc
    td.m_acc[2] -= m_g;

    // insert to queue
    td.m_af = f;

    m_mux.lock();
    m_trackingDataQueue.push_back(td);
    m_mux.unlock();
}

int  IMU_Tracking::get_disp(double *d)
{
    d[0] = m_disp[0];
    d[1] = m_disp[1];
    d[2] = m_disp[2];

    return 0;
}