#include "realman_hw_test_common.hpp"

#include <array>
#include <cmath>
#include <sstream>

namespace {

double maxAbsDiff(const float *left, const float *right, int count) {
    double max_diff = 0.0;
    for (int i = 0; i < count; ++i) {
        max_diff = std::max(max_diff, std::fabs(static_cast<double>(left[i] - right[i])));
    }
    return max_diff;
}

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_joint_telemetry");
    RobotSession session(&suite);
    if (!session.connect()) {
        return suite.finish();
    }

    rm_robot_info_t info = {};
    if (!readRobotInfo(session.handle(), &info, &suite) ||
        !validateDof(static_cast<int>(info.arm_dof), &suite)) {
        return suite.finish();
    }
    const int dof = static_cast<int>(info.arm_dof);

    rm_current_arm_state_t arm_state = {};
    int result = rm_get_current_arm_state(session.handle(), &arm_state);
    suite.expect(result == 0, "rm_get_current_arm_state", "error=" + std::to_string(result));
    if (result == 0) {
        suite.expect(isFiniteArray(arm_state.joint, dof),
                     "arm_state_joints_finite",
                     formatArray(arm_state.joint, dof, "J"));
    }

    std::array<float, ARM_DOF> joint_degree = {};
    result = rm_get_joint_degree(session.handle(), joint_degree.data());
    suite.expect(result == 0, "rm_get_joint_degree", "error=" + std::to_string(result));
    if (result == 0 && isFiniteArray(arm_state.joint, dof)) {
        const double diff = maxAbsDiff(arm_state.joint, joint_degree.data(), dof);
        suite.expect(diff <= 0.1, "joint_degree_matches_arm_state", "max_diff_deg=" + std::to_string(diff));
    }

    std::array<float, ARM_DOF> current = {};
    result = rm_get_current_joint_current(session.handle(), current.data());
    suite.expect(result == 0, "rm_get_current_joint_current", "error=" + std::to_string(result));
    if (result == 0) {
        bool current_ok = true;
        for (int i = 0; i < dof; ++i) {
            current_ok = current_ok && inRange(current[static_cast<size_t>(i)], -10000.0F, 10000.0F);
        }
        suite.expect(current_ok, "joint_current_range", formatArray(current.data(), dof, "J"));
    }

    std::array<float, ARM_DOF> voltage = {};
    result = rm_get_current_joint_voltage(session.handle(), voltage.data());
    suite.expect(result == 0, "rm_get_current_joint_voltage", "error=" + std::to_string(result));
    if (result == 0) {
        bool voltage_ok = true;
        for (int i = 0; i < dof; ++i) {
            voltage_ok = voltage_ok && inRange(voltage[static_cast<size_t>(i)], 18.0F, 30.0F);
        }
        suite.expect(voltage_ok, "joint_voltage_range", formatArray(voltage.data(), dof, "J"));
    }

    std::array<float, ARM_DOF> temperature = {};
    result = rm_get_current_joint_temperature(session.handle(), temperature.data());
    suite.expect(result == 0, "rm_get_current_joint_temperature", "error=" + std::to_string(result));
    if (result == 0) {
        bool temperature_ok = true;
        for (int i = 0; i < dof; ++i) {
            temperature_ok = temperature_ok && inRange(temperature[static_cast<size_t>(i)], 0.0F, 85.0F);
        }
        suite.expect(temperature_ok, "joint_temperature_range", formatArray(temperature.data(), dof, "J"));
    }

    std::array<uint8_t, ARM_DOF> enabled = {};
    result = rm_get_joint_en_state(session.handle(), enabled.data());
    suite.expect(result == 0, "rm_get_joint_en_state", "error=" + std::to_string(result));
    if (result == 0) {
        bool all_enabled = true;
        for (int i = 0; i < dof; ++i) {
            all_enabled = all_enabled && enabled[static_cast<size_t>(i)] == 1;
        }
        suite.expect(all_enabled, "all_joints_enabled");
    }

    std::array<uint16_t, ARM_DOF> err_flag = {};
    std::array<uint16_t, ARM_DOF> brake_state = {};
    result = rm_get_joint_err_flag(session.handle(), err_flag.data(), brake_state.data());
    suite.expect(result == 0, "rm_get_joint_err_flag", "error=" + std::to_string(result));
    if (result == 0) {
        bool no_joint_error = true;
        for (int i = 0; i < dof; ++i) {
            no_joint_error = no_joint_error && err_flag[static_cast<size_t>(i)] == 0;
        }
        suite.expect(no_joint_error, "joint_error_flags_zero");
    }

    return suite.finish();
}
