// =============================================================================
//  modbus_slave.cpp -- eModbus-based RTU server (slave) implementation
// =============================================================================
//
//  Sprint 1 scope:
//      - Input registers (FC 0x04): firmware version, uptime, mode, heap,
//        rudder angle/raw/valid, heading + ROT (stub for Sprint 1), battery
//        and actuator current placeholders.
//      - Discrete inputs (FC 0x02): engaged flag, all DIs, alarm bits.
//      - Holding registers (FC 0x03/0x06/0x10): mode request, heading
//        setpoint, brightness, alarm volume, dodge offset.
//      - Coils (FC 0x01/0x05/0x0F): engage/disengage requests, ack-all,
//        knob arm (reserved).
//
//  In Sprint 1 the writes that try to change to a non-STANDBY mode are
//  silently coerced (the modes::request_mode() helper rejects them with a
//  warning).
// =============================================================================

#include "modbus_slave.h"

#include <Arduino.h>

#include <ModbusServerRTU.h>

#include "../hal/di_do.h"
#include "../hal/pinout.h"
#include "../hal/rudder_sensor.h"
#include "../modes/standby.h"
#include "../pid/pid_inner_task.h"
#include "../pid/pid_outer_task.h"
#include "../system/ar_log.h"
#include "../system/task_config.h"
#include "modbus_registers.h"
#include "nmea2000_consumer.h"
#include "../hal/knob_encoder.h"
#include "../safety/safety_monitor.h"
#include "../system/hwid.h"

namespace arautopilot::protocols::modbus {

namespace {
constexpr const char* TAG = "AR/MB";

// eModbus server with a 1000 ms timeout for client transactions.
ModbusServerRTU g_server(1000, PIN_RS485_DE);

bool g_running = false;
volatile uint32_t g_req_count = 0;
volatile uint32_t g_err_count = 0;

// ----- Storage for the writable side (coils + holding registers) -----------
// Discrete inputs and input registers are computed on the fly from the live
// telemetry helpers, so they don't need backing storage. Coils and holding
// registers do (eModbus expects deterministic reads of the values it last
// stored).
struct HoldingStorage {
    uint16_t mode_request = 0;
    int16_t  heading_setpoint_x100 = 0;
    uint16_t brightness_pct = 80;
    uint16_t alarm_volume_pct = 60;
    int16_t  dodge_offset_deg_x100 = 0;
    int16_t  pid_inner_setpoint_req_x100 = 0;
    uint16_t pid_inner_kp_req_x1000 = 0;
    uint16_t pid_inner_ki_req_x1000 = 0;
    uint16_t pid_inner_kd_req_x1000 = 0;
    uint16_t pid_outer_heading_sp_req_x100 = 0;
    uint16_t pid_outer_speed_kn_req_x10 = 150;  // 15.0 kn default
    uint16_t pid_outer_kp_req_x1000 = 0;
    uint16_t pid_outer_ki_req_x1000 = 0;
    uint16_t pid_outer_kd_req_x1000 = 0;
    // Sprint 5: True Course + Track Keeping
    uint16_t true_course_sp_x100 = 0;
    uint16_t xte_gain_x1000 = 500;      // 0.5 deg/m default
    uint16_t xte_max_correction_x100 = 2000;  // 20.0 deg default
};
HoldingStorage g_holding;

struct CoilStorage {
    bool engage_request = false;
    bool disengage_request = false;
    bool ack_all = false;
    bool knob_arm = false;
};
CoilStorage g_coils;

// ----- Helpers ---------------------------------------------------------------

uint16_t read_input_register(uint16_t addr) {
    switch (addr) {
        case INPUT_FW_VERSION_MAJOR: return 0;
        case INPUT_FW_VERSION_MINOR: return 1;
        case INPUT_FW_VERSION_PATCH: return 0;
        case INPUT_SCHEMA_VERSION:   return 0;
        case INPUT_UPTIME_SECONDS_LO: return (uint16_t)((millis() / 1000U) & 0xFFFFU);
        case INPUT_UPTIME_SECONDS_HI: return (uint16_t)(((millis() / 1000U) >> 16) & 0xFFFFU);
        case INPUT_CURRENT_MODE:     return (uint16_t)modes::current_mode();
        case INPUT_FREE_HEAP_KB:     return (uint16_t)(ESP.getFreeHeap() / 1024U);
        case INPUT_MIN_FREE_HEAP_KB: return (uint16_t)(ESP.getMinFreeHeap() / 1024U);

        // Sprint 8: Hardware ID (MAC eFuse, 3 × uint16)
        case INPUT_HWID_MAC_01:
        case INPUT_HWID_MAC_23:
        case INPUT_HWID_MAC_45: {
            uint8_t mac[6] = {};
            system::hwid_get_mac(mac);
            if (addr == INPUT_HWID_MAC_01) return (uint16_t)((mac[0] << 8) | mac[1]);
            if (addr == INPUT_HWID_MAC_23) return (uint16_t)((mac[2] << 8) | mac[3]);
            return                                (uint16_t)((mac[4] << 8) | mac[5]);
        }

        case INPUT_RUDDER_ANGLE_DEG_X100: {
            auto r = hal::rudder_sensor_latest();
            int v = (int)(r.angle_deg * 100.0f);
            if (v < -32768) v = -32768;
            if (v >  32767) v =  32767;
            return (uint16_t)(int16_t)v;
        }
        case INPUT_RUDDER_RAW_ADC: {
            auto r = hal::rudder_sensor_latest();
            return (uint16_t)r.raw_adc;
        }
        case INPUT_RUDDER_VALID: {
            auto r = hal::rudder_sensor_latest();
            return r.valid ? 1 : 0;
        }

        case INPUT_HEADING_DEG_X100: {
            auto n = nmea2000::nmea2000_latest();
            int v = (int)(n.heading_deg * 100.0f);
            if (v < 0) v = 0;
            if (v > 35999) v = 35999;
            return (uint16_t)v;
        }
        case INPUT_ROT_DPS_X100: {
            auto n = nmea2000::nmea2000_latest();
            int v = (int)(n.rate_of_turn_dps * 100.0f);
            if (v < -32768) v = -32768;
            if (v >  32767) v =  32767;
            return (uint16_t)(int16_t)v;
        }
        case INPUT_HEADING_AGE_MS: {
            auto n = nmea2000::nmea2000_latest();
            uint32_t age = (millis() - n.heading_age_ms);
            if (!n.heading_valid) age = 60000;
            if (age > 60000) age = 60000;
            return (uint16_t)age;
        }

        case INPUT_BATTERY_VOLTAGE_X100: {
            // 1:6 voltage divider on AI2; 12-bit ADC, 3.3 V reference.
            const int raw = analogRead(PIN_AI2_BATTERY_VOLTAGE);
            const float vbat = (float)raw * (3.3f * 6.0f / 4095.0f);
            int v = (int)(vbat * 100.0f);
            if (v < 0) v = 0;
            if (v > 32767) v = 32767;
            return (uint16_t)v;
        }
        case INPUT_ACTUATOR_CURRENT_X100: {
            // Hall-effect transducer on AI3, 50 A FS.
            const int raw = analogRead(PIN_AI3_ACTUATOR_CURRENT);
            const float iact = (float)raw * (3.3f * 50.0f / 4095.0f);
            int v = (int)(iact * 100.0f);
            if (v < 0) v = 0;
            if (v > 32767) v = 32767;
            return (uint16_t)v;
        }

        // ----- PID inner-loop telemetry (Sprint 2) -----
        case INPUT_PID_INNER_SETPOINT_X100: {
            int v = (int)(pid::pid_inner_setpoint_deg() * 100.0f);
            if (v < -32768) v = -32768;
            if (v >  32767) v =  32767;
            return (uint16_t)(int16_t)v;
        }
        case INPUT_PID_INNER_OUTPUT_X100: {
            int v = (int)(pid::pid_inner_last_output_pct() * 100.0f);
            if (v < -32768) v = -32768;
            if (v >  32767) v =  32767;
            return (uint16_t)(int16_t)v;
        }
        case INPUT_PID_INNER_ERROR_X100: {
            int v = (int)(pid::pid_inner_last_error_deg() * 100.0f);
            if (v < -32768) v = -32768;
            if (v >  32767) v =  32767;
            return (uint16_t)(int16_t)v;
        }
        case INPUT_PID_INNER_KP_X1000:
        case INPUT_PID_INNER_KI_X1000:
        case INPUT_PID_INNER_KD_X1000: {
            float kp, ki, kd;
            pid::pid_inner_get_gains(kp, ki, kd);
            float v;
            if (addr == INPUT_PID_INNER_KP_X1000)      v = kp;
            else if (addr == INPUT_PID_INNER_KI_X1000) v = ki;
            else                                        v = kd;
            int scaled = (int)(v * 1000.0f);
            if (scaled < 0) scaled = 0;
            if (scaled > 65535) scaled = 65535;
            return (uint16_t)scaled;
        }

        // ----- PID outer-loop telemetry (Sprint 3) -----
        case INPUT_PID_OUTER_HEADING_SP_X100: {
            int v = (int)(pid::pid_outer_heading_setpoint_deg() * 100.0f);
            if (v < 0) v = 0;
            if (v > 35999) v = 35999;
            return (uint16_t)v;
        }
        case INPUT_PID_OUTER_RUDDER_SP_X100: {
            int v = (int)(pid::pid_outer_last_rudder_setpoint_deg() * 100.0f);
            if (v < -32768) v = -32768;
            if (v >  32767) v =  32767;
            return (uint16_t)(int16_t)v;
        }
        case INPUT_PID_OUTER_ERROR_X100: {
            int v = (int)(pid::pid_outer_last_error_deg() * 100.0f);
            if (v < -32768) v = -32768;
            if (v >  32767) v =  32767;
            return (uint16_t)(int16_t)v;
        }
        case INPUT_PID_OUTER_SPEED_KN_X10: {
            int v = (int)(pid::pid_outer_speed_kn() * 10.0f);
            if (v < 0) v = 0;
            if (v > 65535) v = 65535;
            return (uint16_t)v;
        }
        case INPUT_PID_OUTER_KP_X1000:
        case INPUT_PID_OUTER_KI_X1000:
        case INPUT_PID_OUTER_KD_X1000: {
            float kp, ki, kd;
            pid::pid_outer_get_gains(kp, ki, kd);
            float v;
            if (addr == INPUT_PID_OUTER_KP_X1000)      v = kp;
            else if (addr == INPUT_PID_OUTER_KI_X1000) v = ki;
            else                                        v = kd;
            int scaled = (int)(v * 1000.0f);
            if (scaled < 0) scaled = 0;
            if (scaled > 65535) scaled = 65535;
            return (uint16_t)scaled;
        }

        // ----- Sprint 5: COG / SOG / XTE telemetry -----
        case INPUT_COG_DEG_X100: {
            auto cs = nmea2000::nmea2000_cog_sog();
            if (!cs.valid) return 0;
            int v = (int)(cs.cog_deg * 100.0f);
            if (v < 0) v = 0;
            if (v > 35999) v = 35999;
            return (uint16_t)v;
        }
        case INPUT_SOG_KN_X10: {
            auto cs = nmea2000::nmea2000_cog_sog();
            if (!cs.valid) return 0;
            int v = (int)(cs.sog_kn * 10.0f);
            if (v < 0) v = 0;
            if (v > 65535) v = 65535;
            return (uint16_t)v;
        }
        case INPUT_COG_AGE_MS: {
            auto cs = nmea2000::nmea2000_cog_sog();
            uint32_t age = cs.valid ? (millis() - cs.age_ms) : 60000;
            if (age > 60000) age = 60000;
            return (uint16_t)age;
        }
        case INPUT_XTE_DM_SIGNED: {
            auto nd = nmea2000::nmea2000_nav_data();
            if (!nd.valid) return 0;
            int v = (int)(nd.xte_m * 10.0f);  // metres → decimetres
            if (v < -32768) v = -32768;
            if (v >  32767) v =  32767;
            return (uint16_t)(int16_t)v;
        }
        case INPUT_XTE_AGE_MS: {
            auto nd = nmea2000::nmea2000_nav_data();
            uint32_t age = nd.valid ? (millis() - nd.age_ms) : 60000;
            if (age > 60000) age = 60000;
            return (uint16_t)age;
        }
        case INPUT_DTW_M: {
            auto nd = nmea2000::nmea2000_nav_data();
            if (!nd.valid) return 0;
            uint32_t dtw = (uint32_t)nd.dtw_m;
            if (dtw > 65535) dtw = 65535;
            return (uint16_t)dtw;
        }

        default:
            return 0;
    }
}

bool read_discrete(uint16_t addr) {
    switch (addr) {
        case DISCRETE_PILOT_ENGAGED:        return !modes::is_standby();
        case DISCRETE_DI_DISENGAGE_BUTTON:  return hal::di_read(PIN_DI1_DISENGAGE_BUTTON);
        case DISCRETE_DI_LIMIT_PORT:        return hal::di_read(PIN_DI2_LIMIT_SWITCH_PORT);
        case DISCRETE_DI_LIMIT_STBD:        return hal::di_read(PIN_DI3_LIMIT_SWITCH_STBD);
        case DISCRETE_DI_EXTERNAL_ALARM:    return hal::di_read(PIN_DI4_EXTERNAL_ALARM);
        case DISCRETE_DI_MANUAL_CONFIRM:    return hal::di_read(PIN_DI5_MANUAL_CONFIRM);
        case DISCRETE_ACTUATOR_POWER:       return hal::do_state(PIN_DO3_ACTUATOR_POWER);
        case DISCRETE_ACTUATOR_DRIVING_PORT:return hal::do_state(PIN_DO1_PUMP_PORT);
        case DISCRETE_ACTUATOR_DRIVING_STBD:return hal::do_state(PIN_DO2_PUMP_STBD);
        // Alarm bits: wired in Sprint 6 via safety_monitor.
        case DISCRETE_ALARM_OFF_COURSE:         return safety::safety_alarm_bits().off_course;
        case DISCRETE_ALARM_OFF_COURSE_SEVERE:  return safety::safety_alarm_bits().off_course_severe;
        case DISCRETE_ALARM_RUDDER_NOT_RESP:    return safety::safety_alarm_bits().rudder_not_resp;
        case DISCRETE_ALARM_HEADING_LOST:       return safety::safety_alarm_bits().heading_lost;
        case DISCRETE_ALARM_ACTUATOR_OVERCURR:  return safety::safety_alarm_bits().actuator_overcurr;
        case DISCRETE_ALARM_VOLTAGE_LOW:        return safety::safety_alarm_bits().voltage_low;
        case DISCRETE_ALARM_LIMIT_REACHED:      return safety::safety_alarm_bits().limit_reached;
        case DISCRETE_ALARM_WATCHDOG_TRIPPED:   return safety::safety_alarm_bits().watchdog_tripped;
        case DISCRETE_ALARM_VMS_CRITICAL:       return safety::safety_alarm_bits().vms_critical;
        case DISCRETE_ANY_ALARM_ACTIVE:         return safety::safety_alarm_bits().any();
        default:
            return false;
    }
}

uint16_t read_holding(uint16_t addr) {
    switch (addr) {
        case HOLDING_MODE_REQUEST:           return g_holding.mode_request;
        case HOLDING_HEADING_SETPOINT_X100:  return (uint16_t)g_holding.heading_setpoint_x100;
        case HOLDING_BRIGHTNESS_PCT:         return g_holding.brightness_pct;
        case HOLDING_ALARM_VOLUME_PCT:       return g_holding.alarm_volume_pct;
        case HOLDING_DODGE_OFFSET_DEG_X100:  return (uint16_t)g_holding.dodge_offset_deg_x100;
        case HOLDING_PID_INNER_SETPOINT_REQ_X100: return (uint16_t)g_holding.pid_inner_setpoint_req_x100;
        case HOLDING_PID_INNER_KP_REQ_X1000: return g_holding.pid_inner_kp_req_x1000;
        case HOLDING_PID_INNER_KI_REQ_X1000: return g_holding.pid_inner_ki_req_x1000;
        case HOLDING_PID_INNER_KD_REQ_X1000: return g_holding.pid_inner_kd_req_x1000;
        case HOLDING_PID_OUTER_HEADING_SP_REQ_X100: return g_holding.pid_outer_heading_sp_req_x100;
        case HOLDING_PID_OUTER_SPEED_KN_REQ_X10:    return g_holding.pid_outer_speed_kn_req_x10;
        case HOLDING_PID_OUTER_KP_REQ_X1000:        return g_holding.pid_outer_kp_req_x1000;
        case HOLDING_PID_OUTER_KI_REQ_X1000:        return g_holding.pid_outer_ki_req_x1000;
        case HOLDING_PID_OUTER_KD_REQ_X1000:        return g_holding.pid_outer_kd_req_x1000;
        case HOLDING_TRUE_COURSE_SP_X100:            return g_holding.true_course_sp_x100;
        case HOLDING_XTE_GAIN_X1000:                 return g_holding.xte_gain_x1000;
        case HOLDING_XTE_MAX_CORRECTION_X100:        return g_holding.xte_max_correction_x100;
        default:                             return 0;
    }
}

// Returns Modbus exception code (0 = ok). Side effect: stores value.
Modbus::Error write_holding(uint16_t addr, uint16_t value) {
    switch (addr) {
        case HOLDING_MODE_REQUEST:
            if (value > 4) return Modbus::Error::ILLEGAL_DATA_VALUE;
            g_holding.mode_request = value;
            modes::request_mode((modes::Mode)value);
            return Modbus::Error::SUCCESS;
        case HOLDING_HEADING_SETPOINT_X100:
            g_holding.heading_setpoint_x100 = (int16_t)value;
            return Modbus::Error::SUCCESS;
        case HOLDING_BRIGHTNESS_PCT:
            if (value > 100) return Modbus::Error::ILLEGAL_DATA_VALUE;
            g_holding.brightness_pct = value;
            return Modbus::Error::SUCCESS;
        case HOLDING_ALARM_VOLUME_PCT:
            if (value > 100) return Modbus::Error::ILLEGAL_DATA_VALUE;
            g_holding.alarm_volume_pct = value;
            return Modbus::Error::SUCCESS;
        case HOLDING_DODGE_OFFSET_DEG_X100:
            g_holding.dodge_offset_deg_x100 = (int16_t)value;
            return Modbus::Error::SUCCESS;

        // ----- PID inner-loop tunables (Sprint 2) -----
        case HOLDING_PID_INNER_SETPOINT_REQ_X100: {
            int16_t sv = (int16_t)value;
            g_holding.pid_inner_setpoint_req_x100 = sv;
            pid::pid_inner_set_setpoint_deg((float)sv * 0.01f);
            return Modbus::Error::SUCCESS;
        }
        case HOLDING_PID_OUTER_HEADING_SP_REQ_X100: {
            if (value > 35999) return Modbus::Error::ILLEGAL_DATA_VALUE;
            g_holding.pid_outer_heading_sp_req_x100 = value;
            pid::pid_outer_set_heading_setpoint_deg((float)value * 0.01f);
            return Modbus::Error::SUCCESS;
        }
        case HOLDING_PID_OUTER_SPEED_KN_REQ_X10: {
            if (value > 800) return Modbus::Error::ILLEGAL_DATA_VALUE;  // 80 kn cap
            g_holding.pid_outer_speed_kn_req_x10 = value;
            pid::pid_outer_set_speed_kn((float)value * 0.1f);
            return Modbus::Error::SUCCESS;
        }
        case HOLDING_PID_OUTER_KP_REQ_X1000:
        case HOLDING_PID_OUTER_KI_REQ_X1000:
        case HOLDING_PID_OUTER_KD_REQ_X1000: {
            if (addr == HOLDING_PID_OUTER_KP_REQ_X1000) {
                g_holding.pid_outer_kp_req_x1000 = value;
            } else if (addr == HOLDING_PID_OUTER_KI_REQ_X1000) {
                g_holding.pid_outer_ki_req_x1000 = value;
            } else {
                g_holding.pid_outer_kd_req_x1000 = value;
            }
            float kp = (float)g_holding.pid_outer_kp_req_x1000 * 0.001f;
            float ki = (float)g_holding.pid_outer_ki_req_x1000 * 0.001f;
            float kd = (float)g_holding.pid_outer_kd_req_x1000 * 0.001f;
            if (kp <= 0.0f) {
                return Modbus::Error::ILLEGAL_DATA_VALUE;
            }
            pid::pid_outer_update_gains(kp, ki, kd);
            return Modbus::Error::SUCCESS;
        }
        case HOLDING_PID_INNER_KP_REQ_X1000:
        case HOLDING_PID_INNER_KI_REQ_X1000:
        case HOLDING_PID_INNER_KD_REQ_X1000: {
            // Update the requested-gain shadow, then push all three to the
            // live controller. We do all three together so partial writes
            // don't leave the gains inconsistent.
            if (addr == HOLDING_PID_INNER_KP_REQ_X1000) {
                g_holding.pid_inner_kp_req_x1000 = value;
            } else if (addr == HOLDING_PID_INNER_KI_REQ_X1000) {
                g_holding.pid_inner_ki_req_x1000 = value;
            } else {
                g_holding.pid_inner_kd_req_x1000 = value;
            }
            float kp = (float)g_holding.pid_inner_kp_req_x1000 * 0.001f;
            float ki = (float)g_holding.pid_inner_ki_req_x1000 * 0.001f;
            float kd = (float)g_holding.pid_inner_kd_req_x1000 * 0.001f;
            // Refuse zero kp -- the rest of the algorithm assumes kp > 0
            // for back-calculation anti-windup. If the operator writes 0
            // we ignore it (leave whatever the firmware booted with).
            if (kp <= 0.0f) {
                return Modbus::Error::ILLEGAL_DATA_VALUE;
            }
            pid::pid_inner_update_gains(kp, ki, kd);
            return Modbus::Error::SUCCESS;
        }
        // ----- Sprint 5: True Course + XTE parameters -----
        case HOLDING_TRUE_COURSE_SP_X100: {
            if (value > 35999) return Modbus::Error::ILLEGAL_DATA_VALUE;
            g_holding.true_course_sp_x100 = value;
            pid::pid_outer_set_cog_setpoint_deg((float)value * 0.01f);
            return Modbus::Error::SUCCESS;
        }
        case HOLDING_XTE_GAIN_X1000: {
            g_holding.xte_gain_x1000 = value;
            pid::pid_outer_set_xte_gain((float)value * 0.001f);
            return Modbus::Error::SUCCESS;
        }
        case HOLDING_XTE_MAX_CORRECTION_X100: {
            if (value > 9000) return Modbus::Error::ILLEGAL_DATA_VALUE;  // 90 deg cap
            g_holding.xte_max_correction_x100 = value;
            pid::pid_outer_set_xte_max_correction((float)value * 0.01f);
            return Modbus::Error::SUCCESS;
        }

        default:
            return Modbus::Error::ILLEGAL_DATA_ADDRESS;
    }
}

Modbus::Error write_coil(uint16_t addr, bool value) {
    switch (addr) {
        case COIL_CMD_ENGAGE_REQUEST:
            g_coils.engage_request = value;
            // Sprint 1: requesting engagement does nothing yet -- modes
            // engine is implemented in Sprint 3+. Just log and store.
            if (value) {
                AR_LOGI(TAG, "engage request received (no-op in Sprint 1)");
            }
            return Modbus::Error::SUCCESS;
        case COIL_CMD_DISENGAGE_REQUEST:
            g_coils.disengage_request = value;
            if (value) {
                modes::force_standby("modbus-disengage");
            }
            return Modbus::Error::SUCCESS;
        case COIL_CMD_ACK_ALL_ALARMS:
            g_coils.ack_all = value;
            if (value) {
                AR_LOGI(TAG, "ack-all-alarms received (no alarms in Sprint 1)");
            }
            return Modbus::Error::SUCCESS;
        case COIL_CMD_KNOB_ARM:
            g_coils.knob_arm = value;
            hal::knob_encoder_set_armed(value);
            return Modbus::Error::SUCCESS;
        default:
            return Modbus::Error::ILLEGAL_DATA_ADDRESS;
    }
}

// ----- eModbus worker callbacks ---------------------------------------------

ModbusMessage on_read_input(ModbusMessage req) {
    uint16_t start, count;
    req.get(2, start);
    req.get(4, count);
    if (count == 0 ||
        (uint32_t)start + count > (uint32_t)INPUT_MAX_ADDR + 1U ||
        count > 125) {
        ++g_err_count;
        ModbusMessage resp;
        resp.setError(req.getServerID(), req.getFunctionCode(),
                      Modbus::Error::ILLEGAL_DATA_VALUE);
        return resp;
    }
    ModbusMessage resp;
    resp.add(req.getServerID(), req.getFunctionCode(),
             (uint8_t)(count * 2));
    for (uint16_t i = 0; i < count; ++i) {
        resp.add(read_input_register(start + i));
    }
    ++g_req_count;
    return resp;
}

ModbusMessage on_read_discrete(ModbusMessage req) {
    uint16_t start, count;
    req.get(2, start);
    req.get(4, count);
    if (count == 0 ||
        (uint32_t)start + count > (uint32_t)DISCRETE_MAX_ADDR + 1U ||
        count > 2000) {
        ++g_err_count;
        ModbusMessage resp;
        resp.setError(req.getServerID(), req.getFunctionCode(),
                      Modbus::Error::ILLEGAL_DATA_VALUE);
        return resp;
    }
    ModbusMessage resp;
    uint8_t byte_count = (uint8_t)((count + 7) / 8);
    resp.add(req.getServerID(), req.getFunctionCode(), byte_count);
    for (uint8_t b = 0; b < byte_count; ++b) {
        uint8_t v = 0;
        for (uint8_t bit = 0; bit < 8; ++bit) {
            uint16_t idx = b * 8U + bit;
            if (idx < count && read_discrete(start + idx)) {
                v |= (uint8_t)(1U << bit);
            }
        }
        resp.add(v);
    }
    ++g_req_count;
    return resp;
}

ModbusMessage on_read_holding(ModbusMessage req) {
    uint16_t start, count;
    req.get(2, start);
    req.get(4, count);
    if (count == 0 ||
        (uint32_t)start + count > (uint32_t)HOLDING_MAX_ADDR + 1U ||
        count > 125) {
        ++g_err_count;
        ModbusMessage resp;
        resp.setError(req.getServerID(), req.getFunctionCode(),
                      Modbus::Error::ILLEGAL_DATA_VALUE);
        return resp;
    }
    ModbusMessage resp;
    resp.add(req.getServerID(), req.getFunctionCode(),
             (uint8_t)(count * 2));
    for (uint16_t i = 0; i < count; ++i) {
        resp.add(read_holding(start + i));
    }
    ++g_req_count;
    return resp;
}

ModbusMessage on_write_holding(ModbusMessage req) {
    uint16_t addr, value;
    req.get(2, addr);
    req.get(4, value);
    auto err = write_holding(addr, value);
    if (err != Modbus::Error::SUCCESS) {
        ++g_err_count;
        ModbusMessage resp;
        resp.setError(req.getServerID(), req.getFunctionCode(), err);
        return resp;
    }
    // Echo per Modbus 0x06 convention.
    ModbusMessage resp;
    resp.add(req.getServerID(), req.getFunctionCode(), addr, value);
    ++g_req_count;
    return resp;
}

ModbusMessage on_write_coil(ModbusMessage req) {
    uint16_t addr, value;
    req.get(2, addr);
    req.get(4, value);
    // 0xFF00 = ON, 0x0000 = OFF per Modbus spec.
    if (value != 0x0000 && value != 0xFF00) {
        ++g_err_count;
        ModbusMessage resp;
        resp.setError(req.getServerID(), req.getFunctionCode(),
                      Modbus::Error::ILLEGAL_DATA_VALUE);
        return resp;
    }
    auto err = write_coil(addr, value == 0xFF00);
    if (err != Modbus::Error::SUCCESS) {
        ++g_err_count;
        ModbusMessage resp;
        resp.setError(req.getServerID(), req.getFunctionCode(), err);
        return resp;
    }
    ModbusMessage resp;
    resp.add(req.getServerID(), req.getFunctionCode(), addr, value);
    ++g_req_count;
    return resp;
}

}  // namespace

void modbus_slave_init() {
    // Register worker callbacks BEFORE starting the server (eModbus
    // requirement).
    g_server.registerWorker(SLAVE_ID, READ_INPUT_REGISTER, &on_read_input);
    g_server.registerWorker(SLAVE_ID, READ_DISCR_INPUT,    &on_read_discrete);
    g_server.registerWorker(SLAVE_ID, READ_HOLD_REGISTER,  &on_read_holding);
    g_server.registerWorker(SLAVE_ID, WRITE_HOLD_REGISTER, &on_write_holding);
    g_server.registerWorker(SLAVE_ID, WRITE_COIL,          &on_write_coil);

    AR_LOGI(TAG,
            "modbus_slave_init: slave_id=%u  inputs=%u  discretes=%u  "
            "holdings=%u  coils=%u",
            (unsigned)SLAVE_ID, (unsigned)INPUT_COUNT, (unsigned)DISCRETE_COUNT,
            (unsigned)HOLDING_COUNT, (unsigned)COIL_COUNT);
}

void modbus_slave_start() {
    // UART2 on the AR-NMEA-IO is wired to RS-485.
    Serial2.begin(BAUDRATE, SERIAL_8N1, PIN_RS485_RX, PIN_RS485_TX);
    g_server.begin(Serial2, AR_TASK_CORE_COMMS);
    g_running = true;
    AR_LOGI(TAG, "modbus_slave_start: listening on UART2 @ %u baud (RX=%d TX=%d DE=%d)",
            (unsigned)BAUDRATE, PIN_RS485_RX, PIN_RS485_TX, PIN_RS485_DE);
}

bool modbus_slave_is_running() { return g_running; }

uint32_t modbus_slave_request_count() { return g_req_count; }

uint32_t modbus_slave_error_count() { return g_err_count; }

}  // namespace arautopilot::protocols::modbus
