#pragma once
#include <ros/assert.h>
#include <eigen3/Eigen/Dense>
#include <iostream>

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

#include <ceres/ceres.h>

/**
 * @brief
 *     15: 残差向量的长度(p,v,q,ba,bg)
 *      7: 第1个优化参数的长度(para_Pose[i])
 *      9: 第2个优化参数的长度(para_SpeedBias[i])
 *      7: 第3个优化参数的长度(para_Pose[j])
 *      9: 第4个优化参数的长度(para_SpeedBias[j])
 */
class IMUFactor : public ceres::SizedCostFunction<15, 7, 9, 7, 9> {
 public:
  IMUFactor() = delete;
  IMUFactor(IntegrationBase *_pre_integration) : pre_integration(_pre_integration) {}

  virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const {
    // parameters[0], parameters[1], parameters[2], parameters[3]分别对应4个输入参数, 它们的长度依次是7,9,7,9

    Eigen::Vector3d Pi(parameters[0][0], parameters[0][1], parameters[0][2]);
    Eigen::Quaterniond Qi(parameters[0][6], parameters[0][3], parameters[0][4], parameters[0][5]);

    Eigen::Vector3d Vi(parameters[1][0], parameters[1][1], parameters[1][2]);
    Eigen::Vector3d Bai(parameters[1][3], parameters[1][4], parameters[1][5]);
    Eigen::Vector3d Bgi(parameters[1][6], parameters[1][7], parameters[1][8]);

    Eigen::Vector3d Pj(parameters[2][0], parameters[2][1], parameters[2][2]);
    Eigen::Quaterniond Qj(parameters[2][6], parameters[2][3], parameters[2][4], parameters[2][5]);

    Eigen::Vector3d Vj(parameters[3][0], parameters[3][1], parameters[3][2]);
    Eigen::Vector3d Baj(parameters[3][3], parameters[3][4], parameters[3][5]);
    Eigen::Vector3d Bgj(parameters[3][6], parameters[3][7], parameters[3][8]);

#if 0
        if ((Bai - pre_integration->linearized_ba).norm() > 0.10 ||
            (Bgi - pre_integration->linearized_bg).norm() > 0.01)
        {
            pre_integration->repropagate(Bai, Bgi);
        }
#endif

    // 在优化迭代的过程中, 预积分值是不变的, 输入的状态值会被不断的更新, 然后不断的调用evaluate()计算更新后的IMU残差
    Eigen::Map<Eigen::Matrix<double, 15, 1>> residual(residuals);
    residual = pre_integration->evaluate(Pi, Qi, Vi, Bai, Bgi, Pj, Qj, Vj, Baj, Bgj);

    Eigen::Matrix<double, 15, 15> sqrt_info =
        Eigen::LLT<Eigen::Matrix<double, 15, 15>>(pre_integration->covariance.inverse()).matrixL().transpose();
    // sqrt_info.setIdentity();

    // 这是因为真正的优化项其实是 Mahalanobis 距离: $d = r^T P^{-1} r$, P是协方差,
    // Mahalanobis距离 其实相当于一个残差加权, 协方差大的加权小, 协方差小的加权大, 着重优化那些比较确定的残差
    // 而 ceres只接受最小二乘优化, 也就是 $min e^T e$, 所以把 $P^{-1}$ 做 LLT分解, 即 $LL^T=P^{−1}$
    // 则 $d = r^T (L L^T) r = (L^T r)^T (L^T r)$
    // 令 $r' = (L^T r)$, 作为新的优化误差
    // 所以， sqrt_info 等于 $L^T$
    residual = sqrt_info * residual;  // 为了保证 IMU 和 视觉參差项在尺度上保持一致，一般会采用与量纲无关的马氏距离

    if (jacobians) {
      const double &sum_dt = pre_integration->sum_dt;

      if (pre_integration->jacobian.maxCoeff() > 1e8 || pre_integration->jacobian.minCoeff() < -1e8) {
        ROS_WARN("numerical unstable in preintegration");
      }

      if (jacobians[0]) {
        Eigen::Map<Eigen::Matrix<double, 15, 7, Eigen::RowMajor>> jacobian_pose_i(jacobians[0]);

        jacobian_pose_i.setZero();
        jacobian_pose_i.block<3, 3>(O_P, O_P) = -Qi.inverse().toRotationMatrix();
        jacobian_pose_i.block<3, 3>(O_P, O_R) =
            Utility::skewSymmetric(Qi.inverse() * (0.5 * G * sum_dt * sum_dt + Pj - Pi - Vi * sum_dt));
#if 0
                jacobian_pose_i.block<3, 3>(O_R, O_R) = -(Qj.inverse() * Qi).toRotationMatrix();
#else
        jacobian_pose_i.block<3, 3>(O_R, O_R) =
            -(Utility::Qleft(Qj.inverse() * Qi) * Utility::Qright(pre_integration->corrected_delta_q))
                 .bottomRightCorner<3, 3>();
#endif
        jacobian_pose_i.block<3, 3>(O_V, O_R) = Utility::skewSymmetric(Qi.inverse() * (G * sum_dt + Vj - Vi));

        jacobian_pose_i = sqrt_info * jacobian_pose_i;

        if (jacobian_pose_i.maxCoeff() > 1e8 || jacobian_pose_i.minCoeff() < -1e8) {
          ROS_WARN("numerical unstable in preintegration");
          // std::cout << sqrt_info << std::endl;
          // ROS_BREAK();
        }
      }
      if (jacobians[1]) {
        Eigen::Map<Eigen::Matrix<double, 15, 9, Eigen::RowMajor>> jacobian_speedbias_i(jacobians[1]);

        jacobian_speedbias_i.setZero();
        jacobian_speedbias_i.block<3, 3>(O_P, O_V - O_V) = -Qi.inverse().toRotationMatrix() * sum_dt;
        jacobian_speedbias_i.block<3, 3>(O_P, O_BA - O_V) = -pre_integration->dp_dba;
        jacobian_speedbias_i.block<3, 3>(O_P, O_BG - O_V) = -pre_integration->dp_dbg;

#if 0
                jacobian_speedbias_i.block<3, 3>(O_R, O_BG - O_V) = -dq_dbg;
#else
        jacobian_speedbias_i.block<3, 3>(O_R, O_BG - O_V) =
            -Utility::Qleft(Qj.inverse() * Qi * pre_integration->corrected_delta_q).bottomRightCorner<3, 3>() *
            pre_integration->dq_dbg;
#endif

        jacobian_speedbias_i.block<3, 3>(O_V, O_V - O_V) = -Qi.inverse().toRotationMatrix();
        jacobian_speedbias_i.block<3, 3>(O_V, O_BA - O_V) = -pre_integration->dv_dba;
        jacobian_speedbias_i.block<3, 3>(O_V, O_BG - O_V) = -pre_integration->dv_dbg;
        jacobian_speedbias_i.block<3, 3>(O_BA, O_BA - O_V) = -Eigen::Matrix3d::Identity();
        jacobian_speedbias_i.block<3, 3>(O_BG, O_BG - O_V) = -Eigen::Matrix3d::Identity();

        jacobian_speedbias_i = sqrt_info * jacobian_speedbias_i;

        // ROS_ASSERT(fabs(jacobian_speedbias_i.maxCoeff()) < 1e8);
        // ROS_ASSERT(fabs(jacobian_speedbias_i.minCoeff()) < 1e8);
      }
      if (jacobians[2]) {
        Eigen::Map<Eigen::Matrix<double, 15, 7, Eigen::RowMajor>> jacobian_pose_j(jacobians[2]);

        jacobian_pose_j.setZero();
        jacobian_pose_j.block<3, 3>(O_P, O_P) = Qi.inverse().toRotationMatrix();
#if 0
                jacobian_pose_j.block<3, 3>(O_R, O_R) = Eigen::Matrix3d::Identity();
#else
        jacobian_pose_j.block<3, 3>(O_R, O_R) =
            Utility::Qleft(pre_integration->corrected_delta_q.inverse() * Qi.inverse() * Qj).bottomRightCorner<3, 3>();
#endif
        jacobian_pose_j = sqrt_info * jacobian_pose_j;

        // ROS_ASSERT(fabs(jacobian_pose_j.maxCoeff()) < 1e8);
        // ROS_ASSERT(fabs(jacobian_pose_j.minCoeff()) < 1e8);
      }
      if (jacobians[3]) {
        Eigen::Map<Eigen::Matrix<double, 15, 9, Eigen::RowMajor>> jacobian_speedbias_j(jacobians[3]);

        jacobian_speedbias_j.setZero();
        jacobian_speedbias_j.block<3, 3>(O_V, O_V - O_V) = Qi.inverse().toRotationMatrix();
        jacobian_speedbias_j.block<3, 3>(O_BA, O_BA - O_V) = Eigen::Matrix3d::Identity();
        jacobian_speedbias_j.block<3, 3>(O_BG, O_BG - O_V) = Eigen::Matrix3d::Identity();

        jacobian_speedbias_j = sqrt_info * jacobian_speedbias_j;

        // ROS_ASSERT(fabs(jacobian_speedbias_j.maxCoeff()) < 1e8);
        // ROS_ASSERT(fabs(jacobian_speedbias_j.minCoeff()) < 1e8);
      }
    }
    return true;
  }

  // bool Evaluate_Direct(double const *const *parameters, Eigen::Matrix<double, 15, 1> &residuals,
  // Eigen::Matrix<double, 15, 30> &jacobians);

  // void checkCorrection();
  // void checkTransition();
  // void checkJacobian(double **parameters);
  IntegrationBase *pre_integration;
};
