/*
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.
*/

/*
 * Before compiling this example for NRF52, complete the following steps:
 * - Download the S212 SoftDevice from <a href="https://www.thisisant.com/developer/components/nrf52832" target="_blank">thisisant.com</a>.
 * - Extract the downloaded zip file and copy the S212 SoftDevice headers to <tt>\<InstallFolder\>/components/softdevice/s212/headers</tt>.
 * If you are using Keil packs, copy the files into a @c headers folder in your example folder.
 * - Make sure that @ref ANT_LICENSE_KEY in @c nrf_sdm.h is uncommented.
 */

#include <stdio.h>
#include <stdlib.h>
#include <stdint.h>
#include "nrf.h"
#include "app_error.h"
#include "app_timer.h"
#include "bsp.h"
#include "boards.h"
#include "softdevice_handler.h"
#include "ant_stack_config.h"
#include "ant_channel_config.h"
#include "ant_stack_handler_types.h"
#include "ant_interface.h"
#include "ant_parameters.h"
#include "commands.h"


#define APP_TIMER_PRESCALER           0                     /**< Value of the RTC1 PRESCALER register. */
#define APP_TIMER_OP_QUEUE_SIZE       2u                    /**< Size of timer operation queues. */

#define INDICATE_SELF

#define SERIAL_NUMBER_ADDRESS       ((uint32_t) 0x10000060)  // FICR + 60

#define ANT_SCAN_CHANNEL_NUMBER     ((uint8_t) 0)            /**< Scanning channel number. */
#define ANT_RESPONSE_CHANNEL_NUMBER ((uint8_t) 1)            /**< Response channel number. */

#define ANT_NETWORK_NUMBER          ((uint8_t) 0)            /**< Default public network number. */
#define ANT_BS_CHANNEL_TYPE         CHANNEL_TYPE_SLAVE       /**< Channel type background scanning (Bi-directional slave). */
#define ANT_BS_DEVICE_NUMBER        ((uint16_t) 0)           /**< Device number (wild-card). */

#define ANT_DEVICE_TYPE             ((uint8_t) 1)            /**< Device type. */
#define ANT_TRANSMISSION_TYPE       ((uint8_t) 0x15)         /**< Transmission type. */
#define ANT_FREQUENCY               ((uint8_t) 25)           /**< Radio frequency. */

#define MAX_DEVICES                 ((uint8_t) 16)           /**< Maximum number of nodes supported in this network. */
#define ANT_HOME_ID                 ((uint16_t) 0x1234)      /**< Unique ID for the entire network. */

#define SCAN_TIMER_TICKS            APP_TIMER_TICKS(500u, APP_TIMER_PRESCALER)    /**< Scan timer ticks. */

#define MAX_RETRIES                 ((uint8_t) 5)            /**< Max retries. */

#define CONVERT_DEVICE_NUM_TO_INDEX(device_number) (device_number % MAX_DEVICES)    /**< Hash function. */

typedef struct
{
    uint8_t rssi;               /**< RSSI. */
    uint16_t device_number;     /**< Device number. */
}node_t;


static uint8_t m_tx_buffer[ANT_STANDARD_DATA_PAYLOAD_SIZE];  /**< Transmit buffer. */
static node_t m_node_list[MAX_DEVICES];                      /**< List of state of all devices in the network. */

static uint8_t m_command;                                    /**< Command. */
static uint8_t m_retries;                                    /**< Retries. */

APP_TIMER_DEF(m_scan_timer_id);                              /**< Scan timer. */


/**@brief Send command to node with highest RSSI.
 * If a global command was selected, send to all.
 */
static void send_command(void)
{
    uint16_t device_number = 0;
    uint8_t  max_rssi      = 0;
    uint32_t err_code;

    // Loop through nodes and pick highest rssi node
    for(int node = 0; node < MAX_DEVICES; node++)
    {
        if(m_node_list[node].device_number != 0)
        {
            if(m_node_list[node].rssi > max_rssi)
            {
                max_rssi = m_node_list[node].rssi;
                device_number = m_node_list[node].device_number;
            }
        }
    }

    // Do not send the command if no device was found
    // Close scanning channel and return
    if( device_number == 0 )
    {
        LEDS_ON(BSP_LED_2_MASK);
        LEDS_OFF(BSP_LED_0_MASK);
        err_code = sd_ant_channel_close(ANT_SCAN_CHANNEL_NUMBER);
        APP_ERROR_CHECK(err_code);
        return;
    }
    else
    {
        LEDS_OFF(BSP_LED_2_MASK);
    }

    err_code = sd_ant_channel_id_set(ANT_RESPONSE_CHANNEL_NUMBER,
                                     device_number,
                                     ANT_DEVICE_TYPE,
                                     ANT_TRANSMISSION_TYPE);
    APP_ERROR_CHECK(err_code);

    switch( m_command )
    {
        case COMMAND_STATE_ON:
            m_command = COMMAND_LIGHT_ON;
            break;

        case COMMAND_STATE_OFF:
            m_command = COMMAND_LIGHT_OFF;
            break;

        case COMMAND_STATE_ALL_ON:
            device_number = ADDRESS_ALL_NODES;
            m_command = COMMAND_LIGHT_ON;
            break;

        case COMMAND_STATE_ALL_OFF:
            device_number = ADDRESS_ALL_NODES;
            m_command = COMMAND_LIGHT_OFF;
            break;
        
        default:
            break;
    }

    m_tx_buffer[0] = MOBILE_COMMAND_PAGE;
    m_tx_buffer[1] = 0xFF;
    m_tx_buffer[2] = device_number;
    m_tx_buffer[3] = 0xFF;
    m_tx_buffer[4] = 0xFF;
    m_tx_buffer[5] = 0xFF;
    m_tx_buffer[6] = 0xFF;
    m_tx_buffer[7] = m_command;

    err_code = sd_ant_acknowledge_message_tx(ANT_RESPONSE_CHANNEL_NUMBER,
                                             ANT_STANDARD_DATA_PAYLOAD_SIZE,
                                             m_tx_buffer);
    APP_ERROR_CHECK(err_code);
}

/**@brief Handler for scan timer event 
 * @param p_context     Pointer to context
 */
static void scan_timeout_event(void * p_context)
{
    send_command();
    m_retries = MAX_RETRIES;
}

/**@brief Start sequence for sending command over scanning channel
 *
 * @param[in] command   Command selected with button 
 */
static void start_command_sequence(uint8_t command)
{
    uint32_t err_code;
    
    LEDS_ON(BSP_LED_0_MASK);

    for(int node = 0; node < MAX_DEVICES; node++)
    {
        m_node_list[node].device_number = 0;
        m_node_list[node].rssi = 0;
    }
    m_command = command;
    
    err_code = sd_ant_rx_scan_mode_start(ANT_SCAN_CHANNEL_NUMBER);   
    APP_ERROR_CHECK(err_code);
    
    err_code = app_timer_start(m_scan_timer_id, SCAN_TIMER_TICKS, NULL);
    APP_ERROR_CHECK(err_code);
}


/**@brief Function for handling button events.
 *
 * @param[in] evt     BSP event
 */
static void bsp_evt_handler(bsp_event_t evt)
{    
    switch (evt)
    {
        case BSP_EVENT_KEY_0:
            LEDS_OFF(BSP_LED_2_MASK);
            start_command_sequence(COMMAND_STATE_ON);
            break;

        case BSP_EVENT_KEY_1:
            LEDS_OFF(BSP_LED_2_MASK);
            start_command_sequence(COMMAND_STATE_OFF);
            break;

        case BSP_EVENT_KEY_2:
            LEDS_OFF(BSP_LED_2_MASK);
            start_command_sequence(COMMAND_STATE_ALL_ON);
            break;

        case BSP_EVENT_KEY_3:
            LEDS_OFF(BSP_LED_2_MASK);
            start_command_sequence(COMMAND_STATE_ALL_OFF);
            break;

        default:
            break;
    }
}


/**@brief Function for the Timer and BSP initialization.
 */
static void utils_setup(void)
{
    uint32_t err_code;

    APP_TIMER_INIT(APP_TIMER_PRESCALER, APP_TIMER_OP_QUEUE_SIZE, false);
    err_code = bsp_init(BSP_INIT_LED | BSP_INIT_BUTTONS,
                        APP_TIMER_TICKS(100, APP_TIMER_PRESCALER),
                        bsp_evt_handler);
    APP_ERROR_CHECK(err_code);
}



/**@brief initialize application
 */
static void continuous_scan_init()
{
    uint32_t err_code;

    // Set library config to report RSSI and Device ID
    err_code = sd_ant_lib_config_set(ANT_LIB_CONFIG_MESG_OUT_INC_RSSI | ANT_LIB_CONFIG_MESG_OUT_INC_DEVICE_ID);
    APP_ERROR_CHECK(err_code);
    
    // Configure channel 0 for scanning mode, but do not open it.
    // The scanning channel will be opened in scan mode for a short amount of time on a button press.
    ant_channel_config_t channel_config =
    {
        .channel_number     = ANT_SCAN_CHANNEL_NUMBER,
        .channel_type       = ANT_BS_CHANNEL_TYPE,
        .ext_assign         = 0,
        .rf_freq            = ANT_FREQUENCY,
        .transmission_type  = ANT_TRANSMISSION_TYPE,
        .device_type        = ANT_DEVICE_TYPE,
        .device_number      = ANT_BS_DEVICE_NUMBER,
        .channel_period     = 0,    // Not used, since this is going to be scanning
        .network_number     = ANT_NETWORK_NUMBER,
    };
    
    err_code = ant_channel_init(&channel_config);
    APP_ERROR_CHECK(err_code);
    
    // Assign a second channel for sending messages on the reverse direction
    // There is no need to configure any other parameters, this channel is never opened;
    // its resources are used by ANT to send messages in the reverse direction while in
    // continuous scanning mode.
    err_code = sd_ant_channel_assign(ANT_RESPONSE_CHANNEL_NUMBER,
                                     ANT_BS_CHANNEL_TYPE,
                                     ANT_NETWORK_NUMBER,
                                     0x00);
    APP_ERROR_CHECK(err_code);
}

/** @brief Add a newly discovered node to the m_node_list
 *
 * @param[in] device_number   The device_number of the current to be added
 * @param[in] rssi            The RSSI from the current node to be added
 */
void add_node_to_list(uint16_t device_number, uint8_t rssi)
{
   uint8_t index = MAX_DEVICES; // Specifed maximum total number in the array
   uint8_t start = CONVERT_DEVICE_NUM_TO_INDEX(device_number); // Start index of the array

    uint8_t node_device_number;
    for(int i = 0; i < MAX_DEVICES; i++ )
    {
        node_device_number = m_node_list[(start + i) % MAX_DEVICES].device_number;

        if( node_device_number == device_number ) // If this node already exists, don't add it
            return;

        if(node_device_number == 0) // If we found a place to put it
        {   // Find the best place to put this node (the place closest to the hash function result)
            index = (start + i) % MAX_DEVICES;
            break;
        }
    }

    if( index == MAX_DEVICES )
        return;

    m_node_list[index].device_number = device_number;
    m_node_list[index].rssi = rssi;
}

/**@brief Process ANT message on ANT scanner
 *
 * @param[in] p_ant_event ANT message content.
 */
void continuous_scan_event_handler(ant_evt_t * p_ant_evt)
{
    uint32_t err_code;
    uint8_t  message_rssi;
    uint16_t message_device_number;

    ANT_MESSAGE * p_ant_message = (ANT_MESSAGE *) p_ant_evt->msg.evt_buffer;

    switch(p_ant_evt->event)
    {
        case EVENT_RX:
            if(p_ant_message->ANT_MESSAGE_aucPayload[0] == DEVICE_STATUS_PAGE)
            {
                LEDS_INVERT(BSP_LED_1_MASK);
                if(p_ant_message->ANT_MESSAGE_stExtMesgBF.bANTDeviceID && p_ant_message->ANT_MESSAGE_stExtMesgBF.bANTRssi)
                {
                    message_device_number = (uint16_t)(p_ant_message->ANT_MESSAGE_aucExtData[0] | ((uint16_t)p_ant_message->ANT_MESSAGE_aucExtData[1] << 8));
                    message_rssi = p_ant_message->ANT_MESSAGE_aucExtData[5];
                    add_node_to_list(message_device_number, message_rssi);
                }
            }
            break;

        case EVENT_TRANSFER_TX_COMPLETED:
            LEDS_OFF(BSP_LED_0_MASK | BSP_LED_1_MASK);
            err_code = sd_ant_channel_close(ANT_SCAN_CHANNEL_NUMBER);
            APP_ERROR_CHECK(err_code);
            break;

        case EVENT_TRANSFER_TX_FAILED:
            if(m_retries > 0)
            {
                send_command();
                m_retries--;
            }
            else
            {
                LEDS_OFF(BSP_LED_0_MASK | BSP_LED_1_MASK);
                err_code = sd_ant_channel_close(ANT_SCAN_CHANNEL_NUMBER);
                APP_ERROR_CHECK(err_code);
            }
            break;

        default:
            break;
    }
}



/* Main function */
int main(void)
{
    uint32_t err_code;
    nrf_clock_lf_cfg_t clock_lf_cfg = NRF_CLOCK_LFCLKSRC;
    
    utils_setup();
    
    // Setup SoftDevice and events handler
    err_code = softdevice_ant_evt_handler_set(continuous_scan_event_handler);
    APP_ERROR_CHECK(err_code);

    err_code = softdevice_handler_init(&clock_lf_cfg, NULL, 0, NULL);
    APP_ERROR_CHECK(err_code);

    err_code = ant_stack_static_config();
    APP_ERROR_CHECK(err_code);
        
    continuous_scan_init();

    err_code = app_timer_create(&m_scan_timer_id,
                                APP_TIMER_MODE_SINGLE_SHOT,
                                scan_timeout_event);
    APP_ERROR_CHECK(err_code);

    // Enter main loop
    for (;;)
    {
        err_code = sd_app_evt_wait();
        APP_ERROR_CHECK(err_code);
    }
}

/**
 *@}
 **/
