Measure/include/tool/measure_filter.h

/*
 * @Description: 测量结果滤波器
 * @Author: yct
 * @Date: 2021-03-15 14:41:46
 * @LastEditTime: 2022-01-18 13:38:27
 * @LastEditors: yct
 */

#ifndef MEASURE_FILTER_HH
#define MEASURE_FILTER_HH
#include "tool/singleton.h"
#include "tool/common_data.h"
#include "error_code/error_code.hpp"
#include <map>
#include <deque>
#include <thread>
#include <mutex>
#include <chrono>
#include <vector>
#include <algorithm>
#include "glog/logging.h"

#define FILTER_SIZE 10
#define MAX_QUEUE_SIZE 20
#define MAX_TIME_INTERVAL_MILLI 5000

class Measure_filter : public Singleton<Measure_filter>
{
    // 子类必须把父类设定为友元函数，这样父类才能使用子类的私有构造函数。
    friend class Singleton<Measure_filter>;

public:
    // 必须关闭拷贝构造和赋值构造，只能通过 get_instance 函数来进行操作唯一的实例。
    Measure_filter(const Measure_filter &) = delete;
    Measure_filter &operator=(const Measure_filter &) = delete;
    ~Measure_filter()
    {
        mb_exit = true;
        if (mp_work_thread != nullptr)
        {
            if(mp_work_thread->joinable())
            {
                mp_work_thread->join();
            }
            delete mp_work_thread;
            mp_work_thread = nullptr;
        }
    }

    void work_thread_func()
    {
        while(!mb_exit)
        {
            {
                std::lock_guard<std::mutex> lck(m_mutex);
                for (auto iter = m_measure_results_map.begin(); iter != m_measure_results_map.end(); iter++)
                {
                    std::deque<Common_data::Car_wheel_information_stamped> *t_queue = &(iter->second);
                    // 剔除超时数据
                    for (size_t i = 0; i < t_queue->size(); i++)
                    {
                        double dtime = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now() - t_queue->front().measure_time).count();
                        if(dtime > MAX_TIME_INTERVAL_MILLI)
                        {
                            t_queue->pop_front();
                        }
                    }
                    
                    // 维持队列长度
                    while(t_queue->size() > MAX_QUEUE_SIZE)
                    {
                        t_queue->pop_front();
                    }
                }
            }
            usleep(1000 * 10);
        }
    }

    // 更新测量数据回调
    void update_data(int terminal_id, Common_data::Car_wheel_information_stamped data)
    {
        if(!data.wheel_data.correctness)
            return;
//        LOG(INFO) << data.wheel_data.to_string();
        // 未创建队列
        {
            std::lock_guard<std::mutex> lck(m_mutex);
            if (m_measure_results_map.find(terminal_id) == m_measure_results_map.end())
            {
                std::deque<Common_data::Car_wheel_information_stamped> t_deque;
                t_deque.push_back(data);
                m_measure_results_map.insert(std::pair<int, std::deque<Common_data::Car_wheel_information_stamped>>(terminal_id, t_deque));
            }
            else //已创建
            {
                m_measure_results_map[terminal_id].push_back(data);
                // 队列管理放在线程中自动控制
            }
        }
    }

    // 获取滤波后最新测量数据
    Error_manager get_filtered_wheel_information(int terminal_id, Common_data::Car_wheel_information& result)
    {
        std::lock_guard<std::mutex> lck(m_mutex);
        // 检查数据量
        if(m_measure_results_map.find(terminal_id) == m_measure_results_map.end() || m_measure_results_map.find(terminal_id)->second.size() < FILTER_SIZE)
        {
            return Error_manager(WJ_FILTER_LACK_OF_RESULT, MINOR_ERROR,
                    (std::string("缺少足够用于滤波的结果")+
                    std::to_string(terminal_id)+
                    std::string(m_measure_results_map.find(terminal_id) == m_measure_results_map.end()?
                        "end":
                        std::to_string(m_measure_results_map.find(terminal_id)->second.size()))).c_str());
        }
        // 填充待滤波结果到数组
        std::deque<Common_data::Car_wheel_information_stamped>* tp_deque = &m_measure_results_map.find(terminal_id)->second;
        std::vector<Common_data::Car_wheel_information_stamped> t_result_vec;
        Common_data::Car_wheel_information_stamped t_avg_result;
        for (std::deque<Common_data::Car_wheel_information_stamped>::reverse_iterator t_iter = tp_deque->rbegin(); t_iter != tp_deque->rend(); t_iter++)
        {
            t_result_vec.push_back(*t_iter);
            t_avg_result += *t_iter;
            if (t_result_vec.size() >= FILTER_SIZE)
                break;
        }
        if(t_result_vec.size()<FILTER_SIZE)
            return Error_manager(WJ_FILTER_LACK_OF_RESULT, MINOR_ERROR, "结果缺失");
        // 各结果计算平均,并打分
        t_avg_result /= t_result_vec.size();
        std::vector<float> t_score_vec, t_sorted_score_vec;
        for (size_t i = 0; i < t_result_vec.size(); i++)
        {
            t_score_vec.push_back((t_result_vec[i] - t_avg_result).calc_score());
            t_sorted_score_vec.push_back((t_result_vec[i] - t_avg_result).calc_score());
        }
        // 排序,丢掉最高两个(即与均值偏差最大两个,剩下平均获得最终值)
        std::sort(t_sorted_score_vec.begin(), t_sorted_score_vec.end());
        Common_data::Car_wheel_information_stamped t_final_result;
        int t_result_count = 0;
        for (size_t i = 0; i < t_score_vec.size(); i++)
        {
            if(t_score_vec[i] > t_sorted_score_vec[t_sorted_score_vec.size()-3])
            {
                continue;
            }else{
                t_final_result += t_result_vec[i];
                t_result_count++;
            }
        }
        if(t_result_count<=0)
            return Error_manager(WJ_FILTER_FLUCTUATING, MINOR_ERROR, "结果波动");
        t_final_result /= t_result_count;
        result = t_final_result.wheel_data;

    //    LOG(INFO) << "\navg: \n\t"<<t_avg_result.wheel_data.to_string()<<"final: \t"<<t_final_result.wheel_data.to_string();
        return SUCCESS;
    }

private:
    // 父类的构造函数必须保护，子类的构造函数必须私有。
    Measure_filter()
    {
        mb_exit = false;
        mp_work_thread = new std::thread(&Measure_filter::work_thread_func, this);
    }

    // 各终端测量数据队列
    std::map<int, std::deque<Common_data::Car_wheel_information_stamped> > m_measure_results_map;

    std::mutex m_mutex;

    std::thread *mp_work_thread;

    bool mb_exit;
};

#endif // !MEASURE_FILTER_HH