/**
 * @file telemetry.c
 * @brief Telemetry-frame creation, checksum finalisation, and serialisation.
 */

#include "telemetry.h"

#include <string.h>

#include "app_uart_protocol.h"

void telemetry_reset(telemetry_frame_t *frame)
{
    if (frame == NULL)
    {
        return;
    }

    memset(frame->words, 0, sizeof(frame->words));
    frame->words[0] = APP_PACKET_HEADER_WORK_CONFIG;
}

void telemetry_set_message_id(telemetry_frame_t *frame, uint16_t message_id)
{
    if (frame != NULL)
    {
        frame->words[13] = message_id;
    }
}

void telemetry_set_live_data(telemetry_frame_t *frame,
                             uint16_t laser1_power,
                             uint16_t laser2_power,
                             uint16_t input_pf3_raw,
                             uint16_t input_pf4_raw,
                             uint16_t laser1_temperature,
                             uint16_t laser2_temperature,
                             uint16_t external_temperature_1_raw,
                             uint16_t external_temperature_2_raw,
                             uint16_t rail_3v3_raw,
                             uint16_t rail_5v1_raw,
                             uint16_t rail_5v2_raw,
                             uint16_t rail_7v0_raw)
{
    if (frame == NULL)
    {
        return;
    }

    frame->words[1] = laser1_power;
    frame->words[2] = laser2_power;
    frame->words[3] = input_pf3_raw;
    frame->words[4] = input_pf4_raw;
    frame->words[5] = laser1_temperature;
    frame->words[6] = laser2_temperature;
    frame->words[7] = external_temperature_1_raw;
    frame->words[8] = external_temperature_2_raw;
    frame->words[9] = rail_3v3_raw;
    frame->words[10] = rail_5v1_raw;
    frame->words[11] = rail_5v2_raw;
    frame->words[12] = rail_7v0_raw;
}

void telemetry_finalize(telemetry_frame_t *frame)
{
    if (frame == NULL)
    {
        return;
    }

    frame->words[14] = app_protocol_calculate_checksum(&frame->words[1], 13u);
}

void telemetry_to_bytes(const telemetry_frame_t *frame, uint8_t *out_bytes)
{
    uint8_t index;

    if ((frame == NULL) || (out_bytes == NULL))
    {
        return;
    }

    for (index = 0u; index < APP_TELEMETRY_WORD_COUNT; ++index)
    {
        out_bytes[index * 2u] = (uint8_t)(frame->words[index] & 0x00FFu);
        out_bytes[(index * 2u) + 1u] = (uint8_t)((frame->words[index] >> 8) & 0x00FFu);
    }
}