#include "plsr_modbus_control.h"
#include "modbus_data_store.h"
#include "plsr_address_map.h"
#include "plsr_core.h"
#include "plsr_job.h"
#include "plsr_modbus_data.h"
#include <stddef.h>
#include <string.h>

#define PLSR_MODBUS_MAGIC_LOW             (0x504CU)
#define PLSR_MODBUS_MAGIC_HIGH            (0x5352U)
#define PLSR_MODBUS_CAPABILITIES          (0x0007U)
#define PLSR_MODBUS_CALL_REQUEST_WORDS    (16UL)
#define PLSR_MODBUS_CALL_RESPONSE_WORDS   (12UL)
#define PLSR_MODBUS_COMMAND_REQUEST_WORDS (8UL)
#define PLSR_MODBUS_COMMAND_RESPONSE_WORDS (8UL)
#define PLSR_MODBUS_S0_HEADER_WORDS       (10UL)
#define PLSR_MODBUS_S0_SEGMENT_WORDS      (10UL)
#define PLSR_MODBUS_S1_WORDS              (4UL)
#define PLSR_MODBUS_HASH_OFFSET           (2166136261UL)
#define PLSR_MODBUS_HASH_PRIME            (16777619UL)

typedef struct
{
    PLSR_CALL call;
    uint32_t fingerprint;
    uint8_t valid;
} PLSR_MODBUS_COMMITTED_CALL;

static uint16_t PlsrModbusBaseAddress;
static uint8_t PlsrModbusEnabled;
static uint32_t PlsrModbusLastCallRequestSequence;
static uint32_t PlsrModbusLastCommandRequestSequence;
static uint32_t PlsrModbusStatusGeneration[PLSR_AXIS_COUNT];
static PLSR_MODBUS_COMMITTED_CALL PlsrModbusCommitted[PLSR_AXIS_COUNT];
static uint16_t PlsrModbusStatusWords[PLSR_AXIS_COUNT]
                                      [PLSR_MODBUS_AXIS_STATUS_WORDS];
static const uint16_t PlsrModbusZeroWindow[PLSR_MODBUS_WINDOW_WORDS] = {0U};

static void PlsrModbusPutU32(uint16_t *words,
                             uint32_t offset,
                             uint32_t value)
{
    words[offset] = (uint16_t)(value & 0xFFFFUL);
    words[offset + 1UL] = (uint16_t)(value >> 16U);
}

static void PlsrModbusPutU64(uint16_t *words,
                             uint32_t offset,
                             uint64_t value)
{
    words[offset] = (uint16_t)(value & 0xFFFFULL);
    words[offset + 1UL] = (uint16_t)((value >> 16U) & 0xFFFFULL);
    words[offset + 2UL] = (uint16_t)((value >> 32U) & 0xFFFFULL);
    words[offset + 3UL] = (uint16_t)(value >> 48U);
}

static uint32_t PlsrModbusGetU32(const uint16_t *words, uint32_t offset)
{
    return ((uint32_t)words[offset + 1UL] << 16U) | words[offset];
}

static uint64_t PlsrModbusGetU64(const uint16_t *words, uint32_t offset)
{
    return ((uint64_t)words[offset + 3UL] << 48U)
           | ((uint64_t)words[offset + 2UL] << 32U)
           | ((uint64_t)words[offset + 1UL] << 16U)
           | words[offset];
}

static uint8_t PlsrModbusReadWords(uint32_t offset,
                                   uint16_t *words,
                                   uint32_t wordCount)
{
    uint32_t index;

    if (words == NULL)
    {
        return 0U;
    }
    for (index = 0UL; index < wordCount; index++)
    {
        if (ModbusDataReadWord(MODBUS_DATA_DEVICE_D,
                               (uint32_t)PlsrModbusBaseAddress + offset + index,
                               &words[index]) == 0U)
        {
            return 0U;
        }
    }
    return 1U;
}

static uint8_t PlsrModbusRangesOverlap(uint32_t firstA,
                                       uint32_t countA,
                                       uint32_t firstB,
                                       uint32_t countB)
{
    return ((firstA < (firstB + countB)) && (firstB < (firstA + countA)))
               ? 1U
               : 0U;
}

static uint32_t PlsrModbusHashWord(uint32_t hash, uint16_t value)
{
    hash ^= (uint8_t)(value & 0x00FFU);
    hash *= PLSR_MODBUS_HASH_PRIME;
    hash ^= (uint8_t)(value >> 8U);
    hash *= PLSR_MODBUS_HASH_PRIME;
    return hash;
}

static uint32_t PlsrModbusHashU32(uint32_t hash, uint32_t value)
{
    hash = PlsrModbusHashWord(hash, (uint16_t)(value & 0xFFFFUL));
    return PlsrModbusHashWord(hash, (uint16_t)(value >> 16U));
}

static PLSR_RESULT PlsrModbusFingerprintCall(const PLSR_CALL *call,
                                              uint32_t *fingerprint)
{
    uint16_t word;
    int32_t segmentCount;
    uint32_t s0Words;
    uint32_t index;
    uint32_t hash = PLSR_MODBUS_HASH_OFFSET;

    if ((call == NULL) || (fingerprint == NULL)
        || (call->source.readDword == NULL)
        || (call->source.readWord == NULL)
        || (call->source.validateWords == NULL))
    {
        return PLSR_RESULT_INVALID_ARGUMENT;
    }
    if (call->source.readDword(call->source.context,
                               call->s0.device,
                               call->s0.address,
                               &segmentCount) == 0U)
    {
        return PLSR_RESULT_DATA_ACCESS;
    }
    if ((segmentCount < 1) || (segmentCount > (int32_t)PLSR_MAX_SEGMENTS))
    {
        return PLSR_RESULT_SEGMENT_OVERFLOW;
    }
    s0Words = PLSR_MODBUS_S0_HEADER_WORDS
              + (uint32_t)segmentCount * PLSR_MODBUS_S0_SEGMENT_WORDS;
    if ((call->source.validateWords(call->source.context,
                                    call->s0.device,
                                    call->s0.address,
                                    s0Words) == 0U)
        || (call->source.validateWords(call->source.context,
                                       call->s1.device,
                                       call->s1.address,
                                       PLSR_MODBUS_S1_WORDS) == 0U))
    {
        return PLSR_RESULT_DATA_ACCESS;
    }
    if (((call->s0.device == PLSR_DEVICE_D)
         && (PlsrModbusRangesOverlap(call->s0.address,
                                     s0Words,
                                     PlsrModbusBaseAddress,
                                     PLSR_MODBUS_WINDOW_WORDS) != 0U))
        || ((call->s1.device == PLSR_DEVICE_D)
            && (PlsrModbusRangesOverlap(call->s1.address,
                                        PLSR_MODBUS_S1_WORDS,
                                        PlsrModbusBaseAddress,
                                        PLSR_MODBUS_WINDOW_WORDS) != 0U)))
    {
        return PLSR_RESULT_BLOCK_OVERLAP;
    }

    hash = PlsrModbusHashWord(hash, (uint16_t)call->s0.device);
    hash = PlsrModbusHashU32(hash, call->s0.address);
    for (index = 0UL; index < s0Words; index++)
    {
        if (call->source.readWord(call->source.context,
                                  call->s0.device,
                                  call->s0.address + index,
                                  &word) == 0U)
        {
            return PLSR_RESULT_DATA_ACCESS;
        }
        hash = PlsrModbusHashWord(hash, word);
    }
    hash = PlsrModbusHashWord(hash, (uint16_t)call->s1.device);
    hash = PlsrModbusHashU32(hash, call->s1.address);
    for (index = 0UL; index < PLSR_MODBUS_S1_WORDS; index++)
    {
        if (call->source.readWord(call->source.context,
                                  call->s1.device,
                                  call->s1.address + index,
                                  &word) == 0U)
        {
            return PLSR_RESULT_DATA_ACCESS;
        }
        hash = PlsrModbusHashWord(hash, word);
    }
    hash = PlsrModbusHashWord(hash, (uint16_t)call->s2.type);
    hash = PlsrModbusHashWord(hash, (uint16_t)call->s2.data.device);
    hash = PlsrModbusHashU32(hash, call->s2.data.address);
    hash = PlsrModbusHashU32(hash, (uint32_t)call->s2.constant);
    if (call->s2.type == PLSR_OPERAND_DATA)
    {
        for (index = 0UL; index < 2UL; index++)
        {
            if (call->source.readWord(call->source.context,
                                      call->s2.data.device,
                                      call->s2.data.address + index,
                                      &word) == 0U)
            {
                return PLSR_RESULT_DATA_ACCESS;
            }
            hash = PlsrModbusHashWord(hash, word);
        }
    }
    hash = PlsrModbusHashWord(hash, call->dAxis);
    hash = PlsrModbusHashWord(hash, call->outputModeOverride);
    *fingerprint = hash;
    return PLSR_RESULT_OK;
}

static PLSR_RESULT PlsrModbusBuildCall(const uint16_t *request,
                                        PLSR_CALL *call)
{
    uint16_t s2Type;
    uint16_t outputMode;

    if ((request == NULL) || (call == NULL))
    {
        return PLSR_RESULT_INVALID_ARGUMENT;
    }
    s2Type = request[8UL];
    outputMode = request[13UL];
    if ((request[2UL] > (uint16_t)PLSR_DEVICE_FD)
        || (request[5UL] > (uint16_t)PLSR_DEVICE_FD)
        || (s2Type > (uint16_t)PLSR_OPERAND_DATA)
        || ((s2Type == (uint16_t)PLSR_OPERAND_DATA)
            && (request[9UL] > (uint16_t)PLSR_DEVICE_FD))
        || (request[12UL] >= PLSR_AXIS_COUNT)
        || ((outputMode > (uint16_t)PLSR_OUTPUT_CW_CCW)
            && (outputMode != PLSR_OUTPUT_MODE_FROM_SFD)))
    {
        return PLSR_RESULT_INVALID_ARGUMENT;
    }

    (void)memset(call, 0, sizeof(*call));
    call->sequence = PlsrModbusGetU32(request, 0UL);
    call->s0.device = (PLSR_DEVICE_TYPE)request[2UL];
    call->s0.address = PlsrModbusGetU32(request, 3UL);
    call->s1.device = (PLSR_DEVICE_TYPE)request[5UL];
    call->s1.address = PlsrModbusGetU32(request, 6UL);
    call->s2.type = (PLSR_OPERAND_TYPE)s2Type;
    call->s2.data.device = (PLSR_DEVICE_TYPE)request[9UL];
    call->s2.data.address = PlsrModbusGetU32(request, 10UL);
    call->s2.constant = (int32_t)PlsrModbusGetU32(request, 10UL);
    call->dAxis = (uint8_t)request[12UL];
    call->outputModeOverride = (uint8_t)outputMode;
    PlsrModbusDataSourceInit(&call->source);
    return PLSR_RESULT_OK;
}

static void PlsrModbusPublishCallResponse(uint32_t sequence,
                                          uint16_t operation,
                                          PLSR_RESULT result,
                                          const PLSR_PARSE_DETAIL *detail,
                                          uint8_t committed)
{
    uint16_t response[PLSR_MODBUS_CALL_RESPONSE_WORDS] = {0U};

    PlsrModbusPutU32(response, 0UL, sequence);
    response[2UL] = operation;
    response[3UL] = (uint16_t)result;
    if (detail != NULL)
    {
        response[4UL] = (uint16_t)detail->result;
        response[5UL] = (uint16_t)detail->block;
        PlsrModbusPutU32(response, 6UL, detail->address);
        PlsrModbusPutU32(response, 8UL, (uint32_t)detail->value);
        response[10UL] = detail->segment;
    }
    response[11UL] = committed;
    (void)ModbusDataWriteWords(MODBUS_DATA_DEVICE_D,
                               (uint32_t)PlsrModbusBaseAddress
                                   + PLSR_MODBUS_CALL_RESPONSE_OFFSET,
                               response,
                               PLSR_MODBUS_CALL_RESPONSE_WORDS);
}

static void PlsrModbusHandleCallRequest(void)
{
    uint16_t request[PLSR_MODBUS_CALL_REQUEST_WORDS];
    PLSR_CALL call;
    PLSR_PARSE_DETAIL detail;
    PLSR_RESULT result;
    uint32_t sequence;
    uint32_t fingerprint;
    uint16_t operation;
    uint8_t axis = 0U;

    if (PlsrModbusReadWords(PLSR_MODBUS_CALL_REQUEST_OFFSET,
                            request,
                            PLSR_MODBUS_CALL_REQUEST_WORDS) == 0U)
    {
        return;
    }
    sequence = PlsrModbusGetU32(request, 0UL);
    if ((sequence == 0UL) || (sequence == PlsrModbusLastCallRequestSequence))
    {
        return;
    }
    PlsrModbusLastCallRequestSequence = sequence;
    operation = request[14UL];
    (void)memset(&detail, 0, sizeof(detail));
    result = PlsrModbusBuildCall(request, &call);
    if (result == PLSR_RESULT_OK)
    {
        axis = call.dAxis;
    }

    if ((result == PLSR_RESULT_OK) && (operation == PLSR_MODBUS_CALL_COMMIT))
    {
        result = PlsrValidateCall(&call, &detail);
        if (result == PLSR_RESULT_OK)
        {
            result = PlsrModbusFingerprintCall(&call, &fingerprint);
        }
        if (result == PLSR_RESULT_OK)
        {
            PlsrModbusCommitted[axis].call = call;
            PlsrModbusCommitted[axis].fingerprint = fingerprint;
            PlsrModbusCommitted[axis].valid = 1U;
        }
        else
        {
            PlsrModbusCommitted[axis].valid = 0U;
        }
    }
    else if ((result == PLSR_RESULT_OK)
             && (operation == PLSR_MODBUS_CALL_START))
    {
        if (PlsrModbusCommitted[axis].valid == 0U)
        {
            result = PLSR_RESULT_INVALID_STATE;
        }
        else
        {
            result = PlsrModbusFingerprintCall(
                &PlsrModbusCommitted[axis].call,
                &fingerprint);
            if ((result == PLSR_RESULT_OK)
                && (fingerprint != PlsrModbusCommitted[axis].fingerprint))
            {
                result = PLSR_RESULT_BUSY;
            }
            if (result == PLSR_RESULT_OK)
            {
                result = PlsrValidateCall(&PlsrModbusCommitted[axis].call,
                                          &detail);
            }
            if (result == PLSR_RESULT_OK)
            {
                PlsrModbusCommitted[axis].call.sequence = sequence;
                result = PlsrPostCall(&PlsrModbusCommitted[axis].call);
            }
        }
    }
    else if (result == PLSR_RESULT_OK)
    {
        result = PLSR_RESULT_INVALID_ARGUMENT;
    }

    PlsrModbusPublishCallResponse(
        sequence,
        operation,
        result,
        &detail,
        (axis < PLSR_AXIS_COUNT) ? PlsrModbusCommitted[axis].valid : 0U);
}

static void PlsrModbusPublishCommandResponse(uint32_t sequence,
                                             uint16_t opcode,
                                             uint16_t axis,
                                             PLSR_RESULT result)
{
    uint16_t response[PLSR_MODBUS_COMMAND_RESPONSE_WORDS] = {0U};

    PlsrModbusPutU32(response, 0UL, sequence);
    response[2UL] = opcode;
    response[3UL] = axis;
    response[4UL] = (uint16_t)result;
    (void)ModbusDataWriteWords(MODBUS_DATA_DEVICE_D,
                               (uint32_t)PlsrModbusBaseAddress
                                   + PLSR_MODBUS_COMMAND_RESPONSE_OFFSET,
                               response,
                               PLSR_MODBUS_COMMAND_RESPONSE_WORDS);
}

static void PlsrModbusHandleCommandRequest(void)
{
    uint16_t request[PLSR_MODBUS_COMMAND_REQUEST_WORDS];
    PLSR_COMMAND command;
    PLSR_RESULT result;
    uint32_t sequence;

    if (PlsrModbusReadWords(PLSR_MODBUS_COMMAND_REQUEST_OFFSET,
                            request,
                            PLSR_MODBUS_COMMAND_REQUEST_WORDS) == 0U)
    {
        return;
    }
    sequence = PlsrModbusGetU32(request, 0UL);
    if ((sequence == 0UL)
        || (sequence == PlsrModbusLastCommandRequestSequence))
    {
        return;
    }
    PlsrModbusLastCommandRequestSequence = sequence;
    (void)memset(&command, 0, sizeof(command));
    if ((request[2UL] > (uint16_t)PLSR_CMD_SELF_TEST)
        || (request[2UL] == (uint16_t)PLSR_CMD_START)
        || (request[3UL] >= PLSR_AXIS_COUNT))
    {
        result = PLSR_RESULT_INVALID_ARGUMENT;
    }
    else
    {
        command.sequence = sequence;
        command.opcode = (PLSR_COMMAND_OPCODE)request[2UL];
        command.axis = (uint8_t)request[3UL];
        command.argument = (int64_t)PlsrModbusGetU64(request, 4UL);
        result = PlsrPostCommand(&command);
    }
    PlsrModbusPublishCommandResponse(sequence,
                                     request[2UL],
                                     request[3UL],
                                     result);
}

static void PlsrModbusPublishAxisStatus(uint8_t axis)
{
    PLSR_STATUS status;
    uint16_t *words = PlsrModbusStatusWords[axis];
    uint32_t flags = 0UL;
    uint32_t generation;

    if (PlsrGetStatus(axis, &status) != PLSR_RESULT_OK)
    {
        return;
    }
    generation = PlsrModbusStatusGeneration[axis] + 2UL;
    if (generation == 0UL)
    {
        generation = 2UL;
    }
    PlsrModbusStatusGeneration[axis] = generation;
    (void)memset(words, 0, sizeof(PlsrModbusStatusWords[axis]));
    if (status.busy != 0U) flags |= (1UL << 0U);
    if (status.pulseActive != 0U) flags |= (1UL << 1U);
    if (status.done != 0U) flags |= (1UL << 2U);
    if (status.wait != 0U) flags |= (1UL << 3U);
    if (status.directionPositive != 0U) flags |= (1UL << 4U);
    if (status.positionValid != 0U) flags |= (1UL << 5U);
    if (status.jobValid != 0U) flags |= (1UL << 6U);
    if (status.speedClamped != 0U) flags |= (1UL << 7U);
    if (status.positionOverflow != 0U) flags |= (1UL << 8U);
    if (status.positiveLimitActive != 0U) flags |= (1UL << 9U);
    if (status.negativeLimitActive != 0U) flags |= (1UL << 10U);
    if (status.emergencyLatched != 0U) flags |= (1UL << 11U);
    if (status.backlashActive != 0U) flags |= (1UL << 12U);

    PlsrModbusPutU32(words, 0UL, generation);
    words[2UL] = (uint16_t)status.state;
    PlsrModbusPutU32(words, 3UL, flags);
    words[5UL] = (uint16_t)status.outputMode;
    words[6UL] = (uint16_t)status.error;
    words[7UL] = (uint16_t)status.stopReason;
    words[8UL] = (uint16_t)status.lastCommandResult;
    words[9UL] = status.s2Set;
    PlsrModbusPutU32(words, 10UL, status.lastCommandSequence);
    PlsrModbusPutU32(words, 12UL, status.illegalTransitionCount);
    PlsrModbusPutU32(words, 14UL, status.pendingEvents);
    PlsrModbusPutU64(words, 16UL, (uint64_t)status.logicalPosition);
    PlsrModbusPutU64(words, 20UL, (uint64_t)status.taskPulses);
    PlsrModbusPutU64(words, 24UL, (uint64_t)status.totalPulses);
    PlsrModbusPutU64(words, 28UL, status.physicalPulses);
    words[32UL] = status.segmentCount;
    words[33UL] = status.startSegment;
    words[34UL] = status.currentSegment;
    words[35UL] = status.directionPoint;
    words[36UL] = status.highResourceMask;
    PlsrModbusPutU32(words, 38UL, status.currentFrequencyHz);
    PlsrModbusPutU32(words, 40UL, status.targetFrequencyHz);
    PlsrModbusPutU32(words, 42UL, status.liveFrequencyRejectCount);
    words[44UL] = (uint16_t)status.lastLiveFrequencyResult;
    PlsrModbusPutU32(words, 46UL, generation);
    (void)ModbusDataWriteWords(
        MODBUS_DATA_DEVICE_D,
        (uint32_t)PlsrModbusBaseAddress + PLSR_MODBUS_AXIS_STATUS_OFFSET
            + (uint32_t)axis * PLSR_MODBUS_AXIS_STATUS_WORDS,
        words,
        PLSR_MODBUS_AXIS_STATUS_WORDS);
}

PLSR_RESULT PlsrModbusControlInit(uint16_t baseAddress)
{
    uint16_t header[8] = {0U};

    if (ModbusDataValidateWords(MODBUS_DATA_DEVICE_D,
                                baseAddress,
                                PLSR_MODBUS_WINDOW_WORDS) == 0U)
    {
        return PLSR_RESULT_DATA_ACCESS;
    }
    PlsrModbusBaseAddress = baseAddress;
    PlsrModbusEnabled = 0U;
    PlsrModbusLastCallRequestSequence = 0UL;
    PlsrModbusLastCommandRequestSequence = 0UL;
    (void)memset(PlsrModbusCommitted, 0, sizeof(PlsrModbusCommitted));
    (void)memset(PlsrModbusStatusGeneration,
                 0,
                 sizeof(PlsrModbusStatusGeneration));
    if (ModbusDataWriteWords(MODBUS_DATA_DEVICE_D,
                             baseAddress,
                             PlsrModbusZeroWindow,
                             PLSR_MODBUS_WINDOW_WORDS) == 0U)
    {
        return PLSR_RESULT_DATA_ACCESS;
    }
    header[0UL] = PLSR_MODBUS_MAGIC_LOW;
    header[1UL] = PLSR_MODBUS_MAGIC_HIGH;
    header[2UL] = PLSR_MODBUS_PROTOCOL_VERSION;
    header[3UL] = (uint16_t)PLSR_MODBUS_WINDOW_WORDS;
    header[4UL] = PLSR_MODBUS_CAPABILITIES;
    if (ModbusDataWriteWords(MODBUS_DATA_DEVICE_D,
                             baseAddress,
                             header,
                             8UL) == 0U)
    {
        return PLSR_RESULT_DATA_ACCESS;
    }
    PlsrModbusEnabled = 1U;
    PlsrModbusControlPoll();
    return PLSR_RESULT_OK;
}

void PlsrModbusControlPoll(void)
{
    uint8_t axis;

    if (PlsrModbusEnabled == 0U)
    {
        return;
    }
    PlsrModbusHandleCallRequest();
    PlsrModbusHandleCommandRequest();
    for (axis = 0U; axis < PLSR_AXIS_COUNT; axis++)
    {
        PlsrModbusPublishAxisStatus(axis);
    }
}

uint8_t PlsrModbusControlIsEnabled(void)
{
    return PlsrModbusEnabled;
}

uint16_t PlsrModbusControlGetBaseAddress(void)
{
    return PlsrModbusBaseAddress;
}