#include <unity.h>

#include "battery_estimator.h"

using namespace roro;

void setUp() {}
void tearDown() {}

void test_curve_end_points_and_clamping() {
    TEST_ASSERT_EQUAL(100, BatteryEstimator::percentFromMillivolts(4200));
    TEST_ASSERT_EQUAL(100, BatteryEstimator::percentFromMillivolts(4350));
    TEST_ASSERT_EQUAL(0, BatteryEstimator::percentFromMillivolts(3270));
    TEST_ASSERT_EQUAL(0, BatteryEstimator::percentFromMillivolts(2900));
}

void test_curve_is_monotonic() {
    int previous = 0;
    for (int mv = 3200; mv <= 4250; mv += 5) {
        int p = BatteryEstimator::percentFromMillivolts(mv);
        TEST_ASSERT_TRUE(p >= previous);
        previous = p;
    }
}

void test_curve_interpolates_between_points() {
    // 3840 mV = 50 %, 3850 mV = 55 %
    TEST_ASSERT_EQUAL(50, BatteryEstimator::percentFromMillivolts(3840));
    TEST_ASSERT_INT_WITHIN(1, 52, BatteryEstimator::percentFromMillivolts(3845));
}

void test_first_sample_is_reported_immediately() {
    BatteryEstimator b;
    TEST_ASSERT_FALSE(b.hasReading());
    b.addSample(4000);
    TEST_ASSERT_TRUE(b.hasReading());
    TEST_ASSERT_EQUAL(4000, b.millivolts());
}

void test_single_outlier_barely_moves_the_estimate() {
    BatteryEstimator b;
    for (int i = 0; i < 8; i++) b.addSample(3900);
    int before = b.percent();
    b.addSample(3500);  // e.g. a transmit burst sagging the voltage
    TEST_ASSERT_INT_WITHIN(6, before, b.percent());
}

void test_estimate_follows_a_sustained_change() {
    BatteryEstimator b;
    for (int i = 0; i < 8; i++) b.addSample(4100);
    for (int i = 0; i < 8; i++) b.addSample(3800);
    TEST_ASSERT_EQUAL(3800, b.millivolts());
}

int main() {
    UNITY_BEGIN();
    RUN_TEST(test_curve_end_points_and_clamping);
    RUN_TEST(test_curve_is_monotonic);
    RUN_TEST(test_curve_interpolates_between_points);
    RUN_TEST(test_first_sample_is_reported_immediately);
    RUN_TEST(test_single_outlier_barely_moves_the_estimate);
    RUN_TEST(test_estimate_follows_a_sustained_change);
    return UNITY_END();
}