#include "real_match_2d/real_match_2d.h"
#include "filter/filter.h"

namespace hlzn_slam {

template <typename T>
constexpr T Power(T base, int exponent) {
  return (exponent != 0) ? base * Power(base, exponent - 1) : T(1);
}

// Calculates a^2.
template <typename T>
constexpr T Pow2(T a) {
  return Power(a, 2);
}

RealTimeMatch2D::RealTimeMatch2D() : linear_range_(0.5), 
                                     angle_range_(0.12), 
                                     linear_step_(0.05), 
                                     angle_step_(0.017)
{
    transforms_ = 
        __generateTransfroms(linear_range_, angle_range_, linear_step_, angle_step_);
}

void RealTimeMatch2D::setSearchRange(const float linear_range, const float angle_range, const float linear_step, const float angle_step)
{
    linear_range_ = linear_range;
    angle_range_ = angle_range;
    linear_step_ = linear_step;
    angle_step_ = angle_step;

    transforms_ = 
        __generateTransfroms(linear_range, angle_range, linear_step, angle_step);
}

float RealTimeMatch2D::match(const common::PointCloud& point_cloud, 
                            const common::Rigid2f& init_pose,
                            common::Rigid2f& opt_pose, 
                            Map& map,
                            bool use_smart)
{
    std::vector<common::Rigid2f> transforms = 
        __generateTransfroms(linear_range_, angle_range_, linear_step_, angle_step_);

    if (!use_smart) {
        return __match(point_cloud, init_pose, opt_pose, transforms, map);
    }
    return __smartMatch(point_cloud, init_pose, opt_pose, transforms, map);
}

float RealTimeMatch2D::towerMatch(const common::PointCloud& point_cloud, const common::Rigid2f& init_pose, 
                                 common::Rigid2f& opt_pose, Map& map, const float multiple, bool use_smart)
{
    const float linear_range = linear_range_;
    const float angle_range = angle_range_;
    const float linear_step = linear_step_;
    const float angle_step = angle_step_ * multiple;

    common::Rigid2f mid;

    std::vector<common::Rigid2f> transforms = 
        __generateTransfroms(linear_range, angle_range, linear_step, angle_step);

    if (!use_smart) {
        __match(point_cloud, init_pose, mid, transforms, map);
    }
    else {
        __smartMatch(point_cloud, init_pose, mid, transforms, map);
    }
    transforms = 
        __generateTransfroms(linear_step, angle_step, linear_step_, angle_step_);
    
    if (!use_smart) {
        return __match(point_cloud, mid, opt_pose, transforms, map);
    }
    return __smartMatch(point_cloud, mid, opt_pose, transforms, map);
}

float RealTimeMatch2D::__match(const common::PointCloud& point_cloud, const common::Rigid2f& init_pose, 
             common::Rigid2f& opt_pose, const std::vector<common::Rigid2f>& transforms, Map& map)
{
    float maxmal_cost = 0;
    
    // 滤波
    common::PointCloud filter = point_cloud;// filter::MapVoxelInterpolationFilter(point_cloud, map, map.getResolution());

    for (const common::Rigid2f& transform : transforms) {
        common::PointCloud trans_cloud =
            __transformPointCloud(filter, init_pose * transform);

        float cost = map.getCost(trans_cloud);
        
        // 对抗长直走廊,对抗激光点云过于稀疏
        cost *= std::exp(-Pow2((transform.translation().norm() * 1e-1 + fabs(transform.rotation().angle()) * 1e-1)));
        if (cost > maxmal_cost) {
            maxmal_cost = cost;

            opt_pose = init_pose * transform;
        }
    }
    // if (maxmal_cost < 1e-2) { // 0.01
    //     return false;
    // }
    return maxmal_cost;
}

float RealTimeMatch2D::__smartMatch(const common::PointCloud& point_cloud, 
                                    const common::Rigid2f& init_pose, 
                                    common::Rigid2f& opt_pose, 
                                    const std::vector<common::Rigid2f>& transforms, 
                                    Map& map)
{
    std::vector<common::PointCloud> group;
    common::PointCloud element;

    //　分组
    for (int i = 0;i < point_cloud.data.size();i++) {
        if (!element.data.empty()) {
            float dist = hypot(element.data.back()(0) - point_cloud.data[i](0), 
                                element.data.back()(1) - point_cloud.data[i](1));
            
            if (dist > 0.2 || i == point_cloud.data.size() - 1) {
                group.push_back(element);
                element.data.clear();
            }
        }
        element.data.push_back(point_cloud.data[i]);
    }
    common::PointCloud filter = point_cloud;// filter::MapVoxelInterpolationFilter(point_cloud, map, map.getResolution());
    float maxmal_cost = 0;
    // LOG(INFO)<<"size: "<<point_cloud.data.size()<<" size: "<<group.size();
    for (const common::Rigid2f& transform : transforms) {
        float cost = 0;

        for (common::PointCloud& element : group) {
            common::PointCloud trans_cloud =
                __transformPointCloud(element, init_pose * transform);
            
            cost += map.getCost(trans_cloud);
        }
        
        // 对抗长直走廊,对抗激光点云过于稀疏
        cost *= std::exp(-Pow2((transform.translation().norm() * 1e-1 + fabs(transform.rotation().angle()) * 1e-1)));
        if (cost > maxmal_cost) {
            maxmal_cost = cost;

            opt_pose = init_pose * transform;
        }
    }
    return maxmal_cost;
}

common::PointCloud RealTimeMatch2D::__transformPointCloud(
    const common::PointCloud& point_cloud, const common::Rigid2f& transform)
{
    common::PointCloud trans_cloud;

    trans_cloud.data.resize(point_cloud.data.size());

    for (int i = 0;i < point_cloud.data.size();i++) {
        Eigen::Vector3f trans;
        common::Rigid2f::Vector vector = point_cloud.data[i].head<2>();
        
        vector = transform * vector;
        trans = Eigen::Vector3f(vector(0), vector(1), 0.0);
        trans_cloud.data[i] = trans;
    }
    trans_cloud.time = point_cloud.time;

    return trans_cloud;
}

std::vector<common::Rigid2f> RealTimeMatch2D::__generateTransfroms(const float linear_range, 
                                                                   const float angle_range,
                                                                   const float linear_step,
                                                                   const float angle_step)
{
    std::vector<common::Rigid2f> transforms;
    int linear_num = ceil(linear_range / linear_step) * 2;
    int angular_num = ceil(angle_range / angle_step) * 2;
    int size = linear_num * linear_num * angular_num;
    int i = 0;
    
    if (size < 1) {
        return transforms;
    }
    transforms.resize(size);
    for (int row = 0;row < linear_num;row++) {
        for (int col = 0;col < linear_num;col++) {
            float x = -linear_range + static_cast<float>(row * linear_step);
            float y = -linear_range + static_cast<float>(col * linear_step);
            Eigen::Matrix<float, 2, 1> translation(x, y);

            for (int a = 0;a < angular_num;a++) {
                float angle = -angle_range + static_cast<float>(a * angle_step);

                transforms[i++] = common::Rigid2f(translation, angle);
            }
        }
    }

    return transforms;
}

}