Activity Intensity Tracker

#include <zephyr.h>
#include <sys/printk.h>
#include <drivers/pwm.h>

#if defined(CONFIG_BOARD_NRF5340DK_NRF5340_CPUAPP) ||  defined(CONFIG_BOARD_NRF5340DK_NRF5340_CPUAPPNS) || defined(CONFIG_BOARD_NRF9160DK_NRF9160NS) || defined(CONFIG_BOARD_NRF9160DK_NRF9160)

/*ADC definitions and includes*/
#include <hal/nrf_saadc.h>
#define ADC_DEVICE_NAME DT_LABEL(DT_INST(0, nordic_nrf_saadc))
#define ADC_RESOLUTION 10
#define ADC_GAIN ADC_GAIN_1_6
#define ADC_REFERENCE ADC_REF_INTERNAL
#define ADC_ACQUISITION_TIME ADC_ACQ_TIME(ADC_ACQ_TIME_MICROSECONDS, 10)
#define ADC_1ST_CHANNEL_ID 0  
#define ADC_1ST_CHANNEL_INPUT NRF_SAADC_INPUT_AIN0
#define ADC_2ND_CHANNEL_ID 1  
#define ADC_2ND_CHANNEL_INPUT NRF_SAADC_INPUT_AIN1
#define ADC_3RD_CHANNEL_ID 2  
#define ADC_3RD_CHANNEL_INPUT NRF_SAADC_INPUT_AIN2

#define PWM_DEVICE_NAME DT_PROP(DT_NODELABEL(pwm0), label)
#define PWM_CH0_PIN DT_PROP(DT_NODELABEL(pwm0), ch0_pin)

#else
#error "Choose supported board or add new board for the application"
#endif

#include <zephyr.h>
#include <device.h>
#include <drivers/gpio.h>
#include <drivers/adc.h>
#include <string.h>
#include <drivers/pwm.h>

#include <stdio.h>

#define PWM_MAX 253
#define TIMER_INTERVAL_MSEC 200
//#define TIMER_INTERVAL_MSEC 200
#define BUFFER_SIZE 3

struct k_timer my_timer;
const struct device *adc_dev;
const struct device *pwm_dev;

static struct adc_channel_cfg m_1st_channel_cfg = {
	.gain = ADC_GAIN,
	.reference = ADC_REFERENCE,
	.acquisition_time = ADC_ACQUISITION_TIME,
	.channel_id = ADC_1ST_CHANNEL_ID,
#if defined(CONFIG_ADC_CONFIGURABLE_INPUTS)
	.input_positive = ADC_1ST_CHANNEL_INPUT,
#endif
};

static uint8_t channel_ids[BUFFER_SIZE] = {
	ADC_1ST_CHANNEL_ID,
	ADC_2ND_CHANNEL_ID,
	ADC_3RD_CHANNEL_ID,
};

static int16_t m_sample_buffer[BUFFER_SIZE];

static int32_t adc_vref;

static struct adc_sequence sequence = {
    .channels = BIT(ADC_1ST_CHANNEL_ID),
    .buffer = m_sample_buffer,
    .buffer_size = sizeof(m_sample_buffer),
    .resolution = ADC_RESOLUTION,
};

static int adc_sample_data_forwarder(void)
{
	int ret;

	if (!adc_dev) {
		return -1;
	}

	ret = adc_read(adc_dev, &sequence);
	if (ret) {
        printk("adc_read() failed with code %d\n", ret);
	}

	for (int i = 0; i < BUFFER_SIZE - 1; i++) {

			float raw_value = (float)m_sample_buffer[i];

			printf("%.3f,", raw_value);

	}
    printf("%.3f\r\n", (float)m_sample_buffer[BUFFER_SIZE - 1]);

	return ret;
}

void adc_sample_event(struct k_timer *timer_id){
    int err = adc_sample_data_forwarder();
    if (err) {
        printk("Error in adc sampling: %d\n", err);
    }
}

void main(void)
{
    int err;

    //PWM0 setup
    pwm_dev = device_get_binding(PWM_DEVICE_NAME);
    if (!pwm_dev) {
	    printk("device_get_binding() PWM0 failed\n");
	}
    
    //Timer setup
    k_timer_init(&my_timer, adc_sample_event, NULL);
    k_timer_start(&my_timer, K_MSEC(TIMER_INTERVAL_MSEC), K_MSEC(TIMER_INTERVAL_MSEC));


    //ADC0 setup
    adc_dev = device_get_binding(ADC_DEVICE_NAME);
	if (!adc_dev) {
        printk("device_get_binding ADC_0 (=%s) failed\n", ADC_DEVICE_NAME);
    } 
    
	/*
	 * Configure channels individually prior to sampling
	 */
	for (uint8_t i = 0; i < BUFFER_SIZE; i++) {
		m_1st_channel_cfg.channel_id = channel_ids[i];
#ifdef CONFIG_ADC_NRFX_SAADC
		m_1st_channel_cfg.input_positive = SAADC_CH_PSELP_PSELP_AnalogInput0
					     + channel_ids[i];
#endif

		err = adc_channel_setup(adc_dev, &m_1st_channel_cfg);
        if (err) {
            printk("Error in adc setup: %d\n", err);
        }

		sequence.channels |= BIT(channel_ids[i]);
	}

	adc_vref = adc_ref_internal(adc_dev);

    #if defined(CONFIG_BOARD_NRF9160DK_NRF9160NS) ||  defined(CONFIG_BOARD_NRF5340DK_NRF5340_CPUAPPNS)
    NRF_SAADC_NS->TASKS_CALIBRATEOFFSET = 1;
    #elif defined(CONFIG_BOARD_NRF9160DK_NRF9160) || defined(CONFIG_BOARD_NRF5340DK_NRF5340_CPUAPP)
    NRF_SAADC->TASKS_CALIBRATEOFFSET = 1;
    #else
    #error "Choose supported board or add new board for the application"
    #endif
}

/* Generated by Edge Impulse
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in
* all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
// Generated on: 06.06.2021 14:58:37

#ifndef trained_model_GEN_H
#define trained_model_GEN_H

#include "edge-impulse-sdk/tensorflow/lite/c/common.h"

// Sets up the model with init and prepare steps.
TfLiteStatus trained_model_init( void*(*alloc_fnc)(size_t,size_t) );
// Returns the input tensor with the given index.
TfLiteTensor *trained_model_input(int index);
// Returns the output tensor with the given index.
TfLiteTensor *trained_model_output(int index);
// Runs inference for the model.
TfLiteStatus trained_model_invoke();
//Frees memory allocated
TfLiteStatus trained_model_reset( void (*free)(void* ptr) );


// Returns the number of input tensors.
inline size_t trained_model_inputs() {
  return 1;
}
// Returns the number of output tensors.
inline size_t trained_model_outputs() {
  return 1;
}

inline void *trained_model_input_ptr(int index) {
  return trained_model_input(index)->data.data;
}
inline size_t trained_model_input_size(int index) {
  return trained_model_input(index)->bytes;
}
inline int trained_model_input_dims_len(int index) {
  return trained_model_input(index)->dims->data[0];
}
inline int *trained_model_input_dims(int index) {
  return &trained_model_input(index)->dims->data[1];
}

inline void *trained_model_output_ptr(int index) {
  return trained_model_output(index)->data.data;
}
inline size_t trained_model_output_size(int index) {
  return trained_model_output(index)->bytes;
}
inline int trained_model_output_dims_len(int index) {
  return trained_model_output(index)->dims->data[0];
}
inline int *trained_model_output_dims(int index) {
  return &trained_model_output(index)->dims->data[1];
}

#endif

/* Generated by Edge Impulse
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in
* all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/

#ifndef _EI_CLASSIFIER_MODEL_METADATA_H_
#define _EI_CLASSIFIER_MODEL_METADATA_H_

#include <stdint.h>

#define EI_CLASSIFIER_NONE                       255
#define EI_CLASSIFIER_UTENSOR                    1
#define EI_CLASSIFIER_TFLITE                     2
#define EI_CLASSIFIER_CUBEAI                     3
#define EI_CLASSIFIER_TFLITE_FULL                4
#define EI_CLASSIFIER_TENSAIFLOW                 5
#define EI_CLASSIFIER_TENSORRT                   6

#define EI_CLASSIFIER_SENSOR_UNKNOWN             -1
#define EI_CLASSIFIER_SENSOR_MICROPHONE          1
#define EI_CLASSIFIER_SENSOR_ACCELEROMETER       2
#define EI_CLASSIFIER_SENSOR_CAMERA              3

// These must match the enum values in TensorFlow Lite's "TfLiteType"
#define EI_CLASSIFIER_DATATYPE_FLOAT32           1
#define EI_CLASSIFIER_DATATYPE_INT8              9

#define EI_CLASSIFIER_PROJECT_ID                 8639
#define EI_CLASSIFIER_PROJECT_OWNER              "vincent wong"
#define EI_CLASSIFIER_PROJECT_NAME               "wesee-project-1"
#define EI_CLASSIFIER_PROJECT_DEPLOY_VERSION     2
#define EI_CLASSIFIER_NN_INPUT_FRAME_SIZE        33
#define EI_CLASSIFIER_RAW_SAMPLE_COUNT           17
#define EI_CLASSIFIER_RAW_SAMPLES_PER_FRAME      3
#define EI_CLASSIFIER_DSP_INPUT_FRAME_SIZE       (EI_CLASSIFIER_RAW_SAMPLE_COUNT * EI_CLASSIFIER_RAW_SAMPLES_PER_FRAME)
#define EI_CLASSIFIER_INPUT_WIDTH                0
#define EI_CLASSIFIER_INPUT_HEIGHT               0
#define EI_CLASSIFIER_INTERVAL_MS                142.857142857143
#define EI_CLASSIFIER_LABEL_COUNT                3
#define EI_CLASSIFIER_HAS_ANOMALY                0
#define EI_CLASSIFIER_FREQUENCY                  7
#define EI_CLASSIFIER_USE_QUANTIZED_DSP_BLOCK    0


#define EI_CLASSIFIER_OBJECT_DETECTION           0


#define EI_CLASSIFIER_TFLITE_ARENA_SIZE          3673
#define EI_CLASSIFIER_TFLITE_INPUT_DATATYPE      EI_CLASSIFIER_DATATYPE_INT8
#define EI_CLASSIFIER_TFLITE_INPUT_QUANTIZED     1
#define EI_CLASSIFIER_TFLITE_INPUT_SCALE         0.20492251217365265
#define EI_CLASSIFIER_TFLITE_INPUT_ZEROPOINT     -128
#define EI_CLASSIFIER_TFLITE_OUTPUT_DATATYPE     EI_CLASSIFIER_DATATYPE_INT8
#define EI_CLASSIFIER_TFLITE_OUTPUT_QUANTIZED    1
#define EI_CLASSIFIER_TFLITE_OUTPUT_SCALE        0.00390625
#define EI_CLASSIFIER_TFLITE_OUTPUT_ZEROPOINT    -128
#define EI_CLASSIFIER_INFERENCING_ENGINE         EI_CLASSIFIER_TFLITE
#define EI_CLASSIFIER_COMPILED                   1
#define EI_CLASSIFIER_HAS_TFLITE_OPS_RESOLVER    1

#define EI_CLASSIFIER_SENSOR                     EI_CLASSIFIER_SENSOR_UNKNOWN
#ifndef EI_CLASSIFIER_SLICES_PER_MODEL_WINDOW
#define EI_CLASSIFIER_SLICES_PER_MODEL_WINDOW    4
#endif // EI_CLASSIFIER_SLICES_PER_MODEL_WINDOW
#define EI_CLASSIFIER_SLICE_SIZE                 (EI_CLASSIFIER_RAW_SAMPLE_COUNT / EI_CLASSIFIER_SLICES_PER_MODEL_WINDOW)

#if EI_CLASSIFIER_INFERENCING_ENGINE == EI_CLASSIFIER_TFLITE && EI_CLASSIFIER_USE_FULL_TFLITE == 1
#undef EI_CLASSIFIER_INFERENCING_ENGINE
#undef EI_CLASSIFIER_HAS_TFLITE_OPS_RESOLVER
#define EI_CLASSIFIER_INFERENCING_ENGINE          EI_CLASSIFIER_TFLITE_FULL
#define EI_CLASSIFIER_HAS_TFLITE_OPS_RESOLVER     0
#if EI_CLASSIFIER_COMPILED == 1
#error "Cannot use full TensorFlow Lite with EON"
#endif
#endif // EI_CLASSIFIER_INFERENCING_ENGINE == EI_CLASSIFIER_TFLITE && EI_CLASSIFIER_USE_FULL_TFLITE == 1

const char* ei_classifier_inferencing_categories[] = { "high", "low", "medium" };

typedef struct {
    uint16_t implementation_version;
    int axes;
    float scale_axes;
    bool average;
    bool minimum;
    bool maximum;
    bool rms;
    bool stdev;
    bool skewness;
    bool kurtosis;
} ei_dsp_config_flatten_t;

typedef struct {
    uint16_t implementation_version;
    int axes;
    const char * channels;
} ei_dsp_config_image_t;

typedef struct {
    uint16_t implementation_version;
    int axes;
    int num_cepstral;
    float frame_length;
    float frame_stride;
    int num_filters;
    int fft_length;
    int win_size;
    int low_frequency;
    int high_frequency;
    float pre_cof;
    int pre_shift;
} ei_dsp_config_mfcc_t;

typedef struct {
    uint16_t implementation_version;
    int axes;
    float frame_length;
    float frame_stride;
    int num_filters;
    int fft_length;
    int low_frequency;
    int high_frequency;
    int win_size;
} ei_dsp_config_mfe_t;

typedef struct {
    uint16_t implementation_version;
    int axes;
    float scale_axes;
} ei_dsp_config_raw_t;

typedef struct {
    uint16_t implementation_version;
    int axes;
    float scale_axes;
    const char * filter_type;
    float filter_cutoff;
    int filter_order;
    int fft_length;
    int spectral_peaks_count;
    float spectral_peaks_threshold;
    const char * spectral_power_edges;
} ei_dsp_config_spectral_analysis_t;

typedef struct {
    uint16_t implementation_version;
    int axes;
    float frame_length;
    float frame_stride;
    int fft_length;
    bool show_axes;
} ei_dsp_config_spectrogram_t;

typedef struct {
    uint16_t implementation_version;
    int axes;
    float frame_length;
    float frame_stride;
    int num_filters;
    int fft_length;
    int low_frequency;
    int high_frequency;
    float pre_cof;
    bool invert_features;
} ei_dsp_config_audio_syntiant_t;

uint8_t ei_dsp_config_3_axes[] = { 0, 1, 2 };
const uint32_t ei_dsp_config_3_axes_size = 3;
ei_dsp_config_spectral_analysis_t ei_dsp_config_3 = {
    1,
    3,
    1.00000f,
    "low",
    3.00000f,
    6,
    128,
    3,
    0.10000f,
    "0.1, 0.5, 1.0, 2.0, 5.0"
};

#endif // _EI_CLASSIFIER_MODEL_METADATA_H_

#elif defined(EI_CLASSIFIER_SENSOR) && EI_CLASSIFIER_SENSOR == EI_CLASSIFIER_SENSOR_ADC
// ADC start

/* Private variables ------------------------------------------------------- */
static float acc_buf[EI_CLASSIFIER_DSP_INPUT_FRAME_SIZE];
static int acc_sample_count = 0;

static bool acc_data_callback(const void *sample_buf, uint32_t byteLength)
{
    float *buffer = (float *)sample_buf;
    for(int i = 0; i < (int)(byteLength / sizeof(float)); i++) {
        acc_buf[acc_sample_count + i] = buffer[i];
    }

    return true;
}

void run_nn(bool debug)
{
    char ble_printf[64] = {0};
    bool stop_inferencing = false;
    // summary of inferencing settings (from model_metadata.h)
    ei_printf("CLASSIFIER SENSOR (%d)\n", EI_CLASSIFIER_SENSOR);
    ei_printf("Inferencing settings:\n");
    ei_printf("\tInterval: ");
    ei_printf_float((float)EI_CLASSIFIER_INTERVAL_MS);
    ei_printf("ms.\n");
    ei_printf("\tFrame size: %d\n", EI_CLASSIFIER_DSP_INPUT_FRAME_SIZE);
    ei_printf("\tSample length: ");
    ei_printf_float(1000.0f * static_cast<float>(EI_CLASSIFIER_RAW_SAMPLE_COUNT) /
                  (1000.0f / static_cast<float>(EI_CLASSIFIER_INTERVAL_MS)));
    ei_printf("ms.\n");
    ei_printf("\tNo. of classes: %d\n", sizeof(ei_classifier_inferencing_categories) /
                                            sizeof(ei_classifier_inferencing_categories[0]));

    ei_printf("Starting inferencing, press 'b' to break\n");

    ei_adc_sample_start(&acc_data_callback, EI_CLASSIFIER_INTERVAL_MS);

    while (stop_inferencing == false) {
        ei_printf("Starting inferencing in 2 seconds...\n");

        // instead of wait_ms, we'll wait on the signal, this allows threads to cancel us...
        if (ei_sleep(2000) != EI_IMPULSE_OK) {
            break;
        }

        if(ei_user_invoke_stop()) {
            ei_printf("Inferencing stopped by user\r\n");
            break;
        }

        if (stop_inferencing) {
            break;
        }

        ei_printf("Sampling...\n");

        /* Run sampler */
        acc_sample_count = 0;
        for(int i = 0; i < EI_CLASSIFIER_RAW_SAMPLE_COUNT; i++) {
            ei_adc_read_data();
            acc_sample_count += EI_CLASSIFIER_RAW_SAMPLES_PER_FRAME;
        }

        // Create a data structure to represent this window of data
        signal_t signal;
        int err = numpy::signal_from_buffer(acc_buf, EI_CLASSIFIER_DSP_INPUT_FRAME_SIZE, &signal);
        if (err != 0) {
            ei_printf("ERR: signal_from_buffer failed (%d)\n", err); 
        }

        // run the impulse: DSP, neural network and the Anomaly algorithm
        ei_impulse_result_t result = { 0 };
        EI_IMPULSE_ERROR ei_error = run_classifier(&signal, &result, debug);
        if (ei_error != EI_IMPULSE_OK) {
            ei_printf("Failed to run impulse (%d)\n", ei_error);
            break;
        }

        // print the predictions
        ei_printf("Predictions (DSP: %d ms., Classification: %d ms., Anomaly: %d ms.): \n",
            result.timing.dsp, result.timing.classification, result.timing.anomaly);
        for (size_t ix = 0; ix < EI_CLASSIFIER_LABEL_COUNT; ix++) {            
            ei_printf("    %s: \t", result.classification[ix].label);
            ei_printf_float(result.classification[ix].value);
            ei_printf("\r\n");
        }
#if EI_CLASSIFIER_HAS_ANOMALY == 1
        ei_printf("    anomaly score: ");
        ei_printf_float(result.anomaly);
        ei_printf("\r\n");        
#endif

    /*BLE PRINTF*/
        for (size_t ix = 0; ix < EI_CLASSIFIER_LABEL_COUNT; ix++) {            
            sprintf(ble_printf, "%s: %f\n", result.classification[ix].label, result.classification[ix].value);
            ble_nus_send_data(ble_printf, strlen(ble_printf));
        }
#if EI_CLASSIFIER_HAS_ANOMALY == 1
        sprintf(ble_printf, "anomaly score: %f\n", result.anomaly);
        ble_nus_send_data(ble_printf, strlen(ble_printf));
#endif

        memset(ble_printf, 0x00, sizeof(ble_printf));
    /*END BLE PRINTF*/

        if(ei_user_invoke_stop() || ei_ble_user_invoke_stop()) {
            ei_printf("Inferencing stopped by user\r\n");
            ble_nus_send_data("Inferencing stopped by user\n", strlen("Inferencing stopped by user\n"));
            memset(ble_printf, 0x00, sizeof(ble_printf));
            break;
        }
    }
}

// ADC end

#ifndef EI_ADC_SENSOR
#define EI_ADC_SENSOR

/* Include ----------------------------------------------------------------- */
#include "ei_sampler.h"
#include <zephyr.h>
#include <device.h>
#include <devicetree.h>
#include <drivers/sensor.h>
#include "iis2dlpc_reg.h"

#define ADC_DEVICE_LABEL "ADC"
#define IIS2DLPC_ADDRESS    0x19

/** Number of axis used and sample data format */
typedef float sample_format_t;
#define N_AXIS_SAMPLED          3 
#define SIZEOF_N_AXIS_SAMPLED   (sizeof(sample_format_t) * N_AXIS_SAMPLED)


#if defined(CONFIG_BOARD_NRF5340DK_NRF5340_CPUAPP) ||  defined(CONFIG_BOARD_NRF5340DK_NRF5340_CPUAPPNS) || defined(CONFIG_BOARD_NRF9160DK_NRF9160NS) || defined(CONFIG_BOARD_NRF9160DK_NRF9160)

/*ADC definitions and includes*/
#include <hal/nrf_saadc.h>
#define ADC_DEVICE_NAME DT_LABEL(DT_INST(0, nordic_nrf_saadc))
#define ADC_RESOLUTION 10
#define ADC_GAIN ADC_GAIN_1_6
#define ADC_REFERENCE ADC_REF_INTERNAL
#define ADC_ACQUISITION_TIME ADC_ACQ_TIME(ADC_ACQ_TIME_MICROSECONDS, 10)
#define ADC_1ST_CHANNEL_ID 0  
#define ADC_1ST_CHANNEL_INPUT NRF_SAADC_INPUT_AIN0
#define ADC_2ND_CHANNEL_ID 1  
#define ADC_2ND_CHANNEL_INPUT NRF_SAADC_INPUT_AIN1
#define ADC_3RD_CHANNEL_ID 2  
#define ADC_3RD_CHANNEL_INPUT NRF_SAADC_INPUT_AIN2

#else
#error "Choose supported board or add new board for the application"
#endif



/* Function prototypes ----------------------------------------------------- */
bool ei_adc_init(void);
void ei_adc_read_data(void);
bool ei_adc_sample_start(sampler_callback callback, float sample_interval_ms);
bool ei_adc_setup_data_sampling(void);
void ei_adc_read_data_one(void);

#endif

/* Include ----------------------------------------------------------------- */
#include <stdint.h>
#include <stdlib.h>

#include "ei_config_types.h"
#include "ei_adcsensor.h"
#include "ei_device_nordic_nrf52.h"
#include "sensor_aq.h"

#include <drivers/gpio.h>
#include <drivers/adc.h>

#include <drivers/i2c.h>


#include <zephyr.h>

/* Constant defines -------------------------------------------------------- */
#define CONVERT_G_TO_MS2    9.80665f

#define ACC_SAMPLE_TIME_MS  0.912f
#define FLASH_WRITE_TIME_MS 0.5036f

#define ACC_SAMPLE_TIME_US  255
#define FLASH_WRITE_TIME_US 543
#define CORRECTION_TIME_US  400

extern ei_config_t *ei_config_get_config();
extern EI_CONFIG_ERROR ei_config_set_sample_interval(float interval);

//stmdev_ctx_t dev_ctx;

sampler_callback  adc_cb_sampler;

//int16_t data_raw_acceleration[N_AXIS_SAMPLED];

uint32_t adc_sample_interval_real_us = 0;
static bool adc_init = false;


/* ADC start */

#define TIMER_INTERVAL_MSEC 200
//#define TIMER_INTERVAL_MSEC 200
#define BUFFER_SIZE 3

struct k_timer my_timer;
const struct device *adc_dev;

static struct adc_channel_cfg m_1st_channel_cfg = {
	.gain = ADC_GAIN,
	.reference = ADC_REFERENCE,
	.acquisition_time = ADC_ACQUISITION_TIME,
	.channel_id = ADC_1ST_CHANNEL_ID,
#if defined(CONFIG_ADC_CONFIGURABLE_INPUTS)
	.input_positive = ADC_1ST_CHANNEL_INPUT,
#endif
};

static uint8_t channel_ids[BUFFER_SIZE] = {
	ADC_1ST_CHANNEL_ID,
	ADC_2ND_CHANNEL_ID,
	ADC_3RD_CHANNEL_ID,
};

static int16_t m_sample_buffer[BUFFER_SIZE];
static float acceleration_g[BUFFER_SIZE];

static int32_t adc_vref;

static struct adc_sequence sequence = {
    .channels = BIT(ADC_1ST_CHANNEL_ID),
    .buffer = m_sample_buffer,
    .buffer_size = sizeof(m_sample_buffer),
    .resolution = ADC_RESOLUTION,
};

/* ADC end */



/**
 * @brief      Setup I2C config and accelerometer convert value
 *
 * @return     false if communinication error occured
 */
bool ei_adc_init(void)
{
    int err;

    //ADC0 setup
    adc_dev = device_get_binding(ADC_DEVICE_NAME);
	if (!adc_dev) {
        printk("device_get_binding ADC_0 (=%s) failed\n", ADC_DEVICE_NAME);
    } 
    
   	/*
	 * Configure channels individually prior to sampling
	 */
	for (uint8_t i = 0; i < BUFFER_SIZE; i++) {
		m_1st_channel_cfg.channel_id = channel_ids[i];
#ifdef CONFIG_ADC_NRFX_SAADC
		m_1st_channel_cfg.input_positive = SAADC_CH_PSELP_PSELP_AnalogInput0
					     + channel_ids[i];
#endif

		err = adc_channel_setup(adc_dev, &m_1st_channel_cfg);
        if (err) {
            printk("Error in adc setup: %d\n", err);
        }

		sequence.channels |= BIT(channel_ids[i]);
	}

	adc_vref = adc_ref_internal(adc_dev);

    #if defined(CONFIG_BOARD_NRF9160DK_NRF9160NS) ||  defined(CONFIG_BOARD_NRF5340DK_NRF5340_CPUAPPNS)
    NRF_SAADC_NS->TASKS_CALIBRATEOFFSET = 1;
    #elif defined(CONFIG_BOARD_NRF9160DK_NRF9160) || defined(CONFIG_BOARD_NRF5340DK_NRF5340_CPUAPP)
    NRF_SAADC->TASKS_CALIBRATEOFFSET = 1;
    #else
    #error "Choose supported board or add new board for the application"
    #endif

    ei_printf("Sensor " ADC_DEVICE_LABEL " init OK\n");
    adc_init = true;
    return true;
}

/**
 * @brief      Get data from sensor, convert and call callback to handle
 */
void ei_adc_read_data(void)
{
    int ret;

    if (adc_dev) {
        ret = adc_read(adc_dev, &sequence);
        if (ret) {
            printk("adc_read() failed with code %d\n", ret);
            return;
        }

        for (int i = 0; i < BUFFER_SIZE - 1; i++) {
                acceleration_g[i] = (float)m_sample_buffer[i];
        } 
    } else {  // send zero if no data
        acceleration_g[0] = 0.0f;
        acceleration_g[1] = 0.0f;
        acceleration_g[2] = 0.0f;
    }

    adc_cb_sampler((const void *)&acceleration_g[0], SIZEOF_N_AXIS_SAMPLED);
    k_usleep(adc_sample_interval_real_us);

}

/**
 * @brief      Setup timing and data handle callback function
 *
 * @param[in]  callsampler         Function to handle the sampled data
 * @param[in]  sample_interval_ms  The sample interval milliseconds
 *
 * @return     true
 */
bool ei_adc_sample_start(sampler_callback callsampler, float sample_interval_ms)
{
    adc_cb_sampler = callsampler;

    adc_sample_interval_real_us = uint32_t(sample_interval_ms * 1000);
    adc_sample_interval_real_us = adc_sample_interval_real_us - (FLASH_WRITE_TIME_US + ACC_SAMPLE_TIME_US + CORRECTION_TIME_US);

    ei_printf("adc_sample_interval_real_us = %d us\n", adc_sample_interval_real_us);

    EiDevice.set_state(eiStateSampling);
    
    return true;
}

/**
 * @brief      Setup payload header
 *
 * @return     true
 */
bool ei_adc_setup_data_sampling(void)
{
    // not use?
    return true;
}


/* Static functions -------------------------------------------------------- */