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

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

#include "canon_event_sink.hh"
#include "linuxcnc_hal_adapter.hh"
#include "linuxcnc_tool_adapter.hh"

namespace {

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

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");
}

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::ABSOLUTE;
    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 char *name, double expected)
{
    int status = 0;
    double value = 0.0;
    const int rc = interp.find_named_param(name, &status, &value);
    std::cout << name << ": rc=" << rc << " found=" << status << " value=" << value << "\n";
    return rc == INTERP_OK && status == 1 && std::fabs(value - expected) < 0.000001;
}

bool has_event(const std::string &needle)
{
    for (const auto &event : standalone::canon_events()) {
        if (event.find(needle) != std::string::npos) {
            return true;
        }
    }
    return false;
}

}  // namespace

int main()
{
    Interp interp;
    initialize_interp(interp);

    bool ok = true;
    ok &= read_hal_named(interp, "_hal[motion.switchkins-type]", 0.0);

    const int m68_to_tcp_rc = interp.execute("M68 E3 Q1\n");
    std::cout << "m68_to_tcp=" << m68_to_tcp_rc << "\n";
    ok &= m68_to_tcp_rc == INTERP_OK;
    ok &= read_hal_named(interp, "_hal[motion.switchkins-type]", 1.0);
    ok &= read_hal_named(interp, "_hal[motion.analog-out-03]", 1.0);

    const int m66_sync_rc = interp.execute("M66 E0 L0\n");
    std::cout << "m66_sync=" << m66_sync_rc << "\n";
    std::cout << "m66_input_flag=" << interp._setup.input_flag << "\n";
    std::cout << "m66_input_index=" << interp._setup.input_index << "\n";
    std::cout << "m66_input_digital=" << interp._setup.input_digital << "\n";
    ok &= m66_sync_rc == INTERP_EXECUTE_FINISH;
    ok &= interp._setup.input_flag;
    ok &= interp._setup.input_index == 0;
    ok &= !interp._setup.input_digital;

    const int m68_to_identity_rc = interp.execute("M68 E3 Q0\n");
    std::cout << "m68_to_identity=" << m68_to_identity_rc << "\n";
    ok &= m68_to_identity_rc == INTERP_OK;
    ok &= read_hal_named(interp, "_hal[motion.switchkins-type]", 0.0);

    print_bool("canon_has_m68_tcp", has_event("SET_AUX_OUTPUT_VALUE index=3 value=1"));
    print_bool("canon_has_m66_sync", has_event("WAIT index=0 input_type=0 wait_type=0"));
    print_bool("canon_has_m68_identity", has_event("SET_AUX_OUTPUT_VALUE index=3 value=0"));
    ok &= has_event("SET_AUX_OUTPUT_VALUE index=3 value=1");
    ok &= has_event("WAIT index=0 input_type=0 wait_type=0");
    ok &= has_event("SET_AUX_OUTPUT_VALUE index=3 value=0");

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