/**
 * @file    shot_capture.c
 * @brief   Shot Capture module
 * @author  Eric Xu
 * @date    Oct 15 2025
 * @version 0.0.1
 *
 * @section LICENSES
 * Copyright (C) Morgan Advanced Programmable Systems, Inc.
 */


/*--------------------------------------------------------------------------------------------------
 * INCLUDE FILES
 *------------------------------------------------------------------------------------------------*/
#include <stdint.h>
#include <stdbool.h>
#include <stdlib.h>
#include <string.h>

#include <zephyr/kernel.h>
#include <zephyr/timing/timing.h>

#include "shot_capture.h"

#include "../app/shot_storage.h"
#include "../app/ble.h"  

#include "../lib/debug.h"
#include "../lib/fs.h"
#include "../lib/errno.h"
#include "../lib/print.h"

#include "../bsp/als.h"
#include "../bsp/imu.h"
#include "../bsp/imu_reg.h"
#include "../bsp/wdt.h"

#include "../wrappers/i2c.h"


/*--------------------------------------------------------------------------------------------------
 * MACROS & CONSTANTS
 *------------------------------------------------------------------------------------------------*/


/*--------------------------------------------------------------------------------------------------
 * PUBLIC CONSTANTS & VARIABLES
 *------------------------------------------------------------------------------------------------*/
K_MUTEX_DEFINE(shot_data_mutex); /* Protect shared shot_data struct */
K_MUTEX_DEFINE(FIFO_ring_buf_mutex); /* Protect shared FIFO ring buffer struct */
K_MUTEX_DEFINE(stat_mutex); /* Protect debug statistic variables */

volatile FIFO_ring_buf_s FIFO_ring_buf = {.buf              = NULL,
                                          .write_ptr        = 0,
                                          .read_ptr         = 0,
                                          .loop_ctr         = 0};

volatile shot_data_status_s shot_data = {.start                = 0,
                                         .location             = 0,
                                         .words_per_sample_set = WORDS_PER_SAMPLE_SET,
                                         .bytes_per_sample_set = BYTES_PER_SAMPLE_SET,
                                         .size                 = SHOTDATA_SIZE_IN_BYTES_DEFAULT,
                                         .bytes_after_shot     = BYTES_AFTER_SHOT_DEFAULT,
                                         .marked               = false,
                                         .ready                = false,
                                         .saved                = false};


/*--------------------------------------------------------------------------------------------------
 * PRIVATE CONSTANTS & VARIABLES
 *------------------------------------------------------------------------------------------------*/
static FS_file_t _shot_capture_file;
static SHOT_CAPTURE_config_s _shot_capture_config = {
    .shot_duration   = SHOTDATA_DURATION_DEFAULT,
    .shot_location   = SHOTDATA_LOCATION_DEFAULT,
    .fs_g            = LSM6DS3TR_C_FS_G_ABSTR_250dps,
    .odr_g           = LSM6DS3TR_C_HZ_TO_ODR_G(GYRO_ODR_DEFAULT_HZ),
    .fs_xl           = LSM6DS3TR_C_FS_XL_2g,
    .odr_xl          = LSM6DS3TR_C_HZ_TO_ODR_XL(ACCEL_ODR_DEFAULT_HZ),
    .tap_ths         = 25,
    .quiet_time      = LSM6DS3TR_C_QUIET_4_OVER_ODR_XL,
    .shock_time      = LSM6DS3TR_C_SHOCK_16_OVER_ODR_XL,
    .user_shot_cnt   = 0,
    .shot_cnt        = 0,
    .shot_capture_en = 1
};
static atomic_t _shot_capture_is_enabled = ATOMIC_INIT(0);
static atomic_t _start_capture           = ATOMIC_INIT(0);

static uint32_t _user_shot_cnt_rt;
static uint32_t _shot_cnt_rt;
static uint8_t _shot_capture_en_rt;
static LSM6DS3TR_C_FS_G_ABSTR_e _fs_g_orig;
static LSM6DS3TR_C_ODR_G_e      _odr_g_orig;
static LSM6DS3TR_C_FS_XL_e      _fs_xl_orig;
static LSM6DS3TR_C_ODR_XL_e     _odr_xl_orig;
K_MUTEX_DEFINE(shot_capture_enable_mutex); /* Serialize shot capture enable/disable process */
/* Idle fence for shot capture task */
K_SEM_DEFINE(shot_capture_idle_sem, 0, 1);
static atomic_t _shot_capture_idle_requested = ATOMIC_INIT(0);

atomic_t _shot_storage_buf_is_in_use = ATOMIC_INIT(0);


/*--------------------------------------------------------------------------------------------------
 * THREAD DEFINITIONS
 *------------------------------------------------------------------------------------------------*/
K_THREAD_STACK_DEFINE(_shot_capture_task_stack, SHOT_CAPTURE_TASK_STACK_SIZE);


/*--------------------------------------------------------------------------------------------------
 * PRIVATE FUNCTION PROTOTYPES
 *------------------------------------------------------------------------------------------------*/
static void _shot_capture_task(void *p1, void *p2, void *p3);
static ERRNO_e _read_shot_data_into_ring_buf(uint32_t fifo_unread_sample_set_in_bytes, bool end);
static ERRNO_e _read_FIFO_into_ring_buf(int32_t overshoot, uint32_t bytes_to_read);
static void _print_shot_data(void);
static void _shot_capture_config_read(void);
static ERRNO_e _shot_capture_config_write(void);
static void _shot_capture_shotdata_param_cal(void);


/*--------------------------------------------------------------------------------------------------
 * PUBLIC FUNCTION DEFINITIOPNS
 *------------------------------------------------------------------------------------------------*/
void SHOT_CAPTURE_init(void)
{
    PRINT(INFO, "Initializing SHOT_CAPTURE...\r\n");

    /* Initialize IMU. */
    if (IMU_init() < ERRNO_SUCCESS) {
        PRINT(CRITICAL, "Failure to initialize IMU.\r\n");
    }

    /* read shot capture config from nvm */
    _shot_capture_config_read();

    _user_shot_cnt_rt   = _shot_capture_config.user_shot_cnt;
    _shot_cnt_rt        = _shot_capture_config.shot_cnt;
    _shot_capture_en_rt = _shot_capture_config.shot_capture_en;
}

void SHOT_CAPTURE_tap_ths_set(uint8_t tap_ths)
{
    if (tap_ths > 31) {
        PRINT(ERROR, "Invalid tap threshold. Use default tap threshold.\r\n");
        if (_shot_capture_config.tap_ths != 4) {
            _shot_capture_config.tap_ths = 4;
            _shot_capture_config_write();
        }
    } else {
        if (_shot_capture_config.tap_ths != tap_ths) {
            _shot_capture_config.tap_ths = tap_ths;
            _shot_capture_config_write();
        }
    }

    if (atomic_get(&_shot_capture_is_enabled)) {
        /* set tap threshold to be tap_ths * fs_xl/2^5 */
        if (IMU_set_tap_ths(_shot_capture_config.tap_ths) != ERRNO_SUCCESS) {
            PRINT(ERROR, "Failed to set tap threshold in IMU.\r\n");
        }
    }
}

void SHOT_CAPTURE_quiet_time_set(LSM6DS3TR_C_QUIET_e quiet_time)
{
    if (quiet_time > LSM6DS3TR_C_QUIET_12_OVER_ODR_XL) {
        PRINT(ERROR, "Invalid quiet time. Use default quiet time.\r\n");
        if(_shot_capture_config.quiet_time != LSM6DS3TR_C_QUIET_4_OVER_ODR_XL) {
            _shot_capture_config.quiet_time = LSM6DS3TR_C_QUIET_4_OVER_ODR_XL;
            _shot_capture_config_write();
        }
    } else {
        if (_shot_capture_config.quiet_time != quiet_time) {
            _shot_capture_config.quiet_time = quiet_time;
            _shot_capture_config_write();
        }
    }

    if (atomic_get(&_shot_capture_is_enabled)) {
        /* set the quiet time */
        if (IMU_set_quiet(_shot_capture_config.quiet_time) != ERRNO_SUCCESS) {
            PRINT(ERROR, "Failed to set quiet time in IMU.\r\n");
        }
    }
}

void SHOT_CAPTURE_shock_time_set(LSM6DS3TR_C_SHOCK_e shock_time)
{
    if (shock_time > LSM6DS3TR_C_SHOCK_24_OVER_ODR_XL) {
        PRINT(ERROR, "Invalid shock time. Use default shock time.\r\n");
        if(_shot_capture_config.shock_time != LSM6DS3TR_C_SHOCK_16_OVER_ODR_XL) {
            _shot_capture_config.shock_time = LSM6DS3TR_C_SHOCK_16_OVER_ODR_XL;
            _shot_capture_config_write();
        }
    } else {
        if (_shot_capture_config.shock_time != shock_time) {
            _shot_capture_config.shock_time = shock_time;
            _shot_capture_config_write();
        }
    }

    if (atomic_get(&_shot_capture_is_enabled)) {
        /* set the shock time */
        if (IMU_set_shock(_shot_capture_config.shock_time) != ERRNO_SUCCESS) {
            PRINT(ERROR, "Failed to set shock time in IMU.\r\n");
        }
    }
}

void SHOT_CAPTURE_shotdata_duration_location_set(uint8_t duration, uint8_t location)
{
    if (duration < SHOTDATA_DURATION_MIN || duration > SHOTDATA_DURATION_MAX) {
        PRINT(ERROR, "Invalid shot data duration. Use default shotdata duration.\r\n");
        if (_shot_capture_config.shot_duration != SHOTDATA_DURATION_DEFAULT) {
            _shot_capture_config.shot_duration = SHOTDATA_DURATION_DEFAULT;
            _shot_capture_config.shot_location = SHOTDATA_DURATION_DEFAULT / 2;
            _shot_capture_config_write();
            _shot_capture_shotdata_param_cal();
        }
    } else {
        if (_shot_capture_config.shot_duration != duration || _shot_capture_config.shot_location != location) {
            _shot_capture_config.shot_duration = duration;
            if (location < 0 || location > duration - 1) {
                PRINT(ERROR, "Invalid shot data location. Set to middle of shotdata duration.\r\n");
                _shot_capture_config.shot_location = duration / 2;
            } else {
                _shot_capture_config.shot_location = location;
            }
            _shot_capture_config_write();
            _shot_capture_shotdata_param_cal();
        }
    }
}

SHOT_CAPTURE_config_s *SHOT_CAPTURE_config_get(void)
{
    return &_shot_capture_config;
}

ERRNO_e SHOT_CAPTURE_enable(bool enable)
{
    ERRNO_e err = ERRNO_SUCCESS;
    uint8_t reg_val = 0;
    uint8_t *p_data;
    uint32_t size = 0;

    k_mutex_lock(&shot_capture_enable_mutex, K_FOREVER);

    if (enable && !atomic_get(&_shot_capture_is_enabled) && _shot_capture_config.shot_capture_en && !atomic_get(&_shot_storage_buf_is_in_use)) {
        k_mutex_lock(&shot_data_mutex, K_FOREVER);
        size = shot_data.size;
        k_mutex_unlock(&shot_data_mutex);
        err = SHOT_STORAGE_get_buf(size, &p_data);
        if (err != ERRNO_SUCCESS) {
            PRINT(ERROR, "Cannot enable shot capture, shot storage buffer allocation failed!\r\n");
            goto out;
        } else {
            k_mutex_lock(&FIFO_ring_buf_mutex, K_FOREVER);
            FIFO_ring_buf.buf = p_data;
            FIFO_ring_buf.write_ptr = 0;
            k_mutex_unlock(&FIFO_ring_buf_mutex);
        }

        /* enable shot capture interrupt mode */
        IMU_set_shot_capture_mode(true);

        /* save the original settings for accel and gyro */
        err = IMU_get_fs_xl(&_fs_xl_orig);
        
        if (err == ERRNO_SUCCESS) {
            err = IMU_get_odr_xl(&_odr_xl_orig);
        }

        if (err == ERRNO_SUCCESS) {
            err = IMU_get_fs_g(&_fs_g_orig);
        }

        if (err == ERRNO_SUCCESS) {
            err = IMU_get_odr_g(&_odr_g_orig);
        }

        /* set accel and gyro to desired settings for shot capture */
        if (err == ERRNO_SUCCESS) {
            err = IMU_set_fs_xl(_shot_capture_config.fs_xl);
        }
       
        if (err == ERRNO_SUCCESS) {
            err = IMU_set_odr_xl(_shot_capture_config.odr_xl);
        }

        if (err == ERRNO_SUCCESS) {
            err = IMU_set_fs_g(_shot_capture_config.fs_g);
        }

        if (err == ERRNO_SUCCESS) {
            err = IMU_set_odr_g(_shot_capture_config.odr_g);
        }

        if (err == ERRNO_SUCCESS) {
            err = IMU_fifo_th_set(1800);
        }

        /* set tap threshold to be 4 * fs_xl/2^5 which is 250mg when fs_xl = 2g*/
        if (err == ERRNO_SUCCESS) {
            err = IMU_set_tap_ths(_shot_capture_config.tap_ths);
        }

        /* set the quiet time to be 4 * 1/ODR_XL
         * in the double-tap case, the quiet time window is the time during which a second tap is
         * ignored after the detection of the first tap (for debounce). When the lir bit is set to 0, the quiet time
         * also defines the length of the interrupt pulse (in both single and double-tap cases).
         * since we set lir to 1, the quiet time here has no effect.
         */
        if (err == ERRNO_SUCCESS) {
            err = IMU_set_quiet(_shot_capture_config.quiet_time);
        }

        /* set the shock time
         * the shock time window defines the maximum duration of the overthreshold event:
         * the acceleration must return below the threshold before the shock window has expired,
         * otherwise the tap event is not detected.
         */
        if (err == ERRNO_SUCCESS) {
            err = IMU_set_shock(_shot_capture_config.shock_time);
        }

        /* enable interrrupt for tap event */
        if (err == ERRNO_SUCCESS) {
            err = IMU_set_interrupts_enable(LSM6DS3TR_C_INTERRUPTS_ENABLE);
        }

        /* enable tap detection on x direction of imu*/
        if (err == ERRNO_SUCCESS) {
            err = IMU_set_tap_x_en(LSM6DS3TR_C_TAP_X_EN_ENABLED);
        }

        /* enable latched interrupt*/
        if (err == ERRNO_SUCCESS) {
            err = IMU_set_lir(LSM6DS3TR_C_LIR_ENABLED);
        }

        /**
         * Single-tap interrupt driven to INT1 pin
         * According to AN5130 In order to enable the latch feature on the single-tap interrupt
         * signal, both the LIR bit and the INT1_DOUBLE_TAP bit of MD1_CFG have to be set to 1:
         * the interrupt is kept active until the TAP_SRC register is read.
         */
        if (err == ERRNO_SUCCESS) {
            err = IMU_set_int1_single_tap(LSM6DS3TR_C_INT1_SINGLE_TAP_ENABLED);
        }

        if (err == ERRNO_SUCCESS) {
            err = IMU_set_int1_double_tap(LSM6DS3TR_C_INT1_DOUBLE_TAP_ENABLED);
        }

        if (err == ERRNO_SUCCESS) {
            atomic_set(&_shot_capture_is_enabled, 1);
        }

        PRINT(INFO, "SHOT_CAPTURE enabled.\r\n");
    } else if ((!enable || !_shot_capture_config.shot_capture_en || atomic_get(&_shot_storage_buf_is_in_use)) && atomic_get(&_shot_capture_is_enabled)) {
        /* Disable single-tap interrupt driven to INT1 pin */
        err = I2C_bytes_write(I2C_CH_1, LSM6DS3_I2C_DEV_ADDR, LSM6DS3TR_C_MD1_CFG, &reg_val, 1);
        
        /* Disable interrupts, lir, and tap detection on X, Y, Z axis */
        if (err == ERRNO_SUCCESS) {
            err = I2C_bytes_write(I2C_CH_1, LSM6DS3_I2C_DEV_ADDR, LSM6DS3TR_C_TAP_CFG, &reg_val, 1);
        }

        /* reads tap source register to clear the interrupt source */
        if (err == ERRNO_SUCCESS) {
            err = I2C_bytes_read(I2C_CH_1, LSM6DS3_I2C_DEV_ADDR, LSM6DS3TR_C_TAP_SRC, &reg_val, 1);
        }

        /* disable IMU fifo and FTH interrupt is disconnected to INT1 pin*/
        if (err == ERRNO_SUCCESS) {
            err = IMU_fifo_th_set(0);
        }

        /* if shot_capture_sem is non zero reset it because _shot_capture_task no longer needs to run */
        k_sem_reset(&shot_capture_sem);

        /* restore original accel and gyro settings */
        if (err == ERRNO_SUCCESS && _fs_xl_orig != _shot_capture_config.fs_xl) {
            err = IMU_set_fs_xl(_fs_xl_orig);
        }

        if (err == ERRNO_SUCCESS && _odr_xl_orig != _shot_capture_config.odr_xl) {
            err = IMU_set_odr_xl(_odr_xl_orig);
        }

        if (err == ERRNO_SUCCESS && _fs_g_orig != _shot_capture_config.fs_g) {
            err = IMU_set_fs_g(_fs_g_orig);
        }

        if (err == ERRNO_SUCCESS && _odr_g_orig != _shot_capture_config.odr_g) {
            err = IMU_set_odr_g(_odr_g_orig);
        }

        k_mutex_lock(&FIFO_ring_buf_mutex, K_FOREVER);
        FIFO_ring_buf.buf = NULL;
        k_mutex_unlock(&FIFO_ring_buf_mutex);

        /* disable shot capture interrupt mode */
        IMU_set_shot_capture_mode(false);

        atomic_set(&_shot_capture_is_enabled, 0);

        PRINT(INFO, "SHOT_CAPTURE disabled.\r\n");
    } else {
        err =  ERRNO_SUCCESS;
        goto out;
    }

out:   
    k_mutex_unlock(&shot_capture_enable_mutex);
    return err;
}

bool SHOT_CAPTURE_is_enabled(void)
{
    return atomic_get(&_shot_capture_is_enabled) ? true : false;
}

ERRNO_e SHOT_CAPTURE_save_shot_cnt(void)
{
    ERRNO_e err = ERRNO_SUCCESS;

    if (_user_shot_cnt_rt   != _shot_capture_config.user_shot_cnt ||
        _shot_cnt_rt        != _shot_capture_config.shot_cnt ||
        _shot_capture_en_rt != _shot_capture_config.shot_capture_en) {
        //_user_shot_cnt_rt   = _shot_capture_config.user_shot_cnt;
        //_shot_cnt_rt        = _shot_capture_config.shot_cnt;
        //_shot_capture_en_rt = _shot_capture_config.shot_capture_en;
        err = _shot_capture_config_write();
    }
    
    return err;
}

void SHOT_CAPTURE_task_install(void)
{
    static        k_tid_t           id;
    static struct k_thread          data;

    id = k_thread_create(&data,
                          _shot_capture_task_stack,
                          SHOT_CAPTURE_TASK_STACK_SIZE,
                          _shot_capture_task,
                          NULL,
                          NULL,
                          NULL,
                          SHOT_CAPTURE_THREAD_PRIORITY,
                          0,
                          K_NO_WAIT);
    k_thread_name_set(&data, "SHOT_CAPTURE_task");
}

void SHOT_CAPTURE_task_disable_and_wait(void)
{
    /* 1. Disable capture (serialized with FSM) */
    atomic_set(&_shot_storage_buf_is_in_use, 1);
    SHOT_CAPTURE_enable(false);

    /* 2. Request idle confirmation from _shot_capture_task */
    k_sem_reset(&shot_capture_idle_sem);
    atomic_set(&_shot_capture_idle_requested, 1);
    
    /* 
     * 3. Wake the capture task so it can see the idle request.
     *    If it's already waiting on shot_capture_sem, this
     *    lets it loop once and post the idle confirmation.
     */
    k_sem_give(&shot_capture_sem);
    
    /* 4. Wait for confirmation that capture task is idle */
    k_sem_take(&shot_capture_idle_sem, K_FOREVER);
    atomic_set(&_shot_capture_idle_requested, 0);
}

void SHOT_CAPTURE_task_resume(void)
{
    atomic_set(&_shot_storage_buf_is_in_use, 0);
}

void SHOT_CAPTURE_cli_cmd(int32_t argc, char **argv)
{
    if (argc == 0) {
        PRINT (INFO, "SHOT CAPTURE\r\n");
        PRINT (INFO, "-------------------------------------------\r\n");
        PRINT (INFO, "shot capture enabled: %s\r\n", SHOT_CAPTURE_is_enabled() ? "true" : "false");
    } else if (argc == 1) {
        if (!strcmp (argv[0], "-ps") || !strcmp (argv[0], "--printshot")) {
            _print_shot_data();
        } else if (!strcmp (argv[0], "-sc") || !strcmp (argv[0], "--startcapture")) {
            atomic_set(&_start_capture, 1);
            PRINT(INFO, "Manual start capture triggered.\r\n");
        } else if (!strcmp (argv[0], "-tw") || !strcmp (argv[0], "--testwrite")) {
            uint8_t *p_data;
            uint32_t test_size = 1000;
            uint32_t write_ptr_offset = 700; /* 70% into buffer to test wraparound */
            ERRNO_e err;

            /* 1. Get shared buffer from shot_storage */
            err = SHOT_STORAGE_get_buf(test_size, &p_data);
            if (err != ERRNO_SUCCESS) {
                PRINT(ERROR, "Failed to get buffer: %d\r\n", err);
                return;
            }

            /* 2. Fill buffer with sequential data starting at write_ptr offset.
             *    Stream cursor reads from write_ptr forward with wrapping:
             *    buf[700]=0, buf[701]=1, ..., buf[999]=299, buf[0]=300, ..., buf[699]=699
             */
            for (uint32_t i = 0; i < test_size; i++) {
                uint32_t buf_pos = (write_ptr_offset + i) % test_size;
                p_data[buf_pos] = (uint8_t)(i & 0xFF);
            }

            /* 3. Set up shot_data so file_create picks up the right values */
            k_mutex_lock(&shot_data_mutex, K_FOREVER);
            shot_data.start = write_ptr_offset;
            shot_data.size  = test_size;
            shot_data.time  = k_uptime_get_32();
            shot_data.ambient_light_intensity = 12345;
            shot_data.marked = true;
            shot_data.ready  = true;
            shot_data.saved  = false;
            k_mutex_unlock(&shot_data_mutex);

            /* 4. Create file and write to flash directly (synchronous) */
            err = SHOT_STORAGE_file_create();
            if (err != ERRNO_SUCCESS) {
                PRINT(ERROR, "Failed to create file: %d\r\n", err);
                return;
            }
            SHOT_STORAGE_write();

            /* 5. Fill buffer with new sequential data starting at write_ptr offset. */
            for (uint32_t i = 0; i < test_size; i++) {
                uint32_t buf_pos = (write_ptr_offset + i) % test_size;
                p_data[buf_pos] = (uint8_t)((i+10) & 0xFF);
            }

            /* 6. Reset shot_data flags */
            k_mutex_lock(&shot_data_mutex, K_FOREVER);
            shot_data.marked = false;
            shot_data.ready  = false;
            shot_data.saved  = true;
            k_mutex_unlock(&shot_data_mutex);

            PRINT(INFO, "Test write complete!\r\n");
            PRINT(INFO, "  Data size: %u bytes\r\n", test_size);
            PRINT(INFO, "  Write ptr (start): %u (tests buffer wraparound)\r\n", write_ptr_offset);
            PRINT(INFO, "  Expected content after header: 0,1,2,...,255,0,1,...,187 (repeating mod 256)\r\n");
            PRINT(INFO, "  Run 'shot_storage --read' to verify.\r\n");
        } else {
            goto USAGE;
        }
    } else if (argc == 2) {
        if (!strcmp(argv[0], "-e") || !strcmp(argv[0], "--enable")) {
            if (!strcmp(argv[1], "true")) {
                SHOT_CAPTURE_enable(true);
            } else if (!strcmp(argv[1], "false")) {
                SHOT_CAPTURE_enable(false);
            } else {
                goto USAGE;
            }
        } else if (!strcmp(argv[0], "-ss") || !strcmp(argv[0], "--setshock")) {
            if (!strcmp(argv[1], "0")) {
                SHOT_CAPTURE_shock_time_set(LSM6DS3TR_C_SHOCK_4_OVER_ODR_XL);
            } else if (!strcmp(argv[1], "1")) {
                SHOT_CAPTURE_shock_time_set(LSM6DS3TR_C_SHOCK_8_OVER_ODR_XL);
            } else if (!strcmp(argv[1], "2")) {
                SHOT_CAPTURE_shock_time_set(LSM6DS3TR_C_SHOCK_16_OVER_ODR_XL);
            } else if (!strcmp(argv[1], "3")) {
                SHOT_CAPTURE_shock_time_set(LSM6DS3TR_C_SHOCK_24_OVER_ODR_XL);
            } else {
                goto USAGE;
            }
        } else if (!strcmp(argv[0], "-sq") || !strcmp(argv[0], "--setquiet")) {
            if (!strcmp(argv[1], "0")) {
                SHOT_CAPTURE_quiet_time_set(LSM6DS3TR_C_QUIET_2_OVER_ODR_XL);
            } else if (!strcmp(argv[1], "1")) {
                SHOT_CAPTURE_quiet_time_set(LSM6DS3TR_C_QUIET_4_OVER_ODR_XL);
            } else if (!strcmp(argv[1], "2")) {
                SHOT_CAPTURE_quiet_time_set(LSM6DS3TR_C_QUIET_8_OVER_ODR_XL);
            } else if (!strcmp(argv[1], "3")) {
                SHOT_CAPTURE_quiet_time_set(LSM6DS3TR_C_QUIET_12_OVER_ODR_XL);
            } else {
                goto USAGE;
            }
        } else if (!strcmp(argv[0], "-sths") || !strcmp(argv[0], "--setthreshold")) {
            if (atoi(argv[1]) >= 0 && atoi(argv[1]) <= 31) {
                SHOT_CAPTURE_tap_ths_set((uint8_t)atoi(argv[1]));
            } else {
                goto USAGE;
            }
        } else {
            goto USAGE;
        }
    } else if (argc == 3) {
        if (!strcmp(argv[0], "-sdl") || !strcmp(argv[0], "--shotdurloc")) {
            uint8_t duration = (uint8_t)atoi(argv[1]);
            uint8_t location = (uint8_t)atoi(argv[2]);
            SHOT_CAPTURE_shotdata_duration_location_set(duration, location);
        } else {
            goto USAGE;
        }
    }
    else {
        USAGE:
        PRINT(INFO, "Usage: shot_capture [OPTION...]\r\n");
        PRINT(INFO, "  -ps | --printshot     Prints shot data from ring buffer\r\n");
        PRINT(INFO, "  -sc | --startcapture  Manually starts shot capture\r\n");
        PRINT(INFO, "  -tw | --testwrite     Writes test data to flash (use 'shot_storage --read' to verify)\r\n");
        PRINT(INFO, "  -e | --enable         Enables shot capture\r\n");
        PRINT(INFO, "    <%%b>               true  = enable\r\n");
        PRINT(INFO, "                        false = disable\r\n");
        PRINT(INFO, "  -ss | --setshock      Sets shock time\r\n");
        PRINT(INFO, "    <%%d>               0 = 4/ODR_XL 1 = 8/ODR_XL 2 = 16/ODR_XL 3 = 24/ODR_XL\r\n");
        PRINT(INFO, "  -sq | --setquiet      Sets quiet time\r\n");
        PRINT(INFO, "    <%%d>               0 = 2/ODR_XL 1 = 4/ODR_XL 2 = 8/ODR_XL 3 = 12/ODR_XL\r\n");
        PRINT(INFO, "  -sths | --setthreshold  Sets tap threshold\r\n");
        PRINT(INFO, "    <%%d>                 Tap threshold value (0-31)\r\n");
        PRINT(INFO, "  -sdl | --shotdurationandlocation  Sets shot duration and location\r\n");
        PRINT(INFO, "    <%%d>                           Shot duration\r\n");
        PRINT(INFO, "    <%%d>                           Shot location\r\n");
    }
}

/*--------------------------------------------------------------------------------------------------
 * PRIVATE FUNCTION DEFINITIONS
 *------------------------------------------------------------------------------------------------*/
static void _shot_capture_task(void *p1, void *p2, void *p3)
{
    uint16_t fifo_unread_words;
    uint8_t empty;
    uint8_t smart_full;
    uint8_t fifo_overrun;
    uint8_t fifo_fth_hit;
    uint8_t tap_src_reg;
    uint32_t fifo_unread_sample_set_in_bytes;
    int32_t overshoot;
    uint32_t bytes_to_read;
    uint32_t bytes_to_read_into_dummy = 0;
    uint32_t bytes_to_read_after_shot = 0;

    //timing_init();
    //timing_start();

    while(1) {
        /* Wait for IMU interrupt indefinitely*/
        int ret = k_sem_take(&shot_capture_sem, K_FOREVER);
        if (ret != 0) {
            /* if k_sem_reset is called ret is -EAGAIN, in that case just continue and wait for the next give */
            continue;
        }

        /* Check if someone requested us to confirm idle */
        if (atomic_get(&_shot_capture_idle_requested)) {
            k_sem_give(&shot_capture_idle_sem);
            /* Stay in loop, will block again on shot_capture_sem.
             * Since capture is disabled, no new IMU interrupts will come. */
            continue;
        }

        DBG_STAT_INC_SAFE(fifo_read_task_counter, stat_mutex);
        /* always check the single tap source register first to clear the interrupt source*/
        I2C_bytes_read(I2C_CH_1, LSM6DS3_I2C_DEV_ADDR, LSM6DS3TR_C_TAP_SRC, &tap_src_reg, 1);
        /**
         * Dectected shot and the ring buffer does not contain shot data then record current
         * shot into ring buffer
         */
        k_mutex_lock(&shot_data_mutex, K_FOREVER);
        k_mutex_lock(&FIFO_ring_buf_mutex, K_FOREVER);
        if (((tap_src_reg & 0x40) || atomic_get(&_start_capture)) && !shot_data.marked) {
            shot_data.saved = false;
            bytes_to_read_after_shot = shot_data.bytes_after_shot;
            DBG_STAT_INC_SAFE(read_shot_data_counter, stat_mutex);
            IMU_fifo_status_1to2_read(&fifo_unread_words, &empty, &smart_full, &fifo_overrun, &fifo_fth_hit);
            /**
             * each sample set has shot_data.words_per_sample_set 16-bit samples depending on the
             * odr of acclerometer and gyroscope
             * we always work on the integer # of sample set in bytes
             */
            fifo_unread_sample_set_in_bytes = (fifo_unread_words / shot_data.words_per_sample_set) * shot_data.bytes_per_sample_set;
            /* shot data is at the end of the IMU FIFO */
            _read_shot_data_into_ring_buf(fifo_unread_sample_set_in_bytes, true);

            ///* disable shot data interrupt*/
            //I2C_bytes_write (I2C_CH_1, LSM6DS3_I2C_DEV_ADDR, LSM6DS3TR_C_MD1_CFG, (uint8_t[]){0}, 1);
        } else {
            /**
             * 1. no shot detected and shot data is not marked in the ring buffer F,T
             * 2. no shot detected and shot data is marked in the ring buffer F,F
             * 3. shot detected and shot data is marked in the ring buffer T,F
             */
            DBG_STAT_INC_SAFE(fifo_waterM_total_hit_counter, stat_mutex);
            IMU_fifo_status_1to2_read(&fifo_unread_words, &empty, &smart_full, &fifo_overrun, &fifo_fth_hit);
            if (fifo_fth_hit) {
                DBG_STAT_INC_SAFE(fifo_waterM_hit_counter, stat_mutex);
                DBG_STAT_INC_IF_SAFE(fifo_overrun, fifo_overrun_counter, stat_mutex);
                DBG_STAT_TRACK_VALUE_SAFE(fifo_unread_words, unread_words_before_read, stat_mutex);
                DBG_STAT_TRACK_MAX_SAFE(fifo_unread_words, max_unread_words_before_read, stat_mutex);
                fifo_unread_sample_set_in_bytes = (fifo_unread_words / shot_data.words_per_sample_set) * shot_data.bytes_per_sample_set;
                /**
                * shot data is ready but has not been written completely into the flash
                * dump shot data into a dummy buffer to not to cause corruption in the ring buffer
                */
                if (shot_data.ready == true) {
                    DBG_STAT_INC_SAFE(read_into_dummy_counter, stat_mutex);
                    IMU_set_fifo_mode(LSM6DS3TR_C_FIFO_MODE_BYPASS); /* reset the fifo content */
                    IMU_set_fifo_mode(LSM6DS3TR_C_FIFO_MODE_CONTINUOUS);
                    //I2C_bytes_read(I2C_CH_1, LSM6DS3_I2C_DEV_ADDR, LSM6DS3TR_C_FIFO_DATA_OUT_L, &_dummy_buf[0], fifo_unread_sample_set_in_bytes);
                } else {
                    DBG_STAT_INC_SAFE(fifo_read_FTH_counter, stat_mutex);
                    if (shot_data.marked == true) {
                        /**
                        * Takes care of scenario 2 and 3. Read FIFO high watermark data to specified amount
                        * defined in shot_data.bytes_after_shot
                        */
                        if (bytes_to_read_after_shot > fifo_unread_sample_set_in_bytes) {
                            bytes_to_read = fifo_unread_sample_set_in_bytes;
                            bytes_to_read_after_shot =  bytes_to_read_after_shot - fifo_unread_sample_set_in_bytes;
                        } else {
                            if (bytes_to_read_after_shot < fifo_unread_sample_set_in_bytes) {
                                bytes_to_read_into_dummy = fifo_unread_sample_set_in_bytes - bytes_to_read_after_shot;
                            }
                            bytes_to_read = bytes_to_read_after_shot;
                            shot_data.ready = true;
                        }
                        overshoot = FIFO_ring_buf.write_ptr + bytes_to_read - shot_data.size;
                        _read_FIFO_into_ring_buf(overshoot, bytes_to_read);

                        if (bytes_to_read_into_dummy) {
                            /* blocking read */
                            IMU_set_fifo_mode(LSM6DS3TR_C_FIFO_MODE_BYPASS); /* reset the fifo content */
                            IMU_set_fifo_mode(LSM6DS3TR_C_FIFO_MODE_CONTINUOUS);
                            //I2C_bytes_read(I2C_CH_1, LSM6DS3_I2C_DEV_ADDR, LSM6DS3TR_C_FIFO_DATA_OUT_L, &_dummy_buf[0], bytes_to_read_into_dummy);
                            bytes_to_read_into_dummy = 0;
                        }

                        if (shot_data.ready) {
                            shot_data.start = FIFO_ring_buf.write_ptr;
                            SHOT_STORAGE_begin_write();
                        }
                        IMU_fifo_status_1to2_read(&fifo_unread_words, &empty, &smart_full, &fifo_overrun, &fifo_fth_hit);
                        DBG_STAT_TRACK_VALUE_SAFE(fifo_unread_words, unread_words_after_read, stat_mutex);
                        DBG_STAT_TRACK_MAX_SAFE(fifo_unread_words, max_unread_words_after_read, stat_mutex);
                    } else {
                        /* takes care of scenario 1 and read FIFO high watermark data*/
                        if (NULL != FIFO_ring_buf.buf) {
                            /* break it into 2 reads if this read is going to exceed the ring buffer size */
                            overshoot = FIFO_ring_buf.write_ptr + fifo_unread_sample_set_in_bytes - shot_data.size;
                            _read_FIFO_into_ring_buf(overshoot, fifo_unread_sample_set_in_bytes);
                            /* debug*/
                            IMU_fifo_status_1to2_read(&fifo_unread_words, &empty, &smart_full, &fifo_overrun, &fifo_fth_hit);
                            DBG_STAT_TRACK_VALUE_SAFE(fifo_unread_words, unread_words_after_read, stat_mutex);
                            DBG_STAT_TRACK_MAX_SAFE(fifo_unread_words, max_unread_words_after_read, stat_mutex);

                            /* check if the shot motion happens when during the i2c transfer of FIFO high watermark data*/
                            I2C_bytes_read(I2C_CH_1, LSM6DS3_I2C_DEV_ADDR, LSM6DS3TR_C_TAP_SRC, &tap_src_reg, 1);

                            if ((tap_src_reg & 0x40) || atomic_get(&_start_capture)) {
                                shot_data.saved = false;
                                DBG_STAT_INC_SAFE(read_shot_data_after_fifo_read_counter, stat_mutex);
                                IMU_fifo_status_1to2_read(&fifo_unread_words, &empty, &smart_full, &fifo_overrun, &fifo_fth_hit);
                                fifo_unread_sample_set_in_bytes = (fifo_unread_words / shot_data.words_per_sample_set) * shot_data.bytes_per_sample_set; /* convert integer # of sample set to bytes*/
                                /* shot data is not necessarily at the end of IMU FIFO*/
                                _read_shot_data_into_ring_buf(fifo_unread_sample_set_in_bytes, false);
                                k_sem_take(&shot_capture_sem, K_NO_WAIT);
                            }
                        } else {
                            IMU_set_fifo_mode(LSM6DS3TR_C_FIFO_MODE_BYPASS); /* reset the fifo content */
                            IMU_set_fifo_mode(LSM6DS3TR_C_FIFO_MODE_CONTINUOUS);
                            //I2C_bytes_read(I2C_CH_1, LSM6DS3_I2C_DEV_ADDR, LSM6DS3TR_C_FIFO_DATA_OUT_L, &_dummy_buf[0], fifo_unread_sample_set_in_bytes);
                            /* debug*/
                            IMU_fifo_status_1to2_read(&fifo_unread_words, &empty, &smart_full, &fifo_overrun, &fifo_fth_hit);
                            DBG_STAT_TRACK_VALUE_SAFE(fifo_unread_words, unread_words_after_read, stat_mutex);
                            DBG_STAT_TRACK_MAX_SAFE(fifo_unread_words, max_unread_words_after_read, stat_mutex);
                        }
                    }
                }
            }
        }
        atomic_set(&_start_capture, 0);
        k_mutex_unlock(&FIFO_ring_buf_mutex);
        k_mutex_unlock(&shot_data_mutex);
    }
}

    // start_time = timing_counter_get();
    // busy_wait_active = true;
    // k_busy_wait(300000);
    // busy_wait_active = false;
    // end_time = timing_counter_get();
    // cycles_with_dma = timing_cycles_get(&start_time, &end_time);

static ERRNO_e _read_shot_data_into_ring_buf(uint32_t fifo_unread_sample_set_in_bytes, bool end)
{
    ERRNO_e err;
    int32_t overshoot;
    uint32_t shot_location;
    uint32_t unread_sample_set;
    uint32_t unread_sample_set_in_half;
    
    ALS_read((ALS_data_s *) &shot_data.ambient_light_intensity);
    shot_data.time = k_uptime_get_32();

    /* determine the location of shot_data in the ring buffer */
    if (end == false) {
        /* if shot happens during reading high watermark data, we guess the shot data happens in the middle */
        unread_sample_set = fifo_unread_sample_set_in_bytes / shot_data.bytes_per_sample_set;
        if (unread_sample_set == 1) {
            unread_sample_set_in_half = 1;
        } else {
            unread_sample_set_in_half = unread_sample_set / 2;
        }
        overshoot = FIFO_ring_buf.write_ptr + (unread_sample_set_in_half * shot_data.bytes_per_sample_set) - shot_data.size;
    } else {
        overshoot = FIFO_ring_buf.write_ptr + fifo_unread_sample_set_in_bytes - shot_data.size;
    }

    if (overshoot < 0) {
        shot_data.location = FIFO_ring_buf.write_ptr + fifo_unread_sample_set_in_bytes - shot_data.bytes_per_sample_set;
    } else {
        if (overshoot > 0) {
            shot_data.location = overshoot - shot_data.bytes_per_sample_set;

        } else {
            shot_data.location = shot_data.size - shot_data.bytes_per_sample_set;
        }
    }

    /* break it into 2 reads if this read is going to exceed the buffer size*/
    overshoot = FIFO_ring_buf.write_ptr + fifo_unread_sample_set_in_bytes - shot_data.size;
    err = _read_FIFO_into_ring_buf(overshoot, fifo_unread_sample_set_in_bytes);

    shot_data.marked = true;

    return err;
}

static ERRNO_e _read_FIFO_into_ring_buf(int32_t overshoot, uint32_t bytes_to_read)
{
    ERRNO_e err;

    if (overshoot < 0) {
        err = I2C_bytes_read(I2C_CH_1, LSM6DS3_I2C_DEV_ADDR, LSM6DS3TR_C_FIFO_DATA_OUT_L, &(FIFO_ring_buf.buf[FIFO_ring_buf.write_ptr]), bytes_to_read);
        FIFO_ring_buf.write_ptr = FIFO_ring_buf.write_ptr + bytes_to_read;
    } else if (overshoot > 0) {
        err = I2C_bytes_read(I2C_CH_1, LSM6DS3_I2C_DEV_ADDR, LSM6DS3TR_C_FIFO_DATA_OUT_L, &(FIFO_ring_buf.buf[FIFO_ring_buf.write_ptr]), (shot_data.size - FIFO_ring_buf.write_ptr));
        err = I2C_bytes_read(I2C_CH_1, LSM6DS3_I2C_DEV_ADDR, LSM6DS3TR_C_FIFO_DATA_OUT_L, &(FIFO_ring_buf.buf[0]), overshoot);
        FIFO_ring_buf.write_ptr = overshoot;
        FIFO_ring_buf.loop_ctr += 1;
    } else {
        err = I2C_bytes_read(I2C_CH_1, LSM6DS3_I2C_DEV_ADDR, LSM6DS3TR_C_FIFO_DATA_OUT_L, &(FIFO_ring_buf.buf[FIFO_ring_buf.write_ptr]), bytes_to_read);
        FIFO_ring_buf.write_ptr = 0;
        FIFO_ring_buf.loop_ctr += 1;
    }

    return err;
}

static void _print_shot_data(void)
{
    uint32_t read_ptr;
    bool is_shot_data_marked;
    bool is_shot_data_ready;
    bool is_shot_data_saved;
    uint32_t size;

    k_mutex_lock(&shot_data_mutex, K_FOREVER);
    is_shot_data_marked = shot_data.marked;
    is_shot_data_ready  = shot_data.ready;
    is_shot_data_saved  = shot_data.saved;
    size                = shot_data.size;
    read_ptr            = shot_data.start;
    k_mutex_unlock(&shot_data_mutex);

    if (is_shot_data_ready && FIFO_ring_buf.buf != NULL) {
        /* print each sample set in G GyroX, GyroY, GyroZ, AccX, AccY, AccZ format */
        PRINT(INFO, "%d\r\n", LSM6DS3TR_C_FS_XL_TO_G(_shot_capture_config.fs_xl));
        PRINT(INFO, "%d\r\n", LSM6DS3TR_C_ODR_XL_TO_HZ(_shot_capture_config.odr_xl));
        PRINT(INFO, "GyroX, GyroY, GyroZ, AccX, AccY, AccZ\r\n");
        for (int i = 0; i < size; i += 12) {
            PRINT (INFO, "%d,%d,%d,%d,%d,%d\r\n", *((int16_t *) &FIFO_ring_buf.buf[read_ptr]), \
                                                  *((int16_t *) &FIFO_ring_buf.buf[read_ptr + 2]), \
                                                  *((int16_t *) &FIFO_ring_buf.buf[read_ptr + 4]), \
                                                  *((int16_t *) &FIFO_ring_buf.buf[read_ptr + 6]), \
                                                  *((int16_t *) &FIFO_ring_buf.buf[read_ptr + 8]), \
                                                  *((int16_t *) &FIFO_ring_buf.buf[read_ptr + 10]));
            if ((read_ptr + 10) == (size - 2)) {
                read_ptr = 0;
            } else {
                read_ptr += 12;
            }
            /* prevent watchdog timeout due to slow print on serial port over uart*/
            WDT_feed (); 
        }
    
    k_mutex_lock(&shot_data_mutex, K_FOREVER);
    shot_data.marked = false;
    shot_data.ready  = false;
    shot_data.saved  = true;
    k_mutex_unlock(&shot_data_mutex);
    } else {
        PRINT (INFO, "Shot data is not ready!\r\n");
    }
}

static void _shot_capture_config_read(void)
{
    ERRNO_e               err;
    SHOT_CAPTURE_config_s cfg;

    err = FS_open_or_create(SHOT_CAPTURE_FILENAME, &_shot_capture_file, sizeof(SHOT_CAPTURE_config_s), FS_FILE_DEPTH_DEFAULT);
    if (err == ERRNO_SUCCESS) {
        /* Read file. */
        err = FS_read(_shot_capture_file, (uint8_t *) &cfg, 0, sizeof(SHOT_CAPTURE_config_s));

        /* Check file entry is valid */
        if (err != ERRNO_SUCCESS) {
            /* write defaults to file */
            _shot_capture_config_write();
            return;
        }

        /* Check file entry is valid. */
        if (cfg.shot_duration < 1 || cfg.shot_duration > 10 ||
            cfg.shot_location < 0 || cfg.shot_location >= cfg.shot_duration ||
            cfg.shot_duration * 3 * 2 * (LSM6DS3TR_C_ODR_G_TO_HZ(cfg.odr_g) + LSM6DS3TR_C_ODR_XL_TO_HZ(cfg.odr_xl)) > _SHOT_STORAGE_FILE_SIZE) {
            PRINT(ERROR, "Invalid shot_duration in shot capture configuration file. Writing defaults.\r\n");
            _shot_capture_config_write();
            return;
        }

        /* Update shot capture configuration to what was read from flash. */
        _shot_capture_config = cfg;
        if (_shot_capture_config.shot_duration != SHOTDATA_DURATION_DEFAULT ||
            _shot_capture_config.shot_location != SHOTDATA_LOCATION_DEFAULT ||
            _shot_capture_config.odr_g  != LSM6DS3TR_C_ODR_G_416Hz ||
            _shot_capture_config.odr_xl != LSM6DS3TR_C_ODR_XL_416Hz) {
            _shot_capture_shotdata_param_cal();
        }
    } else if (err == ERRNO_SIZE_MISMATCH) {
        /* Config has been modified, need to delete old file and write new configuration. */
        err = FS_delete(SHOT_CAPTURE_FILENAME);

        if (err != ERRNO_SUCCESS) {
            /* Something else is going on... */
            PRINT(ERROR, "Failure to delete old shot capture file.\r\n");
            return;
        }

        /* Create new file. */
        err = FS_open_or_create (SHOT_CAPTURE_FILENAME, &_shot_capture_file, sizeof (SHOT_CAPTURE_config_s), FS_FILE_DEPTH_DEFAULT);
        if (err != ERRNO_SUCCESS) {
            PRINT(ERROR, "Failure to create shot capture configuration file.\r\n");
        }
        else {
            PRINT(INFO, "New shot capture created successfully\r\n");
        }

        /* Write default config to file. */
        _shot_capture_config_write();
    } else {
        /* write default */
        _shot_capture_config_write();
    }
}

static ERRNO_e _shot_capture_config_write(void)
{
    ERRNO_e err;

    err = FS_write(_shot_capture_file, (uint8_t *) &_shot_capture_config);
    if (err != ERRNO_SUCCESS) {
        PRINT(ERROR, "Failed to write shot capture configuration to file.\r\n");        
    }
    return err;
}

static void _shot_capture_shotdata_param_cal(void)
{
    uint32_t sample_set_odr_Hz;

    sample_set_odr_Hz = LSM6DS3TR_C_ODR_XL_TO_HZ(MIN((uint8_t)_shot_capture_config.odr_g, (uint8_t)_shot_capture_config.odr_xl));

    k_mutex_lock(&shot_data_mutex, K_FOREVER);
    shot_data.words_per_sample_set = ((LSM6DS3TR_C_ODR_G_TO_HZ(_shot_capture_config.odr_g) / sample_set_odr_Hz) +
                                     (LSM6DS3TR_C_ODR_XL_TO_HZ(_shot_capture_config.odr_xl) / sample_set_odr_Hz)) * 3;
    shot_data.bytes_per_sample_set = shot_data.words_per_sample_set * 2;
    shot_data.size                 = _shot_capture_config.shot_duration * sample_set_odr_Hz * shot_data.bytes_per_sample_set;
    shot_data.bytes_after_shot     = (_shot_capture_config.shot_duration - _shot_capture_config.shot_location) * sample_set_odr_Hz * shot_data.bytes_per_sample_set;
    k_mutex_unlock(&shot_data_mutex);
}
