#include "battery_estimator.h"

#include <algorithm>

namespace roro {

namespace {
struct Point {
    int millivolts;
    int percent;
};
const Point kCurve[] = {
    {3270, 0},  {3610, 5},  {3690, 10}, {3710, 15}, {3730, 20}, {3750, 25}, {3770, 30},
    {3790, 35}, {3800, 40}, {3820, 45}, {3840, 50}, {3850, 55}, {3870, 60}, {3910, 65},
    {3950, 70}, {3980, 75}, {4020, 80}, {4080, 85}, {4110, 90}, {4150, 95}, {4200, 100},
};
const size_t kPoints = sizeof(kCurve) / sizeof(kCurve[0]);
}  // namespace

int BatteryEstimator::percentFromMillivolts(int mv) {
    if (mv <= kCurve[0].millivolts) return 0;
    if (mv >= kCurve[kPoints - 1].millivolts) return 100;
    for (size_t i = 1; i < kPoints; i++) {
        if (mv <= kCurve[i].millivolts) {
            const auto& lo = kCurve[i - 1];
            const auto& hi = kCurve[i];
            return lo.percent + (mv - lo.millivolts) * (hi.percent - lo.percent) /
                                    (hi.millivolts - lo.millivolts);
        }
    }
    return 100;
}

void BatteryEstimator::addSample(int mv) {
    samples_[next_] = mv;
    next_ = (next_ + 1) % kWindow;
    if (count_ < kWindow) count_++;
}

int BatteryEstimator::millivolts() const {
    if (count_ == 0) return 0;
    // Median: ignores short sags (radio transmit bursts) that would drag an average down.
    std::array<int, kWindow> sorted = samples_;
    auto middle = sorted.begin() + count_ / 2;
    std::nth_element(sorted.begin(), middle, sorted.begin() + count_);
    return *middle;
}

}  // namespace roro