//
//  imu_factor_pnp.h
//  VINS_ios
//
//  Created by 栗大人 on 2017/6/18.
//  Copyright © 2017年 栗大人. All rights reserved.
//

#ifndef imu_factor_pnp_h
#define imu_factor_pnp_h

#include <iostream>
#include <eigen3/Eigen/Dense>
#include "utility.hpp"
#include "integration_base.h"
#include "global_param.hpp"
#include <ceres/ceres.h>

using namespace Eigen;
class IMUFactorPnP : public ceres::SizedCostFunction<15, 7, 3, 6, 7, 3, 6>
{
public:
    IMUFactorPnP() = delete;
    IMUFactorPnP(IntegrationBase* _pre_integration):pre_integration(_pre_integration)
    {
    }
    virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const
    {
        
        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[2][0], parameters[2][1], parameters[2][2]);
        Eigen::Vector3d Bgi(parameters[2][3], parameters[2][4], parameters[2][5]);
        
        Eigen::Vector3d Pj(parameters[3][0], parameters[3][1], parameters[3][2]);
        Eigen::Quaterniond Qj(parameters[3][6], parameters[3][3], parameters[3][4], parameters[3][5]);
        
        Eigen::Vector3d Vj(parameters[4][0], parameters[4][1], parameters[4][2]);
        Eigen::Vector3d Baj(parameters[5][0], parameters[5][1], parameters[5][2]);
        Eigen::Vector3d Bgj(parameters[5][3], parameters[5][4], parameters[5][5]);
        
        //Eigen::Matrix<double, 15, 15> Fd;
        //Eigen::Matrix<double, 15, 12> Gd;
        
        //Eigen::Vector3d pPj = Pi + Vi * sum_t - 0.5 * g * sum_t * sum_t + corrected_delta_p;
        //Eigen::Quaterniond pQj = Qi * delta_q;
        //Eigen::Vector3d pVj = Vi - g * sum_t + corrected_delta_v;
        //Eigen::Vector3d pBaj = Bai;
        //Eigen::Vector3d pBgj = Bgi;
        
        //Vi + Qi * delta_v - g * sum_dt = Vj;
        //Qi * delta_q = Qj;
        
        //delta_p = Qi.inverse() * (0.5 * g * sum_dt * sum_dt + Pj - Pi);
        //delta_v = Qi.inverse() * (g * sum_dt + Vj - Vi);
        //delta_q = Qi.inverse() * Qj;
        
#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
        
        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();
        residual = sqrt_info * residual;
        
        if (jacobians)
        {
            double sum_dt = pre_integration->sum_dt;
            Eigen::Matrix3d dp_dba = pre_integration->jacobian.template block<3, 3>(O_P, O_BA);
            Eigen::Matrix3d dp_dbg = pre_integration->jacobian.template block<3, 3>(O_P, O_BG);
            
            Eigen::Matrix3d dq_dbg = pre_integration->jacobian.template block<3, 3>(O_R, O_BG);
            
            Eigen::Matrix3d dv_dba = pre_integration->jacobian.template block<3, 3>(O_V, O_BA);
            Eigen::Matrix3d dv_dbg = pre_integration->jacobian.template block<3, 3>(O_V, O_BG);
            
            if (pre_integration->jacobian.maxCoeff() > 1e8 || pre_integration->jacobian.minCoeff() < -1e8)
            {
                std::cout << pre_integration->jacobian << std::endl;
                ///                ROS_BREAK();
            }
            
            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();
                Vector3d G{0,0,GRAVITY};
                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
                Eigen::Quaterniond corrected_delta_q = pre_integration->delta_q * Utility::deltaQ(dq_dbg * (Bgi - pre_integration->linearized_bg));
                jacobian_pose_i.block<3, 3>(O_R, O_R) = -(Utility::Qleft(Qj.inverse() * Qi) * Utility::Qright(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)
                {
                    std::cout << sqrt_info << std::endl;
                    //assert(false);
                }
            }
            if (jacobians[1]) //speed i
            {
                Eigen::Map<Eigen::Matrix<double, 15, 3, Eigen::RowMajor>> jacobian_speed_i(jacobians[1]);
                jacobian_speed_i.setZero();
                jacobian_speed_i.block<3, 3>(O_P, O_V - O_V) = -Qi.inverse().toRotationMatrix() * sum_dt;
                
                jacobian_speed_i.block<3, 3>(O_V, O_V - O_V) = -Qi.inverse().toRotationMatrix();
                
                jacobian_speed_i = sqrt_info * jacobian_speed_i;
                
                //assert(fabs(jacobian_speedbias_i.maxCoeff()) < 1e8);
                //assert(fabs(jacobian_speedbias_i.minCoeff()) < 1e8);
            }
            if (jacobians[2]) //bias i
            {
                Eigen::Map<Eigen::Matrix<double, 15, 6, Eigen::RowMajor>> jacobian_bias_i(jacobians[2]);
                jacobian_bias_i.setZero();
                jacobian_bias_i.block<3, 3>(O_P, O_BA - O_V - O_R) = -dp_dba;
                jacobian_bias_i.block<3, 3>(O_P, O_BG - O_V - O_R) = -dp_dbg;
                
#if 0
                jacobian_bias_i.block<3, 3>(O_R, O_BG - O_V - O_R) = -dq_dbg;
#else
                Eigen::Quaterniond corrected_delta_q = pre_integration->delta_q * Utility::deltaQ(dq_dbg * (Bgi - pre_integration->linearized_bg));
                jacobian_bias_i.block<3, 3>(O_R, O_BG - O_V - O_R) = -Utility::Qleft(Qj.inverse() * Qi * corrected_delta_q).bottomRightCorner<3, 3>() * dq_dbg;
#endif
                
                jacobian_bias_i.block<3, 3>(O_V, O_BA - O_V - O_R) = -dv_dba;
                jacobian_bias_i.block<3, 3>(O_V, O_BG - O_V - O_R) = -dv_dbg;
                
                jacobian_bias_i.block<3, 3>(O_BA, O_BA - O_V - O_R) = -Eigen::Matrix3d::Identity();
                
                jacobian_bias_i.block<3, 3>(O_BG, O_BG - O_V - O_R) = -Eigen::Matrix3d::Identity();
                
                jacobian_bias_i = sqrt_info * jacobian_bias_i;
                
                //assert(fabs(jacobian_speedbias_i.maxCoeff()) < 1e8);
                //assert(fabs(jacobian_speedbias_i.minCoeff()) < 1e8);
            }
            if (jacobians[3])
            {
                Eigen::Map<Eigen::Matrix<double, 15, 7, Eigen::RowMajor>> jacobian_pose_j(jacobians[3]);
                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
                Eigen::Quaterniond corrected_delta_q = pre_integration->delta_q * Utility::deltaQ(dq_dbg * (Bgi - pre_integration->linearized_bg));
                jacobian_pose_j.block<3, 3>(O_R, O_R) = Utility::Qleft(corrected_delta_q.inverse() * Qi.inverse() * Qj).bottomRightCorner<3, 3>();
#endif
                
                jacobian_pose_j = sqrt_info * jacobian_pose_j;
                
                //assert(fabs(jacobian_pose_j.maxCoeff()) < 1e8);
                //assert(fabs(jacobian_pose_j.minCoeff()) < 1e8);
            }
            if (jacobians[4])
            {
                Eigen::Map<Eigen::Matrix<double, 15, 3, Eigen::RowMajor>> jacobian_speed_j(jacobians[4]);
                jacobian_speed_j.setZero();
                
                jacobian_speed_j.block<3, 3>(O_V, O_V - O_V) = Qi.inverse().toRotationMatrix();
                
                jacobian_speed_j = sqrt_info * jacobian_speed_j;
                
                //assert(fabs(jacobian_speedbias_j.maxCoeff()) < 1e8);
                //assert(fabs(jacobian_speedbias_j.minCoeff()) < 1e8);
            }
            if (jacobians[5])
            {
                Eigen::Map<Eigen::Matrix<double, 15, 6, Eigen::RowMajor>> jacobian_bias_j(jacobians[5]);
                jacobian_bias_j.setZero();
                
                jacobian_bias_j.block<3, 3>(O_BA, O_BA - O_V - O_R) = Eigen::Matrix3d::Identity();
                
                jacobian_bias_j.block<3, 3>(O_BG, O_BG - O_V - O_R) = Eigen::Matrix3d::Identity();
                
                jacobian_bias_j = sqrt_info * jacobian_bias_j;
                
                //assert(fabs(jacobian_speedbias_j.maxCoeff()) < 1e8);
                //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;
    
};

#endif /* imu_factor_pnp_h */
