#pragma once

// For Filter
#include <cstddef>
#include "esp_timer.h"
#include "SoftFilters.h"
// #define ONE_EURO_FCMIN 1e-5f
// #define ONE_EURO_BETA 1e-7f
// #define ONE_EURO_DCUTOFF 1e-5f

// From https://github.com/rpatel3001/BleDistance/blob/master/ble_dist.h
#define ONE_EURO_FCMIN 0.0001
#define ONE_EURO_BETA 0.05
#define ONE_EURO_DCUTOFF 1.0

#define NO_RSSI (-128)
#define DEFAULT_TX (-6)

// For NimbleDistanceSensor
#include "esphome/core/component.h"
#include "esphome/components/nimble_tracker/nimble_tracker.h"

namespace esphome
{
    namespace nimble_distance_custom
    {
        class Filter
        {
        public:
            Filter(float fcmin, float beta, float dcutoff);
            bool filter(float rssi);
            Reading<Differential<float>> output;

        protected:
            OneEuroFilter<float, unsigned long> one_euro_;
            DifferentialFilter<float, unsigned long> diff_filter_;
        };

        typedef struct
        {
            NimBLEAddress address;
            float distance;
        } NimbleDistanceCustomResult;

        class NimbleDistanceCustomComponent:
                                     public Component,
                                     public nimble_tracker::NimbleDeviceListener
        {
        public:
            void setup() override;
            int get_1m_rssi(nimble_tracker::NimbleTrackerEvent *tracker_event);
            void set_max_distance(float);

        protected:
            bool update_state(nimble_tracker::NimbleTrackerEvent *tracker_event) override;
            virtual void on_result(NimbleDistanceCustomResult& result);
            Filter *filter_;
            int rssi_ = NO_RSSI, newest_ = NO_RSSI, recent_ = NO_RSSI, oldest_ = NO_RSSI;

            int8_t ref_rssi_ = -75;
            float absorption_ = 3.5f;
            float last_reported_position_ = 0;
            int64_t last_reported_micro_seconds_ = 0;

            float max_distance_ = 16.0;
        };
    } // namespace nimble_distance
} // namespace esphome