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

#include <cmath>
#include <cstdint>
#include <filesystem>
#include <fstream>
#include <iostream>
#include <limits>
#include <string>
#include <utility>
#include <vector>

#ifndef RM_TEST_OUTPUT_DIR
#error "RM_TEST_OUTPUT_DIR must be defined so tests cannot write into the source tree"
#endif

namespace {

namespace trajectory = rm_control::modules::trajectory;

class VendorDragTrajectoryParserTestSuite {
public:
    int Run() {
        std::error_code filesystem_error;
        std::filesystem::create_directories(TestOutputRoot(), filesystem_error);
        Expect(!filesystem_error,
               "test output root should be created under RM_TEST_OUTPUT_DIR: " +
                   filesystem_error.message());

        TestParsesStrictSevenJointJsonLines();
        TestUsesExplicitRawUnitScale();
        TestRejectsInvalidArgumentsAndTimestampOverflow();
        TestRejectsMalformedVendorLines();
        TestRejectsEmptyAndNonRegularInputs();
        return failures_;
    }

private:
    int failures_ = 0;

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

    std::filesystem::path TestOutputRoot() const {
        return std::filesystem::path(RM_TEST_OUTPUT_DIR) /
               "vendor_drag_trajectory_parser_tests";
    }

    std::filesystem::path TestFile(const std::string &name) const {
        return TestOutputRoot() / name;
    }

    bool WriteBinaryFile(const std::filesystem::path &path, const std::string &content) {
        std::ofstream output(path, std::ios::binary | std::ios::trunc);
        output.write(content.data(), static_cast<std::streamsize>(content.size()));
        return static_cast<bool>(output);
    }

    bool NearlyEqual(float actual, float expected) const {
        return std::abs(actual - expected) <= 0.00001F;
    }

    void TestParsesStrictSevenJointJsonLines() {
        const std::string content =
            "{\"point\":[1000,-2000,0,500,250,-750,3000]}\r\n"
            " { \"point\" : [2000,-1000,1000,1500,1250,250,4000] } \r\n";
        const auto path = TestFile("valid_crlf.txt");
        Expect(WriteBinaryFile(path, content), "valid CRLF fixture should be written");

        const auto result = trajectory::ParseVendorDragTrajectoryJsonLines(path, 20);
        Expect(result.ok, "valid vendor JSONL should parse: " + result.error_message);
        if (!result.ok) {
            return;
        }

        Expect(result.value.source_file == path, "result should preserve the source path");
        Expect(result.value.trajectory_point_count == 2U,
               "result should report two trajectory points");
        Expect(result.value.samples.size() == 2U, "result should contain two samples");
        Expect(result.value.source_file_size_bytes == content.size(),
               "result should report the original byte size");
        Expect(result.value.source_file_sha256 ==
                   "b2274aa15734a7373e528fef19d4f3676b1e8f22a03d60dd56c36a2aa8997080",
               "result should report the SHA-256 of the exact CRLF bytes");
        Expect(result.value.timestamp_is_derived,
               "vendor timestamps should be explicitly marked derived");
        Expect(result.value.sample_period_ms == 20,
               "result should preserve the explicit sample period");
        Expect(result.value.raw_units_per_degree == 1000,
               "default vendor scale should be 1000 raw units per degree");
        Expect(result.value.source == trajectory::kRealmanVendorDragTrajectorySource,
               "result should identify the vendor source format");

        const auto &first = result.value.samples[0];
        const auto &second = result.value.samples[1];
        Expect(first.sample_index == 0U && second.sample_index == 1U,
               "sample indexes should be contiguous and start at zero");
        Expect(first.elapsed_ms == 0 && second.elapsed_ms == 20,
               "timestamps should be derived from the explicit period");
        Expect(first.joints_deg.size() == 7U && second.joints_deg.size() == 7U,
               "every RM75 sample should contain seven joints");
        Expect(NearlyEqual(first.joints_deg[0], 1.0F) &&
                   NearlyEqual(first.joints_deg[1], -2.0F) &&
                   NearlyEqual(first.joints_deg[6], 3.0F),
               "raw integer joints should convert to degree without changing order");
        Expect(first.source == trajectory::kRealmanVendorDragTrajectorySource &&
                   second.source == trajectory::kRealmanVendorDragTrajectorySource,
               "every sample should retain vendor source provenance");
    }

    void TestUsesExplicitRawUnitScale() {
        const auto path = TestFile("explicit_scale.txt");
        Expect(WriteBinaryFile(path, "{\"point\":[2000,0,0,0,0,0,0]}\n"),
               "explicit-scale fixture should be written");

        const auto result = trajectory::ParseVendorDragTrajectoryJsonLines(path, 5, 2000);
        Expect(result.ok, "positive explicit raw unit scale should parse");
        if (result.ok) {
            Expect(result.value.raw_units_per_degree == 2000,
                   "result should preserve the explicit raw scale");
            Expect(NearlyEqual(result.value.samples[0].joints_deg[0], 1.0F),
                   "explicit raw scale should control degree conversion");
        }
    }

    void TestRejectsInvalidArgumentsAndTimestampOverflow() {
        const auto path = TestFile("arguments.txt");
        const std::string three_points =
            "{\"point\":[0,0,0,0,0,0,0]}\n"
            "{\"point\":[0,0,0,0,0,0,0]}\n"
            "{\"point\":[0,0,0,0,0,0,0]}\n";
        Expect(WriteBinaryFile(path, three_points), "argument fixture should be written");

        Expect(!trajectory::ParseVendorDragTrajectoryJsonLines({}, 1).ok,
               "empty input path should be rejected");
        Expect(!trajectory::ParseVendorDragTrajectoryJsonLines(path, 0).ok,
               "zero sample period should be rejected");
        Expect(!trajectory::ParseVendorDragTrajectoryJsonLines(path, -1).ok,
               "negative sample period should be rejected");
        Expect(!trajectory::ParseVendorDragTrajectoryJsonLines(path, 1, 0).ok,
               "zero raw units per degree should be rejected");
        Expect(!trajectory::ParseVendorDragTrajectoryJsonLines(path, 1, -1000).ok,
               "negative raw units per degree should be rejected");
        Expect(!trajectory::ParseVendorDragTrajectoryJsonLines(
                    path, std::numeric_limits<std::int64_t>::max()).ok,
               "derived timestamp multiplication overflow should be rejected");
    }

    void TestRejectsMalformedVendorLines() {
        const std::vector<std::pair<std::string, std::string>> invalid_files = {
            {"blank_line.txt",
             "{\"point\":[0,0,0,0,0,0,0]}\n\n{\"point\":[0,0,0,0,0,0,0]}\n"},
            {"unknown_key.txt", "{\"joints\":[0,0,0,0,0,0,0]}\n"},
            {"six_joints.txt", "{\"point\":[0,0,0,0,0,0]}\n"},
            {"eight_joints.txt", "{\"point\":[0,0,0,0,0,0,0,0]}\n"},
            {"floating_joint.txt", "{\"point\":[0,0,0,0,0,0,1.5]}\n"},
            {"leading_zero.txt", "{\"point\":[00,0,0,0,0,0,0]}\n"},
            {"positive_sign.txt", "{\"point\":[+1,0,0,0,0,0,0]}\n"},
            {"extra_field.txt", "{\"point\":[0,0,0,0,0,0,0],\"time\":1}\n"},
            {"array_root.txt", "[[0,0,0,0,0,0,0]]\n"},
            {"int64_overflow.txt",
             "{\"point\":[9223372036854775808,0,0,0,0,0,0]}\n"},
            {"negative_int64_overflow.txt",
             "{\"point\":[-9223372036854775809,0,0,0,0,0,0]}\n"},
            {"trailing_object.txt",
             "{\"point\":[0,0,0,0,0,0,0]}{\"point\":[0,0,0,0,0,0,0]}\n"},
        };

        for (const auto &fixture : invalid_files) {
            const auto path = TestFile(fixture.first);
            Expect(WriteBinaryFile(path, fixture.second),
                   "malformed fixture should be written: " + fixture.first);
            const auto result = trajectory::ParseVendorDragTrajectoryJsonLines(path, 10);
            Expect(!result.ok, "malformed vendor line should be rejected: " + fixture.first);
            if (!result.ok && fixture.first != "blank_line.txt") {
                Expect(result.error_message.find("line 1") != std::string::npos,
                       "format error should include physical line context: " + fixture.first);
            }
        }
    }

    void TestRejectsEmptyAndNonRegularInputs() {
        const auto empty_path = TestFile("empty.txt");
        Expect(WriteBinaryFile(empty_path, ""), "empty fixture should be written");
        Expect(!trajectory::ParseVendorDragTrajectoryJsonLines(empty_path, 10).ok,
               "empty vendor trajectory should be rejected");
        Expect(!trajectory::ParseVendorDragTrajectoryJsonLines(TestOutputRoot(), 10).ok,
               "directory input should be rejected");
        Expect(!trajectory::ParseVendorDragTrajectoryJsonLines(
                    TestFile("missing.txt"), 10).ok,
               "missing input should be rejected");
    }
};

} // namespace

int main() {
    VendorDragTrajectoryParserTestSuite suite;
    const int failures = suite.Run();
    if (failures == 0) {
        std::cout << "[PASS] rm_vendor_drag_trajectory_parser_tests\n";
        return 0;
    }

    std::cerr << failures << " test(s) failed\n";
    return 1;
}
