/**
 * @file vendor_drag_trajectory_parser.cpp
 * @brief RealMan 厂商拖动轨迹 JSONL 的严格离线解析实现。
 *
 * @author 唐永康
 * @date 2026-07-10
 */
#include "rm_control/modules/trajectory/vendor_drag_trajectory_parser.h"

#include "rm_control/common/rm75_constants.h"
#include "rm_control/modules/trajectory/sha256_file_checksum.h"

#include <array>
#include <cmath>
#include <cstdint>
#include <fstream>
#include <limits>
#include <string>
#include <string_view>

namespace rm_control::modules::trajectory {
namespace {

using RawJointPoint = std::array<std::int64_t, common::kRm75JointCount>;

constexpr std::size_t kMaximumVendorJsonLineLength = 1024U;

struct FileSnapshot {
    std::uintmax_t size_bytes = 0U;
    std::filesystem::file_time_type modification_time{};
};

std::string PathForError(const std::filesystem::path &path) {
    return path.string();
}

bool IsJsonWhitespace(char character) {
    return character == ' ' || character == '\t' || character == '\r' || character == '\n';
}

bool IsAsciiDigit(char character) {
    return character >= '0' && character <= '9';
}

bool IsBlankJsonLine(std::string_view line) {
    for (const char character : line) {
        if (!IsJsonWhitespace(character)) {
            return false;
        }
    }
    return true;
}

class JsonLineCursor {
public:
    explicit JsonLineCursor(std::string_view text) : text_(text) {}

    bool Consume(char expected) {
        SkipWhitespace();
        if (position_ >= text_.size() || text_[position_] != expected) {
            return false;
        }
        ++position_;
        return true;
    }

    bool ConsumeLiteral(std::string_view expected) {
        SkipWhitespace();
        if (text_.substr(position_, expected.size()) != expected) {
            return false;
        }
        position_ += expected.size();
        return true;
    }

    bool IsAtEnd() {
        SkipWhitespace();
        return position_ == text_.size();
    }

    common::Result<std::int64_t> ParseInteger() {
        SkipWhitespace();
        const bool is_negative = ConsumeMinusSign();
        if (position_ >= text_.size() || !IsAsciiDigit(text_[position_])) {
            return common::Result<std::int64_t>::failure("expected a JSON integer");
        }
        if (text_[position_] == '0' && position_ + 1U < text_.size() &&
            IsAsciiDigit(text_[position_ + 1U])) {
            return common::Result<std::int64_t>::failure(
                "JSON integer must not contain a leading zero");
        }
        return ParseIntegerMagnitude(is_negative);
    }

private:
    void SkipWhitespace() {
        while (position_ < text_.size() && IsJsonWhitespace(text_[position_])) {
            ++position_;
        }
    }

    bool ConsumeMinusSign() {
        if (position_ < text_.size() && text_[position_] == '-') {
            ++position_;
            return true;
        }
        return false;
    }

    common::Result<std::int64_t> ParseIntegerMagnitude(bool is_negative) {
        const std::uint64_t positive_limit =
            static_cast<std::uint64_t>(std::numeric_limits<std::int64_t>::max());
        const std::uint64_t magnitude_limit = is_negative ? positive_limit + 1U : positive_limit;
        std::uint64_t magnitude = 0U;
        while (position_ < text_.size() && IsAsciiDigit(text_[position_])) {
            const std::uint64_t digit = static_cast<std::uint64_t>(text_[position_] - '0');
            if (magnitude > (magnitude_limit - digit) / 10U) {
                return common::Result<std::int64_t>::failure("JSON integer exceeds int64 range");
            }
            magnitude = magnitude * 10U + digit;
            ++position_;
        }
        if (is_negative && magnitude == positive_limit + 1U) {
            return common::Result<std::int64_t>::success(
                std::numeric_limits<std::int64_t>::min());
        }
        const auto signed_magnitude = static_cast<std::int64_t>(magnitude);
        return common::Result<std::int64_t>::success(
            is_negative ? -signed_magnitude : signed_magnitude);
    }

    std::string_view text_;
    std::size_t position_ = 0U;
};

template <typename T>
common::Result<T> LineFailure(std::size_t line_number, const std::string &message) {
    return common::Result<T>::failure(
        "vendor trajectory line " + std::to_string(line_number) + ": " + message);
}

common::Result<RawJointPoint> ParseVendorPointLine(std::string_view line,
                                                   std::size_t line_number) {
    JsonLineCursor cursor(line);
    if (!cursor.Consume('{') || !cursor.ConsumeLiteral("\"point\"") ||
        !cursor.Consume(':') || !cursor.Consume('[')) {
        return LineFailure<RawJointPoint>(
            line_number, "expected exact object format {\"point\":[...]}");
    }

    RawJointPoint point{};
    for (std::size_t joint_index = 0U; joint_index < point.size(); ++joint_index) {
        const auto raw_value = cursor.ParseInteger();
        if (!raw_value.ok) {
            return LineFailure<RawJointPoint>(
                line_number, "joint " + std::to_string(joint_index + 1U) + " " +
                                 raw_value.error_message);
        }
        point[joint_index] = raw_value.value;
        if (joint_index + 1U < point.size() && !cursor.Consume(',')) {
            return LineFailure<RawJointPoint>(line_number, "expected exactly seven integers");
        }
    }

    if (!cursor.Consume(']') || !cursor.Consume('}') || !cursor.IsAtEnd()) {
        return LineFailure<RawJointPoint>(
            line_number, "expected exactly seven integers and no additional fields");
    }
    return common::Result<RawJointPoint>::success(point);
}

common::Result<FileSnapshot> CaptureFileSnapshot(const std::filesystem::path &file_path) {
    std::error_code filesystem_error;
    const std::filesystem::file_status status =
        std::filesystem::symlink_status(file_path, filesystem_error);
    if (filesystem_error) {
        return common::Result<FileSnapshot>::failure(
            "failed to inspect vendor trajectory " + PathForError(file_path) + ": " +
            filesystem_error.message());
    }
    if (!std::filesystem::is_regular_file(status)) {
        return common::Result<FileSnapshot>::failure(
            "vendor trajectory must be a regular file and not a symbolic link: " +
            PathForError(file_path));
    }

    FileSnapshot snapshot;
    snapshot.size_bytes = std::filesystem::file_size(file_path, filesystem_error);
    if (filesystem_error) {
        return common::Result<FileSnapshot>::failure(
            "failed to read vendor trajectory size " + PathForError(file_path) + ": " +
            filesystem_error.message());
    }
    snapshot.modification_time = std::filesystem::last_write_time(file_path, filesystem_error);
    if (filesystem_error) {
        return common::Result<FileSnapshot>::failure(
            "failed to read vendor trajectory modification time " + PathForError(file_path) +
            ": " + filesystem_error.message());
    }
    return common::Result<FileSnapshot>::success(snapshot);
}

common::Result<void> VerifyFileSnapshot(const std::filesystem::path &file_path,
                                        const FileSnapshot &expected,
                                        const std::string &operation) {
    const auto actual = CaptureFileSnapshot(file_path);
    if (!actual.ok) {
        return common::Result<void>::failure(actual.error_message);
    }
    if (actual.value.size_bytes != expected.size_bytes ||
        actual.value.modification_time != expected.modification_time) {
        return common::Result<void>::failure(
            "vendor trajectory changed during " + operation + ": " + PathForError(file_path));
    }
    return common::Result<void>::success();
}

common::Result<std::string> CalculateStableChecksum(const std::filesystem::path &file_path,
                                                    const FileSnapshot &snapshot,
                                                    const std::string &operation) {
    const auto checksum = CalculateFileSha256(file_path);
    if (!checksum.ok) {
        return checksum;
    }
    const auto unchanged = VerifyFileSnapshot(file_path, snapshot, operation);
    if (!unchanged.ok) {
        return common::Result<std::string>::failure(unchanged.error_message);
    }
    return checksum;
}

common::Result<common::TrajectorySample> MakeTrajectorySample(
    const RawJointPoint &raw_point,
    std::size_t sample_index,
    std::int64_t sample_period_ms,
    std::int64_t raw_units_per_degree) {
    const auto maximum_index = static_cast<std::size_t>(
        std::numeric_limits<long long>::max() / sample_period_ms);
    if (sample_index > maximum_index) {
        return common::Result<common::TrajectorySample>::failure(
            "derived timestamp exceeds int64 millisecond range");
    }

    common::TrajectorySample sample;
    sample.sample_index = sample_index;
    sample.elapsed_ms = static_cast<long long>(sample_index) * sample_period_ms;
    sample.source = kRealmanVendorDragTrajectorySource;
    sample.joints_deg.reserve(common::kRm75JointCount);
    for (const std::int64_t raw_joint : raw_point) {
        const double joint_deg = static_cast<double>(raw_joint) /
                                 static_cast<double>(raw_units_per_degree);
        if (!std::isfinite(joint_deg) ||
            std::abs(joint_deg) > static_cast<double>(std::numeric_limits<float>::max())) {
            return common::Result<common::TrajectorySample>::failure(
                "converted joint angle exceeds finite float range");
        }
        sample.joints_deg.push_back(static_cast<float>(joint_deg));
    }
    return common::Result<common::TrajectorySample>::success(sample);
}

common::Result<std::vector<common::TrajectorySample>> ReadVendorSamples(
    const std::filesystem::path &file_path,
    std::int64_t sample_period_ms,
    std::int64_t raw_units_per_degree) {
    std::ifstream input(file_path, std::ios::binary);
    if (!input) {
        return common::Result<std::vector<common::TrajectorySample>>::failure(
            "failed to open vendor trajectory for parsing: " + PathForError(file_path));
    }

    std::vector<common::TrajectorySample> samples;
    std::string line;
    std::size_t line_number = 0U;
    while (std::getline(input, line)) {
        ++line_number;
        if (line.size() > kMaximumVendorJsonLineLength) {
            return LineFailure<std::vector<common::TrajectorySample>>(
                line_number, "line exceeds the strict 1024-byte limit");
        }
        if (IsBlankJsonLine(line)) {
            return LineFailure<std::vector<common::TrajectorySample>>(
                line_number, "blank lines are not allowed");
        }
        const auto raw_point = ParseVendorPointLine(line, line_number);
        if (!raw_point.ok) {
            return common::Result<std::vector<common::TrajectorySample>>::failure(
                raw_point.error_message);
        }
        const auto sample = MakeTrajectorySample(
            raw_point.value, samples.size(), sample_period_ms, raw_units_per_degree);
        if (!sample.ok) {
            return LineFailure<std::vector<common::TrajectorySample>>(
                line_number, sample.error_message);
        }
        samples.push_back(sample.value);
    }
    if (!input.eof()) {
        return common::Result<std::vector<common::TrajectorySample>>::failure(
            "failed while reading vendor trajectory: " + PathForError(file_path));
    }
    if (samples.empty()) {
        return common::Result<std::vector<common::TrajectorySample>>::failure(
            "vendor trajectory contains no samples: " + PathForError(file_path));
    }
    return common::Result<std::vector<common::TrajectorySample>>::success(samples);
}

common::Result<void> ValidateParseArguments(const std::filesystem::path &file_path,
                                            std::int64_t sample_period_ms,
                                            std::int64_t raw_units_per_degree) {
    if (file_path.empty()) {
        return common::Result<void>::failure("vendor trajectory path must not be empty");
    }
    if (sample_period_ms <= 0) {
        return common::Result<void>::failure(
            "vendor trajectory sample_period_ms must be greater than zero");
    }
    if (raw_units_per_degree <= 0) {
        return common::Result<void>::failure(
            "vendor trajectory raw_units_per_degree must be greater than zero");
    }
    return common::Result<void>::success();
}

} // namespace

common::Result<VendorDragTrajectoryParseResult> ParseVendorDragTrajectoryJsonLines(
    const std::filesystem::path &file_path,
    std::int64_t sample_period_ms,
    std::int64_t raw_units_per_degree) {
    const auto arguments =
        ValidateParseArguments(file_path, sample_period_ms, raw_units_per_degree);
    if (!arguments.ok) {
        return common::Result<VendorDragTrajectoryParseResult>::failure(
            arguments.error_message);
    }

    const auto initial_snapshot = CaptureFileSnapshot(file_path);
    if (!initial_snapshot.ok) {
        return common::Result<VendorDragTrajectoryParseResult>::failure(
            initial_snapshot.error_message);
    }
    const auto checksum_before_parse = CalculateStableChecksum(
        file_path, initial_snapshot.value, "initial SHA-256 calculation");
    if (!checksum_before_parse.ok) {
        return common::Result<VendorDragTrajectoryParseResult>::failure(
            checksum_before_parse.error_message);
    }

    const auto samples = ReadVendorSamples(file_path, sample_period_ms, raw_units_per_degree);
    const auto unchanged_after_parse =
        VerifyFileSnapshot(file_path, initial_snapshot.value, "JSONL parsing");
    if (!unchanged_after_parse.ok) {
        return common::Result<VendorDragTrajectoryParseResult>::failure(
            unchanged_after_parse.error_message);
    }
    if (!samples.ok) {
        return common::Result<VendorDragTrajectoryParseResult>::failure(samples.error_message);
    }

    const auto checksum_after_parse = CalculateStableChecksum(
        file_path, initial_snapshot.value, "final SHA-256 calculation");
    if (!checksum_after_parse.ok) {
        return common::Result<VendorDragTrajectoryParseResult>::failure(
            checksum_after_parse.error_message);
    }
    if (checksum_after_parse.value != checksum_before_parse.value) {
        return common::Result<VendorDragTrajectoryParseResult>::failure(
            "vendor trajectory content changed while it was being parsed: " +
            PathForError(file_path));
    }

    VendorDragTrajectoryParseResult result;
    result.source_file = file_path;
    result.samples = samples.value;
    result.trajectory_point_count = result.samples.size();
    result.source_file_size_bytes = initial_snapshot.value.size_bytes;
    result.source_file_sha256 = checksum_after_parse.value;
    result.sample_period_ms = sample_period_ms;
    result.raw_units_per_degree = raw_units_per_degree;
    return common::Result<VendorDragTrajectoryParseResult>::success(result);
}

} // namespace rm_control::modules::trajectory
