#include "rm_control/core/hardware_execution_gate.h"

#include <sstream>
#include <string>

namespace rm_control::core {
namespace {

constexpr int kMinimumSpeedPercent = 1;
constexpr int kMaximumSpeedPercent = 100;

void AddMissingCondition(bool condition,
                         const std::string &rejection_reason,
                         HardwareExecutionGateDecision *decision) {
    if (!condition) {
        decision->rejection_reasons.push_back(rejection_reason);
    }
}

void ValidateConfiguredMaximumSpeed(const HardwareExecutionGateInput &input,
                                    HardwareExecutionGateDecision *decision) {
    const bool speed_is_valid = input.configured_max_speed_percent >= kMinimumSpeedPercent &&
                                input.configured_max_speed_percent <= kMaximumSpeedPercent;
    AddMissingCondition(speed_is_valid,
                        "configured maximum speed percent must be in the range 1..100",
                        decision);
}

void ValidateRequiredSpeed(const HardwareExecutionGateInput &input,
                           const HardwareExecutionGateRequirements &requirements,
                           HardwareExecutionGateDecision *decision) {
    if (requirements.required_speed_percent < 0 ||
        requirements.required_speed_percent > kMaximumSpeedPercent) {
        decision->rejection_reasons.emplace_back(
            "required hardware speed percent must be in the range 0..100");
        return;
    }

    if (requirements.required_speed_percent > input.configured_max_speed_percent) {
        decision->rejection_reasons.emplace_back(
            "required hardware speed percent exceeds configured maximum speed percent");
    }
}

std::string JoinRejectionReasons(const std::vector<std::string> &rejection_reasons) {
    std::ostringstream message;
    message << "hardware execution refused";
    for (const auto &reason : rejection_reasons) {
        message << "; " << reason;
    }
    return message.str();
}

} // namespace

HardwareExecutionGateDecision EvaluateHardwareExecutionGate(
    const HardwareExecutionGateInput &input,
    const HardwareExecutionGateRequirements &requirements) {
    HardwareExecutionGateDecision decision;

    AddMissingCondition(input.live_requested, "--live was not explicitly requested", &decision);
    AddMissingCondition(input.hardware_execution_enabled,
                        "hardware_execution_enabled is false",
                        &decision);
    AddMissingCondition(input.arm_connected, "robot arm is not connected", &decision);
    AddMissingCondition(input.hand_connected, "dexterous hand is not connected", &decision);
    AddMissingCondition(input.emergency_stop_verified,
                        "emergency stop state has not been verified as safe",
                        &decision);
    AddMissingCondition(input.safety_limits_loaded,
                        "safety limits have not been loaded",
                        &decision);
    AddMissingCondition(input.operator_confirmed,
                        "operator second confirmation is missing",
                        &decision);
    AddMissingCondition(input.trajectory_validated,
                        "trajectory has not passed validation",
                        &decision);
    AddMissingCondition(input.workspace_and_virtual_wall_verified,
                        "workspace and virtual wall have not been verified",
                        &decision);

    ValidateConfiguredMaximumSpeed(input, &decision);
    ValidateRequiredSpeed(input, requirements, &decision);

    if (requirements.require_native_replay_without_speed_control_acceptance) {
        AddMissingCondition(input.native_replay_without_speed_control_accepted,
                            "native replay without speed control was not explicitly accepted",
                            &decision);
    }
    if (requirements.require_native_replay_speed_verification) {
        AddMissingCondition(input.native_replay_speed_within_config_verified,
                            "native replay speed has not been independently verified "
                            "within the configured limit",
                            &decision);
    }
    if (requirements.require_confirmed_trajectory_sample_period) {
        AddMissingCondition(input.trajectory_sample_period_confirmed,
                            "trajectory sample period has not been confirmed",
                            &decision);
    }
    if (requirements.require_confirmed_vendor_raw_units_per_degree) {
        AddMissingCondition(input.vendor_raw_units_per_degree_confirmed,
                            "vendor raw-units-per-degree ratio has not been confirmed",
                            &decision);
    }

    decision.execution_allowed = decision.rejection_reasons.empty();
    return decision;
}

common::Result<void> RequireHardwareExecutionPermission(
    const HardwareExecutionGateInput &input,
    const HardwareExecutionGateRequirements &requirements) {
    const auto decision = EvaluateHardwareExecutionGate(input, requirements);
    if (!decision.execution_allowed) {
        return common::Result<void>::failure(JoinRejectionReasons(decision.rejection_reasons));
    }
    return common::Result<void>::success();
}

} // namespace rm_control::core
