/**
 * RH56F1 EtherCAT Hand Controller - C API Wrapper Implementation (Multi-Instance)
 * 
 * Supports multiple hand controller instances on different EtherCAT masters.
 * Each instance has its own master, domain, slave, and control thread.
 */

#include "rh56f1_wrapper.h"
#include "rh56f1_source_hash.h"
#include <cerrno>
#include <csignal>
#include <cstdio>
#include <cstring>
#include <sys/resource.h>
#include <sys/time.h>
#include <sys/types.h>
#include <unistd.h>
#include <ctime>
#include <sys/mman.h>
#include <sched.h>
#include <pthread.h>
#include <atomic>
#include <chrono>
#include <thread>

#include "ecrt.h"

// Set to true to print EtherCAT/control loop messages to console; false to silence.
static const bool RH56F1_VERBOSE = false;

// Constants
static const uint32_t PERIOD_NS = 2000000;  // 2ms = 500Hz
static const uint32_t NSEC_PER_SEC = 1000000000;
static const uint32_t VENDOR_ID = 0x00000001;
static const uint32_t PRODUCT_ID = 0x00009252;

// PDO configuration
static ec_pdo_entry_info_t slave_0_pdo_entries[] = {
    {0x7000, 0x01, 16}, {0x7000, 0x02, 16}, {0x7000, 0x03, 16}, {0x7000, 0x04, 16},
    {0x7000, 0x05, 16}, {0x7000, 0x06, 16}, {0x7000, 0x07, 16}, {0x7000, 0x08, 16},
    {0x7000, 0x09, 16}, {0x7000, 0x0a, 16}, {0x7000, 0x0b, 16}, {0x7000, 0x0c, 16},
    {0x7000, 0x0d, 16}, {0x7000, 0x0e, 16}, {0x7000, 0x0f, 16}, {0x7000, 0x10, 16},
    {0x7000, 0x11, 16}, {0x7000, 0x12, 16}, {0x7000, 0x13, 16},
    {0x6000, 0x01, 16}, {0x6000, 0x02, 16}, {0x6000, 0x03, 16}, {0x6000, 0x04, 16},
    {0x6000, 0x05, 16}, {0x6000, 0x06, 16}, {0x6000, 0x07, 16}, {0x6000, 0x08, 16},
    {0x6000, 0x09, 16}, {0x6000, 0x0a, 16}, {0x6000, 0x0b, 16}, {0x6000, 0x0c, 16},
    {0x6000, 0x0d, 16}, {0x6000, 0x0e, 16}, {0x6000, 0x0f, 16}, {0x6000, 0x10, 16},
    {0x6000, 0x11, 16}, {0x6000, 0x12, 16}, {0x6000, 0x13, 16}, {0x6000, 0x14, 16},
    {0x6000, 0x15, 16}, {0x6000, 0x16, 16}, {0x6000, 0x17, 16}, {0x6000, 0x18, 16},
    {0x6000, 0x19, 16}, {0x6000, 0x1a, 16}, {0x6000, 0x1b, 16}, {0x6000, 0x1c, 16},
    {0x6000, 0x1d, 16}, {0x6000, 0x1e, 16}, {0x6000, 0x1f, 16}, {0x6000, 0x20, 16},
    {0x6000, 0x21, 16}, {0x6000, 0x22, 16}, {0x6000, 0x23, 16}, {0x6000, 0x24, 16},
    {0x6000, 0x25, 16}, {0x6000, 0x26, 16}, {0x6000, 0x27, 16}, {0x6000, 0x28, 16},
    {0x6000, 0x29, 16}, {0x6000, 0x2a, 16}, {0x6000, 0x2b, 16}, {0x6000, 0x2c, 16},
    {0x6000, 0x2d, 16}, {0x6000, 0x2e, 16}, {0x6000, 0x2f, 16}, {0x6000, 0x30, 16},
    {0x6000, 0x31, 16}, {0x6000, 0x32, 16}, {0x6000, 0x33, 16}, {0x6000, 0x34, 16},
    {0x6000, 0x35, 16}, {0x6000, 0x36, 16}, {0x6000, 0x37, 16}, {0x6000, 0x38, 16},
    {0x6000, 0x39, 16}, {0x6000, 0x3a, 16}, {0x6000, 0x3b, 16}, {0x6000, 0x3c, 16},
    {0x6000, 0x3d, 16}, {0x6000, 0x3e, 16}, {0x6000, 0x3f, 16}, {0x6000, 0x40, 16},
    {0x6000, 0x41, 16}, {0x6000, 0x42, 16}, {0x6000, 0x43, 16}, {0x6000, 0x44, 16},
    {0x6000, 0x45, 16}, {0x6000, 0x46, 16}, {0x6000, 0x47, 16}, {0x6000, 0x48, 16},
    {0x6000, 0x49, 16}, {0x6000, 0x4a, 16}, {0x6000, 0x4b, 16}, {0x6000, 0x4c, 16},
};

static ec_pdo_info_t slave_0_pdos[] = {
    {0x1601, 19, slave_0_pdo_entries + 0},  // RxPDO (outputs)
    {0x1A00, 76, slave_0_pdo_entries + 19}, // TxPDO (inputs)
};

static ec_sync_info_t slave_0_syncs[] = {
    {0, EC_DIR_OUTPUT, 0, nullptr, EC_WD_DISABLE},
    {1, EC_DIR_INPUT, 0, nullptr, EC_WD_DISABLE},
    {2, EC_DIR_OUTPUT, 1, slave_0_pdos + 0, EC_WD_ENABLE},
    {3, EC_DIR_INPUT, 1, slave_0_pdos + 1, EC_WD_DISABLE},
    {0xFF, EC_DIR_OUTPUT, 0, nullptr, EC_WD_DISABLE}
};

// Instance context structure
struct rh56f1_context {
    // EtherCAT state
    ec_master_t* master;
    ec_master_state_t master_state;
    ec_domain_t* domain;
    ec_slave_config_t* slave_config;
    ec_slave_config_state_t slave_config_state;
    uint8_t* domain_pd;
    
    // Configuration
    uint32_t master_index;
    uint32_t slave_position;
    
    // PDO offsets
    uint16_t out_offsets[19];  // Output PDO offsets
    uint16_t in_offsets[76];   // Input PDO offsets
    
    // Data storage
    int16_t input_values[76];
    uint16_t output_values[19];
    pthread_mutex_t data_mutex;
    
    // Control thread
    pthread_t control_thread;
    std::atomic<bool> running;
    
    // State monitoring
    int state_check_counter;
    bool slave_operational;
    
    // Error handling
    char error_msg[256];
};

// Helper functions
static void write_pdo_u16(rh56f1_context* ctx, int index, uint16_t value) {
    if (index >= 0 && index < 19 && ctx->domain_pd) {
        EC_WRITE_U16(ctx->domain_pd + ctx->out_offsets[index], value);
    }
}

static int16_t read_pdo_u16(rh56f1_context* ctx, int index) {
    if (index >= 0 && index < 76 && ctx->domain_pd) {
        uint16_t unsigned_value = EC_READ_U16(ctx->domain_pd + ctx->in_offsets[index]);
        return (int16_t)unsigned_value;
    }
    return 0;
}

// Check master state
static void check_master_state(rh56f1_context* ctx) {
    ec_master_state_t ms;
    ecrt_master_state(ctx->master, &ms);

    if (RH56F1_VERBOSE) {
        if (ms.slaves_responding != ctx->master_state.slaves_responding) {
            printf("EtherCAT[%u]: %u slave(s) responding.\n", ctx->master_index, ms.slaves_responding);
        }
        if (ms.al_states != ctx->master_state.al_states) {
            printf("EtherCAT[%u]: AL states: 0x%02X.\n", ctx->master_index, ms.al_states);
        }
        if (ms.link_up != ctx->master_state.link_up) {
            printf("EtherCAT[%u]: Link is %s.\n", ctx->master_index, ms.link_up ? "up" : "down");
        }
    }

    ctx->master_state = ms;
}

// Check slave configuration state
static void check_slave_config_state(rh56f1_context* ctx) {
    ec_slave_config_state_t s;
    ecrt_slave_config_state(ctx->slave_config, &s);

    if (s.al_state != ctx->slave_config_state.al_state) {
        if (RH56F1_VERBOSE) {
            printf("EtherCAT[%u]: Slave AL state changed to 0x%02X", ctx->master_index, s.al_state);
            if (s.al_state == 0x08) {
                printf(" (OPERATIONAL)\n");
            } else if (s.al_state == 0x04) {
                printf(" (SAFEOP)\n");
            } else if (s.al_state == 0x02) {
                printf(" (PREOP)\n");
            } else if (s.al_state == 0x01) {
                printf(" (INIT)\n");
            } else {
                printf("\n");
            }
        }
        if (s.al_state == 0x08) {
            ctx->slave_operational = true;
        }
    }
    if (s.online != ctx->slave_config_state.online) {
        if (RH56F1_VERBOSE) {
            printf("EtherCAT[%u]: Slave %s.\n", ctx->master_index, s.online ? "online" : "offline");
        }
    }
    if (s.operational != ctx->slave_config_state.operational) {
        if (RH56F1_VERBOSE) {
            printf("EtherCAT[%u]: Slave %soperational.\n", ctx->master_index, s.operational ? "" : "not ");
        }
        ctx->slave_operational = s.operational;
    }

    ctx->slave_config_state = s;
}

// Cyclic task
static void cyclic_task(rh56f1_context* ctx) {
    // Receive process data
    ecrt_master_receive(ctx->master);
    ecrt_domain_process(ctx->domain);

    // Check states periodically
    if (ctx->state_check_counter++ % 1000 == 0) {
        check_master_state(ctx);
        check_slave_config_state(ctx);
    }

    pthread_mutex_lock(&ctx->data_mutex);
    
    // Read all input values
    for (int i = 0; i < 76; i++) {
        ctx->input_values[i] = read_pdo_u16(ctx, i);
    }
    
    // Write all output values
    for (int i = 0; i < 19; i++) {
        write_pdo_u16(ctx, i, ctx->output_values[i]);
    }
    
    pthread_mutex_unlock(&ctx->data_mutex);

    // Send process data
    ecrt_domain_queue(ctx->domain);
    ecrt_master_send(ctx->master);
}

// Real-time control thread
static void* control_thread_func(void* arg) {
    rh56f1_context* ctx = (rh56f1_context*)arg;
    
    // Set real-time priority
    struct sched_param param = {};
    param.sched_priority = sched_get_priority_max(SCHED_FIFO);
    if (sched_setscheduler(0, SCHED_FIFO, &param) == -1 && RH56F1_VERBOSE) {
        perror("sched_setscheduler failed");
    }

    // Lock memory
    if (mlockall(MCL_CURRENT | MCL_FUTURE) == -1 && RH56F1_VERBOSE) {
        fprintf(stderr, "Warning: Failed to lock memory: %s\n", strerror(errno));
    }

    if (RH56F1_VERBOSE) {
        printf("RH56F1[%u]: Starting RT control loop at 500Hz\n", ctx->master_index);
    }

    struct timespec wakeup_time;
    clock_gettime(CLOCK_MONOTONIC, &wakeup_time);
    wakeup_time.tv_sec += 1;
    wakeup_time.tv_nsec = 0;

    while (ctx->running.load()) {
        int ret = clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &wakeup_time, nullptr);
        if (ret) {
            if (RH56F1_VERBOSE) {
                fprintf(stderr, "clock_nanosleep(): %s\n", strerror(ret));
            }
            break;
        }

        cyclic_task(ctx);

        wakeup_time.tv_nsec += PERIOD_NS;
        while (wakeup_time.tv_nsec >= NSEC_PER_SEC) {
            wakeup_time.tv_nsec -= NSEC_PER_SEC;
            wakeup_time.tv_sec++;
        }
    }

    if (RH56F1_VERBOSE) {
        printf("RH56F1[%u]: Control loop stopped\n", ctx->master_index);
    }
    return nullptr;
}

// API Implementation

rh56f1_handle_t rh56f1_create(uint32_t master_index, uint32_t slave_position) {
    // Allocate context with value-initialization
    rh56f1_context* ctx = new rh56f1_context{};
    
    ctx->master_index = master_index;
    ctx->slave_position = slave_position;
    ctx->running.store(false);
    ctx->state_check_counter = 0;
    ctx->slave_operational = false;
    pthread_mutex_init(&ctx->data_mutex, nullptr);
    
    // Request EtherCAT master
    ctx->master = ecrt_request_master(master_index);
    if (!ctx->master) {
        snprintf(ctx->error_msg, sizeof(ctx->error_msg), "Failed to request master %u", master_index);
        delete ctx;
        return nullptr;
    }

    // Create domain
    ctx->domain = ecrt_master_create_domain(ctx->master);
    if (!ctx->domain) {
        snprintf(ctx->error_msg, sizeof(ctx->error_msg), "Failed to create domain");
        ecrt_release_master(ctx->master);
        delete ctx;
        return nullptr;
    }

    // Configure slave
    ctx->slave_config = ecrt_master_slave_config(ctx->master, 0, slave_position, VENDOR_ID, PRODUCT_ID);
    if (!ctx->slave_config) {
        snprintf(ctx->error_msg, sizeof(ctx->error_msg), "Failed to get slave configuration");
        ecrt_release_master(ctx->master);
        delete ctx;
        return nullptr;
    }

    // Configure PDOs
    if (ecrt_slave_config_pdos(ctx->slave_config, EC_END, slave_0_syncs)) {
        snprintf(ctx->error_msg, sizeof(ctx->error_msg), "Failed to configure PDOs");
        ecrt_release_master(ctx->master);
        delete ctx;
        return nullptr;
    }

    // Register PDO entries
    ec_pdo_entry_reg_t domain1_regs[96];  // 19 outputs + 76 inputs + 1 end marker
    int reg_idx = 0;

    // Output PDOs
    for (int i = 0; i < 19; i++) {
        domain1_regs[reg_idx++] = {
            0, static_cast<uint16_t>(slave_position), VENDOR_ID, PRODUCT_ID, 0x7000, (uint8_t)(i + 1),
            reinterpret_cast<unsigned int*>(&ctx->out_offsets[i]), nullptr
        };
    }

    // Input PDOs
    for (int i = 0; i < 76; i++) {
        domain1_regs[reg_idx++] = {
            0, static_cast<uint16_t>(slave_position), VENDOR_ID, PRODUCT_ID, 0x6000, (uint8_t)(i + 1),
            reinterpret_cast<unsigned int*>(&ctx->in_offsets[i]), nullptr
        };
    }

    // End marker
    domain1_regs[reg_idx] = {};

    if (ecrt_domain_reg_pdo_entry_list(ctx->domain, domain1_regs)) {
        snprintf(ctx->error_msg, sizeof(ctx->error_msg), "PDO entry registration failed");
        ecrt_release_master(ctx->master);
        delete ctx;
        return nullptr;
    }

    // Activate master
    if (ecrt_master_activate(ctx->master)) {
        snprintf(ctx->error_msg, sizeof(ctx->error_msg), "Failed to activate master");
        ecrt_release_master(ctx->master);
        delete ctx;
        return nullptr;
    }

    // Get domain data pointer
    ctx->domain_pd = ecrt_domain_data(ctx->domain);
    if (!ctx->domain_pd) {
        snprintf(ctx->error_msg, sizeof(ctx->error_msg), "Failed to get domain data pointer");
        ecrt_release_master(ctx->master);
        delete ctx;
        return nullptr;
    }

    if (RH56F1_VERBOSE) {
        printf("RH56F1: Initialized successfully (master %u, slave %u)\n", master_index, slave_position);
    }
    return (rh56f1_handle_t)ctx;
}

int rh56f1_start(rh56f1_handle_t handle) {
    if (!handle) {
        return -1;
    }
    
    rh56f1_context* ctx = (rh56f1_context*)handle;
    
    if (ctx->running.load()) {
        snprintf(ctx->error_msg, sizeof(ctx->error_msg), "Already running");
        return -1;
    }

    ctx->running.store(true);

    if (pthread_create(&ctx->control_thread, nullptr, control_thread_func, ctx) != 0) {
        snprintf(ctx->error_msg, sizeof(ctx->error_msg), "Failed to create control thread");
        ctx->running.store(false);
        return -1;
    }

    return 0;
}

int rh56f1_stop(rh56f1_handle_t handle) {
    if (!handle) {
        return -1;
    }
    
    rh56f1_context* ctx = (rh56f1_context*)handle;
    
    if (!ctx->running.load()) {
        return 0;
    }

    ctx->running.store(false);
    pthread_join(ctx->control_thread, nullptr);

    return 0;
}

int rh56f1_deactivate(rh56f1_handle_t handle) {
    if (!handle) {
        return -1;
    }
    rh56f1_context* ctx = (rh56f1_context*)handle;
    if (!ctx->master) {
        return 0;
    }
    if (RH56F1_VERBOSE) {
        printf("RH56F1[%u]: Deactivating master (keeping reservation)\n", ctx->master_index);
    }
    int ret = ecrt_master_deactivate(ctx->master);
    return (ret == 0) ? 0 : -1;
}

int rh56f1_activate(rh56f1_handle_t handle) {
    if (!handle) {
        return -1;
    }
    rh56f1_context* ctx = (rh56f1_context*)handle;
    if (!ctx->master) {
        snprintf(ctx->error_msg, sizeof(ctx->error_msg), "No master");
        return -1;
    }
    if (ecrt_master_activate(ctx->master)) {
        snprintf(ctx->error_msg, sizeof(ctx->error_msg), "Failed to activate master");
        return -1;
    }
    return 0;
}

void rh56f1_destroy(rh56f1_handle_t handle) {
    if (!handle) {
        return;
    }
    
    rh56f1_context* ctx = (rh56f1_context*)handle;
    
    // Stop if running
    rh56f1_stop(handle);

    // Deactivate and release master; give kernel time to leave OP before releasing reservation
    if (ctx->master) {
        ecrt_master_deactivate(ctx->master);
        std::this_thread::sleep_for(std::chrono::milliseconds(500));
        ecrt_release_master(ctx->master);
    }

    // Destroy mutex
    pthread_mutex_destroy(&ctx->data_mutex);

    if (RH56F1_VERBOSE) {
        printf("RH56F1[%u]: Cleaned up\n", ctx->master_index);
    }

    // Free context
    delete ctx;
}

int rh56f1_get_all_inputs(rh56f1_handle_t handle, int16_t* values, int size) {
    if (!handle || !values || size < 76) {
        return -1;
    }
    
    rh56f1_context* ctx = (rh56f1_context*)handle;

    pthread_mutex_lock(&ctx->data_mutex);
    memcpy(values, ctx->input_values, sizeof(int16_t) * 76);
    pthread_mutex_unlock(&ctx->data_mutex);

    return 76;
}

int rh56f1_get_input(rh56f1_handle_t handle, int index, int16_t* value) {
    if (!handle || !value || index < 0 || index >= 76) {
        return -1;
    }
    
    rh56f1_context* ctx = (rh56f1_context*)handle;

    pthread_mutex_lock(&ctx->data_mutex);
    *value = ctx->input_values[index];
    pthread_mutex_unlock(&ctx->data_mutex);

    return 0;
}

int rh56f1_set_output(rh56f1_handle_t handle, int index, uint16_t value) {
    if (!handle || index < 0 || index >= 19) {
        return -1;
    }
    
    rh56f1_context* ctx = (rh56f1_context*)handle;

    pthread_mutex_lock(&ctx->data_mutex);
    ctx->output_values[index] = value;
    pthread_mutex_unlock(&ctx->data_mutex);

    return 0;
}

int rh56f1_is_running(rh56f1_handle_t handle) {
    if (!handle) {
        return 0;
    }
    
    rh56f1_context* ctx = (rh56f1_context*)handle;
    return ctx->running.load() ? 1 : 0;
}

const char* rh56f1_get_error(rh56f1_handle_t handle) {
    if (!handle) {
        return nullptr;
    }
    
    rh56f1_context* ctx = (rh56f1_context*)handle;
    return ctx->error_msg[0] ? ctx->error_msg : nullptr;
}

int rh56f1_read_sdo(rh56f1_handle_t handle, uint16_t index, uint8_t subindex, int16_t* value) {
    if (!handle || !value) {
        return -1;
    }
    
    rh56f1_context* ctx = (rh56f1_context*)handle;

    uint8_t target_buffer[2] = {0};
    size_t result_size = 0;
    uint32_t abort_code = 0;

    int ret = ecrt_master_sdo_upload(
        ctx->master,
        ctx->slave_position,
        index,
        subindex,
        target_buffer,
        sizeof(target_buffer),
        &result_size,
        &abort_code
    );

    if (ret < 0 || result_size != 2) {
        snprintf(ctx->error_msg, sizeof(ctx->error_msg), 
                 "SDO read failed: index=0x%04X sub=0x%02X ret=%d abort=0x%08X",
                 index, subindex, ret, abort_code);
        return -1;
    }

    *value = (int16_t)(target_buffer[0] | (target_buffer[1] << 8));
    return 0;
}

int rh56f1_write_sdo(rh56f1_handle_t handle, uint16_t index, uint8_t subindex, int16_t value) {
    if (!handle) {
        return -1;
    }
    
    rh56f1_context* ctx = (rh56f1_context*)handle;

    uint8_t data[2];
    data[0] = value & 0xFF;
    data[1] = (value >> 8) & 0xFF;

    uint32_t abort_code = 0;

    int ret = ecrt_master_sdo_download(
        ctx->master,
        ctx->slave_position,
        index,
        subindex,
        data,
        sizeof(data),
        &abort_code
    );

    if (ret < 0) {
        snprintf(ctx->error_msg, sizeof(ctx->error_msg),
                 "SDO write failed: index=0x%04X sub=0x%02X ret=%d abort=0x%08X",
                 index, subindex, ret, abort_code);
        return -1;
    }

    if (RH56F1_VERBOSE) {
        printf("SDO Write[%u]: 0x%04X:0x%02X = %d (0x%04X)\n",
               ctx->master_index, index, subindex, value, (uint16_t)value);
    }
    return 0;
}
