#define private public
#include "emc/rs274ngc/rs274ngc_interp.hh"
#undef private

#include <cmath>
#include <cstring>
#include <filesystem>
#include <iostream>
#include <string>
#include <vector>

#include "canon_event_sink.hh"
#include "emc/ini/inifile.hh"
#include "emc/rs274ngc/interp_internal.hh"
#include "linuxcnc_hal_adapter.hh"
#include "linuxcnc_tool_adapter.hh"
#include "tooldata.hh"

#ifndef LINUXCNC_VENDOR_DIR
#error "LINUXCNC_VENDOR_DIR must point at wasm-port/vendor/linuxcnc"
#endif

namespace {

struct MachineCase {
    const char *label;
    const char *ini_rel;
    int expected_remaps;
    bool has_m430;
    double m428_switchkins;
    double m429_switchkins;
    double m430_switchkins;
};

struct DemoCase {
    const char *label;
    const char *rel;
    double expected_switchkins;
};

std::filesystem::path vendor_path(const std::string &rel)
{
    return std::filesystem::path(LINUXCNC_VENDOR_DIR) / rel;
}

void print_bool(const std::string &name, bool value)
{
    std::cout << name << "=" << (value ? 1 : 0) << "\n";
}

void print_interp_error(Interp &interp, const std::string &label, int rc)
{
    char error_buf[LINELEN] = {0};
    interp.error_text(rc, error_buf, sizeof(error_buf));
    std::cout << label << "_error_text=" << error_buf << "\n";
    std::cout << label << "_sequence_number=" << interp._setup.sequence_number << "\n";
    std::cout << label << "_linetext=" << interp._setup.linetext << "\n";
    std::cout << label << "_blocktext=" << interp._setup.blocktext << "\n";
}

constexpr int kMaxDemoFileSteps = 8192;

void seed_switchkins_hal()
{
    standalone::reset_hal_adapter();

    hal_data_u switchkins{};
    switchkins.f = 0.0;
    standalone::set_hal_value(standalone::HalValueKind::Pin, "motion.switchkins-type",
                              HAL_FLOAT, switchkins);
    standalone::set_hal_value(standalone::HalValueKind::Pin, "motion.analog-out-03",
                              HAL_FLOAT, switchkins);
    standalone::link_hal_pin_alias("motion.analog-out-03", "motion.switchkins-type");

    // Values mirror the vendored LinuxCNC xyzab-tdr sample INI/XML HAL defaults.
    const struct {
        const char *name;
        double value;
    } xyzab_tdr_defaults[] = {
        {"xyzab_tdr_kins.x-offset", -20.0},
        {"xyzab_tdr_kins.z-offset", -10.0},
        {"xyzab_tdr_kins.x-rot-point", -50.0},
        {"xyzab_tdr_kins.y-rot-point", -50.0},
        {"xyzab_tdr_kins.z-rot-point", -100.0},
    };
    for (const auto &entry : xyzab_tdr_defaults) {
        hal_data_u value{};
        value.f = entry.value;
        standalone::set_hal_value(standalone::HalValueKind::Pin, entry.name, HAL_FLOAT, value);
    }
}

void initialize_interp(Interp &interp)
{
    seed_switchkins_hal();
    standalone::reset_tool_adapter();
    standalone::reset_canon_events();

    interp._setup.length_units = CANON_UNITS_MM;
    interp._setup.distance_mode = DISTANCE_MODE::ABSOLUTE;
    interp._setup.ijk_distance_mode = DISTANCE_MODE::INCREMENTAL;
    interp._setup.feed_mode = FEED_MODE::UNITS_PER_MINUTE;
    interp._setup.plane = CANON_PLANE::XY;
    interp._setup.motion_mode = G_0;
    interp._setup.percent_flag = false;
    interp._setup.sequence_number = 0;
    interp._setup.parameter_occurrence = 0;
    interp._setup.num_spindles = 1;
    interp._setup.feature_set = FEATURE_INI_VARS | FEATURE_HAL_PIN_VARS;
    interp._setup.parameter_g73_peck_clearance = 1.0;
    interp._setup.parameter_g83_peck_clearance = 1.0;
    interp._setup.parameters[5599] = 1.0;
    std::memcpy(interp._readers, Interp::default_readers, sizeof(Interp::default_readers));
    interp.init_named_parameters();
}

bool read_hal_named(Interp &interp, const std::string &label, const char *name, double expected)
{
    int status = 0;
    double value = 0.0;
    const int rc = interp.find_named_param(name, &status, &value);
    std::cout << label << "_" << name << ": rc=" << rc << " found=" << status
              << " value=" << value << "\n";
    return rc == INTERP_OK && status == 1 && std::fabs(value - expected) < 0.000001;
}

bool parse_machine_remaps(Interp &interp, const MachineCase &machine)
{
    const auto ini_path = vendor_path(machine.ini_rel);
    const auto machine_dir = ini_path.parent_path();
    const auto remap_dir = machine_dir / "remap_subs";

    linuxcnc::IniFile ini(ini_path.string());
    const bool opened = static_cast<bool>(ini);
    print_bool(std::string(machine.label) + "_ini_open", opened);
    if (!opened) {
        return false;
    }

    std::snprintf(interp._setup.program_prefix, sizeof(interp._setup.program_prefix), "%s",
                  machine_dir.c_str());
    interp._setup.subroutines[0] = strstore(remap_dir.c_str());

    bool ok = true;
    if (auto tool_table = ini.findString("TOOL_TABLE", "EMCIO")) {
        const auto tool_path = machine_dir / *tool_table;
        tooldata_init(false);
        tooldata_set_db(DB_NOTUSED);
        const int tool_rc = tooldata_load(tool_path.c_str());
        std::cout << machine.label << "_tool_table_load=" << tool_rc << "\n";
        ok &= tool_rc == 0;
        ok &= interp.load_tool_table() == INTERP_OK;
    }

    int remap_count = 0;
    int line_number = 0;
    while (auto remap = ini.findString(++remap_count, "REMAP", "RS274NGC")) {
        line_number = ini.lineOf(remap_count, "REMAP", "RS274NGC").second;
        const int rc = interp.parse_remap(remap->c_str(), line_number);
        std::cout << machine.label << "_parse_remap_" << remap_count << "=" << rc << "\n";
        ok &= rc == INTERP_OK;
    }

    remap_count -= 1;
    std::cout << machine.label << "_parsed_remap_count=" << remap_count << "\n";
    ok &= remap_count == machine.expected_remaps;
    print_bool(std::string(machine.label) + "_remaps_ready", ok);
    return ok;
}

std::string command_label(const char *command)
{
    std::string value = command ? command : "";
    while (!value.empty() && (value.back() == '\n' || value.back() == '\r')) {
        value.pop_back();
    }
    return value;
}

bool execute_step(Interp &interp, const std::string &label, const char *command, double expected)
{
    const std::string step = command_label(command);
    int rc = interp.execute(command);
    std::cout << label << "_" << step << "_execute_0=" << rc << "\n";

    int finish_count = 0;
    while (rc == INTERP_EXECUTE_FINISH && finish_count < 8) {
        ++finish_count;
        rc = interp.execute();
        std::cout << label << "_" << step << "_execute_" << finish_count << "=" << rc << "\n";
    }

    std::cout << label << "_" << step << "_execute_finish_count=" << finish_count << "\n";
    bool ok = rc == INTERP_OK;
    ok &= read_hal_named(interp, label, "_hal[motion.switchkins-type]", expected);
    return ok;
}

bool execute_demo_file(const MachineCase &machine, const DemoCase &demo)
{
    Interp interp;
    initialize_interp(interp);

    const std::string label = std::string(machine.label) + "_" + demo.label;
    bool ok = parse_machine_remaps(interp, machine);

    const auto file_path = vendor_path(demo.rel);
    const int open_rc = interp.open(file_path.c_str());
    std::cout << label << "_file_open=" << open_rc << "\n";
    if (open_rc != INTERP_OK) {
        print_bool(label + "_linuxcnc_demo_file_execute", false);
        return false;
    }

    int read_count = 0;
    int execute_count = 0;
    int finish_count = 0;
    int final_rc = INTERP_OK;
    bool reached_exit = false;

    while (read_count < kMaxDemoFileSteps && execute_count < kMaxDemoFileSteps) {
        const int read_rc = interp.read();
        if (read_rc == INTERP_ENDFILE) {
            std::cout << label << "_file_read_eof=" << read_rc << "\n";
            final_rc = read_rc;
            break;
        }

        ++read_count;
        if ((read_rc != INTERP_OK) && (read_rc != INTERP_EXECUTE_FINISH)) {
            std::cout << label << "_file_read_" << read_count << "=" << read_rc << "\n";
            print_interp_error(interp, label + "_file_read", read_rc);
            final_rc = read_rc;
            ok = false;
            break;
        }

        int execute_rc = interp.execute();
        ++execute_count;
        final_rc = execute_rc;
        if (execute_rc == INTERP_EXECUTE_FINISH) {
            ++finish_count;
        }

        if ((execute_rc != INTERP_OK) && (execute_rc != INTERP_EXIT)) {
            if (execute_rc != INTERP_EXECUTE_FINISH) {
                std::cout << label << "_file_execute_" << execute_count << "="
                          << execute_rc << "\n";
                print_interp_error(interp, label + "_file_execute", execute_rc);
                ok = false;
                final_rc = execute_rc;
                break;
            }
        }
        if (execute_rc == INTERP_EXIT) {
            reached_exit = true;
            final_rc = execute_rc;
            break;
        }
    }

    std::cout << label << "_file_read_count=" << read_count << "\n";
    std::cout << label << "_file_execute_count=" << execute_count << "\n";
    std::cout << label << "_file_finish_count=" << finish_count << "\n";
    std::cout << label << "_file_final_rc=" << final_rc << "\n";
    print_bool(label + "_file_reached_exit", reached_exit);

    ok &= reached_exit;
    ok &= read_count > 0;
    ok &= execute_count > 0;
    ok &= read_count < kMaxDemoFileSteps;
    ok &= execute_count < kMaxDemoFileSteps;
    ok &= read_hal_named(interp, label, "_hal[motion.switchkins-type]",
                         demo.expected_switchkins);

    print_bool(label + "_linuxcnc_demo_file_execute", ok);
    return ok;
}

bool check_machine(const MachineCase &machine)
{
    Interp interp;
    initialize_interp(interp);

    bool ok = parse_machine_remaps(interp, machine);
    ok &= read_hal_named(interp, machine.label, "_hal[motion.switchkins-type]", 0.0);
    ok &= execute_step(interp, machine.label, "M429\n", machine.m429_switchkins);
    ok &= execute_step(interp, machine.label, "M428\n", machine.m428_switchkins);
    if (machine.has_m430) {
        ok &= execute_step(interp, machine.label, "M430\n", machine.m430_switchkins);
    }
    ok &= execute_step(interp, machine.label, "M429\n", machine.m429_switchkins);

    print_bool(std::string(machine.label) + "_linuxcnc_remap_execute_path", ok);
    return ok;
}

}  // namespace

int main()
{
    const MachineCase cases[] = {
        {
            "xyzac_trt",
            "configs/sim/axis/vismach/5axis/table-rotary-tilting/xyzac-trt.ini",
            3,
            true,
            1.0,
            0.0,
            2.0,
        },
        {
            "xyzbc_trt",
            "configs/sim/axis/vismach/5axis/table-rotary-tilting/xyzbc-trt.ini",
            3,
            true,
            1.0,
            0.0,
            2.0,
        },
        {
            "xyzab_tdr",
            "configs/sim/axis/vismach/5axis/table-dual-rotary/xyzab-tdr.ini",
            2,
            false,
            1.0,
            0.0,
            0.0,
        },
        {
            "bridgemill",
            "configs/sim/axis/vismach/5axis/bridgemill/5axis.ini",
            3,
            true,
            0.0,
            1.0,
            2.0,
        },
    };

    bool ok = true;
    for (const auto &machine : cases) {
        ok &= check_machine(machine);
    }

    const DemoCase demos[] = {
        {
            "xyzac_switchkins",
            "configs/sim/axis/vismach/5axis/table-rotary-tilting/demos/"
            "xyzac_switchkins.ngc",
            0.0,
        },
        {
            "xyzbc_switchkins",
            "configs/sim/axis/vismach/5axis/table-rotary-tilting/demos/"
            "xyzbc_switchkins.ngc",
            0.0,
        },
        {
            "xyzac_switchkins_test_1",
            "configs/sim/axis/vismach/5axis/table-rotary-tilting/demos/"
            "xyzac_switchkins_test_1.ngc",
            0.0,
        },
        {
            "xyzac_switchkins_test_2",
            "configs/sim/axis/vismach/5axis/table-rotary-tilting/demos/"
            "xyzac_switchkins_test_2.ngc",
            0.0,
        },
        {
            "xyzac_switchkins_test_3",
            "configs/sim/axis/vismach/5axis/table-rotary-tilting/demos/"
            "xyzac_switchkins_test_3.ngc",
            0.0,
        },
        {
            "boat_xyzac",
            "configs/sim/axis/vismach/5axis/table-rotary-tilting/demos/"
            "boat-xyzac.ngc",
            0.0,
        },
        {
            "boat_xyzbc",
            "configs/sim/axis/vismach/5axis/table-rotary-tilting/demos/"
            "boat-xyzbc.ngc",
            0.0,
        },
        {
            "impeller_7bl_xyzac",
            "configs/sim/axis/vismach/5axis/table-rotary-tilting/demos/"
            "impeller-7bl-xyzac.ngc",
            0.0,
        },
        {
            "xyzab_tdr_demo",
            "configs/sim/axis/vismach/5axis/table-dual-rotary/demos/"
            "xyzab-tdr-demo.ngc",
            1.0,
        },
        {
            "bridgemill_5axisgui",
            "configs/sim/axis/vismach/5axis/bridgemill/5axisgui.ngc",
            0.0,
        },
    };

    ok &= execute_demo_file(cases[0], demos[0]);
    ok &= execute_demo_file(cases[1], demos[1]);
    ok &= execute_demo_file(cases[0], demos[2]);
    ok &= execute_demo_file(cases[0], demos[3]);
    ok &= execute_demo_file(cases[0], demos[4]);
    ok &= execute_demo_file(cases[0], demos[5]);
    ok &= execute_demo_file(cases[1], demos[6]);
    ok &= execute_demo_file(cases[0], demos[7]);
    ok &= execute_demo_file(cases[2], demos[8]);
    ok &= execute_demo_file(cases[3], demos[9]);

    print_bool("fiveaxis_linuxcnc_remap_execute_path", ok);
    return ok ? 0 : 1;
}
