#include "simulink_interface.h"

#include <string.h>

static SimulinkInterface_InputBus_t g_input_bus;
static SimulinkInterface_OutputBus_t g_output_bus;
static SimulinkTelemetryPacket_t g_telemetry_packet;
static uint32_t g_telemetry_sequence;
static uint8_t g_last_enable;
static uint8_t g_last_test_mode;

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

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

    return crc;
}

void SimulinkInterface_Init(void)
{
    memset(&g_input_bus, 0, sizeof(g_input_bus));
    memset(&g_output_bus, 0, sizeof(g_output_bus));
    memset(&g_telemetry_packet, 0, sizeof(g_telemetry_packet));
    g_telemetry_sequence = 0UL;
    g_last_enable = 0U;
    g_last_test_mode = AD_PARAM_ID_MODE_IDLE;
    AD_ParamID_Init();
    SimulinkInterface_PackTelemetry();
}

void SimulinkInterface_StepFast(void)
{
    SimulinkInterface_UnpackCommand();
    AD_ParamID_StepFast(&g_input_bus.measurements);
    SimulinkInterface_UpdateInputs();
    SimulinkInterface_UpdateOutputs();
}

void SimulinkInterface_StepSlow(void)
{
    AD_ParamID_StepSlow();
    SimulinkInterface_UpdateInputs();
    SimulinkInterface_PackTelemetry();
}

void SimulinkInterface_UpdateInputs(void)
{
    const AD_MotorParameters_t *params = AD_ParamID_GetParameters();

    if (params != 0) {
        g_input_bus.motor_parameters = *params;
    }
    g_input_bus.param_id_status = AD_ParamID_GetStatus();
    g_input_bus.param_id_faults = AD_ParamID_GetFaults();
    g_input_bus.param_id_mode = AD_ParamID_GetMode();
}

void SimulinkInterface_UpdateOutputs(void)
{
    g_output_bus.telemetry_size_bytes = (uint32_t)SimulinkInterface_GetTelemetrySize();
}

void SimulinkInterface_PackTelemetry(void)
{
    uint16_t crc;

    g_telemetry_packet.header.magic = SIMULINK_TELEMETRY_MAGIC;
    g_telemetry_packet.header.version = SIMULINK_INTERFACE_VERSION;
    g_telemetry_packet.header.payload_size = (uint16_t)sizeof(g_telemetry_packet.payload);
    g_telemetry_packet.header.sequence = g_telemetry_sequence++;
    g_telemetry_packet.header.crc16 = 0U;
    g_telemetry_packet.header.reserved = 0U;

    g_telemetry_packet.payload.measurements = g_input_bus.measurements;
    g_telemetry_packet.payload.motor_parameters = g_input_bus.motor_parameters;
    g_telemetry_packet.payload.param_id_status = g_input_bus.param_id_status;
    g_telemetry_packet.payload.param_id_faults = g_input_bus.param_id_faults;
    g_telemetry_packet.payload.param_id_mode = g_input_bus.param_id_mode;
    g_telemetry_packet.payload.command_enable = g_output_bus.command.enable;
    g_telemetry_packet.payload.command_test_mode = g_output_bus.command.test_mode;
    g_telemetry_packet.payload.reserved = 0U;

    crc = crc16_ccitt((const uint8_t *)&g_telemetry_packet.payload,
                      sizeof(g_telemetry_packet.payload));
    g_telemetry_packet.header.crc16 = crc;
    g_output_bus.telemetry_ready = 1U;
    g_output_bus.telemetry_size_bytes = (uint32_t)sizeof(g_telemetry_packet);
}

void SimulinkInterface_UnpackCommand(void)
{
    AD_ParamID_SafetyLimits_t limits;
    const AD_Command_t *command = &g_output_bus.command;

    if (command->reset_faults != 0U) {
        AD_ParamID_Reset();
        g_output_bus.command.reset_faults = 0U;
    }

    limits.overcurrent_A = command->current_limit_A;
    limits.overvoltage_V = command->voltage_limit_V;
    limits.undervoltage_V = 0.0f;
    limits.overtemperature_C = 0.0f;
    limits.speed_limit_rpm = command->speed_limit_rpm;
    limits.timeout_us = AD_PARAM_ID_TIMEOUT_US_DEFAULT;
    AD_ParamID_SetSafetyLimits(&limits);

    if (command->enable == 0U) {
        AD_ParamID_SetSoftwareEnable(0U);
        AD_ParamID_Stop();
    } else {
        AD_ParamID_SetSoftwareEnable(1U);
        if ((command->enable != g_last_enable) ||
            (command->test_mode != g_last_test_mode)) {
            AD_ParamID_Start(command->test_mode);
        }
    }

    g_last_enable = command->enable;
    g_last_test_mode = command->test_mode;
}

void SimulinkInterface_SetMeasurements(const AD_Measurements_t *meas)
{
    if (meas != 0) {
        g_input_bus.measurements = *meas;
    }
}

void SimulinkInterface_SetCommand(const AD_Command_t *command)
{
    if (command != 0) {
        g_output_bus.command = *command;
    }
}

const SimulinkInterface_InputBus_t* SimulinkInterface_GetInputBus(void)
{
    return &g_input_bus;
}

SimulinkInterface_OutputBus_t* SimulinkInterface_GetOutputBus(void)
{
    return &g_output_bus;
}

const SimulinkTelemetryPacket_t* SimulinkInterface_GetTelemetryPacket(void)
{
    return &g_telemetry_packet;
}

const uint8_t* SimulinkInterface_GetTelemetryBytes(void)
{
    return (const uint8_t *)&g_telemetry_packet;
}

size_t SimulinkInterface_GetTelemetrySize(void)
{
    return sizeof(g_telemetry_packet);
}