#include <iostream>
#include <fstream>
#include <sstream>
#include <vector>
#include "UWBFilter.h"
struct Point {
    float x;
    float y;
};

int main() {
    std::ifstream file("a3.txt"); // 假设数据保存在名为 data.txt 的文件中
    std::ofstream fileOut("a3_out.txt");
    std::vector<Point> points;
    UWBFilter filter;
    if (file.is_open() && fileOut.is_open()) {
        std::string line;
        // 逐行读取数据
        while (std::getline(file, line)) {
            std::stringstream ss(line);
            Point p;
            char delimiter;
            // 从每行中读取 x 和 y 值，并用逗号分隔
            if (ss >> p.x >> delimiter >> p.y) {
                float xf = p.x;
                float yf = p.y;
                float v = 0.0f;
                filter.update(xf, yf, v);
                fileOut << p.x << "," << p.y << "," << xf << "," << yf << "," << v << std::endl;
            }
        }
        file.close();
        fileOut.close();
    }
    else {
        std::cerr << "无法打开文件!" << std::endl;
        return 1;
    }

    // 打印读取到的数据
    for (const auto& point : points) {
        std::cout << "x: " << point.x << ", y: " << point.y << std::endl;
    }

    return 0;
}