/*
 * Display_Color.c
 *
 * Created: Mon Jun 23 2025 13:15:42
 *  Author Chris
 */
#include "Display_Color.h"

// ============================================================================================
// Defines


// ============================================================================================
// Variables


// ============================================================================================
// Function Declarations


/*******************************************************************
	Functions
*******************************************************************/
/**
 * Interpolate between two RGB565 colors
 * @param start_color Starting color in RGB565 format
 * @param end_color   Ending color in RGB565 format  
 * @param factor      Interpolation factor (0.0 = start_color, 1.0 = end_color)
 * @return Interpolated color in RGB565 format
 */
Display_Color Display_Color_Interpolate_Float(Display_Color start_color, Display_Color end_color, float factor)
{
    // Clamp factor to [0, 1] range
    if (factor <= 0.0f) return start_color;
    if (factor >= 1.0f) return end_color;
    
    int Start_R = RED_FROM_RGB565(start_color);
    int Start_G = GREEN_FROM_RGB565(start_color);
    int Start_B = BLUE_FROM_RGB565(start_color);
    
    int End_R = RED_FROM_RGB565(end_color);
    int End_G = GREEN_FROM_RGB565(end_color);
    int End_B = BLUE_FROM_RGB565(end_color);
    
    // Interpolate each component
    int Interp_R = Start_R + (int)((float)(End_R - Start_R) * factor); 
    int Interp_G = Start_G + (int)((float)(End_G - Start_G) * factor);
    int Interp_B = Start_B + (int)((float)(End_B - Start_B) * factor);
    
    return DISPLAY_COLOR_FROM_RGB565(Interp_R, Interp_G, Interp_B);
}

/**
 * Alternative integer-only version for better performance on RP2350
 * Uses fixed-point arithmetic to avoid floating point operations
 * @param start_color Starting color in RGB565 format
 * @param end_color   Ending color in RGB565 format
 * @param factor      Interpolation factor as integer (0-256, where 256 = 1.0)
 * @return Interpolated color in RGB565 format
 */
Display_Color Display_Color_Interpolate_Fixpoint(Display_Color start_color, Display_Color end_color, uint16_t factor)
{
    // Clamp factor to [0, 256] range
    if (factor == 0) return start_color;
    if (factor >= 256) return end_color;
    
    int Start_R = RED_FROM_RGB565(start_color);
    int Start_G = GREEN_FROM_RGB565(start_color);
    int Start_B = BLUE_FROM_RGB565(start_color);
    
    int End_R = RED_FROM_RGB565(end_color);
    int End_G = GREEN_FROM_RGB565(end_color);
    int End_B = BLUE_FROM_RGB565(end_color);
    
    // Interpolate using fixed-point arithmetic
    uint16_t Interp_R = Start_R + (((End_R - Start_R) * factor) >> 8);
    uint16_t Interp_G = Start_G + (((End_G - Start_G) * factor) >> 8);
    uint16_t Interp_B = Start_B + (((End_B - Start_B) * factor) >> 8);

    return DISPLAY_COLOR_FROM_RGB565(Interp_R, Interp_G, Interp_B);
}

/**
 * Fast RGB565 alpha blending using fixed-point arithmetic
 * Optimized for RP2350's ARM Cortex-M33
 * @param src_color Source color in RGB565 format
 * @param dst_color Destination color in RGB565 format (from buffer)
 * @param alpha Alpha value (0-255, where 255 = fully opaque)
 * @return Blended color in RGB565 format
 */
__attribute__((always_inline))
inline Display_Color Display_Color_Blend_RGB565(Display_Color src_color, Display_Color dst_color, uint8_t alpha)
{
    // Early exit for fully opaque/transparent
    if (alpha == UINT8_MAX) {
        return src_color;
    }
    
    if (alpha == 0) {
        return dst_color;
    }

    // Extract RGB components from source
    uint16_t Src_R = RED_FROM_RGB565(src_color);
    uint16_t Src_G = GREEN_FROM_RGB565(src_color);
    uint16_t Src_B = BLUE_FROM_RGB565(src_color);
    
    // Extract RGB components from destination
    uint16_t Dst_R = RED_FROM_RGB565(dst_color);
    uint16_t Dst_G = GREEN_FROM_RGB565(dst_color);
    uint16_t Dst_B = BLUE_FROM_RGB565(dst_color);
    
    // Alpha blend using fixed-point arithmetic (alpha already 0-255)
    // Formula: result = (src * alpha + dst * (255 - alpha)) / 255
    uint16_t Inv_Alpha = 255 - alpha;
    
    uint16_t Blended_R = (Src_R * alpha + Dst_R * Inv_Alpha + 128) / 255;
    uint16_t Blended_G = (Src_G * alpha + Dst_G * Inv_Alpha + 128) / 255;
    uint16_t Blended_B = (Src_B * alpha + Dst_B * Inv_Alpha + 128) / 255;

    return DISPLAY_COLOR_FROM_RGB565(Blended_R, Blended_G, Blended_B);
}


/*******************************************************************
	Internal Functions
*******************************************************************/