#include "rgb.h"
#include "system.h"

/* ====== SPI Register Defines (in case stc32g.h lacks them) ====== */
#ifndef P_SW1
sfr P_SW1  = 0xA2;
sfr SPCTL  = 0xCE;
sfr SPSTAT = 0xCD;
sfr SPDAT  = 0xCF;
#endif

void SPI_Init(void)
{
    P_SW1 = (P_SW1 & ~0x0C) | 0x04; 
    SPCTL = 0xD1;                   
    SPSTAT = 0xC0;                  
}

void Encode_Byte(u8 val, u8 *out)
{
    u8 i;
    for (i = 0; i < 4; i++)
    {
        u8 pair_val = 0;
        if (val & (0x80 >> (2 * i)))
        {
            pair_val |= 0xC0;
        }
        else
        {
            pair_val |= 0x80;
        }
        if (val & (0x80 >> (2 * i + 1)))
        {
            pair_val |= 0x0C;
        }
        else
        {
            pair_val |= 0x08;
        }
        out[i] = pair_val;
    }
}

void RGB_Send(u8 g1, u8 r1, u8 b1, u8 g2, u8 r2, u8 b2)
{
    u8 idata buf[6];
    u8 i, mask;
    buf[0] = g1; buf[1] = r1; buf[2] = b1;
    buf[3] = g2; buf[4] = r2; buf[5] = b2;

    EA = 0;

    P2M1 &= ~(1 << 3);
    P2M0 |= (1 << 3);

    P2M1 &= ~(1 << 5);
    P2M0 |= (1 << 5);
    MOTOR = 0;

    for (i = 0; i < 6; i++)
    {
        for (mask = 0x80; mask; mask >>= 1)
        {
            if (buf[i] & mask)
            {
                RGB_DIN = 1;
                _nop_(); _nop_(); _nop_(); _nop_(); _nop_();
                _nop_(); _nop_(); _nop_(); _nop_(); _nop_();
                _nop_(); _nop_(); _nop_(); _nop_(); _nop_();
                RGB_DIN = 0;
                _nop_(); _nop_(); _nop_(); _nop_(); _nop_();
                _nop_();
            }
            else
            {
                RGB_DIN = 1;
                _nop_(); _nop_(); _nop_();
                RGB_DIN = 0;
                _nop_(); _nop_(); _nop_(); _nop_(); _nop_();
                _nop_(); _nop_(); _nop_(); _nop_(); _nop_();
                _nop_(); _nop_(); _nop_(); _nop_(); _nop_();
            }
        }
    }

    RGB_DIN = 0;
    for (i = 0; i < 200; i++)
    {
        _nop_(); _nop_(); _nop_();
    }

    EA = 1;
}

void RGB_Bitbang_Send24(u8 g, u8 r, u8 b)
{
    (void)g; (void)r; (void)b;
}