/**
  * @project Project
  * @brief   测试算法: 对算法优化结果进行优化,由于算法数据来源存在错误概率,该算法用于评估算法数据.
  * @author  lz
  * @data    2023/6/13
 **/
#pragma once

#include "defines.hpp"
#include "define.pb.h"
#include "error_code/error_code.hpp"

#include <deque>

class TrajxtoryRegression {
public:
    /**
      * @brief   更新全部的车辆轨迹数据, 更新后会自动根据当前轨迹队列进行回归.
      * @param   deque 完整的轨迹数据, 更新后当前的轨迹数据就是这个输入的轨迹数据.
      * @return  数据回归结果
      */
    Error_manager upTrajxtoryQueue(std::deque<measure_statu> &deque) {
        m_trajxtory_deque = deque;

        return fitTrajxtory();
    }

    /**
      * @brief   添加一组的轨迹数据, 添加后会更新轨迹回归参数.
      * @param   trqjxtory 轨迹数据
      * @return  数据回归结果
      */
    Error_manager upTrajxtoryQueue(measure_statu &trqjxtory) {
        m_trajxtory_deque.pop_front();
        m_trajxtory_deque.emplace_back(trqjxtory);

        return fitTrajxtory();
    }

    /**
      * @brief   刷新轨迹数据, 再次进行回归, 默认情况下会释放一组轨迹数据
      * @param   erase 是否释放队列第一个轨迹数据
      * @return  数据回归结果
      */
    Error_manager upTrajxtoryQueue(bool erase = true) {
        if (erase) {
            m_trajxtory_deque.pop_front();
        }

        return fitTrajxtory();
    }

    /**
      * @brief   计算输入轨迹数据相对于当前回归数据的损失值.
      * @param   trqjxtory 轨迹数据
      * @return  损失值
      */
    double lossTrajxtory(measure_statu &trqjxtory) {
        // TODO
        return -1;
    }

    /**
      * @brief   获取目前存储的轨迹数据
      * @param   deque 传出轨迹数据
      * @return  NULL
      */
    Error_manager getTrajxtoryQueue(std::deque<measure_statu> &deque) {
        deque = m_trajxtory_deque;

        return Error_code::SUCCESS;
    }

    /**
      * @brief   获取轨迹回归参数
      * @param   TODO
      * @return  NULL
      */
    Error_manager getRegressionParam() {
        return Error_code::FAILED;
    }




private:
    Error_manager fitTrajxtory() {
        // TODO
        pcl::PointCloud<pcl::PointXY>::Ptr clouds;
        for (auto &tra: m_trajxtory_deque) {
            float x = tra.info().cx();
            float y = tra.info().cy();
            float theta = tra.info().theta();
            float l = tra.info().wheelbase();
            pcl::PointXY point{};
            point.x = (float)(x - l * sin((double)theta));
            point.y = (float)(y - l * cos((double)theta));
            clouds->emplace_back(point);
        }



        return Error_code::FAILED;
    }

private:
    std::deque<measure_statu> m_trajxtory_deque;

    // 回归参数 TODO
    struct TrajxtoryParam {
        pcl::PointXY location;
        float v;
        float omega;
        std::chrono::steady_clock::time_point time_stamp;

    };
};
