#include "realman_hw_test_common.hpp"

#include <algorithm>
#include <chrono>
#include <cmath>

namespace {

constexpr int kMotionJoint = 2; // 1-based
constexpr float kMotionDeltaDeg = 2.0F;
constexpr int kMotionSpeedPercent = 5;
constexpr int kCountdownSeconds = 3;
constexpr double kMaxFinalErrorDeg = 0.1;

void copyJoints(const float source[ARM_DOF], float target[ARM_DOF]) {
    for (int i = 0; i < ARM_DOF; ++i) {
        target[i] = source[i];
    }
}

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 moveAndAssert(rm_robot_handle *handle,
                   realman_hw_test::TestSuite *suite,
                   const char *name,
                   const float target[ARM_DOF],
                   int dof) {
    const auto start = std::chrono::steady_clock::now();
    const int result = rm_movej(handle, target, kMotionSpeedPercent, 0, 0, 1);
    const auto end = std::chrono::steady_clock::now();
    const double duration = std::chrono::duration<double>(end - start).count();

    suite->expect(result == 0, std::string(name) + "_rm_movej", "error=" + std::to_string(result));
    if (result != 0) {
        rm_set_arm_stop(handle);
        return false;
    }

    rm_current_arm_state_t actual = {};
    const int state_result = rm_get_current_arm_state(handle, &actual);
    suite->expect(state_result == 0, std::string(name) + "_read_actual", "error=" + std::to_string(state_result));
    if (state_result != 0) {
        return false;
    }

    const double error = maxAbsDiff(target, actual.joint, dof);
    suite->info(std::string(name) + "_duration_s", std::to_string(duration));
    suite->info(std::string(name) + "_final_error_deg", std::to_string(error));
    return suite->expect(error <= kMaxFinalErrorDeg,
                         std::string(name) + "_final_error_limit",
                         "error_deg=" + std::to_string(error));
}

} // namespace

int main() {
    using namespace realman_hw_test;

    TestSuite suite("realman_hw_motion_tracking");
    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);

    suite.expect(kMotionJoint >= 1 && kMotionJoint <= dof, "motion_joint_valid");
    suite.expect(kMotionDeltaDeg > 0.0F && kMotionDeltaDeg <= 3.0F,
                 "motion_delta_safe",
                 "delta_deg=" + std::to_string(kMotionDeltaDeg));
    suite.expect(kMotionSpeedPercent >= 1 && kMotionSpeedPercent <= 10,
                 "motion_speed_safe",
                 "speed_percent=" + std::to_string(kMotionSpeedPercent));

    rm_current_arm_state_t start_state = {};
    int result = rm_get_current_arm_state(session.handle(), &start_state);
    suite.expect(result == 0, "read_start_state", "error=" + std::to_string(result));
    if (result != 0) {
        return suite.finish();
    }
    suite.info("start_joints", formatArray(start_state.joint, dof, "J"));

    std::cout << "Motion test starts after countdown. Keep E-stop reachable.\n";
    for (int second = kCountdownSeconds; second > 0; --second) {
        std::cout << "Moving in " << second << "...\n";
        sleepMs(1000);
    }

    float positive[ARM_DOF] = {};
    float negative[ARM_DOF] = {};
    copyJoints(start_state.joint, positive);
    copyJoints(start_state.joint, negative);
    positive[kMotionJoint - 1] += kMotionDeltaDeg;
    negative[kMotionJoint - 1] -= kMotionDeltaDeg;

    bool ok = true;
    ok = ok && moveAndAssert(session.handle(), &suite, "move_positive", positive, dof);
    ok = ok && moveAndAssert(session.handle(), &suite, "return_positive", start_state.joint, dof);
    ok = ok && moveAndAssert(session.handle(), &suite, "move_negative", negative, dof);
    ok = ok && moveAndAssert(session.handle(), &suite, "return_negative", start_state.joint, dof);

    if (!ok) {
        rm_set_arm_stop(session.handle());
    }

    return suite.finish();
}
