#include "ad_can_telemetry.h"

#include <string.h>

#include "ad_project_config.h"
#include "main.h"
#include "simulink_interface.h"

#ifndef AD_CAN_TELEMETRY_ENABLE
#define AD_CAN_TELEMETRY_ENABLE 1
#endif

#ifndef AD_CAN_TELEMETRY_STD_ID_BASE
#define AD_CAN_TELEMETRY_STD_ID_BASE 0x5A0UL
#endif

#ifndef AD_CAN_TELEMETRY_NOMINAL_PRESCALER
#define AD_CAN_TELEMETRY_NOMINAL_PRESCALER 17UL
#endif

#ifndef AD_CAN_TELEMETRY_NOMINAL_TIME_SEG1
#define AD_CAN_TELEMETRY_NOMINAL_TIME_SEG1 15UL
#endif

#ifndef AD_CAN_TELEMETRY_NOMINAL_TIME_SEG2
#define AD_CAN_TELEMETRY_NOMINAL_TIME_SEG2 4UL
#endif

#define AD_CAN_TELEMETRY_FRAME_BYTES 8U
#define AD_CAN_TELEMETRY_MAX_FRAMES  ((SIMULINK_TELEMETRY_MAX_BYTES + AD_CAN_TELEMETRY_FRAME_BYTES - 1U) / AD_CAN_TELEMETRY_FRAME_BYTES)

FDCAN_HandleTypeDef hfdcan1;

static AD_CAN_TelemetryState_t g_can_state;
static uint8_t g_tx_packet[SIMULINK_TELEMETRY_MAX_BYTES];
static uint16_t g_tx_size;
static uint8_t g_tx_frame_index;
static uint8_t g_tx_frame_count;

static void update_status_flags(void)
{
    uint32_t flags = 0UL;

    if (g_can_state.initialized != 0U) {
        flags |= AD_CAN_TELEMETRY_STATUS_INITIALIZED;
    }
    if (g_can_state.started != 0U) {
        flags |= AD_CAN_TELEMETRY_STATUS_STARTED;
    }
    if (g_can_state.tx_busy != 0U) {
        flags |= AD_CAN_TELEMETRY_STATUS_TX_BUSY;
    }
    if ((g_can_state.status_flags & AD_CAN_TELEMETRY_STATUS_TX_DROPPED) != 0UL) {
        flags |= AD_CAN_TELEMETRY_STATUS_TX_DROPPED;
    }
    if ((g_can_state.status_flags & AD_CAN_TELEMETRY_STATUS_ERROR) != 0UL) {
        flags |= AD_CAN_TELEMETRY_STATUS_ERROR;
    }

    g_can_state.status_flags = flags;
}

static uint32_t can_dlc_from_len(uint8_t len)
{
    switch (len) {
    case 0U:
        return FDCAN_DLC_BYTES_0;
    case 1U:
        return FDCAN_DLC_BYTES_1;
    case 2U:
        return FDCAN_DLC_BYTES_2;
    case 3U:
        return FDCAN_DLC_BYTES_3;
    case 4U:
        return FDCAN_DLC_BYTES_4;
    case 5U:
        return FDCAN_DLC_BYTES_5;
    case 6U:
        return FDCAN_DLC_BYTES_6;
    case 7U:
        return FDCAN_DLC_BYTES_7;
    default:
        return FDCAN_DLC_BYTES_8;
    }
}

void AD_CAN_Telemetry_Init(void)
{
#if AD_CAN_TELEMETRY_ENABLE != 0
    memset(&g_can_state, 0, sizeof(g_can_state));
    memset(g_tx_packet, 0, sizeof(g_tx_packet));
    g_tx_size = 0U;
    g_tx_frame_index = 0U;
    g_tx_frame_count = 0U;

    g_can_state.base_id = AD_CAN_TELEMETRY_STD_ID_BASE;

    hfdcan1.Instance = FDCAN1;
    hfdcan1.Init.ClockDivider = FDCAN_CLOCK_DIV1;
    hfdcan1.Init.FrameFormat = FDCAN_FRAME_CLASSIC;
    hfdcan1.Init.Mode = FDCAN_MODE_NORMAL;
    hfdcan1.Init.AutoRetransmission = ENABLE;
    hfdcan1.Init.TransmitPause = DISABLE;
    hfdcan1.Init.ProtocolException = DISABLE;
    hfdcan1.Init.NominalPrescaler = AD_CAN_TELEMETRY_NOMINAL_PRESCALER;
    hfdcan1.Init.NominalSyncJumpWidth = 1U;
    hfdcan1.Init.NominalTimeSeg1 = AD_CAN_TELEMETRY_NOMINAL_TIME_SEG1;
    hfdcan1.Init.NominalTimeSeg2 = AD_CAN_TELEMETRY_NOMINAL_TIME_SEG2;
    hfdcan1.Init.DataPrescaler = AD_CAN_TELEMETRY_NOMINAL_PRESCALER;
    hfdcan1.Init.DataSyncJumpWidth = 1U;
    hfdcan1.Init.DataTimeSeg1 = AD_CAN_TELEMETRY_NOMINAL_TIME_SEG1;
    hfdcan1.Init.DataTimeSeg2 = AD_CAN_TELEMETRY_NOMINAL_TIME_SEG2;
    hfdcan1.Init.StdFiltersNbr = 0U;
    hfdcan1.Init.ExtFiltersNbr = 0U;
    hfdcan1.Init.TxFifoQueueMode = FDCAN_TX_FIFO_OPERATION;

    if (HAL_FDCAN_Init(&hfdcan1) != HAL_OK) {
        g_can_state.error_count++;
        g_can_state.last_error = HAL_FDCAN_GetError(&hfdcan1);
        g_can_state.status_flags |= AD_CAN_TELEMETRY_STATUS_ERROR;
        update_status_flags();
        return;
    }

    g_can_state.initialized = 1U;

    if (HAL_FDCAN_ConfigGlobalFilter(&hfdcan1,
                                     FDCAN_REJECT,
                                     FDCAN_REJECT,
                                     FDCAN_REJECT_REMOTE,
                                     FDCAN_REJECT_REMOTE) != HAL_OK) {
        g_can_state.error_count++;
        g_can_state.last_error = HAL_FDCAN_GetError(&hfdcan1);
        g_can_state.status_flags |= AD_CAN_TELEMETRY_STATUS_ERROR;
    }

    if (HAL_FDCAN_Start(&hfdcan1) == HAL_OK) {
        g_can_state.started = 1U;
    } else {
        g_can_state.error_count++;
        g_can_state.last_error = HAL_FDCAN_GetError(&hfdcan1);
        g_can_state.status_flags |= AD_CAN_TELEMETRY_STATUS_ERROR;
    }

    update_status_flags();
#else
    memset(&g_can_state, 0, sizeof(g_can_state));
#endif
}

uint8_t AD_CAN_Telemetry_QueuePacket(const uint8_t *data, uint16_t size)
{
#if AD_CAN_TELEMETRY_ENABLE != 0
    if ((data == 0) ||
        (size == 0U) ||
        (size > SIMULINK_TELEMETRY_MAX_BYTES) ||
        (g_can_state.started == 0U) ||
        (g_can_state.tx_busy != 0U)) {
        g_can_state.dropped_packets++;
        g_can_state.status_flags |= AD_CAN_TELEMETRY_STATUS_TX_DROPPED;
        update_status_flags();
        return 0U;
    }

    memcpy(g_tx_packet, data, size);
    g_tx_size = size;
    g_tx_frame_index = 0U;
    g_tx_frame_count = (uint8_t)((size + AD_CAN_TELEMETRY_FRAME_BYTES - 1U) / AD_CAN_TELEMETRY_FRAME_BYTES);
    if (g_tx_frame_count > AD_CAN_TELEMETRY_MAX_FRAMES) {
        g_tx_frame_count = AD_CAN_TELEMETRY_MAX_FRAMES;
    }
    g_can_state.tx_busy = 1U;
    g_can_state.last_size_bytes = size;
    g_can_state.status_flags &= ~AD_CAN_TELEMETRY_STATUS_TX_DROPPED;
    update_status_flags();
    AD_CAN_Telemetry_Loop();
    return 1U;
#else
    (void)data;
    (void)size;
    return 0U;
#endif
}

void AD_CAN_Telemetry_Loop(void)
{
#if AD_CAN_TELEMETRY_ENABLE != 0
    FDCAN_TxHeaderTypeDef header;
    uint8_t frame[AD_CAN_TELEMETRY_FRAME_BYTES];
    uint16_t offset;
    uint16_t remaining;
    uint8_t len;

    if ((g_can_state.started == 0U) || (g_can_state.tx_busy == 0U)) {
        update_status_flags();
        return;
    }

    while ((g_tx_frame_index < g_tx_frame_count) &&
           (HAL_FDCAN_GetTxFifoFreeLevel(&hfdcan1) > 0UL)) {
        offset = (uint16_t)g_tx_frame_index * AD_CAN_TELEMETRY_FRAME_BYTES;
        remaining = (uint16_t)(g_tx_size - offset);
        len = (remaining > AD_CAN_TELEMETRY_FRAME_BYTES) ?
              AD_CAN_TELEMETRY_FRAME_BYTES :
              (uint8_t)remaining;

        memset(frame, 0, sizeof(frame));
        memcpy(frame, &g_tx_packet[offset], len);

        memset(&header, 0, sizeof(header));
        header.Identifier = (uint32_t)(AD_CAN_TELEMETRY_STD_ID_BASE + g_tx_frame_index);
        header.IdType = FDCAN_STANDARD_ID;
        header.TxFrameType = FDCAN_DATA_FRAME;
        header.DataLength = can_dlc_from_len(len);
        header.ErrorStateIndicator = FDCAN_ESI_ACTIVE;
        header.BitRateSwitch = FDCAN_BRS_OFF;
        header.FDFormat = FDCAN_CLASSIC_CAN;
        header.TxEventFifoControl = FDCAN_NO_TX_EVENTS;
        header.MessageMarker = g_tx_frame_index;

        if (HAL_FDCAN_AddMessageToTxFifoQ(&hfdcan1, &header, frame) != HAL_OK) {
            g_can_state.error_count++;
            g_can_state.last_error = HAL_FDCAN_GetError(&hfdcan1);
            g_can_state.status_flags |= AD_CAN_TELEMETRY_STATUS_ERROR;
            update_status_flags();
            return;
        }

        g_tx_frame_index++;
        g_can_state.tx_frames++;
    }

    if (g_tx_frame_index >= g_tx_frame_count) {
        g_can_state.tx_busy = 0U;
        g_can_state.tx_packets++;
        g_tx_size = 0U;
        g_tx_frame_index = 0U;
        g_tx_frame_count = 0U;
    }

    update_status_flags();
#endif
}

uint8_t AD_CAN_Telemetry_IsStarted(void)
{
    return g_can_state.started;
}

uint8_t AD_CAN_Telemetry_IsTxBusy(void)
{
    return g_can_state.tx_busy;
}

const AD_CAN_TelemetryState_t* AD_CAN_Telemetry_GetState(void)
{
    update_status_flags();
    return &g_can_state;
}
