/*
 * @Description: match scan with grid map
 * @Author: yct
 * @Date: 2020-04-03 13:10:37
 * @LastEditTime: 2020-10-26 15:02:52
 * @LastEditors: yct
 */

#include "ceres_scan_match.h"

bool read_pointcloud(std::string filename, pcl::PointCloud<pcl::PointXYZ>::Ptr &pointcloud)
{
    // std::lock_guard<std::mutex> lck(m_cloud_mutex);
    pointcloud->clear();
    std::ifstream in(filename);
    if (!in.is_open())
    {
        std::cout << "failed to open file " << filename << std::endl;
        return false;
    }
    while (!in.eof())
    {
        std::string t_linestr;
        if (getline(in, t_linestr))
        {
            std::vector<std::string> str_vector;
            std::stringstream ss(t_linestr);
            std::string num_str;
            while (getline(ss, num_str, ' '))
            {
                str_vector.push_back(num_str);
            }
            if (str_vector.size() != 3)
            {
                std::cout << "unsupported point cloud / cannot find point x y z value: " << filename << std::endl;
                return false;
            }
            pcl::PointXYZ t_cloud;
            t_cloud.x = stod(str_vector[0]);
            t_cloud.y = stod(str_vector[1]);
            t_cloud.z = stod(str_vector[2]);
            pointcloud->push_back(t_cloud);
        }
    }
    in.close();
    std::cout << "file read finished with points " << pointcloud->size() << std::endl;
    return true;
}

Ceres_scan_match::Ceres_scan_match(std::string filename, Eigen::Vector3d trans)
{
    // m_odom_pub = m_nh.advertise<nav_msgs::Odometry>("odom", 1, true);
    m_scan_subscriber = m_nh.subscribe("scan", 1, &Ceres_scan_match::scan_callback, this);
    m_imu_subscriber = m_nh.subscribe("/imu/data", 1, &Ceres_scan_match::imu_callback, this);
    m_initial_pose_subscriber = m_nh.subscribe("/initialpose", 1, &Ceres_scan_match::initial_pose_callback, this);
    m_cloud_publisher = m_nh.advertise<sensor_msgs::PointCloud2>("match_cloud", 10);
    m_imu_pose_publisher = m_nh.advertise<nav_msgs::Odometry>("inter_pose", 1);
    m_cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
    m_locate_timepoint = ros::Time::now();
    m_imu_vec.resize(MOVING_WINDOW_SIZE);
    m_start_index = 0;
    m_end_index = 0;
    //read_pointcloud("/home/youchen/Documents/git_ws/src/map_scan_match/my1.txt", m_cloud);
    m_trans = trans;
    // m_trans << 0.0, 0.0, 0.0;
    if (read_map(filename))
    {
        m_probability_array.rows = m_global_map.rows;
        m_probability_array.cols = m_global_map.cols;
        for (size_t i = 0; i < m_global_map.rows; i++)
        {
            for (size_t j = 0; j < m_global_map.cols; j++)
            {
                // 处理原始图片空白区域
                double value = static_cast<double>(m_global_map.at<float>(i, j));
                if(value < 0.51 && value > 0.49)
                    value = 0.9;
                m_probability_array.m_probability_vector.push_back(value);
            }  
        }
        m_initialized = true;
        std::cout << "initialized" << std::endl;
    }
    else
    {
        m_initialized = false;
    }
    m_initial_pose_updated = false;
    m_initial_pose.setIdentity();

}

Ceres_scan_match::~Ceres_scan_match()
{
}

#include <pcl/common/transforms.h>
bool Ceres_scan_match::Solve(Eigen::Vector3d &trans, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, cv::Mat global_map)
{
    // std::chrono::steady_clock::time_point t0 = std::chrono::steady_clock::now();
    // trans[0] += m_map_origin[0];
    // trans[1] += m_map_origin[1];
    // double tr[3] = {0.0, 0.0, 0.0};
    // std::cout<<"we get points: "<< cloud->size()<<std::endl;
    ceres::Problem problem;
    problem.AddResidualBlock(
        new ceres::AutoDiffCostFunction<BicubicInterpolationCost, ceres::DYNAMIC, 3>(
            new BicubicInterpolationCost(m_probability_array, cloud, m_resolution, m_map_origin, 50.0 / sqrt(static_cast<double>(cloud->size()))), cloud->size()),
        nullptr,
        trans.data());

    problem.AddResidualBlock(
        new ceres::AutoDiffCostFunction<TranslationCost, 2, 3>(
            new TranslationCost(Eigen::Vector2d(trans[0], trans[1]), 10.0)),
        nullptr,
        trans.data());

    problem.AddResidualBlock(
        new ceres::AutoDiffCostFunction<RotationCost, 1, 3>(
            new RotationCost(trans[2], 10.0)),
        nullptr,
        trans.data());

    ceres::Solver::Options options;
    options.linear_solver_type = ceres::DENSE_QR;
    options.minimizer_progress_to_stdout = false;
    options.max_num_iterations = 20;
    options.num_threads = 1;
    // options.parameter_tolerance = 1e-6;
    // options.function_tolerance = 1e-6;
    ceres::Solver::Summary summary;
    ceres::Solve(options, &problem, &summary);
    std::cout << summary.BriefReport() << std::endl;

    // std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
    // int duration = std::chrono::duration_cast<std::chrono::microseconds>(t1 - t0).count();
    // std::cout << "real solve time us: " << duration << std::endl;

    // 发布点云
    // trans[0] -= m_map_origin[0];
    // trans[1] -= m_map_origin[1];
    std::cout << "final trans: "<< trans.transpose() << std::endl;
    pcl::PointCloud<pcl::PointXYZ>::Ptr temp_cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
    Eigen::Affine3f transform_1 = Eigen::Affine3f::Identity();
    transform_1.translation() << trans[0], trans[1], 0.0;
    transform_1.rotate(Eigen::AngleAxisf(trans[2], Eigen::Vector3f::UnitZ()));
    pcl::transformPointCloud(*cloud, *temp_cloud, transform_1);
    sensor_msgs::PointCloud2 output_origin;
    pcl::toROSMsg(*temp_cloud, output_origin);
    output_origin.header.frame_id = "map";
    m_cloud_publisher.publish(output_origin);

    tf::TransformBroadcaster tf_broadcaster_;
    tf::Transform transform;
    transform.setOrigin(tf::Vector3(trans[0], trans[1], 0.0));
    transform.setRotation(tf::Quaternion(0, 0, sin(trans[2]/2.0), cos(trans[2]/2.0)));
    tf::StampedTransform transform_msg(transform, ros::Time::now(), "map", "laser");
    tf_broadcaster_.sendTransform(transform_msg);


    // // 发布里程计数据
    // int period = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::steady_clock::now() - m_tf_update_time).count();
    // m_tf_update_time = std::chrono::steady_clock::now();
    // static Eigen::Vector3d s_last_trans;
    // Eigen::Vector3d delta_trans = trans - s_last_trans;
    // s_last_trans = trans;
    // nav_msgs::Odometry t_odom;
    // t_odom.child_frame_id = "base_link";
    // t_odom.header.frame_id = "map";
    // t_odom.header.stamp = ros::Time::now();
    // t_odom.pose.pose.position.x = trans[0];
    // t_odom.pose.pose.position.y = trans[1];
    // t_odom.pose.pose.position.z = 0;
    // t_odom.pose.pose.orientation.x = 0.0;
    // t_odom.pose.pose.orientation.y = 0.0;
    // t_odom.pose.pose.orientation.z = sin(trans[2] / 2.0);
    // t_odom.pose.pose.orientation.w = cos(trans[2] / 2.0);
    // t_odom.twist.twist.linear.x = hypot(delta_trans[0], delta_trans[1]) / (period/1000.0);
    // t_odom.twist.twist.angular.z = delta_trans[2] / (period/1000.0);
    // std::cout << "odom: "<< t_odom.twist.twist.linear.x << ", " << t_odom.twist.twist.angular.z <<std::endl;
    // m_odom_pub.publish(t_odom);
}

bool Ceres_scan_match::Solve()
{
    std::cout << "--- start solve ---" << std::endl;
    std::chrono::steady_clock::time_point t0 = std::chrono::steady_clock::now();
    Eigen::Vector3d trans;
    // if (m_initial_pose_updated)
    // {
        // 设置位姿初值
        m_mutex.lock();
        trans << m_initial_pose.getOrigin().getX(), m_initial_pose.getOrigin().getY(), asin(m_initial_pose.getRotation().getZ()) * 2.0;
        std::cout << "init trans: "<< trans.transpose() << std::endl;
        m_mutex.unlock();
        m_initial_pose_updated = false;
    // }else{
        // trans << 1.5, -0.2, -0.75;
        // trans << 0.7, 0.0, 0.1;
    // }

    // 点云抽析
    pcl::ApproximateVoxelGrid<pcl::PointXYZ> approximate_voxel_filter;
    approximate_voxel_filter.setLeafSize(m_resolution, m_resolution, m_resolution);
    approximate_voxel_filter.setInputCloud(m_cloud);
    approximate_voxel_filter.filter(*m_cloud);
    std::cout<<"cloud size: "<<m_cloud->size()<<std::endl;

    // // 临时添加，测试carto地图用
    // trans << 0.0, 0.0, 0.0;
    // cv::Mat t_map = cv::imread("/home/youchen/Documents/git_ws/src/map_scan_match/my1.jpg", cv::IMREAD_GRAYSCALE);
    // t_map.convertTo(t_map, CV_32FC1, 1.0 / 255.0);m_imu_pose_publisher
    // read_pointcloud("/home/youchen/Documents/git_ws/src/map_scan_match/my1.txt", m_cloud);

    Solve(trans, m_cloud, m_global_map);
    m_initial_pose.setOrigin(tf::Vector3(trans[0], trans[1], 0));
    m_initial_pose.setRotation(tf::Quaternion(tf::Vector3(0,0,1), trans[2]));

    // 保存定位结果
    m_locate_pose.setOrigin(tf::Vector3(trans[0], trans[1], 0));
    m_locate_pose.setRotation(tf::Quaternion(tf::Vector3(0,0,1), trans[2]));

    std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
    int duration = std::chrono::duration_cast<std::chrono::microseconds>(t1 - t0).count();
    std::cout << "solve time us: " << duration << std::endl;

    // cv::Mat t_map = cv::imread("./src/map_scan_match/interpolate.jpg", cv::IMREAD_GRAYSCALE);
    // cv::namedWindow("map", cv::WINDOW_FREERATIO);
    // cv::imshow("map", t_map);
    // cv::waitKey();
}

bool Ceres_scan_match::read_map(std::string conf_filename)
{
    std::cout << "conf_file: " << conf_filename << std::endl;
    std::ifstream in(conf_filename);
    if (!in.is_open())
    {
        std::cerr << "Failed to open " << conf_filename << std::endl;
        return false;
    }
    while (!in.eof())
    {
        std::string t_line_str;
        if (getline(in, t_line_str))
        {
            std::stringstream ss(t_line_str);
            std::string t_str;
            if (getline(ss, t_str, ' '))
            {
                if (t_str == "image:")
                {
                    m_map_path = t_line_str.substr(t_str.length() + 1, t_line_str.length() - t_str.length() - 1);
                    m_global_map = cv::imread(m_map_path, cv::IMREAD_GRAYSCALE);
                    // 图像归一化
                    m_global_map.convertTo(m_global_map, CV_32FC1, 1.0 / 255.0);
                    // // 值反转
                    // m_global_map = cv::Mat::ones(m_global_map.size(), m_global_map.type()) - m_global_map;
                    
                    // std::cout<<m_global_map(cv::Range(0, 10), cv::Range(0, 10))<<std::endl;
                    // std::cout<<cv::Scalar(m_global_map.at<uchar>(20, 12))<<std::endl;
                    // std::cout<<cv::Scalar(m_global_map.at<uchar>(5, 8))<<std::endl;
                    // cv::namedWindow("origin map", cv::WINDOW_FREERATIO);
                    // cv::imshow("origin map", m_global_map);
                }
                else if (t_str == "resolution:")
                {
                    m_resolution = stod(t_line_str.substr(t_str.length() + 1, t_line_str.length() - t_str.length() - 1));
                }
                else if (t_str == "origin:")
                {
                    std::string t_origin_str = t_line_str.substr(t_str.length() + 2, t_line_str.length() - t_str.length() - 3);
                    std::stringstream ss_origin(t_origin_str);
                    std::string t_origin_value_str;
                    for (size_t i = 0; i < 3; i++)
                    {
                        if (getline(ss_origin, t_origin_value_str, ','))
                        {
                            m_map_origin[i] = stod(t_origin_value_str);
                        }
                    }
                }
            }
        }
    }
    // std::cout<<m_map_path<<std::endl;
    // std::cout<<m_resolution<<std::endl;
    // std::cout<<m_map_origin<<std::endl;
    // cv::waitKey();
    return true;
}

void Ceres_scan_match::scan_callback(const sensor_msgs::LaserScan::ConstPtr &scan_msg)
{
    if (scan_msg->ranges.size() <= 0)
        return;
    if (m_initialized)
    {
        // 有最新imu数据时,判断z轴偏差值,超过10度丢掉该帧数据
        double t_time_interval_sec = (ros::Time::now() - m_imu_vec[m_end_index].header.stamp).toSec();
        if(t_time_interval_sec < 0.2)
        {
            tf::Quaternion orient;
            orient.setX(m_imu_vec[m_end_index].orientation.x);
            orient.setY(m_imu_vec[m_end_index].orientation.y);
            orient.setZ(m_imu_vec[m_end_index].orientation.z);
            orient.setW(m_imu_vec[m_end_index].orientation.w);
            orient.normalize();
            double cos_half_angle = cos(asin(orient.getW()));
            Eigen::Vector3d axis = {orient.getX() / cos_half_angle, orient.getY() / cos_half_angle, orient.getZ() / cos_half_angle};
            if (axis.transpose() * Eigen::Vector3d(0,0,1) < cos(3.0 * M_PI / 180.0))
            {
                std::cout << "please place device horizontally!" << std::endl;
                return;
            }
        }

        // std::cout << "-------------- scan callback --------------" << std::endl;
        m_mutex.lock();
        m_cloud->clear();
        double l, angle;
        // std::cout<<"scan callback1"<<std::endl;
        for (size_t i = 0; i < scan_msg->ranges.size(); i++)
        {
            if(std::isnan(scan_msg->ranges[i])) continue;
            // std::cout<<"scan callback2"<<std::endl;
            // std::cout<<"scan callback3"<<scan_msg->intensities[0]<<std::endl;
            l = scan_msg->ranges[i];
            angle = scan_msg->angle_min + scan_msg->angle_increment * i;
            // std::cout<<"scan callback4"<<std::endl;
            m_cloud->push_back(pcl::PointXYZ(l * cos(angle), l * sin(angle), 0.0));
            // std::cout<<"scan callback5"<<std::endl;
        }
        std::cout << "cloud size: " << m_cloud->size() << std::endl;
        m_mutex.unlock();
        
        if(Solve()){
            if((ros::Time::now() - m_locate_timepoint).toSec()*1000>=TIME_INTERVAL_LIMIT_MILLI){
                std::cout << "locate timeout" << std::endl;
            }
            m_locate_timepoint = ros::Time::now();
        }
    }
}

void Ceres_scan_match::imu_callback(const sensor_msgs::Imu::ConstPtr &img_msg)
{
    static double offset_x = 0;
    static double offset_y = 0;
    // std::cout << "imu callback" << std::endl;
    if (m_imu_vec.size() != MOVING_WINDOW_SIZE)
    {
        std::cout << "vec size error." << std::endl;
    }
    nav_msgs::Odometry t_pose_to_pub;
    t_pose_to_pub.header = img_msg->header;
    t_pose_to_pub.header.frame_id = "/map";
    m_imu_vec[m_end_index] = *img_msg;
    m_end_index = (m_end_index + 1) % MOVING_WINDOW_SIZE;
    // 下标重叠,移动起始下标,相当于丢掉多余历史数据
    if(m_end_index == m_start_index)
    {
        m_start_index = (m_start_index + 1) % MOVING_WINDOW_SIZE;
    }
    // 0.5s内存在定位结果则插值,否则认为已出现定位异常,不进行插值
    if((ros::Time::now()-m_locate_timepoint).toSec() < 0.5){
        std::lock_guard<std::mutex> lck(m_mutex);
        update_imu_interpolated_pose(m_initial_pose);

        t_pose_to_pub.pose.pose.position.x = m_initial_pose.getOrigin().x();
        t_pose_to_pub.pose.pose.position.y = m_initial_pose.getOrigin().y();
        t_pose_to_pub.pose.pose.position.z = m_initial_pose.getOrigin().z();
        t_pose_to_pub.pose.pose.orientation.x = m_initial_pose.getRotation().x();
        t_pose_to_pub.pose.pose.orientation.y = m_initial_pose.getRotation().y();
        t_pose_to_pub.pose.pose.orientation.z = m_initial_pose.getRotation().z();
        t_pose_to_pub.pose.pose.orientation.w = m_initial_pose.getRotation().w();
        
        m_imu_pose_publisher.publish(t_pose_to_pub);
    }
}

void Ceres_scan_match::update_imu_interpolated_pose(tf::Pose &pose)
{
    ros::Time current = ros::Time::now();
    pose.setOrigin(m_locate_pose.getOrigin());
    pose.setRotation(m_locate_pose.getRotation());
    // 队列中数据滤波
    // 队列数据积分
    uint t_start = m_start_index, t_end = m_end_index;
    if(t_start == t_end)
    {
        std::cout << "imu vector is empty." << std::endl;
        return;
    }
    if(t_start > t_end)
    {
        t_end += MOVING_WINDOW_SIZE;
    }
    for (size_t i = t_start; i < t_end; i++)
    {
        // 早于最近定位结果的imu数据不参与计算
        uint t_curr = i % MOVING_WINDOW_SIZE;
        uint t_prev = i<1?(i+MOVING_WINDOW_SIZE-1):(i-1)% MOVING_WINDOW_SIZE;
        if ((m_imu_vec[t_curr].header.stamp - m_locate_timepoint).toSec() < 0)
        {
            continue;
        }
        // 计算时间差
        double t_time_interval_sec = 0.0;
        if (i == t_start)
        {
            t_time_interval_sec = (m_imu_vec[t_curr].header.stamp - m_locate_timepoint).toSec();
        }
        else
        {
            t_time_interval_sec = (m_imu_vec[t_curr].header.stamp - m_imu_vec[t_prev].header.stamp).toSec();
        }
        // 时间差超过0.2s数据不参与计算
        if(t_time_interval_sec > 0.2)
        {
            continue;
        }
        tf::Quaternion t_quat;
        geometry_msgs::Vector3 t_rpy = m_imu_vec[t_curr].angular_velocity;
        t_quat.setRPY(t_rpy.x * t_time_interval_sec, t_rpy.y * t_time_interval_sec, t_rpy.z * t_time_interval_sec);
        // 当前存入加速度a与角速度ang
        pose.getOrigin().setX(pose.getOrigin().x() + 0.5 * m_imu_vec[t_curr].linear_acceleration.x * std::pow(t_time_interval_sec,2));
        pose.getOrigin().setY(pose.getOrigin().y() + 0.5 * m_imu_vec[t_curr].linear_acceleration.y * std::pow(t_time_interval_sec,2));
        pose.getOrigin().setZ(pose.getOrigin().z() + 0.5 * m_imu_vec[t_curr].linear_acceleration.z * std::pow(t_time_interval_sec,2));
        pose.setRotation(pose.getRotation() * t_quat);
    }
}

void Ceres_scan_match::initial_pose_callback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &initial_pose_msg)
{
    geometry_msgs::Point position = initial_pose_msg->pose.pose.position;
    geometry_msgs::Quaternion quat = initial_pose_msg->pose.pose.orientation;
    std::lock_guard<std::mutex> lck(m_mutex);
    m_initial_pose.setOrigin(tf::Vector3(position.x, position.y, position.z));
    m_initial_pose.setRotation(tf::Quaternion(quat.x, quat.y, quat.z, quat.w));
    m_initial_pose_updated = true;
}