#include "nimble_distance_custom.h"

namespace esphome
{
    namespace nimble_distance_custom
    {
        static const char *const TAG = "nimble_distance_custom";

        static int median_of_3(int a, int b, int c)
        {
            int the_max = std::max(std::max(a, b), c);
            int the_min = std::min(std::min(a, b), c);
            // unnecessarily clever code
            int the_median = the_max ^ the_min ^ a ^ b ^ c;
            return (the_median);
        }
        int NimbleDistanceCustomComponent::get_1m_rssi(nimble_tracker::NimbleTrackerEvent *tracker_event)
        {
            return this->ref_rssi_; //+ tracker_event->getTXPower() + 99;
        }

        Filter::Filter(float fcmin, float beta, float dcutoff) : one_euro_{OneEuroFilter<float, unsigned long>(1, fcmin, beta, dcutoff)}
        {
        }

        bool Filter::filter(float rssi)
        {
            Reading<float, unsigned long> inter1, inter2;
            // TODO: should we take into consideration micro seconds (returned from esp_timer_get_time())
            // vs mili seconds (implementation used in ESPresence?)
            inter1.timestamp = esp_timer_get_time();
            inter1.value = rssi;

            return this->one_euro_.push(&inter1, &inter2) && this->diff_filter_.push(&inter2, &this->output);
        }

        void NimbleDistanceCustomComponent::setup()
        {
            this->filter_ = new Filter(ONE_EURO_FCMIN, ONE_EURO_BETA, ONE_EURO_DCUTOFF);
        }


        void NimbleDistanceCustomComponent::set_max_distance(float max_distance) {
            this->max_distance_ = max_distance;
        }

        // Defined distance formula using
        // https://medium.com/beingcoders/convert-rssi-value-of-the-ble-bluetooth-low-energy-beacons-to-meters-63259f307283
        // and copied a lot of code from
        // https://github.com/ESPresense/ESPresense/blob/master/lib/BleFingerprint/BleFingerprint.cpp
        bool NimbleDistanceCustomComponent::update_state(nimble_tracker::NimbleTrackerEvent *tracker_event)
        {
            this->oldest_ = this->recent_;
            this->recent_ = this->newest_;
            this->newest_ = tracker_event->getRSSI();
            this->rssi_ = median_of_3(this->oldest_, this->recent_, this->newest_);

            float ratio = (this->get_1m_rssi(tracker_event) - this->rssi_) / (10.0f * this->absorption_);
            float raw = std::pow(10, ratio);

            if (!this->filter_->filter(raw))
            {
                ESP_LOGD(TAG, "Not enough data to calculate distance.");
                return false;
            }

            if (this->max_distance_ > 0 && this->filter_->output.value.position > this->max_distance_)
                return false;

            auto skip_distance = 0.5f;
            auto skip_ms = 5000;
            auto skip_micro_seconds = skip_ms * 1000;
            auto now = esp_timer_get_time();

            if ((abs(this->filter_->output.value.position - this->last_reported_position_) < skip_distance) && (this->last_reported_micro_seconds_ > 0) && ((now - this->last_reported_micro_seconds_) < skip_micro_seconds))
            {
                return false;
            }

            this->last_reported_micro_seconds_ = now;
            this->last_reported_position_ = this->filter_->output.value.position;
            // /this->publish_state(this->filter_->output.value.position);

            auto res = NimbleDistanceCustomResult{
                tracker_event->getAddress(),
                this->filter_->output.value.position
            };

            this->on_result(res);

            return true;
        }
    } // namespace nimble_distance

} // namespace esphome