/** 
 * @file    virtual_led_ring.c
 * @brief   Virtual LED ring module to simulate visual effect of having more LEDs.
 * @author  Kenny Tran
 * @date    Nov 13 2025
 * @version 0.0.1
 *
 * @section LICENSES
 * Copyright (C) Morgan Advanced Programmable Systems, Inc.
 */


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

#include "../lib/errno.h"

#include "../bsp/led_ring.h"

#include "virtual_led_ring.h"


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


/*--------------------------------------------------------------------------------------------------
 * PRIVATE TYPE DEFINITIONS
 *------------------------------------------------------------------------------------------------*/
typedef struct _virtual_led
{
    uint16_t r;
    uint16_t g;
    uint16_t b;
} _virtual_led_s;


/*--------------------------------------------------------------------------------------------------
 * PRIVATE VARIABLES
 *------------------------------------------------------------------------------------------------*/
static bool           _virtual_led_ring_init = false;
static _virtual_led_s _virtual_led_ring_buf[LED_RING_PIXELS];


/*--------------------------------------------------------------------------------------------------
 * PRIVATE FUNCTION PROTOTYPES
 *------------------------------------------------------------------------------------------------*/
static void    _virtural_led_ring_acc_clear (void);
static void    _virtual_led_ring_acc_add (uint16_t idx, uint16_t r, uint16_t g, uint16_t b);
static ERRNO_e _virtual_led_ring_acc_commit (void);
static float   _virtual_led_ring_distance_calc(float pos1, float pos2);


/*--------------------------------------------------------------------------------------------------
 * PUBLIC FUNCTIONS
 *------------------------------------------------------------------------------------------------*/
ERRNO_e VIRTUAL_LED_RING_init (void)
{
    ERRNO_e err;

    if (_virtual_led_ring_init)
    {
        return ERRNO_ALREADY_INITIALIZED;
    }

    _virtural_led_ring_acc_clear ();

    err = LED_RING_init ();
    if (err != ERRNO_SUCCESS)
    {
        return err;
    }

    _virtual_led_ring_init = true;

    return ERRNO_SUCCESS;
}

ERRNO_e VIRTUAL_LED_RING_set (uint16_t idx, uint8_t r, uint8_t g, uint8_t b, VIRTUAL_LED_RING_mode_e mode)
{
    float position;

    if (!_virtual_led_ring_init)
    {
        return ERRNO_NOT_INITIALIZED;
    }
    else if (idx >= VIRTUAL_LED_RING_PIXELS)
    {
        return ERRNO_OUT_OF_RANGE;
    }
    
    position = (float) idx / (float) VIRTUAL_LED_RING_MULTIPLIER;

    return VIRTUAL_LED_RING_set_position (position, r, g, b, mode);
}

ERRNO_e VIRTUAL_LED_RING_set_position (float position, uint8_t r, uint8_t g, uint8_t b, VIRTUAL_LED_RING_mode_e mode)
{
    if (!_virtual_led_ring_init)
    {
        return ERRNO_NOT_INITIALIZED;
    }
    else if (position < 0.0f || position >= (float) LED_RING_PIXELS)
    {
        return ERRNO_OUT_OF_RANGE;
    }
    
    // _virtural_led_ring_acc_clear ();
    
    switch (mode)
    {
        case VIRTUAL_LED_RING_MODE_LINEAR:
        {
            /* Linear interpolation between two adjacent LEDs. */
            uint16_t led_low  = (uint16_t) position;
            uint16_t led_high = (led_low + 1) % LED_RING_PIXELS;
            float    fraction = position - (float) led_low;
            
            uint16_t brightness_low_r = (uint16_t) (r * (1.0f - fraction));
            uint16_t brightness_low_g = (uint16_t) (g * (1.0f - fraction));
            uint16_t brightness_low_b = (uint16_t) (b * (1.0f - fraction));
            
            uint16_t brightness_high_r = (uint16_t) (r * fraction);
            uint16_t brightness_high_g = (uint16_t) (g * fraction);
            uint16_t brightness_high_b = (uint16_t) (b * fraction);
            
            _virtual_led_ring_acc_add (led_low, brightness_low_r, brightness_low_g, brightness_low_b);
            _virtual_led_ring_acc_add (led_high, brightness_high_r, brightness_high_g, brightness_high_b);
            break;
        }
        
        case VIRTUAL_LED_RING_MODE_GAUSSIAN:
        {
            uint16_t i;

            /* Gaussian blur across multiple LEDs. */
            for (i = 0; i < LED_RING_PIXELS; i++)
            {
                float distance = _virtual_led_ring_distance_calc (position, (float) i);
                
                /* Gaussian falloff: exp(-(distance^2) / (2 * width^2)) */
                float intensity = expf (-(distance * distance) / (2.0f * VIRTUAL_LED_RING_GAUSSIAN_WIDTH * VIRTUAL_LED_RING_GAUSSIAN_WIDTH));
                
                /* Only add if intensity is significant (optimization). */
                if (intensity > 0.01f)
                {
                    _virtual_led_ring_acc_add (i, (uint16_t) (r * intensity), 
                                                  (uint16_t) (g * intensity),
                                                  (uint16_t) (b * intensity));
                }
            }
            break;
        }
        
        case VIRTUAL_LED_RING_MODE_COMET:
        {
            /* Trailing comet effect. */
            uint16_t i;
            uint16_t head  = (uint16_t) position;
            float    decay = 1.0f;
            
            for (i = 0; i < VIRTUAL_LED_RING_COMET_TAIL_LENGTH; i++)
            {
                uint16_t led_idx = (head - i + LED_RING_PIXELS) % LED_RING_PIXELS;
                
                _virtual_led_ring_acc_add (led_idx, (uint16_t) (r * decay),
                                                    (uint16_t) (g * decay),
                                                    (uint16_t) (b * decay));
                
                decay *= VIRTUAL_LED_RING_COMET_DECAY;
            }
            break;
        }
        
        default:
            return ERRNO_INVALID_PARAM;
    }
    
    return _virtual_led_ring_acc_commit ();
}

ERRNO_e VIRTUAL_LED_RING_clear (void)
{
    if (!_virtual_led_ring_init)
    {
        return ERRNO_NOT_INITIALIZED;
    }

    _virtural_led_ring_acc_clear ();

    return LED_RING_clear ();
}

ERRNO_e VIRTUAL_LED_RING_update (void)
{
    if (!_virtual_led_ring_init)
    {
        return ERRNO_NOT_INITIALIZED;
    }

    return LED_RING_update ();
}

ERRNO_e VIRTUAL_LED_RING_enable (bool en)
{
    if (!_virtual_led_ring_init)
    {
        return ERRNO_NOT_INITIALIZED;
    }

    return LED_RING_enable (en);
}

/*--------------------------------------------------------------------------------------------------
 * PRIVATE FUNCTIONS
 *------------------------------------------------------------------------------------------------*/
static void _virtural_led_ring_acc_clear (void)
{
    memset (_virtual_led_ring_buf, 0, sizeof (_virtual_led_ring_buf));
}

static void _virtual_led_ring_acc_add (uint16_t idx, uint16_t r, uint16_t g, uint16_t b)
{
    if (idx >= LED_RING_PIXELS)
    {
        return;
    }
    
    _virtual_led_ring_buf[idx].r += r;
    _virtual_led_ring_buf[idx].g += g;
    _virtual_led_ring_buf[idx].b += b;
    
    /* Clamp to max brightness. */
    if (_virtual_led_ring_buf[idx].r > LED_RING_MAX_BRIGHTNESS)
    {
        _virtual_led_ring_buf[idx].r = LED_RING_MAX_BRIGHTNESS;
    }
    if (_virtual_led_ring_buf[idx].g > LED_RING_MAX_BRIGHTNESS)
    {
        _virtual_led_ring_buf[idx].g = LED_RING_MAX_BRIGHTNESS;
    }
    if (_virtual_led_ring_buf[idx].b > LED_RING_MAX_BRIGHTNESS)
    {
        _virtual_led_ring_buf[idx].b = LED_RING_MAX_BRIGHTNESS;
    }
}

static ERRNO_e _virtual_led_ring_acc_commit (void)
{
    ERRNO_e  err;
    uint16_t i;
    
    for (i = 0; i < LED_RING_PIXELS; i++)
    {
        err = LED_RING_set (i, (uint8_t) _virtual_led_ring_buf[i].r, 
                               (uint8_t) _virtual_led_ring_buf[i].g,
                               (uint8_t) _virtual_led_ring_buf[i].b);
        if (err != ERRNO_SUCCESS)
        {
            return err;
        }
    }
    
    return ERRNO_SUCCESS;
}

static float _virtual_led_ring_distance_calc(float pos1, float pos2)
{
    float dist = fabsf(pos1 - pos2);
    
    /* Account for wraparound. */
    if (dist > (LED_RING_PIXELS / 2.0f))
    {
        dist = LED_RING_PIXELS - dist;
    }
    
    return dist;
}
