/*******************************************************************************
 TEST medi50  Dec 23
 ******************************************************************************/
#include "sdk_common.h"
			
#include <stdbool.h>
#include <stdint.h>
#include <stdio.h>
#include <string.h>
#include "nrf.h"
#include "boards.h"
#include "app_error.h"
#include "nrf_drv_saadc.h"
#include "nrfx_gpiote.h"
#include "app_timer.h"
#include "nrf_drv_timer.h"
#include "nrf_delay.h"
#include "nrf_drv_ppi.h"
#include "ada2200_spi.h"
#include "ble_nus.h"
#include "fstorage.h"
#include "measurements.h"
#include "meas_pd_voltage_custom.h"
#include "mcp4725_i2c.h"
#include "ad5272_i2c.h"
#include "main_timer.h"
#include "battery_saadc.h"
#include "main.h"
#include "app_raw_main.h"

#define CUSTOM_REF_VOLTAGE_IN_MILLIVOLTS   	600.0f                                     /**< Reference voltage (in milli volts) used by ADC while doing conversion. */
#define CUSTOM_PRE_SCALING_COMPENSATION    	6.0f                                       /**< The ADC is configured to use VDD with 1/3 prescaling as input. And hence the result of conversion is to be multiplied by 3 to get the actual value of the battery voltage.*/
#define CUSTOM_ADC_RES_10BITS             	1024.0f                                    /**< Maximum digital value for 10-bit ADC conversion. */
#define st_c_max																20
/**@brief Macro to convert the result of ADC conversion in millivolts.
 *
 * @param[in]  ADC_VALUE   ADC result.
 *
 * @retval     Result converted to millivolts.
 */
#define CUSTOM_VOUT_IN_MILLI_VOLTS(ADC_VALUE)\
				(((((ADC_VALUE) * CUSTOM_REF_VOLTAGE_IN_MILLIVOLTS) / CUSTOM_ADC_RES_10BITS) * CUSTOM_PRE_SCALING_COMPENSATION)*2)


uint8_t custom_samples_in_buffer = 8;

uint32_t	s_cnt;
uint8_t LED_list_custom[5];
uint8_t PD_list_custom[4];

uint8_t led_custom_list_a[5]	= {2,3,4,5,6};
uint8_t pd_custom_list_a[4]	= {5,7,8,10};

uint8_t led_custom_list_b[5]	= {20,21,22,23,24};
uint8_t pd_custom_list_b[4]	= {16,14,13,11};

int32_t c_max;
extern volatile bool processing;
extern volatile bool data_tx_in_progress;
extern volatile bool ble_connection_st;


uint32_t c_cnt;
static int8_t 	pd_no 		= -1;
static int8_t 	led_no 		= -1;
static int8_t	buf_no		= 0;
//static int8_t	c_max		= 5;
static int32_t	t_ms		= 0;
double	custom_cycle_buff[36][CUSTOM_CYCLE_CNT] = 
				{
					{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, //0
					{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, //1
					{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, //2
					{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, //3
					{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, //4
					{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, //5
					{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, //6
					{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, //7
					{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, //8
					{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, //9
					{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, //10
					{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, //11
					
					{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, //12
					{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, //13
					{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, //14
					{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, //15
					{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, //16
					{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, //17
					{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, //18
					{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, //19
					{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, //20
					{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, //21
					{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, //22
					{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, //23
					
					{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, //24
					{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, //25
					{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, //26
					{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, //27
					{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, //28
					{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, //29
					{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, //30
					{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, //31
					{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, //32
					{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, //33
					{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, //34
					{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, //35
				};	
double	custom_cycle_send_buff[CUSTOM_LED_NO][CUSTOM_PD_NO] = 
				{
					{0, 0, 0, 0},
					{0, 0, 0, 0},
					{0, 0, 0, 0},
					{0, 0, 0, 0},
					{0, 0, 0, 0}
				};


#define SAMPLES_IN_BUFFER	4095+32
static nrf_saadc_value_t pd_custom_adc_buf[2][SAMPLES_IN_BUFFER];

bool pd_adc_custom_a_start	= false;
bool pd_adc_custom_b_start	= false;
bool pd_adc_custom_c_start	= false;
bool pd_adc_custom_d_start	= false;
bool pd_adc_custom_end		= false;
bool pd_adc_custom_start = false;

bool custom_testing = false;

//
bool custom_add_data;
extern bool motion_raw_data_enabled;
extern char	ble_tx_buffer[BLE_NUS_MAX_DATA_LEN];
//add imu



char custom_tx_buffer[BLE_NUS_MAX_DATA_LEN];



char custom_tx_d_buffer_0[st_c_max][BLE_NUS_MAX_DATA_LEN];
char custom_tx_d_buffer_1[st_c_max][BLE_NUS_MAX_DATA_LEN];
char custom_tx_d_buffer_2[st_c_max][BLE_NUS_MAX_DATA_LEN];
char custom_tx_d_buffer_3[st_c_max][BLE_NUS_MAX_DATA_LEN];
char custom_tx_d_buffer_4[st_c_max][BLE_NUS_MAX_DATA_LEN];

char custom_tx_d_buffer_5[st_c_max][BLE_NUS_MAX_DATA_LEN];
char custom_tx_d_buffer_6[st_c_max][BLE_NUS_MAX_DATA_LEN];
char custom_tx_d_buffer_7[st_c_max][BLE_NUS_MAX_DATA_LEN];
char custom_tx_d_buffer_8[st_c_max][BLE_NUS_MAX_DATA_LEN];
char custom_tx_d_buffer_9[st_c_max][BLE_NUS_MAX_DATA_LEN];



extern which_cmd_t cmd_type_t;

APP_TIMER_DEF(m_custom_check_loop_timer_id);
APP_TIMER_DEF(m_custom_send_loop_timer_id);
#if FEATURE_DELAY
#define CUSTOM_SEND_LOOP_INTERVAL 500
#else
#define CUSTOM_SEND_LOOP_INTERVAL 100
#endif
#define CUSTOM_CHECK_LOOP_INTERVAL 1



#if FEATURE_DEBUG_REPEAT_TEST 
extern uint32_t rpt_cnt;
#endif

#if FEATURE_PRINTF
APP_TIMER_DEF(m_custom_loop_timer_id);
#define CUSTOM_LOOP_INTERVAL              1

void custom_timer_loop(void * p_context)
{
	UNUSED_PARAMETER(p_context);

    ret_code_t err_code = nrf_drv_saadc_sample();
    APP_ERROR_CHECK(err_code);
}

void custom_timer_start(void)
{
	APP_ERROR_CHECK(app_timer_start(m_custom_loop_timer_id, APP_TIMER_TICKS(CUSTOM_LOOP_INTERVAL), NULL));	
}

void custom_timer_stop(void)
{
	APP_ERROR_CHECK(app_timer_stop(m_custom_loop_timer_id));	
}

void custom_timer_init(void)
{
    APP_ERROR_CHECK(app_timer_create(&m_custom_loop_timer_id, APP_TIMER_MODE_REPEATED, custom_timer_loop));	
}
#else
static nrf_ppi_channel_t	m_ppi_channel;
#endif


#if !FEATURE_PRINTF
void custom_ppi_init(void)
{
    ret_code_t err_code;

    err_code = nrf_drv_ppi_init();
    APP_ERROR_CHECK(err_code);

    uint32_t gpiote_event_addr = nrf_drv_gpiote_in_event_addr_get(ADA2200_SYNCO_PIN);	
    uint32_t saadc_sample_task_addr   = nrf_drv_saadc_sample_task_get();

    /* setup ppi channel so that timer compare event is triggering sample task in SAADC */
    err_code = nrf_drv_ppi_channel_alloc(&m_ppi_channel);
    APP_ERROR_CHECK(err_code);

    err_code = nrf_drv_ppi_channel_assign(m_ppi_channel,
                                          gpiote_event_addr,
                                          saadc_sample_task_addr);
    APP_ERROR_CHECK(err_code);
}


void custom_ppi_uninit(void)
{
    ret_code_t err_code;

	err_code = nrf_drv_ppi_uninit();
    APP_ERROR_CHECK(err_code);
}


void custom_sampling_event_enable(void)
{
    ret_code_t err_code = nrf_drv_ppi_channel_enable(m_ppi_channel);
    APP_ERROR_CHECK(err_code);
}


void custom_sampling_event_disable(void)
{
	ret_code_t err_code = nrf_drv_ppi_channel_disable(m_ppi_channel);
	APP_ERROR_CHECK(err_code);
}
#endif


/**@brief Function for handling the ADC interrupt.
 *
 * @details  This function will fetch the conversion result from the ADC, convert the value into
 *           percentage and send it to peer.
 */
static void custom_voltage_handler(nrf_drv_saadc_evt_t const * p_event)	/* PD Voltage reading */
{
	ret_code_t err_code;
	double sum = 0.0f;

	uint32_t custom_clk_delay = m_config.pd_delay_us/16;
		if(ble_connection_st == 0) {
								custom_adc_end();
								custom_send_timer_stop();
								printf("Custom ADC STOP 1\r\n");
						}
		
			else{			
    if (p_event->type == NRF_DRV_SAADC_EVT_DONE)
    {
		err_code = nrf_drv_saadc_buffer_convert(p_event->data.done.p_buffer, custom_samples_in_buffer + custom_clk_delay);
		APP_ERROR_CHECK(err_code);
		
		if(led_no == -1) {
			led_no = 0;
			pd_no = 0;

			led_on(LED_list_custom[led_no]);
			led_pd_matching_value_set(LED_list_custom[led_no]); /* MCP4725 DAC setting and PD on */

		} else {

			for(uint16_t i = custom_clk_delay; i < custom_clk_delay + custom_samples_in_buffer; i++){
				custom_cycle_buff[buf_no][i-custom_clk_delay] = (CUSTOM_VOUT_IN_MILLI_VOLTS(p_event->data.done.p_buffer[i])) * -1;
			}
#if FEATURE_PRINTF
			printf("-----------------Read ADC // led_no = %d(%d), pd_no = %d(%d), buf_no = %d\r\n\r\n", led_no, LED_list_custom[led_no], pd_no,  PD_list_custom[pd_no], buf_no);
#endif

			buf_no++;

			if(pd_no < CUSTOM_PD_NO - 1) {
				pd_no++;
				led_pd_matching_value_set(LED_list_custom[led_no]); /* MCP4725 DAC setting and PD on */
			
			}else if(pd_no >= CUSTOM_PD_NO - 1) {
				pd_no = 0;

#if FEATURE_PRINTF
				printf("\r\n\r\n");
#endif

				if(led_no < CUSTOM_LED_NO - 1) {
					led_no++;
					led_on(LED_list_custom[led_no]);
					led_pd_matching_value_set(LED_list_custom[led_no]); /* MCP4725 DAC setting and PD on */

				} else if(led_no >= CUSTOM_LED_NO - 1) {
					pd_no = -1;
					led_no = -1;
#if FEATURE_PRINTF
					printf("\r\nEnded\r\n");
#endif
					custom_adc_end();

#if FEATURE_DETAIL_VALUE_CUSTOM
					sum = 0;
					printf("\r\nCustom_============custom_cycle_buff =\r\n");
					for(uint16_t i = 0; i < 36; i++){
						for(uint16_t j = 0; j < custom_samples_in_buffer; j++){
							printf("%lf\r\n", custom_cycle_buff[i][j]);
						}
						for(uint16_t k = 0; k < custom_samples_in_buffer; k++){
							sum += custom_cycle_buff[i][k];
						}
						printf("\t%lf\r\n", sum);
						sum = 0;
					}
					printf("\r\n");
#endif

					///////////////////////////////////////////////////////////////////////////////////////////
					uint8_t k =0;
					sum = 0;
					for(uint16_t i = 0; i < 20; i++){
						for(uint16_t j = 0; j < custom_samples_in_buffer; j++){
							sum += custom_cycle_buff[i][j];
						}
						
						custom_cycle_send_buff[i/4][k++] = sum;
						sum = 0;

						if(k >= 4) k = 0;
					}
					///////////////////////////////////////////////////////////////////////////////////////////

					/* Send data */
					/*********************/
					if(cmd_type_t == CMD_UART) {
						printf("S%dTi%d,\t%lf,\t%lf,\t%lf,\t%lf,\r\n",c_cnt, LED_list_custom[0], custom_cycle_send_buff[0][0], custom_cycle_send_buff[0][1], custom_cycle_send_buff[0][2], custom_cycle_send_buff[0][3]);//, custom_cycle_send_buff[0][4], custom_cycle_send_buff[0][5]);
						printf("Ti%d,\t%lf,\t%lf,\t%lf,\t%lf,\r\n", LED_list_custom[1], custom_cycle_send_buff[1][0], custom_cycle_send_buff[1][1], custom_cycle_send_buff[1][2], custom_cycle_send_buff[1][3]);//, custom_cycle_send_buff[1][4], custom_cycle_send_buff[1][5]);
						printf("Ti%d,\t%lf,\t%lf,\t%lf,\t%lf,\r\n", LED_list_custom[2], custom_cycle_send_buff[2][0], custom_cycle_send_buff[2][1], custom_cycle_send_buff[2][2], custom_cycle_send_buff[2][3]);//, custom_cycle_send_buff[2][4], custom_cycle_send_buff[2][5]);
						printf("Ti%d,\t%lf,\t%lf,\t%lf,\t%lf,\r\n", LED_list_custom[3], custom_cycle_send_buff[3][0], custom_cycle_send_buff[3][1], custom_cycle_send_buff[3][2], custom_cycle_send_buff[3][3]);//, custom_cycle_send_buff[3][4], custom_cycle_send_buff[3][5]);
						printf("Ti%d,\t%lf,\t%lf,\t%lf,\t%lf,\r\n", LED_list_custom[4], custom_cycle_send_buff[4][0], custom_cycle_send_buff[4][1], custom_cycle_send_buff[4][2], custom_cycle_send_buff[4][3]);//, custom_cycle_send_buff[4][4], custom_cycle_send_buff[4][5]);
						printf("\r\n");
					} else if(cmd_type_t == CMD_BLE ) {
						if(pd_adc_custom_a_start == true ) {
							sprintf(custom_tx_d_buffer_0[c_cnt], "M%d,Ti%d,\t%lf,\t%lf,\t%lf,\t%lf,\r\n",c_cnt, LED_list_custom[0], custom_cycle_send_buff[0][0], custom_cycle_send_buff[0][1], custom_cycle_send_buff[0][2], custom_cycle_send_buff[0][3]);
							sprintf(custom_tx_d_buffer_1[c_cnt], "M%d,Ti%d,\t%lf,\t%lf,\t%lf,\t%lf,\r\n",c_cnt, LED_list_custom[1], custom_cycle_send_buff[1][0], custom_cycle_send_buff[1][1], custom_cycle_send_buff[1][2], custom_cycle_send_buff[1][3]);
							sprintf(custom_tx_d_buffer_2[c_cnt], "M%d,Ti%d,\t%lf,\t%lf,\t%lf,\t%lf,\r\n",c_cnt, LED_list_custom[2], custom_cycle_send_buff[2][0], custom_cycle_send_buff[2][1], custom_cycle_send_buff[2][2], custom_cycle_send_buff[2][3]);
							sprintf(custom_tx_d_buffer_3[c_cnt], "M%d,Ti%d,\t%lf,\t%lf,\t%lf,\t%lf,\r\n",c_cnt, LED_list_custom[3], custom_cycle_send_buff[3][0], custom_cycle_send_buff[3][1], custom_cycle_send_buff[3][2], custom_cycle_send_buff[3][3]);
							sprintf(custom_tx_d_buffer_4[c_cnt], "M%d,Ti%d,\t%lf,\t%lf,\t%lf,\t%lf,\r\n",c_cnt, LED_list_custom[4], custom_cycle_send_buff[4][0], custom_cycle_send_buff[4][1], custom_cycle_send_buff[4][2], custom_cycle_send_buff[4][3]);
							
						}else if(pd_adc_custom_b_start == true) {
							sprintf(custom_tx_d_buffer_5[c_cnt], "M%d,Ti%d,\t%lf,\t%lf,\t%lf,\t%lf\r\n",c_cnt, LED_list_custom[0], custom_cycle_send_buff[0][0], custom_cycle_send_buff[0][1], custom_cycle_send_buff[0][2], custom_cycle_send_buff[0][3]);
							sprintf(custom_tx_d_buffer_6[c_cnt], "M%d,Ti%d,\t%lf,\t%lf,\t%lf,\t%lf\r\n",c_cnt, LED_list_custom[1], custom_cycle_send_buff[1][0], custom_cycle_send_buff[1][1], custom_cycle_send_buff[1][2], custom_cycle_send_buff[1][3]);
							sprintf(custom_tx_d_buffer_7[c_cnt], "M%d,Ti%d,\t%lf,\t%lf,\t%lf,\t%lf\r\n",c_cnt, LED_list_custom[2], custom_cycle_send_buff[2][0], custom_cycle_send_buff[2][1], custom_cycle_send_buff[2][2], custom_cycle_send_buff[2][3]);
							sprintf(custom_tx_d_buffer_8[c_cnt], "M%d,Ti%d,\t%lf,\t%lf,\t%lf,\t%lf\r\n",c_cnt, LED_list_custom[3], custom_cycle_send_buff[3][0], custom_cycle_send_buff[3][1], custom_cycle_send_buff[3][2], custom_cycle_send_buff[3][3]);
							sprintf(custom_tx_d_buffer_9[c_cnt], "M%d,Ti%d,\t%lf,\t%lf,\t%lf,\t%lf\r\n",c_cnt, LED_list_custom[4], custom_cycle_send_buff[4][0], custom_cycle_send_buff[4][1], custom_cycle_send_buff[4][2], custom_cycle_send_buff[4][3]);
							
}
					}
					/*********************/

					buf_no = 0;

					if(ble_connection_st == 0) {
							custom_adc_end();
							custom_send_timer_stop();
							printf("Custom ADC STOP 1");
						}
					else if(pd_adc_custom_a_start == true) {		// B mode
						pd_adc_custom_a_start = false;
						pd_adc_custom_b_start = true;
					
						custom_adc_start();

					}else if(pd_adc_custom_b_start == true) { // Completed
						pd_adc_custom_b_start = false;



//					}
//				else if(pd_adc_custom_a_start == true) { // Completed
//						pd_adc_custom_a_start = false;
						
						
						if(cmd_type_t == CMD_UART) {
							custom_send_timer_stop();
							
						}else if(cmd_type_t == CMD_BLE) {
							

							
						}

//						if(ble_connection_st == 1) {
//							battery_timer_start();
//						}

						led_off(99);
						pd_off(99);
						custom_adc_total_start();
					}		
				}
			}
		}
   }
  }
}

void custom_check_loop(void * p_context)	
{
	UNUSED_PARAMETER(p_context);
	custom_check_timer_stop();
	if ( custom_testing == false)
	{
		 printf("%d ms \r\n",t_ms);
			sprintf(custom_tx_buffer, "Measure Time :%d ms \r\n",t_ms);
	}
	else
	{
		t_ms++;
		custom_check_timer_start();
	}
}




void custom_send_loop(void * p_context)	/* For x ms */
{
	UNUSED_PARAMETER(p_context);
	custom_send_timer_stop();
	if(ble_connection_st){
	 if (data_tx_in_progress==true) {
        // Data transmission is still in progress
		  printf("skip \r\n");
        return;
    }
	
	static uint8_t order = 0;
  
	switch(order) {
		case 0:
			data_tx_handler(custom_tx_d_buffer_0[s_cnt]);
			break;

		case 1:
			data_tx_handler(custom_tx_d_buffer_1[s_cnt]);
			break;
				
		case 2:
			data_tx_handler(custom_tx_d_buffer_2[s_cnt]);
			break;

		case 3:
			data_tx_handler(custom_tx_d_buffer_3[s_cnt]);
			break;

		case 4:
			data_tx_handler(custom_tx_d_buffer_4[s_cnt]);
			break;

		case 5:
			data_tx_handler(custom_tx_d_buffer_5[s_cnt]);
			break;
			
		case 6:
			data_tx_handler(custom_tx_d_buffer_6[s_cnt]);
			break;

		case 7:
			data_tx_handler(custom_tx_d_buffer_7[s_cnt]);
			break;
				
		case 8:
			data_tx_handler(custom_tx_d_buffer_8[s_cnt]);
			break;

		case 9:
			data_tx_handler(custom_tx_d_buffer_9[s_cnt]);


			order = 0;
			custom_send_start();
#if FEATURE_PRINTF
			printf("custom_send Completed\r\n");
#endif


			return;

		default:
			break;
	}
	
						if(ble_connection_st == 0) {
							custom_send_timer_stop();
							order = 0;
							printf("Custom ADC STOP 2\r\n");}
							else{
								
								
								
								
								order++;
								custom_send_timer_start();	
								
							}
}

}
void custom_send_start(void)
{
	s_cnt++;
	if (s_cnt>c_max)
		{
			custom_send_timer_stop();
			
			
			data_tx_handler(ble_tx_buffer);
			data_tx_handler(custom_tx_buffer);
			battery_timer_start();
		}
	else 	
	{
		custom_send_timer_start();
	}
}
void custom_adc_total_start(void)
{
	
	if (pd_adc_custom_start==true)
	{ 
//	    custom_testing = false;
//			motion_raw_data_enabled = true;
//			custom_add_data = true;
//			icm42670_main();
//		
//			data_tx_handler(ble_tx_buffer);
			
			t_ms=0; 
		  custom_testing = true;
			custom_check_timer_start();	
			c_cnt=0;
			pd_adc_custom_start=false;
			pd_adc_custom_a_start=true;
			custom_adc_start();
			c_cnt++;
			
	}
	
	else if (c_cnt<c_max)
		{
			pd_adc_custom_a_start=true;
			custom_adc_start();
		  c_cnt++;
		}
	else if (c_cnt==c_max)
	{
		custom_testing = false;
		motion_raw_data_enabled = true;
		custom_add_data = true;
		icm42670_main();
		if(cmd_type_t == CMD_BLE){
		 s_cnt=0;
			custom_send_start();
			
			
		}
		c_cnt++;
		printf ("FINISH SEND\r\n");
		processing = false;
	}
}



void custom_adc_start(void)
{
	pd_no		= -1;
	led_no		= -1;
	buf_no		= 0;

	if(pd_adc_custom_a_start == true) {
#if FEATURE_PRINTF
		printf("\r\n===== Custom_A start ====================\r\n");
		printf("LED : ");
#endif
		for(uint8_t i =0; i < CUSTOM_LED_NO; i++) {
			LED_list_custom[i] = led_custom_list_a[i];
#if FEATURE_PRINTF
			printf("%d ", LED_list_custom[i]);
#endif
		}
#if FEATURE_PRINTF
		printf("\r\n");
		printf("PD  : ");
#endif
		for(uint8_t i =0; i < CUSTOM_PD_NO; i++) {
			PD_list_custom[i] = pd_custom_list_a[i];
#if FEATURE_PRINTF
			printf("%d ", PD_list_custom[i]);
#endif
		}
#if FEATURE_PRINTF
		printf("\r\n\r\n");
#endif		
	}

	if(pd_adc_custom_b_start == true) {
#if FEATURE_PRINTF
		printf("\r\n===== Custom_B start ====================\r\n");
		printf("LED : ");
#endif
		for(uint8_t i =0; i < CUSTOM_LED_NO; i++) {
			LED_list_custom[i] = led_custom_list_b[i];
#if FEATURE_PRINTF
			printf("%d ", LED_list_custom[i]);
#endif
		}
		
#if FEATURE_PRINTF
		printf("\r\n");
		printf("PD  : ");
#endif

		for(uint8_t i =0; i < CUSTOM_PD_NO; i++) {
			PD_list_custom[i] = pd_custom_list_b[i];
#if FEATURE_PRINTF
			printf("%d ", PD_list_custom[i]);
#endif
		}
#if FEATURE_PRINTF
		printf("\r\n\r\n");
#endif		
	}

//	if(pd_adc_custom_c_start == true) {
//#if FEATURE_PRINTF
//		printf("\r\n===== Custom_C start ====================\r\n");
//		printf("LED : ");
//#endif
//		for(uint8_t i =0; i < CUSTOM_LED_NO; i++) {
//			LED_list_custom[i] = led_custom_list_c[i];
//#if FEATURE_PRINTF
//			printf("%d ", LED_list_custom[i]);
//#endif
//		}
//		
//#if FEATURE_PRINTF
//		printf("\r\n");
//		printf("PD  : ");
//#endif

//		for(uint8_t i =0; i < CUSTOM_PD_NO; i++) {
//			PD_list_custom[i] = pd_custom_list_c[i];
//#if FEATURE_PRINTF
//			printf("%d ", PD_list_custom[i]);
//#endif
//		}
//#if FEATURE_PRINTF
//		printf("\r\n\r\n");
//#endif		
//	}

//	if(pd_adc_custom_d_start == true) {
//#if FEATURE_PRINTF
//		printf("\r\n===== Custom_D start ====================\r\n");
//		printf("LED : ");
//#endif
//		for(uint8_t i =0; i < CUSTOM_LED_NO; i++) {
//			LED_list_custom[i] = led_custom_list_d[i];
//#if FEATURE_PRINTF
//			printf("%d ", LED_list_custom[i]);
//#endif
//		}
//		
//#if FEATURE_PRINTF
//		printf("\r\n");
//		printf("PD  : ");
//#endif

//		for(uint8_t i =0; i < CUSTOM_PD_NO; i++) {
//			PD_list_custom[i] = pd_custom_list_d[i];
//#if FEATURE_PRINTF
//			printf("%d ", PD_list_custom[i]);
//#endif
//		}
//#if FEATURE_PRINTF
//		printf("\r\n\r\n");
//#endif		
//	}

	for(uint16_t i = 0; i < 72; i++) {
		for(uint16_t j = 0; j < CUSTOM_CYCLE_CNT; j++) {
			custom_cycle_buff[i][j] = 0.0f;
		}
	}
	for(uint8_t i = 0; i < CUSTOM_LED_NO; i++) {
		for(uint8_t j = 0; j < CUSTOM_PD_NO; j++) {
			custom_cycle_send_buff[i][j] = 0.0f;
		}
	}

	custom_adc_start2();

}


void custom_adc_start2(void) 
{
	custom_adc_init();
#if FEATURE_PRINTF
	custom_timer_init();
	custom_timer_start();
#else
	custom_irq_init();
	custom_ppi_init();
	custom_sampling_event_enable();	
#endif
}


void custom_adc_end(void) 
{
#if FEATURE_PRINTF
	printf("custom_adc_end\r\n");
	custom_timer_stop();
#endif

#if !FEATURE_PRINTF
	custom_sampling_event_disable();
	custom_irq_uninit();
	custom_ppi_uninit();
#endif
	custom_adc_uninit();
	
	
}

void custom_adc_end_2(void) 
{
#if FEATURE_PRINTF
	printf("custom_adc_end\r\n");
	custom_timer_stop();
#endif

#if !FEATURE_PRINTF
	custom_sampling_event_disable();
	custom_irq_uninit();
	custom_ppi_uninit();
#endif
	custom_adc_uninit();
	

}






void custom_adc_init(void)
{
#if FEATURE_PRINTF
	printf("custom_adc_init\r\n");
#endif

	static nrfx_saadc_config_t default_config;
	default_config.resolution			= (nrf_saadc_resolution_t)NRFX_SAADC_CONFIG_RESOLUTION; /* Resolution is 10bits */
	default_config.oversample			= (nrf_saadc_oversample_t)NRFX_SAADC_CONFIG_OVERSAMPLE; /* Over Sampling Disabled */
	default_config.interrupt_priority	= NRFX_SAADC_CONFIG_IRQ_PRIORITY;						/* Interrupt Priority is 0(Highest) */
	default_config.low_power_mode		= NRFX_SAADC_CONFIG_LP_MODE;							/* Low Power Mode is Disabled */

	static nrf_saadc_channel_config_t config;
	config.resistor_p	= NRF_SAADC_RESISTOR_DISABLED;
	config.resistor_n	= NRF_SAADC_RESISTOR_DISABLED;
	config.gain 		= NRF_SAADC_GAIN1_6;
	config.reference	= NRF_SAADC_REFERENCE_INTERNAL;
	config.acq_time 	= NRF_SAADC_ACQTIME_3US;
	config.mode 		= NRF_SAADC_MODE_DIFFERENTIAL;
	config.burst		= NRF_SAADC_BURST_DISABLED;
	config.pin_p		= (nrf_saadc_input_t)(NRF_SAADC_INPUT_AIN0);
	config.pin_n		= (nrf_saadc_input_t)(NRF_SAADC_INPUT_AIN1);
	
	ret_code_t err_code = nrf_drv_saadc_init(&default_config, custom_voltage_handler);
	APP_ERROR_CHECK(err_code);

	err_code = nrf_drv_saadc_channel_init(0, &config);
	APP_ERROR_CHECK(err_code);

	err_code = nrf_drv_saadc_buffer_convert(pd_custom_adc_buf[0], custom_samples_in_buffer + m_config.pd_delay_us/16);
	APP_ERROR_CHECK(err_code);
}



void custom_adc_uninit(void)
{
#if FEATURE_PRINTF
	printf("pd_custom_adc_uninit\r\n");
#endif

	nrf_drv_saadc_uninit();
	nrf_drv_saadc_channel_uninit(0);
}


#if !FEATURE_PRINTF
void custom_irq_init(void)
{
    ret_code_t err_code;

	/* Initialize int pin */
	if (!nrfx_gpiote_is_init())
	{
	  err_code = nrfx_gpiote_init();
	  APP_ERROR_CHECK(err_code);
	}

    nrfx_gpiote_in_config_t in_config = NRFX_GPIOTE_CONFIG_IN_SENSE_LOTOHI(true);
    in_config.pull = NRF_GPIO_PIN_PULLDOWN;

    err_code = nrfx_gpiote_in_init(ADA2200_SYNCO_PIN, &in_config, NULL);
    APP_ERROR_CHECK(err_code);

    nrfx_gpiote_in_event_enable(ADA2200_SYNCO_PIN, true);
}


void custom_irq_uninit(void)
{
	nrfx_gpiote_in_event_disable(ADA2200_SYNCO_PIN);
	nrfx_gpiote_in_uninit(ADA2200_SYNCO_PIN);
}
#endif


void custom_send_timer_start(void)
{
	APP_ERROR_CHECK(app_timer_start(m_custom_send_loop_timer_id, APP_TIMER_TICKS(CUSTOM_SEND_LOOP_INTERVAL), NULL));
}


void custom_send_timer_stop(void)
{
	APP_ERROR_CHECK(app_timer_stop(m_custom_send_loop_timer_id));

}


void custom_send_timer_init(void)
{
    APP_ERROR_CHECK(app_timer_create(&m_custom_send_loop_timer_id, APP_TIMER_MODE_SINGLE_SHOT, custom_send_loop));
}



void custom_check_timer_start(void)
{
	APP_ERROR_CHECK(app_timer_start(m_custom_check_loop_timer_id, APP_TIMER_TICKS(CUSTOM_CHECK_LOOP_INTERVAL), NULL));
}

void custom_check_timer_stop(void)
{
	APP_ERROR_CHECK(app_timer_stop(m_custom_check_loop_timer_id));

}
void custom_check_timer_init(void)
{
    APP_ERROR_CHECK(app_timer_create(&m_custom_check_loop_timer_id, APP_TIMER_MODE_SINGLE_SHOT, custom_check_loop));
}
