#include <stdint.h>
#include <string.h>
#include <stdarg.h>
#include <stdlib.h>
#include <stdio.h>
#include "nrf_soc.h"
#include "nrf_sdm.h"
#include "ble.h"
#include "ble_hci.h"
#include "nrf_drv_config.h"  //board specific data, based on examples with additional Defines for LEDs
#include "boards.h"
//Added:
#include "app_timer.h"
#include "ble_advdata.h"
#include "app_error.h"
#include "softdevice_handler.h"
#include "nrf_gpio.h"
//#include "SEGGER_RTT.h"
#include "led_softblink.h"

/*
#define BOARD_PCA20006	1
#define BSP_DEFINES_ONLY 1
#define BSP_SIMPLE 1
*/
#ifndef LED_RGB_RED
#define LED_RGB_RED		BSP_LED_2
#endif
#ifndef LED_RGB_GREEN
#define LED_RGB_GREEN	BSP_LED_0
#endif
#ifndef LED_RGB_BLUE
#define LED_RGB_BLUE	BSP_LED_1
#endif

APP_TIMER_DEF(m_main_timer_id);

#define DEBUG						1
#define PERIPHERAL_DEVICE_NAME      	"DSA"
#define CENTRAL_LINK_COUNT          0                                  /**< Number of central links used by the application. When changing this number remember to adjust the RAM settings*/
#define PERIPHERAL_LINK_COUNT       0                                  /**< Number of peripheral links used by the application. When changing this number remember to adjust the RAM settings*/

#define MAIN_SAMPLE_RATE			1  // sample rate for main loop in seconds
#define ADV_INTERVAL				500 // Advertisement interval in milliseconds
#define SCAN_WINDOW_MS					95  //scan window in milliseconds
#define SCAN_INTERVAL_MS				100  // scan interval in milliseconds
#define NON_CONNECTABLE_ADV_INTERVAL    MSEC_TO_UNITS(ADV_INTERVAL, UNIT_0_625_MS)
#define SCAN_WINDOW					MSEC_TO_UNITS(SCAN_WINDOW_MS, UNIT_0_625_MS)  //scan window in 0.625 mus
#define SCAN_INTERVAL				MSEC_TO_UNITS(SCAN_INTERVAL_MS, UNIT_0_625_MS)  // scan interval in 0.625 mus

#define STATUS_I					1
#define STATUS_S 					0

#define TAG_ID						12 // 0..120
#define INITIAL_STATUS				STATUS_S

#define INIT_SHOW_STATUS_LED				1
#define TIMEOUT_KONTAKT 			30 // Timeout for received adv. packet in seconds. 0..255 before infect contact status is resest
#define LIMIT_INFECT 				60  // Time needed with established contact for infection in seconds
#define LIMIT_RSSI		 			-60
#define TX_POWER					-20
#define TAG_INFO_LENGTH 			19
#define TIME_ZERO					0x0000   // Time initialization
#define NO_SOURCE					0x00, 0x00, 0x00, 0x00, \
										0x00, 0x00, 0x00, 0x00, \
										0x00, 0x00, 0x00, 0x00, \
										0x00, 0x00, 0x00
#define POS_ID						7
#define POS_STATUS					POS_ID+1
#define POS_TIME					POS_ID+2
#define POS_NAME_START				POS_ID+21
#define SEARCH_LEN					4

// General application timer settings.
#define APP_TIMER_PRESCALER             0    // Value of the RTC1 PRESCALER register.
#define APP_TIMER_OP_QUEUE_SIZE         10    // Size of timer operation queues.


static ble_gap_adv_params_t m_adv_params;
static ble_gap_scan_params_t m_scan_param;
static ble_advdata_manuf_data_t manuf_specific_data;
static ble_advdata_t advdata;

static uint8_t status_tag = INITIAL_STATUS;

static uint8_t search[SEARCH_LEN] = {0x44, 0x53, 0x41, 0x00};  //DSA0
static uint8_t kontakt = 0;

static int8_t limit_rssi = LIMIT_RSSI;
static uint8_t show_status_led = INIT_SHOW_STATUS_LED;
static uint8_t led_status = LED_RGB_GREEN;
static uint8_t limit_timeout_kontakt = TIMEOUT_KONTAKT; // 60 sekunden
static uint16_t limit_time_infect = LIMIT_INFECT; //10 Minuten
static uint16_t time_counter = TIME_ZERO;
static uint8_t infect_source[15] = {NO_SOURCE};


static uint8_t m_beacon_info[TAG_INFO_LENGTH] =
{
    TAG_ID,
	STATUS_S,
	0x00, 0x00,
    NO_SOURCE
};

// For central tag to set and reset
/*
#define CENTRAL_INFO_LENGTH =
static uint8_t m_beacon_info[CENTRAL_INFO_LENGTH] =
{
	0x00,					// TAG_ID = 0
	MODE,  //0xFF to act	// 0..255 0xFF to act on all tags;
	RESET_ALL,				// 0..1 seconds
	SET_SHOW_STATUS_LED,	// 0..1 seconds
	SHOW_STATUS_LED,		// 0..1 seconds
	SET_TIMEOUT_KONTAKT,	// 0..1 seconds
	TIMEOUT_KONTAKT,   		// 0..255 seconds
	SET_LIMIT_INFECT,  		// 0..1 seconds
	LIMIT_INFECT,			// 0..255 minutes
	SET_LIMIT_RSSI,			// 0..1 seconds
	LIMIT_RSSI,				// 0..255 ( -RX given)
}*/

static void gap_params_init(void)
{
    uint32_t                err_code;
    ble_gap_conn_sec_mode_t sec_mode;
    
    BLE_GAP_CONN_SEC_MODE_SET_OPEN(&sec_mode);
    
    //err_code = sd_ble_gap_device_name_set(&sec_mode, (const uint8_t *)device_name, 4);
    err_code = sd_ble_gap_device_name_set(&sec_mode, (const uint8_t *)PERIPHERAL_DEVICE_NAME, strlen(PERIPHERAL_DEVICE_NAME));
    APP_ERROR_CHECK(err_code);
    err_code = sd_ble_gap_tx_power_set(TX_POWER);
}

static void advertising_init(void)
{
    uint32_t err_code; 
    uint8_t flags = BLE_GAP_ADV_FLAG_BR_EDR_NOT_SUPPORTED;

    manuf_specific_data.company_identifier = 0xFFFF;
    m_beacon_info[1] = status_tag;
    manuf_specific_data.data.p_data = (uint8_t *) m_beacon_info;
    manuf_specific_data.data.size   = TAG_INFO_LENGTH;

    memset(&advdata, 0, sizeof(advdata));
    
    advdata.name_type             = BLE_ADVDATA_FULL_NAME;

    advdata.flags				  = flags;
   advdata.p_manuf_specific_data = &manuf_specific_data;


    err_code = ble_advdata_set(&advdata, NULL);
    APP_ERROR_CHECK(err_code);

    // Initialize advertising parameters (used when starting advertising).
    memset(&m_adv_params, 0, sizeof(m_adv_params));

    m_adv_params.type        = BLE_GAP_ADV_TYPE_ADV_NONCONN_IND;
    m_adv_params.interval    = NON_CONNECTABLE_ADV_INTERVAL;

}

static void advertising_start(void)
{
    uint32_t err_code;

    err_code = sd_ble_gap_adv_start(&m_adv_params);
    APP_ERROR_CHECK(err_code);
}

static void on_ble_evt(ble_evt_t * p_ble_evt)
{
    const ble_gap_evt_t   * p_gap_evt = &p_ble_evt->evt.gap_evt;
    static uint8_t sender_id =0;
	static uint8_t i = 0;
	uint32_t err_code;

    switch (p_ble_evt->header.evt_id)
    {
        case BLE_GAP_EVT_ADV_REPORT:
        {
        	if(	p_gap_evt->params.adv_report.data[POS_NAME_START] == search[0] && \
        		p_gap_evt->params.adv_report.data[POS_NAME_START+1] == search[1] && \
				p_gap_evt->params.adv_report.data[POS_NAME_START+2] == search[2] )
        	{
        		//nrf_gpio_pin_toggle(LED_RGB_BLUE);  // debug
        		sender_id = p_gap_evt->params.adv_report.data[POS_ID];
        		if( sender_id <=120){
        			if( sender_id == search[3] )
        			{
        				// Reset by center user to be implemented
        				if( p_gap_evt->params.adv_report.data[POS_ID+1]== 0xFF){
        					// Switch to control mode
        					if( p_gap_evt->params.adv_report.data[POS_ID+2]== 1){
        						//  Total reset
        						kontakt = 0;
        						status_tag = INITIAL_STATUS;
        						time_counter = TIME_ZERO;
        						for (i = 0;i<16;i++){
        									infect_source[i] = 0x00;
        						}
        						err_code = led_softblink_stop();
        						APP_ERROR_CHECK(err_code);
        						if( show_status_led == 1)
        						{
        							switch(status_tag)
        							{
        							case STATUS_I:
        								led_status = LED_RGB_RED;
        								break;
        							default:
        								led_status = LED_RGB_GREEN;
        							}
        						}
        						err_code = led_softblink_start((1<<led_status));
        						APP_ERROR_CHECK(err_code);

        					}
        					if( p_gap_evt->params.adv_report.data[POS_ID+3]== 1){
        						// Set LED_Show status
        						show_status_led = p_gap_evt->params.adv_report.data[POS_ID+4];
        						err_code = led_softblink_stop();
        						APP_ERROR_CHECK(err_code);
        						if( show_status_led == 1)
        						{
        							switch(status_tag)
        							{
        							case STATUS_I:
        								led_status = LED_RGB_RED;
        								break;
        							default:
        								led_status = LED_RGB_GREEN;
        							}
        						}
        						err_code = led_softblink_start((1<<led_status));
        						APP_ERROR_CHECK(err_code);

        					}
        					if( p_gap_evt->params.adv_report.data[POS_ID+5]== 1){
        						// Set timeout kontakt
        						limit_timeout_kontakt = p_gap_evt->params.adv_report.data[POS_ID+6];
        					}
        					if( p_gap_evt->params.adv_report.data[POS_ID+7]== 1){
        						// Set time limit infect
        						limit_time_infect = 60*(p_gap_evt->params.adv_report.data[POS_ID+8]);

        					}
        					if( p_gap_evt->params.adv_report.data[POS_ID+9]== 1){
        						// Set limit rssi
        						limit_rssi = -(p_gap_evt->params.adv_report.data[POS_ID+10]);

        					}

        				}
        			}
        			else
        			{
        				if(p_gap_evt->params.adv_report.rssi >= limit_rssi ){
        					if( p_gap_evt->params.adv_report.data[POS_STATUS] == STATUS_I && \
        							status_tag == STATUS_S )
        					{
        						kontakt = 1;
        						//nrf_gpio_pin_toggle(LED_RGB_BLUE); // debug
        						infect_source[sender_id >> 3] |=  (1<< ( 8-(sender_id&7) ) );
        					}

        				}

        			}
        		}
        	}
            break;
        }
        default:
            break;
    }
}

static void ble_evt_dispatch(ble_evt_t * p_ble_evt)
{
    on_ble_evt(p_ble_evt);
}

static void ble_stack_init(void)
{
	nrf_clock_lf_cfg_t clock_lf_cfg = NRF_CLOCK_LFCLKSRC;
    uint32_t err_code;
    ble_enable_params_t ble_enable_params;

    // Initialize the SoftDevice handler module.
    SOFTDEVICE_HANDLER_INIT(&clock_lf_cfg, NULL);

    // Enable BLE stack 

    err_code = softdevice_enable_get_default_config(CENTRAL_LINK_COUNT,
    		PERIPHERAL_LINK_COUNT,
			&ble_enable_params);
    APP_ERROR_CHECK(err_code);

    //Check the ram settings against the used number of links
	CHECK_RAM_START_ADDR(CENTRAL_LINK_COUNT,PERIPHERAL_LINK_COUNT);

	// Enable BLE stack.
	err_code = softdevice_enable(&ble_enable_params);
	APP_ERROR_CHECK(err_code);

	// Register with the SoftDevice handler module for BLE events.
	err_code = softdevice_ble_evt_handler_set(ble_evt_dispatch);
	APP_ERROR_CHECK(err_code);

	// Register with the SoftDevice handler module for System events.
	//err_code = softdevice_sys_evt_handler_set(sys_evt_dispatch);
	//APP_ERROR_CHECK(err_code);
}

// Create timers
static void timer_main_event_handler(void* p_context)
{

	uint32_t err_code;
	static uint8_t i = 0;

	static uint16_t time_kontakt = 0;
	static uint16_t timer_infect = 0;
	static uint16_t infect_time = 0;
	static uint16_t delta_t = 0;




	// Increment time since power on
	time_counter += MAIN_SAMPLE_RATE;

	if( status_tag == STATUS_S){
		if( kontakt==1) {
			time_kontakt = limit_timeout_kontakt;
			kontakt = 0;
		}else {
			if(time_kontakt == 0 ){}
			else {
					time_kontakt -=MAIN_SAMPLE_RATE;
			}
		}

		if( time_kontakt > 0 ) {
			timer_infect +=MAIN_SAMPLE_RATE;
		}else {
			timer_infect = 0;
			for (i = 0;i<16;i++){
					infect_source[i] = 0x00;
				}
		}

		if( timer_infect	> limit_time_infect ){
			status_tag = STATUS_I;
//			nrf_gpio_pins_clear(LED_RGB_GREEN);
			err_code = led_softblink_stop();
			APP_ERROR_CHECK(err_code);
			if( show_status_led == 1)
		    {
		    	switch(status_tag)
		    	{
		    	case STATUS_I:
		    		led_status = LED_RGB_RED;
		    		break;
		    	default:
		    		led_status = LED_RGB_GREEN;
		    	}
		    }
		    err_code = led_softblink_start((1<<led_status));
		    APP_ERROR_CHECK(err_code);
			for (i = 0;i<16;i++){
				m_beacon_info[4+i] = infect_source[i];
			}
			m_beacon_info[1] = status_tag;
			infect_time = time_counter;
			timer_infect = 0;
			time_kontakt = 0;
		}

	}

	//update adv_params
	delta_t = time_counter-infect_time; // FIXME, divide by 60  or right shift by 6  ; >>6

	m_beacon_info[3] = delta_t & 0xff;
	m_beacon_info[2] = (delta_t>>8) & 0xff;


    manuf_specific_data.data.p_data = (uint8_t *) m_beacon_info;
    advdata.p_manuf_specific_data = &manuf_specific_data;
    err_code = ble_advdata_set(&advdata, NULL);
    APP_ERROR_CHECK(err_code);

}

static void create_timers()
{
    uint32_t err_code;


    // Create timers
    err_code = app_timer_create(&m_main_timer_id,
                                APP_TIMER_MODE_REPEATED,
                                timer_main_event_handler);
    APP_ERROR_CHECK(err_code);
}

static void power_manage(void)
{
    uint32_t err_code = sd_app_evt_wait();
    APP_ERROR_CHECK(err_code);
}

static void scan_init(void)
{
    m_scan_param.active = 0;
    m_scan_param.selective = 0;
    m_scan_param.p_whitelist = NULL;
    m_scan_param.timeout = 0;
    m_scan_param.interval = (uint16_t)SCAN_INTERVAL;
    m_scan_param.window = (uint16_t)SCAN_WINDOW;
}

static void scan_start(void)
{  
    uint32_t err_code;
    //(void) sd_ble_gap_scan_stop();
    err_code = sd_ble_gap_scan_start(&m_scan_param);
    APP_ERROR_CHECK(err_code);
}

int main(void)
{
    uint32_t time_ms = MAIN_SAMPLE_RATE*1000; //Time(in milliseconds) for call of "main" loop.

    uint32_t err_code = NRF_SUCCESS;
 /*   nrf_gpio_cfg_output(LED_RGB_RED);
    nrf_gpio_cfg_output(LED_RGB_GREEN);
    nrf_gpio_cfg_output(LED_RGB_BLUE);
*/

    //Toggle LED to show active tag






    ble_stack_init();
    /*
    uint32_t reset_reason=0;
    SEGGER_RTT_printf(0, "Starting main");
    err_code = sd_power_reset_reason_get(&reset_reason);
    SEGGER_RTT_printf(0, "Reset reason = 0x%08x\r\n", reset_reason);
    err_code = sd_power_reset_reason_clr(0xFFFFFFFF);

    sd_power_reset_reason_get(&reset_reason);
    SEGGER_RTT_printf(0, "Reset reason after clear= 0x%08x\r\n", reset_reason);

     */

    gap_params_init();
    advertising_init();

    scan_init();
    // Create application timers.
    APP_TIMER_INIT(APP_TIMER_PRESCALER, APP_TIMER_OP_QUEUE_SIZE, false);
     APP_ERROR_CHECK(err_code);
    create_timers();

    err_code = app_timer_start(m_main_timer_id, APP_TIMER_TICKS(time_ms, APP_TIMER_PRESCALER), NULL);
    APP_ERROR_CHECK(err_code);
    led_sb_init_params_t led_sb_init_param = LED_SB_INIT_DEFAULT_PARAMS(LEDS_MASK);
    led_sb_init_param.duty_cycle_max = 100;
    err_code = led_softblink_init(&led_sb_init_param);
       APP_ERROR_CHECK(err_code);

    advertising_start();
    scan_start();

    if( show_status_led == 1)
    {
    	switch(status_tag)
    	{
    	case STATUS_I:
    		led_status = LED_RGB_RED;
    		break;
    	default:
    		led_status = LED_RGB_GREEN;
    	}
    }
    err_code = led_softblink_start((1<<led_status));
    APP_ERROR_CHECK(err_code);

/*    nrf_gpio_pin_toggle(LED_RGB_RED);
    nrf_gpio_pin_toggle(LED_RGB_GREEN);
    nrf_gpio_pin_toggle(LED_RGB_BLUE);
*/



    for (;; )
    {
        power_manage();
    }
}
