#include "plc_device.h"
#include "plsr_core.h"
#include "plsr_persistence.h"
#include "plsr_resource.h"
#include <stdio.h>
#include <stdlib.h>
#include <string.h>

static unsigned int TestCount;

#define TEST_CHECK(condition)                                                   \
    do                                                                          \
    {                                                                           \
        TestCount++;                                                            \
        if (!(condition))                                                       \
        {                                                                       \
            (void)fprintf(stderr, "FAIL line %d: %s\n", __LINE__, #condition); \
            exit(EXIT_FAILURE);                                                 \
        }                                                                       \
    } while (0)

static void TestReset(void)
{
    PlsrPersistenceTestResetStorage();
    TEST_CHECK(PlcDeviceInit() == PLC_DEVICE_OK);
    TEST_CHECK(PlsrInit() == PLSR_RESULT_OK);
}

static PLSR_RESULT TestPostCommand(uint32_t sequence,
                                   uint8_t axis,
                                   PLSR_COMMAND_OPCODE opcode,
                                   int64_t argument)
{
    PLSR_COMMAND command;

    command.sequence = sequence;
    command.axis = axis;
    command.opcode = opcode;
    command.argument = argument;
    return PlsrPostCommand(&command);
}

static PLSR_RESULT TestPostStart(uint32_t sequence,
                                 uint8_t axis,
                                 PLSR_OUTPUT_MODE mode,
                                 uint8_t directionPoint,
                                 uint8_t directionPositive)
{
    PLSR_START_REQUEST request;

    request.sequence = sequence;
    request.axis = axis;
    request.outputMode = mode;
    request.directionPoint = directionPoint;
    request.directionPositive = directionPositive;
    return PlsrPostStart(&request);
}

static PLSR_STATUS TestGetStatus(uint8_t axis)
{
    PLSR_STATUS status;

    (void)memset(&status, 0, sizeof(status));
    TEST_CHECK(PlsrGetStatus(axis, &status) == PLSR_RESULT_OK);
    return status;
}

static void TestResourceManager(void)
{
    PLSR_RESOURCE_REQUEST request;
    PLSR_RESOURCE_LEASE first;
    PLSR_RESOURCE_LEASE second;
    PLSR_RESOURCE_STATUS status;

    (void)memset(&first, 0, sizeof(first));
    (void)memset(&second, 0, sizeof(second));
    PlsrResourceInit();

    request.ownerAxis = 0U;
    request.outputMode = PLSR_OUTPUT_PULSE_DIR;
    request.dAxis = 0U;
    request.directionPoint = 4U;
    TEST_CHECK(PlsrResourceReserve(&request, &first) == PLSR_RESULT_OK);
    TEST_CHECK(first.highMask == 0x01U);
    TEST_CHECK(first.outputMask == ((1UL << 0U) | (1UL << 4U)));

    request.ownerAxis = 1U;
    request.dAxis = 1U;
    request.directionPoint = 4U;
    TEST_CHECK(PlsrResourceReserve(&request, &second)
               == PLSR_RESULT_RESOURCE_CONFLICT);
    TEST_CHECK(second.valid == 0U);

    request.directionPoint = 8U;
    TEST_CHECK(PlsrResourceReserve(&request, &second)
               == PLSR_RESULT_INVALID_RESOURCE);
    request.directionPoint = 1U;
    TEST_CHECK(PlsrResourceReserve(&request, &second)
               == PLSR_RESULT_INVALID_RESOURCE);

    PlsrResourceRelease(&first);
    PlsrResourceRelease(&first);
    request.ownerAxis = 0U;
    request.outputMode = PLSR_OUTPUT_AB;
    request.dAxis = 0U;
    request.directionPoint = PLSR_DIRECTION_POINT_NONE;
    TEST_CHECK(PlsrResourceReserve(&request, &first) == PLSR_RESULT_OK);
    TEST_CHECK(first.highMask == 0x03U);

    request.ownerAxis = 2U;
    request.outputMode = PLSR_OUTPUT_PULSE_DIR;
    request.dAxis = 2U;
    request.directionPoint = 4U;
    TEST_CHECK(PlsrResourceReserve(&request, &second) == PLSR_RESULT_OK);
    TEST_CHECK(second.highMask == 0x04U);

    request.ownerAxis = 1U;
    request.outputMode = PLSR_OUTPUT_CW_CCW;
    request.dAxis = 1U;
    request.directionPoint = PLSR_DIRECTION_POINT_NONE;
    PlsrResourceRelease(&second);
    TEST_CHECK(PlsrResourceReserve(&request, &second)
               == PLSR_RESULT_INVALID_RESOURCE);

    PlsrResourceGetStatus(&status);
    TEST_CHECK(status.highMask == 0x03U);
    TEST_CHECK(PlsrResourceCheckInvariant() != 0U);
    PlsrResourceRelease(&first);
    PlsrResourceGetStatus(&status);
    TEST_CHECK(status.outputMask == 0UL);
}

static void TestStateMachineAndCommands(void)
{
    PLSR_RESOURCE_STATUS resources;
    PLSR_STATUS status;

    TestReset();
    status = TestGetStatus(0U);
    TEST_CHECK(status.state == PLSR_STATE_IDLE);
    TEST_CHECK(status.directionPoint == PLSR_DIRECTION_POINT_NONE);

    TEST_CHECK(TestPostStart(1U, 0U, PLSR_OUTPUT_PULSE_DIR, 4U, 1U)
               == PLSR_RESULT_QUEUED);
    PlsrProcess();
    status = TestGetStatus(0U);
    TEST_CHECK(status.state == PLSR_STATE_ACCEL);
    TEST_CHECK(status.busy != 0U);
    TEST_CHECK(status.pulseActive != 0U);
    TEST_CHECK(status.done == 0U);
    TEST_CHECK(status.highResourceMask == 0x01U);
    TEST_CHECK(status.directionPoint == 4U);
    TEST_CHECK(status.lastCommandSequence == 1U);
    TEST_CHECK(status.lastCommandResult == PLSR_RESULT_OK);
    TEST_CHECK(TestPostStart(1U, 0U, PLSR_OUTPUT_PULSE_DIR, 4U, 1U)
               == PLSR_RESULT_OK);

    TEST_CHECK(TestPostStart(1U, 1U, PLSR_OUTPUT_PULSE_DIR, 4U, 1U)
               == PLSR_RESULT_QUEUED);
    PlsrProcess();
    status = TestGetStatus(1U);
    TEST_CHECK(status.state == PLSR_STATE_IDLE);
    TEST_CHECK(status.lastCommandResult == PLSR_RESULT_RESOURCE_CONFLICT);
    TEST_CHECK(status.error == PLSR_ERROR_RESOURCE_CONFLICT);

    TEST_CHECK(PlsrPostEvent(0U, PLSR_EVENT_ACCEL_COMPLETE)
               == PLSR_RESULT_OK);
    PlsrProcess();
    TEST_CHECK(TestGetStatus(0U).state == PLSR_STATE_RUN);

    TEST_CHECK(TestPostCommand(2U, 0U, PLSR_CMD_SET_POSITION, 500)
               == PLSR_RESULT_QUEUED);
    PlsrProcess();
    TEST_CHECK(TestGetStatus(0U).lastCommandResult == PLSR_RESULT_BUSY);

    TEST_CHECK(TestPostCommand(3U, 0U, PLSR_CMD_PAUSE, 0)
               == PLSR_RESULT_QUEUED);
    PlsrProcess();
    TEST_CHECK(TestGetStatus(0U).state == PLSR_STATE_DECEL);
    TEST_CHECK(PlsrPostEvent(0U, PLSR_EVENT_DECEL_COMPLETE)
               == PLSR_RESULT_OK);
    PlsrProcess();
    status = TestGetStatus(0U);
    TEST_CHECK(status.state == PLSR_STATE_PAUSED);
    TEST_CHECK(status.busy != 0U);
    TEST_CHECK(status.pulseActive == 0U);
    TEST_CHECK(status.highResourceMask == 0x01U);

    TEST_CHECK(TestPostCommand(4U, 0U, PLSR_CMD_RESUME, 0)
               == PLSR_RESULT_QUEUED);
    PlsrProcess();
    TEST_CHECK(TestGetStatus(0U).state == PLSR_STATE_ACCEL);

    TEST_CHECK(TestPostCommand(5U, 0U, PLSR_CMD_STOP_IMMEDIATE, 0)
               == PLSR_RESULT_QUEUED);
    PlsrProcess();
    status = TestGetStatus(0U);
    TEST_CHECK(status.state == PLSR_STATE_ACCEL);
    TEST_CHECK(status.stopReason == PLSR_STOP_REASON_STOP_IMMEDIATE);
    TEST_CHECK(status.highResourceMask == 0x01U);
    TEST_CHECK(TestPostCommand(5U, 0U, PLSR_CMD_STOP_IMMEDIATE, 0)
               == PLSR_RESULT_OK);

    TEST_CHECK(PlsrPostEvent(0U, PLSR_EVENT_STOP_IMMEDIATE_DONE)
               == PLSR_RESULT_OK);
    PlsrProcess();
    status = TestGetStatus(0U);
    TEST_CHECK(status.state == PLSR_STATE_STOPPED);
    TEST_CHECK(status.busy == 0U);
    TEST_CHECK(status.done == 0U);
    TEST_CHECK(status.highResourceMask == 0U);
    PlsrResourceGetStatus(&resources);
    TEST_CHECK(resources.outputMask == 0UL);

    TEST_CHECK(TestPostStart(6U, 0U, PLSR_OUTPUT_PULSE_DIR, 4U, 0U)
               == PLSR_RESULT_QUEUED);
    PlsrProcess();
    TEST_CHECK(PlsrPostEvent(0U, PLSR_EVENT_JOB_COMPLETE)
               == PLSR_RESULT_OK);
    PlsrProcess();
    status = TestGetStatus(0U);
    TEST_CHECK(status.state == PLSR_STATE_COMPLETED);
    TEST_CHECK(status.done != 0U);
    TEST_CHECK(status.stopReason == PLSR_STOP_REASON_NORMAL_COMPLETE);

    TEST_CHECK(TestPostStart(7U, 0U, PLSR_OUTPUT_PULSE_DIR, 4U, 1U)
               == PLSR_RESULT_QUEUED);
    PlsrProcess();
    TEST_CHECK(TestGetStatus(0U).done == 0U);
    TEST_CHECK(PlsrPostEvent(0U,
                             PLSR_EVENT_JOB_COMPLETE
                                 | PLSR_EVENT_SOFTWARE_EMERGENCY)
               == PLSR_RESULT_OK);
    PlsrProcess();
    status = TestGetStatus(0U);
    TEST_CHECK(status.state == PLSR_STATE_STOPPED);
    TEST_CHECK(status.done == 0U);
    TEST_CHECK(status.stopReason == PLSR_STOP_REASON_SOFTWARE_EMERGENCY);

    TEST_CHECK(PlsrStateTransition(0U,
                                   PLSR_STATE_RUN,
                                   PLSR_TRANSITION_ACCEL_COMPLETE)
               == PLSR_RESULT_INVALID_STATE);
    status = TestGetStatus(0U);
    TEST_CHECK(status.state == PLSR_STATE_ERROR);
    TEST_CHECK(status.error == PLSR_ERROR_ILLEGAL_TRANSITION);
    TEST_CHECK(status.illegalTransitionCount == 1UL);
    TEST_CHECK(TestPostCommand(8U, 0U, PLSR_CMD_RESET_ERROR, 0)
               == PLSR_RESULT_QUEUED);
    PlsrProcess();
    TEST_CHECK(TestGetStatus(0U).state == PLSR_STATE_IDLE);
}

static void TestLimitWaitAndPairs(void)
{
    PLSR_STATUS status;

    TestReset();
    TEST_CHECK(TestPostStart(10U, 0U, PLSR_OUTPUT_PULSE_DIR, 4U, 1U)
               == PLSR_RESULT_QUEUED);
    PlsrProcess();
    TEST_CHECK(PlsrPostEvent(0U,
                             PLSR_EVENT_LIMIT_POSITIVE
                                 | PLSR_EVENT_DECEL_COMPLETE)
               == PLSR_RESULT_OK);
    PlsrProcess();
    status = TestGetStatus(0U);
    TEST_CHECK(status.state == PLSR_STATE_STOPPED);
    TEST_CHECK(status.stopReason == PLSR_STOP_REASON_LIMIT_POSITIVE);
    TEST_CHECK(status.error == PLSR_ERROR_LIMIT_POSITIVE);

    TEST_CHECK(TestPostStart(11U,
                             2U,
                             PLSR_OUTPUT_AB,
                             PLSR_DIRECTION_POINT_NONE,
                             0U)
               == PLSR_RESULT_QUEUED);
    PlsrProcess();
    status = TestGetStatus(2U);
    TEST_CHECK(status.highResourceMask == 0x0CU);
    TEST_CHECK(status.outputMode == PLSR_OUTPUT_AB);
    TEST_CHECK(PlsrPostEvent(2U, PLSR_EVENT_WAIT_BEGIN) == PLSR_RESULT_OK);
    PlsrProcess();
    status = TestGetStatus(2U);
    TEST_CHECK(status.state == PLSR_STATE_WAIT);
    TEST_CHECK(status.busy != 0U);
    TEST_CHECK(status.pulseActive == 0U);
    TEST_CHECK(status.highResourceMask == 0x0CU);

    TEST_CHECK(TestPostCommand(12U, 2U, PLSR_CMD_PAUSE, 0)
               == PLSR_RESULT_QUEUED);
    PlsrProcess();
    TEST_CHECK(TestGetStatus(2U).state == PLSR_STATE_PAUSED);
    TEST_CHECK(TestPostCommand(13U, 2U, PLSR_CMD_STOP_DECEL, 0)
               == PLSR_RESULT_QUEUED);
    PlsrProcess();
    status = TestGetStatus(2U);
    TEST_CHECK(status.state == PLSR_STATE_STOPPED);
    TEST_CHECK(status.highResourceMask == 0U);

    TEST_CHECK(TestPostStart(14U,
                             1U,
                             PLSR_OUTPUT_CW_CCW,
                             PLSR_DIRECTION_POINT_NONE,
                             1U)
               == PLSR_RESULT_QUEUED);
    PlsrProcess();
    status = TestGetStatus(1U);
    TEST_CHECK(status.state == PLSR_STATE_IDLE);
    TEST_CHECK(status.lastCommandResult == PLSR_RESULT_INVALID_RESOURCE);
}

static void TestQueueAndInputValidation(void)
{
    PLSR_COMMAND command;
    uint32_t sequence;

    TestReset();
    TEST_CHECK(PlsrPostEvent(4U, PLSR_EVENT_JOB_COMPLETE)
               == PLSR_RESULT_INVALID_AXIS);
    TEST_CHECK(PlsrPostEvent(0U, 0UL) == PLSR_RESULT_INVALID_ARGUMENT);
    TEST_CHECK(PlsrPostEvent(0U, 0x80000000UL)
               == PLSR_RESULT_INVALID_ARGUMENT);
    TEST_CHECK(PlsrGetStatus(0U, NULL) == PLSR_RESULT_INVALID_ARGUMENT);
    TEST_CHECK(PlsrGetStatus(4U, NULL) == PLSR_RESULT_INVALID_AXIS);

    command.axis = 1U;
    command.opcode = PLSR_CMD_CLEAR_POSITION;
    command.argument = 0;
    for (sequence = 100U;
         sequence < 100U + PLSR_COMMAND_QUEUE_DEPTH;
         sequence++)
    {
        command.sequence = sequence;
        TEST_CHECK(PlsrPostCommand(&command) == PLSR_RESULT_QUEUED);
    }
    command.sequence = 1000U;
    TEST_CHECK(PlsrPostCommand(&command) == PLSR_RESULT_QUEUE_FULL);
    PlsrProcess();
    TEST_CHECK(TestGetStatus(1U).lastCommandResult == PLSR_RESULT_OK);
    TEST_CHECK(TestPostCommand(2000U, 0U, PLSR_CMD_SELF_TEST, 0)
               == PLSR_RESULT_QUEUED);
    PlsrProcess();
    TEST_CHECK(TestGetStatus(0U).lastCommandResult == PLSR_RESULT_OK);
}

int main(void)
{
    TestResourceManager();
    TestStateMachineAndCommands();
    TestLimitWaitAndPairs();
    TestQueueAndInputValidation();

    (void)printf("PASS: %u PLSR core checks\n", TestCount);
    return EXIT_SUCCESS;
}