/*
This software is subject to the license described in the license.txt file
included with this software distribution. You may not use this file except in compliance
with this license.
Copyright (c) Dynastream Innovations Inc. 2015
All rights reserved.
*/

/**@file
 * @brief ANT Scan and Forward implementation
 *
 * @defgroup ant_scan_and_forward_example ANT Scan and Forward Demo
 * @{
 * @ingroup nrf_ant_scan_and_forward
 *
 */

#include <stdbool.h>
#include "scan_and_forward.h"
#include "deviceregistry.h"
#include "ant_parameters.h"
#include "message_cache.h"
#include "commands.h"
#include "boards.h"
#include "app_button.h"
#include "nrf_delay.h"
#include "ant_channel_config.h"
#include "ant_search_config.h"
#include "nordic_common.h"

#define ANT_NETWORK_NUMBER          ((uint8_t) 0)       /**< Default public network number. */

#define ANT_BS_CHANNEL_TYPE         CHANNEL_TYPE_SLAVE  /**< Bi-directional slave */
#define ANT_MS_CHANNEL_TYPE         CHANNEL_TYPE_MASTER /**< Bi-directional master */

#define ANT_SF_NETWORK_ID           ((uint8_t) 1)       /**< Network Number for Scan and Forward Application */

#define ANT_BS_DEVICE_NUMBER        ((uint16_t) 0)      /**< Wild card the device number on the background scanning channel */

#define ANT_DEVICE_TYPE             ((uint8_t) 1)                                   /**< Device type of Scan and Forward beacon */
#define ANT_TRANSMISSION_TYPE       ((uint8_t) ((ANT_SF_NETWORK_ID << 4) | 0x05)) /**< Transmission type is comprised of ANT_SF_NETWORK_ID and 0x05 */

#define ANT_FREQUENCY               ((uint8_t) 25)      /**< 2425 MHz */
#define ANT_CHANNEL_PERIOD          ((uint16_t) 1024)   /**< 32Hz */
#define ANT_CHANNEL_PERIOD_NONE     ((uint16_t) 0x00)   /**< This is not taken into account. */

#define ANT_MAX_NODES_IN_NETWORK    MAX_DEVICES         /**< Max number of nodes in network. Must be <= MAX_DEVICES in device registry */

#define ANT_MAX_CACHE_SIZE              255             /**< Max size of the received message buffer */
#define ANT_CACHE_TIMEOUT_IN_SEC        10              /**< Max duration cached messages will be saved */

#define ANT_DEFAULT_CMD_PG_INTLV_PCT    30              /**< Default percentage internal command pages should be transmitted */

static uint8_t m_node_address;                                          /**< Unique address of node within the network */
static uint8_t m_counter = 0;                                           /**< Index of next device */
static uint8_t m_tx_buffer[ANT_STANDARD_DATA_PAYLOAD_SIZE];             /**< Primary data transmit buffer. */
static uint8_t m_cmd_tx_buffer[ANT_STANDARD_DATA_PAYLOAD_SIZE];         /**< Command data transmit buffer. */
static uint8_t m_tx_page_counter        = 0;                            /**< Transmitted page counter. */
static uint8_t m_cache_tick_tx_counter  = 0;                            /**< Transmitted page counter for cache tick management. */
static uint8_t m_cmd_pg_intlv_pct       = ANT_DEFAULT_CMD_PG_INTLV_PCT; /**< Percentage of the time internal command pages should be transmitted */
static bool    m_flag_hi_pri_cmd        = false;                        /**< Flag to indicate this is a new command. */

static bool             enable_optimized_command_page_priority = true;  /**< Flag to enable/disable command page pattern optimizations */
static sf_message_t     buffer[ANT_MAX_CACHE_SIZE];                     /**< Received message cache buffer. */
static message_cache_t  m_rcvd_messages =                               /**< Received message cache. */
{
    0,
    0,
    0,
    ANT_MAX_CACHE_SIZE,
    buffer
};


/**@brief DEVICE_STATUS_PAGE message is queued to send.
 *
 */
static void sf_master_beacon_message_send(void);


/**@brief Checks if a message has already been received
 *
 * @param[in]   p_buffer            Pointer to buffer with message contents
 */
static bool msg_already_received(uint8_t * p_buffer);


/** @brief Sets up and opens the background scanning channel and the master beacon channel used for the scan and forward demo.
 */
static void sf_ant_channels_setup(void)
{
    uint32_t err_code;

    const ant_channel_config_t ms_channel_config =
    {
        .channel_number    = SF_ANT_MS_CHANNEL_NUMBER,
        .channel_type      = ANT_MS_CHANNEL_TYPE,
        .ext_assign        = 0x00,
        .rf_freq           = ANT_FREQUENCY,
        .transmission_type = ANT_TRANSMISSION_TYPE,
        .device_type       = ANT_DEVICE_TYPE,
        .device_number     = m_node_address,
        .channel_period    = ANT_CHANNEL_PERIOD,
        .network_number    = ANT_NETWORK_NUMBER,
    };

    const ant_channel_config_t bs_channel_config =
    {
        .channel_number    = SF_ANT_BS_CHANNEL_NUMBER,
        .channel_type      = ANT_BS_CHANNEL_TYPE,
        .ext_assign        = EXT_PARAM_ALWAYS_SEARCH,
        .rf_freq           = ANT_FREQUENCY,
        .transmission_type = ANT_TRANSMISSION_TYPE,
        .device_type       = ANT_DEVICE_TYPE,
        .device_number     = ANT_BS_DEVICE_NUMBER,
        .channel_period    = ANT_CHANNEL_PERIOD_NONE,
        .network_number    = ANT_NETWORK_NUMBER,
    };

    const ant_search_config_t bs_search_config =
    {
        .channel_number        = SF_ANT_BS_CHANNEL_NUMBER,
        .low_priority_timeout  = ANT_LOW_PRIORITY_TIMEOUT_DISABLE,
        .high_priority_timeout = ANT_HIGH_PRIORITY_SEARCH_DISABLE,
        .search_sharing_cycles = ANT_SEARCH_SHARING_CYCLES_DISABLE,
        .search_priority       = ANT_SEARCH_PRIORITY_DEFAULT,
        .waveform              = ANT_WAVEFORM_FAST,
    };

    err_code = ant_channel_init(&ms_channel_config);
    APP_ERROR_CHECK(err_code);

    err_code = ant_channel_init(&bs_channel_config);
    APP_ERROR_CHECK(err_code);

    err_code = ant_search_init(&bs_search_config);
    APP_ERROR_CHECK(err_code);

    // Fill tx buffer for the first frame
    sf_master_beacon_message_send();

    // Open master beacon channel
    err_code = sd_ant_channel_open(SF_ANT_MS_CHANNEL_NUMBER);
    APP_ERROR_CHECK(err_code);

    // Open background scanning channel
    err_code = sd_ant_channel_open(SF_ANT_BS_CHANNEL_NUMBER);
    APP_ERROR_CHECK(err_code);
}


void sf_init(void)
{
    dr_init();

    // Get the node address from the switches instead of serial number
#if defined(NODE_ID_FROM_SWITCHES)
    m_node_address = 1 + ((nrf_gpio_pin_read(BSP_SWITCH_1) << 0) |
                     (nrf_gpio_pin_read(BSP_SWITCH_2) << 1) |
                     (nrf_gpio_pin_read(BSP_SWITCH_3) << 2) |
                     (nrf_gpio_pin_read(BSP_SWITCH_4) << 3));
#else
    // Get the node address from the serial number of the device
    m_node_address = (uint8_t)(NRF_FICR->DEVICEID[0]);

    if (m_node_address == ADDRESS_ALL_NODES) // Node address 0 is not allowed
    {
        m_node_address = 1;
    }
    else if (m_node_address == RESERVED) // Node address 0xFF is not allowed
    {
        m_node_address = 0xFE;
    }
#endif

    UNUSED_PARAMETER(dr_device_add(m_node_address));

    for (int i = 0; i < ANT_STANDARD_DATA_PAYLOAD_SIZE; i++)
    {
        m_cmd_tx_buffer[i] = RESERVED;
    }

    sf_ant_channels_setup();
}


void sf_bsp_evt_handler(bsp_event_t evt)
{
    device_t * p_device;

    switch (evt)
    {
        case BSP_EVENT_KEY_0: // Turn self on
            // Update status device registry
            p_device                    = dr_device_get(m_node_address);
            p_device->application_state = STATUS_LIGHT_ON;
            p_device->last_message_sequence_received++;
            LEDS_ON(LEDS_MASK);
            break;

        case BSP_EVENT_KEY_1: // Turn self off
            // Update status device registry
            p_device                    = dr_device_get(m_node_address);
            p_device->application_state = STATUS_LIGHT_OFF;
            p_device->last_message_sequence_received++;
            LEDS_OFF(LEDS_MASK);
            break;

        case BSP_EVENT_KEY_2: // Turn all on
            // Update status device registry
            p_device                    = dr_device_get(m_node_address);
            p_device->application_state = STATUS_LIGHT_ON;
            p_device->last_message_sequence_received++;
            // Update command buffer
            set_cmd_buffer_new(ADDRESS_ALL_NODES, COMMAND_LIGHT_ON, RESERVED, RESERVED);
            LEDS_ON(LEDS_MASK);
            break;

        case BSP_EVENT_KEY_3: // Turn all off
            // Update status device registry
            p_device                    = dr_device_get(m_node_address);
            p_device->application_state = STATUS_LIGHT_OFF;
            p_device->last_message_sequence_received++;
            // Update command buffer
            set_cmd_buffer_new(ADDRESS_ALL_NODES, COMMAND_LIGHT_OFF, RESERVED, RESERVED);
            LEDS_OFF(LEDS_MASK);
            break;

        default:
            return; // no implementation needed
    }
}


void sf_background_scanner_process(ant_evt_t * p_ant_evt)
{
    device_t    * p_device;
    ANT_MESSAGE * p_ant_message = (ANT_MESSAGE *)p_ant_evt->msg.evt_buffer;

    switch (p_ant_evt->event)
    {
        case EVENT_RX:

            // Device Status Page
            if (p_ant_message->ANT_MESSAGE_aucPayload[DATA_PAGE_IND] == DEVICE_STATUS_PAGE)
            {
                uint8_t node            = p_ant_message->ANT_MESSAGE_aucPayload[DEVICE_STATUS_NODE_IND]; // Origin
                uint8_t sequence_number = p_ant_message->ANT_MESSAGE_aucPayload[DEVICE_STATUS_SEQ_NUM_IND];
                uint8_t device_state    = p_ant_message->ANT_MESSAGE_aucPayload[DEVICE_STATUS_STATE_IND];

                // Has this device been seen before?
                if (dr_device_exists(node))
                {
                    // Is this a new message?
                    if (!msg_already_received(p_ant_message->ANT_MESSAGE_aucPayload))
                    {
                        // Update status device registry
                        p_device = dr_device_get(node);
                        p_device->last_message_sequence_received = sequence_number;
                        p_device->application_state              = device_state;
                        m_tx_page_counter                        = dr_index_of_node_get(node) - 1; // Small optimization to send the new device info first
                    }
                }
                // First message received from this device.
                else
                {
                    // Attempt to add the new device
                    if (dr_device_add(node))
                    {
                        // Update status device registry
                        p_device                                 = dr_device_get(node);
                        p_device->application_state              = device_state;
                        p_device->last_message_sequence_received = sequence_number;
                    }
                }
            }
            // Internal Network Command Page
            else if(p_ant_message->ANT_MESSAGE_aucPayload[DATA_PAGE_IND] == INTERNAL_COMMAND_PAGE)
            {
                // Is this a new message?
                if (!msg_already_received(p_ant_message->ANT_MESSAGE_aucPayload))
                {
                    uint8_t node                    = p_ant_message->ANT_MESSAGE_aucPayload[INTERNAL_CMD_DST_IND]; // Destination
                    uint8_t sequence_number         = p_ant_message->ANT_MESSAGE_aucPayload[INTERNAL_CMD_SEQ_NUM_IND];
                    uint8_t cmd_page_interleave_pct = p_ant_message->ANT_MESSAGE_aucPayload[INTERNAL_CMD_CMD_DATA1_IND];
                    uint8_t high_priority_cmd_enable= p_ant_message->ANT_MESSAGE_aucPayload[INTERNAL_CMD_CMD_DATA2_IND];
                    uint8_t node_command            = p_ant_message->ANT_MESSAGE_aucPayload[INTERNAL_CMD_CMD_IND];

                    // Update command buffer
                    set_cmd_buffer_seq(node, node_command, cmd_page_interleave_pct, high_priority_cmd_enable, sequence_number);

                    if (node == m_node_address || node == ADDRESS_ALL_NODES)
                    {
                        // Update status device registry
                        p_device = dr_device_get(m_node_address);
                        p_device->last_message_sequence_received++;

                        switch (node_command)
                        {
                            case COMMAND_LIGHT_OFF:
                                p_device->application_state = STATUS_LIGHT_OFF;
                                LEDS_OFF(LEDS_MASK);
                                break;

                            case COMMAND_LIGHT_ON:
                                p_device->application_state = STATUS_LIGHT_ON;
                                LEDS_ON(LEDS_MASK);
                                break;
                            case COMMAND_CHG_CMD_PG_SET:
                                if(cmd_page_interleave_pct != RESERVED)
                                    m_cmd_pg_intlv_pct = cmd_page_interleave_pct;
                                if(high_priority_cmd_enable != RESERVED)
                                    enable_optimized_command_page_priority = high_priority_cmd_enable;
                                break;
                        }
                    }
                }
            }
            break;

        default:
            break;

    }
}


static void sf_master_beacon_message_send(void)
{
    uint32_t      err_code;
    device_t    * p_device      = NULL;

    // Number of messages being transmitted per second
    uint8_t msg_tx_freq = ANT_CLOCK_FREQUENCY/ANT_CHANNEL_PERIOD;

    // Number of command pages required to be transmitted per second
    uint16_t req_cmd_page_count = msg_tx_freq * m_cmd_pg_intlv_pct / 100;

    // If we are sending a new command, prioritize it. Or if we are unable to send command pages.
    if(m_flag_hi_pri_cmd)
    {
    // Transmit new command for 1 second less 1 channel period
        req_cmd_page_count = msg_tx_freq - 1;
    }

    // Transmit status pages after send all of the required command pages for the channel period
    if(m_tx_page_counter % msg_tx_freq >= req_cmd_page_count)
    {
        // Unflag high priority command rotation once we have started sending status pages
        m_flag_hi_pri_cmd = false;

        // Cycle through available device numbers until we get to a registered device.
        do
        {
            if (m_counter < (MAX_DEVICES - 1))
            {
                m_counter++;
            }
            else
            {
                m_counter = 0;
            }
        }
        while (!dr_device_at_index_exists(m_counter));

        p_device = dr_device_at_index_get(m_counter);

        // Update the primary tx buffer
        m_tx_buffer[DATA_PAGE_IND] = DEVICE_STATUS_PAGE;
        m_tx_buffer[DEVICE_STATUS_NODE_IND] = p_device->node_id;
        m_tx_buffer[2] = RESERVED;
        m_tx_buffer[3] = RESERVED;
        m_tx_buffer[4] = RESERVED;
        m_tx_buffer[5] = RESERVED;
        m_tx_buffer[DEVICE_STATUS_SEQ_NUM_IND] = p_device->last_message_sequence_received;
        m_tx_buffer[DEVICE_STATUS_STATE_IND] = p_device->application_state;

        // Add the message we are transmitting to the cache
        mc_add(&m_rcvd_messages, m_tx_buffer);

        err_code = sd_ant_broadcast_message_tx(SF_ANT_MS_CHANNEL_NUMBER,
                                               ANT_STANDARD_DATA_PAYLOAD_SIZE,
                                               m_tx_buffer);
        APP_ERROR_CHECK(err_code);
    }
    // Send command page when we are not sending status pages
    else
    {
        // Add the message we are transmitting to the cache
        mc_add(&m_rcvd_messages, m_cmd_tx_buffer);

        err_code = sd_ant_broadcast_message_tx(SF_ANT_MS_CHANNEL_NUMBER,
                                               ANT_STANDARD_DATA_PAYLOAD_SIZE,
                                               m_cmd_tx_buffer);
        APP_ERROR_CHECK(err_code);
    }

    // Increment transmitted page counters
    m_tx_page_counter++;
    m_cache_tick_tx_counter++;

    // Piggy back off of the channel period to provide 1 second ticks for cache cleanup
    if (m_cache_tick_tx_counter % msg_tx_freq == 0)
    {
        mc_cleanup(&m_rcvd_messages, ANT_CACHE_TIMEOUT_IN_SEC);
    }
}


void sf_master_beacon_process(ant_evt_t * p_ant_evt)
{
    ANT_MESSAGE * p_ant_message = (ANT_MESSAGE *)p_ant_evt->msg.evt_buffer;

    switch (p_ant_evt->event)
    {
        case EVENT_TX:
            sf_master_beacon_message_send();
            break;

        case EVENT_RX:
            sf_external_received_message_process(   p_ant_message->ANT_MESSAGE_aucPayload[DATA_PAGE_IND],
                                                    p_ant_message->ANT_MESSAGE_aucPayload[MOBILE_CMD_DST_IND],
                                                    p_ant_message->ANT_MESSAGE_aucPayload[MOBILE_CMD_CMD_IND],
                                                    p_ant_message->ANT_MESSAGE_aucPayload[MOBILE_CMD_CMD_DATA1_IND],
                                                    p_ant_message->ANT_MESSAGE_aucPayload[MOBILE_CMD_CMD_DATA2_IND]);
            break;

        default:
            break;
    }
}


static bool msg_already_received(uint8_t * p_buffer)
{
    if (mc_is_in_cache(&m_rcvd_messages, p_buffer))
    {
        return true;
    }

    // New message, store for future comparisons
    mc_add(&m_rcvd_messages, p_buffer);

    return false;
}


void set_cmd_buffer_seq(uint8_t dst, uint8_t cmd, uint8_t data1, uint8_t data2, uint8_t seq)
{
    m_cmd_tx_buffer[DATA_PAGE_IND] = INTERNAL_COMMAND_PAGE;
    m_cmd_tx_buffer[INTERNAL_CMD_DST_IND] = dst;
    m_cmd_tx_buffer[2] = RESERVED;
    m_cmd_tx_buffer[3] = RESERVED;
    m_cmd_tx_buffer[INTERNAL_CMD_CMD_DATA2_IND] = data2;
    m_cmd_tx_buffer[INTERNAL_CMD_CMD_DATA1_IND] = data1;
    m_cmd_tx_buffer[INTERNAL_CMD_SEQ_NUM_IND] = seq;
    m_cmd_tx_buffer[INTERNAL_CMD_CMD_IND] = cmd;

    if(enable_optimized_command_page_priority || cmd == COMMAND_CHG_CMD_PG_SET)
    {
        m_flag_hi_pri_cmd = true;   // Flag that command page should take priority over other pages

        // Small optimization to send the new command first.
        // The master channel is configured to send cmd pages first at the start of every second
        m_tx_page_counter = 0;
    }
}


void set_cmd_buffer_new(uint8_t dst, uint8_t cmd, uint8_t payload0, uint8_t payload1)
{
    set_cmd_buffer_seq(dst, cmd, payload0, payload1, m_cmd_tx_buffer[6]+1);
}


void sf_external_received_message_process(uint8_t page, uint8_t dst, uint8_t data0, uint8_t data1, uint8_t data2)
{
    device_t    * p_device      = NULL;
    uint8_t command = data0;

    // Mobile Command Page
    switch (page)
    {
        case MOBILE_COMMAND_PAGE:

            if (dr_device_exists(dst))
            {
                p_device = dr_device_get(dst);
            }

            switch (command)
            {
                case COMMAND_LIGHT_ON:

                    set_cmd_buffer_new(dst, COMMAND_LIGHT_ON,
                                   RESERVED, RESERVED);

                    if ((p_device != NULL && p_device->node_id == m_node_address)
                                            || dst == ADDRESS_ALL_NODES)
                    {
                        p_device                    = dr_device_get(m_node_address);
                        p_device->application_state = STATUS_LIGHT_ON;
                        p_device->last_message_sequence_received++;
                        LEDS_ON(LEDS_MASK);
                    }
                    break;

                case COMMAND_LIGHT_OFF:

                    set_cmd_buffer_new(dst, COMMAND_LIGHT_OFF,
                                   RESERVED, RESERVED);

                    if ((p_device != NULL && p_device->node_id == m_node_address)
                                            || dst == ADDRESS_ALL_NODES)
                    {
                        p_device                    = dr_device_get(m_node_address);
                        p_device->application_state = STATUS_LIGHT_OFF;
                        p_device->last_message_sequence_received++;
                        LEDS_OFF(LEDS_MASK);
                    }
                    break;

                case COMMAND_CHG_CMD_PG_SET :

                    set_cmd_buffer_new(dst, COMMAND_CHG_CMD_PG_SET,
                                   data1, data2);

                    if (dst == m_node_address || dst == ADDRESS_ALL_NODES)
                    {
                        if(data1 != RESERVED)
                            m_cmd_pg_intlv_pct = data1;
                        if(data2 != RESERVED)
                            enable_optimized_command_page_priority = data2;
                    }
                    break;
            }
            break;
    }
}

