#include "rm_control/core/force_spike_hand_guard.h"

#include <string>

namespace rm_control::core {
namespace {

constexpr std::uint16_t kMaximumO6CommandValue = 255U;

common::Result<void> ValidateO6JointPose(
    const interfaces::O6JointPose &values,
    const char *field_name) {
    for (std::size_t index = 0U; index < values.size(); ++index) {
        if (values[index] > kMaximumO6CommandValue) {
            return common::Result<void>::failure(
                std::string("O6 ") + field_name + " at joint " +
                std::to_string(index) + " must be in range 0..255");
        }
    }
    return common::Result<void>::success();
}

} // namespace

interfaces::O6MotionCommand BuildDefaultO6FiveFingerOpenCommand() {
    interfaces::O6MotionCommand command;
    command.target_positions = {255U, 255U, 0U, 255U, 255U, 255U};
    command.joint_speed_commands = {64U, 64U, 64U, 64U, 64U, 64U};
    command.joint_torque_commands = {3U, 3U, 3U, 3U, 3U, 3U};
    return command;
}

common::Result<void> ValidateForceSpikeHandGuardConfig(
    const ForceSpikeHandGuardConfig &config) {
    const auto detector = modules::force_safety::ValidateForceSpikeDetectorConfig(
        config.detector);
    const auto positions = ValidateO6JointPose(
        config.open_hand_command.target_positions, "target position command");
    const auto speeds = ValidateO6JointPose(
        config.open_hand_command.joint_speed_commands, "speed command");
    const auto torques = ValidateO6JointPose(
        config.open_hand_command.joint_torque_commands, "torque command");
    if (!detector.ok) {
        return detector;
    }
    if (!positions.ok) {
        return positions;
    }
    if (!speeds.ok) {
        return speeds;
    }
    if (!torques.ok) {
        return torques;
    }
    return common::Result<void>::success();
}

ForceSpikeHandGuard::ForceSpikeHandGuard(
    interfaces::IRobotArm &robot,
    interfaces::IDexterousHand &hand,
    ForceSpikeHandGuardConfig config) noexcept
    : robot_(robot), hand_(hand), config_(config) {}

common::Result<ForceSpikeHandGuardStepReport>
ForceSpikeHandGuard::CheckForceOnceAndOpenHandOnSpike() {
    const auto config_check = ValidateForceSpikeHandGuardConfig(config_);
    if (!config_check.ok) {
        return common::Result<ForceSpikeHandGuardStepReport>::failure(
            config_check.error_message);
    }

    const auto force = robot_.getForceData();
    if (!force.ok) {
        return common::Result<ForceSpikeHandGuardStepReport>::failure(
            "force spike hand guard failed to read RM75 force data: " +
            force.error_message);
    }

    const auto sample = modules::force_safety::SelectForceSignal(
        force.value, config_.detector.signal);
    const auto detection = modules::force_safety::UpdateForceSpikeDetector(
        &detector_state_, sample, config_.detector);
    if (!detection.ok) {
        return common::Result<ForceSpikeHandGuardStepReport>::failure(
            detection.error_message);
    }

    ForceSpikeHandGuardStepReport report;
    report.detection = detection.value;
    if (!report.detection.spike_detected) {
        return common::Result<ForceSpikeHandGuardStepReport>::success(report);
    }
    if (config_.latch_after_successful_open_command && open_command_latched_) {
        report.open_command_suppressed_by_latch = true;
        return common::Result<ForceSpikeHandGuardStepReport>::success(report);
    }

    const auto opened = hand_.CommandMotion(config_.open_hand_command);
    if (!opened.ok) {
        return common::Result<ForceSpikeHandGuardStepReport>::failure(
            "force spike hand guard failed to command O6 open hand: " +
            opened.error_message);
    }

    report.open_command_sent = true;
    if (config_.latch_after_successful_open_command) {
        open_command_latched_ = true;
    }
    return common::Result<ForceSpikeHandGuardStepReport>::success(report);
}

void ForceSpikeHandGuard::ResetOpenCommandLatch() noexcept {
    open_command_latched_ = false;
}

} // namespace rm_control::core
