/******************** (C) COPYRIGHT 2020 Geek************************************
* File Name          : pdr_ahrs.c
* Department         : Sensor Algorithm Team
* Current Version    : V1.2
* Author             : logzhan & 
* 
* 
* Date of Issued     : 2020.7.3
* Comments           : PDR AHRS 姿态解算相关
********************************************************************************/
/* Header File Including -----------------------------------------------------*/
#include <math.h>
#include <string.h>
#include "PDRBase.h"
#include "AHRS.h"
#include "Detector.h"
#include "Filter.h"
#include "buffer.h"
#include "Kalman.h"
#include "Utils.h"
#include "Quaternion.h"

/* Macro Definition ----------------------------------------------------------*/
#define AHRS_BUF_LEN                        256
#define FLT_THRES                           0.000001f                    // 浮点数最小阈值
//计算模值
#define GET_MOL(v)                          sqrt((v)[0] * (v)[0] + (v)[1] * (v)[1] + (v)[2] * (v)[2])
//获取数组长度
#define ARR_LEN(arr)                        (sizeof(arr) / sizeof(*arr))
#define SET_BIT(msk)                        (Ahrs.status |= AHRS_STATUS_##msk)
#define CLR_BIT(msk)                        (Ahrs.status &= ~AHRS_STATUS_##msk)
#define TST_BIT(msk)                        (Ahrs.status & AHRS_STATUS_##msk)
#define INC_PTR(buf, ptr)                   (((ptr) + 1) % ((buf).length + 1))

#define GRV_BUF_NME(i)                      "grav_buf_##i"
#define GYR_BUF_NME(i)                      "gyro_buf_##i"
#define MAG_BUF_NME(i)                      "magn_buf_##i"

/* Global Variable Definition ------------------------------------------------*/
/* Macro Definition ----------------------------------------------------------*/

/* Global Variable Definition ------------------------------------------------*/
static IMU     g_imu;
AHRS_t           Ahrs;
static uint64_t    g_ticker;
static BUFFER_LONG g_erro_buf;
static BUFFER_LONG g_grav_buf[3];
static BUFFER_LONG g_gyro_buf[3];
static BUFFER_LONG g_magn_buf[3];

static float dt = 1.0f / AHRS_SAMPLE_FREQ;                           // sample interval in second
static float Kp = 0;                                                 // 2 * proportional gain (Kp)
static float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f;             // 初始四元数

static const float g_x_axis[] = {1, 0, 0};
static const float g_y_axis[] = {0, 1, 0};
static const float g_z_axis[] = {0, 0, 1};

/**----------------------------------------------------------------------
* Function    : MahonyUpdateAHRS
* Description : Mahony姿态更新算法
* Date        : 2020/02/18 logzhan :
*               增加了AHRS稳定性标志位，如果AHRS稳定后不再计算error, 降低运算
                量。能提升PDR仿真10%的运行速度
*---------------------------------------------------------------------**/
static void MahonyUpdateAHRS(float gx, float gy, float gz, float ax, float ay, float az, 
                             float mx, float my, float mz) {
    // 归一化磁力计和加速度计
    Vec3Norm(&ax, &ay, &az);

    float wx = 0.0f, wy = 0.0f, wz = 0.0f;

    
    //if (!((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f))) {

    //    Vec3Norm(&mx, &my, &mz);

    //    // 把磁场从载体系转换到地理坐标系 h = R^-1 * (mx,my,mz)
    //    float hx = 2.0f * (mx * (0.5f - q2 * q2 - q3 * q3) + my * (q1 * q2 - q0 * q3) + mz * (q1 * q3 + q0 * q2));
    //    float hy = 2.0f * (mx * (q1 * q2 + q0 * q3) + my * (0.5f - q1 * q1 - q3 * q3) + mz * (q2 * q3 - q0 * q1));
    //    float hz = 2.0f * (mx * (q1 * q3 - q0 * q2) + my * (q2 * q3 + q0 * q1) + mz * (0.5f - q1 * q1 - q2 * q2));

    //    // 理论上bx = 0, by 指向北向，因为手机IMU数据使用的是东北天(x,y,z)坐标系
    //    float by = (float)sqrt(hx * hx + hy * hy);
    //    float bz = hz;

    //    wx = by * (q0 * q3 + q1 * q2) + bz * (q1 * q3 - q0 * q2);
    //    wy = by * (0.5f - q1 * q1 - q3 * q3) + bz * (q0 * q1 + q2 * q3);
    //    wz = by * (q2 * q3 - q0 * q1) + bz * (0.5f - q1 * q1 - q2 * q2);
    //}

    // 把重力转换到地理坐标系   v = R * (0,0,1)
    float vx = q1*q3 - q0*q2;
    float vy = q0*q1 + q2*q3;
    float vz = q0*q0 - 0.5f + q3*q3;

    // 计算矢量叉乘误差
    float ex = ay*vz - az*vy + my*wz - mz*wy;
    float ey = az*vx - ax*vz + mz*wx - mx*wz;
    float ez = ax*vy - ay*vx + mx*wy - my*wx;

    // Apply proportional feedback
    gx += Kp * ex;
    gy += Kp * ey;
    gz += Kp * ez;

    // 这两个函数占了大量的时间
    if (Ahrs.stable == PDR_FALSE) {
        BufferPush((Buffer_t*)&g_erro_buf, (float)(2 * sqrt(ex * ex + ey * ey + ez * ez)));
        BufferMean((Buffer_t*)&g_erro_buf, &Ahrs.error);
    }

    // 龙格库塔法更新四元数
    float qa = q0; float qb = q1; float qc = q2;
    q0 += (-qb * gx - qc * gy - q3 * gz) * 0.5f * (dt);
    q1 += ( qa * gx + qc * gz - q3 * gy) * 0.5f * (dt);
    q2 += ( qa * gy - qb * gz + q3 * gx) * 0.5f * (dt);
    q3 += ( qa * gz + qb * gy - qc * gx) * 0.5f * (dt);

    // 四元数归一化
    QuaternionNorm(&q0, &q1, &q2, &q3);

    // 四元数转欧拉角
    Ahrs.Pitch = (float)asin(-2.0f * (q3*q1 - q0*q2)) * (180.0f / 3.141592f);
    Ahrs.Yaw   = (float)atan2(q2*q1 + q0*q3, 0.5f - q2*q2 - q3*q3) * (180.0f / 3.141592f);
    Ahrs.Roll  = (float)atan2(q2*q3 + q0*q1, 0.5f - q2*q2 - q1*q1) * (180.0f / 3.141592f);
}

/**---------------------------------------------------------------------
* Function    : AHRS_sensorFrame2EarthFrame
* Description : 转换加速度计值到地球坐标系
* Date        : 2020/7/20 logzhan
*---------------------------------------------------------------------**/
void AHRS_sensorFrame2EarthFrame(float e[], float s[]) {
    float quaVec[4];
    float quaTmp[4];
    float quaCnj[4];

    quaVec[0] = 0;
    quaVec[1] = s[0];
    quaVec[2] = s[1];
    quaVec[3] = s[2];
    QuaternProd(quaTmp, Ahrs.qnb, quaVec);
    QuaternConj(quaCnj, Ahrs.qnb);
    QuaternProd(quaVec, quaTmp, quaCnj);
    e[0] = quaVec[1];
    e[1] = quaVec[2];
    e[2] = quaVec[3];
}

void AHRS_Init(void) {
    int i;
    Ahrs.stable = PDR_FALSE;
    BufferInit((Buffer_t *)&g_erro_buf, "erro_buf", BUFFER_TYPE_QUEUE, AHRS_BUF_LEN);
    for (i = 0; i < 3; ++i) {
        BufferInit((Buffer_t *)&g_grav_buf[i], GRV_BUF_NME(i), BUFFER_TYPE_QUEUE, AHRS_BUF_LEN);
        BufferInit((Buffer_t *)&g_gyro_buf[i], GYR_BUF_NME(i), BUFFER_TYPE_QUEUE, AHRS_BUF_LEN);
        BufferInit((Buffer_t *)&g_magn_buf[i], MAG_BUF_NME(i), BUFFER_TYPE_QUEUE, AHRS_BUF_LEN);
    }
    ResetARHS();
}

void ResetARHS(void) {

    BufferClear((Buffer_t *)&g_erro_buf);
    for (uchar i = 0; i < 3; ++i) {
        BufferClear((Buffer_t *)&g_grav_buf[i]);
        BufferClear((Buffer_t *)&g_gyro_buf[i]);
        BufferClear((Buffer_t *)&g_magn_buf[i]);
    }
    g_ticker = 0;
    q0 = 1.0f;
    q1 = 0.0f;
    q2 = 0.0f;
    q3 = 0.0f;
    memset(&Ahrs, 0, sizeof(Ahrs));
    Ahrs.status = (uint8_t)AHRS_STATUS_RESET_PHASE_0;
    Ahrs.error = -1;
    Ahrs.qnb[0] = 1.0f;
    Ahrs.qnb[1] = 0.0f;
    Ahrs.qnb[2] = 0.0f;
    Ahrs.qnb[3] = 0.0f;
    Ahrs.gravity[0] = 0.0f;
    Ahrs.gravity[1] = 0.0f;
    Ahrs.gravity[2] = 9.8f;
    Ahrs.x_axis[0] = 1.0f;
    Ahrs.x_axis[1] = 0.0f;
    Ahrs.x_axis[2] = 0.0f;
    Ahrs.y_axis[0] = 0.0f;
    Ahrs.y_axis[1] = 1.0f;
    Ahrs.y_axis[2] = 0.0f;
    Ahrs.z_axis[0] = 0.0f;
    Ahrs.z_axis[1] = 0.0f;
    Ahrs.z_axis[2] = 1.0f;
}

/**---------------------------------------------------------------------
* Function    : estimate_sensor_vector
* Description : 利用当前四元数q与陀螺仪，计算在当前姿态下grav
* input       : float eVec[], float gyro[]
* output      : float sVec[]
* Date        : 2020/7/20 logzhan
*---------------------------------------------------------------------**/
static void estimate_sensor_vector(float sVec[], float eVec[], float gyro[]) {
    float q[4];
    float gx, gy, gz;
    float qa, qb, qc;
    float recipNorm;
    float quaVec[4];
    float quaTmp[4];
    float quaCnj[4];

    /* estimate */
    gx = gyro[0] * (0.5f * (dt));
    gy = gyro[1] * (0.5f * (dt));
    gz = gyro[2] * (0.5f * (dt));
    qa = q0;
    qb = q1;
    qc = q2;
    q[0] = q0 + (-qb * gx - qc * gy - q3 * gz);
    q[1] = q1 + (qa * gx + qc * gz - q3 * gy);
    q[2] = q2 + (qa * gy - qb * gz + q3 * gx);
    q[3] = q3 + (qa * gz + qb * gy - qc * gx); 
    recipNorm = InvSqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
    q[0] *= recipNorm;
    q[1] *= recipNorm;
    q[2] *= recipNorm;
    q[3] *= recipNorm;

    /* project */
    QuaternConj(q, q);
    quaVec[0] = 0;
    quaVec[1] = eVec[0];
    quaVec[2] = eVec[1];
    quaVec[3] = eVec[2];
    QuaternProd(quaTmp, q, quaVec);
    QuaternConj(quaCnj, q);
    QuaternProd(quaVec, quaTmp, quaCnj);
    sVec[0] = quaVec[1];
    sVec[1] = quaVec[2];
    sVec[2] = quaVec[3];
}
/**----------------------------------------------------------------------
* Function    : pdr_imuUpdateAhrs
* Description : 1、AHRS融合解算
*               2、Error分析
 *              3、PCA方向计算
* Date        : 2020/6/30 logzhan
*---------------------------------------------------------------------**/
int UpdateAHRS(IMU_t* imu) {
    float gyro[3], acce[3], magn[3], grav[3], mean[2];
	int   index;
	int   count;
    int i;

    for (uchar i = 0; i < 3; ++i) {
        gyro[i] = imu->gyr.s[i];
        acce[i] = imu->acc.s[i];
        magn[i] = imu->mag.s[i];
    }

	//Ahrs.status(初始状态0x80) 按位与 AHRS_STATUS_RESET_PHASE_0(0x80)
    if (Ahrs.status & AHRS_STATUS_RESET_PHASE_0) {

        // ARHS BUF 中存储了256个sensor的缓存
        if (g_ticker <= AHRS_BUF_LEN) {
            for (i = 0; i < 3; ++i) {
                BufferPush((Buffer_t *)&g_grav_buf[i], acce[i]);
                BufferPush((Buffer_t *)&g_gyro_buf[i], gyro[i]);
                BufferPush((Buffer_t *)&g_magn_buf[i], magn[i]);
            }
        }
        // 只有缓存满的时候才计算
        if (g_ticker >= AHRS_BUF_LEN) {
            //int index;
            index = (g_magn_buf[0]._top + AHRS_BUF_LEN + 1 - AHRS_BUF_LEN / 2) % (g_magn_buf[0].length + 1);
            for (i = 0; i < 3; ++i) {
                BufferMean((Buffer_t *)&g_grav_buf[i], &grav[i]);
                magn[i] = g_magn_buf[i].data[index];
                g_grav_buf[i]._top = g_grav_buf[i]._bottom;
            }

            Kp = (float)(2 * 2);
			/* 这里循环运行1000次,收敛，趋于正确姿态 */
            for (i = 0; i < 1000; ++i) {
                MahonyUpdateAHRS(0, 0, 0, grav[0], grav[1], grav[2], magn[0], magn[1], magn[2]);
            }

            AHRS_sensorFrame2EarthFrame(Ahrs.gravity, grav);

            Kp = (float)(2 * 2);
            for (index = INC_PTR(g_gyro_buf[0], index); index != g_gyro_buf[0]._top; index = INC_PTR(g_gyro_buf[0], index)) {
                for (i = 0; i < 3; ++i) {
                    BufferPush((Buffer_t *)&g_grav_buf[i], Ahrs.gravity[i]);
                    BufferMean((Buffer_t *)&g_grav_buf[i], &grav[i]);
                    gyro[i] = g_gyro_buf[i].data[index];
                    acce[i] = g_grav_buf[i].data[index];
                    magn[i] = g_magn_buf[i].data[index];
                }
                estimate_sensor_vector(grav, grav, gyro);

                // AHRS 更新
                MahonyUpdateAHRS(gyro[0], gyro[1], gyro[2], grav[0], grav[1], grav[2], magn[0],
                                 magn[1], magn[2]);

                AHRS_sensorFrame2EarthFrame(Ahrs.gravity, acce);
            }

            Ahrs.qnb[0] = q0;
            Ahrs.qnb[1] = q1;
            Ahrs.qnb[2] = q2;
            Ahrs.qnb[3] = q3;

			CLR_BIT(RESET_PHASE_0);            /*#define CLR_BIT(msk)  (Ahrs.status &= ~AHRS_STATUS_##msk)；清零g_ahrs.status*/
            SET_BIT(RESET_PHASE_1);            /*#define SET_BIT(msk)  (Ahrs.status |= AHRS_STATUS_##msk)；赋值g_ahrs.status，与phase1相同*/
        }
        ++g_ticker;
        //return;
     //Ahrs.status(0x80) 按位与 AHRS_STATUS_RESET_PHASE_1(0x40)
	}else if (Ahrs.status & AHRS_STATUS_RESET_PHASE_1) {
        //PDR_LOGD("阶段1");
        // 应该是判断是否进入PCA的条件
		if (Ahrs.error < 0.3) {

			BufferCount((Buffer_t *)&g_erro_buf, &count);
			memset((void *)mean, 0, sizeof(mean));
			for (index = g_erro_buf._bottom, i = 0; index != g_erro_buf._top; index = INC_PTR(g_erro_buf, index), ++i) {
			    int c = count >> 1;
				if (i < c ) {
					mean[0] += g_erro_buf.data[index];
				}
				else {
					mean[1] += g_erro_buf.data[index];
				}
			}
			int a = count >> 1;
			int b = count - (count >> 1);
			mean[0] /= a;
			mean[1] /= b;

			if (fabs(mean[0] - mean[1]) < 0.025) {
				Kp = (float)(2 * AHRS_KP);

				// 这个标志位决定是否进入PCA方向估计
				CLR_BIT(RESET_PHASE_1);
				SET_BIT(STABLE);
                Ahrs.stable = PDR_TRUE;
			}
		}

		for (i = 0; i < 3; ++i) {
            BufferPush((Buffer_t *)&g_grav_buf[i], Ahrs.gravity[i]);
			BufferMean((Buffer_t *)&g_grav_buf[i], &grav[i]);
		}
		estimate_sensor_vector(grav, grav, gyro);

		MahonyUpdateAHRS(gyro[0], gyro[1], gyro[2], 
                         grav[0], grav[1], grav[2], 
                         magn[0], magn[1], magn[2]);

		Ahrs.qnb[0] = q0;
		Ahrs.qnb[1] = q1;
		Ahrs.qnb[2] = q2;
		Ahrs.qnb[3] = q3;

		AHRS_sensorFrame2EarthFrame(Ahrs.gravity, acce);
		AHRS_sensorFrame2EarthFrame(Ahrs.x_axis, (float*)g_x_axis);
		AHRS_sensorFrame2EarthFrame(Ahrs.y_axis, (float*)g_y_axis);
		AHRS_sensorFrame2EarthFrame(Ahrs.z_axis, (float*)g_z_axis);

		ComputePCA(&Ahrs);

		++g_ticker;
	}
	else {

        // Buffer_mean的计算时间太长了，10ms更新来算，计算256个数的平均
        // 计算量实在太大
		//for (i = 0; i < 3; ++i) {
		//	Buffer_push((BUFFER *)&g_grav_buf[i], Ahrs.gravity[i]);
		//	Buffer_mean((BUFFER *)&g_grav_buf[i], &grav[i]);
		//}
		//estimate_sensor_vector(grav, grav, gyro);

        // logzhan 21/02/06 : 感觉是acc输入精度会高一些
		MahonyUpdateAHRS(gyro[0], gyro[1], gyro[2], 
                         acce[0], acce[1], acce[2],
                         magn[0], magn[1], magn[2]);

		Ahrs.qnb[0] = q0;
		Ahrs.qnb[1] = q1;
		Ahrs.qnb[2] = q2;
		Ahrs.qnb[3] = q3;

        // 把载体系的加速度转换为地理坐标系,
		AHRS_sensorFrame2EarthFrame(Ahrs.gravity, acce);
        // 这部分代码都是可以简化以增加运算速度的，暂时不清楚作用
		AHRS_sensorFrame2EarthFrame(Ahrs.x_axis, (float*)g_x_axis);
		AHRS_sensorFrame2EarthFrame(Ahrs.y_axis, (float*)g_y_axis);
		AHRS_sensorFrame2EarthFrame(Ahrs.z_axis, (float*)g_z_axis);

		++g_ticker;
        return PDR_TRUE;
    }
    return PDR_FALSE;
}