#include "realman_hw_test_common.hpp"

#include <cmath>

namespace {

bool inRange(float value, float min_value, float max_value) {
    return std::isfinite(static_cast<double>(value)) && value >= min_value && value <= max_value;
}

} // namespace

int main() {
    using namespace realman_hw_test;

    TestSuite suite("realman_hw_controller_state");
    RobotSession session(&suite);
    if (!session.connect()) {
        return suite.finish();
    }

    constexpr int kSamples = 10;
    int failures = 0;
    double voltage_sum = 0.0;
    double current_sum = 0.0;
    double temperature_sum = 0.0;
    int err_nonzero_count = 0;

    for (int i = 0; i < kSamples; ++i) {
        float voltage = 0.0F;
        float current = 0.0F;
        float temperature = 0.0F;
        int err_flag = -1;
        const int result = rm_get_controller_state(session.handle(), &voltage, &current, &temperature, &err_flag);
        if (result != 0) {
            ++failures;
        } else {
            voltage_sum += voltage;
            current_sum += current;
            temperature_sum += temperature;
            if (err_flag != 0) {
                ++err_nonzero_count;
            }
        }
        sleepMs(50);
    }

    const int valid_samples = kSamples - failures;
    suite.expect(failures == 0, "rm_get_controller_state_samples", "failures=" + std::to_string(failures));
    if (valid_samples > 0) {
        const float voltage_mean = static_cast<float>(voltage_sum / valid_samples);
        const float current_mean = static_cast<float>(current_sum / valid_samples);
        const float temperature_mean = static_cast<float>(temperature_sum / valid_samples);
        suite.info("controller_voltage_mean_V", std::to_string(voltage_mean));
        suite.info("controller_current_mean_A", std::to_string(current_mean));
        suite.info("controller_temperature_mean_C", std::to_string(temperature_mean));
        suite.expect(inRange(voltage_mean, 18.0F, 30.0F), "controller_voltage_range");
        suite.expect(inRange(current_mean, 0.0F, 10.0F), "controller_current_range");
        suite.expect(inRange(temperature_mean, 0.0F, 85.0F), "controller_temperature_range");
        suite.expect(err_nonzero_count == 0, "controller_error_flag_zero", "nonzero=" + std::to_string(err_nonzero_count));
    }

    return suite.finish();
}
