//------------------------------------------------------------------------------
//-- BluNode v2
//------------------------------------------------------------------------------

#include "mbed.h"
#include "BLE.h"
#include "UARTService.h"

#include "nrf.h"
#include "nrf_temp.h"
#include "nrf_gpio.h"

#define NEED_CONSOLE_OUTPUT 1 /*  Set this if you need debug messages on the console;
* it will have an impact on code-size and power consumption. */

#if NEED_CONSOLE_OUTPUT
#define DEBUG(...) { printf(__VA_ARGS__); }
#else
#define DEBUG(...)
#endif

//------------------------------------------------------------------------------
//-- Atributtes
//------------------------------------------------------------------------------

UARTService *uartServicePtr;
BLE ble;




//DigitalOut activeLed(LED1, 1);
//DigitalOut connectLed(LED2, 0);

DigitalIn  button1(BUTTON1);            // used to trigger the time report
InterruptIn button1Press(BUTTON1);




//------------------------------------------------------------------------------
//-- Definitions - Parameters
//------------------------------------------------------------------------------

const static char     DEVICE_NAME[] = "BN_0302";


#define     BLUCORE_ADV_LENGHT          0x10    // 2 + 1 + 13
#define     BLUCORE_ADV_TYPE_01_LENGHT  0x10    // 2 + 1 + 13
#define     BLUCORE_ADV_PAYLOAD_OFFSET  0x03    // 3
#define     BLUCORE_ADV_PAYLOAD_LENGHT  0x0D    // 13

#define     BLUCORE_COMPANY_ID          0xC7AB
#define     BLUCORE_BLUNODE_TYPE_01     0x01













//------------------------------------------------------------------------------
//-- Definition - Structures
//------------------------------------------------------------------------------

//-- Tipo payload data para nodo tipo 01

union PayloadDataType01 {
    uint8_t raw[16];
    struct {
        uint16_t    CompanyId;
        uint8_t     Type;
        uint16_t    Counter;
        uint16_t    ADC1_Value;
        uint16_t    ADC2_Value;
        uint16_t    ADC3_Value;
        uint16_t    ADC4_Value;
        uint8_t     DataTail[2];
        uint8_t     SOC_Temperature;
    };
};




















//------------------------------------------------------------------------------
//-- Variables
//------------------------------------------------------------------------------


bool isConnected=false;

static int value0,value1,value2,value3; // stored voltage reading from ADC

char dataOu[30];

Ticker  t1; // Utilizado para esperar n-veces antes de iniciar un broadcast
Ticker  t2; // Utilizado para broadcast de mensaje

//------------------------------------------------------------------------------
//-- Agregados para v2
//------------------------------------------------------------------------------


//-- Utils

#define MSB_16(a) (((a) & 0xFF00) >> 8)
#define LSB_16(a) ((a) & 0x00FF)

//-- Variables


#define adc_channel_count 4

static int          adc_channels[adc_channel_count];
static uint16_t     adc_values[adc_channel_count];
static uint16_t     adc_log_values[adc_channel_count];


static uint8_t      soc_temp = 0;

DigitalOut led1(LED1);
DigitalOut led2(LED2);
DigitalOut led3(LED3);
DigitalOut led4(LED4);

//-- flags

//static bool reset_flag;

static bool adv_run_first_time_flag;

//static bool monitor_running_flag;
static bool monitor_stop_flag;

//static bool adv_running_flag;
static bool adv_stop_flag;

//static bool time_monitor_running_flag;
//static bool adv_run_running_flag;


// MAC ADDRESS
    
const uint8_t bluNodeMACaddress[] = {0x01,0x00,0xFE,0xCA,0xAB,0xc7};

uint8_t payload[BLUCORE_ADV_LENGHT];


// Variables de control y notificación

static uint16_t adv_count = 0;


//-- Parámetros de tiempo

static int time_monitor_count         = 0;
static int time_monitor_max_count     = 6;

static int adv_time_interval          = 100;           // 1000 ms = 1 sec
static int adv_time_periods           = 10;              // 4 times
static int adv_run_count              = 0;

static int app_time_monitor_interval  = 10; //5;             // 15 sec
static int app_adv_run_interval       = 1;              // 1 sec


//------------------------------------------------------------------------------
//-- Funciones mejoradas v2
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
//-- adv_start
//------------------------------------------------------------------------------

void adv_start()
{
    ble.startAdvertising();
}

//------------------------------------------------------------------------------



//------------------------------------------------------------------------------
//-- adv_stop
//------------------------------------------------------------------------------
//
// Detiene la emisión de advertising
//
//------------------------------------------------------------------------------

void adv_stop()
{
    ble.stopAdvertising();
}

//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
//-- adv_clear
//------------------------------------------------------------------------------
//
// borra el contenido del paquete de datos
//
//------------------------------------------------------------------------------

void adv_clear()
{
    ble.clearAdvertisingPayload();
}

//------------------------------------------------------------------------------




//------------------------------------------------------------------------------
//-- adv_stop_and_clear
//------------------------------------------------------------------------------
//
// Detiene la emisión de advertising
// borra el contenido del paquete de datos
//
//------------------------------------------------------------------------------

void adv_stop_and_clear()
{
    ble.stopAdvertising();
    ble.clearAdvertisingPayload();
}

//------------------------------------------------------------------------------


//------------------------------------------------------------------------------
//-- adv_payload_set4adv
//------------------------------------------------------------------------------
// set del payload para envío de data después de la lectura de ADC
//------------------------------------------------------------------------------

void adv_payload_set4adv(void)
{
    //-- Company Id
    payload[0]      = MSB_16(BLUCORE_COMPANY_ID);
    payload[1]      = LSB_16(BLUCORE_COMPANY_ID);
    //-- Node Type
    payload[2]      = BLUCORE_BLUNODE_TYPE_01;
    //-- Counter
    payload[3]      = MSB_16(adv_count);
    payload[4]      = LSB_16(adv_count);
    // ADC Channel 1
    payload[5]      = MSB_16(adc_values[0]);
    payload[6]      = LSB_16(adc_values[0]);
    // ADC Channel 2
    payload[7]      = MSB_16(adc_values[1]);
    payload[8]      = LSB_16(adc_values[1]);
    // ADC Channel 3
    payload[9]      = MSB_16(adc_values[2]),
    payload[10]     = LSB_16(adc_values[2]);
    // ADC Channel 4
    payload[11]     = MSB_16(adc_values[3]),
    payload[12]     = LSB_16(adc_values[3]);
    // 2 bytes spare
    payload[13]     = 0x01;
    payload[14]     = 0x02;
    // SOC Temperature
    payload[15]     = soc_temp; //((uint8_t) (( soc_temp & 0x000000FF) & 0xFF ));
    

    
    adv_clear();
    
    ble.accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED);
    ble.setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
    ble.gap().setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
    ble.accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LOCAL_NAME, (uint8_t *)DEVICE_NAME, sizeof(DEVICE_NAME));

    ble.accumulateAdvertisingPayload(GapAdvertisingData::MANUFACTURER_SPECIFIC_DATA, payload, sizeof(payload));

    // Set advertising interval
    
    ble.setAdvertisingInterval(adv_time_interval);
    
    //ble.startAdvertising();
}

//------------------------------------------------------------------------------
//-- Inicialización de canales ADC
//------------------------------------------------------------------------------
//  - Configure Resolution to 10 bits
//  - Configure ADC input selection and prescalar settings
//  - Internal reference voltage of 1.2V. Onethird prescaling -> 3.6V
//  - ADC reference selection to internal 1.2V VBG
//  - Select Pin to be used as ADC input pin -> Disable analog pins
//  - Select external reference pin -> No external reference (we use internal reference of 1.2V)
//------------------------------------------------------------------------------

void adc_channels_init(void)
{

    NRF_ADC->ENABLE = ADC_ENABLE_ENABLE_Enabled;
    NRF_ADC->CONFIG = (ADC_CONFIG_RES_10bit << ADC_CONFIG_RES_Pos) |
                      (ADC_CONFIG_INPSEL_AnalogInputOneThirdPrescaling << ADC_CONFIG_INPSEL_Pos) |
                      (ADC_CONFIG_REFSEL_VBG << ADC_CONFIG_REFSEL_Pos) |
                      (ADC_CONFIG_PSEL_AnalogInput0 << ADC_CONFIG_PSEL_Pos) |
                      (ADC_CONFIG_EXTREFSEL_None << ADC_CONFIG_EXTREFSEL_Pos);

}

//------------------------------------------------------------------------------


//------------------------------------------------------------------------------
//-- lectura de valor canal ADC (1...8)
//------------------------------------------------------------------------------

uint16_t adc_channel_read(int adcChannel)
{
    uint16_t result = 0;

    if (adcChannel > 0 && adcChannel < 9) {

        NRF_ADC->CONFIG   &= ~ADC_CONFIG_PSEL_Msk;

        switch (adcChannel) {
            case 1:
                NRF_ADC->CONFIG   |= ADC_CONFIG_PSEL_AnalogInput0 << ADC_CONFIG_PSEL_Pos;
                break;
            case 2:
                NRF_ADC->CONFIG   |= ADC_CONFIG_PSEL_AnalogInput1 << ADC_CONFIG_PSEL_Pos;
                break;
            case 3:
                NRF_ADC->CONFIG   |= ADC_CONFIG_PSEL_AnalogInput2 << ADC_CONFIG_PSEL_Pos;
                break;
            case 4:
                NRF_ADC->CONFIG   |= ADC_CONFIG_PSEL_AnalogInput3 << ADC_CONFIG_PSEL_Pos;
                break;
            case 5:
                NRF_ADC->CONFIG   |= ADC_CONFIG_PSEL_AnalogInput4 << ADC_CONFIG_PSEL_Pos;
                break;
            case 6:
                NRF_ADC->CONFIG   |= ADC_CONFIG_PSEL_AnalogInput5 << ADC_CONFIG_PSEL_Pos;
                break;
            case 7:
                NRF_ADC->CONFIG   |= ADC_CONFIG_PSEL_AnalogInput6 << ADC_CONFIG_PSEL_Pos;
                break;
            case 8:
                NRF_ADC->CONFIG   |= ADC_CONFIG_PSEL_AnalogInput7 << ADC_CONFIG_PSEL_Pos;
                break;

            default:
                break;
        }

        NRF_ADC->TASKS_START = 1;

        while (((NRF_ADC->BUSY & ADC_BUSY_BUSY_Msk) >> ADC_BUSY_BUSY_Pos) == ADC_BUSY_BUSY_Busy) { }

        result = (uint16_t) ( NRF_ADC -> RESULT & 0x0000FFFF );

    }

    return (result);

}

//------------------------------------------------------------------------------


//------------------------------------------------------------------------------
//-- Lectura de temperatura interna del CPU
//------------------------------------------------------------------------------

uint8_t soc_temperature_read(void)
{
    uint8_t result = 0;
    static uint32_t temp;
    NRF_TEMP->TASKS_START = 1;
    while (NRF_TEMP->EVENTS_DATARDY == 0) {}
    NRF_TEMP->EVENTS_DATARDY = 0;
    temp = NRF_TEMP->TEMP ;
    result = (uint8_t) (( temp & 0x000000FF) & 0xFF );
    NRF_TEMP->TASKS_STOP = 1;

    return (result);

}

//------------------------------------------------------------------------------


//------------------------------------------------------------------------------
//-- Lectura de todos los canales ADC
//------------------------------------------------------------------------------

void adc_channels_read(void)
{
    if(!isConnected) {
        for (int i = 0; i < adc_channel_count; i++) {
            adc_values[i] = adc_channel_read(adc_channels[i]);
        }

        soc_temp = soc_temperature_read();
    }
}

//------------------------------------------------------------------------------



/* Funciones ejecutadas en base de tiempo */

//------------------------------------------------------------------------------
//-- Time Monitor Function
//------------------------------------------------------------------------------
// Se ejecuta cada lapso de tiempo t1
//------------------------------------------------------------------------------

void time_monitor()
{
    time_monitor_count++;
 
    led1 = 0;
    wait_ms(100);
    led1 = 1;
    
    if (time_monitor_count > time_monitor_max_count) 
    {
        // lectura del valor de los canales ADC
        adc_channels_read();
        // en esta versión se actualiza la data de "payload" del advertising
        // to-do: implementar lógica de comparación con valores anteriores
        //adv_stop_and_clear();                 // Detiene y borra "payload"
        adv_count++;                // incrementa el contador de paquetes de datos
        adv_payload_set4adv();
        monitor_stop_flag = true;   // flag de detención del proceso de time_monitor
        adv_run_first_time_flag = true;
        //adv_start();                // Inicia advertising con los datos cargados
    }      
}

//------------------------------------------------------------------------------


//------------------------------------------------------------------------------
//-- Advertising Run Function
//------------------------------------------------------------------------------
// Se ejecuta cada lapso de tiempo t2
//------------------------------------------------------------------------------

void adv_run()
{
    led2 = 0;
    wait_ms(100);
    led2 = 1;
    
    if (adv_run_first_time_flag)
    {
        // reset de la variable de control
        
        adv_run_first_time_flag = false;
        
        // inicia el advertising 
        // pre-req: Se ha cargado el payload antes de activar la función
        
        adv_start();
        

        
    }
    /**/
    
    adv_run_count++;
    
    if (adv_run_count > adv_time_periods)
    {
        // detención del proceso de envio de advertising
        
        //adv_stop_and_clear();
        adv_stop();                                                             // detención de advertising
        adv_stop_flag = true;                                                   // indicación de detención del proceso de advertising
    }    
}



























































//------------------------------------------------------------------------------
//-- Connection Call Back
//------------------------------------------------------------------------------

void connectionCallback(const Gap::ConnectionCallbackParams_t *params)
{
    //DEBUG("Connected \n");

    //connectLed=1;
    isConnected=true;

}

//------------------------------------------------------------------------------


//------------------------------------------------------------------------------
//-- Disconnection Call Back
//------------------------------------------------------------------------------

void disconnectionCallback(const Gap::DisconnectionCallbackParams_t *params)
{
    //ble.startAdvertising();
    isConnected=false;
}

//------------------------------------------------------------------------------


//------------------------------------------------------------------------------
//-- onDataWritten
//------------------------------------------------------------------------------

void onDataWritten(const GattWriteCallbackParams *params)
{
    /*
    if ((uartServicePtr != NULL) && (params->handle == uartServicePtr->getTXCharacteristicHandle())) {
        uint16_t bytesRead = params->len;
        DEBUG("received %u bytes\n\r", bytesRead);
        if (strncmp("param1=",(const char *)params->data,7)==0) {
            sscanf((const char *)params->data,"param1=%i",&advActivePeriod);
            sprintf(dataOu,"Adver=%i sec\n",advActivePeriod);
            DEBUG("%s",dataOu);
            ble.updateCharacteristicValue(uartServicePtr->getRXCharacteristicHandle(),(const uint8_t *)dataOu , 20);

        } else if (strncmp("param2=",(const char *)params->data,7)==0) {
            sscanf((const char *)params->data,"param2=%i",&sleepActivePeriod);
            sprintf(dataOu,"Sleep=%i sec\n",sleepActivePeriod);
            DEBUG("%s",dataOu);
            ble.updateCharacteristicValue(uartServicePtr->getRXCharacteristicHandle(),(const uint8_t *)dataOu, 20);

        } else if (strncmp("scan",(const char *)params->data,4)==0) {
            DEBUG("Scanning\n");
            //my_analogin_read_u16A0();
            value0 = (int) (adc_channel_read(1) & 0x00FFFF);
            sprintf(dataOu,"A0=%i \n",value0);
            DEBUG("%s",dataOu);
            ble.updateCharacteristicValue(uartServicePtr->getRXCharacteristicHandle(),(const uint8_t *)dataOu, 20);
            wait(5);
            //my_analogin_read_u16A1();
            value1 = (int) (adc_channel_read(2) & 0x00FFFF);
            sprintf(dataOu,"A1=%i \n",value1);
            DEBUG("%s",dataOu);
            ble.updateCharacteristicValue(uartServicePtr->getRXCharacteristicHandle(),(const uint8_t *)dataOu, 20);
            wait(5);
            //my_analogin_read_u16A2();
            value2 = (int) (adc_channel_read(3) & 0x00FFFF);
            sprintf(dataOu,"A2=%i \n",value2);
            DEBUG("%s",dataOu);
            ble.updateCharacteristicValue(uartServicePtr->getRXCharacteristicHandle(),(const uint8_t *)dataOu, 20);
            wait(5);
            //my_analogin_read_u16A3();
            value3 = (int) (adc_channel_read(4) & 0x00FFFF);
            sprintf(dataOu,"A3=%i \n",value3);
            DEBUG("%s",dataOu);
            ble.updateCharacteristicValue(uartServicePtr->getRXCharacteristicHandle(),(const uint8_t *)dataOu, 20);


        }
    }
    */
}

//------------------------------------------------------------------------------


















//------------------------------------------------------------------------------
//--
//------------------------------------------------------------------------------

void advActiveFunc (void)
{
    //sleepCall.detach();
    //t1.attach(time_monitor, 10);

}

//------------------------------------------------------------------------------












void SleepFunc(void)
{
    //t.attach(my_analogin_read,1); //repeatedly read analog every second.
    //t.attach(adc_channels_read, 1);
    //t.attach(time_monitor, 10);
    //DEBUG("\nWAKE UP!!");
    //activeLed=1;
}







































//------------------------------------------------------------------------------
//-- advertising init
//------------------------------------------------------------------------------

void adv_init()
{

    /**/
    
    ble.onDisconnection(disconnectionCallback);
    ble.onDataWritten(onDataWritten);
    ble.onConnection(connectionCallback);

    //-- Set MAC address (like UUID)
    
    ble.setAddress(Gap::ADDR_TYPE_PUBLIC, bluNodeMACaddress);
 
    
}

//------------------------------------------------------------------------------







































































//------------------------------------------------------------------------------
//-- App Init
//------------------------------------------------------------------------------
//
// Inicialización de la aplicación
//
//------------------------------------------------------------------------------

void app_init()
{
    nrf_temp_init();
    ble.init();
    adv_init();
    adc_channels_init();
    // reset LED's
    led1 = 1; //0; // Inicia encendido
    led2 = 1;
    led3 = 1;
    led4 = 1;
    
    adv_count = 0;
    monitor_stop_flag = false;
    adv_stop_flag = false;
    
}

//------------------------------------------------------------------------------












//------------------------------------------------------------------------------
//-- Main: BluNode
//------------------------------------------------------------------------------

int main(void)
{
    //-- Inicialización de la aplicación
    
    app_init();
    
    // Inicia el conteo de tiempo de espera, hay que ajustar el tiempo de 
    // revisión de condiciones para esperar el tiempo definido antes del adv
    
    t1.attach(time_monitor, app_time_monitor_interval); 
    

    //UARTService uartService(ble);
    //uartServicePtr = &uartService;
    
    //button1Press.fall(&advActiveFunc);




    /* Infinite Loop*/

    while(true) {
        
        // espera por un evento del stack BLE (Temporizadores, botones, etc...)
        
        ble.waitForEvent();

        // Control de proceso time_monitor
        
        if (monitor_stop_flag)
        {
            // detiene ejecución del proceso "time_monitor"
            t1.detach();
            // reset de variables de control
            monitor_stop_flag = false;
            time_monitor_count = 0;
            // Solo para debug
            //led3 = !led3;
            // activa proceso de advertising
            t2.attach(adv_run, app_adv_run_interval);
        }
        
        // Control de proceso advertising
        
        if (adv_stop_flag)
        {
            // detiene ejecución del proceso "adv_run"
            t2.detach();                                                        // detiene el timer del proceso de advertising
            // reset de variables de control
            adv_stop_flag = false;
            adv_run_count = 0;  
            // solo para debug
            //led4 = !led4;
            // activa proceso de time_monitor
            t1.attach(time_monitor, app_time_monitor_interval);                 // inicia el timer del proceso de espera 
        }

    }
}

//------------------------------------------------------------------------------









