// =============================================================================
//  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 "../system/ar_log.h"
#include "../system/task_config.h"
#include "modbus_registers.h"
#include "nmea2000_consumer.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;
};
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);

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

        // Sprint 1: battery voltage and actuator current wired in Sprint 6
        // (Safety + alarms). For now, return 0.
        case INPUT_BATTERY_VOLTAGE_X100:
        case INPUT_ACTUATOR_CURRENT_X100:
            return 0;

        // ----- 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;
        }

        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: stubs in Sprint 1, wired in Sprint 6.
        case DISCRETE_ALARM_OFF_COURSE:
        case DISCRETE_ALARM_OFF_COURSE_SEVERE:
        case DISCRETE_ALARM_RUDDER_NOT_RESP:
        case DISCRETE_ALARM_HEADING_LOST:
        case DISCRETE_ALARM_ACTUATOR_OVERCURR:
        case DISCRETE_ALARM_VOLTAGE_LOW:
        case DISCRETE_ALARM_LIMIT_REACHED:
        case DISCRETE_ALARM_WATCHDOG_TRIPPED:
        case DISCRETE_ALARM_VMS_CRITICAL:
        case DISCRETE_ANY_ALARM_ACTIVE:
            return false;
        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;
        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_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;
        }
        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;
            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