#include "ad_modbus.h"

#include <string.h>

#include "ad_binary_transport.h"
#include "ad_board.h"
#include "ad_inverter.h"
#include "ad_parameter_identification.h"
#include "ad_project_config.h"
#include "simulink_interface.h"
#include "stm32g4xx_hal_uart_ex.h"

#define AD_MODBUS_PROTOCOL_VERSION          (8U)
#define AD_MODBUS_DEVICE_ID                 (0xAD01U)
#define AD_MODBUS_MAX_FRAME_BYTES           (128U)
#define AD_MODBUS_MAX_READ_REGISTERS        (60U)
#define AD_MODBUS_MAX_WRITE_REGISTERS       ((AD_MODBUS_MAX_FRAME_BYTES - 9U) / 2U)
#define AD_MODBUS_HOLDING_40001_BASE        (40001U)
#define AD_MODBUS_MAX_REGISTER_ADDRESS      ((uint16_t)AD_MODBUS_REG_PARAM_PHASE_SHUNT_MOHM)

#define AD_MODBUS_DEFAULT_CURRENT_LIMIT_A   (10.0f)
#define AD_MODBUS_DEFAULT_OVERVOLTAGE_V     (60.0f)
#define AD_MODBUS_DEFAULT_UNDERVOLTAGE_V    (0.0f)
#define AD_MODBUS_DEFAULT_SPEED_LIMIT_RPM   (1000.0f)
#define AD_MODBUS_DEFAULT_TEMPERATURE_C     (85.0f)

volatile AD_ModbusRegisters_t g_ad_modbus_regs;
volatile AD_ModbusRegisters_t * const MB_REGS = &g_ad_modbus_regs;

static UART_HandleTypeDef *g_uart;
static AD_ModbusState_t g_modbus_state;
static uint8_t g_rx_buffer[AD_MODBUS_MAX_FRAME_BYTES];
static uint8_t g_frame_buffer[AD_MODBUS_MAX_FRAME_BYTES];
static uint8_t g_tx_buffer[AD_MODBUS_MAX_FRAME_BYTES];
static volatile uint16_t g_frame_size;
static uint8_t g_locked_rotor_allowed;
static AD_Command_t g_command_shadow;

static uint16_t crc16_modbus(const uint8_t *data, uint16_t size)
{
    uint16_t crc = 0xFFFFU;
    uint16_t i;
    uint8_t bit;

    for (i = 0U; i < size; i++) {
        crc ^= data[i];
        for (bit = 0U; bit < 8U; bit++) {
            if ((crc & 0x0001U) != 0U) {
                crc = (uint16_t)((crc >> 1) ^ 0xA001U);
            } else {
                crc >>= 1;
            }
        }
    }

    return crc;
}

static uint16_t read_be16(const uint8_t *data)
{
    return (uint16_t)(((uint16_t)data[0] << 8) | data[1]);
}

static uint16_t normalize_register_address(uint16_t address)
{
    uint32_t address32 = address;

    if ((address32 >= AD_MODBUS_HOLDING_40001_BASE) &&
        (address32 <= (AD_MODBUS_HOLDING_40001_BASE + AD_MODBUS_MAX_REGISTER_ADDRESS))) {
        return (uint16_t)(address32 - AD_MODBUS_HOLDING_40001_BASE);
    }

    return address;
}

static void write_be16(uint8_t *data, uint16_t value)
{
    data[0] = (uint8_t)(value >> 8);
    data[1] = (uint8_t)(value & 0xFFU);
}

static uint16_t u32_lo(uint32_t value)
{
    return (uint16_t)(value & 0xFFFFU);
}

static uint16_t u32_hi(uint32_t value)
{
    return (uint16_t)((value >> 16) & 0xFFFFU);
}

static uint32_t float_to_u32_scaled(float value, float scale)
{
    float scaled;

    if (value <= 0.0f) {
        return 0UL;
    }

    scaled = value * scale;
    if (scaled > 4294967295.0f) {
        return 0xFFFFFFFFUL;
    }

    return (uint32_t)(scaled + 0.5f);
}

static uint16_t float_to_u16_scaled(float value, float scale)
{
    float scaled = value * scale;

    if (scaled < -32768.0f) {
        return 0x8000U;
    }
    if (scaled > 65535.0f) {
        return 0xFFFFU;
    }
    if (scaled < 0.0f) {
        return (uint16_t)((int16_t)(scaled - 0.5f));
    }

    return (uint16_t)(scaled + 0.5f);
}

static float reg_to_float(uint16_t value, float scale)
{
    return (float)value / scale;
}

static float clamp_rotation_frequency(float frequency_hz)
{
    if (frequency_hz <= 0.0f) {
        return AD_PARAM_ID_DEFAULT_ROTATION_FREQUENCY_HZ;
    }
    if (frequency_hz < 0.1f) {
        return 0.1f;
    }
    if (frequency_hz > AD_PARAM_ID_MAX_ROTATION_FREQUENCY_HZ) {
        return AD_PARAM_ID_MAX_ROTATION_FREQUENCY_HZ;
    }

    return frequency_hz;
}

static uint8_t mode_is_valid(uint16_t mode)
{
    return (uint8_t)((mode <= (uint16_t)AD_PARAM_ID_MODE_ROTATION_3HZ) ? 1U : 0U);
}

static void set_default_command(void)
{
    memset(&g_command_shadow, 0, sizeof(g_command_shadow));
    g_command_shadow.test_mode = AD_PARAM_ID_MODE_IDLE;
    g_command_shadow.pwm_polarity_flags =
        (uint16_t)(AD_INVERTER_DEFAULT_PWM_POLARITY_FLAGS &
                   AD_INVERTER_PWM_POLARITY_MASK);
    g_command_shadow.pwm_timing_mode = (uint16_t)AD_INVERTER_PWM_TIMING_CENTER;
    g_command_shadow.motor_control_type = (uint16_t)AD_PARAM_ID_MOTOR_CONTROL_AD;
    g_command_shadow.pwm_duty_limit = AD_PARAM_ID_DEFAULT_PWM_DUTY_LIMIT;
    g_command_shadow.rotation_frequency_Hz = AD_PARAM_ID_DEFAULT_ROTATION_FREQUENCY_HZ;
    g_command_shadow.rotation_modulation = AD_PARAM_ID_DEFAULT_ROTATION_MODULATION;
    g_command_shadow.rotation_ramp_time_ms =
        (uint16_t)AD_PARAM_ID_DEFAULT_ROTATION_RAMP_TIME_MS;
    g_command_shadow.current_limit_A = AD_MODBUS_DEFAULT_CURRENT_LIMIT_A;
    g_command_shadow.voltage_limit_V = AD_MODBUS_DEFAULT_OVERVOLTAGE_V;
    g_command_shadow.undervoltage_limit_V = AD_MODBUS_DEFAULT_UNDERVOLTAGE_V;
    g_command_shadow.speed_limit_rpm = AD_MODBUS_DEFAULT_SPEED_LIMIT_RPM;
    g_command_shadow.temperature_limit_C = AD_MODBUS_DEFAULT_TEMPERATURE_C;
}

static void refresh_command_shadow(void)
{
    const SimulinkInterface_OutputBus_t *output = SimulinkInterface_GetOutputBus();

    if (output != 0) {
        g_command_shadow = output->command;
    }
}

static void apply_command_shadow(void)
{
    SimulinkInterface_SetCommand(&g_command_shadow);
}

static uint8_t register_is_readable(uint16_t address)
{
    return (uint8_t)(((address >= AD_MODBUS_REG_DEVICE_ID) &&
                     (address <= AD_MODBUS_REG_CONTROL_PWM_POLARITY_FLAGS)) ||
                     ((address >= AD_MODBUS_REG_STATUS_MODE) &&
                      (address <= AD_MODBUS_REG_INVERTER_ID_STAGE)) ||
                      ((address >= AD_MODBUS_REG_CONTROL_PWM_TIMING_MODE) &&
                      (address <= AD_MODBUS_REG_CONTROL_ROTATION_RAMP_TIME_MS)) ||
                     ((address >= AD_MODBUS_REG_MEAS_IA_0P001_A) &&
                      (address <= AD_MODBUS_REG_MEAS_SLIP_0P01_PERCENT)) ||
                     ((address >= AD_MODBUS_REG_PARAM_VALID_MASK_LO) &&
                      (address <= AD_MODBUS_REG_PARAM_PHASE_SHUNT_MOHM)));
}

static uint8_t register_is_writable(uint16_t address)
{
    return (uint8_t)(((address >= AD_MODBUS_REG_CONTROL_ENABLE) &&
                      (address <= AD_MODBUS_REG_CONTROL_PWM_POLARITY_FLAGS)) ||
                      ((address >= AD_MODBUS_REG_CONTROL_PWM_TIMING_MODE) &&
                       (address <= AD_MODBUS_REG_CONTROL_RESET_PHASE_CURRENT_PEAKS)) ||
                      (address == AD_MODBUS_REG_PARAM_POLE_PAIRS) ||
                      (address == AD_MODBUS_REG_PARAM_PHASE_SHUNT_MOHM));
}

static uint16_t read_register_live(uint16_t address)
{
    const AD_Measurements_t *meas = AD_ParamID_GetLastMeasurements();
    const AD_PhaseCurrentPeaks_t *peaks = AD_ParamID_GetPhaseCurrentPeaks();
    const AD_MotorParameters_t *params = AD_ParamID_GetParameters();
    const AD_InverterState_t *inverter = AD_Inverter_GetState();
    uint32_t value32;
    uint32_t status;
    uint32_t faults;

    status = AD_ParamID_GetStatus();
    faults = AD_ParamID_GetFaults();

    switch ((AD_ModbusRegister_t)address) {
    case AD_MODBUS_REG_DEVICE_ID:
        return AD_MODBUS_DEVICE_ID;
    case AD_MODBUS_REG_PROTOCOL_VERSION:
        return AD_MODBUS_PROTOCOL_VERSION;
    case AD_MODBUS_REG_CONTROL_ENABLE:
        return g_command_shadow.enable;
    case AD_MODBUS_REG_CONTROL_MODE:
        return g_command_shadow.test_mode;
    case AD_MODBUS_REG_CONTROL_RESET_FAULTS:
    case AD_MODBUS_REG_CONTROL_STOP:
        return 0U;
    case AD_MODBUS_REG_CONTROL_LOCKED_ROTOR_ALLOWED:
        return g_locked_rotor_allowed;
    case AD_MODBUS_REG_CONTROL_PWM_DUTY_0P0001:
        return float_to_u16_scaled(g_command_shadow.pwm_duty_limit, 10000.0f);
    case AD_MODBUS_REG_LIMIT_CURRENT_0P01_A:
        return float_to_u16_scaled(g_command_shadow.current_limit_A, 100.0f);
    case AD_MODBUS_REG_LIMIT_OVERVOLTAGE_0P1_V:
        return float_to_u16_scaled(g_command_shadow.voltage_limit_V, 10.0f);
    case AD_MODBUS_REG_LIMIT_UNDERVOLTAGE_0P1_V:
        return float_to_u16_scaled(g_command_shadow.undervoltage_limit_V, 10.0f);
    case AD_MODBUS_REG_LIMIT_SPEED_RPM:
        return float_to_u16_scaled(g_command_shadow.speed_limit_rpm, 1.0f);
    case AD_MODBUS_REG_LIMIT_TEMPERATURE_0P1_C:
        return float_to_u16_scaled(g_command_shadow.temperature_limit_C, 10.0f);
    case AD_MODBUS_REG_CONTROL_ROTATION_FREQ_0P1_HZ:
        return float_to_u16_scaled(g_command_shadow.rotation_frequency_Hz, 10.0f);
    case AD_MODBUS_REG_CONTROL_ROTATION_MOD_0P0001:
        return float_to_u16_scaled(g_command_shadow.rotation_modulation, 10000.0f);
    case AD_MODBUS_REG_CONTROL_PWM_POLARITY_FLAGS:
        return (uint16_t)(g_command_shadow.pwm_polarity_flags &
                          (uint16_t)AD_INVERTER_PWM_POLARITY_MASK);

    case AD_MODBUS_REG_STATUS_MODE:
        return AD_ParamID_GetMode();
    case AD_MODBUS_REG_STATUS_FLAGS_LO:
        return u32_lo(status);
    case AD_MODBUS_REG_STATUS_FLAGS_HI:
        return u32_hi(status);
    case AD_MODBUS_REG_FAULT_FLAGS_LO:
        return u32_lo(faults);
    case AD_MODBUS_REG_FAULT_FLAGS_HI:
        return u32_hi(faults);
    case AD_MODBUS_REG_POWER_STAGE_ALLOWED:
        return AD_ParamID_IsPowerStageAllowed();
    case AD_MODBUS_REG_TEST_RUNNING:
        return ((status & AD_PARAM_ID_STATUS_ACTIVE) != 0UL) ? 1U : 0U;
    case AD_MODBUS_REG_INVERTER_PWM_RUNNING:
        return (inverter != 0) ? inverter->pwm_running : 0U;
    case AD_MODBUS_REG_INVERTER_SERVICE_OUTPUT:
        return (inverter != 0) ? inverter->service_output : 0U;
    case AD_MODBUS_REG_INVERTER_ID_STAGE:
        return (inverter != 0) ? inverter->id_stage : 0U;
    case AD_MODBUS_REG_CONTROL_PWM_TIMING_MODE:
        return g_command_shadow.pwm_timing_mode;
    case AD_MODBUS_REG_CONTROL_MOTOR_CONTROL_TYPE:
        return g_command_shadow.motor_control_type;
    case AD_MODBUS_REG_CONTROL_HOST_PROTOCOL:
        return AD_BinaryTransport_GetProtocol();
    case AD_MODBUS_REG_CONTROL_BINARY_TRANSPORT:
        return AD_BinaryTransport_GetTransport();
    case AD_MODBUS_REG_CONTROL_ROTATION_RAMP_TIME_MS:
        return g_command_shadow.rotation_ramp_time_ms;
    case AD_MODBUS_REG_CONTROL_RESET_PHASE_CURRENT_PEAKS:
        return 0U;

    case AD_MODBUS_REG_MEAS_IA_0P001_A:
        return (meas != 0) ? float_to_u16_scaled(meas->ia_A, 1000.0f) : 0U;
    case AD_MODBUS_REG_MEAS_IB_0P001_A:
        return (meas != 0) ? float_to_u16_scaled(meas->ib_A, 1000.0f) : 0U;
    case AD_MODBUS_REG_MEAS_IC_0P001_A:
        return (meas != 0) ? float_to_u16_scaled(meas->ic_A, 1000.0f) : 0U;
    case AD_MODBUS_REG_MEAS_VDC_0P1_V:
        return (meas != 0) ? float_to_u16_scaled(meas->vdc_V, 10.0f) : 0U;
    case AD_MODBUS_REG_MEAS_TEMP_0P1_C:
        return (meas != 0) ? float_to_u16_scaled(meas->temperature_C, 10.0f) : 0U;
    case AD_MODBUS_REG_MEAS_STATUS_LO:
        value32 = (meas != 0) ? meas->status_flags : 0UL;
        return u32_lo(value32);
    case AD_MODBUS_REG_MEAS_STATUS_HI:
        value32 = (meas != 0) ? meas->status_flags : 0UL;
        return u32_hi(value32);
    case AD_MODBUS_REG_MEAS_SPEED_RPM:
        return (meas != 0) ? float_to_u16_scaled(meas->speed_rpm, 1.0f) : 0U;
    case AD_MODBUS_REG_MEAS_IA_RMS_0P001_A:
        return (meas != 0) ? float_to_u16_scaled(meas->ia_rms_A, 1000.0f) : 0U;
    case AD_MODBUS_REG_MEAS_IB_RMS_0P001_A:
        return (meas != 0) ? float_to_u16_scaled(meas->ib_rms_A, 1000.0f) : 0U;
    case AD_MODBUS_REG_MEAS_IC_RMS_0P001_A:
        return (meas != 0) ? float_to_u16_scaled(meas->ic_rms_A, 1000.0f) : 0U;
    case AD_MODBUS_REG_MEAS_TORQUE_0P001_NM:
        return (meas != 0) ? float_to_u16_scaled(meas->torque_Nm, 1000.0f) : 0U;
    case AD_MODBUS_REG_MEAS_IA_PEAK_0P001_A:
        return (peaks != 0) ? float_to_u16_scaled(peaks->ia_peak_A, 1000.0f) : 0U;
    case AD_MODBUS_REG_MEAS_IB_PEAK_0P001_A:
        return (peaks != 0) ? float_to_u16_scaled(peaks->ib_peak_A, 1000.0f) : 0U;
    case AD_MODBUS_REG_MEAS_IC_PEAK_0P001_A:
        return (peaks != 0) ? float_to_u16_scaled(peaks->ic_peak_A, 1000.0f) : 0U;
    case AD_MODBUS_REG_MEAS_SLIP_0P01_PERCENT:
        return (meas != 0) ? float_to_u16_scaled(meas->slip_percent, 100.0f) : 0U;

    case AD_MODBUS_REG_PARAM_VALID_MASK_LO:
        value32 = (params != 0) ? params->valid_mask : 0UL;
        return u32_lo(value32);
    case AD_MODBUS_REG_PARAM_VALID_MASK_HI:
        value32 = (params != 0) ? params->valid_mask : 0UL;
        return u32_hi(value32);
    case AD_MODBUS_REG_PARAM_RS_MOHM:
        return (params != 0) ? float_to_u16_scaled(params->Rs_ohm, 1000.0f) : 0U;
    case AD_MODBUS_REG_PARAM_LS_UH_LO:
        value32 = (params != 0) ? float_to_u32_scaled(params->Ls_H, 1000000.0f) : 0UL;
        return u32_lo(value32);
    case AD_MODBUS_REG_PARAM_LS_UH_HI:
        value32 = (params != 0) ? float_to_u32_scaled(params->Ls_H, 1000000.0f) : 0UL;
        return u32_hi(value32);
    case AD_MODBUS_REG_PARAM_LL_UH_LO:
        value32 = (params != 0) ? float_to_u32_scaled(params->Ll_H, 1000000.0f) : 0UL;
        return u32_lo(value32);
    case AD_MODBUS_REG_PARAM_LL_UH_HI:
        value32 = (params != 0) ? float_to_u32_scaled(params->Ll_H, 1000000.0f) : 0UL;
        return u32_hi(value32);
    case AD_MODBUS_REG_PARAM_RR_MOHM:
        return (params != 0) ? float_to_u16_scaled(params->Rr_ohm, 1000.0f) : 0U;
    case AD_MODBUS_REG_PARAM_LR_UH_LO:
        value32 = (params != 0) ? float_to_u32_scaled(params->Lr_H, 1000000.0f) : 0UL;
        return u32_lo(value32);
    case AD_MODBUS_REG_PARAM_LR_UH_HI:
        value32 = (params != 0) ? float_to_u32_scaled(params->Lr_H, 1000000.0f) : 0UL;
        return u32_hi(value32);
    case AD_MODBUS_REG_PARAM_LM_UH_LO:
        value32 = (params != 0) ? float_to_u32_scaled(params->Lm_H, 1000000.0f) : 0UL;
        return u32_lo(value32);
    case AD_MODBUS_REG_PARAM_LM_UH_HI:
        value32 = (params != 0) ? float_to_u32_scaled(params->Lm_H, 1000000.0f) : 0UL;
        return u32_hi(value32);
    case AD_MODBUS_REG_PARAM_J_NKGM2_LO:
        value32 = (params != 0) ? float_to_u32_scaled(params->J_kg_m2, 1000000000.0f) : 0UL;
        return u32_lo(value32);
    case AD_MODBUS_REG_PARAM_J_NKGM2_HI:
        value32 = (params != 0) ? float_to_u32_scaled(params->J_kg_m2, 1000000000.0f) : 0UL;
        return u32_hi(value32);
    case AD_MODBUS_REG_PARAM_B_NNMS_LO:
        value32 = (params != 0) ? float_to_u32_scaled(params->B_Nm_s, 1000000000.0f) : 0UL;
        return u32_lo(value32);
    case AD_MODBUS_REG_PARAM_B_NNMS_HI:
        value32 = (params != 0) ? float_to_u32_scaled(params->B_Nm_s, 1000000000.0f) : 0UL;
        return u32_hi(value32);
    case AD_MODBUS_REG_PARAM_POLE_PAIRS:
        return AD_ParamID_GetPolePairs();
    case AD_MODBUS_REG_PARAM_PHASE_SHUNT_MOHM:
        return float_to_u16_scaled(AD_Board_GetPhaseShuntResistanceOhm(), 1000.0f);
    default:
        return 0U;
    }
}

void AD_Modbus_RefreshRegisters(void)
{
    uint16_t address;

    refresh_command_shadow();
    for (address = 0U; address < AD_MODBUS_HOLDING_REGISTER_COUNT; address++) {
        if (register_is_readable(address) != 0U) {
            MB_REGS->holding_raw[address] = read_register_live(address);
        } else {
            MB_REGS->holding_raw[address] = 0U;
        }
    }
}

static uint16_t read_register(uint16_t address)
{
    if (address < AD_MODBUS_HOLDING_REGISTER_COUNT) {
        return MB_REGS->holding_raw[address];
    }

    return 0U;
}

typedef struct
{
    uint8_t apply_command;
    uint8_t reset_faults;
    uint8_t stop_requested;
    uint8_t locked_rotor_changed;
    uint8_t pole_pairs_changed;
    uint8_t phase_shunt_changed;
    uint8_t host_protocol_changed;
    uint8_t binary_transport_changed;
    uint8_t reset_phase_current_peaks;
    uint16_t host_protocol;
    uint16_t binary_transport;
    uint16_t pole_pairs;
    uint16_t phase_shunt_mohm;
} AD_ModbusWriteEffects_t;

static void write_effects_clear(AD_ModbusWriteEffects_t *effects)
{
    if (effects != 0) {
        memset(effects, 0, sizeof(*effects));
    }
}

static uint8_t stage_register_write(uint16_t address,
                                    uint16_t value,
                                    AD_Command_t *command,
                                    uint8_t *locked_rotor_allowed,
                                    uint16_t *pole_pairs,
                                    AD_ModbusWriteEffects_t *effects)
{
    if ((command == 0) || (locked_rotor_allowed == 0) ||
        (pole_pairs == 0) || (effects == 0)) {
        return 0U;
    }

    switch ((AD_ModbusRegister_t)address) {
    case AD_MODBUS_REG_CONTROL_ENABLE:
        command->enable = (value != 0U) ? 1U : 0U;
        if (command->enable == 0U) {
            command->test_mode = AD_PARAM_ID_MODE_IDLE;
            effects->stop_requested = 1U;
        }
        effects->apply_command = 1U;
        break;

    case AD_MODBUS_REG_CONTROL_MODE:
        if (mode_is_valid(value) == 0U) {
            return 0U;
        }
        command->test_mode = (uint8_t)value;
        effects->apply_command = 1U;
        break;

    case AD_MODBUS_REG_CONTROL_RESET_FAULTS:
        if (value != 0U) {
            effects->reset_faults = 1U;
        }
        break;

    case AD_MODBUS_REG_CONTROL_STOP:
        if (value != 0U) {
            command->enable = 0U;
            command->test_mode = AD_PARAM_ID_MODE_IDLE;
            effects->apply_command = 1U;
            effects->stop_requested = 1U;
        }
        break;

    case AD_MODBUS_REG_CONTROL_LOCKED_ROTOR_ALLOWED:
        *locked_rotor_allowed = (value != 0U) ? 1U : 0U;
        effects->locked_rotor_changed = 1U;
        break;

    case AD_MODBUS_REG_CONTROL_PWM_DUTY_0P0001:
        command->pwm_duty_limit = reg_to_float(value, 10000.0f);
        effects->apply_command = 1U;
        break;

    case AD_MODBUS_REG_LIMIT_CURRENT_0P01_A:
        command->current_limit_A = reg_to_float(value, 100.0f);
        effects->apply_command = 1U;
        break;

    case AD_MODBUS_REG_LIMIT_OVERVOLTAGE_0P1_V:
        command->voltage_limit_V = reg_to_float(value, 10.0f);
        effects->apply_command = 1U;
        break;

    case AD_MODBUS_REG_LIMIT_UNDERVOLTAGE_0P1_V:
        command->undervoltage_limit_V = reg_to_float(value, 10.0f);
        effects->apply_command = 1U;
        break;

    case AD_MODBUS_REG_LIMIT_SPEED_RPM:
        command->speed_limit_rpm = (float)value;
        effects->apply_command = 1U;
        break;

    case AD_MODBUS_REG_LIMIT_TEMPERATURE_0P1_C:
        command->temperature_limit_C = reg_to_float(value, 10.0f);
        effects->apply_command = 1U;
        break;

    case AD_MODBUS_REG_CONTROL_ROTATION_FREQ_0P1_HZ:
        command->rotation_frequency_Hz =
            clamp_rotation_frequency(reg_to_float(value, 10.0f));
        effects->apply_command = 1U;
        break;

    case AD_MODBUS_REG_CONTROL_ROTATION_MOD_0P0001:
        command->rotation_modulation = reg_to_float(value, 10000.0f);
        effects->apply_command = 1U;
        break;

    case AD_MODBUS_REG_CONTROL_PWM_POLARITY_FLAGS:
        command->pwm_polarity_flags =
            (uint16_t)(value & (uint16_t)AD_INVERTER_PWM_POLARITY_MASK);
        effects->apply_command = 1U;
        break;

    case AD_MODBUS_REG_CONTROL_PWM_TIMING_MODE:
        command->pwm_timing_mode =
            (value == (uint16_t)AD_INVERTER_PWM_TIMING_UP) ?
            (uint16_t)AD_INVERTER_PWM_TIMING_UP :
            (uint16_t)AD_INVERTER_PWM_TIMING_CENTER;
        effects->apply_command = 1U;
        break;

    case AD_MODBUS_REG_CONTROL_MOTOR_CONTROL_TYPE:
        command->motor_control_type =
            (value == (uint16_t)AD_PARAM_ID_MOTOR_CONTROL_BLDC) ?
            (uint16_t)AD_PARAM_ID_MOTOR_CONTROL_BLDC :
            (uint16_t)AD_PARAM_ID_MOTOR_CONTROL_AD;
        effects->apply_command = 1U;
        break;

    case AD_MODBUS_REG_CONTROL_HOST_PROTOCOL:
        if (AD_BinaryTransport_IsProtocolValid(value) == 0U) {
            return 0U;
        }
        effects->host_protocol = value;
        effects->host_protocol_changed = 1U;
        break;

    case AD_MODBUS_REG_CONTROL_BINARY_TRANSPORT:
        if (AD_BinaryTransport_IsTransportValid(value) == 0U) {
            return 0U;
        }
        effects->binary_transport = value;
        effects->binary_transport_changed = 1U;
        break;

    case AD_MODBUS_REG_CONTROL_ROTATION_RAMP_TIME_MS:
        command->rotation_ramp_time_ms = value;
        effects->apply_command = 1U;
        break;

    case AD_MODBUS_REG_CONTROL_RESET_PHASE_CURRENT_PEAKS:
        if (value != 0U) {
            effects->reset_phase_current_peaks = 1U;
        }
        break;

    case AD_MODBUS_REG_PARAM_POLE_PAIRS:
        if ((value != 0U) &&
            (value > (uint16_t)AD_PARAM_ID_MAX_POLE_PAIRS)) {
            return 0U;
        }
        *pole_pairs = value;
        effects->pole_pairs = value;
        effects->pole_pairs_changed = 1U;
        break;

    case AD_MODBUS_REG_PARAM_PHASE_SHUNT_MOHM:
        effects->phase_shunt_mohm = value;
        effects->phase_shunt_changed = 1U;
        break;

    default:
        return 0U;
    }

    return 1U;
}

static void apply_write_effects(const AD_ModbusWriteEffects_t *effects)
{
    if (effects == 0) {
        return;
    }

    if (effects->reset_faults != 0U) {
        AD_Board_ResetAdcFaults();
        AD_ParamID_Reset();
    }
    if (effects->reset_phase_current_peaks != 0U) {
        AD_ParamID_ResetPhaseCurrentPeaks();
    }
    if (effects->locked_rotor_changed != 0U) {
        AD_ParamID_SetLockedRotorAllowed(g_locked_rotor_allowed);
    }
    if (effects->pole_pairs_changed != 0U) {
        AD_ParamID_SetPolePairs(effects->pole_pairs);
    }
    if (effects->phase_shunt_changed != 0U) {
        AD_Board_SetPhaseShuntResistanceOhm(reg_to_float(effects->phase_shunt_mohm, 1000.0f));
    }
    if (effects->apply_command != 0U) {
        apply_command_shadow();
    }
    if (effects->binary_transport_changed != 0U) {
        (void)AD_BinaryTransport_SetTransport(effects->binary_transport);
    }
    if (effects->host_protocol_changed != 0U) {
        (void)AD_BinaryTransport_SetProtocol(effects->host_protocol);
    }
    if (effects->stop_requested != 0U) {
        AD_ParamID_Stop();
    }
}

static uint8_t write_register(uint16_t address, uint16_t value)
{
    AD_Command_t staged_command;
    AD_ModbusWriteEffects_t effects;
    uint8_t staged_locked_rotor_allowed;
    uint16_t staged_pole_pairs;

    refresh_command_shadow();
    staged_command = g_command_shadow;
    staged_locked_rotor_allowed = g_locked_rotor_allowed;
    staged_pole_pairs = AD_ParamID_GetPolePairs();
    write_effects_clear(&effects);

    if (stage_register_write(address,
                             value,
                             &staged_command,
                             &staged_locked_rotor_allowed,
                             &staged_pole_pairs,
                             &effects) == 0U) {
        return 0U;
    }

    if (effects.stop_requested != 0U) {
        staged_command.enable = 0U;
        staged_command.test_mode = AD_PARAM_ID_MODE_IDLE;
    }
    g_command_shadow = staged_command;
    g_locked_rotor_allowed = staged_locked_rotor_allowed;
    apply_write_effects(&effects);
    g_modbus_state.write_count++;
    AD_Modbus_RefreshRegisters();
    return 1U;
}

static void send_response(uint16_t size)
{
    uint16_t crc;

    if ((g_uart == 0) || (size > (AD_MODBUS_MAX_FRAME_BYTES - 2U))) {
        return;
    }

    crc = crc16_modbus(g_tx_buffer, size);
    g_tx_buffer[size++] = (uint8_t)(crc & 0xFFU);
    g_tx_buffer[size++] = (uint8_t)(crc >> 8);
    (void)HAL_UART_Transmit(g_uart, g_tx_buffer, size, 20U);
    g_modbus_state.tx_frames++;
}

static void restart_uart_rx(void)
{
    if (g_uart == 0) {
        return;
    }

    if (HAL_UARTEx_ReceiveToIdle_IT(g_uart, g_rx_buffer, AD_MODBUS_MAX_FRAME_BYTES) == HAL_OK) {
        g_modbus_state.status_flags |= AD_MODBUS_STATUS_UART_RX_ACTIVE;
    } else {
        g_modbus_state.status_flags &= ~AD_MODBUS_STATUS_UART_RX_ACTIVE;
    }
}

static void send_exception(uint8_t slave, uint8_t function, uint8_t exception)
{
    g_tx_buffer[0] = slave;
    g_tx_buffer[1] = function | 0x80U;
    g_tx_buffer[2] = exception;
    g_modbus_state.exception_errors++;
    send_response(3U);
}

static uint8_t range_is_readable(uint16_t start, uint16_t quantity)
{
    uint16_t i;

    if ((quantity == 0U) || (quantity > AD_MODBUS_MAX_READ_REGISTERS)) {
        return 0U;
    }

    for (i = 0U; i < quantity; i++) {
        if (register_is_readable((uint16_t)(start + i)) == 0U) {
            return 0U;
        }
    }

    return 1U;
}

static uint8_t range_is_writable(uint16_t start, uint16_t quantity)
{
    uint16_t i;

    if (quantity == 0U) {
        return 0U;
    }

    for (i = 0U; i < quantity; i++) {
        if (register_is_writable((uint16_t)(start + i)) == 0U) {
            return 0U;
        }
    }

    return 1U;
}

static void process_read_holding(uint8_t slave, const uint8_t *frame, uint16_t size)
{
    uint16_t start;
    uint16_t quantity;
    uint16_t i;
    uint16_t value;

    if (size != 8U) {
        send_exception(slave, frame[1], AD_MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE);
        return;
    }

    start = normalize_register_address(read_be16(&frame[2]));
    quantity = read_be16(&frame[4]);
    if (range_is_readable(start, quantity) == 0U) {
        send_exception(slave, frame[1], AD_MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS);
        return;
    }

    AD_Modbus_RefreshRegisters();
    g_tx_buffer[0] = slave;
    g_tx_buffer[1] = frame[1];
    g_tx_buffer[2] = (uint8_t)(quantity * 2U);
    for (i = 0U; i < quantity; i++) {
        value = read_register((uint16_t)(start + i));
        write_be16(&g_tx_buffer[3U + (i * 2U)], value);
    }

    send_response((uint16_t)(3U + (quantity * 2U)));
}

static void process_write_single(uint8_t slave, const uint8_t *frame, uint16_t size, uint8_t broadcast)
{
    uint16_t address;
    uint16_t value;

    if (size != 8U) {
        if (broadcast == 0U) {
            send_exception(slave, frame[1], AD_MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE);
        }
        return;
    }

    address = normalize_register_address(read_be16(&frame[2]));
    value = read_be16(&frame[4]);
    if (register_is_writable(address) == 0U) {
        if (broadcast == 0U) {
            send_exception(slave, frame[1], AD_MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS);
        }
        return;
    }

    if (write_register(address, value) == 0U) {
        if (broadcast == 0U) {
            send_exception(slave, frame[1], AD_MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE);
        }
        return;
    }

    if (broadcast == 0U) {
        memcpy(g_tx_buffer, frame, 6U);
        send_response(6U);
    }
}

static void process_write_multiple(uint8_t slave, const uint8_t *frame, uint16_t size, uint8_t broadcast)
{
    AD_Command_t staged_command;
    AD_ModbusWriteEffects_t effects;
    uint16_t start;
    uint16_t quantity;
    uint16_t i;
    uint16_t byte_count;
    uint16_t value;
    uint8_t staged_locked_rotor_allowed;
    uint16_t staged_pole_pairs;

    if (size < 9U) {
        if (broadcast == 0U) {
            send_exception(slave, frame[1], AD_MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE);
        }
        return;
    }

    start = normalize_register_address(read_be16(&frame[2]));
    quantity = read_be16(&frame[4]);
    byte_count = frame[6];

    if ((quantity == 0U) ||
        (quantity > AD_MODBUS_MAX_WRITE_REGISTERS) ||
        (byte_count != (quantity * 2U)) ||
        (size != (uint16_t)(9U + byte_count)) ||
        (range_is_writable(start, quantity) == 0U)) {
        if (broadcast == 0U) {
            send_exception(slave, frame[1], AD_MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS);
        }
        return;
    }

    refresh_command_shadow();
    staged_command = g_command_shadow;
    staged_locked_rotor_allowed = g_locked_rotor_allowed;
    staged_pole_pairs = AD_ParamID_GetPolePairs();
    write_effects_clear(&effects);

    for (i = 0U; i < quantity; i++) {
        value = read_be16(&frame[7U + (i * 2U)]);
        if (stage_register_write((uint16_t)(start + i),
                                 value,
                                 &staged_command,
                                 &staged_locked_rotor_allowed,
                                 &staged_pole_pairs,
                                 &effects) == 0U) {
            if (broadcast == 0U) {
                send_exception(slave, frame[1], AD_MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE);
            }
            return;
        }
    }

    if (effects.stop_requested != 0U) {
        staged_command.enable = 0U;
        staged_command.test_mode = AD_PARAM_ID_MODE_IDLE;
    }
    g_command_shadow = staged_command;
    g_locked_rotor_allowed = staged_locked_rotor_allowed;
    apply_write_effects(&effects);
    g_modbus_state.write_count = (uint32_t)(g_modbus_state.write_count + quantity);
    AD_Modbus_RefreshRegisters();

    if (broadcast == 0U) {
        g_tx_buffer[0] = slave;
        g_tx_buffer[1] = frame[1];
        write_be16(&g_tx_buffer[2], read_be16(&frame[2]));
        write_be16(&g_tx_buffer[4], quantity);
        send_response(6U);
    }
}

static void process_frame(const uint8_t *frame, uint16_t size)
{
    uint8_t slave;
    uint8_t function;
    uint8_t broadcast;
    uint16_t received_crc;
    uint16_t computed_crc;

    if (size < 4U) {
        return;
    }

    slave = frame[0];
    if ((slave != AD_MODBUS_SLAVE_ADDRESS) && (slave != 0U)) {
        return;
    }

    received_crc = (uint16_t)frame[size - 2U] | ((uint16_t)frame[size - 1U] << 8);
    computed_crc = crc16_modbus(frame, (uint16_t)(size - 2U));
    if (received_crc != computed_crc) {
        g_modbus_state.crc_errors++;
        g_modbus_state.status_flags |= AD_MODBUS_STATUS_LAST_FRAME_ERROR;
        g_modbus_state.status_flags &= ~AD_MODBUS_STATUS_LAST_FRAME_OK;
        return;
    }

    function = frame[1];
    broadcast = (slave == 0U) ? 1U : 0U;
    g_modbus_state.rx_frames++;
    g_modbus_state.last_function = function;
    g_modbus_state.last_address = (size >= 4U) ? normalize_register_address(read_be16(&frame[2])) : 0U;
    g_modbus_state.last_quantity = (size >= 6U) ? read_be16(&frame[4]) : 0U;
    g_modbus_state.status_flags |= AD_MODBUS_STATUS_LAST_FRAME_OK;
    g_modbus_state.status_flags &= ~AD_MODBUS_STATUS_LAST_FRAME_ERROR;

    switch ((AD_ModbusFunction_t)function) {
    case AD_MODBUS_FUNCTION_READ_HOLDING_REGISTERS:
        if (broadcast == 0U) {
            process_read_holding(slave, frame, size);
        }
        break;
    case AD_MODBUS_FUNCTION_WRITE_SINGLE_REGISTER:
        process_write_single(slave, frame, size, broadcast);
        break;
    case AD_MODBUS_FUNCTION_WRITE_MULTIPLE_REGISTERS:
        process_write_multiple(slave, frame, size, broadcast);
        break;
    default:
        if (broadcast == 0U) {
            send_exception(slave, function, AD_MODBUS_EXCEPTION_ILLEGAL_FUNCTION);
        }
        break;
    }
}

void AD_Modbus_Init(UART_HandleTypeDef *uart)
{
    g_uart = uart;
    memset(&g_modbus_state, 0, sizeof(g_modbus_state));
    memset(g_rx_buffer, 0, sizeof(g_rx_buffer));
    memset(g_frame_buffer, 0, sizeof(g_frame_buffer));
    memset(g_tx_buffer, 0, sizeof(g_tx_buffer));
    memset((void *)&g_ad_modbus_regs, 0, sizeof(g_ad_modbus_regs));
    g_frame_size = 0U;
    g_locked_rotor_allowed = 0U;
    set_default_command();

    g_modbus_state.initialized = (uart != 0) ? 1U : 0U;
    g_modbus_state.slave_address = AD_MODBUS_SLAVE_ADDRESS;
    g_modbus_state.baudrate = AD_MODBUS_UART_BAUDRATE;
    if (g_modbus_state.initialized != 0U) {
        g_modbus_state.status_flags = AD_MODBUS_STATUS_INITIALIZED;
        restart_uart_rx();
    }
    AD_Modbus_RefreshRegisters();
}

void AD_Modbus_Loop(void)
{
#if AD_MODBUS_ENABLE != 0
    uint8_t frame[AD_MODBUS_MAX_FRAME_BYTES];
    uint16_t frame_size = 0U;
    uint32_t primask;

    AD_Modbus_RefreshRegisters();

    if ((g_uart == 0) || (g_modbus_state.initialized == 0U)) {
        return;
    }

    if (g_frame_size > 0U) {
        primask = __get_PRIMASK();
        __disable_irq();
        frame_size = g_frame_size;
        if (frame_size > AD_MODBUS_MAX_FRAME_BYTES) {
            frame_size = AD_MODBUS_MAX_FRAME_BYTES;
        }
        memcpy(frame, g_frame_buffer, frame_size);
        g_frame_size = 0U;
        g_modbus_state.rx_count = 0U;
        if (primask == 0UL) {
            __enable_irq();
        }

        if (frame_size > 0U) {
            process_frame(frame, frame_size);
            AD_Modbus_RefreshRegisters();
            g_modbus_state.status_flags &= ~AD_MODBUS_STATUS_FRAME_READY;
        }
    }
#endif
}

void HAL_UARTEx_RxEventCallback(UART_HandleTypeDef *huart, uint16_t Size)
{
    HAL_UART_RxEventTypeTypeDef event;
    uint16_t size = Size;

    if ((g_uart == 0) || (huart != g_uart)) {
        return;
    }

    event = HAL_UARTEx_GetRxEventType(huart);
    if ((event == HAL_UART_RXEVENT_TC) && (size >= AD_MODBUS_MAX_FRAME_BYTES)) {
        g_modbus_state.rx_overflows++;
        g_modbus_state.status_flags |= AD_MODBUS_STATUS_RX_OVERFLOW;
        g_modbus_state.status_flags |= AD_MODBUS_STATUS_LAST_FRAME_ERROR;
        restart_uart_rx();
        return;
    }

    if (size > AD_MODBUS_MAX_FRAME_BYTES) {
        size = AD_MODBUS_MAX_FRAME_BYTES;
    }

    if (size > 0U) {
        if (g_frame_size == 0U) {
            memcpy(g_frame_buffer, g_rx_buffer, size);
            g_frame_size = size;
            g_modbus_state.rx_count = size;
            g_modbus_state.status_flags |= AD_MODBUS_STATUS_FRAME_READY;
        } else {
            g_modbus_state.rx_overflows++;
            g_modbus_state.status_flags |= AD_MODBUS_STATUS_RX_OVERFLOW;
            g_modbus_state.status_flags |= AD_MODBUS_STATUS_LAST_FRAME_ERROR;
        }
    }

    restart_uart_rx();
}

void HAL_UART_ErrorCallback(UART_HandleTypeDef *huart)
{
    if ((g_uart == 0) || (huart != g_uart)) {
        return;
    }

    g_modbus_state.uart_errors++;
    g_modbus_state.status_flags |= AD_MODBUS_STATUS_LAST_FRAME_ERROR;

    if (huart->RxState != HAL_UART_STATE_BUSY_RX) {
        restart_uart_rx();
    }
}

const AD_ModbusState_t* AD_Modbus_GetState(void)
{
    return &g_modbus_state;
}

const volatile AD_ModbusRegisters_t* AD_Modbus_GetRegisters(void)
{
    AD_Modbus_RefreshRegisters();
    return MB_REGS;
}
