#define _CRT_SECURE_NO_WARNINGS
//#define NDEBUG

#include <deque>
#include <vector>
#include <cmath>
#include <memory>
#include <string>
#include <iostream>
#include <chrono>
#include <functional>
#include <assert.h>

#include "rclcpp/rclcpp.hpp"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/buffer.h"
#include "tf2_ros/transform_broadcaster.h"
#include "geometry_msgs/msg/transform_stamped.hpp"

using namespace std::chrono_literals;


std::vector<double> butterworth_filter(const std::vector<double>& input, double cutoff_frequency, double sampling_rate, int order)
{
    int N = input.size();
    std::vector<double> output(N, 0.0);

    // cf, init
    double nyquist = 0.5 * sampling_rate;
    double normalized_cutoff = cutoff_frequency / nyquist;

    std::vector<double> a(order + 1, 0.0);
    std::vector<double> b(order + 1, 0.0);

    double theta = M_PI * normalized_cutoff;
    double d = 1.0 / std::tan(theta);
    double d_squared = d * d;

    a[0] = 1.0 / (1.0 + std::sqrt(2.0) * d + d_squared);
    a[1] = 2.0 * a[0];
    a[2] = a[0];
    b[1] = 2.0 * (1.0 - d_squared) * a[0];
    b[2] = (1.0 - std::sqrt(2.0) * d + d_squared) * a[0];

    for (int i = 2; i < N; ++i)
    {
#ifdef NDEBUG
#else
        printf("\nFiltering index: %i\n",i);
#endif
        output[i] = a[0] * input[i] + a[1] * input[i - 1] + a[2] * input[i - 2] - b[1] * output[i - 1] - b[2] * output[i - 2];
    }

    return output;
}

class Filter
{
public:
    Filter() {}

    void set_history_size(size_t new_size)
    {
        history_size_ = new_size;
        if (x_history_.size() > history_size_) x_history_.resize(history_size_);
        if (y_history_.size() > history_size_) y_history_.resize(history_size_);
        if (z_history_.size() > history_size_) z_history_.resize(history_size_);
    }

    geometry_msgs::msg::TransformStamped apply_filter(const geometry_msgs::msg::TransformStamped& transform)
    {
        x_history_.push_back(transform.transform.translation.x);
        y_history_.push_back(transform.transform.translation.y);
        z_history_.push_back(transform.transform.translation.z);

        if (x_history_.size() > history_size_) x_history_.pop_front();
        if (y_history_.size() > history_size_) y_history_.pop_front();
        if (z_history_.size() > history_size_) z_history_.pop_front();

#ifdef NDEBUG
#else
        printf("\nHistory Size: %zu", history_size_);
        printf("\nDeque x_history_ size: %zu", x_history_.size());
        printf("\nDeque y_history_ size: %zu", y_history_.size());
        printf("\nDeque z_history_ size: %zu", z_history_.size());
#endif
        geometry_msgs::msg::TransformStamped filtered_transform = transform;

        if (x_history_.size() >= history_size_)
        {
            std::vector<double> x_filtered = butterworth_filter(
                std::vector<double>(x_history_.begin(), x_history_.end()), cutoff_frequency_, sampling_rate_, order_);
            std::vector<double> y_filtered = butterworth_filter(
                std::vector<double>(y_history_.begin(), y_history_.end()), cutoff_frequency_, sampling_rate_, order_);
            std::vector<double> z_filtered = butterworth_filter(
                std::vector<double>(z_history_.begin(), z_history_.end()), cutoff_frequency_, sampling_rate_, order_);

            filtered_transform.transform.translation.x = x_filtered.back();
            filtered_transform.transform.translation.y = y_filtered.back();
            filtered_transform.transform.translation.z = z_filtered.back();
        }

        return filtered_transform;
    }

private:
    std::deque<double> x_history_;
    std::deque<double> y_history_;
    std::deque<double> z_history_;
    size_t history_size_ = 20;
    double cutoff_frequency_ = 10.0;
    double sampling_rate_ = 100.0;
    int order_ = 2;
};


class FrameListener : public rclcpp::Node
{
public:
    FrameListener() : Node("frame_listener_node"), buffer_(get_clock()), listener_(buffer_), filter_(std::make_shared<Filter>())
    {
        timer_ = this->create_wall_timer(50ms, std::bind(&FrameListener::timer_callback, this));
        publisher_ = this->create_publisher<geometry_msgs::msg::TransformStamped>("LHR_BFB7B3C7_filtered", 10);
    }

private:
    void timer_callback()
    {
        geometry_msgs::msg::TransformStamped transformStamped;
        try
        {
            transformStamped = buffer_.lookupTransform("libsurvive_world", "LHR-BFB7B3C7", tf2::TimePointZero);
            RCLCPP_INFO(this->get_logger(), "Received transform (unfiltered): "
                "Translation: [%f, %f, %f], "
                "Rotation: [x: %f, y: %f, z: %f, w: %f]",
                transformStamped.transform.translation.x,
                transformStamped.transform.translation.y,
                transformStamped.transform.translation.z,
                transformStamped.transform.rotation.x,
                transformStamped.transform.rotation.y,
                transformStamped.transform.rotation.z,
                transformStamped.transform.rotation.w);

            geometry_msgs::msg::TransformStamped filtered_transform = filter_->apply_filter(transformStamped);

            publisher_->publish(filtered_transform);

#ifdef NDEBUG
#else
            RCLCPP_INFO(this->get_logger(), "Published unfiltered transform.");
#endif
        }
        catch (tf2::TransformException& ex)
        {
            RCLCPP_WARN(this->get_logger(), "Could not transform 'LHR-BFB7B3C7' to 'libsurvive_world': %s", ex.what());
        }
    }

    tf2_ros::Buffer buffer_;
    tf2_ros::TransformListener listener_;
    rclcpp::Publisher<geometry_msgs::msg::TransformStamped>::SharedPtr publisher_;
    rclcpp::TimerBase::SharedPtr timer_;
    std::shared_ptr<Filter> filter_;
};


class FramePublisher : public rclcpp::Node
{
public:
    FramePublisher() : Node("frame_publisher_node"), buffer_(get_clock()), listener_(buffer_)
    {
        timer_ = this->create_wall_timer(50ms, std::bind(&FramePublisher::timer_callback, this));
        broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(this);
    }

private:
    void timer_callback()
    {
        geometry_msgs::msg::TransformStamped transformStamped;
        try
        {
            transformStamped = buffer_.lookupTransform("libsurvive_world", "LHR-BFB7B3C7", tf2::TimePointZero);
            RCLCPP_INFO(this->get_logger(), "Publishing transform (filtered): "
                "Translation: [%f, %f, %f], "
                "Rotation: [x: %f, y: %f, z: %f, w: %f]",
                transformStamped.transform.translation.x,
                transformStamped.transform.translation.y,
                transformStamped.transform.translation.z,
                transformStamped.transform.rotation.x,
                transformStamped.transform.rotation.y,
                transformStamped.transform.rotation.z,
                transformStamped.transform.rotation.w);

            transformStamped.child_frame_id = "LHR-BFB7B3C7_filtered";

            broadcaster_->sendTransform(transformStamped);

#ifdef NDEBUG
#else
            RCLCPP_INFO(this->get_logger(), "Published filtered transform.");
#endif

        }
        catch (tf2::TransformException& ex)
        {
            RCLCPP_WARN(this->get_logger(), "Could not transform 'LHR-BFB7B3C7' to 'libsurvive_world': %s", ex.what());
        }
    }

    tf2_ros::Buffer buffer_;
    tf2_ros::TransformListener listener_;
    std::shared_ptr<tf2_ros::TransformBroadcaster> broadcaster_;
    rclcpp::TimerBase::SharedPtr timer_;
};


int main(int argc, char* argv[])
{
    rclcpp::init(argc, argv);

#ifdef NDEBUG
#else
    printf("\nROS run"); 
#endif

    auto frame_listener_node = std::make_shared<FrameListener>();
    auto frame_publisher_node = std::make_shared<FramePublisher>();

    rclcpp::executors::SingleThreadedExecutor executor;
    executor.add_node(frame_listener_node);
    executor.add_node(frame_publisher_node);
    executor.spin();

    rclcpp::shutdown();
    return 0;
}