#include "rm_interface.h"

#include <array>
#include <chrono>
#include <cmath>
#include <cstdarg>
#include <cstdio>
#include <iomanip>
#include <iostream>
#include <thread>

namespace {

// ===== 用户主要修改区：在 CLion 里改这里，然后重新 Build/Run =====
constexpr const char *kRobotIp = "192.168.1.18";
constexpr int kRobotPort = 8080;
constexpr int kTimeoutMs = 1000;

enum class TargetMode {
    kRelativeJointOffset, // 基于当前关节角做偏移，更适合调试
    kAbsoluteJointAngles, // 直接移动到指定绝对关节角
};

constexpr TargetMode kTargetMode = TargetMode::kRelativeJointOffset;

// 7 自由度关节角，单位：度。RM75 使用 J1..J7。
// 相对模式：目标 = 当前关节角 + kRelativeJointOffsetsDeg。
constexpr std::array<float, ARM_DOF> kRelativeJointOffsetsDeg = {
    0.0F, 5.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F,
};

// 绝对模式：目标 = kAbsoluteTargetJointsDeg。
// 首次使用绝对模式前，建议先把程序打印的 Current joints 复制到这里，再小幅修改。
constexpr std::array<float, ARM_DOF> kAbsoluteTargetJointsDeg = {
    0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F,
};

constexpr int kSpeedPercent = 10;
constexpr bool kReturnToStartAfterMove = false;
constexpr bool kDryRunOnly = false;

// 安全限幅。需要更大动作时，先在低速、小幅验证后再调大。
constexpr float kMaxSingleJointDeltaDeg = 10.0F;
constexpr int kMaxAllowedSpeedPercent = 20;
constexpr int kSafetyCountdownSeconds = 5;
// ===== 用户主要修改区结束 =====

void apiLogCallback(const char *message, va_list args) {
    if (message == nullptr) {
        return;
    }

    char buffer[1024] = {};
    std::vsnprintf(buffer, sizeof(buffer), message, args);
    std::cerr << "[rm_api] " << buffer << '\n';
}

int destroyApi() {
#if defined(REALMAN_RM_API_HAS_RM_DESTROY)
    return rm_destroy();
#else
    return rm_destory();
#endif
}

void cleanup(rm_robot_handle *handle) {
    if (handle != nullptr) {
        const int delete_result = rm_delete_robot_arm(handle);
        if (delete_result != 0) {
            std::cerr << "rm_delete_robot_arm failed, error code: " << delete_result << '\n';
        }
    }

    const int destroy_result = destroyApi();
    if (destroy_result != 0) {
        std::cerr << "RealMan API destroy failed, error code: " << destroy_result << '\n';
    }
}

void printJoints(const char *label, const float joints[ARM_DOF], int dof) {
    std::cout << label;
    for (int i = 0; i < dof; ++i) {
        std::cout << (i == 0 ? " [" : ", ")
                  << "J" << (i + 1) << "=" << std::fixed << std::setprecision(3) << joints[i];
    }
    std::cout << "] deg\n";
}

void printPose(const char *label, const rm_pose_t &pose) {
    std::cout << label
              << " position[m]=(" << std::fixed << std::setprecision(4)
              << pose.position.x << ", " << pose.position.y << ", " << pose.position.z
              << "), euler[rad]=(" << pose.euler.rx << ", " << pose.euler.ry << ", " << pose.euler.rz
              << ")\n";
}

bool readArmState(rm_robot_handle *handle, rm_current_arm_state_t *state) {
    if (handle == nullptr || state == nullptr) {
        return false;
    }

    const int result = rm_get_current_arm_state(handle, state);
    if (result != 0) {
        std::cerr << "rm_get_current_arm_state failed, error code: " << result << '\n';
        return false;
    }
    return true;
}

bool allFinite(const float joints[ARM_DOF], int dof) {
    for (int i = 0; i < dof; ++i) {
        if (!std::isfinite(joints[i])) {
            std::cerr << "Joint J" << (i + 1) << " is not finite: " << joints[i] << '\n';
            return false;
        }
    }
    return true;
}

bool validateSpeed() {
    if (kSpeedPercent < 1 || kSpeedPercent > kMaxAllowedSpeedPercent) {
        std::cerr << "kSpeedPercent must be in range 1.." << kMaxAllowedSpeedPercent
                  << ". Current value: " << kSpeedPercent << '\n';
        return false;
    }
    return true;
}

bool validateTargetDelta(const float current[ARM_DOF], const float target[ARM_DOF], int dof) {
    if (!allFinite(current, dof) || !allFinite(target, dof)) {
        return false;
    }

    bool has_motion = false;
    for (int i = 0; i < dof; ++i) {
        const float delta = target[i] - current[i];
        if (std::fabs(delta) > 0.001F) {
            has_motion = true;
        }
        if (std::fabs(delta) > kMaxSingleJointDeltaDeg) {
            std::cerr << "Refuse to move J" << (i + 1) << " by " << delta
                      << " deg. Limit is +/-" << kMaxSingleJointDeltaDeg << " deg.\n";
            return false;
        }
    }

    if (!has_motion) {
        std::cerr << "Target equals current joints; no motion will be sent.\n";
        return false;
    }
    return true;
}

void buildTargetJoints(const float current[ARM_DOF], float target[ARM_DOF], int dof) {
    for (int i = 0; i < dof; ++i) {
        if (kTargetMode == TargetMode::kRelativeJointOffset) {
            target[i] = current[i] + kRelativeJointOffsetsDeg[static_cast<size_t>(i)];
        } else {
            target[i] = kAbsoluteTargetJointsDeg[static_cast<size_t>(i)];
        }
    }

    for (int i = dof; i < ARM_DOF; ++i) {
        target[i] = current[i];
    }
}

void countdownBeforeMotion() {
    std::cout << "\nSafety countdown before motion. Clear workspace and keep E-stop reachable.\n";
    for (int second = kSafetyCountdownSeconds; second > 0; --second) {
        std::cout << "Moving in " << second << "...\n";
        std::this_thread::sleep_for(std::chrono::seconds(1));
    }
}

bool moveJointTarget(rm_robot_handle *handle,
                     const char *label,
                     const float target[ARM_DOF],
                     int dof) {
    printJoints(label, target, dof);

    if (kDryRunOnly) {
        std::cout << "Dry run enabled. rm_movej was not sent.\n";
        return true;
    }

    const int result = rm_movej(handle, target, kSpeedPercent, 0, 0, 1);
    if (result != 0) {
        std::cerr << "rm_movej failed, error code: " << result << '\n';
        const int stop_result = rm_set_arm_stop(handle);
        if (stop_result != 0) {
            std::cerr << "rm_set_arm_stop failed, error code: " << stop_result << '\n';
        }
        return false;
    }

    rm_current_arm_state_t actual = {};
    if (!readArmState(handle, &actual)) {
        return false;
    }
    printJoints("Actual joints after move:", actual.joint, dof);
    printPose("Actual TCP pose:", actual.pose);
    return true;
}

} // namespace

int main() {
    std::cout.setf(std::ios::unitbuf);
    std::cerr.setf(std::ios::unitbuf);

    if (!validateSpeed()) {
        return 64;
    }

    rm_set_log_call_back(apiLogCallback, 2);
    rm_set_timeout(kTimeoutMs);

    const int init_result = rm_init(RM_TRIPLE_MODE_E);
    if (init_result != 0) {
        std::cerr << "rm_init failed, error code: " << init_result << '\n';
        return 1;
    }

    std::cout << "RealMan API version: " << rm_api_version() << '\n'
              << "Connecting to " << kRobotIp << ':' << kRobotPort << "...\n";

    rm_robot_handle *handle = rm_create_robot_arm(kRobotIp, kRobotPort);
    if (handle == nullptr || handle->id < 0) {
        std::cerr << "Failed to connect to RealMan arm.\n";
        cleanup(handle);
        return 2;
    }

    rm_robot_info_t robot_info = {};
    int result = rm_get_robot_info(handle, &robot_info);
    if (result != 0) {
        std::cerr << "rm_get_robot_info failed, error code: " << result << '\n';
        cleanup(handle);
        return 3;
    }

    const int dof = static_cast<int>(robot_info.arm_dof);
    if (dof <= 0 || dof > ARM_DOF) {
        std::cerr << "Invalid arm DOF reported by controller: " << dof << '\n';
        cleanup(handle);
        return 4;
    }

    rm_current_arm_state_t start_state = {};
    if (!readArmState(handle, &start_state)) {
        cleanup(handle);
        return 5;
    }

    float target[ARM_DOF] = {};
    buildTargetJoints(start_state.joint, target, dof);

    std::cout << "Connected. Arm DOF: " << dof << ", speed: " << kSpeedPercent << "%\n"
              << "Target mode: "
              << (kTargetMode == TargetMode::kRelativeJointOffset ? "relative joint offset" : "absolute joint angles")
              << '\n';
    printJoints("Current joints:", start_state.joint, dof);
    printPose("Current TCP pose:", start_state.pose);
    printJoints("Planned target joints:", target, dof);

    if (!validateTargetDelta(start_state.joint, target, dof)) {
        cleanup(handle);
        return 6;
    }

    countdownBeforeMotion();

    if (!moveJointTarget(handle, "Move target:", target, dof)) {
        cleanup(handle);
        return 7;
    }

    if (kReturnToStartAfterMove) {
        if (!moveJointTarget(handle, "Return to start:", start_state.joint, dof)) {
            cleanup(handle);
            return 8;
        }
    }

    cleanup(handle);
    std::cout << "Joint pose config demo finished successfully.\n";
    return 0;
}
