#pragma once

#include "../parameters.h"
#include "../utility/utility.h"

#include <ceres/ceres.h>
using namespace Eigen;
using namespace std;

#define CHECK_J 0

class IntegrationBase {
 public:
  IntegrationBase() = delete;
  IntegrationBase(const Eigen::Vector3d &_acc_0,
                  const Eigen::Vector3d &_gyr_0,
                  const Eigen::Vector3d &_linearized_ba,
                  const Eigen::Vector3d &_linearized_bg)
      : acc_0{_acc_0},
        gyr_0{_gyr_0},
        linearized_acc{_acc_0},
        linearized_gyr{_gyr_0},
        linearized_ba{_linearized_ba},
        linearized_bg{_linearized_bg},
        jacobian{Eigen::Matrix<double, 15, 15>::Identity()},
        covariance{Eigen::Matrix<double, 15, 15>::Zero()},
        sum_dt{0.0},
        delta_p{Eigen::Vector3d::Zero()},
        delta_q{Eigen::Quaterniond::Identity()},
        delta_v{Eigen::Vector3d::Zero()}

  {
    noise = Eigen::Matrix<double, 18, 18>::Zero();
    noise.block<3, 3>(0, 0) = (ACC_N * ACC_N) * Eigen::Matrix3d::Identity();
    noise.block<3, 3>(3, 3) = (GYR_N * GYR_N) * Eigen::Matrix3d::Identity();
    noise.block<3, 3>(6, 6) = (ACC_N * ACC_N) * Eigen::Matrix3d::Identity();
    noise.block<3, 3>(9, 9) = (GYR_N * GYR_N) * Eigen::Matrix3d::Identity();
    noise.block<3, 3>(12, 12) = (ACC_W * ACC_W) * Eigen::Matrix3d::Identity();
    noise.block<3, 3>(15, 15) = (GYR_W * GYR_W) * Eigen::Matrix3d::Identity();
  }

  /**
   * @brief 存入IMU数据，并进行预积分
   *
   * @param dt  [时间间隔]
   * @param acc [加速度数据]
   * @param gyr [陀螺仪数据]
   */
  void push_back(double dt, const Eigen::Vector3d &acc, const Eigen::Vector3d &gyr) {
    dt_buf.push_back(dt);
    acc_buf.push_back(acc);
    gyr_buf.push_back(gyr);

    //! 进入预积分的阶段
    propagate(dt, acc, gyr);
  }

  /**
   * @brief
   *
   * @param _linearized_ba
   * @param _linearized_bg
   */
  void repropagate(const Eigen::Vector3d &_linearized_ba, const Eigen::Vector3d &_linearized_bg) {
    sum_dt = 0.0;
    acc_0 = linearized_acc;
    gyr_0 = linearized_gyr;
    delta_p.setZero();
    delta_q.setIdentity();
    delta_v.setZero();
    linearized_ba = _linearized_ba;
    linearized_bg = _linearized_bg;
    jacobian.setIdentity();
    covariance.setZero();

    for (int i = 0; i < static_cast<int>(dt_buf.size()); i++) propagate(dt_buf[i], acc_buf[i], gyr_buf[i]);
  }

  /**
   * @brief 预积分主程序，使用中值法在离散时间下积分
   *
   * @param _dt                  [时间段]
   * @param _acc_0               [上次加速度值，如果该预计分阶段的首次积分，则上次和本次的两个测量值是一致的]
   * @param _acc_1               [本次加速度值]
   * @param delta_p              [上次的预积分结果，初始值为0]
   * @param linearized_bg        [上次的预积分的陀螺仪bias]
   * @param result_delta_p       [本次的预积分结果]
   * @param result_linearized_ba [本次的预积分的加速度bias]
   * @param update_jacobian      [是否更新雅克比矩阵]
   */
  void midPointIntegration(double _dt,
                           const Eigen::Vector3d &_acc_0,
                           const Eigen::Vector3d &_gyr_0,
                           const Eigen::Vector3d &_acc_1,
                           const Eigen::Vector3d &_gyr_1,
                           const Eigen::Vector3d &delta_p,
                           const Eigen::Quaterniond &delta_q,
                           const Eigen::Vector3d &delta_v,
                           const Eigen::Vector3d &linearized_ba,
                           const Eigen::Vector3d &linearized_bg,
                           Eigen::Vector3d &result_delta_p,
                           Eigen::Quaterniond &result_delta_q,
                           Eigen::Vector3d &result_delta_v,
                           Eigen::Vector3d &result_linearized_ba,
                           Eigen::Vector3d &result_linearized_bg,
                           bool update_jacobian) {
    Vector3d un_gyr = 0.5 * (_gyr_0 + _gyr_1) - linearized_bg;
    result_delta_q = delta_q * Quaterniond(1, un_gyr(0) * _dt / 2, un_gyr(1) * _dt / 2, un_gyr(2) * _dt / 2);

    Vector3d un_acc_0 = delta_q * (_acc_0 - linearized_ba);
    Vector3d un_acc_1 = result_delta_q * (_acc_1 - linearized_ba);
    Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);

    result_delta_p = delta_p + delta_v * _dt + 0.5 * un_acc * _dt * _dt;
    result_delta_v = delta_v + un_acc * _dt;

    result_linearized_ba = linearized_ba;  // 预积分的过程中Bias没有发生改变
    result_linearized_bg = linearized_bg;

    // 离散状态下在计算协方差矩阵的时候为：P' = FPF' + GQG'
    // F矩阵的行顺序为：P, Q, V, Ba, Bg
    if (update_jacobian) {
      Matrix3d R_w_x = Utility::skewSymmetric(0.5 * (_gyr_0 + _gyr_1) - linearized_bg);
      Matrix3d R_a_0_x = Utility::skewSymmetric(_acc_0 - linearized_ba);
      Matrix3d R_a_1_x = Utility::skewSymmetric(_acc_1 - linearized_ba);

      MatrixXd F = MatrixXd::Zero(15, 15);
      F.block<3, 3>(0, 0) = Matrix3d::Identity();
      F.block<3, 3>(0, 3) =
          -0.25 * delta_q.toRotationMatrix() * R_a_0_x * _dt * _dt +
          -0.25 * result_delta_q.toRotationMatrix() * R_a_1_x * (Matrix3d::Identity() - R_w_x * _dt) * _dt * _dt;
      F.block<3, 3>(0, 6) = MatrixXd::Identity(3, 3) * _dt;
      F.block<3, 3>(0, 9) = -0.25 * (delta_q.toRotationMatrix() + result_delta_q.toRotationMatrix()) * _dt * _dt;
      F.block<3, 3>(0, 12) = -0.25 * result_delta_q.toRotationMatrix() * R_a_1_x * _dt * _dt * -_dt;
      F.block<3, 3>(3, 3) = Matrix3d::Identity() - R_w_x * _dt;
      F.block<3, 3>(3, 12) = -1.0 * MatrixXd::Identity(3, 3) * _dt;
      F.block<3, 3>(6, 3) =
          -0.5 * delta_q.toRotationMatrix() * R_a_0_x * _dt +
          -0.5 * result_delta_q.toRotationMatrix() * R_a_1_x * (Matrix3d::Identity() - R_w_x * _dt) * _dt;
      F.block<3, 3>(6, 6) = Matrix3d::Identity();
      F.block<3, 3>(6, 9) = -0.5 * (delta_q.toRotationMatrix() + result_delta_q.toRotationMatrix()) * _dt;
      F.block<3, 3>(6, 12) = -0.5 * result_delta_q.toRotationMatrix() * R_a_1_x * _dt * -_dt;
      F.block<3, 3>(9, 9) = Matrix3d::Identity();
      F.block<3, 3>(12, 12) = Matrix3d::Identity();

      MatrixXd V = MatrixXd::Zero(15, 18);
      V.block<3, 3>(0, 0) = 0.25 * delta_q.toRotationMatrix() * _dt * _dt;
      V.block<3, 3>(0, 3) = 0.25 * -result_delta_q.toRotationMatrix() * R_a_1_x * _dt * _dt * 0.5 * _dt;
      V.block<3, 3>(0, 6) = 0.25 * result_delta_q.toRotationMatrix() * _dt * _dt;
      V.block<3, 3>(0, 9) = V.block<3, 3>(0, 3);
      V.block<3, 3>(3, 3) = 0.5 * MatrixXd::Identity(3, 3) * _dt;
      V.block<3, 3>(3, 9) = 0.5 * MatrixXd::Identity(3, 3) * _dt;
      V.block<3, 3>(6, 0) = 0.5 * delta_q.toRotationMatrix() * _dt;
      V.block<3, 3>(6, 3) = 0.5 * -result_delta_q.toRotationMatrix() * R_a_1_x * _dt * 0.5 * _dt;
      V.block<3, 3>(6, 6) = 0.5 * result_delta_q.toRotationMatrix() * _dt;
      V.block<3, 3>(6, 9) = V.block<3, 3>(6, 3);
      V.block<3, 3>(9, 12) = MatrixXd::Identity(3, 3) * _dt;
      V.block<3, 3>(12, 15) = MatrixXd::Identity(3, 3) * _dt;

#if CHECK_J
      step_jacobian = F;
      step_V = V;
#endif

      jacobian = F * jacobian;
      covariance = F * covariance * F.transpose() + V * noise * V.transpose();
    }
  }

  /**
   * @brief IMU预积分
   *
   * @param _dt 时间间隔
   * @param _acc_1 加速度计数据
   * @param _gyr_1 陀螺仪数据
   */
  void propagate(double _dt, const Eigen::Vector3d &_acc_1, const Eigen::Vector3d &_gyr_1) {
    dt = _dt;
    acc_1 = _acc_1;
    gyr_1 = _gyr_1;

    Vector3d result_delta_p;
    Quaterniond result_delta_q;
    Vector3d result_delta_v;
    Vector3d result_linearized_ba;
    Vector3d result_linearized_bg;

    midPointIntegration(_dt, acc_0, gyr_0, _acc_1, _gyr_1, delta_p, delta_q, delta_v, linearized_ba, linearized_bg,
                        result_delta_p, result_delta_q, result_delta_v, result_linearized_ba, result_linearized_bg,
                        true);

#if CHECK_J
    checkJacobian(_dt, acc_0, gyr_0, acc_1, gyr_1, delta_p, delta_q, delta_v, linearized_ba, linearized_bg);
#endif

    delta_p = result_delta_p;
    delta_q = result_delta_q;
    delta_v = result_delta_v;
    linearized_ba = result_linearized_ba;
    linearized_bg = result_linearized_bg;
    delta_q.normalize();

    sum_dt += dt;

    acc_0 = acc_1;
    gyr_0 = gyr_1;
  }

  /**
   * @brief 计算IMU测量模型的残差
   *        Pi，Qi，Vi，Bai，Bgi  [前一次预积分结果]
   *        Pj，Qj，Vj，Baj，Bgj  [后一次预积分结果]
   * @param Pi
   * @param Qi
   * @param Vi
   * @param Bai
   * @param Bgi
   * @param Pj
   * @param Qj
   * @param Vj
   * @param Baj
   * @param Bgj
   * @return Eigen::Matrix<double, 15, 1>
   */
  Eigen::Matrix<double, 15, 1> evaluate(const Eigen::Vector3d &Pi,
                                        const Eigen::Quaterniond &Qi,
                                        const Eigen::Vector3d &Vi,
                                        const Eigen::Vector3d &Bai,
                                        const Eigen::Vector3d &Bgi,
                                        const Eigen::Vector3d &Pj,
                                        const Eigen::Quaterniond &Qj,
                                        const Eigen::Vector3d &Vj,
                                        const Eigen::Vector3d &Baj,
                                        const Eigen::Vector3d &Bgj) {
    Eigen::Matrix<double, 15, 1> residuals;

    dp_dba = jacobian.block<3, 3>(O_P, O_BA);
    dp_dbg = jacobian.block<3, 3>(O_P, O_BG);
    dq_dbg = jacobian.block<3, 3>(O_R, O_BG);
    dv_dba = jacobian.block<3, 3>(O_V, O_BA);
    dv_dbg = jacobian.block<3, 3>(O_V, O_BG);

    Eigen::Vector3d dba = Bai - linearized_ba;
    Eigen::Vector3d dbg = Bgi - linearized_bg;

    // IMU预积分的结果,消除掉acc bias和gyro bias的影响, 对应IMU model中的\hat{\alpha},\hat{\beta},\hat{\gamma}
    corrected_delta_q = delta_q * Utility::deltaQ(dq_dbg * dbg);
    corrected_delta_v = delta_v + dv_dba * dba + dv_dbg * dbg;
    corrected_delta_p = delta_p + dp_dba * dba + dp_dbg * dbg;

    // IMU项residual计算,输入参数是状态的估计值, 上面correct_delta_*是预积分值, 二者求'diff'得到residual
    residuals.block<3, 1>(O_P, 0) =
        Qi.inverse() * (0.5 * G * sum_dt * sum_dt + Pj - Pi - Vi * sum_dt) - corrected_delta_p;
    residuals.block<3, 1>(O_R, 0) = 2 * (corrected_delta_q.inverse() * (Qi.inverse() * Qj)).vec();
    residuals.block<3, 1>(O_V, 0) = Qi.inverse() * (G * sum_dt + Vj - Vi) - corrected_delta_v;
    residuals.block<3, 1>(O_BA, 0) = Baj - Bai;
    residuals.block<3, 1>(O_BG, 0) = Bgj - Bgi;

    return residuals;
  }

  void eulerIntegration(double _dt,
                        const Eigen::Vector3d &_acc_0,
                        const Eigen::Vector3d &_gyr_0,
                        const Eigen::Vector3d &_acc_1,
                        const Eigen::Vector3d &_gyr_1,
                        const Eigen::Vector3d &delta_p,
                        const Eigen::Quaterniond &delta_q,
                        const Eigen::Vector3d &delta_v,
                        const Eigen::Vector3d &linearized_ba,
                        const Eigen::Vector3d &linearized_bg,
                        Eigen::Vector3d &result_delta_p,
                        Eigen::Quaterniond &result_delta_q,
                        Eigen::Vector3d &result_delta_v,
                        Eigen::Vector3d &result_linearized_ba,
                        Eigen::Vector3d &result_linearized_bg,
                        bool update_jacobian) {
    result_delta_p = delta_p + delta_v * _dt + 0.5 * (delta_q * (_acc_1 - linearized_ba)) * _dt * _dt;
    result_delta_v = delta_v + delta_q * (_acc_1 - linearized_ba) * _dt;
    Vector3d omg = _gyr_1 - linearized_bg;
    omg = omg * _dt / 2;
    Quaterniond dR(1, omg(0), omg(1), omg(2));
    result_delta_q = (delta_q * dR);
    result_linearized_ba = linearized_ba;
    result_linearized_bg = linearized_bg;
    if (update_jacobian) {
      Vector3d w_x = _gyr_1 - linearized_bg;
      Vector3d a_x = _acc_1 - linearized_ba;
      Matrix3d R_w_x, R_a_x;
      R_w_x << 0, -w_x(2), w_x(1), w_x(2), 0, -w_x(0), -w_x(1), w_x(0), 0;
      R_a_x << 0, -a_x(2), a_x(1), a_x(2), 0, -a_x(0), -a_x(1), a_x(0), 0;
      MatrixXd A = MatrixXd::Zero(15, 15);
      // one step euler 0.5
      A.block<3, 3>(0, 3) = 0.5 * (-1 * delta_q.toRotationMatrix()) * R_a_x * _dt;
      A.block<3, 3>(0, 6) = MatrixXd::Identity(3, 3);
      A.block<3, 3>(0, 9) = 0.5 * (-1 * delta_q.toRotationMatrix()) * _dt;
      A.block<3, 3>(3, 3) = -R_w_x;
      A.block<3, 3>(3, 12) = -1 * MatrixXd::Identity(3, 3);
      A.block<3, 3>(6, 3) = (-1 * delta_q.toRotationMatrix()) * R_a_x;
      A.block<3, 3>(6, 9) = (-1 * delta_q.toRotationMatrix());
      // cout<<"A"<<endl<<A<<endl;
      MatrixXd U = MatrixXd::Zero(15, 12);
      U.block<3, 3>(0, 0) = 0.5 * delta_q.toRotationMatrix() * _dt;
      U.block<3, 3>(3, 3) = MatrixXd::Identity(3, 3);
      U.block<3, 3>(6, 0) = delta_q.toRotationMatrix();
      U.block<3, 3>(9, 6) = MatrixXd::Identity(3, 3);
      U.block<3, 3>(12, 9) = MatrixXd::Identity(3, 3);
      // put outside
      Eigen::Matrix<double, 12, 12> noise = Eigen::Matrix<double, 12, 12>::Zero();
      noise.block<3, 3>(0, 0) = (ACC_N * ACC_N) * Eigen::Matrix3d::Identity();
      noise.block<3, 3>(3, 3) = (GYR_N * GYR_N) * Eigen::Matrix3d::Identity();
      noise.block<3, 3>(6, 6) = (ACC_W * ACC_W) * Eigen::Matrix3d::Identity();
      noise.block<3, 3>(9, 9) = (GYR_W * GYR_W) * Eigen::Matrix3d::Identity();
      // write F directly
      MatrixXd F, V;
      F = (MatrixXd::Identity(15, 15) + _dt * A);
      V = _dt * U;
#if CHECK_J
      step_jacobian = F;
      step_V = V;
#endif
      jacobian = F * jacobian;
      covariance = F * covariance * F.transpose() + V * noise * V.transpose();
    }
  }

#if CHECK_J
  void checkJacobian(double _dt,
                     const Eigen::Vector3d &_acc_0,
                     const Eigen::Vector3d &_gyr_0,
                     const Eigen::Vector3d &_acc_1,
                     const Eigen::Vector3d &_gyr_1,
                     const Eigen::Vector3d &delta_p,
                     const Eigen::Quaterniond &delta_q,
                     const Eigen::Vector3d &delta_v,
                     const Eigen::Vector3d &linearized_ba,
                     const Eigen::Vector3d &linearized_bg) {
    cout << "---------------------------------------------------------------------" << endl;

    Vector3d result_delta_p;
    Quaterniond result_delta_q;
    Vector3d result_delta_v;
    Vector3d result_linearized_ba;
    Vector3d result_linearized_bg;
    midPointIntegration(_dt, _acc_0, _gyr_0, _acc_1, _gyr_1, delta_p, delta_q, delta_v, linearized_ba, linearized_bg,
                        result_delta_p, result_delta_q, result_delta_v, result_linearized_ba, result_linearized_bg, 0);
    Vector3d turb_delta_p;
    Quaterniond turb_delta_q;
    Vector3d turb_delta_v;
    Vector3d turb_linearized_ba;
    Vector3d turb_linearized_bg;
    Vector3d turb(0.0001, -0.003, 0.003);
    midPointIntegration(_dt, _acc_0, _gyr_0, _acc_1, _gyr_1, delta_p + turb, delta_q, delta_v, linearized_ba,
                        linearized_bg, turb_delta_p, turb_delta_q, turb_delta_v, turb_linearized_ba, turb_linearized_bg,
                        0);
    cout << "turb p       " << endl;
    cout << "p diff       " << (turb_delta_p - result_delta_p).transpose() << endl;
    cout << "p jacob diff " << (step_jacobian.block<3, 3>(0, 0) * turb).transpose() << endl;
    cout << "q diff       " << ((result_delta_q.inverse() * turb_delta_q).vec() * 2).transpose() << endl;
    cout << "q jacob diff " << (step_jacobian.block<3, 3>(3, 0) * turb).transpose() << endl;
    cout << "v diff       " << (turb_delta_v - result_delta_v).transpose() << endl;
    cout << "v jacob diff " << (step_jacobian.block<3, 3>(6, 0) * turb).transpose() << endl;
    cout << "ba diff      " << (turb_linearized_ba - result_linearized_ba).transpose() << endl;
    cout << "ba jacob diff" << (step_jacobian.block<3, 3>(9, 0) * turb).transpose() << endl;
    cout << "bg diff " << (turb_linearized_bg - result_linearized_bg).transpose() << endl;
    cout << "bg jacob diff " << (step_jacobian.block<3, 3>(12, 0) * turb).transpose() << endl;
    midPointIntegration(_dt, _acc_0, _gyr_0, _acc_1, _gyr_1, delta_p,
                        delta_q * Quaterniond(1, turb(0) / 2, turb(1) / 2, turb(2) / 2), delta_v, linearized_ba,
                        linearized_bg, turb_delta_p, turb_delta_q, turb_delta_v, turb_linearized_ba, turb_linearized_bg,
                        0);
    cout << "turb q       " << endl;
    cout << "p diff       " << (turb_delta_p - result_delta_p).transpose() << endl;
    cout << "p jacob diff " << (step_jacobian.block<3, 3>(0, 3) * turb).transpose() << endl;
    cout << "q diff       " << ((result_delta_q.inverse() * turb_delta_q).vec() * 2).transpose() << endl;
    cout << "q jacob diff " << (step_jacobian.block<3, 3>(3, 3) * turb).transpose() << endl;
    cout << "v diff       " << (turb_delta_v - result_delta_v).transpose() << endl;
    cout << "v jacob diff " << (step_jacobian.block<3, 3>(6, 3) * turb).transpose() << endl;
    cout << "ba diff      " << (turb_linearized_ba - result_linearized_ba).transpose() << endl;
    cout << "ba jacob diff" << (step_jacobian.block<3, 3>(9, 3) * turb).transpose() << endl;
    cout << "bg diff      " << (turb_linearized_bg - result_linearized_bg).transpose() << endl;
    cout << "bg jacob diff" << (step_jacobian.block<3, 3>(12, 3) * turb).transpose() << endl;
    midPointIntegration(_dt, _acc_0, _gyr_0, _acc_1, _gyr_1, delta_p, delta_q, delta_v + turb, linearized_ba,
                        linearized_bg, turb_delta_p, turb_delta_q, turb_delta_v, turb_linearized_ba, turb_linearized_bg,
                        0);
    cout << "turb v       " << endl;
    cout << "p diff       " << (turb_delta_p - result_delta_p).transpose() << endl;
    cout << "p jacob diff " << (step_jacobian.block<3, 3>(0, 6) * turb).transpose() << endl;
    cout << "q diff       " << ((result_delta_q.inverse() * turb_delta_q).vec() * 2).transpose() << endl;
    cout << "q jacob diff " << (step_jacobian.block<3, 3>(3, 6) * turb).transpose() << endl;
    cout << "v diff       " << (turb_delta_v - result_delta_v).transpose() << endl;
    cout << "v jacob diff " << (step_jacobian.block<3, 3>(6, 6) * turb).transpose() << endl;
    cout << "ba diff      " << (turb_linearized_ba - result_linearized_ba).transpose() << endl;
    cout << "ba jacob diff" << (step_jacobian.block<3, 3>(9, 6) * turb).transpose() << endl;
    cout << "bg diff      " << (turb_linearized_bg - result_linearized_bg).transpose() << endl;
    cout << "bg jacob diff" << (step_jacobian.block<3, 3>(12, 6) * turb).transpose() << endl;
    midPointIntegration(_dt, _acc_0, _gyr_0, _acc_1, _gyr_1, delta_p, delta_q, delta_v, linearized_ba + turb,
                        linearized_bg, turb_delta_p, turb_delta_q, turb_delta_v, turb_linearized_ba, turb_linearized_bg,
                        0);
    cout << "turb ba       " << endl;
    cout << "p diff       " << (turb_delta_p - result_delta_p).transpose() << endl;
    cout << "p jacob diff " << (step_jacobian.block<3, 3>(0, 9) * turb).transpose() << endl;
    cout << "q diff       " << ((result_delta_q.inverse() * turb_delta_q).vec() * 2).transpose() << endl;
    cout << "q jacob diff " << (step_jacobian.block<3, 3>(3, 9) * turb).transpose() << endl;
    cout << "v diff       " << (turb_delta_v - result_delta_v).transpose() << endl;
    cout << "v jacob diff " << (step_jacobian.block<3, 3>(6, 9) * turb).transpose() << endl;
    cout << "ba diff      " << (turb_linearized_ba - result_linearized_ba).transpose() << endl;
    cout << "ba jacob diff" << (step_jacobian.block<3, 3>(9, 9) * turb).transpose() << endl;
    cout << "bg diff      " << (turb_linearized_bg - result_linearized_bg).transpose() << endl;
    cout << "bg jacob diff" << (step_jacobian.block<3, 3>(12, 9) * turb).transpose() << endl;
    midPointIntegration(_dt, _acc_0, _gyr_0, _acc_1, _gyr_1, delta_p, delta_q, delta_v, linearized_ba,
                        linearized_bg + turb, turb_delta_p, turb_delta_q, turb_delta_v, turb_linearized_ba,
                        turb_linearized_bg, 0);
    cout << "turb bg       " << endl;
    cout << "p diff       " << (turb_delta_p - result_delta_p).transpose() << endl;
    cout << "p jacob diff " << (step_jacobian.block<3, 3>(0, 12) * turb).transpose() << endl;
    cout << "q diff       " << ((result_delta_q.inverse() * turb_delta_q).vec() * 2).transpose() << endl;
    cout << "q jacob diff " << (step_jacobian.block<3, 3>(3, 12) * turb).transpose() << endl;
    cout << "v diff       " << (turb_delta_v - result_delta_v).transpose() << endl;
    cout << "v jacob diff " << (step_jacobian.block<3, 3>(6, 12) * turb).transpose() << endl;
    cout << "ba diff      " << (turb_linearized_ba - result_linearized_ba).transpose() << endl;
    cout << "ba jacob diff" << (step_jacobian.block<3, 3>(9, 12) * turb).transpose() << endl;
    cout << "bg diff      " << (turb_linearized_bg - result_linearized_bg).transpose() << endl;
    cout << "bg jacob diff" << (step_jacobian.block<3, 3>(12, 12) * turb).transpose() << endl;
    midPointIntegration(_dt, _acc_0 + turb, _gyr_0, _acc_1, _gyr_1, delta_p, delta_q, delta_v, linearized_ba,
                        linearized_bg, turb_delta_p, turb_delta_q, turb_delta_v, turb_linearized_ba, turb_linearized_bg,
                        0);
    cout << "turb acc_0       " << endl;
    cout << "p diff       " << (turb_delta_p - result_delta_p).transpose() << endl;
    cout << "p jacob diff " << (step_V.block<3, 3>(0, 0) * turb).transpose() << endl;
    cout << "q diff       " << ((result_delta_q.inverse() * turb_delta_q).vec() * 2).transpose() << endl;
    cout << "q jacob diff " << (step_V.block<3, 3>(3, 0) * turb).transpose() << endl;
    cout << "v diff       " << (turb_delta_v - result_delta_v).transpose() << endl;
    cout << "v jacob diff " << (step_V.block<3, 3>(6, 0) * turb).transpose() << endl;
    cout << "ba diff      " << (turb_linearized_ba - result_linearized_ba).transpose() << endl;
    cout << "ba jacob diff" << (step_V.block<3, 3>(9, 0) * turb).transpose() << endl;
    cout << "bg diff      " << (turb_linearized_bg - result_linearized_bg).transpose() << endl;
    cout << "bg jacob diff" << (step_V.block<3, 3>(12, 0) * turb).transpose() << endl;
    midPointIntegration(_dt, _acc_0, _gyr_0 + turb, _acc_1, _gyr_1, delta_p, delta_q, delta_v, linearized_ba,
                        linearized_bg, turb_delta_p, turb_delta_q, turb_delta_v, turb_linearized_ba, turb_linearized_bg,
                        0);
    cout << "turb _gyr_0       " << endl;
    cout << "p diff       " << (turb_delta_p - result_delta_p).transpose() << endl;
    cout << "p jacob diff " << (step_V.block<3, 3>(0, 3) * turb).transpose() << endl;
    cout << "q diff       " << ((result_delta_q.inverse() * turb_delta_q).vec() * 2).transpose() << endl;
    cout << "q jacob diff " << (step_V.block<3, 3>(3, 3) * turb).transpose() << endl;
    cout << "v diff       " << (turb_delta_v - result_delta_v).transpose() << endl;
    cout << "v jacob diff " << (step_V.block<3, 3>(6, 3) * turb).transpose() << endl;
    cout << "ba diff      " << (turb_linearized_ba - result_linearized_ba).transpose() << endl;
    cout << "ba jacob diff" << (step_V.block<3, 3>(9, 3) * turb).transpose() << endl;
    cout << "bg diff      " << (turb_linearized_bg - result_linearized_bg).transpose() << endl;
    cout << "bg jacob diff" << (step_V.block<3, 3>(12, 3) * turb).transpose() << endl;
    midPointIntegration(_dt, _acc_0, _gyr_0, _acc_1 + turb, _gyr_1, delta_p, delta_q, delta_v, linearized_ba,
                        linearized_bg, turb_delta_p, turb_delta_q, turb_delta_v, turb_linearized_ba, turb_linearized_bg,
                        0);
    cout << "turb acc_1       " << endl;
    cout << "p diff       " << (turb_delta_p - result_delta_p).transpose() << endl;
    cout << "p jacob diff " << (step_V.block<3, 3>(0, 6) * turb).transpose() << endl;
    cout << "q diff       " << ((result_delta_q.inverse() * turb_delta_q).vec() * 2).transpose() << endl;
    cout << "q jacob diff " << (step_V.block<3, 3>(3, 6) * turb).transpose() << endl;
    cout << "v diff       " << (turb_delta_v - result_delta_v).transpose() << endl;
    cout << "v jacob diff " << (step_V.block<3, 3>(6, 6) * turb).transpose() << endl;
    cout << "ba diff      " << (turb_linearized_ba - result_linearized_ba).transpose() << endl;
    cout << "ba jacob diff" << (step_V.block<3, 3>(9, 6) * turb).transpose() << endl;
    cout << "bg diff      " << (turb_linearized_bg - result_linearized_bg).transpose() << endl;
    cout << "bg jacob diff" << (step_V.block<3, 3>(12, 6) * turb).transpose() << endl;
    midPointIntegration(_dt, _acc_0, _gyr_0, _acc_1, _gyr_1 + turb, delta_p, delta_q, delta_v, linearized_ba,
                        linearized_bg, turb_delta_p, turb_delta_q, turb_delta_v, turb_linearized_ba, turb_linearized_bg,
                        0);
    cout << "turb _gyr_1       " << endl;
    cout << "p diff       " << (turb_delta_p - result_delta_p).transpose() << endl;
    cout << "p jacob diff " << (step_V.block<3, 3>(0, 9) * turb).transpose() << endl;
    cout << "q diff       " << ((result_delta_q.inverse() * turb_delta_q).vec() * 2).transpose() << endl;
    cout << "q jacob diff " << (step_V.block<3, 3>(3, 9) * turb).transpose() << endl;
    cout << "v diff       " << (turb_delta_v - result_delta_v).transpose() << endl;
    cout << "v jacob diff " << (step_V.block<3, 3>(6, 9) * turb).transpose() << endl;
    cout << "ba diff      " << (turb_linearized_ba - result_linearized_ba).transpose() << endl;
    cout << "ba jacob diff" << (step_V.block<3, 3>(9, 9) * turb).transpose() << endl;
    cout << "bg diff      " << (turb_linearized_bg - result_linearized_bg).transpose() << endl;
    cout << "bg jacob diff" << (step_V.block<3, 3>(12, 9) * turb).transpose() << endl;

    cout << "---------------------------------------------------------------------" << endl;
  }
#endif

  double dt;
  Eigen::Vector3d acc_0, gyr_0;
  Eigen::Vector3d acc_1, gyr_1;

  const Eigen::Vector3d linearized_acc, linearized_gyr;
  Eigen::Vector3d linearized_ba, linearized_bg;

  Eigen::Matrix<double, 15, 15> jacobian, covariance;
  Eigen::Matrix<double, 18, 18> noise;

#if CHECK_J
  Eigen::Matrix<double, 15, 15> step_jacobian;
  Eigen::Matrix<double, 15, 18> step_V;
#endif

  double sum_dt;
  Eigen::Vector3d delta_p;
  Eigen::Vector3d delta_v;
  Eigen::Quaterniond delta_q;

  Eigen::Matrix3d dp_dba;
  Eigen::Matrix3d dp_dbg;
  Eigen::Matrix3d dq_dbg;
  Eigen::Matrix3d dv_dba;
  Eigen::Matrix3d dv_dbg;
  Eigen::Vector3d corrected_delta_v;
  Eigen::Vector3d corrected_delta_p;
  Eigen::Quaterniond corrected_delta_q;

  std::vector<double> dt_buf;
  std::vector<Eigen::Vector3d> acc_buf;
  std::vector<Eigen::Vector3d> gyr_buf;
};
