/* Copyright (c) 2014 Nordic Semiconductor. All Rights Reserved.
 *
 * The information contained herein is property of Nordic Semiconductor ASA.
 * Terms and conditions of usage are described in detail in NORDIC
 * SEMICONDUCTOR STANDARD SOFTWARE LICENSE AGREEMENT.
 *
 * Licensees are granted free, non-transferable use of the information. NO
 * WARRANTY of ANY KIND is provided. This heading must NOT be removed from
 * the file.
 *
 */

/** @file
*
* @defgroup ppi_example_main main.c
* @{
* @ingroup ppi_example
* @brief PPI Example Application main file.
*
* This file contains the source code for a sample application using PPI to communicate between timers.
*
*/
#include <stdbool.h>
#include <stdint.h>
#include <stdio.h>
#include "nrf.h"
#include "nrf_delay.h"
#include "app_error.h"
#include "boards.h"
#include "nrf_drv_ppi.h"
#include "nrf_drv_timer.h"
#include "nordic_common.h"
#include "nrf_drv_gpiote.h"
 #include "SEGGER_RTT.h"

#define NRF_LOG_MODULE_NAME "APP"
#include "nrf_log.h"
#include "nrf_log_ctrl.h"

#include "bsp.h"
#include "app_button.h"

#ifdef BSP_LED_0
    #define GPIO_OUTPUT_PIN_NUMBER BSP_LED_0  /**< Pin number for output. */
#endif
#ifndef GPIO_OUTPUT_PIN_NUMBER
    #error "Please indicate output pin"
#endif

int printf(const char *fmt,...);
uint8_t my_array[100];
uint32_t counter = 0;

const nrf_drv_timer_t timer0 = NRF_DRV_TIMER_INSTANCE(0);


static uint32_t *radio_register = (uint32_t *) 0x40001000;
nrf_ppi_channel_t ppi_channel1, ppi_channel2, ppi_channel3, ppi_channel4, ppi_channel5, ppi_channel6, ppi_channel7, ppi_channel8, ppi_channel9, ppi_channel10, ppi_channel11, ppi_channel11, ppi_channel12, ppi_channel13;



uint8_t packet[] = {0b01000000, 15, 0, 0xee, 0x03, 0x21, 0x35, 0x1c, 0xee, 0x07, 0x09,'R','a','g','h','a','v'};
uint32_t recv_time = 0;
uint32_t ack_time = 0;


void
write_uint32(uint8_t *dest, uint32_t src) {
    uint32_t low_mask = (1<<9) - 1;
    dest[0] = (src    ) & low_mask;
    dest[1] = (src>> 8) & low_mask;
    dest[2] = (src>>16) & low_mask;
    dest[3] = (src>>24) & low_mask;
}
uint32_t
read_uint32(uint8_t *bytes) {
    return (bytes[3]<<24) | (bytes[2]<<16) | (bytes[1]<<8) | (bytes[0]);
}


// Timer event handler. Not used since timer is used only for PPI.
void timer_event_handler(nrf_timer_event_t event_type, void * p_context){}

/** @brief Function for initializing the PPI peripheral.
*/
static void ppi_init(void)
{
    uint32_t err_code = NRF_SUCCESS;

    err_code = nrf_drv_ppi_init();
    APP_ERROR_CHECK(err_code);

    // Configure 1st available PPI channel to stop TIMER0 counter on TIMER1 COMPARE[0] match, which is every even number of seconds.
    err_code = nrf_drv_ppi_channel_alloc(&ppi_channel1);
    APP_ERROR_CHECK(err_code);
    err_code = nrf_drv_ppi_channel_assign(ppi_channel1,
                                          nrf_drv_timer_event_address_get(&timer0, NRF_TIMER_EVENT_COMPARE0),(uint32_t)&NRF_TIMER0->TASKS_CAPTURE[1]);
                                         
    APP_ERROR_CHECK(err_code);

    
   
    // Enable PPI channel
    err_code = nrf_drv_ppi_channel_enable(ppi_channel1);
    APP_ERROR_CHECK(err_code);
   }


/** @brief Function for Timer 0 initialization*/

static void timer0_init(void)
{
    nrf_drv_timer_config_t timer_cfg = NRF_DRV_TIMER_DEFAULT_CONFIG;
     timer_cfg.frequency = NRF_TIMER_FREQ_31250Hz;
     timer_cfg.mode = NRF_TIMER_MODE_COUNTER;
    ret_code_t err_code = nrf_drv_timer_init(&timer0, &timer_cfg, timer_event_handler);
    APP_ERROR_CHECK(err_code);
}





/**
 * @brief Function for application main entry.
 */





uint32_t low_mask(char n) {
  return (1<<(n+1)) - 1;
}
 
void set_address0(uint32_t address) {
  uint32_t base = address << 8;
  NRF_RADIO->BASE0 = base;
  uint32_t prefix = (address >> 24) & low_mask(8);
  NRF_RADIO->PREFIX0 = prefix;
}




void radio_init(){
    uint32_t err_code = NRF_SUCCESS;

    
    //Set frequency, advertising channel 37. 
    NRF_RADIO->FREQUENCY = 2UL;  // Frequency bin 2, 2402MHz

    //Set whitening to same as channel
    NRF_RADIO->DATAWHITEIV = 37;
    NRF_RADIO->TXADDRESS = 0; //Base0 + prefix0
    NRF_RADIO->RXADDRESSES = 1;

    //Set TXPOWER
    NRF_RADIO->TXPOWER = RADIO_TXPOWER_TXPOWER_0dBm;

     uint32_t override_val = NRF_FICR->OVERRIDEEN & FICR_OVERRIDEEN_BLE_1MBIT_Msk;

  if (override_val == FICR_OVERRIDEEN_BLE_1MBIT_Override) {
    NRF_RADIO->OVERRIDE0 = NRF_FICR->BLE_1MBIT[0];
    NRF_RADIO->OVERRIDE1 = NRF_FICR->BLE_1MBIT[1];
    NRF_RADIO->OVERRIDE2 = NRF_FICR->BLE_1MBIT[2];
    NRF_RADIO->OVERRIDE3 = NRF_FICR->BLE_1MBIT[3];
    NRF_RADIO->OVERRIDE4 = NRF_FICR->BLE_1MBIT[4];
  }

    NRF_RADIO->MODE = RADIO_MODE_MODE_Nrf_1Mbit;

    //Set CRC initial value
    NRF_RADIO->CRCINIT = 0x555555;
    //Set CRC size and address include
    NRF_RADIO->CRCCNF = 
            (3 << RADIO_CRCCNF_LEN_Pos)  |
            (true << RADIO_CRCCNF_SKIPADDR_Pos);

    NRF_RADIO->CRCPOLY = (1<<24) | (1<<10) | (1<<9) | (1<<6) | (1<<4) | (1<<3) | (1<<1) | 1;

    //Set size of length, s0 and s1
    NRF_RADIO->PCNF0 =
            (1 << RADIO_PCNF0_S0LEN_Pos) |
            (6 << RADIO_PCNF0_LFLEN_Pos) |
            (2 << RADIO_PCNF0_S1LEN_Pos);

    NRF_RADIO->PCNF1 = 
            (100 << RADIO_PCNF1_MAXLEN_Pos) |
            (0 << RADIO_PCNF1_STATLEN_Pos)  |
            (3 << RADIO_PCNF1_BALEN_Pos)    |
            (RADIO_PCNF1_ENDIAN_Little << RADIO_PCNF1_ENDIAN_Pos)   |
            (true << RADIO_PCNF1_WHITEEN_Pos);

    //Set Radio address
    set_address0(0x8E89BED6);


    NRF_RADIO->SHORTS = 
            (1 << RADIO_SHORTS_END_DISABLE_Pos);

}


void timer_init(){
    uint32_t err_code;
    //Configure timer and compare for timer0
    nrf_drv_timer_config_t timer_cfg = NRF_DRV_TIMER_DEFAULT_CONFIG;
    timer_cfg.bit_width = NRF_TIMER_BIT_WIDTH_24;
    timer_cfg.frequency = NRF_TIMER_FREQ_1MHz;

    err_code = nrf_drv_timer_init(&timer0, &timer_cfg, timer_event_handler);
    APP_ERROR_CHECK(err_code);
    nrf_drv_timer_extended_compare(&timer0, NRF_TIMER_CC_CHANNEL0, 5, NRF_TIMER_SHORT_COMPARE0_CLEAR_MASK, false);


    //Configure Timer SHORTS
    NRF_TIMER0->SHORTS = (TIMER_SHORTS_COMPARE0_STOP_Enabled << TIMER_SHORTS_COMPARE0_STOP_Pos);

    nrf_drv_timer_enable(&timer0);
   

}


void ppi_init_new(){



    ret_code_t err_code = nrf_drv_gpiote_init();
    APP_ERROR_CHECK(err_code);

    err_code = nrf_drv_ppi_init();
    APP_ERROR_CHECK(err_code);

    



    //Config event task
    nrf_drv_gpiote_out_config_t task_config4 = GPIOTE_CONFIG_OUT_TASK_TOGGLE(true);
    ret_code_t err_code2 = nrf_drv_gpiote_out_init(24, &task_config4);
    APP_ERROR_CHECK(err_code);

    nrf_drv_gpiote_out_config_t task_config = GPIOTE_CONFIG_OUT_TASK_TOGGLE(true);
    err_code2 = nrf_drv_gpiote_out_init(23, &task_config);
    APP_ERROR_CHECK(err_code);

    nrf_drv_gpiote_out_config_t task_config2 = GPIOTE_CONFIG_OUT_TASK_TOGGLE(true);
    err_code2 = nrf_drv_gpiote_out_init(22, &task_config2);
    APP_ERROR_CHECK(err_code);

    nrf_drv_gpiote_out_config_t task_config3 = GPIOTE_CONFIG_OUT_TASK_TOGGLE(true);
    err_code2 = nrf_drv_gpiote_out_init(21, &task_config3);
    APP_ERROR_CHECK(err_code);


    // Configure 1st available PPI channel to stop TIMER0 counter on TIMER1 COMPARE[0] match, which is every even number of seconds.
    
    //PPI Channels Used for debugging
    err_code = nrf_drv_ppi_channel_alloc(&ppi_channel1);
    APP_ERROR_CHECK(err_code);
    err_code = nrf_drv_ppi_channel_assign(ppi_channel1,
                                          (uint32_t) (&NRF_RADIO->EVENTS_READY),
                                          nrf_drv_gpiote_out_task_addr_get(21));
    APP_ERROR_CHECK(err_code);

    err_code = nrf_drv_ppi_channel_alloc(&ppi_channel6);
    APP_ERROR_CHECK(err_code);
    err_code = nrf_drv_ppi_channel_assign(ppi_channel6,
                                          (uint32_t) (&NRF_RADIO->EVENTS_ADDRESS),
                                          nrf_drv_gpiote_out_task_addr_get(22));
    APP_ERROR_CHECK(err_code);

    err_code = nrf_drv_ppi_channel_alloc(&ppi_channel4);
    APP_ERROR_CHECK(err_code);
    err_code = nrf_drv_ppi_channel_assign(ppi_channel4,
                                          (uint32_t) (&NRF_RADIO->EVENTS_PAYLOAD),
                                          nrf_drv_gpiote_out_task_addr_get(23));
    APP_ERROR_CHECK(err_code);

    err_code = nrf_drv_ppi_channel_alloc(&ppi_channel3);
    APP_ERROR_CHECK(err_code);
    err_code = nrf_drv_ppi_channel_assign(ppi_channel3,
                                          (uint32_t) (&NRF_RADIO->EVENTS_END),
                                          nrf_drv_gpiote_out_task_addr_get(24));
    APP_ERROR_CHECK(err_code);

    //Enable ppi
    err_code = nrf_drv_ppi_channel_enable(ppi_channel1);
    err_code = nrf_drv_ppi_channel_enable(ppi_channel6);
    err_code = nrf_drv_ppi_channel_enable(ppi_channel4);
    err_code = nrf_drv_ppi_channel_enable(ppi_channel3);
    APP_ERROR_CHECK(err_code);
  
    
    //nrf_drv_gpiote_out_task_enable(23);
    nrf_drv_gpiote_out_task_enable(21);
    nrf_drv_gpiote_out_task_enable(22);
    nrf_drv_gpiote_out_task_enable(23);
    nrf_drv_gpiote_out_task_enable(24);
}


void start_crystal_clock(){
    // Start 16 MHz crystal oscillator 
    NRF_CLOCK->EVENTS_HFCLKSTARTED = 0;
     * (int *) 0x40000000 = 1;


    // Wait for the external oscillator to start up 
    while (NRF_CLOCK->EVENTS_HFCLKSTARTED == 0)
    {
        // Do nothing.
    }
     /* Start low frequency crystal oscillator for app_timer(used by bsp)*/
    NRF_CLOCK->LFCLKSRC            = (CLOCK_LFCLKSRC_SRC_Xtal << CLOCK_LFCLKSRC_SRC_Pos);
    NRF_CLOCK->EVENTS_LFCLKSTARTED = 0;
    NRF_CLOCK->TASKS_LFCLKSTART    = 1;

    while (NRF_CLOCK->EVENTS_LFCLKSTARTED == 0)
    {
        // Do nothing.
    }

}



void send_packet(char* send_packet){
    //NRF_RADIO->PACKETPTR = (uint32_t) adv_packet;

    //Set packet pointer
    NRF_RADIO->PACKETPTR = (uint32_t) send_packet;
  
    NRF_RADIO->EVENTS_DISABLED = 0U;
    NRF_RADIO->EVENTS_READY = 0U;
    NRF_RADIO->TASKS_TXEN   = 1U;


    while (NRF_RADIO->EVENTS_DISABLED == 0U)
    {
        // wait
    }

}


void start_receiver(char* receive_packet){
    //Set packet pointer
    NRF_RADIO->PACKETPTR = (uint32_t) receive_packet;

    NRF_RADIO->EVENTS_READY = 0U;
    // Enable radio and wait for ready
    NRF_RADIO->TASKS_RXEN = 1U;
    while (NRF_RADIO->EVENTS_READY == 0U)
    {
        // wait
    }

    NRF_RADIO->EVENTS_END = 0U;
    // Start listening and wait for address received event
    NRF_RADIO->TASKS_START = 1U;


    // Wait for end of packet or buttons state changed
    while (NRF_RADIO->EVENTS_END == 0U)
    {
        // wait
    }
    NRF_TIMER0->TASKS_CAPTURE[1]=1;
    recv_time = NRF_TIMER0->CC[1];

    ack_time = read_uint32(&receive_packet[18]);
}

void send_scan_res(char* scan_res_packet){
    

    NRF_RADIO->EVENTS_DISABLED = 0U;
    NRF_RADIO->EVENTS_READY = 0U;
    NRF_RADIO->TASKS_TXEN   = 1U;
    while (NRF_RADIO->EVENTS_READY == 0U)
    {
        // wait
    }

    //NRF_RADIO->TASKS_START = 1U;

    while (NRF_RADIO->EVENTS_DISABLED == 0U)
    {
        // wait
    }
}


#define BUTTON_DETECTION_DELAY          10

static volatile uint16_t button_tick = 0;
static volatile uint16_t button_min_hand = 0;
static volatile bool button_pressed = false;

static void button_handler(uint8_t button, uint8_t action)
{

    switch(action) {
    case APP_BUTTON_PUSH:
        button_pressed = true;
        break;
    case APP_BUTTON_RELEASE:
        button_pressed = false;
        //SEGGER_RTT_printf(0, "button_ticks are: %d, button_min_hand is %d\n", button_tick, button_min_hand);
        button_tick = 0;
        button_min_hand = 0;
        break;
    }
}


void buttons_init(void)
{
    uint32_t err_code = 0;
    static app_button_cfg_t buttons[] = {
        {1, APP_BUTTON_ACTIVE_LOW, NRF_GPIO_PIN_PULLUP, button_handler}
    };
    err_code = app_button_init((app_button_cfg_t *)buttons,
                               sizeof(buttons) / sizeof(buttons[0]),
                               BUTTON_DETECTION_DELAY);
    APP_ERROR_CHECK(err_code);
    err_code = app_button_enable();
    APP_ERROR_CHECK(err_code);

}



void button_tick_increment(void)
{
    if (button_pressed == true) {
        if (button_tick >= 65534) {
            button_min_hand++;
            button_tick = 0;
        }
        button_tick++;
    }
}

void RADIO_IRQHandler(void) {
    NRF_RADIO->EVENTS_READY = 0;

    uint8_t *packet = (uint8_t *) NRF_RADIO->PACKETPTR;

    NRF_TIMER0->TASKS_CAPTURE[0]=1;
    uint32_t transmit_time = NRF_TIMER0->CC[0];
    NRF_GPIO->OUT ^= (1 << 21); 
    write_uint32(&packet[10], ack_time);
    write_uint32(&packet[14], recv_time);
    write_uint32(&packet[18], transmit_time);
    NRF_RADIO->TASKS_START = 1;
}

int main(void)

{   NVIC_ClearPendingIRQ(RADIO_IRQn);
    NVIC_EnableIRQ(RADIO_IRQn);

    printf("Hello\n");

/*
                           //0b00000010      //Device address: 0xee, 0x1c, 0x35, 0x21, 0x03, 0xf1, sent LSByte first
    uint8_t adv_packet[100] = {0b01000000, 15, 0, 0xee, 0x03, 0x21, 0x35, 0x1c, 0xee, 8, 0x09, 'N','i','c','o','l','a','i'};
    uint8_t recieve_packet[100] = {0};
    uint8_t scan_res_packet[100] = {0b01000100, 16, 0, 0xf1, 0x03, 0x21, 0x35, 0x1c, 0xee, 9, 0x09, 'R','e','s','p','o','n','s', 'e'};
*/
  
    //power_manage();

    int  button_tick = 0;
    uint32_t err_code;
     //app_button_is_pushed(0);
     ppi_init_new();
    radio_init();
    start_crystal_clock();
    timer_init();
    NRF_LOG_PROCESS();
    NRF_RADIO->INTENSET = RADIO_INTENSET_READY_Msk;
    NRF_RADIO->INTENSET = RADIO_INTENSET_ADDRESS_Msk;
    NRF_RADIO->INTENSET = RADIO_INTENSET_PAYLOAD_Msk;

     NRF_GPIO->DIRSET = (1 << 21) | (1 << 22) | (1 << 23) | (1 << 24);
    NRF_GPIO->OUTCLR = (1 << 21) | (1 << 22) | (1 << 23) | (1 << 24);
    int tick = 0;

    uint8_t v_nr_1 = 0;
    uint8_t v_nr_2 = 0;

    while(1){
        // Transmit
        NRF_GPIO->OUTSET = (1 << 24);

        send_packet(packet);
        
        NRF_GPIO->OUTCLR = (1 << 24);
        
        // Receive
        start_receiver(my_array);
        // TODO look in my_array, extract the two timestamps, print them out
    } 
      

}