#include "realman_hw_test_common.hpp"

#include <array>
#include <cmath>
#include <limits>

namespace {

constexpr int kAxisCount = 6;
constexpr int kSamples = 20;
constexpr int kSampleIntervalMs = 50;

struct Stats {
    int count = 0;
    double mean = 0.0;
    double min = std::numeric_limits<double>::infinity();
    double max = -std::numeric_limits<double>::infinity();

    void add(float value) {
        if (!std::isfinite(static_cast<double>(value))) {
            return;
        }
        ++count;
        mean += (static_cast<double>(value) - mean) / count;
        min = std::min(min, static_cast<double>(value));
        max = std::max(max, static_cast<double>(value));
    }

    double range() const {
        return count > 0 ? max - min : 0.0;
    }
};

std::string statsDetail(const std::array<Stats, kAxisCount> &stats) {
    const std::array<const char *, kAxisCount> names = {"Fx", "Fy", "Fz", "Mx", "My", "Mz"};
    std::ostringstream out;
    out << std::fixed << std::setprecision(4);
    for (int i = 0; i < kAxisCount; ++i) {
        out << (i == 0 ? "" : "; ") << names[static_cast<size_t>(i)]
            << "_mean=" << stats[static_cast<size_t>(i)].mean
            << ", range=" << stats[static_cast<size_t>(i)].range();
    }
    return out.str();
}

} // namespace

int main() {
    using namespace realman_hw_test;

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

    rm_robot_info_t info = {};
    if (!readRobotInfo(session.handle(), &info, &suite)) {
        return suite.finish();
    }
    suite.expect(info.force_type == RM_MODEL_RM_SF_E || info.force_type == RM_MODEL_RM_ISF_E,
                 "six_axis_force_supported",
                 std::string("actual=") + forceTypeName(info.force_type));

    std::array<Stats, kAxisCount> raw_stats;
    std::array<Stats, kAxisCount> zero_stats;
    int read_failures = 0;

    for (int sample_index = 0; sample_index < kSamples; ++sample_index) {
        rm_force_data_t data = {};
        const int result = rm_get_force_data(session.handle(), &data);
        if (result != 0) {
            ++read_failures;
        } else {
            for (int axis = 0; axis < kAxisCount; ++axis) {
                raw_stats[static_cast<size_t>(axis)].add(data.force_data[axis]);
                zero_stats[static_cast<size_t>(axis)].add(data.zero_force_data[axis]);
            }
        }
        sleepMs(kSampleIntervalMs);
    }

    suite.expect(read_failures == 0, "rm_get_force_data_samples", "failures=" + std::to_string(read_failures));
    suite.info("raw_force_stats", statsDetail(raw_stats));
    suite.info("zero_force_stats", statsDetail(zero_stats));

    bool raw_force_valid = true;
    bool zero_force_valid = true;
    for (int axis = 0; axis < kAxisCount; ++axis) {
        const bool is_force_axis = axis < 3;
        const double raw_limit = is_force_axis ? 200.0 : 8.0;
        const double zero_limit = is_force_axis ? 30.0 : 2.0;
        raw_force_valid = raw_force_valid &&
                          raw_stats[static_cast<size_t>(axis)].count == kSamples &&
                          std::fabs(raw_stats[static_cast<size_t>(axis)].mean) <= raw_limit;
        zero_force_valid = zero_force_valid &&
                           zero_stats[static_cast<size_t>(axis)].count == kSamples &&
                           std::fabs(zero_stats[static_cast<size_t>(axis)].mean) <= zero_limit;
    }

    suite.expect(raw_force_valid, "raw_force_mean_within_sensor_range");
    suite.expect(zero_force_valid, "zero_force_mean_within_static_range");

    return suite.finish();
}
