#include "rm_control/config/drag_teach_safety_config.h"

#include "rm_control/config/simple_json.h"

#include <cmath>
#include <fstream>
#include <sstream>
#include <string>

namespace rm_control::config {
namespace {

common::Result<std::string> ReadTextFile(const std::filesystem::path &path) {
    std::ifstream input(path);
    if (!input) {
        return common::Result<std::string>::failure(
            "failed to open drag teach safety config: " + path.string());
    }

    std::ostringstream buffer;
    buffer << input.rdbuf();
    if (!input.good() && !input.eof()) {
        return common::Result<std::string>::failure(
            "failed to read drag teach safety config: " + path.string());
    }
    return common::Result<std::string>::success(buffer.str());
}

common::Result<bool> ReadRequiredBool(const JsonValue &root, const std::string &name) {
    const JsonValue *value = root.find(name);
    if (value == nullptr || !value->isBool()) {
        return common::Result<bool>::failure(
            "drag teach safety field '" + name + "' must be bool");
    }
    return common::Result<bool>::success(value->asBool());
}

common::Result<double> ReadRequiredNumber(const JsonValue &root, const std::string &name) {
    const JsonValue *value = root.find(name);
    if (value == nullptr || !value->isNumber() || !std::isfinite(value->asNumber())) {
        return common::Result<double>::failure(
            "drag teach safety field '" + name + "' must be a finite number");
    }
    return common::Result<double>::success(value->asNumber());
}

common::Result<int> ReadRequiredIntegerInRange(const JsonValue &root,
                                               const std::string &name,
                                               int minimum_value,
                                               int maximum_value) {
    const auto value = ReadRequiredNumber(root, name);
    if (!value.ok) {
        return common::Result<int>::failure(value.error_message);
    }
    if (std::floor(value.value) != value.value) {
        return common::Result<int>::failure(
            "drag teach safety field '" + name + "' must be an integer");
    }
    if (value.value < minimum_value || value.value > maximum_value) {
        return common::Result<int>::failure(
            "drag teach safety field '" + name + "' must be in range " +
            std::to_string(minimum_value) + ".." + std::to_string(maximum_value));
    }
    return common::Result<int>::success(static_cast<int>(value.value));
}

common::Result<void> AssignBooleanFields(const JsonValue &root,
                                         DragTeachSafetyConfig *config) {
    const auto hardware_enabled = ReadRequiredBool(root, "hardware_execution_enabled");
    const auto emergency_stop = ReadRequiredBool(root, "emergency_stop_verified");
    const auto limits_loaded = ReadRequiredBool(root, "safety_limits_loaded");
    const auto workspace = ReadRequiredBool(root, "workspace_and_virtual_wall_verified");
    const auto native_replay =
        ReadRequiredBool(root, "native_replay_without_speed_control_accepted");
    const auto sample_period_confirmed =
        ReadRequiredBool(root, "trajectory_sample_period_confirmed");
    const auto raw_units_confirmed =
        ReadRequiredBool(root, "vendor_raw_units_per_degree_confirmed");
    if (!hardware_enabled.ok || !emergency_stop.ok || !limits_loaded.ok ||
        !workspace.ok || !native_replay.ok || !sample_period_confirmed.ok ||
        !raw_units_confirmed.ok) {
        const std::string error = !hardware_enabled.ok ? hardware_enabled.error_message
                                  : !emergency_stop.ok ? emergency_stop.error_message
                                  : !limits_loaded.ok ? limits_loaded.error_message
                                  : !workspace.ok ? workspace.error_message
                                  : !native_replay.ok ? native_replay.error_message
                                  : !sample_period_confirmed.ok
                                      ? sample_period_confirmed.error_message
                                      : raw_units_confirmed.error_message;
        return common::Result<void>::failure(error);
    }

    config->hardware_execution_enabled = hardware_enabled.value;
    config->emergency_stop_verified = emergency_stop.value;
    config->safety_limits_loaded = limits_loaded.value;
    config->workspace_and_virtual_wall_verified = workspace.value;
    config->native_replay_without_speed_control_accepted = native_replay.value;
    config->trajectory_sample_period_confirmed = sample_period_confirmed.value;
    config->vendor_raw_units_per_degree_confirmed = raw_units_confirmed.value;
    return common::Result<void>::success();
}

common::Result<void> AssignCommandLimits(const JsonValue &root,
                                         DragTeachSafetyConfig *config) {
    const auto max_speed =
        ReadRequiredIntegerInRange(root, "max_speed_percent", 1, 100);
    const auto raw_units =
        ReadRequiredIntegerInRange(root, "vendor_raw_units_per_degree", 1, 1000000);
    const auto maximum_o6_speed =
        ReadRequiredIntegerInRange(root, "maximum_o6_speed_command", 0, 255);
    const auto maximum_o6_torque =
        ReadRequiredIntegerInRange(root, "maximum_o6_torque_command", 0, 255);
    if (!max_speed.ok || !raw_units.ok || !maximum_o6_speed.ok ||
        !maximum_o6_torque.ok) {
        return common::Result<void>::failure(
            !max_speed.ok ? max_speed.error_message
            : !raw_units.ok ? raw_units.error_message
            : !maximum_o6_speed.ok ? maximum_o6_speed.error_message
                                   : maximum_o6_torque.error_message);
    }
    config->max_speed_percent = max_speed.value;
    config->vendor_raw_units_per_degree = raw_units.value;
    config->maximum_o6_speed_command =
        static_cast<std::uint16_t>(maximum_o6_speed.value);
    config->maximum_o6_torque_command =
        static_cast<std::uint16_t>(maximum_o6_torque.value);
    return common::Result<void>::success();
}

common::Result<void> AssignTrajectoryAndForceLimits(
    const JsonValue &root,
    DragTeachSafetyConfig *config) {
    const auto maximum_origin_delta =
        ReadRequiredNumber(root, "maximum_origin_move_delta_deg");
    const auto max_jump = ReadRequiredNumber(root, "max_joint_jump_deg");
    const auto max_velocity =
        ReadRequiredNumber(root, "max_joint_velocity_deg_per_second");
    const auto max_acceleration =
        ReadRequiredNumber(root, "max_joint_acceleration_deg_per_second_squared");
    const auto maximum_force =
        ReadRequiredNumber(root, "maximum_absolute_force_newton");
    const auto maximum_torque =
        ReadRequiredNumber(root, "maximum_absolute_torque_newton_meter");
    if (!maximum_origin_delta.ok || !max_jump.ok || !max_velocity.ok ||
        !max_acceleration.ok ||
        !maximum_force.ok || !maximum_torque.ok) {
        const std::string error = !maximum_origin_delta.ok
                                      ? maximum_origin_delta.error_message
                                  : !max_jump.ok ? max_jump.error_message
                                  : !max_velocity.ok ? max_velocity.error_message
                                  : !max_acceleration.ok ? max_acceleration.error_message
                                  : !maximum_force.ok ? maximum_force.error_message
                                                      : maximum_torque.error_message;
        return common::Result<void>::failure(error);
    }
    config->maximum_origin_move_delta_deg =
        static_cast<float>(maximum_origin_delta.value);
    config->max_joint_jump_deg = static_cast<float>(max_jump.value);
    config->max_joint_velocity_deg_per_second = static_cast<float>(max_velocity.value);
    config->max_joint_acceleration_deg_per_second_squared =
        static_cast<float>(max_acceleration.value);
    config->maximum_absolute_force_newton =
        static_cast<float>(maximum_force.value);
    config->maximum_absolute_torque_newton_meter =
        static_cast<float>(maximum_torque.value);
    return common::Result<void>::success();
}

common::Result<void> AssignNumericFields(const JsonValue &root,
                                         DragTeachSafetyConfig *config) {
    const auto command_limits = AssignCommandLimits(root, config);
    if (!command_limits.ok) {
        return command_limits;
    }
    return AssignTrajectoryAndForceLimits(root, config);
}

common::Result<void> ValidateSafetyConfig(const DragTeachSafetyConfig &config) {
    if (config.max_speed_percent <= 0 || config.max_speed_percent > 100) {
        return common::Result<void>::failure("max_speed_percent must be in range 1..100");
    }
    if (!std::isfinite(config.maximum_origin_move_delta_deg) ||
        !std::isfinite(config.max_joint_jump_deg) ||
        !std::isfinite(config.max_joint_velocity_deg_per_second) ||
        !std::isfinite(config.max_joint_acceleration_deg_per_second_squared) ||
        !std::isfinite(config.maximum_absolute_force_newton) ||
        !std::isfinite(config.maximum_absolute_torque_newton_meter) ||
        config.maximum_origin_move_delta_deg <= 0.0F ||
        config.maximum_origin_move_delta_deg > 180.0F ||
        config.max_joint_jump_deg <= 0.0F ||
        config.max_joint_velocity_deg_per_second <= 0.0F ||
        config.max_joint_acceleration_deg_per_second_squared <= 0.0F ||
        config.maximum_absolute_force_newton <= 0.0F ||
        config.maximum_absolute_torque_newton_meter <= 0.0F) {
        return common::Result<void>::failure(
            "origin, trajectory and force safety limits must be positive; "
            "maximum_origin_move_delta_deg must not exceed 180");
    }
    return common::Result<void>::success();
}

} // namespace

common::Result<DragTeachSafetyConfig>
LoadDragTeachSafetyConfig(const std::filesystem::path &config_path) {
    if (config_path.empty()) {
        return common::Result<DragTeachSafetyConfig>::failure(
            "drag teach safety config path must not be empty");
    }

    const auto text = ReadTextFile(config_path);
    if (!text.ok) {
        return common::Result<DragTeachSafetyConfig>::failure(text.error_message);
    }
    const auto json = parseJson(text.value);
    if (!json.ok || !json.value.isObject()) {
        return common::Result<DragTeachSafetyConfig>::failure(
            "invalid drag teach safety JSON in " + config_path.string() + ": " +
            (json.ok ? "root must be an object" : json.error_message));
    }

    DragTeachSafetyConfig config;
    const auto booleans = AssignBooleanFields(json.value, &config);
    const auto numbers = AssignNumericFields(json.value, &config);
    if (!booleans.ok || !numbers.ok) {
        return common::Result<DragTeachSafetyConfig>::failure(
            "invalid drag teach safety config " + config_path.string() + ": " +
            (!booleans.ok ? booleans.error_message : numbers.error_message));
    }
    const auto validation = ValidateSafetyConfig(config);
    if (!validation.ok) {
        return common::Result<DragTeachSafetyConfig>::failure(
            "invalid drag teach safety config " + config_path.string() + ": " +
            validation.error_message);
    }
    return common::Result<DragTeachSafetyConfig>::success(config);
}

} // namespace rm_control::config
