/*
 * @Descripttion: extendedKalmanFilter.h Project
 * @version: Project v0.1
 * @Author: lenotary
 * @Date: 2020/11/18 下午2:59
 * @LastEditor: lenotary
 * @LastEditTime: 2020/11/18 下午2:59
 */

#ifndef SRC_EXTENDED_KALMAN_FILTER_H
#define SRC_EXTENDED_KALMAN_FILTER_H

#include <Eigen/Dense>
#include <nav_msgs/Odometry.h>
#include "common.h"
#include <queue>
#include "tf/tf.h"
struct OdomInfo {
    nav_msgs::Odometry odom;
    Eigen::VectorXd x;
};

class ExtendedKalmanFilter {
public:

    void Initialize(geometry_msgs::Pose pose, double time) {
        Eigen::MatrixXd x(3, 1);
        double angle = tf::getYaw(pose.orientation);

        x << pose.position.x, pose.position.y, angle;
        x_ = x;

        odomTimeStamp_ = time;
        Eigen::MatrixXd F(3, 3);
        F << 1, 0, 0,
                0, 1, 0,
                0, 0, 1;
        SetF(F);
        Eigen::MatrixXd FJ(3, 3);
        FJ << 1, 0, 0,
                0, 1, 0,
                0, 0, 1;
        SetFJ(FJ);

        Eigen::MatrixXd P(3, 3);
        P << 1, 0, 0,
                0, 1, 0,
                0, 0, 100;
        SetP(P);

        Eigen::MatrixXd Q(3, 3);
        Q << 1, 0, 0,
                0, 1, 0,
                0, 0, 1;
        SetQ(Q);

        Eigen::MatrixXd H(3, 3);
        H << 1, 0, 0,
                0, 1, 0,
                0, 0, 1;
        SetH(H);

        Eigen::MatrixXd R(3, 3);
        R << 1, 0, 0,
                0, 1, 0,
                0, 0, 1;
        SetR(R);
        is_initialized_ = true;

    }
    void SetOdomAngle(double angle_in){
        angle_ = angle_in;
    }
    void SetF(Eigen::MatrixXd F_in) {
        F_ = F_in;
    }

    void SetFJ(Eigen::MatrixXd FJ_in) {
        FJ_ = FJ_in;
    }

    void SetP(Eigen::MatrixXd P_in) {
        P_ = P_in;
    }

    void SetQ(Eigen::MatrixXd Q_in) {
        Q_ = Q_in;
    }

    void SetH(Eigen::MatrixXd H_in) {
        H_ = H_in;
    }

    void SetR(Eigen::MatrixXd R_in) {
        R_ = R_in;
    }

    Eigen::MatrixXd GetX() {
        return x_;
    }

    bool IsInitialized() {
        return is_initialized_;
    }

    void InputOdomMeasurement(nav_msgs::Odometry odom) {
        if (odomTimeStamp_ > 1) {
            auto dt = odom.header.stamp.toSec() - odomTimeStamp_;
            double dz = odom.twist.twist.angular.z * dt;
            dz = NormalizeAngle(dz);

            double angle = x_(2) + dz + angle_;
            double dx = odom.twist.twist.linear.x * cos(angle) * dt;
            double dy = odom.twist.twist.linear.x * sin(angle) * dt;

            Eigen::Vector3d u_(dx, dy, dz);
            Eigen::MatrixXd Fj(3, 3);
            Fj << 1, 0, -u_(1),
                    0, 1, u_(0),
                    0, 0, 1;
            SetFJ(Fj);
            Prediction(u_);
        }
        odomTimeStamp_ = odom.header.stamp.toSec();
    }

    void AddLidarMeasurement(geometry_msgs::PoseStamped pose) {
        Eigen::VectorXd z(3, 1);
        double angle = tf::getYaw(pose.pose.orientation);
        z << pose.pose.position.x, pose.pose.position.y, angle;
        zTimeStamp_ = pose.header.stamp.toSec();
        MeasurementUpdate(z);
    }

    void Prediction(Eigen::VectorXd u_) {
        x_ = F_ * x_ + u_;
        Eigen::MatrixXd Ft = FJ_.transpose();
        P_ = FJ_ * P_ * Ft + Q_;
    }

    void MeasurementUpdate(const Eigen::VectorXd &z) {
        Eigen::VectorXd y = z - H_ * x_;
        y(2) = NormalizeAngle(y(2));

//        std::cout << " H " << H_ << std::endl;
//        std::cout << " P " << P_ << std::endl;
//        std::cout << " Ht " << H_.transpose() << std::endl;
//        std::cout << " R " << R_ << std::endl;
        Eigen::MatrixXd S = H_ * P_ * H_.transpose() + R_;
        Eigen::MatrixXd K = P_ * H_.transpose() * S.inverse();
//        std::cout << " Y " << std::endl << y.transpose() << std::endl;
//        std::cout << " K " << std::endl << K << std::endl;
//        ROS_INFO("info %f %f", odomTimeStamp_, zTimeStamp_);
//        std::cout << "info "  << std::endl
//        << K << std::endl;
        x_ = x_ + (K * y);
        int size = x_.size();
        Eigen::MatrixXd I = Eigen::MatrixXd::Identity(size, size);
        P_ = (I - K * H_) * P_;
    }


private:
    bool is_initialized_{false};
    Eigen::VectorXd x_;
    Eigen::MatrixXd F_;
    Eigen::MatrixXd FJ_;
    Eigen::MatrixXd P_;
    Eigen::MatrixXd Q_;
    Eigen::MatrixXd H_;
    Eigen::MatrixXd R_;
    double angle_{0.0};
    double odomTimeStamp_;
    double zTimeStamp_;
};

#endif //SRC_EXTENDED_KALMAN_FILTER_H