/** 
 * @file    fsm.c
 * @brief   Finite State Machine
 * @author  Kenny Tran
 * @date    July 13 2022
 * @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 <math.h>

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

#include "../wrappers/uart.h"
#include "../wrappers/spi.h"
#include "../wrappers/pwm.h"
#include "../wrappers/i2c.h"

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

#include "../bsp/als.h"
#include "../bsp/battery.h"
#include "../bsp/button.h"
#include "../bsp/mcu.h"
#include "../bsp/power.h"
#include "../bsp/nvm_ext.h"
#include "../bsp/imu.h"
#include "../bsp/wdt.h"

#include "../app/app.h"
#include "../app/app_cli.h"
#include "../app/ble.h"
#include "../app/bubble.h"
#include "../app/fiber.h"
#include "../app/event.h"
#include "../app/light_sensor.h"
#include "../app/pin.h"
#include "../app/motion.h"
#include "../app/scope_ring.h"
#include "../app/shot_capture.h"

#include "fsm.h"


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


/*--------------------------------------------------------------------------------------------------
 * THREAD DEFINITIONS
 *------------------------------------------------------------------------------------------------*/
K_THREAD_STACK_DEFINE(_fsm_stack, FSM_STACK_SIZE);


/*--------------------------------------------------------------------------------------------------
 * TYPE DEFINITIONS & ENUMERATIONS
 *------------------------------------------------------------------------------------------------*/
typedef enum
{
    _FSM_EVT_SUBCONFIG_BUBBLE_BRIGHTNESS = 0,
    _FSM_EVT_SUBCONFIG_PIN_BRIGHTNESS,
    _FSM_EVT_SUBCONFIG_FIBER_BRIGHTNESS,
    _FSM_EVT_SUBCONFIG_RING_BACKGROUND_COLOR,
    _FSM_EVT_SUBCONFIG_RING_BACKGROUND_BRIGHTNESS,
    _FSM_EVT_SUBCONFIG_RING_LEVEL_COLOR,
    _FSM_EVT_SUBCONFIG_RING_LEVEL_BRIGHTNESS,
    _FSM_EVT_SUBCONFIG_RING_INDICATORS_COLOR,
    _FSM_EVT_SUBCONFIG_RING_INDICATORS_BRIGHTNESS,
    _FSM_EVT_SUBCONFIG_LEVEL_SENSITIVITY,
    _FSM_EVT_SUBCONFIG_LEVEL_OPTION,
} _fsm_evt_subconfig_e;

typedef enum
{
    _FSM_EVT_SUBCAL_SET_FLAT = 0,
    _FSM_EVT_SUBCAL_FLAT,
    _FSM_EVT_SUBCAL_SET_BACK,
    _FSM_EVT_SUBCAL_BACK,
    _FSM_EVT_SUBCAL_SET_FRONT,
    _FSM_EVT_SUBCAL_FRONT,
} _fsm_evt_subcal_e;


/*--------------------------------------------------------------------------------------------------
 * PRIVATE CONSTANTS & VARIABLES
 *------------------------------------------------------------------------------------------------*/
static struct k_thread      _fsm_thread;
static FS_file_t            _fsm_file;
static FSM_e                _fsm_state = FSM_INIT;
static bool                 _fsm_state_entry = false;
static uint32_t             _fsm_timeout_cnt = 0;
static FSM_config_s         _fsm_config = (FSM_config_s) {
    .motion_timeout_ms = FSM_MOTION_TIMEOUT_PERIOD_MS,
    .auto_brightness   = false,
    .mode              = FSM_MODE_TARGET
};
static bool                 _fsm_evt_busy      = false;
static _fsm_evt_subconfig_e _fsm_evt_subconfig = _FSM_EVT_SUBCONFIG_BUBBLE_BRIGHTNESS;
static _fsm_evt_subcal_e    _fsm_evt_subcal    = _FSM_EVT_SUBCAL_SET_FLAT;
static uint32_t             _fsm_motion_timer  = 0;
static bool                 _fsm_app_config    = false;
static float                _fsm_batt_life     = 0.0;


/*--------------------------------------------------------------------------------------------------
 * PRIVATE FUNCTION PROTOTYPES
 *------------------------------------------------------------------------------------------------*/
static void _fsm_task (void *p1, void *p2, void *p3);
static void _fsm_config_read (void);
static void _fsm_config_write (void);
static void _fsm_state_set (FSM_e state);
static void _fsm_evt_handler (EVENT_e event);
static void _fsm_auto_brightness (bool enabled);
static void _fsm_low_power_enter (bool wake_on_motion, bool wake_on_charge);
static void _fsm_battery_read (void);


/*--------------------------------------------------------------------------------------------------
 * PUBLIC FUNCTION DEFINITIONS
 *------------------------------------------------------------------------------------------------*/
void FSM_init (void)
{
    PRINT (INFO, "Initializing FSM...\r\n");

    /* Fetch FSM state from RAM if stored. */
    if (MCU_ram_fetch ((uint8_t *) &_fsm_state, sizeof (_fsm_state)) != ERRNO_SUCCESS)
    {
        _fsm_state = FSM_INIT;
    }

    /* Initialize battery module. */
    if (BATTERY_init () < ERRNO_SUCCESS)
    {
        PRINT (ERROR, "Failure to initialize Battery.\r\n");
    }

    /* Initialize 5V step-up converter. */
    if (POWER_init () < ERRNO_SUCCESS)
    {
        PRINT (ERROR, "Failure to initialize Power.\r\n");
    }

    /* Fetch configuration settings stored for fsm. */
    _fsm_config_read ();
}

void FSM_level_calibrate (void)
{
    /* Force event calibration. */
    EVENT_force (EVENT_SUBCAL_START);
}

void FSM_task_suspend (void)
{
    k_thread_suspend (&_fsm_thread);
}

void FSM_task_resume (void)
{
    k_thread_resume (&_fsm_thread);
}

void FSM_task_install (void)
{
    static k_tid_t id;

    id = k_thread_create (&_fsm_thread,
                          _fsm_stack,
                          FSM_STACK_SIZE,
                          _fsm_task,
                          NULL,
                          NULL,
                          NULL,
                          FSM_THREAD_PRIORITY,
                          0,
                          K_NO_WAIT);
    k_thread_name_set(&_fsm_thread, "FSM_task");                      
}

void FSM_timout_set (uint32_t ms)
{
    if (ms < 1000)
    {
        PRINT (ERROR, "Minimum timeout is 1000 ms.\r\n");
    }

    _fsm_config.motion_timeout_ms = ms;

    /* Write new settings to file. */
    _fsm_config_write ();
}

uint32_t FSM_timout_get (void)
{
    return _fsm_config.motion_timeout_ms;
}

void FSM_auto_brightness_set (bool enabled)
{
    _fsm_config.auto_brightness = enabled;

    /* Write new settings to file. */
    _fsm_config_write ();

    BLE_notify (BLE_CHAR_FSM_AUTO_BRIGHTNESS, &enabled, sizeof (enabled));
}

bool FSM_auto_brightness_get (void)
{
    return _fsm_config.auto_brightness;
}

void FSM_mode_set (FSM_MODE_e mode)
{
    _fsm_config.mode = (uint8_t) mode;

    /* Write new settings to file. */
    _fsm_config_write ();

    BLE_notify (BLE_CHAR_FSM_MODE, &mode, sizeof (mode));
}

FSM_MODE_e FSM_mode_get (void)
{
    return (FSM_MODE_e) _fsm_config.mode;
}

void FSM_app_config_set (bool val)
{
    _fsm_app_config = val;
}

bool FSM_app_config_get (void)
{
    return _fsm_app_config;
}

float FSM_battery_life_get (void)
{
    return _fsm_batt_life;
}

void FSM_cli_cmd (int32_t argc, char **argv)
{
    if (argc == 0)
    {
        PRINT (INFO, "FSM\r\n");
        PRINT (INFO, "-------------------------------------------\r\n");
        PRINT (INFO, "state                : %d\r\n", _fsm_state);
        PRINT (INFO, "timeout (ms)         : %d\r\n", _fsm_config.motion_timeout_ms);
        PRINT (INFO, "timeout counter (ms) : %d\r\n", _fsm_timeout_cnt);
        PRINT (INFO, "auto brightness      : %s\r\n", (_fsm_config.auto_brightness ? "true" : "false"));
        PRINT (INFO, "mode                 : %d\r\n", _fsm_config.mode);
        PRINT (INFO, "app config           : %s\r\n", (_fsm_app_config ? "true" : "false"));
    }
    else if (argc == 1)
    {
        if (!strcmp (argv[0], "--list"))
        {
            PRINT (INFO, "  0 = FSM_INIT\r\n");
            PRINT (INFO, "  1 = FSM_CHARGING\r\n");
            PRINT (INFO, "  2 = FSM_LOW_BATTERY\r\n");
            PRINT (INFO, "  3 = FSM_POWER_ON\r\n");
            PRINT (INFO, "  4 = FSM_IDLE\r\n");
            PRINT (INFO, "  5 = FSM_SLEEP\r\n");
            PRINT (INFO, "  6 = FSM_3D_SLEEP\r\n");
            PRINT (INFO, "  7 = FSM_CHARGER_CONNECTED\r\n");
            PRINT (INFO, "  8 = FSM_CHARGER_DISCONNECTED\r\n");
            PRINT (INFO, "  9 = FSM_LEVELING\r\n");
            PRINT (INFO, " 10 = FSM_POWER_OFF\r\n");
            PRINT (INFO, " 11 = FSM_OFF\r\n");
            PRINT (INFO, " 12 = FSM_BATTERY_FULL\r\n");
            PRINT (INFO, " 13 = FSM_BLE_ENABLE\r\n");
            PRINT (INFO, " 14 = FSM_BLE_DISABLE\r\n");
            PRINT (INFO, " 15 = FSM_RAINBOW\r\n");
            PRINT (INFO, " 16 = FSM_FLAT_CALIBRATE\r\n");
            PRINT (INFO, " 17 = FSM_BACK_CALIBRATE\r\n");
            PRINT (INFO, " 18 = FSM_FRONT_CALIBRATE\r\n");
            PRINT (INFO, "255 = FSM_DEBUG\r\n");
        }
        else
        {
            goto USAGE;
        }
    }
    else if (argc == 2)
    {
        if (!strcmp (argv[0], "-s"))
        {
            FSM_e state = (FSM_e) strtoul (argv[1], NULL, 0);

            _fsm_state_set (state);
        }
        else if (!strcmp (argv[0], "-m"))
        {
            FSM_MODE_e mode = (FSM_MODE_e) strtoul (argv[1], NULL, 0);

            FSM_mode_set (mode);
        }
        else if (!strcmp (argv[0], "-ab"))
        {
            if (!strcmp (argv[1], "true"))
            {
                FSM_auto_brightness_set (true);
            }
            else if (!strcmp (argv[1], "false"))
            {
                FSM_auto_brightness_set (false);
            }
            else
            {
                goto USAGE;
            }
        }
        else if (!strcmp (argv[0], "-to") || !strcmp (argv[0], "--timeout"))
        {
            uint32_t timeout_ms = strtoul (argv[1], NULL, 0);

            FSM_timout_set (timeout_ms);
        }
        else if (!strcmp (argv[0], "-ac") || !strcmp (argv[0], "--appcfg"))
        {
            if (!strcmp (argv[1], "true"))
            {
                FSM_app_config_set (true);
            }
            else if (!strcmp (argv[1], "false"))
            {
                FSM_app_config_set (false);
            }
            else
            {
                goto USAGE;
            }
        }
        else
        {
            goto USAGE;
        }
    }
    else
    {
USAGE:
        PRINT (INFO, "Usage: fsm [OPTION...]\r\n");
        PRINT (INFO, "  --list             Lists all FSM states\r\n");
        PRINT (INFO, "  -s                 Sets new FSM state\r\n");
        PRINT (INFO, "    <%%d>             [0 - 18, 255]\r\n");
        PRINT (INFO, "  -m                 Sets FSM mode\r\n");
        PRINT (INFO, "    <%%d>             [0 - target, 1 - 3D]\r\n");
        PRINT (INFO, "  -ab                Sets auto brightness\r\n");
        PRINT (INFO, "    <%%b>             true = enabled, false = disabled\r\n");
        PRINT (INFO, "  -to | --timeout    Enables fiber LED\r\n");
        PRINT (INFO, "    <%%d>             timeout in milliseconds\r\n");
        PRINT (INFO, "  -ac | --appcfg     Enables BLE app config\r\n");
        PRINT (INFO, "    <%%b>             true = enabled, false = disabled\r\n");
    }
}


/*--------------------------------------------------------------------------------------------------
 * PRIVATE FUNCTION DEFINITIONS
 *------------------------------------------------------------------------------------------------*/
static void _fsm_task (void *p1, void *p2, void *p3)
{
           EVENT_e              event;
           bool                 state_finished;
    static bool                 in_use           = false;
    static uint32_t             timer_60s        = 0;
    static FSM_e                prev_state       = FSM_INIT;
    static uint32_t             batt_low_timeout = 0;
    static uint32_t             timer_3s         = 0;

    /* On startup, read the battery before turning on LEDs (causing voltage drop). */
    _fsm_battery_read ();

    while (1)
    {
        DBG_STAT_INC(fsm_task_counter);
        DBG_STAT_INC_IF(i2c_transfer_active,fsm_task_during_i2c_counter);
        /* Check for events. */
        event = EVENT_last_get ();

        /* Handle any events from event. */
        _fsm_evt_handler (event);

        /* Process if event occurs, else go into state machine. */
        if (_fsm_evt_busy)
        {
            /* If we are in config and stuck at one state for too long, exit. */
            if (_fsm_timeout_cnt >= FSM_CONFIG_TIMEOUT_PERIOD_MS)
            {
                /* Force exit once we've hit timeout. */
                _fsm_evt_handler (EVENT_CONFIG_EXIT);

                /* Reset event so we are no longer in subconfig. */
                EVENT_reset ();

                /* Reset timeout counter. */
                _fsm_timeout_cnt = 0;
            }
        }
        else
        {
            if (in_use)
            {
                /* Indicate battery status every 60 seconds if in use. */
                if (timer_60s == 0)
                {
                    if (!BATTERY_is_charging ())
                    {
                        /* Don't let it get below 10 %. */
                        if (_fsm_batt_life <= (float) 0.10)
                        {
                            PRINT (INFO, "Dead battery. %f\r\n");
                            _fsm_state_set (FSM_POWER_OFF);
                        }
                        else if (_fsm_batt_life <= (float) 0.20)
                        {
                            PRINT (INFO, "Low battery.\r\n");
                            /* Calling _fsm_state_set() resets timer.  If timeout matches battery check interval,
                            we go to FSM_LOW_BATTERY to blink status then return to operation.  If timer resets,
                            we keep switching states and end up in a loop. */
                            _fsm_state = FSM_LOW_BATTERY;
                        }
                    }

                    /* Increment timer. */
                    timer_60s += FSM_TASK_PERIOD_MS;
                }
                else if (timer_60s >= 60000)
                {
                    /* Reset timer. */
                    timer_60s = 0;
                }
                else
                {
                    /* Increment timer. */
                    timer_60s += FSM_TASK_PERIOD_MS;
                }
            }

            /* Get status of scope ring state. */
            (void) SCOPE_RING_state_get (&state_finished);

            switch (_fsm_state)
            {
                case FSM_INIT:
                    if (BATTERY_is_charging ())
                    {
                        _fsm_state_set (FSM_CHARGING);
                    }
                    else
                    {
                        /* If configured for 3D, go straight to leveling to save time. */
                        if (_fsm_config.mode == FSM_MODE_3D)
                        {
                            _fsm_state_set (FSM_LEVELING);
                        }
                        else
                        {
                            _fsm_state_set (FSM_POWER_ON);
                        }
                    }
                    break;

                case FSM_CHARGING:
                {
                    /* _fsm_timeout_cnt is reset every state change.  Use as a flag for one time entry. */    
                    if (_fsm_state_entry)
                    {
                        /* Enable 5v */
                        POWER_enable (true);

                        /* Disable other LEDs. */
                        SHOT_CAPTURE_enable (false);
                        BUBBLE_enable (false);
                        FIBER_enable (false);
                        PIN_enable (false);
                    
                        SCOPE_RING_state_set (SCOPE_RING_STATE_CHARGING, FSM_CHARGING_BLINK_PERIOD_MS);
                    }
                }
                break;

                case FSM_LOW_BATTERY:
                    if (batt_low_timeout >= FSM_LOW_BATTERY_BLINK_PERIOD_MS * 5)
                    {
                        if (in_use)
                        {
                            /* Calling _fsm_state_set() resets timer.  If timeout matches battery check interval,
                               we go to FSM_LOW_BATTERY to blink status then return to operation.  If timer resets,
                               we keep switching states and end up in a loop. */
                            _fsm_state = FSM_LEVELING;
                        }
                        else
                        {
                            _fsm_state_set (FSM_IDLE);
                        }

                        /* Reset timeout count. */
                        batt_low_timeout = 0;
                    }
                    else
                    {
                        static uint8_t bubble_brightness;

                        if (batt_low_timeout == 0)
                        {
                            bubble_brightness = BUBBLE_brightness_get ();

                            /* Enable 5v */
                            POWER_enable (true);
                            /* Set to max in case user has it off, but don't save config to flash. */
                            BUBBLE_brightness_set (BUBBLE_BRIGHTNESS_STEPS, false);
                        }
                        else if (batt_low_timeout == FSM_LOW_BATTERY_BLINK_PERIOD_MS)
                        {
                            BUBBLE_enable (true);
                        }
                        else if (batt_low_timeout == (FSM_LOW_BATTERY_BLINK_PERIOD_MS * 2))
                        {
                            BUBBLE_enable (false);
                        }
                        else if (batt_low_timeout == (FSM_LOW_BATTERY_BLINK_PERIOD_MS * 3))
                        {
                            BUBBLE_enable (true);
                        }
                        else if (batt_low_timeout == (FSM_LOW_BATTERY_BLINK_PERIOD_MS * 4))
                        {
                            BUBBLE_enable (false);

                            /* Restore bubble brightness. */
                            BUBBLE_brightness_set (bubble_brightness, false);
                        }

                        /* Update timeout count. */
                        batt_low_timeout += FSM_TASK_PERIOD_MS;
                    }
                    break;
                    
                case FSM_POWER_ON:
                    if (_fsm_state_entry)
                    {
                        /* Enable 5v */
                        POWER_enable (true);
                        
                        /* Run through scope ring power up sequence. */
                        SCOPE_RING_state_set (SCOPE_RING_STATE_POWER_ON, 0);
                    }
                    else if (state_finished)
                    {
                        _fsm_state_set (FSM_LEVELING);
                    }
                    break;
                
                case FSM_IDLE:
                    /* If the phone app is configuring parameters, we want to go to level to see changes.*/
                    if (_fsm_app_config)
                    {
                        if (BLE_is_connected ())
                        {
                            IMU_motion_int_enable (false, 0x2, 0x1);
                            _fsm_state_set (FSM_LEVELING);
                        }
                        else
                        {
                            _fsm_app_config = false;
                        }
                    }
                    else if (_fsm_state_entry)
                    {
                        /* When idle, this means scope is stationary.  Perform gyro recalibration. */
                        MOTION_gyro_recalibrate ();

                        /* When idling, we turn off LEDs to save power. */
                        SHOT_CAPTURE_enable (false);
                        BUBBLE_enable (false);
                        FIBER_enable (false);
                        PIN_enable (false);
                        SCOPE_RING_state_set (SCOPE_RING_STATE_OFF, 0);

                        /* Disable 5v */
                        POWER_enable (false);

                        /* Read battery when all LEDs are off. */
                        _fsm_battery_read ();

                        /* Enable IMU motion wake up function every time we enter idle*/
                        IMU_motion_int_enable (true, 0x2, 0x1);

                        /* Reset motion timer. */
                        _fsm_motion_timer = 0;

                        /* Reset in-use flag. */
                        in_use = false;

                        /* Reset 60s timer for low battery status. */
                        timer_60s = 0;
                    }
                    else
                    {
                        /* Check calibration status. */
                        if (MOTION_gyro_recalibrate_status_get () == 1.0f)
                        {
                            /* Never go to sleep if BLE is enabled, else connection breaks. */
                            if (!BLE_is_enabled ())
                            {
                                _fsm_state_set (FSM_SLEEP);
                            }
                            // /* If idle for too long, power down to save power. */
                            // if (_fsm_timeout_cnt >= FSM_IDLE_TIMEOUT_PERIOD_MS)
                            // {
                            //     _fsm_state_set (FSM_OFF);
                            // }
                            // else
                            // {
                            //     /* Never go to sleep if BLE is enabled, else connection breaks. */
                            //     if (!BLE_is_enabled ())
                            //     {
                            //         _fsm_state_set (FSM_SLEEP);
                            //     }
                            // }
                        }
                    }
                    break;

                case FSM_SLEEP:
                    PRINT (INFO, "System is sleeping...\r\n");

                    /* In sleep, enter low power and allow motion and charger to wake processor. */
                    _fsm_low_power_enter (true, false);
                    break;

                case FSM_3D_SLEEP:
                    if (_fsm_state_entry)
                    {
                        /* First power everything off to get proper battery reading. */
                        SHOT_CAPTURE_enable (false);
                        BUBBLE_enable (false);
                        FIBER_enable (false);
                        PIN_enable (false);
                        SCOPE_RING_state_set (SCOPE_RING_STATE_OFF, 0);

                        /* Disable 5v */
                        POWER_enable (false);

                        /* Read battery. */
                        _fsm_battery_read ();

                        /* Never go to sleep if BLE is enabled, else connection breaks. */
                        if (!BLE_is_enabled ())
                        {
                            /* In off state, enter low power and disable motion but enable charger interrupts to wake processor. */
                            _fsm_low_power_enter (false, true);
                        }
                    }
                    /* If in 3D sleep for more than 30 minutes, user likely forgot to turn system off. */
                    else if (_fsm_timeout_cnt >= 1800000)
                    {
                        /* Disable BLE and power off.*/
                        BLE_enable (false, false);

                        _fsm_state_set (FSM_OFF);
                    }
                    break;

                case FSM_CHARGER_CONNECTED:
                    if (_fsm_state_entry)
                    {
                        /* Enable 5v */
                        POWER_enable (true);
                        
                        /* Run through charger connected sequence. */
                        SCOPE_RING_state_set (SCOPE_RING_STATE_CHARGER_CONNECTED, 0);
                    }
                    else if (state_finished)
                    {
                        if (in_use)
                        {
                            _fsm_state_set (FSM_LEVELING);
                        }
                        else
                        {
                            _fsm_state_set (FSM_CHARGING);
                        }
                    }
                    break;

                case FSM_CHARGER_DISCONNECTED:
                    if (_fsm_state_entry)
                    {
                        /* Enable 5v */
                        POWER_enable (true);
                        
                        /* Run through charger disconnected sequence. */
                        SCOPE_RING_state_set (SCOPE_RING_STATE_CHARGER_DISCONNECTED, 0);
                    }
                    else if (state_finished)
                    {
                        if (in_use)
                        {
                            _fsm_state_set (FSM_LEVELING);
                        }
                        else
                        {
                            _fsm_state_set (FSM_BATTERY_FULL);
                        }
                    }
                    break;

                case FSM_LEVELING:
                    /* When device is not stationary, we abort gyro recalibration. */
                    MOTION_gyro_recalibrate_abort ();

                    /* If the phone app is configuring parameters, reset timer so we stay in leveling until config is done.*/
                    if (_fsm_app_config)
                    {
                        if (BLE_is_connected ())
                        {
                            _fsm_timeout_cnt = 0;
                        }
                        else
                        {
                            _fsm_app_config = false;
                        }
                    }

                    if (_fsm_state_entry)
                    {
                        /* Enable 5V power. */
                        POWER_enable (true);
                        
                        _fsm_auto_brightness (_fsm_config.auto_brightness);

                        /* Enable peripherals with saved settings. */
                        SCOPE_RING_state_set (SCOPE_RING_STATE_ON, 0);

                        /* Set in-use flag so we don't shutdown if charger is connected. */
                        in_use = true;
                    }
                    else if (_fsm_timeout_cnt >= _fsm_config.motion_timeout_ms)
                    {
                        _fsm_state_set (FSM_IDLE);
                    }
                    else
                    {
                        /* If we are in the leveling state for FSM_MOTION_TIMEOUT_MAX_MS and no shot has
                           been detected, assume false motion and go to sleep to save power. */
                           // FIXME FSM_MOTION_TIMEOUT_MAX_MS value, sleep or off?
                        if (_fsm_motion_timer >= 120000)
                        {
                            /* When idling, we turn off LEDs to save power. */
                            SHOT_CAPTURE_enable (false);
                            BUBBLE_enable (false);
                            FIBER_enable (false);
                            PIN_enable (false);
                            SCOPE_RING_state_set (SCOPE_RING_STATE_OFF, 0);

                            /* Disable 5v */
                            POWER_enable (false);

                            /* Read battery when all LEDs are off. */
                            _fsm_battery_read ();

                            /* Reset motion timer. */
                            _fsm_motion_timer = 0;

                            /* Reset in-use flag. */
                            in_use = false;

                            /* Reset 60s timer for low battery status. */
                            timer_60s = 0;

                            _fsm_state_set (FSM_SLEEP);
                        }
                        else
                        {
                            _fsm_auto_brightness (_fsm_config.auto_brightness);

                            SHOT_CAPTURE_enable (true);
                            BUBBLE_enable (true);
                            FIBER_enable (true);
                            PIN_enable (true);

                            /* Update motion timer. */
                            _fsm_motion_timer += FSM_TASK_PERIOD_MS;
                        }
                    }
                    break;

                case FSM_POWER_OFF:
                    if (_fsm_state_entry)
                    {
                        /* First power everything off to get proper battery reading. */
                        SHOT_CAPTURE_enable (false);
                        BUBBLE_enable (false);
                        FIBER_enable (false);
                        PIN_enable (false);
                        SCOPE_RING_state_set (SCOPE_RING_STATE_OFF, 0);

                        /* Disable 5v */
                        POWER_enable (false);

                        /* Read battery. */
                        _fsm_battery_read ();

                        /* Reset 3s timer. */
                        timer_3s = 0;
                        
                        /* Enable 5v */
                        POWER_enable (true);
                        
                        /* Run through scope ring power down sequence. */
                        SCOPE_RING_state_set (SCOPE_RING_STATE_POWER_OFF, 0);
                    }
                    else if (state_finished)
                    {
                        /* Disable BLE if enabled before shutting down. FSM_CHARGING is a state of shut down. */
                        if (BLE_is_enabled ())
                        {
                            BLE_enable (false, false);
                        }

                        if (BATTERY_is_charging ())
                        {
                            _fsm_state_set (FSM_CHARGING);
                        }
                        else
                        {
                            _fsm_state_set (FSM_OFF);
                        }
                    }
                    break;

                case FSM_OFF:
                    PRINT (INFO, "System is powering off...\r\n");

                    /* In off state, enter low power and disable motion but enable charger interrupts to wake processor. */
                    _fsm_low_power_enter (false, true);
                    break;

                case FSM_BATTERY_FULL:
                    PRINT (INFO, "Battery is full.  System is powering off...\r\n");

                    /* In battery full state, enter low power and disable motion and charger interrupts that wake processor. */
                    _fsm_low_power_enter (false, false);
                    break;

                case FSM_BLE_ENABLE:
                    if (_fsm_state_entry)
                    {
                        /* Enable 5v */
                        POWER_enable (true);
                        
                        /* Run through scope ring BLE enabled sequence. */
                        SCOPE_RING_state_set (SCOPE_RING_STATE_BLE_ENABLED, 0);
                    }
                    else if (state_finished)
                    {
                        _fsm_state_set (FSM_LEVELING);
                    }
                    break;

                case FSM_BLE_DISABLE:
                    if (_fsm_state_entry)
                    {
                        /* Enable 5v */
                        POWER_enable (true);
                        
                        /* Run through scope ring BLE disabled sequence. */
                        SCOPE_RING_state_set (SCOPE_RING_STATE_BLE_DISABLED, 0);
                    }
                    else if (state_finished)
                    {
                        _fsm_state_set (FSM_LEVELING);
                    }
                    break;

                case FSM_RAINBOW:
                    /* Enable 5V power. */
                    POWER_enable (true);

                    /* Turn everything off. */
                    SHOT_CAPTURE_enable (false);
                    BUBBLE_enable (false);
                    FIBER_enable (false);
                    PIN_enable (false);
                    SCOPE_RING_state_set (SCOPE_RING_STATE_RAINBOW, 0);
                    break;

            
                case FSM_FLAT_CALIBRATE:
                    if (timer_3s == 0)
                    {
                        /* Enable 5v */
                        POWER_enable (true);

                        BUBBLE_enable (false);
                        FIBER_enable (false);
                        PIN_enable (false);
                        SCOPE_RING_state_set (SCOPE_RING_STATE_SET_FLAT, 0);
                        
                        timer_3s += FSM_TASK_PERIOD_MS;
                    }
                    else if (timer_3s < 3000)
                    {
                        timer_3s += FSM_TASK_PERIOD_MS;
                    }
                    else if (timer_3s == 3000)
                    {
                        /* Start calibration */
                        MOTION_calibrate (MOTION_CAL_ORIENTATION_FLAT);

                        /* Set scope ting state to display calibration status. */
                        SCOPE_RING_state_set (SCOPE_RING_STATE_FLAT_CALIBRATION, 0);

                        timer_3s += FSM_TASK_PERIOD_MS;
                    }
                    else if (MOTION_calibrate_status_get (MOTION_CAL_ORIENTATION_FLAT) == (float) 1.0)
                    {
                        uint8_t notify_data = MOTION_CAL_ORIENTATION_FLAT;
                        
                        /* Reset 3 second timer for next calibration. */
                        timer_3s = 0;

                        /* Notify flat cal is done. */
                        BLE_notify (BLE_CHAR_LEVEL_CALIBRATE, &notify_data, sizeof (notify_data));

                        _fsm_evt_handler (EVENT_SUBCAL_NEXT);
                    }
                    break;

                case FSM_BACK_CALIBRATE:
                    if (timer_3s == 0)
                    {
                        /* Enable 5v */
                        POWER_enable (true);

                        BUBBLE_enable (false);
                        FIBER_enable (false);
                        PIN_enable (false);
                        SCOPE_RING_state_set (SCOPE_RING_STATE_SET_BACK, 0);
                        
                        timer_3s += FSM_TASK_PERIOD_MS;
                    }
                    else if (timer_3s < 3000)
                    {
                        timer_3s += FSM_TASK_PERIOD_MS;
                    }
                    else if (timer_3s == 3000)
                    {
                        /* Start calibration */
                        MOTION_calibrate (MOTION_CAL_ORIENTATION_BACK);

                        /* Set scope ting state to display calibration status. */
                        SCOPE_RING_state_set (SCOPE_RING_STATE_BACK_CALIBRATION, 0);

                        timer_3s += FSM_TASK_PERIOD_MS;
                    }
                    else if (MOTION_calibrate_status_get (MOTION_CAL_ORIENTATION_BACK) == (float) 1.0)
                    {
                        uint8_t notify_data = MOTION_CAL_ORIENTATION_BACK;

                        /* Reset 3 second timer for next calibration. */
                        timer_3s = 0;

                        /* Notify back cal is done. */
                        BLE_notify (BLE_CHAR_LEVEL_CALIBRATE, &notify_data, sizeof (notify_data));

                        _fsm_evt_handler (EVENT_SUBCAL_NEXT);
                    }
                    break;

                case FSM_FRONT_CALIBRATE:
                    if (timer_3s == 0)
                    {
                        /* Enable 5v */
                        POWER_enable (true);

                        BUBBLE_enable (false);
                        FIBER_enable (false);
                        PIN_enable (false);
                        SCOPE_RING_state_set (SCOPE_RING_STATE_SET_FRONT, 0);
                        
                        timer_3s += FSM_TASK_PERIOD_MS;
                    }
                    else if (timer_3s < 3000)
                    {
                        timer_3s += FSM_TASK_PERIOD_MS;
                    }
                    else if (timer_3s == 3000)
                    {
                        /* Start calibration */
                        MOTION_calibrate (MOTION_CAL_ORIENTATION_FRONT);

                        /* Set scope ting state to display calibration status. */
                        SCOPE_RING_state_set (SCOPE_RING_STATE_FRONT_CALIBRATION, 0);

                        timer_3s += FSM_TASK_PERIOD_MS;
                    }
                    else if (MOTION_calibrate_status_get (MOTION_CAL_ORIENTATION_FRONT) == (float) 1.0)
                    {
                        uint8_t notify_data = MOTION_CAL_ORIENTATION_FRONT;

                        /* Reset 3 second timer for next calibration. */
                        timer_3s = 0;

                        /* Notify front cal is done. */
                        BLE_notify (BLE_CHAR_LEVEL_CALIBRATE, &notify_data, sizeof (notify_data));

                        _fsm_evt_handler (EVENT_SUBCAL_NEXT);
                    }
                    break;

                case FSM_DEBUG:
                    break;

                default:
                    /* NOTE: Should never get here. Put back into idle. */
                    PRINT (INFO, "ERROR: Invalid FSM state.\r\n");
                    _fsm_state_set (FSM_IDLE);
            }
        }

        _fsm_state_entry = (_fsm_state != prev_state) ? true : false;

        /* Update previous state. */
        prev_state = _fsm_state;

        /* Update elapsed time. */
        _fsm_timeout_cnt += FSM_TASK_PERIOD_MS;

        /* Feed watchdog at end of loop. */
        WDT_feed ();

        k_msleep (FSM_TASK_PERIOD_MS);
    }
}

static void _fsm_config_read (void)
{
    ERRNO_e      err;
    FSM_config_s cfg;

    err = FS_open_or_create (FSM_FILENAME, &_fsm_file, sizeof (FSM_config_s), FS_FILE_DEPTH_DEFAULT);
    if (err == ERRNO_SUCCESS)
    {
        /* Read file. */
        err = FS_read (_fsm_file, (uint8_t *) &cfg, 0, sizeof (FSM_config_s));

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

        /* Validate read file. */
        if (cfg.motion_timeout_ms > FSM_MOTION_TIMEOUT_MAX_MS)
        {
            /* Write defaults to file. */
            _fsm_config_write ();
            return;
        }
        else if (cfg.auto_brightness > 1)
        {
            /* Write defaults to file. */
            _fsm_config_write ();
            return;
        }
        else if (cfg.mode > FSM_MODE_COUNT)
        {
            /* Write defaults to file. */
            _fsm_config_write ();
            return;
        }

        /* Update fsm configuration to what was read from flash and validated. */
        _fsm_config = cfg;
    }
    else if (err == ERRNO_SIZE_MISMATCH)
    {
        /* Config has been modified, need to delete old file and write new configuration. */
        err = FS_delete (FSM_FILENAME);

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

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

        /* Write default config to file. */
        _fsm_config_write ();
    }
    else
    {
        /* Write defaults to file. */
        _fsm_config_write ();
    }
}

static void _fsm_config_write (void)
{
    if (FS_write (_fsm_file, (uint8_t *) &_fsm_config) != ERRNO_SUCCESS)
    {
        PRINT (ERROR, "Failed to write fsm configuration to file.\r\n");
    }
}

static void _fsm_state_set (FSM_e state)
{
    /* Reset timer every time we change states. */
    _fsm_timeout_cnt = 0;

    _fsm_state = state;
}

static void _fsm_evt_handler (EVENT_e event)
{
    uint8_t              bubble_brightness;
    uint8_t              pin_brightness;
    uint8_t              fiber_brightness;
    SCOPE_RING_config_s  scope_ring_config;

    switch (event)
    {
        case EVENT_NONE:
            break;


        case EVENT_CHARGER_CONNECTED:
            _fsm_state_set (FSM_CHARGER_CONNECTED);
            break;

        case EVENT_CHARGER_DISCONNECTED:
            if (_fsm_state == FSM_CHARGING)
            {
                _fsm_state_set (FSM_BATTERY_FULL);
            }
            else
            {
                _fsm_state_set (FSM_CHARGER_DISCONNECTED);
            }
            break;

        case EVENT_BATTERY_FULL:
            if (_fsm_state == FSM_CHARGING)
            {
                _fsm_state_set (FSM_BATTERY_FULL);
            }
            break;

        case EVENT_GYRO_MOTION:
            if (_fsm_state == FSM_LEVELING)
            {
                _fsm_state_set (FSM_LEVELING);
            }
            break;
        
        case EVENT_ACCEL_MOTION:
            if (_fsm_state == FSM_IDLE)
            {
                _fsm_state_set (FSM_LEVELING);
            }
        break;

        case EVENT_POWER:
            /* If power button is pressed while in charging state or 3D sleep, power on and go to leveling. */
            if (_fsm_state == FSM_CHARGING || _fsm_state == FSM_3D_SLEEP)
            {
                _fsm_state_set (FSM_LEVELING);
            }
            else
            {
                _fsm_state_set (FSM_POWER_OFF);
            }
            break;

        case EVENT_DECREASE_PIN_BRIGHTNESS:
            pin_brightness   = PIN_brightness_get ();
            fiber_brightness = FIBER_brightness_get ();

            /* Pin LEDs are active. */
            if (pin_brightness && !fiber_brightness)
            {
                if (pin_brightness > 1)
                {
                    /* Decrease brightness step. */
                    pin_brightness--;
                    /* Set new brightness step. */
                    PIN_brightness_set (pin_brightness, true);
                }
            }
            /* Fiber LEDs are active. */
            else if (fiber_brightness && !pin_brightness)
            {
                if (fiber_brightness > 1)
                {
                    /* Decrease brightness step. */
                    fiber_brightness--;
                    /* Set new brightness step. */
                    FIBER_brightness_set (fiber_brightness, true);
                }
            }
            /* Shouldn't have both active but in case both are... */
            else
            {
                if (pin_brightness > 1)
                {
                    /* Decrease brightness step. */
                    pin_brightness--;
                    /* Set new brightness step. */
                    PIN_brightness_set (pin_brightness, true);
                }
                if (fiber_brightness > 1)
                {
                    /* Decrease brightness step. */
                    fiber_brightness--;
                    /* Set new brightness step. */
                    FIBER_brightness_set (fiber_brightness, true);
                }
            }
            break;

        case EVENT_INCREASE_PIN_BRIGHTNESS:
            pin_brightness   = PIN_brightness_get ();
            fiber_brightness = FIBER_brightness_get ();

            /* If both are inactive, default to pin. */
            if (!pin_brightness && !fiber_brightness)
            {
                if (pin_brightness < PIN_BRIGHTNESS_STEPS)
                {
                    /* Increase brightness step. */
                    pin_brightness++;
                    /* Set new brightness step. */
                    PIN_brightness_set (pin_brightness, true);
                }
            }
            /* Pin LEDs are active. */
            else if (pin_brightness && !fiber_brightness)
            {
                if (pin_brightness < PIN_BRIGHTNESS_STEPS)
                {
                    /* Increase brightness step. */
                    pin_brightness++;
                    /* Set new brightness step. */
                    PIN_brightness_set (pin_brightness, true);
                }
            }
            /* Fiber LEDs are active. */
            else if (fiber_brightness && !pin_brightness)
            {
                if (fiber_brightness < FIBER_BRIGHTNESS_STEPS)
                {
                    /* Increase brightness step. */
                    fiber_brightness++;
                    /* Set new brightness step. */
                    FIBER_brightness_set (fiber_brightness, true);
                }
            }
            /* Shouldn't have both active but in case both are... */
            else
            {
                if (pin_brightness < PIN_BRIGHTNESS_STEPS)
                {
                    /* Increase brightness step. */
                    pin_brightness++;
                    /* Set new brightness step. */
                    PIN_brightness_set (pin_brightness, true);
                }
                if (fiber_brightness < FIBER_BRIGHTNESS_STEPS)
                {
                    /* Increase brightness step. */
                    fiber_brightness++;
                    /* Set new brightness step. */
                    FIBER_brightness_set (fiber_brightness, true);
                }
            }
            break;

        case EVENT_DECREASE_RING_BRIGHTNESS:
            scope_ring_config = SCOPE_RING_config_get ();

            /* Do not fully disable.  Minimum step of 1. */
            if (scope_ring_config.background_brightness > 1)
            {
                scope_ring_config.background_brightness--;

                SCOPE_RING_brightness_set (SCOPE_RING_LED_BACKGROUND, scope_ring_config.background_brightness, false);
            }
            /* Do not fully disable. */
            if (scope_ring_config.indicators_brightness > 1)
            {
                scope_ring_config.indicators_brightness--;

                SCOPE_RING_brightness_set (SCOPE_RING_LED_INDICATORS, scope_ring_config.indicators_brightness, false);
            }
            /* Do not fully disable. */
            if (scope_ring_config.level_brightness > 1)
            {
                scope_ring_config.level_brightness--;

                SCOPE_RING_brightness_set (SCOPE_RING_LED_LEVEL, scope_ring_config.level_brightness, false);
            }

            /* Saving one will save the rest stored in RAM. */
            SCOPE_RING_brightness_set (SCOPE_RING_LED_BACKGROUND, scope_ring_config.background_brightness, true);
            break;

        case EVENT_INCREASE_RING_BRIGHTNESS:
            scope_ring_config = SCOPE_RING_config_get ();

            /* Check if enabled and can be increased. */
            if (scope_ring_config.background_brightness && scope_ring_config.background_brightness < SCOPE_RING_BRIGHTNESS_STEPS)
            {
                scope_ring_config.background_brightness++;

                SCOPE_RING_brightness_set (SCOPE_RING_LED_BACKGROUND, scope_ring_config.background_brightness, false);
            }
            /* Check if enabled and can be increased. */
            if (scope_ring_config.indicators_brightness && scope_ring_config.indicators_brightness < SCOPE_RING_BRIGHTNESS_STEPS)
            {
                scope_ring_config.indicators_brightness++;

                SCOPE_RING_brightness_set (SCOPE_RING_LED_INDICATORS, scope_ring_config.indicators_brightness, false);
            }
            /* Check if enabled and can be increased. */
            if (scope_ring_config.level_brightness && scope_ring_config.level_brightness < SCOPE_RING_BRIGHTNESS_STEPS)
            {
                scope_ring_config.level_brightness++;

                SCOPE_RING_brightness_set (SCOPE_RING_LED_LEVEL, scope_ring_config.level_brightness, false);
            }

            /* Saving one will save the rest stored in RAM. */
            SCOPE_RING_brightness_set (SCOPE_RING_LED_BACKGROUND, scope_ring_config.background_brightness, true);
            break;

        case EVENT_CONFIG_ENTER:
            /* Set event busy flag for configuration. */
            _fsm_evt_busy = true;
            
            /* Enable 5V power. */
            POWER_enable (true);

            /* Reset timer. */
            _fsm_timeout_cnt = 0;

            /* Disable shot capture. */
            SHOT_CAPTURE_enable (false);

            switch (_fsm_evt_subconfig)
            {
                case _FSM_EVT_SUBCONFIG_BUBBLE_BRIGHTNESS:
                    /* Enable bubble and disable all other peripherals. */
                    /* We set to default so user can see and know which one they are setting. */
                    BUBBLE_brightness_set (BUBBLE_BRIGHTNESS_STEP_DEFAULT, true);
                    BUBBLE_enable (true);
                    FIBER_enable (false);
                    PIN_enable (false);
                    SCOPE_RING_state_set (SCOPE_RING_STATE_OFF, 0);
                    break;

                case _FSM_EVT_SUBCONFIG_PIN_BRIGHTNESS:
                    /* Enable pin and disable all other peripherals. */
                    BUBBLE_enable (false);
                    FIBER_enable (false);
                    /* We set to default so user can see and know which one they are setting. */
                    PIN_brightness_set (PIN_BRIGHTNESS_STEP_DEFAULT, true);
                    PIN_enable (true);
                    SCOPE_RING_state_set (SCOPE_RING_STATE_OFF, 0);
                    break;

                case _FSM_EVT_SUBCONFIG_FIBER_BRIGHTNESS:
                    /* Enable fiber and disable all other peripherals. */
                    BUBBLE_enable (false);
                    /* We set to default so user can see and know which one they are setting. */
                    FIBER_brightness_set (FIBER_BRIGHTNESS_STEP_DEFAULT, true);
                    FIBER_enable (true);
                    PIN_enable (false);
                    SCOPE_RING_state_set (SCOPE_RING_STATE_OFF, 0);
                    break;

                case _FSM_EVT_SUBCONFIG_RING_BACKGROUND_COLOR:
                    /* Enable level and disable all other peripherals. */
                    BUBBLE_enable (false);
                    FIBER_enable (false);
                    PIN_enable (false);
                    /* We set to default so user can see and know which one they are setting. */
                    SCOPE_RING_brightness_set (SCOPE_RING_LED_BACKGROUND, SCOPE_RING_BRIGHTNESS_STEP_DEFAULT, true);
                    SCOPE_RING_state_set (SCOPE_RING_STATE_CONFIG_BACKGROUND_COLOR, FSM_CONFIG_BLINK_PERIOD_MS);
                    break;
                
                case _FSM_EVT_SUBCONFIG_RING_BACKGROUND_BRIGHTNESS:
                    SCOPE_RING_state_set (SCOPE_RING_STATE_CONFIG_BACKGROUND_BRIGHTNESS, 0);
                    break;

                case _FSM_EVT_SUBCONFIG_RING_LEVEL_COLOR:
                    /* Enable level and disable all other peripherals. */
                    BUBBLE_enable (false);
                    FIBER_enable (false);
                    PIN_enable (false);
                    /* We set to default so user can see and know which one they are setting. */
                    SCOPE_RING_brightness_set (SCOPE_RING_LED_LEVEL, SCOPE_RING_BRIGHTNESS_STEP_DEFAULT, true);
                    SCOPE_RING_state_set (SCOPE_RING_STATE_CONFIG_LEVEL_COLOR, FSM_CONFIG_BLINK_PERIOD_MS);
                    break;

                case _FSM_EVT_SUBCONFIG_RING_LEVEL_BRIGHTNESS:
                    SCOPE_RING_state_set (SCOPE_RING_STATE_CONFIG_LEVEL_BRIGHTNESS, 0);
                    break;

                case _FSM_EVT_SUBCONFIG_RING_INDICATORS_COLOR:
                    /* Enable level and disable all other peripherals. */
                    BUBBLE_enable (false);
                    FIBER_enable (false);
                    PIN_enable (false);
                    /* We set to default so user can see and know which one they are setting. */
                    SCOPE_RING_brightness_set (SCOPE_RING_LED_INDICATORS, SCOPE_RING_BRIGHTNESS_STEP_DEFAULT, true);
                    SCOPE_RING_state_set (SCOPE_RING_STATE_CONFIG_INDICATORS_COLOR, FSM_CONFIG_BLINK_PERIOD_MS);
                    break;

                case _FSM_EVT_SUBCONFIG_RING_INDICATORS_BRIGHTNESS:
                    SCOPE_RING_state_set (SCOPE_RING_STATE_CONFIG_INDICATORS_BRIGHTNESS, 0);
                    break;

                case _FSM_EVT_SUBCONFIG_LEVEL_SENSITIVITY:
                    SCOPE_RING_state_set (SCOPE_RING_STATE_CONFIG_LEVEL_SENSITIVITY, 0);
                    break;

                case _FSM_EVT_SUBCONFIG_LEVEL_OPTION:
                    SCOPE_RING_state_set (SCOPE_RING_STATE_CONFIG_LEVEL_OPTION, 0);
                    break;
            }
            break;

        case EVENT_SUBCONFIG_PREV:
            /* Reset timer. */
            _fsm_timeout_cnt = 0;

            switch (_fsm_evt_subconfig)
            {
                case _FSM_EVT_SUBCONFIG_BUBBLE_BRIGHTNESS:
                    bubble_brightness = BUBBLE_brightness_get ();

                    if (bubble_brightness > 0)
                    {
                        /* Decrease brightness step. */
                        bubble_brightness--;
                        /* Set new brightness step. */
                        BUBBLE_brightness_set (bubble_brightness, true);
                        /* Need to call enable again to update. */
                        BUBBLE_enable (true);
                    }
                    break;

                case _FSM_EVT_SUBCONFIG_PIN_BRIGHTNESS:
                    pin_brightness = PIN_brightness_get ();

                    if (pin_brightness > 0)
                    {
                        /* Decrease brightness step. */
                        pin_brightness--;
                        /* Set new brightness step. */
                        PIN_brightness_set (pin_brightness, true);
                        /* Need to call enable again to update. */
                        PIN_enable (true);
                    }
                    break;

                case _FSM_EVT_SUBCONFIG_FIBER_BRIGHTNESS:
                    fiber_brightness = FIBER_brightness_get ();

                    if (fiber_brightness > 0)
                    {
                        /* Decrease brightness step. */
                        fiber_brightness--;
                        /* Set new brightness step. */
                        FIBER_brightness_set (fiber_brightness, true);
                        /* Need to call enable again to update. */
                        FIBER_enable (true);
                    }
                    break;

                case _FSM_EVT_SUBCONFIG_RING_BACKGROUND_COLOR:
                    scope_ring_config = SCOPE_RING_config_get ();

                    if (scope_ring_config.background_color == RGB_COLOR_RED)
                    {
                        /* Loop around. */
                        scope_ring_config.background_color = RGB_COLOR_WHITE;
                    }
                    else
                    {
                        scope_ring_config.background_color--;
                    }

                    /* Set new color. */
                    SCOPE_RING_color_set (SCOPE_RING_LED_BACKGROUND, scope_ring_config.background_color);
                    break;

                case _FSM_EVT_SUBCONFIG_RING_BACKGROUND_BRIGHTNESS:
                    scope_ring_config = SCOPE_RING_config_get ();

                    if (scope_ring_config.background_brightness > 0)
                    {
                        /* Decrease brightness step. */
                        scope_ring_config.background_brightness--;
                        /* Set new brightness step. */
                        SCOPE_RING_brightness_set (SCOPE_RING_LED_BACKGROUND, scope_ring_config.background_brightness, true);
                    }
                    break;

                case _FSM_EVT_SUBCONFIG_RING_LEVEL_COLOR:
                    scope_ring_config = SCOPE_RING_config_get ();

                    if (scope_ring_config.level_color == RGB_COLOR_RED)
                    {
                        /* Loop around. */
                        scope_ring_config.level_color = RGB_COLOR_WHITE;
                    }
                    else
                    {
                        scope_ring_config.level_color--;
                    }

                    /* Set new color. */
                    SCOPE_RING_color_set (SCOPE_RING_LED_LEVEL, scope_ring_config.level_color);
                    break;

                case _FSM_EVT_SUBCONFIG_RING_LEVEL_BRIGHTNESS:
                    scope_ring_config = SCOPE_RING_config_get ();

                    if (scope_ring_config.level_brightness > 0)
                    {
                        /* Decrease brightness step. */
                        scope_ring_config.level_brightness--;
                        /* Set new brightness step. */
                        SCOPE_RING_brightness_set (SCOPE_RING_LED_LEVEL, scope_ring_config.level_brightness, true);
                    }
                    break;

                case _FSM_EVT_SUBCONFIG_RING_INDICATORS_COLOR:
                    scope_ring_config = SCOPE_RING_config_get ();

                    if (scope_ring_config.indicators_color == RGB_COLOR_RED)
                    {
                        /* Loop around. */
                        scope_ring_config.indicators_color = RGB_COLOR_WHITE;
                    }
                    else
                    {
                        scope_ring_config.indicators_color--;
                    }

                    /* Set new color. */
                    SCOPE_RING_color_set (SCOPE_RING_LED_INDICATORS, scope_ring_config.indicators_color);
                    break;

                case _FSM_EVT_SUBCONFIG_RING_INDICATORS_BRIGHTNESS:
                    scope_ring_config = SCOPE_RING_config_get ();

                    if (scope_ring_config.indicators_brightness > 0)
                    {
                        /* Decrease brightness step. */
                        scope_ring_config.indicators_brightness--;
                        /* Set new brightness step. */
                        SCOPE_RING_brightness_set (SCOPE_RING_LED_INDICATORS, scope_ring_config.indicators_brightness, true);
                    }
                    break;

                case _FSM_EVT_SUBCONFIG_LEVEL_SENSITIVITY:
                    scope_ring_config = SCOPE_RING_config_get ();

                    if (scope_ring_config.level_sensitivity < (float) SCOPE_RING_LEVEL_SENSITIVITY_MAX)
                    {
                        /* Decrease sensitivity. */
                        scope_ring_config.level_sensitivity+=0.25f;
                        /* Set new level sensitivity. */
                        SCOPE_RING_level_sensitivity_set (scope_ring_config.level_sensitivity);
                    }
                    break;

                case _FSM_EVT_SUBCONFIG_LEVEL_OPTION:
                    scope_ring_config = SCOPE_RING_config_get ();

                    if (scope_ring_config.level_option > SCOPE_RING_LEVEL_OPTION_0)
                    {
                        /* Decrease option. */
                        scope_ring_config.level_option--;
                        /* Set new level option. */
                        SCOPE_RING_level_option_set (scope_ring_config.level_option);
                    }
                    break;
            }
            break;

        case EVENT_SUBCONFIG_MODE_CHANGE:
            /* Reset timer. */
            _fsm_timeout_cnt = 0;

            if (_fsm_evt_subconfig == _FSM_EVT_SUBCONFIG_LEVEL_OPTION)
            {
                /* Circled around. Exit */
                /* Reset subevent for next configuration. */
                _fsm_evt_subconfig = _FSM_EVT_SUBCONFIG_BUBBLE_BRIGHTNESS;
                
                /* Reset event so we are no longer in subconfig. */
                EVENT_reset ();

                /* Reset event busy flag for configuration. */
                _fsm_evt_busy = false;

                _fsm_state_set (FSM_LEVELING);

                /* Set state entry flag for next FSM entry. */
                _fsm_state_entry = true;
                break;
            }
            else
            {
                _fsm_evt_subconfig = (_fsm_evt_subconfig_e) (_fsm_evt_subconfig + 1);
            }
            switch (_fsm_evt_subconfig)
            {
                case _FSM_EVT_SUBCONFIG_BUBBLE_BRIGHTNESS:
                    /* Enable bubble and disable all other peripherals. */
                    /* We set to default so user can see and know which one they are setting. */
                    BUBBLE_brightness_set (BUBBLE_BRIGHTNESS_STEP_DEFAULT, true);
                    BUBBLE_enable (true);
                    FIBER_enable (false);
                    PIN_enable (false);
                    SCOPE_RING_state_set (SCOPE_RING_STATE_OFF, 0);
                    break;
                    
                case _FSM_EVT_SUBCONFIG_PIN_BRIGHTNESS:
                    /* Enable pin and disable all other peripherals. */
                    BUBBLE_enable (false);
                    FIBER_enable (false);
                    /* We set to default so user can see and know which one they are setting. */
                    PIN_brightness_set (PIN_BRIGHTNESS_STEP_DEFAULT, true);
                    PIN_enable (true);
                    SCOPE_RING_state_set (SCOPE_RING_STATE_OFF, 0);
                    break;
                    
                case _FSM_EVT_SUBCONFIG_FIBER_BRIGHTNESS:
                    /* Enable fiber and disable all other peripherals. */
                    BUBBLE_enable (false);
                    /* We set to default so user can see and know which one they are setting. */
                    FIBER_brightness_set (FIBER_BRIGHTNESS_STEP_DEFAULT, true);
                    FIBER_enable (true);
                    PIN_enable (false);
                    SCOPE_RING_state_set (SCOPE_RING_STATE_OFF, 0);
                    break;

                case _FSM_EVT_SUBCONFIG_RING_BACKGROUND_COLOR:
                    /* Enable level and disable all other peripherals. */
                    BUBBLE_enable (false);
                    FIBER_enable (false);
                    PIN_enable (false);
                    /* We set to default so user can see and know which one they are setting. */
                    SCOPE_RING_brightness_set (SCOPE_RING_LED_BACKGROUND, SCOPE_RING_BRIGHTNESS_STEP_DEFAULT, true);
                    SCOPE_RING_state_set (SCOPE_RING_STATE_CONFIG_BACKGROUND_COLOR, FSM_CONFIG_BLINK_PERIOD_MS);
                    break;
                
                case _FSM_EVT_SUBCONFIG_RING_BACKGROUND_BRIGHTNESS:
                    SCOPE_RING_state_set (SCOPE_RING_STATE_CONFIG_BACKGROUND_BRIGHTNESS, 0);
                    break;
                    
                case _FSM_EVT_SUBCONFIG_RING_LEVEL_COLOR:
                    /* Enable level and disable all other peripherals. */
                    BUBBLE_enable (false);
                    FIBER_enable (false);
                    PIN_enable (false);
                    /* We set to default so user can see and know which one they are setting. */
                    SCOPE_RING_brightness_set (SCOPE_RING_LED_LEVEL, SCOPE_RING_BRIGHTNESS_STEP_DEFAULT, true);
                    SCOPE_RING_state_set (SCOPE_RING_STATE_CONFIG_LEVEL_COLOR, FSM_CONFIG_BLINK_PERIOD_MS);
                    break;

                case _FSM_EVT_SUBCONFIG_RING_LEVEL_BRIGHTNESS:
                    SCOPE_RING_state_set (SCOPE_RING_STATE_CONFIG_LEVEL_BRIGHTNESS, 0);
                    break;

                case _FSM_EVT_SUBCONFIG_RING_INDICATORS_COLOR:
                    /* Enable level and disable all other peripherals. */
                    BUBBLE_enable (false);
                    FIBER_enable (false);
                    PIN_enable (false);
                    /* We set to default so user can see and know which one they are setting. */
                    SCOPE_RING_brightness_set (SCOPE_RING_LED_INDICATORS, SCOPE_RING_BRIGHTNESS_STEP_DEFAULT, true);
                    SCOPE_RING_state_set (SCOPE_RING_STATE_CONFIG_INDICATORS_COLOR, FSM_CONFIG_BLINK_PERIOD_MS);
                    break;

                case _FSM_EVT_SUBCONFIG_RING_INDICATORS_BRIGHTNESS:
                    SCOPE_RING_state_set (SCOPE_RING_STATE_CONFIG_INDICATORS_BRIGHTNESS, 0);
                    break;

                case _FSM_EVT_SUBCONFIG_LEVEL_SENSITIVITY:
                    SCOPE_RING_state_set (SCOPE_RING_STATE_CONFIG_LEVEL_SENSITIVITY, 0);
                    break;

                case _FSM_EVT_SUBCONFIG_LEVEL_OPTION:
                    SCOPE_RING_state_set (SCOPE_RING_STATE_CONFIG_LEVEL_OPTION, 0);
                    break;
            }
            break;

        case EVENT_SUBCONFIG_NEXT:
            /* Reset timer. */
            _fsm_timeout_cnt = 0;

            switch (_fsm_evt_subconfig)
            {
                case _FSM_EVT_SUBCONFIG_BUBBLE_BRIGHTNESS:
                    bubble_brightness = BUBBLE_brightness_get ();

                    if (bubble_brightness < BUBBLE_BRIGHTNESS_STEPS)
                    {
                        /* Increase brightness step. */
                        bubble_brightness++;
                        /* Set new brightness step. */
                        BUBBLE_brightness_set (bubble_brightness, true);
                        /* Need to call enable again to update. */
                        BUBBLE_enable (true);
                    }
                    break;

                case _FSM_EVT_SUBCONFIG_PIN_BRIGHTNESS:
                    pin_brightness = PIN_brightness_get ();

                    if (pin_brightness < PIN_BRIGHTNESS_STEPS)
                    {
                        /* Increase brightness step. */
                        pin_brightness++;
                        /* Set new brightness step. */
                        PIN_brightness_set (pin_brightness, true);
                        /* Need to call enable again to update. */
                        PIN_enable (true);
                    }
                    break;

                case _FSM_EVT_SUBCONFIG_FIBER_BRIGHTNESS:
                    fiber_brightness = FIBER_brightness_get ();

                    if (fiber_brightness < FIBER_BRIGHTNESS_STEPS)
                    {
                        /* Increase brightness step. */
                        fiber_brightness++;
                        /* Set new brightness step. */
                        FIBER_brightness_set (fiber_brightness, true);
                        /* Need to call enable again to update. */
                        FIBER_enable (true);
                    }
                    break;

                case _FSM_EVT_SUBCONFIG_RING_BACKGROUND_COLOR:
                    scope_ring_config = SCOPE_RING_config_get ();

                    if (scope_ring_config.background_color == RGB_COLOR_WHITE)
                    {
                        /* Loop around. */
                        scope_ring_config.background_color = RGB_COLOR_RED;
                    }
                    else
                    {
                        scope_ring_config.background_color++;
                    }

                    /* Set new color. */
                    SCOPE_RING_color_set (SCOPE_RING_LED_BACKGROUND, scope_ring_config.background_color);
                    break;

                case _FSM_EVT_SUBCONFIG_RING_BACKGROUND_BRIGHTNESS:
                    scope_ring_config = SCOPE_RING_config_get ();

                    if (scope_ring_config.background_brightness < SCOPE_RING_BRIGHTNESS_STEPS)
                    {
                        /* Increase brightness step. */
                        scope_ring_config.background_brightness++;
                        /* Set new brightness step. */
                        SCOPE_RING_brightness_set (SCOPE_RING_LED_BACKGROUND, scope_ring_config.background_brightness, true);
                    }
                    break;

                case _FSM_EVT_SUBCONFIG_RING_LEVEL_COLOR:
                    scope_ring_config = SCOPE_RING_config_get ();

                    if (scope_ring_config.level_color == RGB_COLOR_WHITE)
                    {
                        /* Loop around. */
                        scope_ring_config.level_color = RGB_COLOR_RED;
                    }
                    else
                    {
                        scope_ring_config.level_color++;
                    }

                    /* Set new color. */
                    SCOPE_RING_color_set (SCOPE_RING_LED_LEVEL, scope_ring_config.level_color);
                    break;

                case _FSM_EVT_SUBCONFIG_RING_LEVEL_BRIGHTNESS:
                    scope_ring_config = SCOPE_RING_config_get ();

                    if (scope_ring_config.level_brightness < SCOPE_RING_BRIGHTNESS_STEPS)
                    {
                        /* Increase brightness step. */
                        scope_ring_config.level_brightness++;
                        /* Set new brightness step. */
                        SCOPE_RING_brightness_set (SCOPE_RING_LED_LEVEL, scope_ring_config.level_brightness, true);
                    }
                    break;

                case _FSM_EVT_SUBCONFIG_RING_INDICATORS_COLOR:
                    scope_ring_config = SCOPE_RING_config_get ();

                    if (scope_ring_config.indicators_color == RGB_COLOR_WHITE)
                    {
                        /* Loop around. */
                        scope_ring_config.indicators_color = RGB_COLOR_RED;
                    }
                    else
                    {
                        scope_ring_config.indicators_color++;
                    }

                    /* Set new color. */
                    SCOPE_RING_color_set (SCOPE_RING_LED_INDICATORS, scope_ring_config.indicators_color);
                    break;

                case _FSM_EVT_SUBCONFIG_RING_INDICATORS_BRIGHTNESS:
                    scope_ring_config = SCOPE_RING_config_get ();

                    if (scope_ring_config.indicators_brightness < SCOPE_RING_BRIGHTNESS_STEPS)
                    {
                        /* Increase brightness step. */
                        scope_ring_config.indicators_brightness++;
                        /* Set new brightness step. */
                        SCOPE_RING_brightness_set (SCOPE_RING_LED_INDICATORS, scope_ring_config.indicators_brightness, true);
                    }
                    break;

                case _FSM_EVT_SUBCONFIG_LEVEL_SENSITIVITY:
                    scope_ring_config = SCOPE_RING_config_get ();

                    if (scope_ring_config.level_sensitivity > (float) SCOPE_RING_LEVEL_SENSITIVITY_MIN)
                    {
                        /* Increase sensitivity. */
                        scope_ring_config.level_sensitivity-=0.25f;
                        /* Set new level sensitivity. */
                        SCOPE_RING_level_sensitivity_set (scope_ring_config.level_sensitivity);
                    }
                    break;

                case _FSM_EVT_SUBCONFIG_LEVEL_OPTION:
                    scope_ring_config = SCOPE_RING_config_get ();

                    if (scope_ring_config.level_option < (SCOPE_RING_LEVEL_OPTION_COUNT - 1))
                    {
                        /* Increase option. */
                        scope_ring_config.level_option++;
                        /* Set new level option. */
                        SCOPE_RING_level_option_set (scope_ring_config.level_option);
                    }
                    break;
            }
            break;

        case EVENT_CONFIG_EXIT:
            /* Reset subevent for next configuration. */
            _fsm_evt_subconfig = _FSM_EVT_SUBCONFIG_BUBBLE_BRIGHTNESS;

            /* Reset event busy flag for configuration. */
            _fsm_evt_busy = false;

            /* Set state entry flag for first iteration in FSM. */
            _fsm_state_entry = true;

            _fsm_state_set (FSM_LEVELING);
            break;

        case EVENT_AUTO_BRIGHTNESS:
            /* Toggle auto brightness setting. */
            FSM_auto_brightness_set (!_fsm_config.auto_brightness);
            PRINT (INFO, "auto brightness = %d...\r\n", _fsm_config.auto_brightness);
            break;

        case EVENT_BLE_ENABLE:
            _fsm_state_set (FSM_BLE_ENABLE);
            BLE_enable (true, true);
            break;

        case EVENT_BLE_DISABLE:
            _fsm_state_set (FSM_BLE_DISABLE);
            BLE_enable (false, true);
            break;

        case EVENT_BLE_CONNECTED:
            break;

        case EVENT_BLE_DISCONNECTED:
            break;

        case EVENT_FACTORY_RESET:
            // TODO / FIXME : Maybe we don't want to erase the entire flash if we event logs are used.
            APP_factory_reset ();
            break;

        case EVENT_SUBCAL_START:
            /* Set event busy flag for calibration. */
            _fsm_evt_busy = true;
            
            /* Enable 5V power. */
            POWER_enable (true);

            /* Reset timer. */
            _fsm_timeout_cnt = 0;

            /* Disable shot capture. */
            SHOT_CAPTURE_enable (false);

            /* Default subcal event state is _FSM_EVT_SUBCAL_SET_FLAT. */
            BUBBLE_enable (false);
            FIBER_enable (false);
            PIN_enable (false);
            SCOPE_RING_state_set (SCOPE_RING_STATE_SET_FLAT, 0);
            break;


        case EVENT_SUBCAL_NEXT:
            /* Reset timer. */
            _fsm_timeout_cnt = 0;

            if (_fsm_evt_subcal == _FSM_EVT_SUBCAL_FRONT)
            {
                /* Circled around. Exit */
                /* Reset subevent for next calibration. */
                _fsm_evt_subcal = _FSM_EVT_SUBCAL_SET_FLAT;
                
                /* Reset event so we are no longer in subcal. */
                EVENT_reset ();

                /* Reset event busy flag for configuration. */
                _fsm_evt_busy = false;

                _fsm_state_set (FSM_LEVELING);
                break;
            }
            else
            {
                _fsm_evt_subcal = (_fsm_evt_subcal_e) (_fsm_evt_subcal + 1);
            }

            switch (_fsm_evt_subcal)
            {
                case _FSM_EVT_SUBCAL_SET_FLAT:
                    _fsm_evt_busy = true;
                    SCOPE_RING_state_set (SCOPE_RING_STATE_SET_FLAT, 0);
                    break;

                case _FSM_EVT_SUBCAL_FLAT:
                    _fsm_evt_busy = false;
                    _fsm_state_set (FSM_FLAT_CALIBRATE);
                    break;

                case _FSM_EVT_SUBCAL_SET_BACK:
                    _fsm_evt_busy = true;
                    SCOPE_RING_state_set (SCOPE_RING_STATE_SET_BACK, 0);
                    break;

                case _FSM_EVT_SUBCAL_BACK:
                    _fsm_evt_busy = false;
                    _fsm_state_set (FSM_BACK_CALIBRATE);
                    break;

                case _FSM_EVT_SUBCAL_SET_FRONT:
                    _fsm_evt_busy = true;
                    SCOPE_RING_state_set (SCOPE_RING_STATE_SET_FRONT, 0);
                    break;

                case _FSM_EVT_SUBCAL_FRONT:
                    _fsm_evt_busy = false;
                    _fsm_state_set (FSM_FRONT_CALIBRATE);
                    break;
            }
            break;
        
        case EVENT_SHOT_DETECTED:
            if (_fsm_config.mode == FSM_MODE_3D)
            {
                _fsm_state_set (FSM_3D_SLEEP);
            }
            else
            {
                /* If shot is detected, reset timeout counter and motion timer. */
                _fsm_timeout_cnt  = 0;
                _fsm_motion_timer = 0;
            }
            break;
            
        default:
            break;
    }
}

static void _fsm_auto_brightness (bool enabled)
{
    static bool                was_enabled = false;
    static uint8_t             bubble_brightness;
    static uint8_t             fiber_brightness;
    static uint8_t             pin_brightness;
    static SCOPE_RING_config_s scope_ring_config;
           uint8_t             step;
     
    if (!enabled && !was_enabled)
    {
        return;
    }
    else if (enabled && !was_enabled)
    {
        /* Preserve settings. */
        bubble_brightness = BUBBLE_brightness_get ();
        fiber_brightness  = FIBER_brightness_get ();
        pin_brightness    = PIN_brightness_get ();
        scope_ring_config = SCOPE_RING_config_get ();
    }

    if (enabled)
    {
        step = LIGHT_SENSOR_step_get ();
        /* Handle return step of 0 (total darkness). */
        if (step == 0)
        {
            /* Set to minimum so we can still see LEDs. */
            step = 1;
        }

        /* Read current config for bubble to see if configured to be on. */
        if (bubble_brightness)
        {
            /* If configured to be on, we scale to light sensor step reading, but do not save config. */
            BUBBLE_brightness_set (step, false);
        }
        /* Read current config for fiber to see if configured to be on. */
        if (fiber_brightness)
        {
            /* If configured to be on, we scale to light sensor step reading, but do not save config. */
            FIBER_brightness_set (step, false);
        }
        /* Read current config for pin to see if configured to be on. */
        if (pin_brightness)
        {
            /* If configured to be on, we scale to light sensor step reading, but do not save config. */
            PIN_brightness_set (step, false);
        }
        /* Read current config for scope ring level to see if configured to be on. */
        if (scope_ring_config.level_brightness)
        {
            if (step < SCOPE_RING_BRIGHTNESS_STEPS)
            {
                /* If configured to be on, we scale to scope ring level step reading, but do not save config. */
                SCOPE_RING_brightness_set (SCOPE_RING_LED_LEVEL, (step + 1), false);
            }
            else
            {
                SCOPE_RING_brightness_set (SCOPE_RING_LED_LEVEL, step, false);
            }
        }
        /* Read current config for scope ring indicators to see if configured to be on. */
        if (scope_ring_config.indicators_brightness)
        {
            if (step < SCOPE_RING_BRIGHTNESS_STEPS)
            {
                /* If configured to be on, we scale to scope ring indicators step reading, but do not save config. */
                SCOPE_RING_brightness_set (SCOPE_RING_LED_INDICATORS, (step + 1), false);
            }
            else
            {
                SCOPE_RING_brightness_set (SCOPE_RING_LED_INDICATORS, step, false);
            }
        }
        /* Read current config for scope ring background to see if configured to be on. */
        if (scope_ring_config.background_brightness)
        {
            /* If configured to be on, we scale to scope ring background step reading, but do not save config.
            * NOTE: We cap brightness for background LEDs to 5 steps, since they are not needed when ambient
            *       lighting conditions are sufficient.  Because of this we will need to scale (divide by 2). */
            SCOPE_RING_brightness_set (SCOPE_RING_LED_BACKGROUND, (uint8_t) ((step / 2) + 1), false);
        }
    }
    else
    {
        BUBBLE_brightness_set (bubble_brightness, false);
        FIBER_brightness_set (fiber_brightness, false);
        PIN_brightness_set (pin_brightness, false);
        SCOPE_RING_brightness_set (SCOPE_RING_LED_LEVEL, scope_ring_config.level_brightness, false);
        SCOPE_RING_brightness_set (SCOPE_RING_LED_INDICATORS, scope_ring_config.indicators_brightness, false);
        SCOPE_RING_brightness_set (SCOPE_RING_LED_BACKGROUND, scope_ring_config.background_brightness, false);
    }

    was_enabled = enabled;
}

static void _fsm_low_power_enter (bool wake_on_motion, bool wake_on_charge)
{
    k_sched_lock ();
    (void) irq_lock ();

    if (wake_on_motion)
    {
        /* Set to leveling here because RAM is retained for the state when waking on boot. */
        _fsm_state_set (FSM_LEVELING);

        /* Enable RAM retention for FSM state.*/
        if (MCU_ram_retain ((uint8_t *) &_fsm_state, sizeof (_fsm_state), true) != ERRNO_SUCCESS)
        {
            PRINT (ERROR, "Failure to configure RAM retention for FSM state.\r\n");
        }
    }
    else
    {
        /* Disable RAM retention of FSM state.*/
        if (MCU_ram_retain ((uint8_t *) &_fsm_state, sizeof (_fsm_state), false) != ERRNO_SUCCESS)
        {
            PRINT (ERROR, "Failure to disable RAM retention for FSM state.\r\n");
        }
    }
    
    /* Turn everything off. */
    SHOT_CAPTURE_enable (false);
    BUBBLE_enable (false);
    FIBER_enable (false);
    PIN_enable (false);
    SCOPE_RING_state_set (SCOPE_RING_STATE_OFF, 0);
    
    if (SHOT_CAPTURE_save_shot_cnt ())
    {
        PRINT (INFO, "Failure to write shot count to flash before OFF.\r\n");
    }

    if (ALS_enable (false))
    {
        PRINT (INFO, "Failure to disable ambient light sensor.\r\n");
    }

    if (wake_on_motion)
    {
        /* Enable interrupts from motion so system can wake. */
        /* Set lowest threshold to lowest threshold for motion/movement detection.
            1 LSB = range / 64 (e.g., for ±2g, 1 LSB ≈ 31.25 mg). */
        if (IMU_motion_int_enable (true, 0x2, 0x1))
        {
            PRINT (INFO, "Failure to enable IMU motion interrupt.\r\n");
        }
    }
    else
    {
        /* Disable motion interrupt so IMU doesn't wake processor on motion. */
        if (IMU_motion_int_enable (false, 0x1, 0x0))
        {
            PRINT (INFO, "Failure to disable IMU motion interrupt.\r\n");
        }
    }
    
    if (IMU_sleep ())
    {
        PRINT (INFO, "Failure to put IMU in sleep mode.\r\n");
    }
    if (NVM_EXT_power_down ())
    {
        PRINT (INFO, "Failure to power down flash.\r\n");
    }
    if (POWER_enable (false))
    {
        PRINT (INFO, "Failure to disable power.\r\n");
    }

    if (BATTERY_wake_on_charge (wake_on_charge))
    {
        PRINT (INFO, "Failure to configure battery wake on charge.\r\n");
    }

    /* Reconfigure GPIO interrupt for center button to be used to wake from sleep. */
    if (BUTTON_wake_on_press (BUTTON_CENTER))
    {
        PRINT (INFO, "Failure to configure GPIO center button as interrupt to wake from sleep. %d\r\n");
    }

    /* Put mcu to sleep. */
    MCU_sleep ();
}

static void _fsm_battery_read (void)
{
    float voltage;

    if (BATTERY_voltage_get (&voltage) != ERRNO_SUCCESS)
    {
        PRINT (ERROR, "Failure to read battery voltage.\r\n");
    }

    _fsm_batt_life = BATTERY_level_calculate (voltage);

    /* Set battery status for feedback on LED ring. */
    SCOPE_RING_battery_indicator_set (_fsm_batt_life);

    /* Notify BLE of battery status. */
    BLE_notify (BLE_CHAR_BATTERY_LEVEL, &_fsm_batt_life, sizeof (_fsm_batt_life));
}
