/*
 * @Description: match scan with grid map
 * @Author: yct
 * @Date: 2020-04-03 13:10:37
 * @LastEditTime: 2020-05-17 23:11:23
 * @LastEditors: yct
 */

#include "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;
}

Scan_match::Scan_match(std::string filename)
{
    m_scan_subscriber = m_nh.subscribe("scan", 1, &Scan_match::scan_callback, this);
    m_initial_pose_subscriber = m_nh.subscribe("/initialpose", 1, &Scan_match::initial_pose_callback, this);
    m_cloud_publisher = m_nh.advertise<sensor_msgs::PointCloud2>("match_cloud", 10);
    m_cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
    read_pointcloud("/home/youchen/Documents/git_ws/src/map_scan_match/my1.txt", m_cloud);
    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();

}

Scan_match::~Scan_match()
{
}

#include <pcl/common/transforms.h>
bool 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, 10.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], 40.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);
}

bool 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);
    // 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]));

    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 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 Scan_match::scan_callback(const sensor_msgs::LaserScan::ConstPtr &scan_msg)
{
    if (scan_msg->ranges.size() <= 0)
        return;
    if (m_initialized)
    {
        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++)
        {
            // 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();
        Solve();
    }
}

void 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;
}