#include "rm_control/core/drag_teach_recorder.h"
#include "rm_control/drivers/realman/realman_robot_arm.h"
#include "rm_control/modules/trajectory/trajectory_file_store.h"

#include <cstdlib>
#include <iostream>
#include <string>

namespace {

using rm_control::core::DragTeachRecordConfig;

void printUsage(const char *program_name) {
    std::cout << "Usage: " << program_name
              << " [session_name] [duration_seconds] [sample_period_ms] [storage_root]"
              << " [--no-vendor-save] [--ordinary-recorded] [--no-singular-wall]\n"
              << "\n"
              << "Default mode is RM75 six-dof force drag teach with position and orientation enabled.\n"
              << "The program records synchronized CSV as the primary artifact.\n";
}

bool parseInt(const char *value, int *out) {
    if (value == nullptr || out == nullptr || value[0] == '\0') {
        return false;
    }

    char *end = nullptr;
    const long parsed = std::strtol(value, &end, 10);
    if (end == value || *end != '\0') {
        return false;
    }
    *out = static_cast<int>(parsed);
    return true;
}

std::string defaultSessionName() {
    return "rm75_drag_pose_" + rm_control::modules::trajectory::timestampString();
}

bool parseArguments(int argc, char **argv, DragTeachRecordConfig *config) {
    if (config == nullptr) {
        return false;
    }
    if (argc > 1 && (std::string(argv[1]) == "-h" || std::string(argv[1]) == "--help")) {
        printUsage(argv[0]);
        std::exit(0);
    }

    config->session_name = defaultSessionName();
    config->storage_root = rm_control::modules::trajectory::defaultTrajectoryStorageRoot();
    config->drag_options.mode = rm_control::common::DragTeachMode::SixDofForcePositionAndOrientation;
    config->drag_options.enable_singular_wall = true;
    config->drag_options.record_vendor_trajectory = true;

    int positional_index = 0;
    for (int i = 1; i < argc; ++i) {
        const std::string arg = argv[i] == nullptr ? "" : argv[i];
        if (arg == "--no-vendor-save") {
            config->save_vendor_trajectory = false;
            continue;
        }
        if (arg == "--ordinary-recorded") {
            config->drag_options.mode = rm_control::common::DragTeachMode::OrdinaryRecorded;
            continue;
        }
        if (arg == "--no-singular-wall") {
            config->drag_options.enable_singular_wall = false;
            continue;
        }
        if (!arg.empty() && arg[0] == '-') {
            std::cerr << "Unknown option: " << arg << '\n';
            return false;
        }

        ++positional_index;
        if (positional_index == 1) {
            config->session_name = arg;
        } else if (positional_index == 2) {
            if (!parseInt(arg.c_str(), &config->duration_seconds)) {
                std::cerr << "Invalid duration_seconds: " << arg << '\n';
                return false;
            }
        } else if (positional_index == 3) {
            if (!parseInt(arg.c_str(), &config->sample_period_ms)) {
                std::cerr << "Invalid sample_period_ms: " << arg << '\n';
                return false;
            }
        } else if (positional_index == 4) {
            config->storage_root = arg;
        } else {
            std::cerr << "Too many positional arguments.\n";
            return false;
        }
    }

    return true;
}

} // namespace

int main(int argc, char **argv) {
    DragTeachRecordConfig config;
    if (!parseArguments(argc, argv, &config)) {
        printUsage(argv[0]);
        return 64;
    }

    rm_control::drivers::realman::RealManRobotArm robot;
    rm_control::common::RobotConnectionConfig connection;
    auto connect_result = robot.connect(connection);
    if (!connect_result.ok) {
        std::cerr << "Failed to connect RM75: " << connect_result.error_message << '\n';
        return 1;
    }

    rm_control::core::DragTeachRecorder recorder(robot);
    auto summary = recorder.record(config, std::cout);
    if (!summary.ok) {
        std::cerr << "Drag teach record failed: " << summary.error_message << '\n';
        return 2;
    }

    std::cout << "Recorded samples: " << summary.value.sample_count << '\n'
              << "CSV: " << summary.value.files.csv_path << '\n'
              << "Vendor: " << summary.value.files.vendor_path << '\n'
              << "Converted C++ target path: " << summary.value.files.converted_cpp_path << '\n';
    return 0;
}
