#include "rm_control/modules/trajectory/trajectory_playback_report_store.h"

#include <algorithm>
#include <cctype>
#include <cmath>
#include <fstream>
#include <iomanip>
#include <sstream>
#include <system_error>

#ifndef RM_RUNTIME_DATA_ROOT
#define RM_RUNTIME_DATA_ROOT "runtime_data"
#endif

namespace rm_control::modules::trajectory {
namespace {

std::string EscapeJsonString(const std::string &value) {
    std::ostringstream escaped;
    for (const unsigned char character : value) {
        if (character == '"' || character == '\\') {
            escaped << '\\' << static_cast<char>(character);
        } else if (character == '\n') {
            escaped << "\\n";
        } else if (character == '\r') {
            escaped << "\\r";
        } else if (character == '\t') {
            escaped << "\\t";
        } else if (character < 0x20U) {
            escaped << "\\u" << std::hex << std::setw(4) << std::setfill('0')
                    << static_cast<int>(character) << std::dec;
        } else {
            escaped << static_cast<char>(character);
        }
    }
    return escaped.str();
}

bool IsSha256(const std::string &value) {
    return value.size() == 64U &&
           std::all_of(value.begin(), value.end(), [](unsigned char character) {
               return std::isxdigit(character) != 0;
           });
}

bool IsSafeReportIdentifier(const std::string &value) {
    return !value.empty() && value.size() <= 64U &&
           std::all_of(value.begin(), value.end(), [](unsigned char character) {
               return std::isalnum(character) != 0 || character == '_' ||
                      character == '-';
           });
}

bool O6ValuesAreValid(const PlaybackReportO6Values &values) {
    return std::all_of(values.begin(), values.end(),
                       [](std::uint16_t value) { return value <= 255U; });
}

common::Result<void> ValidateReport(const ControllerManagedPlaybackReport &report) {
    const bool timing_is_valid = report.sample_period_ms > 0 &&
        report.source_duration_ms >= 0 && report.observed_execution_duration_ms >= 0 &&
        std::isfinite(report.minimum_poll_period_ms) &&
        std::isfinite(report.maximum_poll_period_ms) &&
        std::isfinite(report.average_poll_period_ms) &&
        std::isfinite(report.maximum_absolute_poll_jitter_ms) &&
        report.minimum_poll_period_ms >= 0.0 && report.maximum_poll_period_ms >= 0.0 &&
        report.average_poll_period_ms >= 0.0 &&
        report.maximum_absolute_poll_jitter_ms >= 0.0;
    const bool return_timing_is_valid = !report.returned_from_trajectory_end ||
        (report.return_observed_execution_duration_ms >= 0.0 &&
         std::isfinite(report.return_minimum_poll_period_ms) &&
         std::isfinite(report.return_maximum_poll_period_ms) &&
         std::isfinite(report.return_average_poll_period_ms) &&
         std::isfinite(report.return_maximum_absolute_poll_jitter_ms) &&
         report.return_minimum_poll_period_ms >= 0.0 &&
         report.return_maximum_poll_period_ms >= 0.0 &&
         report.return_average_poll_period_ms >= 0.0 &&
         report.return_maximum_absolute_poll_jitter_ms >= 0.0);
    const bool identifiers_are_valid = report.plan_speed_percent >= 1 &&
        report.plan_speed_percent <= 100 && report.controller_program_id >= 1 &&
        report.controller_program_id <= 100 && report.o6_device_id >= 1U &&
        report.o6_device_id <= 247U;
    const bool return_metadata_is_valid = !report.returned_from_trajectory_end ||
        (!report.return_source_file.empty() &&
         IsSha256(report.return_source_file_sha256) &&
         report.return_controller_program_id >= 1 &&
         report.return_controller_program_id <= 100 &&
         report.return_controller_program_id != report.controller_program_id);
    if (!IsSafeReportIdentifier(report.report_identifier) ||
        report.source_file.empty() || !IsSha256(report.source_file_sha256) ||
        report.trajectory_point_count == 0U || !timing_is_valid ||
        !return_timing_is_valid || !identifiers_are_valid ||
        !return_metadata_is_valid ||
        !O6ValuesAreValid(report.target_positions) ||
        !O6ValuesAreValid(report.speed_commands) ||
        !O6ValuesAreValid(report.torque_commands) ||
        !O6ValuesAreValid(report.final_positions)) {
        return common::Result<void>::failure(
            "controller-managed playback report contains invalid metadata or O6 values");
    }
    return common::Result<void>::success();
}

template <std::size_t Size>
void WriteJsonArray(std::ostream &output, const std::array<std::uint16_t, Size> &values) {
    output << '[';
    for (std::size_t index = 0U; index < values.size(); ++index) {
        output << (index == 0U ? "" : ", ") << values[index];
    }
    output << ']';
}

void WriteReportMetadata(std::ostream &output,
                         const ControllerManagedPlaybackReport &report) {
    output << "{\n  \"report_type\": \"controller_managed_vendor_trajectory\",\n"
           << "  \"report_identifier\": \"" << report.report_identifier << "\",\n"
           << "  \"source_file\": \"" << EscapeJsonString(report.source_file.string())
           << "\",\n  \"source_file_sha256\": \"" << report.source_file_sha256 << "\",\n"
           << "  \"trajectory_point_count\": " << report.trajectory_point_count << ",\n"
           << "  \"sample_period_ms\": " << report.sample_period_ms << ",\n"
           << "  \"source_duration_ms\": " << report.source_duration_ms << ",\n"
           << "  \"timestamp_is_derived\": "
           << (report.timestamp_is_derived ? "true" : "false") << ",\n"
           << "  \"plan_speed_percent\": " << report.plan_speed_percent << ",\n"
           << "  \"controller_program_id\": " << report.controller_program_id << ",\n"
           << "  \"returned_from_trajectory_end\": "
           << (report.returned_from_trajectory_end ? "true" : "false") << ",\n"
           << "  \"return_source_file\": \""
           << EscapeJsonString(report.return_source_file.string()) << "\",\n"
           << "  \"return_source_file_sha256\": \""
           << report.return_source_file_sha256 << "\",\n"
           << "  \"return_controller_program_id\": "
           << report.return_controller_program_id << ",\n";
}

void WriteReportTiming(std::ostream &output,
                       const ControllerManagedPlaybackReport &report) {
    output << "  \"return_observed_execution_duration_ms\": "
           << report.return_observed_execution_duration_ms << ",\n"
           << "  \"return_program_status_poll_count\": "
           << report.return_program_status_poll_count << ",\n"
           << "  \"return_minimum_poll_period_ms\": "
           << report.return_minimum_poll_period_ms << ",\n"
           << "  \"return_maximum_poll_period_ms\": "
           << report.return_maximum_poll_period_ms << ",\n"
           << "  \"return_average_poll_period_ms\": "
           << report.return_average_poll_period_ms << ",\n"
           << "  \"return_maximum_absolute_poll_jitter_ms\": "
           << report.return_maximum_absolute_poll_jitter_ms << ",\n"
           << "  \"return_final_controller_state\": "
           << report.return_final_controller_state << ",\n"
           << "  \"observed_execution_duration_ms\": "
           << report.observed_execution_duration_ms << ",\n"
           << "  \"program_status_poll_count\": " << report.program_status_poll_count << ",\n"
           << "  \"minimum_poll_period_ms\": " << report.minimum_poll_period_ms << ",\n"
           << "  \"maximum_poll_period_ms\": " << report.maximum_poll_period_ms << ",\n"
           << "  \"average_poll_period_ms\": " << report.average_poll_period_ms << ",\n"
           << "  \"maximum_absolute_poll_jitter_ms\": "
           << report.maximum_absolute_poll_jitter_ms << ",\n"
           << "  \"poll_timing_scope\": \"host_program_state_polling_not_point_dispatch\",\n"
           << "  \"final_controller_state\": " << report.final_controller_state << ",\n"
           << "  \"o6_device_id\": " << static_cast<int>(report.o6_device_id) << ",\n";
}

void WriteReportO6Values(std::ostream &output,
                         const ControllerManagedPlaybackReport &report) {
    output << "  \"target_positions\": ";
    WriteJsonArray(output, report.target_positions);
    output << ",\n  \"speed_commands\": ";
    WriteJsonArray(output, report.speed_commands);
    output << ",\n  \"torque_commands\": ";
    WriteJsonArray(output, report.torque_commands);
    output << ",\n  \"final_positions\": ";
    WriteJsonArray(output, report.final_positions);
    output << "\n}\n";
}

void WriteReportJson(std::ostream &output,
                     const ControllerManagedPlaybackReport &report) {
    output << std::fixed << std::setprecision(3);
    WriteReportMetadata(output, report);
    WriteReportTiming(output, report);
    WriteReportO6Values(output, report);
}

} // namespace

std::filesystem::path DefaultTrajectoryPlaybackReportDirectory() {
    return std::filesystem::path(RM_RUNTIME_DATA_ROOT) /
           "trajectory_playback_reports";
}

common::Result<std::filesystem::path>
WriteControllerManagedPlaybackReportWithoutOverwrite(
    const ControllerManagedPlaybackReport &report,
    const std::filesystem::path &output_directory) {
    const auto validation = ValidateReport(report);
    if (!validation.ok) {
        return common::Result<std::filesystem::path>::failure(validation.error_message);
    }
    if (output_directory.empty()) {
        return common::Result<std::filesystem::path>::failure(
            "trajectory playback report directory must not be empty");
    }
    std::error_code filesystem_error;
    std::filesystem::create_directories(output_directory, filesystem_error);
    if (filesystem_error) {
        return common::Result<std::filesystem::path>::failure(
            "failed to create trajectory playback report directory: " +
            filesystem_error.message());
    }
    const std::filesystem::path report_path =
        output_directory /
        ("rm75_o6_playback_" + report.report_identifier + "_" +
         report.source_file_sha256.substr(0U, 12U) + ".json");
    if (std::filesystem::exists(report_path, filesystem_error) || filesystem_error) {
        return common::Result<std::filesystem::path>::failure(
            filesystem_error ? "failed to inspect playback report target: " +
                                   filesystem_error.message()
                             : "refusing to overwrite existing playback report: " +
                                   report_path.string());
    }
    std::ofstream output(report_path, std::ios::out | std::ios::trunc);
    if (!output) {
        return common::Result<std::filesystem::path>::failure(
            "failed to create trajectory playback report: " + report_path.string());
    }
    WriteReportJson(output, report);
    output.flush();
    if (!output.good()) {
        return common::Result<std::filesystem::path>::failure(
            "failed to write trajectory playback report: " + report_path.string());
    }
    output.close();
    return common::Result<std::filesystem::path>::success(report_path);
}

} // namespace rm_control::modules::trajectory
