/*!
 * \file cnav_msg.c
 * \author Valeri Atamaniouk <valeri@swift-nav.com>
 *
 * -----------------------------------------------------------------------------
 * GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
 * This file is part of GNSS-SDR.
 *
 * This file was originally borrowed from libswiftnav
 * <https://github.com/swift-nav/libswiftnav>,
 * a portable C library implementing GNSS related functions and algorithms,
 * and then modified by J. Arribas and C. Fernandez
 *
 * Copyright (C) 2016 Swift Navigation Inc.
 * Contact: Valeri Atamaniouk <valeri@swift-nav.com>
 *
 * SPDX-License-Identifier: LGPL-3.0-only
 *
 */


#include "cnav_msg.h"
#include "bits.h"
#include "edc.h"
#include <limits.h>
#include <string.h>

/** \defgroup GPS_L2 GPS L2
 * GPS L2 operations
 * \{ */
/** \defgroup gps_cnav_decoder Decoder
 * GPS L2C CNAV message decoding.
 * \{ */
/*
 * Block Viterbi decoding parameters.
 */
/** Viterbi decoder reversed polynomial A */
#define GPS_L2C_V27_POLY_A (0x4F) /* 0b01001111 - reversed 0171*/

/** Viterbi decoder reversed polynomial B */
#define GPS_L2C_V27_POLY_B (0x6D) /* 0b01101101 - reversed 0133 */
/*
 * GPS L2C message constants.
 */

/** GPS L2C preamble */
const uint32_t GPS_CNAV_PREAMBLE1 = 0x8BU; /* (0b10001011u) */
/** Inverted GPS L2C preamble */
const uint32_t GPS_CNAV_PREAMBLE2 = 0x74U; /* (0b01110100u) */
/** GPS L2C preamble length in bits */
#define GPS_CNAV_PREAMBLE_LENGTH (8)
/** GPS L2C CNAV message length in bits */
#define GPS_CNAV_MSG_LENGTH (300)
/** GPS LC2 CNAV CRC length in bits */
#define GPS_CNAV_MSG_CRC_LENGTH (24)
/** GPS L2C CNAV message payload length in bits */
#define GPS_CNAV_MSG_DATA_LENGTH (GPS_CNAV_MSG_LENGTH - GPS_CNAV_MSG_CRC_LENGTH)
/** GPS L2C CNAV message lock detector threshold */
#define GPS_CNAV_LOCK_MAX_CRC_FAILS (10)

/**
 * Computes CRC-24Q from a CNAV message buffer.
 * CRC-24Q is computed for 274 bits. For a purpose of 8-bit alignment, the
 * message is assumed to be prepended with four zero bits.
 *
 * \param[in] part Decoder component with payload
 *
 * \return CRC-24Q value.
 *
 * \private
 */
static uint32_t cnav_compute_crc_(cnav_v27_part_t *part)
{
    uint32_t crc = crc24q_bits(0, part->decoded, GPS_CNAV_MSG_DATA_LENGTH,
        part->invert);

    return crc;
}


/**
 * Extracts CRC-24Q from a CNAV message buffer.
 * CRC-24Q value is the last 24 bits from 300 bits message buffer.
 *
 * \param[in] part Decoded component with payload.
 *
 * \return CRC24-Q value.
 *
 * \private
 */
static uint32_t cnav_extract_crc_(const cnav_v27_part_t *part)
{
    uint32_t crc = getbitu(part->decoded, GPS_CNAV_MSG_DATA_LENGTH,
        GPS_CNAV_MSG_CRC_LENGTH);
    if (part->invert)
        {
            crc ^= 0xFFFFFFU;
        }
    return crc;
}


/**
 * Helper to rescan for preamble in the received buffer.
 * Occasionally there could be a false lock on message contents if it the
 * preamble sequence is encountered in the message body. For this case, the
 * function performs for a scan for a preamble with a different offset:
 * - When found, the preamble octet is moved into the head of the buffer.
 * - When not found, only 7 bits are left in the buffer.
 *
 * \param[in,out] part Decoded component.
 *
 * \return None
 *
 * \private
 */
static void cnav_rescan_preamble_(cnav_v27_part_t *part)
{
    part->preamble_seen = false;

    if (part->n_decoded > GPS_CNAV_PREAMBLE_LENGTH + 1)
        {
            size_t i = 0;
            size_t j = 0;
            for (i = 1, j = part->n_decoded - GPS_CNAV_PREAMBLE_LENGTH; i < j; ++i)
                {
                    const uint32_t c = getbitu(part->decoded, i, GPS_CNAV_PREAMBLE_LENGTH);
                    if (GPS_CNAV_PREAMBLE1 == c || GPS_CNAV_PREAMBLE2 == c)
                        {
                            part->preamble_seen = true;
                            part->invert = (GPS_CNAV_PREAMBLE2 == c);
                            /* We shift the accumulated bits to the beginning of the buffer */
                            bitshl(part->decoded, sizeof(part->decoded), i);
                            part->n_decoded -= i;
                            break;
                        }
                }
        }
    if (!part->preamble_seen && part->n_decoded >= GPS_CNAV_PREAMBLE_LENGTH)
        {
            bitshl(part->decoded, sizeof(part->decoded),
                part->n_decoded - GPS_CNAV_PREAMBLE_LENGTH + 1);
            part->n_decoded = GPS_CNAV_PREAMBLE_LENGTH - 1;
        }
}


/**
 * Feed a symbol into Viterbi decoder instance.
 *
 * The method uses block Viterbi decoder. It first accumulates initial number of
 * symbols, and after that runs decoding every time the buffer is full. Only
 * some of the decoded symbols are used.
 *
 * \param[in,out] part Decoder object
 * \param[in]     s    Symbol (0x00 - Hard 0, 0xFF - Hard 1)
 *
 * \return None
 *
 * \private
 */
static void cnav_add_symbol_(cnav_v27_part_t *part, uint8_t ch)
{
    part->symbols[part->n_symbols++] = ch;

    if (part->init)
        {
            /* Initial step - load more symbols without decoding. */
            if (part->n_symbols < (GPS_L2C_V27_INIT_BITS + GPS_L2C_V27_DECODE_BITS) * 2)
                {
                    return;
                }
            part->init = false;
        }
    else if (part->n_symbols < GPS_L2C_V27_DECODE_BITS * 2)
        {
            /* Wait until decoding block is accumulated */
            return;
        }

    /* Feed accumulated symbols into the buffer, reset the number of accumulated
     * symbols. */
    v27_update(&part->dec, part->symbols, (int)part->n_symbols / 2);
    part->n_symbols = 0;

    /* Decode N+M bits, where:
     * - N - Number of bits to put into decoded buffer
     * - M - Number of bits in the tail to ignore.
     */
    unsigned char tmp_bits[(GPS_L2C_V27_DECODE_BITS + GPS_L2C_V27_DELAY_BITS +
                               CHAR_BIT - 1) /
                           CHAR_BIT];

    v27_chainback_likely(&part->dec, tmp_bits,
        GPS_L2C_V27_DECODE_BITS + GPS_L2C_V27_DELAY_BITS);

    /* Read decoded bits and add them to the decoded buffer */
    bitcopy(part->decoded, part->n_decoded, tmp_bits, 0, GPS_L2C_V27_DECODE_BITS);
    part->n_decoded += GPS_L2C_V27_DECODE_BITS;

    /* Depending on the decoder state, one of the following actions are
     * possible:
     * - If no message lock
     *   - If no preamble seen - look for preamble
     *   - If preamble seen - collect 300 bits
     *     - If 300 bits are collected - verify CRC
     *       - If CRC is OK - message lock is acquired
     *       - If CRC fails - rescan for preamble
     *         - If found - continue collecting 300 bits
     *         - If not found - continue preamble wait
     * - If message lock
     *   - If 300 bits collected, compute CRC
     *     - If CRC is OK, message can be decoded
     *     - If CRC is not OK, discard data
     */

    bool retry = true;
    while (retry)
        {
            retry = false;

            if (!part->preamble_seen)
                {
                    /* Rescan for preamble if possible. The first bit is ignored. */
                    cnav_rescan_preamble_(part);
                }
            if (part->preamble_seen && GPS_CNAV_MSG_LENGTH <= part->n_decoded)
                {
                    /* We have collected 300 bits starting from message preamble. Now try
                     * to compute CRC-24Q */
                    const uint32_t crc = cnav_compute_crc_(part);
                    const uint32_t crc2 = cnav_extract_crc_(part);

                    if (part->message_lock)
                        {
                            /* We have message lock */
                            part->crc_ok = (crc == crc2);
                            if (part->crc_ok)
                                {
                                    /* Reset message lock counter */
                                    part->n_crc_fail = 0;
                                }
                            else
                                {
                                    /* Increment message lock counter */
                                    part->n_crc_fail++;
                                    if (part->n_crc_fail > GPS_CNAV_LOCK_MAX_CRC_FAILS)
                                        {
                                            /* CRC has failed too many times - drop the lock. */
                                            part->n_crc_fail = 0;
                                            part->message_lock = false;
                                            part->preamble_seen = false;
                                            /* Try to find a new preamble, reuse data from buffer. */
                                            retry = true;
                                        }
                                }
                        }
                    else if (crc == crc2)
                        {
                            /* CRC match - message can be decoded */
                            part->message_lock = true;
                            part->crc_ok = true;
                            part->n_crc_fail = 0;
                        }
                    else
                        {
                            /* There is no message lock and the CRC check fails. Assume there is
                             * false positive lock - rescan for preamble. */
                            part->crc_ok = false;
                            part->preamble_seen = false;

                            /* CRC mismatch - try to re-scan for preamble */
                            retry = true;
                        }
                }
            else
                {
                    /* No preamble or preamble and less than 300 bits decoded */
                }
        }
}


/**
 * Invert message bits in the buffer.
 *
 * The method inverts bits of the decoded data.
 *
 * \param[in,out] part Decoder component with a message buffer.
 *
 * \return None
 */
static void cnav_msg_invert_(cnav_v27_part_t *part)
{
    size_t i = 0;
    for (i = 0; i < sizeof(part->decoded); i++)
        {
            part->decoded[i] ^= 0xFFU;
        }
}


/**
 * Performs CNAV message decoding.
 *
 * This function decoded CNAV message, if the following conditions are met:
 * - Buffer contains 300 bits.
 * - First 8 bits are matching direct or inverse preamble.
 * - Message data CRC matches one in the buffer.
 *
 * In case the message starts with inverted preamble, the data is inverted
 * before parsing.
 *
 * \param[in,out] part Decoder component.
 * \param[out]    msg  Container for a decoded message.
 * \param[out]    delay Delay of the message in symbols.
 *
 * \retval true The message has been decoded, and \a msg container is populated.
 * \retval false Not enough data or CRC is not correct.
 *
 * \private
 */
static bool cnav_msg_decode_(cnav_v27_part_t *part, cnav_msg_t *msg, uint32_t *delay)
{
    bool res = false;
    if (GPS_CNAV_MSG_LENGTH <= part->n_decoded)
        {
            if (part->crc_ok)
                {
                    /* CRC is OK */
                    if (part->invert)
                        {
                            cnav_msg_invert_(part);
                        }

                    msg->prn = getbitu(part->decoded, 8, 6);
                    msg->msg_id = getbitu(part->decoded, 14, 6);
                    msg->tow = getbitu(part->decoded, 20, 17);
                    msg->alert = getbitu(part->decoded, 37, 1) ? true : false;

                    /* copy RAW message for GNSS-SDR */
                    memcpy(msg->raw_msg, part->decoded, GPS_L2C_V27_DECODE_BITS + GPS_L2C_V27_DELAY_BITS);

                    *delay = (part->n_decoded - GPS_CNAV_MSG_LENGTH + GPS_L2C_V27_DELAY_BITS) * 2 + part->n_symbols;

                    if (part->invert)
                        {
                            cnav_msg_invert_(part);
                        }
                    res = true;
                }
            else
                {
                    /* CRC mismatch - no decoding */
                }
            bitshl(part->decoded, sizeof(part->decoded), GPS_CNAV_MSG_LENGTH);
            part->n_decoded -= GPS_CNAV_MSG_LENGTH;
        }

    return res;
}


/**
 * Initialize CNAV decoder.
 *
 * CNAV decoder contains two Viterbi decoders that are used to estimate bit and
 * message boundary.
 *
 * \param[out] dec Decoder structure.
 *
 * \return None
 */
void cnav_msg_decoder_init(cnav_msg_decoder_t *dec)
{
    memset(dec, 0, sizeof(*dec));
    v27_init(&dec->part1.dec,
        dec->part1.decisions,
        GPS_L2_V27_HISTORY_LENGTH_BITS,
        cnav_msg_decoder_get_poly(),
        0);
    v27_init(&dec->part2.dec,
        dec->part2.decisions,
        GPS_L2_V27_HISTORY_LENGTH_BITS,
        cnav_msg_decoder_get_poly(),
        0);
    dec->part1.init = true;
    dec->part2.init = true;
    cnav_add_symbol_(&dec->part2, 0x80);
}

/**
 * Adds a received symbol to decoder.
 *
 * The method feeds the symbol into the decoder. In case there is a sufficient
 * information to produce a message, the message is decoded and symbol delay is
 * reported.
 * The time of the last input symbol can be computed from the message ToW and
 * delay by the formulae:
 * \code
 * symbolTime_ms = msg->tow * 6000 + *pdelay * 20 (L2)
 * symbolTime_ms = msg->tow * 6000 + *pdelay * 10 (L5)
 * \endcode
 *
 * \param[in,out] dec    Decoder object.
 * \param[in]     symbol Symbol value probability, where 0x00 - 100% of 0,
 *                       0xFF - 100% of 1.
 * \param[out]    msg    Buffer for decoded message. The message is available
 *                       only when message lock is acquired and CRC is correct.
 * \param[out]    pdelay Delay of message generation in symbols.
 *
 * \retval true  The message has been decoded. ToW parameter is available.
 * \retval false More data is required.
 */
bool cnav_msg_decoder_add_symbol(cnav_msg_decoder_t *dec,
    uint8_t symbol,
    cnav_msg_t *msg,
    uint32_t *pdelay)
{
    cnav_add_symbol_(&dec->part1, symbol);
    cnav_add_symbol_(&dec->part2, symbol);

    if (dec->part1.message_lock)
        {
            /* Flush data in decoder. */
            dec->part2.n_decoded = 0;
            dec->part2.n_symbols = 0;
            return cnav_msg_decode_(&dec->part1, msg, pdelay);
        }
    if (dec->part2.message_lock)
        {
            /* Flush data in decoder. */
            dec->part1.n_decoded = 0;
            dec->part1.n_symbols = 0;
            return cnav_msg_decode_(&dec->part2, msg, pdelay);
        }

    return false;
}


/**
 * Provides a singleton polynomial object.
 *
 * The method constructs and returns polynomial object for CNAV message
 * decoding. The same polynomial can be used also for other message handling.
 *
 * The object is initialized on the first call. The method is thread-safe.
 *
 * @return Pointer to polynomial object for CNAV message decoding.
 */
const v27_poly_t *cnav_msg_decoder_get_poly(void)
{
    static v27_poly_t instance;
    static bool initialized = false;

    if (!initialized)
        {
            /* Coefficients for polynomial object */
            const signed char coeffs[2] = {GPS_L2C_V27_POLY_A, GPS_L2C_V27_POLY_B};

            /* Racing condition handling: the data can be potential initialized more
             * than once in case multiple threads request concurrent access. However,
             * nature of the v27_poly_init() function and data alignment ensure that
             * the data returned from the earlier finished call is consistent and can
             * be used even when re-initialization is happening.
             *
             * Other possible approaches are:
             * - Replace late initialization with an explicit call.
             * - Use POSIX synchronization objects like pthread_once_t.
             */
            v27_poly_init(&instance, coeffs);
            initialized = true;
        }
    return &instance;
}

/** \} */
/** \} */