// NeoPixel Driver using Direct Memory Access
//
// This implementation is based on the one by Alan Hawse of Elkhorn Creek,
// documented at https://iotexpert.com/2019/01/08/psoc-6-dma-ws2812-leds/.
// We are grateful to Mr. Hawse for sharing this.

#include "KTag.h"

#define NEOPIXEL_ZOFFSET (1)
#define NEOPIXEL_ONE3  (0b110<<24)
#define NEOPIXEL_ZERO3 (0b100<<24)
#define NEOPIXEL_SPI_BIT_PER_BIT (3)
#define NEOPIXEL_COLOR_PER_PIXEL (3)
#define NEOPIXEL_BYTES_PER_PIXEL (NEOPIXEL_SPI_BIT_PER_BIT * NEOPIXEL_COLOR_PER_PIXEL)
#define FRAME_BUFFER_SIZE (NEOPIXEL_ZOFFSET + (CONFIG_KTAG_MAX_NEOPIXELS_PER_CHANNEL * NEOPIXEL_BYTES_PER_PIXEL))

#if (CONFIG_KTAG_N_NEOPIXEL_CHANNELS == 1)
    static uint8_t NeoPixel_Barrel_Channel_Frame_Buffer[FRAME_BUFFER_SIZE];
#elif (CONFIG_KTAG_N_NEOPIXEL_CHANNELS == 4)  
    static uint8_t NeoPixel_Barrel_Channel_Frame_Buffer[FRAME_BUFFER_SIZE];
    static uint8_t NeoPixel_Receiver_Channel_Frame_Buffer[FRAME_BUFFER_SIZE];
    static uint8_t NeoPixel_Display_Channel_Frame_Buffer[FRAME_BUFFER_SIZE];
    static uint8_t NeoPixel_Effects_Channel_Frame_Buffer[FRAME_BUFFER_SIZE];
#else
    #error "Unsupported number of NeoPixel channels defined.  Supported configurations are 1 and 4."
#endif

static uint8_t* NeoPixel_Frame_Buffers[CONFIG_KTAG_N_NEOPIXEL_CHANNELS] = 
{
#if (CONFIG_KTAG_N_NEOPIXEL_CHANNELS == 1)
    NeoPixel_Barrel_Channel_Frame_Buffer
#elif (CONFIG_KTAG_N_NEOPIXEL_CHANNELS == 4)  
    NeoPixel_Barrel_Channel_Frame_Buffer,
    NeoPixel_Receiver_Channel_Frame_Buffer,
    NeoPixel_Display_Channel_Frame_Buffer,
    NeoPixel_Effects_Channel_Frame_Buffer
#else
    #error "Unsupported number of NeoPixel channels defined.  Supported configurations are 1 and 4."
#endif
};

ColorOrder_T ColorOrderByChannel[CONFIG_KTAG_N_NEOPIXEL_CHANNELS];

// Since the descriptors are (or should be) set to "trigger on descriptor completion" (`.interruptType = CY_DMA_DESCR`),
// this ISR is called after each channel has been written.
static void NeoPixel_DMA_Complete(void)
{
    static BaseType_t xHigherPriorityTaskWoken = pdFALSE;

    Cy_DMA_Channel_ClearInterrupt(DMA_NeoPixel_HW, DMA_NeoPixel_DW_CHANNEL);

    portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
}

#define NEOPIXEL_N_DESCRIPTORS CONFIG_KTAG_N_NEOPIXEL_CHANNELS
static cy_stc_dma_descriptor_t NeoPixel_Descriptors[NEOPIXEL_N_DESCRIPTORS];
static void NeoPixel_Configure_DMA(void)
{
    // I [AH] copied this structure from the PSoC Creator Component configuration 
    // in the generated source.
    const cy_stc_dma_descriptor_config_t NeoPixel_DMA_Descriptor_Config =
    {
    .retrigger       = CY_DMA_RETRIG_IM,
    .interruptType   = CY_DMA_DESCR,
    .triggerOutType  = CY_DMA_1ELEMENT,
    .channelState    = CY_DMA_CHANNEL_ENABLED,
    .triggerInType   = CY_DMA_1ELEMENT,
    .dataSize        = CY_DMA_BYTE,
    .srcTransferSize = CY_DMA_TRANSFER_SIZE_DATA,
    .dstTransferSize = CY_DMA_TRANSFER_SIZE_WORD,
    .descriptorType  = CY_DMA_1D_TRANSFER,
    .srcAddress      = NULL,
    .dstAddress      = NULL,
    .srcXincrement   = 1L,
    .dstXincrement   = 0L,
    .xCount          = 256UL,
    .srcYincrement   = 0L,
    .dstYincrement   = 0L,
    .yCount          = 1UL,
    .nextDescriptor  = NULL
    };

    for (uint_fast8_t i=0; i < NEOPIXEL_N_DESCRIPTORS; i++)
    {
        Cy_DMA_Descriptor_Init(&NeoPixel_Descriptors[i], &NeoPixel_DMA_Descriptor_Config);
        Cy_DMA_Descriptor_SetSrcAddress(&NeoPixel_Descriptors[i], (uint8_t *)&NeoPixel_Frame_Buffers[i][0]);
        Cy_DMA_Descriptor_SetDstAddress(&NeoPixel_Descriptors[i], (void *)&SPI_NeoPixel_HW->TX_FIFO_WR);
        Cy_DMA_Descriptor_SetXloopDataCount(&NeoPixel_Descriptors[i], FRAME_BUFFER_SIZE);
    }

    // Initialize and enable the interrupt from DMA_NeoPixel_HW.
    Cy_SysInt_Init(&DMA_NeoPixel_Int_cfg, &NeoPixel_DMA_Complete);
    NVIC_EnableIRQ(DMA_NeoPixel_Int_cfg.intrSrc);
    Cy_DMA_Channel_SetInterruptMask(DMA_NeoPixel_HW, DMA_NeoPixel_DW_CHANNEL, DMA_NeoPixel_INTR_MASK);
    
    Cy_DMA_Enable(DMA_NeoPixel_HW);    
}

// Function: NeoPixel_Trigger_DMA
// This function sets up the channel... then enables it to dump the frameBuffer to pixels.
void NeoPixel_Trigger_DMA(uint_fast8_t channel)
{    
    cy_stc_dma_channel_config_t channel_config;	
    channel_config.descriptor  = &NeoPixel_Descriptors[channel];
    channel_config.preemptable = DMA_NeoPixel_PREEMPTABLE;
    channel_config.priority    = DMA_NeoPixel_PRIORITY;
    channel_config.enable      = false;
    Cy_DMA_Channel_Init(DMA_NeoPixel_HW, DMA_NeoPixel_DW_CHANNEL, &channel_config);
    Cy_DMA_Channel_Enable(DMA_NeoPixel_HW, DMA_NeoPixel_DW_CHANNEL);
}

//! Takes an 8-bit value representing a color level and turns it into a WS2812 bit code...
/*!
 *  ...where 1=110 and 0=011
 *  One input byte turns into three output bytes of a uint32_t.
 */
uint32_t NeoPixel_ConvertTo3Code(uint8_t input)
{
    uint32_t rval=0;
    for (uint_fast8_t i=0; i < 8; i++)
    {
        if (input % 2)
        {
            rval |= NEOPIXEL_ONE3;
        }
        else
        {
            rval |= NEOPIXEL_ZERO3;
        }
        rval = rval >> 3;
        
        input = input >> 1;
    }
    return rval;
}

//! Takes a position and a three byte RGB value and updates the corresponding NeoPixel_Frame_Buffer with the correct nine bytes.
SystemKResult_T HW_NeoPixels_Set_RGB(NeoPixelsChannel_T channel, uint8_t position, uint8_t red, uint8_t green, uint8_t blue)
{
    typedef union {
    uint8_t bytes[4];
    uint32_t word;
    } NeoPixel_ColorByNumber;
    
    NeoPixel_ColorByNumber color;
    ColorOrder_T order = ColorOrderByChannel[channel];

    if (order == COLOR_ORDER_RGB)
    {
        color.word = NeoPixel_ConvertTo3Code(red);
        NeoPixel_Frame_Buffers[channel][position*NEOPIXEL_BYTES_PER_PIXEL+0+NEOPIXEL_ZOFFSET] = color.bytes[2];
        NeoPixel_Frame_Buffers[channel][position*NEOPIXEL_BYTES_PER_PIXEL+1+NEOPIXEL_ZOFFSET] = color.bytes[1];
        NeoPixel_Frame_Buffers[channel][position*NEOPIXEL_BYTES_PER_PIXEL+2+NEOPIXEL_ZOFFSET] = color.bytes[0];
        
        color.word = NeoPixel_ConvertTo3Code(green);
        NeoPixel_Frame_Buffers[channel][position*NEOPIXEL_BYTES_PER_PIXEL+3+NEOPIXEL_ZOFFSET] = color.bytes[2];
        NeoPixel_Frame_Buffers[channel][position*NEOPIXEL_BYTES_PER_PIXEL+4+NEOPIXEL_ZOFFSET] = color.bytes[1];
        NeoPixel_Frame_Buffers[channel][position*NEOPIXEL_BYTES_PER_PIXEL+5+NEOPIXEL_ZOFFSET] = color.bytes[0];

        color.word = NeoPixel_ConvertTo3Code(blue);
        NeoPixel_Frame_Buffers[channel][position*NEOPIXEL_BYTES_PER_PIXEL+6+NEOPIXEL_ZOFFSET] = color.bytes[2];
        NeoPixel_Frame_Buffers[channel][position*NEOPIXEL_BYTES_PER_PIXEL+7+NEOPIXEL_ZOFFSET] = color.bytes[1];
        NeoPixel_Frame_Buffers[channel][position*NEOPIXEL_BYTES_PER_PIXEL+8+NEOPIXEL_ZOFFSET] = color.bytes[0];
    }
    else if (order == COLOR_ORDER_GRB)
    {
        color.word = NeoPixel_ConvertTo3Code(green);
        NeoPixel_Frame_Buffers[channel][position*NEOPIXEL_BYTES_PER_PIXEL+0+NEOPIXEL_ZOFFSET] = color.bytes[2];
        NeoPixel_Frame_Buffers[channel][position*NEOPIXEL_BYTES_PER_PIXEL+1+NEOPIXEL_ZOFFSET] = color.bytes[1];
        NeoPixel_Frame_Buffers[channel][position*NEOPIXEL_BYTES_PER_PIXEL+2+NEOPIXEL_ZOFFSET] = color.bytes[0];
        
        color.word = NeoPixel_ConvertTo3Code(red);
        NeoPixel_Frame_Buffers[channel][position*NEOPIXEL_BYTES_PER_PIXEL+3+NEOPIXEL_ZOFFSET] = color.bytes[2];
        NeoPixel_Frame_Buffers[channel][position*NEOPIXEL_BYTES_PER_PIXEL+4+NEOPIXEL_ZOFFSET] = color.bytes[1];
        NeoPixel_Frame_Buffers[channel][position*NEOPIXEL_BYTES_PER_PIXEL+5+NEOPIXEL_ZOFFSET] = color.bytes[0];

        color.word = NeoPixel_ConvertTo3Code(blue);
        NeoPixel_Frame_Buffers[channel][position*NEOPIXEL_BYTES_PER_PIXEL+6+NEOPIXEL_ZOFFSET] = color.bytes[2];
        NeoPixel_Frame_Buffers[channel][position*NEOPIXEL_BYTES_PER_PIXEL+7+NEOPIXEL_ZOFFSET] = color.bytes[1];
        NeoPixel_Frame_Buffers[channel][position*NEOPIXEL_BYTES_PER_PIXEL+8+NEOPIXEL_ZOFFSET] = color.bytes[0];
    }
    else
    {
        // Color order is not handled--log this and use RGB.
        {
            static bool error_logged = false;
            if (error_logged == false)
            {
               COMM_Console_Print_String("Color order ");
               COMM_Console_Print_UInt8(order);
               COMM_Console_Print_String(" not yet supported!");
               error_logged = true;
            }
        }
        
        color.word = NeoPixel_ConvertTo3Code(red);
        NeoPixel_Frame_Buffers[channel][position*NEOPIXEL_BYTES_PER_PIXEL+0+NEOPIXEL_ZOFFSET] = color.bytes[2];
        NeoPixel_Frame_Buffers[channel][position*NEOPIXEL_BYTES_PER_PIXEL+1+NEOPIXEL_ZOFFSET] = color.bytes[1];
        NeoPixel_Frame_Buffers[channel][position*NEOPIXEL_BYTES_PER_PIXEL+2+NEOPIXEL_ZOFFSET] = color.bytes[0];
        
        color.word = NeoPixel_ConvertTo3Code(green);
        NeoPixel_Frame_Buffers[channel][position*NEOPIXEL_BYTES_PER_PIXEL+3+NEOPIXEL_ZOFFSET] = color.bytes[2];
        NeoPixel_Frame_Buffers[channel][position*NEOPIXEL_BYTES_PER_PIXEL+4+NEOPIXEL_ZOFFSET] = color.bytes[1];
        NeoPixel_Frame_Buffers[channel][position*NEOPIXEL_BYTES_PER_PIXEL+5+NEOPIXEL_ZOFFSET] = color.bytes[0];

        color.word = NeoPixel_ConvertTo3Code(blue);
        NeoPixel_Frame_Buffers[channel][position*NEOPIXEL_BYTES_PER_PIXEL+6+NEOPIXEL_ZOFFSET] = color.bytes[2];
        NeoPixel_Frame_Buffers[channel][position*NEOPIXEL_BYTES_PER_PIXEL+7+NEOPIXEL_ZOFFSET] = color.bytes[1];
        NeoPixel_Frame_Buffers[channel][position*NEOPIXEL_BYTES_PER_PIXEL+8+NEOPIXEL_ZOFFSET] = color.bytes[0];
    }
    
    return SYSTEMK_RESULT_SUCCESS;
}


//! Initializes the hardware.
SystemKResult_T HW_NeoPixels_Init(void)
{
    Cy_SCB_SPI_Init(SPI_NeoPixel_HW, &SPI_NeoPixel_config, &SPI_NeoPixel_context);
    Cy_SCB_SPI_Enable(SPI_NeoPixel_HW);
    NeoPixel_Configure_DMA();
    
#if (CONFIG_KTAG_N_NEOPIXEL_CHANNELS == 1)
    ColorOrderByChannel[NEOPIXEL_CHANNEL_BARREL] = NVM_BARREL_COLOR_ORDER;
#elif (CONFIG_KTAG_N_NEOPIXEL_CHANNELS == 4)  
    ColorOrderByChannel[NEOPIXEL_CHANNEL_BARREL] = NVM_BARREL_COLOR_ORDER;
    ColorOrderByChannel[NEOPIXEL_CHANNEL_RECEIVER] = NVM_RECEIVER_COLOR_ORDER;
    ColorOrderByChannel[NEOPIXEL_CHANNEL_DISPLAY] = NVM_DISPLAY_COLOR_ORDER;
    ColorOrderByChannel[NEOPIXEL_CHANNEL_EFFECTS] = NVM_EFFECTS_COLOR_ORDER;
#else
    #error "Unsupported number of NeoPixel channels defined.  Supported configurations are 1 and 4."
#endif

    return SYSTEMK_RESULT_SUCCESS;
}


#if (CONFIG_KTAG_N_NEOPIXEL_CHANNELS == 1)
static inline __attribute__((always_inline)) void NeoPixels_Set_Color_On_All_Channels(uint8_t position, color_t color) 
{
    HW_NeoPixels_Set_RGB(NEOPIXEL_CHANNEL_BARREL, position, Gamma8[Red(color)], Gamma8[Green(color)], Gamma8[Blue(color)]);
}

static inline __attribute__((always_inline)) void NeoPixel_Disable_All_Channels() 
{
    // Nothing to do.
}

static inline __attribute__((always_inline)) void NeoPixel_Enable_Channel(uint_fast8_t  __attribute__ ((unused)) channel)
{
    // Nothing to do.
}

static inline __attribute__((always_inline)) void NeoPixel_Disable_Channel(uint_fast8_t  __attribute__ ((unused)) channel)
{
    // Nothing to do.
}
#elif (CONFIG_KTAG_N_NEOPIXEL_CHANNELS == 4)  

static inline __attribute__((always_inline)) void NeoPixel_Enable_Barrel_Channel() 
{
    Cy_GPIO_Write(Pin_NeoPixel_Select_0_PORT, Pin_NeoPixel_Select_0_NUM, 1);
}

static inline __attribute__((always_inline)) void NeoPixel_Disable_Barrel_Channel() 
{
    Cy_GPIO_Write(Pin_NeoPixel_Select_0_PORT, Pin_NeoPixel_Select_0_NUM, 0);
}

static inline __attribute__((always_inline)) void NeoPixel_Enable_Receiver_Channel()
{
    Cy_GPIO_Write(Pin_NeoPixel_Select_1_PORT, Pin_NeoPixel_Select_1_NUM, 1);
}

static inline __attribute__((always_inline)) void NeoPixel_Disable_Receiver_Channel() 
{
    Cy_GPIO_Write(Pin_NeoPixel_Select_1_PORT, Pin_NeoPixel_Select_1_NUM, 0);
}

static inline __attribute__((always_inline)) void NeoPixel_Enable_Display_Channel() 
{
    Cy_GPIO_Write(Pin_NeoPixel_Select_2_PORT, Pin_NeoPixel_Select_2_NUM, 1);
}

static inline __attribute__((always_inline)) void NeoPixel_Disable_Display_Channel() 
{
    Cy_GPIO_Write(Pin_NeoPixel_Select_2_PORT, Pin_NeoPixel_Select_2_NUM, 0);
}

static inline __attribute__((always_inline)) void NeoPixel_Enable_Effects_Channel()
{
    Cy_GPIO_Write(Pin_NeoPixel_Select_3_PORT, Pin_NeoPixel_Select_3_NUM, 1);
}

static inline __attribute__((always_inline)) void NeoPixel_Disable_Effects_Channel() 
{
    Cy_GPIO_Write(Pin_NeoPixel_Select_3_PORT, Pin_NeoPixel_Select_3_NUM, 0);
}

static inline __attribute__((always_inline)) void NeoPixel_Enable_All_Channels() 
{
    NeoPixel_Enable_Barrel_Channel();
    NeoPixel_Enable_Receiver_Channel();
    NeoPixel_Enable_Display_Channel();
    NeoPixel_Enable_Effects_Channel();
}

static inline __attribute__((always_inline)) void NeoPixel_Disable_All_Channels() 
{
    NeoPixel_Disable_Barrel_Channel();
    NeoPixel_Disable_Receiver_Channel();
    NeoPixel_Disable_Display_Channel();
    NeoPixel_Disable_Effects_Channel();
}

static inline __attribute__((always_inline)) void NeoPixel_Enable_Channel(uint_fast8_t channel)
{
    switch (channel)
    {
        case NEOPIXEL_CHANNEL_BARREL:
            NeoPixel_Enable_Barrel_Channel();
            break;

        case NEOPIXEL_CHANNEL_RECEIVER:
            NeoPixel_Enable_Receiver_Channel();
            break;

         case NEOPIXEL_CHANNEL_DISPLAY:
            NeoPixel_Enable_Display_Channel();
            break;

         case NEOPIXEL_CHANNEL_EFFECTS:
            NeoPixel_Enable_Effects_Channel();
            break;
            
         default:
            // Do nothing.
            break;
    }
}

static inline __attribute__((always_inline)) void NeoPixel_Disable_Channel(uint_fast8_t channel)
{
    switch (channel)
    {
        case NEOPIXEL_CHANNEL_BARREL:
            NeoPixel_Disable_Barrel_Channel();
            break;

        case NEOPIXEL_CHANNEL_RECEIVER:
            NeoPixel_Disable_Receiver_Channel();
            break;

         case NEOPIXEL_CHANNEL_DISPLAY:
            NeoPixel_Disable_Display_Channel();
            break;

         case NEOPIXEL_CHANNEL_EFFECTS:
            NeoPixel_Disable_Effects_Channel();
            break;
            
         default:
            // Do nothing.
            break;
    }
}
#else
    #error "Unsupported number of NeoPixel channels defined.  Supported configurations are 1 and 4."
#endif

SystemKResult_T HW_NeoPixels_Publish(void)
{
    // Update the NeoPixels using DMA.
    for (uint_fast8_t Current_NeoPixel_Channel = 0; Current_NeoPixel_Channel < CONFIG_KTAG_N_NEOPIXEL_CHANNELS; Current_NeoPixel_Channel++)
    {
        xSemaphoreTake(NeoPixels_Semaphore, portMAX_DELAY);
        NeoPixel_Enable_Channel(Current_NeoPixel_Channel);
        NeoPixel_Trigger_DMA(Current_NeoPixel_Channel);
        // Allow time for the DMA transfer to go out on the wire.
        vTaskDelay(portTICK_PERIOD_MS);
        NeoPixel_Disable_Channel(Current_NeoPixel_Channel);
        xSemaphoreGive(NeoPixels_Semaphore);
    }
    
    return SYSTEMK_RESULT_SUCCESS;
}

//! \todo Refactor this somehow...it doesn't belong here.
color_t HW_NeoPixels_Get_My_Color(void)
{
   return PROTOCOLS_GetColor(GetWeaponFromID(NVM_WEAPON_ID).Protocol, NVM_TEAM_ID, NVM_PLAYER_ID); 
}