#include "realman_trajectory_common.hpp"

#include <thread>

namespace {

using realman_trajectory::RobotConnectionConfig;
using realman_trajectory::RobotSession;
using realman_trajectory::TrajectoryPoint;

constexpr int kDefaultReplaySpeedPercent = 5;
constexpr int kMaximumReplaySpeedPercent = 20;
constexpr float kDefaultMaxStepDeltaDeg = 5.0F;
constexpr float kDefaultStartToleranceDeg = 5.0F;
constexpr int kSafetyCountdownSeconds = 5;

struct ReplayConfig {
    std::string input_csv_path;
    bool execute_motion = false;
    int speed_percent = kDefaultReplaySpeedPercent;
    float max_step_delta_deg = kDefaultMaxStepDeltaDeg;
    float start_tolerance_deg = kDefaultStartToleranceDeg;
    RobotConnectionConfig robot;
};

void printUsage(const char *program_name) {
    std::cout << "Usage: " << program_name
              << " <input_csv> [dry-run|execute] [speed_percent] [max_step_delta_deg]"
              << " [start_tolerance_deg] [robot_ip] [port] [timeout_ms]\n"
              << "\n"
              << "Examples:\n"
              << "  " << program_name << " reports/realman_trajectories/demo.csv dry-run\n"
              << "  " << program_name << " reports/realman_trajectories/demo.csv execute 5 5 5\n"
              << "\n"
              << "Dry-run is the default. Use execute only with a clear workspace and reachable E-stop.\n";
}

bool parseReplayMode(const char *value, ReplayConfig *config) {
    if (value == nullptr || config == nullptr) {
        return false;
    }

    const std::string mode = value;
    if (mode == "dry-run") {
        config->execute_motion = false;
        return true;
    }
    if (mode == "execute") {
        config->execute_motion = true;
        return true;
    }

    std::cerr << "Replay mode must be dry-run or execute. Actual: " << mode << '\n';
    return false;
}

bool parseReplayArguments(int argc, char **argv, ReplayConfig *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);
    }
    if (argc < 2) {
        std::cerr << "input_csv is required.\n";
        return false;
    }

    config->input_csv_path = argv[1];
    if (argc > 2 && !parseReplayMode(argv[2], config)) {
        return false;
    }
    if (argc > 3 && !realman_trajectory::parseIntArgument(argv[3], &config->speed_percent)) {
        std::cerr << "Invalid speed_percent: " << argv[3] << '\n';
        return false;
    }
    if (argc > 4 && !realman_trajectory::parseFloatArgument(argv[4], &config->max_step_delta_deg)) {
        std::cerr << "Invalid max_step_delta_deg: " << argv[4] << '\n';
        return false;
    }
    if (argc > 5 && !realman_trajectory::parseFloatArgument(argv[5], &config->start_tolerance_deg)) {
        std::cerr << "Invalid start_tolerance_deg: " << argv[5] << '\n';
        return false;
    }
    return argc <= 9;
}

bool parseReplayConnectionArguments(int argc, char **argv, ReplayConfig *config) {
    if (argc > 6) {
        config->robot.robot_ip = argv[6];
    }
    if (argc > 7 && !realman_trajectory::parseIntArgument(argv[7], &config->robot.robot_port)) {
        std::cerr << "Invalid port: " << argv[7] << '\n';
        return false;
    }
    if (argc > 8 && !realman_trajectory::parseIntArgument(argv[8], &config->robot.timeout_ms)) {
        std::cerr << "Invalid timeout_ms: " << argv[8] << '\n';
        return false;
    }
    if (argc > 9) {
        std::cerr << "Too many arguments.\n";
        return false;
    }
    return true;
}

bool validateReplayConfig(const ReplayConfig &config) {
    if (config.input_csv_path.empty()) {
        std::cerr << "input_csv must not be empty.\n";
        return false;
    }
    if (config.speed_percent <= 0 || config.speed_percent > kMaximumReplaySpeedPercent) {
        std::cerr << "speed_percent must be in range 1.." << kMaximumReplaySpeedPercent << ".\n";
        return false;
    }
    if (config.max_step_delta_deg <= 0.0F || config.start_tolerance_deg <= 0.0F) {
        std::cerr << "max_step_delta_deg and start_tolerance_deg must be greater than 0.\n";
        return false;
    }
    return config.robot.robot_port > 0 && config.robot.robot_port <= 65535 && config.robot.timeout_ms > 0;
}

bool validateConsecutiveStepDeltas(const std::vector<TrajectoryPoint> &points,
                                   int dof,
                                   float max_step_delta_deg) {
    for (size_t i = 1; i < points.size(); ++i) {
        const float step_delta = realman_trajectory::maxAbsJointDelta(points[i - 1].joint_deg, points[i].joint_deg, dof);
        if (step_delta > max_step_delta_deg) {
            std::cerr << "Refuse trajectory: step " << i << " joint delta " << step_delta
                      << " deg exceeds limit " << max_step_delta_deg << " deg.\n";
            return false;
        }
    }
    return true;
}

bool validateRobotStartsNearFirstPoint(rm_robot_handle *handle,
                                       const TrajectoryPoint &first_point,
                                       int dof,
                                       float tolerance_deg) {
    rm_current_arm_state_t current_state = {};
    if (!realman_trajectory::readCurrentArmState(handle, &current_state)) {
        return false;
    }

    const std::array<float, ARM_DOF> current_joints = realman_trajectory::copyJointArray(current_state.joint);
    const float start_delta = realman_trajectory::maxAbsJointDelta(current_joints, first_point.joint_deg, dof);
    std::cout << "Current joints: " << realman_trajectory::formatJoints(current_joints, dof) << '\n';
    std::cout << "CSV first point: " << realman_trajectory::formatJoints(first_point.joint_deg, dof) << '\n';
    if (start_delta > tolerance_deg) {
        std::cerr << "Robot is not near the first trajectory point. Max delta " << start_delta
                  << " deg exceeds tolerance " << tolerance_deg << " deg.\n";
        return false;
    }
    return true;
}

void countdownBeforeMotion() {
    std::cout << "Safety countdown before trajectory replay.\n";
    for (int second = kSafetyCountdownSeconds; second > 0; --second) {
        std::cout << "Moving in " << second << "...\n";
        std::this_thread::sleep_for(std::chrono::seconds(1));
    }
}

bool sendJointPoint(rm_robot_handle *handle, const TrajectoryPoint &point, int speed_percent) {
    const int result = rm_movej(handle, point.joint_deg.data(), speed_percent, 0, 0, 1);
    if (result != 0) {
        std::cerr << "rm_movej failed, error code: " << result << '\n';
        const int stop_result = rm_set_arm_stop(handle);
        if (stop_result != 0) {
            std::cerr << "rm_set_arm_stop failed, error code: " << stop_result << '\n';
        }
        return false;
    }
    return true;
}

bool replayTrajectory(rm_robot_handle *handle,
                      const std::vector<TrajectoryPoint> &points,
                      const ReplayConfig &config,
                      int dof) {
    countdownBeforeMotion();
    for (size_t i = 1; i < points.size(); ++i) {
        if (!sendJointPoint(handle, points[i], config.speed_percent)) {
            return false;
        }
        if (i == points.size() - 1 || i % 10 == 0) {
            std::cout << "Reached point " << i << "/" << (points.size() - 1)
                      << " joints " << realman_trajectory::formatJoints(points[i].joint_deg, dof) << '\n';
        }
    }
    return true;
}

void printTrajectorySummary(const std::vector<TrajectoryPoint> &points, int dof) {
    std::cout << "Loaded " << points.size() << " trajectory points.\n";
    std::cout << "First point: " << realman_trajectory::formatJoints(points.front().joint_deg, dof) << '\n';
    std::cout << "Last point: " << realman_trajectory::formatJoints(points.back().joint_deg, dof) << '\n';
}

} // namespace

int main(int argc, char **argv) {
    std::cout.setf(std::ios::unitbuf);
    std::cerr.setf(std::ios::unitbuf);

    ReplayConfig config;
    if (!parseReplayArguments(argc, argv, &config) ||
        !parseReplayConnectionArguments(argc, argv, &config) ||
        !validateReplayConfig(config)) {
        printUsage(argv[0]);
        return 64;
    }

    std::vector<TrajectoryPoint> trajectory_points;
    if (!realman_trajectory::loadTrajectoryCsv(config.input_csv_path, &trajectory_points)) {
        return 1;
    }

    const int dof = realman_trajectory::kExpectedRm75Dof;
    printTrajectorySummary(trajectory_points, dof);
    if (!validateConsecutiveStepDeltas(trajectory_points, dof, config.max_step_delta_deg)) {
        return 2;
    }
    if (!config.execute_motion) {
        std::cout << "Dry-run finished. No motion command was sent.\n";
        return 0;
    }

    RobotSession session(config.robot);
    if (!session.connect()) {
        return 3;
    }

    int robot_dof = 0;
    if (!realman_trajectory::readAndValidateRobotDof(session.handle(), &robot_dof)) {
        return 4;
    }
    if (!validateRobotStartsNearFirstPoint(session.handle(), trajectory_points.front(), robot_dof, config.start_tolerance_deg)) {
        return 5;
    }
    if (!replayTrajectory(session.handle(), trajectory_points, config, robot_dof)) {
        return 6;
    }

    std::cout << "Trajectory replay finished successfully.\n";
    return 0;
}
