/** 
 * @file    scope_ring.c
 * @brief   Scope ring module
 * @author  Kenny Tran
 * @date    Apr 05 2023
 * @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/kernel.h>

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

#include "../bsp/battery.h"
#include "../bsp/led_ring.h"
#include "../bsp/power.h"
#include "../bsp/wdt.h"

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

#include "scope_ring.h"


/*--------------------------------------------------------------------------------------------------
 * MACROS & CONSTANTS
 *------------------------------------------------------------------------------------------------*/
#define _SCOPE_RING_N_LEDS              (LED_RING_PIXELS)
#define _SCOPE_RING_R_INDICATOR_IDX     (_SCOPE_RING_N_LEDS / 4)
#define _SCOPE_RING_L_INDICATOR_IDX     (_SCOPE_RING_N_LEDS - _SCOPE_RING_R_INDICATOR_IDX)


/*--------------------------------------------------------------------------------------------------
 * THREAD DEFINITIONS
 *------------------------------------------------------------------------------------------------*/
K_THREAD_STACK_DEFINE(_scope_ring_stack, SCOPE_RING_STACK_SIZE);


/*--------------------------------------------------------------------------------------------------
 * TYPE DEFINITIONS & ENUMERATIONS
 *------------------------------------------------------------------------------------------------*/


/*--------------------------------------------------------------------------------------------------
 * PRIVATE CONSTANTS & VARIABLES
 *------------------------------------------------------------------------------------------------*/
static struct k_thread     _scope_ring_thread;
static FS_file_t           _scope_ring_file;
static SCOPE_RING_config_s _scope_ring_config = (SCOPE_RING_config_s) {
    .indicators_color      = SCOPE_RING_INDICATORS_COLOR_DEFAULT,
    .indicators_brightness = SCOPE_RING_BRIGHTNESS_STEP_DEFAULT,
    .level_color           = SCOPE_RING_LEVEL_COLOR_DEFAULT,
    .level_brightness      = SCOPE_RING_BRIGHTNESS_STEP_DEFAULT,
    .background_color      = SCOPE_RING_BACKGROUND_COLOR_DEFAULT,
    .background_brightness = SCOPE_RING_BRIGHTNESS_STEP_DEFAULT,
    .level_option          = SCOPE_RING_LEVEL_OPTION_DEFAULT,
    .level_viscosity       = SCOPE_RING_LEVEL_VISCOSITY_DEFAULT,
    .level_sensitivity     = SCOPE_RING_LEVEL_SENSITIVITY_DEFAULT
};
static SCOPE_RING_state_e  _scope_ring_state = SCOPE_RING_STATE_OFF;
static uint32_t            _scope_ring_timer_cnt = 0;
static float               _scope_ring_battery_life   = 0.0;
static uint32_t            _scope_ring_state_duration = 0;
static bool                _scope_ring_state_finished = false;


/*--------------------------------------------------------------------------------------------------
 * PRIVATE FUNCTION PROTOTYPES
 *------------------------------------------------------------------------------------------------*/
static void  _scope_ring_task (void *p1, void *p2, void *p3);
static void  _scope_ring_level_process (float angle, SCOPE_RING_level_option_e level_option);
static void  _scope_ring_background_set (void);
static void  _scope_ring_mirrored_set (uint8_t led, uint8_t r, uint8_t g, uint8_t b);
static void  _scope_ring_enable (bool en);
static void  _scope_ring_config_read (void);
static void  _scope_ring_config_write (void);

static float _scope_ring_angle_hysteresis_get (float angle);
static float _scope_ring_level_viscosity_get (float angle, float alpha);


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

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

    /* Initialize LED ring. */
    if (LED_RING_init () < ERRNO_SUCCESS)
    {
        PRINT (ERROR, "Failure to initialize LED ring.\r\n");
        return;
    }

    /* Initialize filesystem. */
    if (FS_init () < ERRNO_SUCCESS)
    {
        PRINT (ERROR, "Failure to initialize filesystem.\r\n");
        return;
    }

    /* Fetch configuration settings stored for scope ring. */
    _scope_ring_config_read ();
}

void SCOPE_RING_state_set (SCOPE_RING_state_e state, uint32_t duration_ms)
{
    if (state >= SCOPE_RING_STATE_INVALID)
    {
        PRINT (ERROR, "Invalid state.\r\n");
        return;
    }

    /* Reset task timer on every state change. */
    _scope_ring_timer_cnt      = 0;

    /* Reset state finished flag. */
    _scope_ring_state_finished = false;

    _scope_ring_state          = state;
    _scope_ring_state_duration = duration_ms;
}

SCOPE_RING_state_e SCOPE_RING_state_get (bool *p_finshed)
{
    if (p_finshed == NULL)
    {
        return _scope_ring_state;
    }
    else
    {
        *p_finshed = _scope_ring_state_finished;
    }

    return _scope_ring_state;
}

void SCOPE_RING_color_set (SCOPE_RING_led_e led, RGB_color_e color)
{
    if (color >= RGB_COLOR_COUNT)
    {
        PRINT (ERROR, "Invalid color.\r\n");
        return;
    }

    switch (led)
    {
        case SCOPE_RING_LED_BACKGROUND:
            _scope_ring_config.background_color = color;
            BLE_notify (BLE_CHAR_RING_COLOR, &color, sizeof (color));
            break;

        case SCOPE_RING_LED_INDICATORS:
            _scope_ring_config.indicators_color = color;
            BLE_notify (BLE_CHAR_INDICATOR_COLOR, &color, sizeof (color));
            break;

        case SCOPE_RING_LED_LEVEL:
            _scope_ring_config.level_color = color;
            BLE_notify (BLE_CHAR_LEVEL_COLOR, &color, sizeof (color));
            break;

        default:
            PRINT (ERROR, "ERROR : Invalid LED.\r\n");
            return;
    }

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

void SCOPE_RING_brightness_set (SCOPE_RING_led_e led, uint8_t step, bool save_settings)
{
    if (step > SCOPE_RING_BRIGHTNESS_STEPS)
    {
        PRINT (ERROR, "Invalid brightness step.\r\n");
        return;
    }

    switch (led)
    {
        case SCOPE_RING_LED_BACKGROUND:
            _scope_ring_config.background_brightness = step;
            break;

        case SCOPE_RING_LED_INDICATORS:
            _scope_ring_config.indicators_brightness = step;
            break;

        case SCOPE_RING_LED_LEVEL:
            _scope_ring_config.level_brightness = step;
            break;

        default:
            PRINT (ERROR, "Invalid LED.\r\n");
    }

    if (save_settings)
    {
        /* Write new settings to file. */
        _scope_ring_config_write ();

        switch (led)
        {
            case SCOPE_RING_LED_BACKGROUND:
                BLE_notify (BLE_CHAR_RING_BRIGHTNESS, &step, sizeof (step));
                break;

            case SCOPE_RING_LED_INDICATORS:
                BLE_notify (BLE_CHAR_INDICATOR_BRIGHTNESS, &step, sizeof (step));
                break;

            case SCOPE_RING_LED_LEVEL:
                BLE_notify (BLE_CHAR_LEVEL_BRIGHTNESS, &step, sizeof (step));
                break;

            default:
                PRINT (ERROR, "Invalid LED.\r\n");
        }
    }
}

void SCOPE_RING_battery_indicator_set (float percentage)
{
    if (percentage > 1.0)
    {
        PRINT (ERROR, "Invalid battery percentage. %f\r\n", percentage);
        _scope_ring_battery_life = 1.0;
    }
    else if (percentage < 0.0)
    {
        PRINT (ERROR, "Invalid battery percentage.%f\r\n", percentage);
        _scope_ring_battery_life = 0.0;
    }
    else
    {
        _scope_ring_battery_life = percentage;
    }
}

void SCOPE_RING_level_option_set (SCOPE_RING_level_option_e option)
{
    if (option >= SCOPE_RING_LEVEL_OPTION_COUNT)
    {
        PRINT (ERROR, "Invalid leveling option.\r\n");
        _scope_ring_config.level_option = SCOPE_RING_LEVEL_COLOR_DEFAULT;
    }
    else
    {
        _scope_ring_config.level_option = option;
    }

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

    BLE_notify (BLE_CHAR_LEVEL_OPTION, &option, sizeof (option));
}

void SCOPE_RING_level_viscosity_set (float alpha)
{
    _scope_ring_config.level_viscosity = alpha;

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

    BLE_notify (BLE_CHAR_LEVEL_VISCOSITY, &alpha, sizeof (alpha));
}

void SCOPE_RING_level_sensitivity_set (float sensitivity)
{
    _scope_ring_config.level_sensitivity = sensitivity;

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

    BLE_notify (BLE_CHAR_LEVEL_SENSITIVITY, &sensitivity, sizeof (sensitivity));
}

SCOPE_RING_config_s SCOPE_RING_config_get (void)
{
    return _scope_ring_config;
}

void SCOPE_RING_task_suspend (void)
{
    k_thread_suspend (&_scope_ring_thread);
}

void SCOPE_RING_task_resume (void)
{
    k_thread_resume (&_scope_ring_thread);
}

void SCOPE_RING_task_install (void)
{
    static k_tid_t id;

    id = k_thread_create (&_scope_ring_thread,
                          _scope_ring_stack,
                          SCOPE_RING_STACK_SIZE,
                          _scope_ring_task,
                          NULL,
                          NULL,
                          NULL,
                          SCOPE_RING_THREAD_PRIORITY,
                          0,
                          K_NO_WAIT);
    k_thread_name_set(&_scope_ring_thread, "SCOPE_RING_task");                      
}

void SCOPE_RING_cli_cmd (int32_t argc, char **argv)
{
    if (argc == 0)
    {
        PRINT (INFO, "SCOPE RING\r\n");
        PRINT (INFO, "-------------------------------------------\r\n");
        PRINT (INFO, "timer counter (ms) : %d\r\n", _scope_ring_timer_cnt);

        PRINT (INFO, "State                       : ");
        switch (_scope_ring_state)
        {
            case SCOPE_RING_STATE_OFF:
                PRINT (INFO, "off\r\n");
                break;
            case SCOPE_RING_STATE_ON:
                PRINT (INFO, "on\r\n");
                break;
            case SCOPE_RING_STATE_SET_FLAT:
                PRINT (INFO, "setflat\r\n");
                break;
            case SCOPE_RING_STATE_FLAT_CALIBRATION:
                PRINT (INFO, "calflat\r\n");
                break;
            case SCOPE_RING_STATE_SET_BACK:
                PRINT (INFO, "setback\r\n");
                break;
            case SCOPE_RING_STATE_BACK_CALIBRATION:
                PRINT (INFO, "calback\r\n");
                break;
            case SCOPE_RING_STATE_SET_FRONT:
                PRINT (INFO, "setfront\r\n");
                break;
            case SCOPE_RING_STATE_FRONT_CALIBRATION:
                PRINT (INFO, "calfront\r\n");
                break;
            case SCOPE_RING_STATE_CONFIG_LEVEL_COLOR:
                PRINT (INFO, "lc\r\n");
                break;
            case SCOPE_RING_STATE_CONFIG_LEVEL_BRIGHTNESS:
                PRINT (INFO, "lb\r\n");
                break;
            case SCOPE_RING_STATE_CONFIG_INDICATORS_COLOR:
                PRINT (INFO, "ic\r\n");
                break;
            case SCOPE_RING_STATE_CONFIG_INDICATORS_BRIGHTNESS:
                PRINT (INFO, "ib\r\n");
                break;
            case SCOPE_RING_STATE_CONFIG_BACKGROUND_COLOR:
                PRINT (INFO, "bc\r\n");
                break;
            case SCOPE_RING_STATE_CONFIG_BACKGROUND_BRIGHTNESS:
                PRINT (INFO, "bb\r\n");
                break;
            case SCOPE_RING_STATE_RAINBOW:
                PRINT (INFO, "rainbow\r\n");
                break;
            case SCOPE_RING_STATE_CHARGING:
                PRINT (INFO, "charging\r\n");
                break;
            case SCOPE_RING_STATE_CHARGER_CONNECTED:
                PRINT (INFO, "cc\r\n");
                break;
            case SCOPE_RING_STATE_CHARGER_DISCONNECTED:
                PRINT (INFO, "cd\r\n");
                break;            
            case SCOPE_RING_STATE_POWER_ON:
                PRINT (INFO, "power_on\r\n");
                break;
            case SCOPE_RING_STATE_POWER_OFF:
                PRINT (INFO, "power_off\r\n");
                break;
            case SCOPE_RING_STATE_BLE_ENABLED:
                PRINT (INFO, "ble_en\r\n");
                break;
            case SCOPE_RING_STATE_BLE_DISABLED:
                PRINT (INFO, "ble_dis\r\n");
                break;
        }
        PRINT (INFO, "Indicators color            : ");
        switch (_scope_ring_config.indicators_color)
        {
            case RGB_COLOR_RED:
                PRINT (INFO, "red\r\n");
                break;
            case RGB_COLOR_GREEN:
                PRINT (INFO, "green\r\n");
                break;
            case RGB_COLOR_BLUE:
                PRINT (INFO, "blue\r\n");
                break;
            case RGB_COLOR_ORANGE:
                PRINT (INFO, "orange\r\n");
                break;
            case RGB_COLOR_YELLOW:
                PRINT (INFO, "yellow\r\n");
                break;
            case RGB_COLOR_PURPLE:
                PRINT (INFO, "purple\r\n");
                break;
            case RGB_COLOR_PINK:
                PRINT (INFO, "pink\r\n");
                break;
            case RGB_COLOR_WHITE:
                PRINT (INFO, "white\r\n");
                break;
        }
        PRINT (INFO, "Indicators brightness step  : %d\r\n", _scope_ring_config.indicators_brightness);
        PRINT (INFO, "Level color                 : ");
        switch (_scope_ring_config.level_color)
        {
            case RGB_COLOR_RED:
                PRINT (INFO, "red\r\n");
                break;
            case RGB_COLOR_GREEN:
                PRINT (INFO, "green\r\n");
                break;
            case RGB_COLOR_BLUE:
                PRINT (INFO, "blue\r\n");
                break;
            case RGB_COLOR_ORANGE:
                PRINT (INFO, "orange\r\n");
                break;
            case RGB_COLOR_YELLOW:
                PRINT (INFO, "yellow\r\n");
                break;
            case RGB_COLOR_PURPLE:
                PRINT (INFO, "purple\r\n");
                break;
            case RGB_COLOR_PINK:
                PRINT (INFO, "pink\r\n");
                break;
            case RGB_COLOR_WHITE:
                PRINT (INFO, "white\r\n");
                break;
        }
        PRINT (INFO, "Level brightness step       : %d\r\n", _scope_ring_config.level_brightness);
        PRINT (INFO, "Background color            : ");
        switch (_scope_ring_config.background_color)
        {
            case RGB_COLOR_RED:
                PRINT (INFO, "red\r\n");
                break;
            case RGB_COLOR_GREEN:
                PRINT (INFO, "green\r\n");
                break;
            case RGB_COLOR_BLUE:
                PRINT (INFO, "blue\r\n");
                break;
            case RGB_COLOR_ORANGE:
                PRINT (INFO, "orange\r\n");
                break;
            case RGB_COLOR_YELLOW:
                PRINT (INFO, "yellow\r\n");
                break;
            case RGB_COLOR_PURPLE:
                PRINT (INFO, "purple\r\n");
                break;
            case RGB_COLOR_PINK:
                PRINT (INFO, "pink\r\n");
                break;
            case RGB_COLOR_WHITE:
                PRINT (INFO, "white\r\n");
                break;
        }
        PRINT (INFO, "Background brightness step  : %d\r\n", _scope_ring_config.background_brightness);
        PRINT (INFO, "Leveling option             : %d\r\n", _scope_ring_config.level_option);
        PRINT (INFO, "Level sensitivity (degrees) : %f\r\n", (SCOPE_RING_LEVEL_SENSITIVITY_MAX +
                                                              SCOPE_RING_LEVEL_SENSITIVITY_MIN -
                                                              _scope_ring_config.level_sensitivity));
        PRINT (INFO, "Level viscosity (alpha)     : %f\r\n", _scope_ring_config.level_viscosity);
    }
    else if (argc == 2 || argc == 3)
    {
        if (!strcmp (argv[0], "-s") || !strcmp (argv[0], "--set"))
        {
            if (!strcmp (argv[1], "off"))
            {
                SCOPE_RING_state_set (SCOPE_RING_STATE_OFF, 0);
            }
            else if (!strcmp (argv[1], "on"))
            {
                SCOPE_RING_state_set (SCOPE_RING_STATE_ON, 0);
            }
            else if (!strcmp (argv[1], "setflat"))
            {
                SCOPE_RING_state_set (SCOPE_RING_STATE_SET_FLAT, 0);
            }
            else if (!strcmp (argv[1], "calflat"))
            {
                SCOPE_RING_state_set (SCOPE_RING_STATE_FLAT_CALIBRATION, 0);
            }
            else if (!strcmp (argv[1], "setback"))
            {
                SCOPE_RING_state_set (SCOPE_RING_STATE_SET_BACK, 0);
            }
            else if (!strcmp (argv[1], "calback"))
            {
                SCOPE_RING_state_set (SCOPE_RING_STATE_BACK_CALIBRATION, 0);
            }
            else if (!strcmp (argv[1], "setfront"))
            {
                SCOPE_RING_state_set (SCOPE_RING_STATE_SET_FRONT, 0);
            }
            else if (!strcmp (argv[1], "calfront"))
            {
                SCOPE_RING_state_set (SCOPE_RING_STATE_FRONT_CALIBRATION, 0);
            }
            else if (!strcmp (argv[1], "lc"))
            {
                SCOPE_RING_state_set (SCOPE_RING_STATE_CONFIG_LEVEL_COLOR, 0);
            }
            else if (!strcmp (argv[1], "lb"))
            {
                SCOPE_RING_state_set (SCOPE_RING_STATE_CONFIG_LEVEL_BRIGHTNESS, 0);
            }
            else if (!strcmp (argv[1], "ic"))
            {
                SCOPE_RING_state_set (SCOPE_RING_STATE_CONFIG_INDICATORS_COLOR, 0);
            }
            else if (!strcmp (argv[1], "ib"))
            {
                SCOPE_RING_state_set (SCOPE_RING_STATE_CONFIG_INDICATORS_BRIGHTNESS, 0);
            }
            else if (!strcmp (argv[1], "bc"))
            {
                SCOPE_RING_state_set (SCOPE_RING_STATE_CONFIG_BACKGROUND_COLOR, 0);
            }
            else if (!strcmp (argv[1], "bb"))
            {
                SCOPE_RING_state_set (SCOPE_RING_STATE_CONFIG_BACKGROUND_BRIGHTNESS, 0);
            }
            else if (!strcmp (argv[1], "rainbow"))
            {
                SCOPE_RING_state_set (SCOPE_RING_STATE_RAINBOW, 0);
            }
            else if (!strcmp (argv[1], "charging"))
            {
                SCOPE_RING_state_set (SCOPE_RING_STATE_CHARGING, 0);
            }
            else if (!strcmp (argv[1], "cc"))
            {
                SCOPE_RING_state_set (SCOPE_RING_STATE_CHARGER_CONNECTED, 0);
            }
            else if (!strcmp (argv[1], "cd"))
            {
                SCOPE_RING_state_set (SCOPE_RING_STATE_CHARGER_DISCONNECTED, 0);
            }
            else if (!strcmp (argv[1], "power_on"))
            {
                SCOPE_RING_state_set (SCOPE_RING_STATE_POWER_ON, 0);
            }
            else if (!strcmp (argv[1], "power_off"))
            {
                SCOPE_RING_state_set (SCOPE_RING_STATE_POWER_OFF, 0);
            }
            else if (!strcmp (argv[1], "ble_en"))
            {
                SCOPE_RING_state_set (SCOPE_RING_STATE_BLE_ENABLED, 0);
            }
            else if (!strcmp (argv[1], "ble_dis"))
            {
                SCOPE_RING_state_set (SCOPE_RING_STATE_BLE_DISABLED, 0);
            }
            else
            {
                goto USAGE;
            }
        }
        else if (!strcmp (argv[0], "-i") || !strcmp (argv[0], "--indicators"))
        {
            if (!strcmp (argv[1], "red"))
            {
                SCOPE_RING_color_set (SCOPE_RING_LED_INDICATORS, RGB_COLOR_RED);
            }
            else if (!strcmp (argv[1], "green"))
            {
                SCOPE_RING_color_set (SCOPE_RING_LED_INDICATORS, RGB_COLOR_GREEN);
            }
            else if (!strcmp (argv[1], "blue"))
            {
                SCOPE_RING_color_set (SCOPE_RING_LED_INDICATORS, RGB_COLOR_BLUE);
            }
            else if (!strcmp (argv[1], "orange"))
            {
                SCOPE_RING_color_set (SCOPE_RING_LED_INDICATORS, RGB_COLOR_ORANGE);
            }
            else if (!strcmp (argv[1], "yellow"))
            {
                SCOPE_RING_color_set (SCOPE_RING_LED_INDICATORS, RGB_COLOR_YELLOW);
            }
            else if (!strcmp (argv[1], "purple"))
            {
                SCOPE_RING_color_set (SCOPE_RING_LED_INDICATORS, RGB_COLOR_PURPLE);
            }
            else if (!strcmp (argv[1], "pink"))
            {
                SCOPE_RING_color_set (SCOPE_RING_LED_INDICATORS, RGB_COLOR_PINK);
            }
            else if (!strcmp (argv[1], "white"))
            {
                SCOPE_RING_color_set (SCOPE_RING_LED_INDICATORS, RGB_COLOR_WHITE);
            }
            else
            {
                goto USAGE;
            }

            if (argc == 3)
            {
                uint8_t step = (uint8_t) strtoul (argv[2], NULL, 0);

                SCOPE_RING_brightness_set (SCOPE_RING_LED_INDICATORS, step, true);
            }
        }
        else if (!strcmp (argv[0], "-l") || !strcmp (argv[0], "--level"))
        {
            if (!strcmp (argv[1], "red"))
            {
                SCOPE_RING_color_set (SCOPE_RING_LED_LEVEL, RGB_COLOR_RED);
            }
            else if (!strcmp (argv[1], "green"))
            {
                SCOPE_RING_color_set (SCOPE_RING_LED_LEVEL, RGB_COLOR_GREEN);
            }
            else if (!strcmp (argv[1], "blue"))
            {
                SCOPE_RING_color_set (SCOPE_RING_LED_LEVEL, RGB_COLOR_BLUE);
            }
            else if (!strcmp (argv[1], "orange"))
            {
                SCOPE_RING_color_set (SCOPE_RING_LED_LEVEL, RGB_COLOR_ORANGE);
            }
            else if (!strcmp (argv[1], "yellow"))
            {
                SCOPE_RING_color_set (SCOPE_RING_LED_LEVEL, RGB_COLOR_YELLOW);
            }
            else if (!strcmp (argv[1], "purple"))
            {
                SCOPE_RING_color_set (SCOPE_RING_LED_LEVEL, RGB_COLOR_PURPLE);
            }
            else if (!strcmp (argv[1], "pink"))
            {
                SCOPE_RING_color_set (SCOPE_RING_LED_LEVEL, RGB_COLOR_PINK);
            }
            else if (!strcmp (argv[1], "white"))
            {
                SCOPE_RING_color_set (SCOPE_RING_LED_LEVEL, RGB_COLOR_WHITE);
            }
            else
            {
                goto USAGE;
            }

            if (argc == 3)
            {
                uint8_t step = (uint8_t) strtoul (argv[2], NULL, 0);

                SCOPE_RING_brightness_set (SCOPE_RING_LED_LEVEL, step, true);
            }
        }
        else if (!strcmp (argv[0], "-b") || !strcmp (argv[0], "--background"))
        {
            if (!strcmp (argv[1], "red"))
            {
                SCOPE_RING_color_set (SCOPE_RING_LED_BACKGROUND, RGB_COLOR_RED);
            }
            else if (!strcmp (argv[1], "green"))
            {
                SCOPE_RING_color_set (SCOPE_RING_LED_BACKGROUND, RGB_COLOR_GREEN);
            }
            else if (!strcmp (argv[1], "blue"))
            {
                SCOPE_RING_color_set (SCOPE_RING_LED_BACKGROUND, RGB_COLOR_BLUE);
            }
            else if (!strcmp (argv[1], "orange"))
            {
                SCOPE_RING_color_set (SCOPE_RING_LED_BACKGROUND, RGB_COLOR_ORANGE);
            }
            else if (!strcmp (argv[1], "yellow"))
            {
                SCOPE_RING_color_set (SCOPE_RING_LED_BACKGROUND, RGB_COLOR_YELLOW);
            }
            else if (!strcmp (argv[1], "purple"))
            {
                SCOPE_RING_color_set (SCOPE_RING_LED_BACKGROUND, RGB_COLOR_PURPLE);
            }
            else if (!strcmp (argv[1], "pink"))
            {
                SCOPE_RING_color_set (SCOPE_RING_LED_BACKGROUND, RGB_COLOR_PINK);
            }
            else if (!strcmp (argv[1], "white"))
            {
                SCOPE_RING_color_set (SCOPE_RING_LED_BACKGROUND, RGB_COLOR_WHITE);
            }
            else
            {
                goto USAGE;
            }

            if (argc == 3)
            {
                uint8_t step = (uint8_t) strtoul (argv[2], NULL, 0);

                SCOPE_RING_brightness_set (SCOPE_RING_LED_BACKGROUND, step, true);
            }
        }
        else if (!strcmp (argv[0], "-ss") || !strcmp (argv[0], "--sensitivity"))
        {
            if (argc == 2)
            {
                float sensitivity = strtof (argv[1], NULL);

                SCOPE_RING_level_sensitivity_set (sensitivity);
            }
            else
            {
                goto USAGE;
            }
        }
        else if (!strcmp (argv[0], "-v") || !strcmp (argv[0], "--viscosity"))
        {
            if (argc == 2)
            {
                float alpha = strtof (argv[1], NULL);

                SCOPE_RING_level_viscosity_set (alpha);
            }
            else
            {
                goto USAGE;
            }
        }
        else if (!strcmp (argv[0], "-lo"))
        {
            uint8_t option = (uint8_t) strtoul (argv[1], NULL, 0);

            SCOPE_RING_level_option_set ((SCOPE_RING_level_option_e) option);
        }
        else
        {
            goto USAGE;
        }
    }
    else
    {
USAGE:
        PRINT (INFO, "Usage: scope_ring [OPTION...]\r\n");
        PRINT (INFO, "  -s | --set           Sets scope ring state\r\n");
        PRINT (INFO, "    <%%s>               off\r\n");
        PRINT (INFO, "                       on\r\n");
        PRINT (INFO, "                       setflat\r\n");
        PRINT (INFO, "                       calflat\r\n");
        PRINT (INFO, "                       setback\r\n");
        PRINT (INFO, "                       calback\r\n");
        PRINT (INFO, "                       setfront\r\n");
        PRINT (INFO, "                       calfront\r\n");
        PRINT (INFO, "                       lc - level color\r\n");
        PRINT (INFO, "                       lb - level brightness\r\n");
        PRINT (INFO, "                       ic - indicators color\r\n");
        PRINT (INFO, "                       ib - indicators brightness\r\n");
        PRINT (INFO, "                       bc - background color\r\n");
        PRINT (INFO, "                       bb - background brightness\r\n");
        PRINT (INFO, "                       rainbow\r\n");
        PRINT (INFO, "                       charging\r\n");
        PRINT (INFO, "                       cc - charger connected\r\n");
        PRINT (INFO, "                       cd - charger disconnected\r\n");
        PRINT (INFO, "                       power_on\r\n");
        PRINT (INFO, "                       power_off\r\n");
        PRINT (INFO, "  -i | --indicators    Sets color and brightness of the indicators\r\n");
        PRINT (INFO, "    <%%s>               red\r\n");
        PRINT (INFO, "                       green\r\n");
        PRINT (INFO, "                       blue\r\n");
        PRINT (INFO, "                       orange\r\n");
        PRINT (INFO, "                       yellow\r\n");
        PRINT (INFO, "                       purple\r\n");
        PRINT (INFO, "                       pink\r\n");
        PRINT (INFO, "                       white\r\n");
        PRINT (INFO, "    <%%d>               [0-%d]\r\n", SCOPE_RING_BRIGHTNESS_STEPS);
        PRINT (INFO, "  -l | --level         Sets the level colors\r\n");
        PRINT (INFO, "    <%%s>               red\r\n");
        PRINT (INFO, "                       green\r\n");
        PRINT (INFO, "                       blue\r\n");
        PRINT (INFO, "                       orange\r\n");
        PRINT (INFO, "                       yellow\r\n");
        PRINT (INFO, "                       purple\r\n");
        PRINT (INFO, "                       pink\r\n");
        PRINT (INFO, "                       white\r\n");
        PRINT (INFO, "    <%%d>               [0-%d]\r\n", SCOPE_RING_BRIGHTNESS_STEPS);
        PRINT (INFO, "  -b | --background    Sets the background colors\r\n");
        PRINT (INFO, "    <%%s>               red\r\n");
        PRINT (INFO, "                       green\r\n");
        PRINT (INFO, "                       blue\r\n");
        PRINT (INFO, "                       orange\r\n");
        PRINT (INFO, "                       yellow\r\n");
        PRINT (INFO, "                       purple\r\n");
        PRINT (INFO, "                       pink\r\n");
        PRINT (INFO, "                       white\r\n");
        PRINT (INFO, "    <%%d>               [0-%d]\r\n", SCOPE_RING_BRIGHTNESS_STEPS);
        PRINT (INFO, "  -ss | --sensitivity  Sets the LED level sensitivity\r\n");
        PRINT (INFO, "    <%%f>               (level, %.1f to %.1f)\r\n", SCOPE_RING_LEVEL_SENSITIVITY_MIN, SCOPE_RING_LEVEL_SENSITIVITY_MAX);
        PRINT (INFO, "  -v | --viscosity     Sets the LED level viscosity\r\n");
        PRINT (INFO, "    <%%f>               (alpha)\r\n");
        PRINT (INFO, "  -lo                  Sets the leveling option\r\n");
        PRINT (INFO, "    <%%d>               [0-2]\r\n");
        
    }
}


/*--------------------------------------------------------------------------------------------------
 * PRIVATE FUNCTION DEFINITIONS
 *------------------------------------------------------------------------------------------------*/
static void _scope_ring_task (void *p1, void *p2, void *p3)
{
    while (1)
    {
        uint8_t led;

        /* If timer counter has reached specified task duration, set state finished flag. */
        // if ((_scope_ring_state_duration != 0) && (_scope_ring_timer_cnt == _scope_ring_state_duration))
        // {
        //     _scope_ring_state_finished = true;
        // }

        switch (_scope_ring_state)
        {
            case SCOPE_RING_STATE_OFF:
                /* Turn off ring LEDs. */
                _scope_ring_enable (false);
                break;

            case SCOPE_RING_STATE_ON:
                if (_scope_ring_config.indicators_brightness == 0 &&
                    _scope_ring_config.level_brightness      == 0 &&
                    _scope_ring_config.background_brightness == 0)
                {
                    /* Disable FET to save on quiescent power. */
                    _scope_ring_enable (false);
                }
                else
                {
                    float angle = MOTION_angle_get (MOTION_AXIS_Y);

                    _scope_ring_background_set ();
                    _scope_ring_level_process (angle, _scope_ring_config.level_option);

                    /* Enable FET for power to ring prior to sending LED ring data. */
                    _scope_ring_enable (true);
                }
                break;

            case SCOPE_RING_STATE_SET_FLAT:
                {
                    static bool    up_down    = true;
                    static uint8_t brightness = 0;

                    for (led = 0; led < _SCOPE_RING_N_LEDS; led++)
                    {
                        LED_RING_set (led, brightness, 0, 0);
                    }

                    _scope_ring_enable (true);

                    if (up_down)
                    {
                        if (brightness == 100)
                        {
                            up_down = false;
                        }
                        else
                        {
                            brightness++;
                        }
                    }
                    else
                    {
                        if (brightness == 0)
                        {
                            up_down = true;
                        }
                        else
                        {
                            brightness--;
                        }
                    }
                }
                break;

            case SCOPE_RING_STATE_FLAT_CALIBRATION:
                {
                    static uint8_t prev_bound = 0;
                           uint8_t led_bound  = (uint32_t) (MOTION_calibrate_status_get (MOTION_CAL_ORIENTATION_FLAT) * (float) _SCOPE_RING_N_LEDS);
                           uint8_t led_idx;

                    if (led_bound == prev_bound)
                    {
                        break;
                    }
                    else
                    {
                        if (LED_RING_clear () != ERRNO_SUCCESS)
                        {
                            PRINT (ERROR, "Failure to clear LED ring.\r\n");
                        }   

                        for (led_idx = 0; led_idx < led_bound; led_idx++)
                        {
                            LED_RING_set (led_idx, 100, 0, 0);
                        }

                        /* Enable FET for power to ring prior to sending LED ring data. */
                        _scope_ring_enable (true);
                    }

                    prev_bound = led_bound;
                }
                break;

            case SCOPE_RING_STATE_SET_BACK:
                {
                    static bool    up_down    = true;
                    static uint8_t brightness = 0;

                    for (led = 0; led < _SCOPE_RING_N_LEDS; led++)
                    {
                        LED_RING_set (led, 0, brightness, 0);
                    }

                    _scope_ring_enable (true);

                    if (up_down)
                    {
                        if (brightness == 100)
                        {
                            up_down = false;
                        }
                        else
                        {
                            brightness++;
                        }
                    }
                    else
                    {
                        if (brightness == 0)
                        {
                            up_down = true;
                        }
                        else
                        {
                            brightness--;
                        }
                    }
                }
                break;

            case SCOPE_RING_STATE_BACK_CALIBRATION:
                {
                    static uint8_t prev_bound = 0;
                           uint8_t led_bound  = (uint32_t) (MOTION_calibrate_status_get (MOTION_CAL_ORIENTATION_BACK) * (float) _SCOPE_RING_N_LEDS);
                           uint8_t led_idx;

                    if (led_bound == prev_bound)
                    {
                        break;
                    }
                    else
                    {
                        if (LED_RING_clear () != ERRNO_SUCCESS)
                        {
                            PRINT (ERROR, "Failure to clear LED ring.\r\n");
                        }   

                        for (led_idx = 0; led_idx < led_bound; led_idx++)
                        {
                            LED_RING_set (led_idx, 0, 100, 0);
                        }

                        /* Enable FET for power to ring prior to sending LED ring data. */
                        _scope_ring_enable (true);
                    }

                    prev_bound = led_bound;
                }
                break;

            case SCOPE_RING_STATE_SET_FRONT:
                {
                    static bool    up_down    = true;
                    static uint8_t brightness = 0;

                    for (led = 0; led < _SCOPE_RING_N_LEDS; led++)
                    {
                        LED_RING_set (led, 0, 0, brightness);
                    }

                    _scope_ring_enable (true);

                    if (up_down)
                    {
                        if (brightness == 100)
                        {
                            up_down = false;
                        }
                        else
                        {
                            brightness++;
                        }
                    }
                    else
                    {
                        if (brightness == 0)
                        {
                            up_down = true;
                        }
                        else
                        {
                            brightness--;
                        }
                    }
                }
                break;

            case SCOPE_RING_STATE_FRONT_CALIBRATION:
                {
                    static uint8_t prev_bound = 0;
                           uint8_t led_bound  = (uint32_t) (MOTION_calibrate_status_get (MOTION_CAL_ORIENTATION_FRONT) * (float) _SCOPE_RING_N_LEDS);
                           uint8_t led_idx;

                    if (led_bound == prev_bound)
                    {
                        break;
                    }
                    else
                    {
                        if (LED_RING_clear () != ERRNO_SUCCESS)
                        {
                            PRINT (ERROR, "Failure to clear LED ring.\r\n");
                        }   

                        for (led_idx = 0; led_idx < led_bound; led_idx++)
                        {
                            LED_RING_set (led_idx, 0, 0, 100);
                        }

                        /* Enable FET for power to ring prior to sending LED ring data. */
                        _scope_ring_enable (true);
                    }

                    prev_bound = led_bound;
                }
                break;


            case SCOPE_RING_STATE_CONFIG_LEVEL_COLOR:
                if (_scope_ring_timer_cnt >= (_scope_ring_state_duration / 2))
                {
                    if (LED_RING_clear () != ERRNO_SUCCESS)
                    {
                        PRINT (ERROR, "Failure to clear LED ring.\r\n");
                    }

                    /* Enable FET for power to ring prior to sending LED ring data. */
                    _scope_ring_enable (true);

                    if (_scope_ring_timer_cnt == _scope_ring_state_duration)
                    {
                        _scope_ring_timer_cnt = 0;
                    }
                }
                else
                {
                    if (LED_RING_clear () != ERRNO_SUCCESS)
                    {
                        PRINT (ERROR, "Failure to clear LED ring.\r\n");
                    }
                    _scope_ring_level_process (0.0, _scope_ring_config.level_option);

                    /* Enable FET for power to ring prior to sending LED ring data. */
                    _scope_ring_enable (true);
                }
                break;

            case SCOPE_RING_STATE_CONFIG_LEVEL_BRIGHTNESS:
                if (LED_RING_clear () != ERRNO_SUCCESS)
                {
                    PRINT (ERROR, "Failure to clear LED ring.\r\n");
                }
                _scope_ring_level_process (0.0, _scope_ring_config.level_option);

                /* Enable FET for power to ring prior to sending LED ring data. */
                _scope_ring_enable (true);
                break;

            case SCOPE_RING_STATE_CONFIG_INDICATORS_COLOR:
                if (_scope_ring_timer_cnt >= (_scope_ring_state_duration / 2))
                {
                    if (LED_RING_clear () != ERRNO_SUCCESS)
                    {
                        PRINT (ERROR, "Failure to clear LED ring.\r\n");
                    }

                    /* Enable FET for power to ring prior to sending LED ring data. */
                    _scope_ring_enable (true);

                    if (_scope_ring_timer_cnt == _scope_ring_state_duration)
                    {
                        _scope_ring_timer_cnt = 0;
                    }
                }
                else
                {
                    /* SCOPE_RING_BRIGHTNESS_STEPS + 1 extra step for off. */
                    RGB_s indicators = RGB_gamma_corrected (_scope_ring_config.indicators_color,
                                                            _scope_ring_config.indicators_brightness,
                                                            (SCOPE_RING_BRIGHTNESS_STEPS + 1),
                                                            RGB_BRIGHTNESS_MAX,
                                                            SCOPE_RING_BRIGHTNESS_GAMMA_CF);

                    if (LED_RING_clear () != ERRNO_SUCCESS)
                    {
                        PRINT (ERROR, "Failure to clear LED ring.\r\n");
                    }   

                    LED_RING_set ( 3, indicators.r, indicators.g, indicators.b);
                    LED_RING_set ( 9, indicators.r, indicators.g, indicators.b);
                    LED_RING_set (15, indicators.r, indicators.g, indicators.b);
                    LED_RING_set (21, indicators.r, indicators.g, indicators.b);

                    /* Enable FET for power to ring prior to sending LED ring data. */
                    _scope_ring_enable (true);
                }
                break;

            case SCOPE_RING_STATE_CONFIG_INDICATORS_BRIGHTNESS:
                {
                    /* SCOPE_RING_BRIGHTNESS_STEPS + 1 extra step for off. */
                    RGB_s indicators = RGB_gamma_corrected (_scope_ring_config.indicators_color,
                                                            _scope_ring_config.indicators_brightness,
                                                            (SCOPE_RING_BRIGHTNESS_STEPS + 1),
                                                            RGB_BRIGHTNESS_MAX,
                                                            SCOPE_RING_BRIGHTNESS_GAMMA_CF);

                    if (LED_RING_clear () != ERRNO_SUCCESS)
                    {
                        PRINT (ERROR, "Failure to clear LED ring.\r\n");
                    }   

                    LED_RING_set ( 3, indicators.r, indicators.g, indicators.b);
                    LED_RING_set ( 9, indicators.r, indicators.g, indicators.b);
                    LED_RING_set (15, indicators.r, indicators.g, indicators.b);
                    LED_RING_set (21, indicators.r, indicators.g, indicators.b);

                    /* Enable FET for power to ring prior to sending LED ring data. */
                    _scope_ring_enable (true);
                }
                break;

            case SCOPE_RING_STATE_CONFIG_BACKGROUND_COLOR:
                if (_scope_ring_timer_cnt >= (_scope_ring_state_duration / 2))
                {
                    if (LED_RING_clear () != ERRNO_SUCCESS)
                    {
                        PRINT (ERROR, "Failure to clear LED ring.\r\n");
                    }

                    /* Enable FET for power to ring prior to sending LED ring data. */
                    _scope_ring_enable (true);

                    if (_scope_ring_timer_cnt == _scope_ring_state_duration)
                    {
                        _scope_ring_timer_cnt = 0;
                    }
                }
                else
                {                    
                    if (LED_RING_clear () != ERRNO_SUCCESS)
                    {
                        PRINT (ERROR, "Failure to clear LED ring.\r\n");
                    }

                    _scope_ring_background_set ();

                    /* Enable FET for power to ring prior to sending LED ring data. */
                    _scope_ring_enable (true);
                }
                break;
                
            case SCOPE_RING_STATE_CONFIG_BACKGROUND_BRIGHTNESS:
                if (LED_RING_clear () != ERRNO_SUCCESS)
                {
                    PRINT (ERROR, "Failure to clear LED ring.\r\n");
                }
                _scope_ring_background_set ();
                /* Enable FET for power to ring prior to sending LED ring data. */
                _scope_ring_enable (true);
                break;

            case SCOPE_RING_STATE_CONFIG_LEVEL_SENSITIVITY:
                {
                    uint8_t bound = (uint8_t) ((SCOPE_RING_LEVEL_SENSITIVITY_MAX +
                                                SCOPE_RING_LEVEL_SENSITIVITY_MIN -
                                                _scope_ring_config.level_sensitivity) /
                                               (float) SCOPE_RING_LEVEL_SENSITIVITY_MAX *
                                               (float) LED_RING_PIXELS);

                    if (LED_RING_clear () != ERRNO_SUCCESS)
                    {
                        PRINT (ERROR, "Failure to clear LED ring.\r\n");
                    }

                    /* Use purple to display sensitivity. */
                    RGB_s purple = RGB_gamma_corrected (RGB_COLOR_PURPLE,
                                                        10,
                                                        (SCOPE_RING_BRIGHTNESS_STEPS + 1),
                                                        RGB_BRIGHTNESS_MAX,
                                                        SCOPE_RING_BRIGHTNESS_GAMMA_CF);

                    for (led = 0; led < bound; led++)
                    {
                        LED_RING_set (led, purple.r, purple.g, purple.b);
                    }
                    /* Enable FET for power to ring prior to sending LED ring data. */
                    _scope_ring_enable (true);
                }
                break;

            case SCOPE_RING_STATE_CONFIG_LEVEL_OPTION:
                {
                    RGB_s indicators = RGB_gamma_corrected (_scope_ring_config.indicators_color,
                                                            (RGB_BRIGHTNESS_MAX / 2),
                                                            SCOPE_RING_BRIGHTNESS_STEP_DEFAULT,
                                                            RGB_BRIGHTNESS_MAX,
                                                            SCOPE_RING_BRIGHTNESS_GAMMA_CF);

                    if (LED_RING_clear () != ERRNO_SUCCESS)
                    {
                        PRINT (ERROR, "Failure to clear LED ring.\r\n");
                    }

                    switch (_scope_ring_config.level_option)
                    {
                        case SCOPE_RING_LEVEL_OPTION_0:
                            LED_RING_set (15, indicators.r, indicators.g, indicators.b);
                            LED_RING_set (21, indicators.r, indicators.g, indicators.b);
                            break;
                        case SCOPE_RING_LEVEL_OPTION_1:
                            LED_RING_set (18, indicators.r, indicators.g, indicators.b);
                            break;
                        case SCOPE_RING_LEVEL_OPTION_2:
                            LED_RING_set ( 9, indicators.r, indicators.g, indicators.b);
                            LED_RING_set (21, indicators.r, indicators.g, indicators.b);
                            break;
                    }
                    /* Enable FET for power to ring prior to sending LED ring data. */
                    _scope_ring_enable (true);
                }
                break;

            case SCOPE_RING_STATE_RAINBOW:
                {
                    static uint16_t count = 0;
                    LED_RING_rainbow (count, 1, 255, 100, 1);
                    
                    count += 256;

                    _scope_ring_enable (true);
                }
                break;

            case SCOPE_RING_STATE_CHARGING:
                if (_scope_ring_timer_cnt < (_scope_ring_state_duration / 2))
                {
                    uint8_t bounds = (uint8_t) (_scope_ring_battery_life * _SCOPE_RING_N_LEDS);

                    if (LED_RING_clear () != ERRNO_SUCCESS)
                    {
                        PRINT (ERROR, "Failure to clear LED ring.\r\n");
                    }

                    for (led = 0; led < bounds; led++)
                    {
                        LED_RING_set (led, 0, 0, 50);
                    }

                    /* Enable FET for power to ring prior to sending LED ring data. */
                    _scope_ring_enable (true);
                }
                else if (_scope_ring_timer_cnt == (_scope_ring_state_duration / 2))
                {
                    float voltage;
                    float life_remaining;

                    /* Disable scope ring to prevent voltage drop before getting battery reading. */
                    _scope_ring_enable (false);

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

                    life_remaining = BATTERY_level_calculate (voltage);

                    /* Set battery status for feedback on LED ring. */
                    SCOPE_RING_battery_indicator_set (life_remaining);
                }
                else if (_scope_ring_timer_cnt >= _scope_ring_state_duration)
                {
                    _scope_ring_timer_cnt = 0;
                }
                break;

            case SCOPE_RING_STATE_CHARGER_CONNECTED:
                if (!_scope_ring_state_finished)
                {
                    static uint8_t bounds = 0;

                    if (LED_RING_clear () != ERRNO_SUCCESS)
                    {
                        PRINT (ERROR, "Failure to clear LED ring.\r\n");
                    }

                    for (led = 0; led < bounds; led++)
                    {
                        LED_RING_set (led, 50, 50, 50);
                    }

                    /* Enable FET for power to ring prior to sending LED ring data. */
                    _scope_ring_enable (true);

                    if (bounds == (_SCOPE_RING_N_LEDS - 1))
                    {
                        /* Force finish flag. */
                        _scope_ring_state_finished = true;

                        bounds = 0;
                    }
                    else
                    {
                        bounds++;
                    }
                }
                break;

            case SCOPE_RING_STATE_CHARGER_DISCONNECTED:
                if (!_scope_ring_state_finished)
                {
                    static uint8_t bounds = (_SCOPE_RING_N_LEDS - 1);

                    if (LED_RING_clear () != ERRNO_SUCCESS)
                    {
                        PRINT (ERROR, "Failure to clear LED ring.\r\n");
                    }

                    for (led = 0; led < bounds; led++)
                    {
                        LED_RING_set (led, 50, 50, 50);
                    }

                    /* Enable FET for power to ring prior to sending LED ring data. */
                    _scope_ring_enable (true);

                    if (bounds == 0)
                    {
                        /* Force finish flag. */
                        _scope_ring_state_finished = true;

                        bounds = (_SCOPE_RING_N_LEDS - 1);
                    }
                    else
                    {
                        bounds--;
                    }
                }
                break;

            case SCOPE_RING_STATE_POWER_ON:
                if (!_scope_ring_state_finished)
                {    
                           uint8_t bounds     = (uint8_t) (_scope_ring_battery_life * _SCOPE_RING_N_LEDS);
                    static uint8_t brightness = 0;

                    for (led = 0; led < _SCOPE_RING_N_LEDS; led++)
                    {
                        if (led < bounds)
                        {
                            LED_RING_set (led, 0, brightness, 0);
                        }
                        else
                        {
                            LED_RING_set (led, 0, 0, 0);
                        }
                    }

                    _scope_ring_enable (true);

                    if (brightness == 100)
                    {
                        _scope_ring_state_finished = true;
                        brightness = 0;
                    }
                    else
                    {
                        brightness++;
                    }
                }
                break;

            case SCOPE_RING_STATE_POWER_OFF:
                if (!_scope_ring_state_finished)
                {    
                           uint8_t bounds     = (uint8_t) (_scope_ring_battery_life * _SCOPE_RING_N_LEDS);
                    static uint8_t brightness = 100;

                    for (led = 0; led < _SCOPE_RING_N_LEDS; led++)
                    {
                        if (led < bounds)
                        {
                            LED_RING_set (led, 0, brightness, 0);
                        }
                        else
                        {
                            LED_RING_set (led, 0, 0, 0);
                        }
                    }

                    _scope_ring_enable (true);

                    if (brightness > 0)
                    {
                        brightness--;
                    }
                    else if (brightness == 0)
                    {
                        _scope_ring_state_finished = true;
                        brightness = 100;
                    }
                }
                break;

            case SCOPE_RING_STATE_BLE_ENABLED:
                if (!_scope_ring_state_finished)
                {
                    static uint8_t bounds = 0;

                    if (LED_RING_clear () != ERRNO_SUCCESS)
                    {
                        PRINT (ERROR, "Failure to clear LED ring.\r\n");
                    }

                    for (led = 0; led < bounds; led++)
                    {
                        LED_RING_set (led, 0, 0, 50);
                    }

                    /* Enable FET for power to ring prior to sending LED ring data. */
                    _scope_ring_enable (true);

                    if (bounds == (_SCOPE_RING_N_LEDS - 1))
                    {
                        /* Force finish flag. */
                        _scope_ring_state_finished = true;

                        bounds = 0;
                    }
                    else
                    {
                        bounds++;
                    }
                }
                break;

            case SCOPE_RING_STATE_BLE_DISABLED:
                if (!_scope_ring_state_finished)
                {
                    static uint8_t bounds = (_SCOPE_RING_N_LEDS - 1);

                    if (LED_RING_clear () != ERRNO_SUCCESS)
                    {
                        PRINT (ERROR, "Failure to clear LED ring.\r\n");
                    }

                    for (led = (_SCOPE_RING_N_LEDS - 1); led > bounds; led--)
                    {
                        LED_RING_set (led, 0, 0, 50);
                    }

                    /* Enable FET for power to ring prior to sending LED ring data. */
                    _scope_ring_enable (true);

                    if (bounds == 0)
                    {
                        /* Force finish flag. */
                        _scope_ring_state_finished = true;

                        bounds = (_SCOPE_RING_N_LEDS - 1);
                    }
                    else
                    {
                        bounds--;
                    }
                }
                break;
        }

        _scope_ring_timer_cnt += SCOPE_RING_TASK_PERIOD_MS;

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

        k_msleep (SCOPE_RING_TASK_PERIOD_MS);
    }
}

static void _scope_ring_level_process (float angle, SCOPE_RING_level_option_e option)
{
    uint8_t led_idx;
    uint8_t multiplier;
    RGB_s   level;
    RGB_s   indicators;

    /* Negating angle turns on opposite LED.  Indicating to cant the other direction to level. */
    // angle = -angle;

    /* Calculate scaled level color and brightness. SCOPE_RING_BRIGHTNESS_STEPS + 1 extra step for off. */
    level = RGB_gamma_corrected (_scope_ring_config.level_color,
                                 _scope_ring_config.level_brightness,
                                 (SCOPE_RING_BRIGHTNESS_STEPS + 1),
                                 RGB_BRIGHTNESS_MAX,
                                 SCOPE_RING_BRIGHTNESS_GAMMA_CF);

    /* Calculate scaled indicators color and brightness. SCOPE_RING_BRIGHTNESS_STEPS + 1 extra step for off. */
    indicators = RGB_gamma_corrected (_scope_ring_config.indicators_color,
                                      _scope_ring_config.indicators_brightness,
                                      (SCOPE_RING_BRIGHTNESS_STEPS + 1),
                                      RGB_BRIGHTNESS_MAX,
                                      SCOPE_RING_BRIGHTNESS_GAMMA_CF);

    // angle = _scope_ring_angle_hysteresis_get (angle);

    angle = _scope_ring_level_viscosity_get (angle, _scope_ring_config.level_viscosity);
    
    switch (option)
    {
        case SCOPE_RING_LEVEL_OPTION_0:
            /* Check if angle is within desired window. */
            if (angle >= -_scope_ring_config.level_sensitivity &&
                angle <=  _scope_ring_config.level_sensitivity)
            {
                if (_scope_ring_config.level_brightness == 0)
                {
                    _scope_ring_background_set ();
                }
                else
                {
                    _scope_ring_mirrored_set (0, level.r, level.g, level.b);
                }
            }
            else if (angle < 0)
            {
                multiplier = _SCOPE_RING_N_LEDS - _SCOPE_RING_L_INDICATOR_IDX;

                for (led_idx = _SCOPE_RING_L_INDICATOR_IDX; led_idx < _SCOPE_RING_N_LEDS; led_idx++)
                {
                    if (angle <= (-_scope_ring_config.level_sensitivity * multiplier--))
                    {
                        _scope_ring_mirrored_set (led_idx, indicators.r, indicators.g, indicators.b);
                        return;
                    }
                }
            }
            else
            {
                multiplier = _SCOPE_RING_R_INDICATOR_IDX;

                for (led_idx = _SCOPE_RING_R_INDICATOR_IDX; led_idx > 0 ; led_idx--)
                {
                    if (angle >= (_scope_ring_config.level_sensitivity * multiplier--))
                    {
                        _scope_ring_mirrored_set (led_idx, indicators.r, indicators.g, indicators.b);
                        return;
                    }
                }
            }
            break;

        case SCOPE_RING_LEVEL_OPTION_1:
            /* Check if angle is within desired window. */
            if (angle >= -_scope_ring_config.level_sensitivity &&
                angle <=  _scope_ring_config.level_sensitivity)
            {
                if (_scope_ring_config.level_brightness == 0)
                {
                    _scope_ring_background_set ();
                }
                else
                {
                    /* Set both left and right leds when level. */
                    LED_RING_set (_SCOPE_RING_R_INDICATOR_IDX, level.r, level.g, level.b);
                    LED_RING_set (_SCOPE_RING_L_INDICATOR_IDX, level.r, level.g, level.b);
                }
            }
            else if (angle < 0)
            {
                /* Set left LED if we need to cant left for level. */
                LED_RING_set (_SCOPE_RING_L_INDICATOR_IDX, indicators.r, indicators.g, indicators.b);
            }
            else
            {
                /* Set right LED if we need to cant right for level. */
                LED_RING_set (_SCOPE_RING_R_INDICATOR_IDX, indicators.r, indicators.g, indicators.b);
            }
            break;

        case SCOPE_RING_LEVEL_OPTION_2:
            /* Check if angle is within desired window. */
            if (angle >= -_scope_ring_config.level_sensitivity &&
                angle <=  _scope_ring_config.level_sensitivity)
            {
                if (_scope_ring_config.level_brightness == 0)
                {
                    _scope_ring_background_set ();
                }
                else
                {
                    /* Set both left and right leds when level. */
                    LED_RING_set (_SCOPE_RING_R_INDICATOR_IDX, level.r, level.g, level.b);
                    LED_RING_set (_SCOPE_RING_L_INDICATOR_IDX, level.r, level.g, level.b);

                    // LED_RING_set (_SCOPE_RING_R_INDICATOR_IDX-1, level.r, level.g, level.b);
                    // LED_RING_set (_SCOPE_RING_R_INDICATOR_IDX+1, level.r, level.g, level.b);

                    // LED_RING_set (_SCOPE_RING_L_INDICATOR_IDX-1, level.r, level.g, level.b);
                    // LED_RING_set (_SCOPE_RING_L_INDICATOR_IDX+1, level.r, level.g, level.b);
                }
            }
            else if (angle < 0)
            {
                multiplier = _SCOPE_RING_N_LEDS - _SCOPE_RING_L_INDICATOR_IDX - 2;

                for (led_idx = (_SCOPE_RING_N_LEDS - 2); led_idx > _SCOPE_RING_L_INDICATOR_IDX; led_idx--)
                {
                    if (angle <= (-_scope_ring_config.level_sensitivity * multiplier--))
                    {
                        LED_RING_set (led_idx, indicators.r, indicators.g, indicators.b);
                        LED_RING_set ((led_idx + (_SCOPE_RING_N_LEDS / 2)) % _SCOPE_RING_N_LEDS, indicators.r, indicators.g, indicators.b);
                        return;
                    }
                }
            }
            else
            {
                multiplier = _SCOPE_RING_R_INDICATOR_IDX - 2;

                for (led_idx = 2; led_idx < _SCOPE_RING_R_INDICATOR_IDX; led_idx++)
                {
                    if (angle >= (_scope_ring_config.level_sensitivity * multiplier--))
                    {
                        LED_RING_set (led_idx, indicators.r, indicators.g, indicators.b);
                        LED_RING_set (led_idx + (_SCOPE_RING_N_LEDS / 2), indicators.r, indicators.g, indicators.b);
                        return;
                    }
                }
            }
            break;
    }
}

static void _scope_ring_background_set (void)
{
    uint8_t led_idx;
    /* Background LEDs never need to be full brightness, we scale back max to 40 instead of 255.
     * With max of 40, we use a different gamma value of 1.2.
     * SCOPE_RING_BRIGHTNESS_STEPS + 1 extra step for off. */
    RGB_s   ring = RGB_gamma_corrected (_scope_ring_config.background_color,
                                        _scope_ring_config.background_brightness,
                                        (SCOPE_RING_BRIGHTNESS_STEPS + 1),
                                        40,
                                        1.2);

    /* Turn on all leds to ring color. */
    for (led_idx = 0; led_idx < _SCOPE_RING_N_LEDS; led_idx++)
    {
        LED_RING_set (led_idx, ring.r, ring.g, ring.b);
    }
}

static void _scope_ring_mirrored_set (uint8_t led, uint8_t r, uint8_t g, uint8_t b)
{
    /* Set top half. */
    LED_RING_set (led, r, g, b);

    /* Check if we are on left half. */
    if (led > (_SCOPE_RING_N_LEDS / 2))
    {
        /* Also set mirrored side. */
        LED_RING_set ((_SCOPE_RING_N_LEDS - led) + (_SCOPE_RING_N_LEDS / 2), r, g, b);
    }
    /* Check if we are on right half. */
    else
    {
        /* Also set mirrored side. */
        LED_RING_set (((_SCOPE_RING_N_LEDS / 2) - led), r, g, b);
    }
}

static void _scope_ring_enable (bool en)
{
    if (!en)
    {
        /* If disabled, send data to turn off all LEDs before removing power. */
        if (LED_RING_clear () != ERRNO_SUCCESS)
        {
            PRINT (ERROR, "Failure to clear LED ring.\r\n");
        }

        if (LED_RING_update () != ERRNO_SUCCESS)
        {
            PRINT (ERROR, "Failure to update LED ring.\r\n");
        }

        /* Enable / disable FET gating LED ring power. */
        LED_RING_enable (false);
    }
    else
    {
        /* Enable FET gating LED ring power before sending pattern. */
        LED_RING_enable (true);

        if (LED_RING_update () != ERRNO_SUCCESS)
        {
            PRINT (ERROR, "Failure to update LED ring.\r\n");
        }
    }
}

static void _scope_ring_config_read (void)
{
    ERRNO_e             err;
    SCOPE_RING_config_s cfg;

    err = FS_open_or_create (SCOPE_RING_FILENAME, &_scope_ring_file, sizeof (SCOPE_RING_config_s), FS_FILE_DEPTH_DEFAULT);
    if (err == ERRNO_SUCCESS)
    {
        /* Read file. */
        err = FS_read (_scope_ring_file, (uint8_t *) &cfg, 0, sizeof (SCOPE_RING_config_s));

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

        /* Check if file contents are valid. */
        if (cfg.indicators_color      >= RGB_COLOR_INVALID             ||
            cfg.level_color           >= RGB_COLOR_INVALID             ||
            cfg.background_color      >= RGB_COLOR_INVALID             ||
            cfg.indicators_brightness > SCOPE_RING_BRIGHTNESS_STEPS    ||
            cfg.level_brightness      > SCOPE_RING_BRIGHTNESS_STEPS    ||
            cfg.background_brightness > SCOPE_RING_BRIGHTNESS_STEPS    || 
            cfg.level_option          >= SCOPE_RING_LEVEL_OPTION_COUNT || 
            cfg.level_sensitivity      <= (float) 0.0                   ||
            cfg.level_viscosity       <= (float) 0.0)
        {
            /* If not valid, write current configuration to file. */
            _scope_ring_config_write ();
            return;
        }

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

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

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

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

static void _scope_ring_config_write (void)
{
    if (FS_write (_scope_ring_file, (uint8_t *) &_scope_ring_config) != ERRNO_SUCCESS)
    {
        PRINT (ERROR, "Failed to write scope ring configuration to file.\r\n");
    }
}

static float _scope_ring_angle_hysteresis_get (float angle)
{
    static float prev_angle = 0.0;

    /* Increasing. */
    if (angle > prev_angle)
    {
        if (angle > prev_angle + SCOPE_RING_HYS_ANGLE_THRESHOLD)
        {
            /* Update previous angle. */
            prev_angle = angle;
        }
        else
        {
            angle = prev_angle;
        }
    }
    /* Decreasing. */
    else if (angle < prev_angle)
    {
        if (angle < prev_angle - SCOPE_RING_HYS_ANGLE_THRESHOLD)
        {
            /* Update previous angle. */
            prev_angle = angle;
        }
        else
        {
            angle = prev_angle;
        }
    }
    
    return angle;
}

static float _scope_ring_level_viscosity_get (float angle, float alpha)
{
    static float previous_angle = 0.0;
    
    previous_angle = alpha * angle + (1.0f - alpha) * previous_angle;

    return previous_angle;
}
