#include "rm_control/core/arm_o6_trajectory_task.h"
#include "rm_control/modules/trajectory/vendor_drag_trajectory_parser.h"

#include "mocks/mock_robot_arm.h"

#include <array>
#include <chrono>
#include <cstddef>
#include <filesystem>
#include <fstream>
#include <iostream>
#include <string>
#include <system_error>
#include <vector>

#ifndef RM_TEST_OUTPUT_DIR
#define RM_TEST_OUTPUT_DIR "."
#endif

namespace {

constexpr int kReplayLoopCount = 10;
constexpr int kForceSpikeCycleIndex = 4;
constexpr float kForceSpikeNewton = 6.0F;

struct TestContext {
    int failures = 0;

    void Expect(bool condition, const std::string &message) {
        if (!condition) {
            ++failures;
            std::cerr << "[FAIL] " << message << '\n';
        }
    }
};

const std::string &ValidTrajectoryJsonLines() {
    static const std::string content =
        "{\"point\":[0,0,0,0,0,0,0]}\n"
        "{\"point\":[100,0,0,0,0,0,0]}\n"
        "{\"point\":[200,0,0,0,0,0,0]}\n";
    return content;
}

std::filesystem::path TestOutputRoot() {
    static const std::filesystem::path output_root = [] {
        const auto unique_suffix =
            std::chrono::steady_clock::now().time_since_epoch().count();
        const std::filesystem::path path =
            std::filesystem::path(RM_TEST_OUTPUT_DIR) /
            ("rm_trajectory_replay_force_spike_thumb_index_" +
             std::to_string(unique_suffix));
        std::error_code filesystem_error;
        std::filesystem::create_directories(path, filesystem_error);
        if (filesystem_error) {
            std::cerr << "[FAIL] failed to create test output: "
                      << filesystem_error.message() << '\n';
        }
        return path;
    }();
    return output_root;
}

rm_control::modules::trajectory::VendorDragTrajectoryParseResult
CreateReadOnlyTrajectoryFixture() {
    const std::filesystem::path path = TestOutputRoot() / "loop_replay.jsonl";
    {
        std::ofstream output(path, std::ios::binary | std::ios::trunc);
        output << ValidTrajectoryJsonLines();
    }
    const auto parsed =
        rm_control::modules::trajectory::ParseVendorDragTrajectoryJsonLines(path, 100);
    if (!parsed.ok) {
        std::cerr << "[FAIL] failed to parse loop replay fixture: "
                  << parsed.error_message << '\n';
        return {};
    }
    std::error_code filesystem_error;
    const auto read_only_permissions = std::filesystem::perms::owner_read |
        std::filesystem::perms::group_read | std::filesystem::perms::others_read;
    std::filesystem::permissions(
        path, read_only_permissions, std::filesystem::perm_options::replace,
        filesystem_error);
    if (filesystem_error) {
        std::cerr << "[FAIL] failed to make loop replay fixture read-only: "
                  << filesystem_error.message() << '\n';
    }
    return parsed.value;
}

rm_control::modules::trajectory::VendorDragTrajectoryParseResult MakeTrajectory() {
    static const auto trajectory = CreateReadOnlyTrajectoryFixture();
    return trajectory;
}

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

rm_control::core::ArmO6TrajectoryTaskConfig MakeTaskConfig() {
    rm_control::core::ArmO6TrajectoryTaskConfig config;
    config.validation_limits.maximum_joint_jump_deg = 1.0;
    config.validation_limits.maximum_joint_velocity_deg_per_second = 10.0;
    config.validation_limits.maximum_joint_acceleration_deg_per_second_squared = 100.0;
    config.origin_speed_percent = 5;
    config.plan_speed_percent = 5;
    config.controller_program_id = 1;
    config.grip_pose = {140U, 100U, 55U, 55U, 55U, 55U};
    config.grip_speed_commands = {64U, 64U, 64U, 64U, 64U, 64U};
    config.grip_torque_commands = {80U, 80U, 80U, 80U, 80U, 80U};
    config.maximum_o6_speed_command = 64U;
    config.maximum_o6_torque_command = 80U;
    config.maximum_origin_delta_deg = 5.0;
    config.origin_position_tolerance_deg = 0.5;
    config.program_status_poll_period = std::chrono::milliseconds(0);
    config.program_execution_timeout = std::chrono::milliseconds(50);
    config.hand_status_poll_period = std::chrono::milliseconds(0);
    config.hand_motion_timeout = std::chrono::milliseconds(0);
    config.open_hand_on_force_spike_during_replay = true;
    config.force_spike_hand_guard_config.open_hand_command =
        BuildThumbAndIndexOpenCommand();
    return config;
}

rm_control::core::HardwareExecutionGateInput MakeAllowedGate() {
    rm_control::core::HardwareExecutionGateInput gate;
    gate.live_requested = true;
    gate.hardware_execution_enabled = true;
    gate.arm_connected = true;
    gate.hand_connected = true;
    gate.emergency_stop_verified = true;
    gate.safety_limits_loaded = true;
    gate.operator_confirmed = true;
    gate.trajectory_validated = true;
    gate.trajectory_sample_period_confirmed = true;
    gate.vendor_raw_units_per_degree_confirmed = true;
    gate.workspace_and_virtual_wall_verified = true;
    gate.configured_max_speed_percent = 10;
    return gate;
}

void MakeRobotSafetyArraysExplicit(rm_control::tests::MockRobotArm *robot) {
    robot->safety_status.joint_enable_states.fill(1);
    robot->safety_status.joint_error_codes.fill(0);
    robot->safety_status.arm_error_codes.clear();
    robot->safety_status.controller_error_code = 0;
}

rm_control::interfaces::DexterousHandReadiness MakeReadyHand(
    const rm_control::interfaces::O6JointPose &positions) {
    rm_control::interfaces::DexterousHandReadiness readiness;
    readiness.ready = true;
    readiness.device_id = 0x27U;
    readiness.freedom = 6U;
    readiness.direction = 'R';
    readiness.joint_positions = positions;
    return readiness;
}

rm_control::common::ForceData MakeWorkZeroForce(
    const std::array<float, 6U> &sample) {
    rm_control::common::ForceData data;
    data.work_zero = sample;
    return data;
}

class RecordingDexterousHand final : public rm_control::interfaces::IDexterousHand {
public:
    explicit RecordingDexterousHand(
        rm_control::interfaces::O6JointPose initial_positions)
        : current_positions_(initial_positions) {}

    int readiness_calls = 0;
    int command_calls = 0;
    std::vector<rm_control::interfaces::O6MotionCommand> motion_commands;
    rm_control::common::Result<void> command_result =
        rm_control::common::Result<void>::success();

    rm_control::common::Result<rm_control::interfaces::DexterousHandReadiness>
    ReadReadiness() override {
        ++readiness_calls;
        return rm_control::common::Result<
            rm_control::interfaces::DexterousHandReadiness>::success(
            MakeReadyHand(current_positions_));
    }

    rm_control::common::Result<void> CommandMotion(
        const rm_control::interfaces::O6MotionCommand &command) override {
        ++command_calls;
        motion_commands.push_back(command);
        if (!command_result.ok) {
            return command_result;
        }
        current_positions_ = command.target_positions;
        return rm_control::common::Result<void>::success();
    }

private:
    rm_control::interfaces::O6JointPose current_positions_{};
};

void AppendForceSamplesForReplayCycle(
    rm_control::tests::MockRobotArm *robot,
    bool force_spike_during_monitoring) {
    robot->force_data_samples.push_back(
        MakeWorkZeroForce({0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F}));
    robot->force_data_samples.push_back(
        MakeWorkZeroForce({0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F}));
    robot->force_data_samples.push_back(MakeWorkZeroForce(
        {force_spike_during_monitoring ? kForceSpikeNewton : 0.0F,
         0.0F,
         0.0F,
         0.0F,
         0.0F,
         0.0F}));
    robot->force_data_samples.push_back(
        MakeWorkZeroForce({0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F}));
}

void AppendControllerStatesForReplayCycle(
    rm_control::tests::MockRobotArm *robot,
    int controller_program_id) {
    robot->program_run_states.push_back(
        rm_control::common::ControllerProgramRunState{
            1, controller_program_id, 0, 5});
    robot->program_run_states.push_back(
        rm_control::common::ControllerProgramRunState{
            1, controller_program_id, 0, 5});
    robot->program_run_states.push_back(
        rm_control::common::ControllerProgramRunState{
            0, controller_program_id, 0, 5});
}

void ConfigureRobotForTenReplayCycles(
    rm_control::tests::MockRobotArm *robot,
    const rm_control::modules::trajectory::VendorDragTrajectoryParseResult &trajectory,
    const rm_control::core::ArmO6TrajectoryTaskConfig &config) {
    MakeRobotSafetyArraysExplicit(robot);
    robot->program_run_states.clear();
    robot->force_data_samples.clear();
    robot->completed_program_joint_targets.clear();
    for (int cycle = 0; cycle < kReplayLoopCount; ++cycle) {
        AppendControllerStatesForReplayCycle(robot, config.controller_program_id);
        AppendForceSamplesForReplayCycle(
            robot, cycle == kForceSpikeCycleIndex);
        robot->completed_program_joint_targets.push_back(
            trajectory.samples.back().joints_deg);
    }
}

void TestTenReplayCyclesOpenThumbAndIndexOnForceSpike(TestContext *context) {
    const auto trajectory = MakeTrajectory();
    const auto config = MakeTaskConfig();
    rm_control::tests::MockRobotArm robot;
    ConfigureRobotForTenReplayCycles(&robot, trajectory, config);
    RecordingDexterousHand hand(config.grip_pose);
    rm_control::core::ArmO6TrajectoryTask task(robot, hand);

    for (int cycle = 0; cycle < kReplayLoopCount; ++cycle) {
        const auto result = task.Run(trajectory, config, MakeAllowedGate());
        context->Expect(result.ok, "each replay cycle should complete");
        if (result.ok) {
            context->Expect(
                result.value.observed_running_state,
                "each replay cycle should observe the controller running state");
            context->Expect(
                result.value.force_spike_open_command_sent ==
                    (cycle == kForceSpikeCycleIndex),
                "only the force spike cycle should report an O6 open command");
        }
    }

    const auto thumb_index_open = BuildThumbAndIndexOpenCommand();
    context->Expect(
        robot.execute_vendor_file_calls == kReplayLoopCount,
        "test should execute the trajectory file exactly ten times");
    context->Expect(
        robot.move_joint_calls == kReplayLoopCount,
        "each replay cycle should still move to the trajectory origin through the task");
    context->Expect(
        robot.program_state_calls == kReplayLoopCount * 3,
        "each replay cycle should poll running, running, then completed");
    context->Expect(
        robot.force_data_calls == kReplayLoopCount * 4,
        "each replay cycle should read preflight, two monitor samples, and pre-grip force");
    context->Expect(
        hand.command_calls == kReplayLoopCount + 1,
        "one force-spike open command plus ten final grip commands should be sent");
    context->Expect(
        hand.motion_commands.size() ==
            static_cast<std::size_t>(kReplayLoopCount + 1),
        "test should capture every O6 command in order");
    if (hand.motion_commands.size() ==
        static_cast<std::size_t>(kReplayLoopCount + 1)) {
        const std::size_t open_command_index =
            static_cast<std::size_t>(kForceSpikeCycleIndex);
        context->Expect(
            hand.motion_commands[open_command_index].target_positions ==
                thumb_index_open.target_positions,
            "force spike command must open thumb and index during replay");
        context->Expect(
            hand.motion_commands[open_command_index].joint_speed_commands ==
                thumb_index_open.joint_speed_commands,
            "force spike command should use the configured thumb/index speed");
        context->Expect(
            hand.motion_commands[open_command_index].joint_torque_commands ==
                thumb_index_open.joint_torque_commands,
            "force spike command should use the configured thumb/index torque");
        context->Expect(
            hand.motion_commands[open_command_index + 1U].target_positions ==
                config.grip_pose,
            "final grip command must still run after the spike-cycle trajectory completes");
    }
}

} // namespace

int main() {
    TestContext context;
    TestTenReplayCyclesOpenThumbAndIndexOnForceSpike(&context);

    if (context.failures == 0) {
        std::cout << "[PASS] rm_trajectory_replay_force_spike_thumb_index_tests\n";
        return 0;
    }
    std::cerr << context.failures << " test(s) failed\n";
    return 1;
}
