#include "linuxcnc_hal_adapter.hh"

#include <cstdarg>
#include <cstdio>
#include <new>
#include <sstream>
#include <string>
#include <unordered_map>
#include <vector>

namespace {

struct HalEntry {
    hal_type_t type = HAL_TYPE_UNINITIALIZED;
    hal_data_u value{};
    bool connected = true;
};

std::unordered_map<std::string, HalEntry> &pins()
{
    static std::unordered_map<std::string, HalEntry> values;
    return values;
}

std::unordered_map<std::string, HalEntry> &signals()
{
    static std::unordered_map<std::string, HalEntry> values;
    return values;
}

std::unordered_map<std::string, HalEntry> &params()
{
    static std::unordered_map<std::string, HalEntry> values;
    return values;
}

std::vector<void *> &hal_allocations()
{
    static std::vector<void *> values;
    return values;
}

std::unordered_map<std::string, HalEntry> &values_for(standalone::HalValueKind kind)
{
    switch (kind) {
    case standalone::HalValueKind::Pin:
        return pins();
    case standalone::HalValueKind::Signal:
        return signals();
    case standalone::HalValueKind::Param:
        return params();
    }
    return pins();
}

std::unordered_map<std::string, std::vector<std::string>> &pin_aliases()
{
    static std::unordered_map<std::string, std::vector<std::string>> values;
    return values;
}

std::unordered_map<int, int> &digital_inputs()
{
    static std::unordered_map<int, int> values;
    return values;
}

std::unordered_map<int, double> &analog_inputs()
{
    static std::unordered_map<int, double> values;
    return values;
}

int lookup_hal_value(const char *name, std::unordered_map<std::string, HalEntry> &values,
                     hal_type_t *type, hal_data_u **ptr, bool *connected)
{
    if (!name || !type || !ptr) {
        return -1;
    }

    auto it = values.find(name);
    if (it == values.end()) {
        return -1;
    }

    *type = it->second.type;
    *ptr = &it->second.value;
    if (connected) {
        *connected = it->second.connected;
    }
    return 0;
}

}  // namespace

namespace standalone {

void reset_hal_adapter()
{
    pins().clear();
    signals().clear();
    params().clear();
    pin_aliases().clear();
    digital_inputs().clear();
    analog_inputs().clear();
}

void set_hal_value(HalValueKind kind, const char *name, hal_type_t type, hal_data_u value,
                   bool connected)
{
    values_for(kind)[name] = HalEntry{type, value, connected};
}

void link_hal_pin_alias(const char *source, const char *alias)
{
    if (!source || !alias) {
        return;
    }
    pin_aliases()[source].push_back(alias);
}

void sync_motion_analog_output(int index, double value)
{
    hal_data_u analog{};
    analog.f = value;

    std::ostringstream out_name;
    out_name << "motion.analog-out-" << index;
    set_hal_value(HalValueKind::Pin, out_name.str().c_str(), HAL_FLOAT, analog);

    std::ostringstream padded_out_name;
    padded_out_name << "motion.analog-out-0" << index;
    set_hal_value(HalValueKind::Pin, padded_out_name.str().c_str(), HAL_FLOAT, analog);

    const std::string names[] = {out_name.str(), padded_out_name.str()};
    for (const auto &name : names) {
        auto aliases = pin_aliases().find(name);
        if (aliases == pin_aliases().end()) {
            continue;
        }
        for (const auto &alias : aliases->second) {
            set_hal_value(HalValueKind::Pin, alias.c_str(), HAL_FLOAT, analog);
        }
    }
}

void set_external_digital_input(int index, int value)
{
    digital_inputs()[index] = value ? 1 : 0;
}

void set_external_analog_input(int index, double value)
{
    analog_inputs()[index] = value;
}

int external_digital_input(int index, int def)
{
    auto it = digital_inputs().find(index);
    return it == digital_inputs().end() ? def : it->second;
}

double external_analog_input(int index, double def)
{
    auto it = analog_inputs().find(index);
    return it == analog_inputs().end() ? def : it->second;
}

}  // namespace standalone

int hal_init(const char *)
{
    return 1;
}

int hal_ready(int)
{
    return 0;
}

int hal_exit(int)
{
    return 0;
}

void *hal_malloc(long int size)
{
    if (size <= 0) {
        return nullptr;
    }
    void *ptr = ::operator new(static_cast<std::size_t>(size), std::nothrow);
    if (ptr) {
        hal_allocations().push_back(ptr);
    }
    return ptr;
}

int hal_pin_bit_new(const char *, hal_pin_dir_t, hal_bit_t **data_ptr_addr, int)
{
    if (!data_ptr_addr) {
        return -1;
    }
    *data_ptr_addr = new hal_bit_t{};
    return 0;
}

int hal_pin_float_new(const char *, hal_pin_dir_t, hal_float_t **data_ptr_addr, int)
{
    if (!data_ptr_addr) {
        return -1;
    }
    *data_ptr_addr = new hal_float_t{};
    return 0;
}

int hal_pin_u32_new(const char *, hal_pin_dir_t, hal_u32_t **data_ptr_addr, int)
{
    if (!data_ptr_addr) {
        return -1;
    }
    *data_ptr_addr = new hal_u32_t{};
    return 0;
}

int hal_pin_s32_new(const char *, hal_pin_dir_t, hal_s32_t **data_ptr_addr, int)
{
    if (!data_ptr_addr) {
        return -1;
    }
    *data_ptr_addr = new hal_s32_t{};
    return 0;
}

int hal_pin_u64_new(const char *, hal_pin_dir_t, hal_u64_t **data_ptr_addr, int)
{
    if (!data_ptr_addr) {
        return -1;
    }
    *data_ptr_addr = new hal_u64_t{};
    return 0;
}

int hal_pin_s64_new(const char *, hal_pin_dir_t, hal_s64_t **data_ptr_addr, int)
{
    if (!data_ptr_addr) {
        return -1;
    }
    *data_ptr_addr = new hal_s64_t{};
    return 0;
}

template <typename T, int (*Create)(const char *, hal_pin_dir_t, T **, int)>
int hal_pin_newf_impl(hal_pin_dir_t dir, T **data_ptr_addr, int comp_id, const char *fmt,
                      va_list ap)
{
    if (!fmt) {
        return -1;
    }
    char name[HAL_NAME_LEN * 2]{};
    vsnprintf(name, sizeof(name), fmt, ap);
    return Create(name, dir, data_ptr_addr, comp_id);
}

int hal_pin_bit_newf(hal_pin_dir_t dir, hal_bit_t **data_ptr_addr, int comp_id,
                     const char *fmt, ...)
{
    va_list ap;
    va_start(ap, fmt);
    const int rc = hal_pin_newf_impl<hal_bit_t, hal_pin_bit_new>(
        dir, data_ptr_addr, comp_id, fmt, ap);
    va_end(ap);
    return rc;
}

int hal_pin_float_newf(hal_pin_dir_t dir, hal_float_t **data_ptr_addr, int comp_id,
                       const char *fmt, ...)
{
    va_list ap;
    va_start(ap, fmt);
    const int rc = hal_pin_newf_impl<hal_float_t, hal_pin_float_new>(
        dir, data_ptr_addr, comp_id, fmt, ap);
    va_end(ap);
    return rc;
}

int hal_pin_u32_newf(hal_pin_dir_t dir, hal_u32_t **data_ptr_addr, int comp_id,
                     const char *fmt, ...)
{
    va_list ap;
    va_start(ap, fmt);
    const int rc = hal_pin_newf_impl<hal_u32_t, hal_pin_u32_new>(
        dir, data_ptr_addr, comp_id, fmt, ap);
    va_end(ap);
    return rc;
}

int hal_pin_s32_newf(hal_pin_dir_t dir, hal_s32_t **data_ptr_addr, int comp_id,
                     const char *fmt, ...)
{
    va_list ap;
    va_start(ap, fmt);
    const int rc = hal_pin_newf_impl<hal_s32_t, hal_pin_s32_new>(
        dir, data_ptr_addr, comp_id, fmt, ap);
    va_end(ap);
    return rc;
}

int hal_pin_u64_newf(hal_pin_dir_t dir, hal_u64_t **data_ptr_addr, int comp_id,
                     const char *fmt, ...)
{
    va_list ap;
    va_start(ap, fmt);
    const int rc = hal_pin_newf_impl<hal_u64_t, hal_pin_u64_new>(
        dir, data_ptr_addr, comp_id, fmt, ap);
    va_end(ap);
    return rc;
}

int hal_pin_s64_newf(hal_pin_dir_t dir, hal_s64_t **data_ptr_addr, int comp_id,
                     const char *fmt, ...)
{
    va_list ap;
    va_start(ap, fmt);
    const int rc = hal_pin_newf_impl<hal_s64_t, hal_pin_s64_new>(
        dir, data_ptr_addr, comp_id, fmt, ap);
    va_end(ap);
    return rc;
}

int hal_param_float_newf(hal_pin_dir_t, hal_float_t *data_addr, int, const char *, ...)
{
    if (!data_addr) {
        return -1;
    }
    *data_addr = 0.0;
    return 0;
}

int hal_get_pin_value_by_name(const char *name, hal_type_t *type, hal_data_u **ptr,
                              bool *connected)
{
    return lookup_hal_value(name, pins(), type, ptr, connected);
}

int hal_get_signal_value_by_name(const char *name, hal_type_t *type, hal_data_u **ptr,
                                 bool *connected)
{
    return lookup_hal_value(name, signals(), type, ptr, connected);
}

int hal_get_param_value_by_name(const char *name, hal_type_t *type, hal_data_u **ptr)
{
    return lookup_hal_value(name, params(), type, ptr, nullptr);
}
