Subversion Repositories Code-Repo

#include <xc.h>
#include <plib.h>
#include "defines.h"
#include "CUBE.h"
#include "SPI1.h"
#include "glcdfont.h"

static CUBE_DATA *cube_data_ptr;

inline void Cube_Delay() {
    // Small delay to ensure that latch speeds are < 30Mhz
    Nop();
    Nop();
    Nop();
}

void Cube_Init(CUBE_DATA *data, char BC) {
    cube_data_ptr = data;
    cube_data_ptr->current_layer = 0;
    cube_data_ptr->rotation_counter = 0;
    cube_data_ptr->frame_state = IDLE;
    cube_data_ptr->frame_escape = 0;

    SFT_D = 0;
    SFT_S = 0;
    SFT_K = 0;
    SFT_R = 0;
    GSLAT = 0;
    XBLNK = 0;
    
    SFT_D_TRIS = 0;
    SFT_S_TRIS = 0;
    SFT_K_TRIS = 0;
    SFT_R_TRIS = 0;
    GSLAT_TRIS = 0;
    XBLNK_TRIS = 0;

    // Clear the shift register
    Cube_Delay();
    SFT_K = 1;
    Cube_Delay();
    SFT_K = 0;
    Cube_Delay();
    SFT_S = 1;
    Cube_Delay();
    SFT_S = 0;
    Cube_Delay();
    SFT_R = 1;

    Cube_Write_DCS(BC);
    Cube_Clear();
    Cube_Overlay_Clear();
}

void Cube_Timer_Interrupt(void) {
    // OR values in the overlay array with the display array
    int i,j;
    for (i = 0; i < CUBE_LAYER_COUNT; i++) {
        for (j = 0; j < GCS_LAYER_SIZE; j++) {
            cube_data_ptr->GCS_WRITE[i][j] = cube_data_ptr->GCS[i][j] | cube_data_ptr->GCS_OVERLAY[i][j];
        }
    }
    // Write to the GCS register
    SPI1_Write(cube_data_ptr->GCS_WRITE[cube_data_ptr->current_layer], GCS_LAYER_SIZE, &Cube_GCS_Write_Callback);
}

////////////////////////
// Callback functions //
////////////////////////

void Cube_DCS_Write_Callback(void) {
    // GSLAT must be >7ms after DCS write
    Delay_MS(7);
    GSLAT = 0;
    Cube_Delay();
    GSLAT = 1;
    Cube_Delay();
    GSLAT = 0;
}

void Cube_GCS_Write_Callback(void) {
    // Disable LED output and latch in written data to GCS
    XBLNK = 0;
    Cube_Delay();
    GSLAT = 1;
    // Set the shift register to turn on the current layer
    int i;
    for (i = 0; i < CUBE_LAYER_COUNT; i++) {
        Cube_Delay();
        SFT_D = (i == CUBE_LAYER_COUNT - cube_data_ptr->current_layer - 1) ? 1 : 0;
        Cube_Delay();
        SFT_K = 1;
        Cube_Delay();
        SFT_K = 0;
    }
    Cube_Delay();
    SFT_S = 1;
    Cube_Delay();
    SFT_S = 0;
    Cube_Delay();
    // Enable LED output
    XBLNK = 1;
    Cube_Delay();
    GSLAT = 0;

    cube_data_ptr->current_layer = (cube_data_ptr->current_layer == CUBE_LAYER_COUNT-1)
            ? 0 : cube_data_ptr->current_layer + 1;
}

////////////////////////////
// Cube control functions //
////////////////////////////

void Cube_Write_DCS(char BC) {
    XBLNK = 0;
    int i,j;
    // Write configuration data to the DC/BC/FC/UD registers
    unsigned char DCS[GCS_LAYER_SIZE] = {0};
    for (i = 0; i < 8; i++) {
        int offset = i * GCS_REG_SIZE;

        for (j = 0; j < 21; j++) {
            DCS[offset + j] = 0xFF; // Dot correction
        }

        // Warning: do not set BC > 0x6F
        DCS[offset + 21] = BC; // Global red brightness
        DCS[offset + 22] = BC; // Global green brightness
        DCS[offset + 23] = BC; // Global blue brightness

        // DC low range, auto repeat, no timing reset, 8 bit counter mode
        DCS[offset + 24] = 0x68; // 0110 1000
    }
    
    GSLAT = 1;
    SPI1_Write(DCS, GCS_LAYER_SIZE, &Cube_DCS_Write_Callback);
    Delay_MS(10); // Delay until the entire DCS write is finished
}

void Cube_Clear(void) {
    int i,j;
    for (i = 0; i < CUBE_LAYER_COUNT; i++)
        for (j = 0; j < GCS_LAYER_SIZE; j++)
            cube_data_ptr->GCS[i][j] = 0x00;
}

void Cube_Set_All(int R, int G, int B) {
    // Set all pixels in the cube to the given color
    R &= 0x0FFF;
    G &= 0x0FFF;
    B &= 0x0FFF;
    int i,j,k;
    for (i = 0; i < CUBE_LAYER_COUNT; i++) {
        for (j = 0; j < CUBE_ROW_COUNT; j++) {
            int j_var = j * GCS_REG_SIZE;
            for (k = 0; k < 4; k++) {
                int k_var = j_var + (k * 9);
                cube_data_ptr->GCS[i][k_var+0] = R & 0xFF;;
                cube_data_ptr->GCS[i][k_var+1] = (G << 4) | (R >> 8);
                cube_data_ptr->GCS[i][k_var+2] = G >> 4;
                cube_data_ptr->GCS[i][k_var+3] = B & 0xFF;
                cube_data_ptr->GCS[i][k_var+4] = (R << 4) | (B >> 8);
                cube_data_ptr->GCS[i][k_var+5] = R >> 4;
                cube_data_ptr->GCS[i][k_var+6] = G & 0xFF;
                cube_data_ptr->GCS[i][k_var+7] = (B << 4) | (G >> 8);
                cube_data_ptr->GCS[i][k_var+8] = B >> 4;
            }
        }
    }
}

void Cube_Set_Layer(int layer, int R, int G, int B) {
    // Set all pixels in the specified layer to the given color
    R &= 0x0FFF;
    G &= 0x0FFF;
    B &= 0x0FFF;
    int i,j;
    for (i = 0; i < CUBE_ROW_COUNT; i++) {
        int i_var = i * GCS_REG_SIZE;
        for (j = 0; j < 4; j++) {
            int j_var = i_var + (j * 9);
            cube_data_ptr->GCS[layer][j_var+0] = R & 0xFF;;
            cube_data_ptr->GCS[layer][j_var+1] = (G << 4) | (R >> 8);
            cube_data_ptr->GCS[layer][j_var+2] = G >> 4;
            cube_data_ptr->GCS[layer][j_var+3] = B & 0xFF;
            cube_data_ptr->GCS[layer][j_var+4] = (R << 4) | (B >> 8);
            cube_data_ptr->GCS[layer][j_var+5] = R >> 4;
            cube_data_ptr->GCS[layer][j_var+6] = G & 0xFF;
            cube_data_ptr->GCS[layer][j_var+7] = (B << 4) | (G >> 8);
            cube_data_ptr->GCS[layer][j_var+8] = B >> 4;
        }
    }
}

void Cube_Set_Pixel(int layer, int row, int column, int R, int G, int B) {
    // Set the specified pixel to the given color
    R &= 0x0FFF;
    G &= 0x0FFF;
    B &= 0x0FFF;
    int var = row * GCS_REG_SIZE + (column / 2 * 9);
    switch (column % 2) {
        case 0:
            cube_data_ptr->GCS[layer][var+0] = R & 0xFF;
            cube_data_ptr->GCS[layer][var+1] = (G << 4) | (R >> 8);
            cube_data_ptr->GCS[layer][var+2] = G >> 4;
            cube_data_ptr->GCS[layer][var+3] = B & 0xFF;
            cube_data_ptr->GCS[layer][var+4] = (cube_data_ptr->GCS[layer][var+4] & 0xF0) | (B >> 8);
            break;
        case 1:
            cube_data_ptr->GCS[layer][var+4] = (cube_data_ptr->GCS[layer][var+4] & 0x0F) | (R << 4);
            cube_data_ptr->GCS[layer][var+5] = R >> 4;
            cube_data_ptr->GCS[layer][var+6] = G & 0xFF;
            cube_data_ptr->GCS[layer][var+7] = (B << 4) | (G >> 8);
            cube_data_ptr->GCS[layer][var+8] = B >> 4;
            break;
    }
}

void Cube_Get_Pixel(int layer, int row, int column, int* R, int* G, int* B) {
    int var = row * GCS_REG_SIZE + (column / 2 * 9);
    switch (column % 2) {
        // Concatenate lower byte and upper byte of each color channel
        case 0:
            *R = cube_data_ptr->GCS[layer][var+0] | ((cube_data_ptr->GCS[layer][var+1] & 0x0F) << 8);
            *G = (cube_data_ptr->GCS[layer][var+1] >> 4) | (cube_data_ptr->GCS[layer][var+2] << 4);
            *B = cube_data_ptr->GCS[layer][var+3] | ((cube_data_ptr->GCS[layer][var+4] & 0x0F) << 8);
            break;
        case 1:
            *R = (cube_data_ptr->GCS[layer][var+4] >> 4) | (cube_data_ptr->GCS[layer][var+5] << 4);
            *G = cube_data_ptr->GCS[layer][var+6] | ((cube_data_ptr->GCS[layer][var+7] & 0x0F) << 8);
            *B = (cube_data_ptr->GCS[layer][var+7] >> 4) | (cube_data_ptr->GCS[layer][var+8] << 4);
            break;
    }
}

void Cube_Move_Pixel(int layer1, int row1, int column1, int layer2, int row2, int column2) {
    // Copies data from pixel 1 to pixel 2
    // Note: destination pixel value is overwritten
    int prev_R, prev_G, prev_B;
    Cube_Get_Pixel(layer1, row1, column1, &prev_R, &prev_G, &prev_B);
    Cube_Set_Pixel(layer2, row2, column2, prev_R, prev_G, prev_B);
}

void Cube_Rotate_Shell(char shell, char direction) {
    // Shell is the layer to rotate, with the outermost being 0
    int layer, origin_R, origin_G, origin_B;;
    for (layer = 0; layer < CUBE_LAYER_COUNT; layer++) {
        if (direction) {
            switch(shell) {
                case 0:
                    // Rotate outermost layer
                    Cube_Get_Pixel(layer, 0, 0, &origin_R, &origin_G, &origin_B);
                    Cube_Move_Pixel(layer, 0, 1, layer, 0, 0);
                    Cube_Move_Pixel(layer, 0, 2, layer, 0, 1);
                    Cube_Move_Pixel(layer, 0, 3, layer, 0, 2);
                    Cube_Move_Pixel(layer, 0, 4, layer, 0, 3);
                    Cube_Move_Pixel(layer, 0, 5, layer, 0, 4);
                    Cube_Move_Pixel(layer, 0, 6, layer, 0, 5);
                    Cube_Move_Pixel(layer, 0, 7, layer, 0, 6);
                    Cube_Move_Pixel(layer, 1, 7, layer, 0, 7);
                    Cube_Move_Pixel(layer, 2, 7, layer, 1, 7);
                    Cube_Move_Pixel(layer, 3, 7, layer, 2, 7);
                    Cube_Move_Pixel(layer, 4, 7, layer, 3, 7);
                    Cube_Move_Pixel(layer, 5, 7, layer, 4, 7);
                    Cube_Move_Pixel(layer, 6, 7, layer, 5, 7);
                    Cube_Move_Pixel(layer, 7, 7, layer, 6, 7);
                    Cube_Move_Pixel(layer, 7, 6, layer, 7, 7);
                    Cube_Move_Pixel(layer, 7, 5, layer, 7, 6);
                    Cube_Move_Pixel(layer, 7, 4, layer, 7, 5);
                    Cube_Move_Pixel(layer, 7, 3, layer, 7, 4);
                    Cube_Move_Pixel(layer, 7, 2, layer, 7, 3);
                    Cube_Move_Pixel(layer, 7, 1, layer, 7, 2);
                    Cube_Move_Pixel(layer, 7, 0, layer, 7, 1);
                    Cube_Move_Pixel(layer, 6, 0, layer, 7, 0);
                    Cube_Move_Pixel(layer, 5, 0, layer, 6, 0);
                    Cube_Move_Pixel(layer, 4, 0, layer, 5, 0);
                    Cube_Move_Pixel(layer, 3, 0, layer, 4, 0);
                    Cube_Move_Pixel(layer, 2, 0, layer, 3, 0);
                    Cube_Move_Pixel(layer, 1, 0, layer, 2, 0);
                    Cube_Set_Pixel(layer, 1, 0, origin_R, origin_G, origin_B);
                    break;
                case 1:
                    // Rotate second to outermost layer
                    Cube_Get_Pixel(layer, 1, 1, &origin_R, &origin_G, &origin_B);
                    Cube_Move_Pixel(layer, 1, 2, layer, 1, 1);
                    Cube_Move_Pixel(layer, 1, 3, layer, 1, 2);
                    Cube_Move_Pixel(layer, 1, 4, layer, 1, 3);
                    Cube_Move_Pixel(layer, 1, 5, layer, 1, 4);
                    Cube_Move_Pixel(layer, 1, 6, layer, 1, 5);
                    Cube_Move_Pixel(layer, 2, 6, layer, 1, 6);
                    Cube_Move_Pixel(layer, 3, 6, layer, 2, 6);
                    Cube_Move_Pixel(layer, 4, 6, layer, 3, 6);
                    Cube_Move_Pixel(layer, 5, 6, layer, 4, 6);
                    Cube_Move_Pixel(layer, 6, 6, layer, 5, 6);
                    Cube_Move_Pixel(layer, 6, 5, layer, 6, 6);
                    Cube_Move_Pixel(layer, 6, 4, layer, 6, 5);
                    Cube_Move_Pixel(layer, 6, 3, layer, 6, 4);
                    Cube_Move_Pixel(layer, 6, 2, layer, 6, 3);
                    Cube_Move_Pixel(layer, 6, 1, layer, 6, 2);
                    Cube_Move_Pixel(layer, 5, 1, layer, 6, 1);
                    Cube_Move_Pixel(layer, 4, 1, layer, 5, 1);
                    Cube_Move_Pixel(layer, 3, 1, layer, 4, 1);
                    Cube_Move_Pixel(layer, 2, 1, layer, 3, 1);
                    Cube_Set_Pixel(layer, 2, 1, origin_R, origin_G, origin_B);
                    break;
                case 2:
                    // Rotate second to innermost layer
                    Cube_Get_Pixel(layer, 2, 2, &origin_R, &origin_G, &origin_B);
                    Cube_Move_Pixel(layer, 2, 3, layer, 2, 2);
                    Cube_Move_Pixel(layer, 2, 4, layer, 2, 3);
                    Cube_Move_Pixel(layer, 2, 5, layer, 2, 4);
                    Cube_Move_Pixel(layer, 3, 5, layer, 2, 5);
                    Cube_Move_Pixel(layer, 4, 5, layer, 3, 5);
                    Cube_Move_Pixel(layer, 5, 5, layer, 4, 5);
                    Cube_Move_Pixel(layer, 5, 4, layer, 5, 5);
                    Cube_Move_Pixel(layer, 5, 3, layer, 5, 4);
                    Cube_Move_Pixel(layer, 5, 2, layer, 5, 3);
                    Cube_Move_Pixel(layer, 4, 2, layer, 5, 2);
                    Cube_Move_Pixel(layer, 3, 2, layer, 4, 2);
                    Cube_Set_Pixel(layer, 3, 2, origin_R, origin_G, origin_B);
                    break;
                case 3:
                    // Rotate innermost layer
                    Cube_Get_Pixel(layer, 3, 3, &origin_R, &origin_G, &origin_B);
                    Cube_Move_Pixel(layer, 3, 4, layer, 3, 3);
                    Cube_Move_Pixel(layer, 4, 4, layer, 3, 4);
                    Cube_Move_Pixel(layer, 4, 3, layer, 4, 4);
                    Cube_Set_Pixel(layer, 4, 3, origin_R, origin_G, origin_B);
                    break;
            }
        } else {
            switch(shell) {
                case 0:
                    // Rotate outermost layer
                    Cube_Get_Pixel(layer, 0, 0, &origin_R, &origin_G, &origin_B);
                    Cube_Move_Pixel(layer, 1, 0, layer, 0, 0);
                    Cube_Move_Pixel(layer, 2, 0, layer, 1, 0);
                    Cube_Move_Pixel(layer, 3, 0, layer, 2, 0);
                    Cube_Move_Pixel(layer, 4, 0, layer, 3, 0);
                    Cube_Move_Pixel(layer, 5, 0, layer, 4, 0);
                    Cube_Move_Pixel(layer, 6, 0, layer, 5, 0);
                    Cube_Move_Pixel(layer, 7, 0, layer, 6, 0);
                    Cube_Move_Pixel(layer, 7, 1, layer, 7, 0);
                    Cube_Move_Pixel(layer, 7, 2, layer, 7, 1);
                    Cube_Move_Pixel(layer, 7, 3, layer, 7, 2);
                    Cube_Move_Pixel(layer, 7, 4, layer, 7, 3);
                    Cube_Move_Pixel(layer, 7, 5, layer, 7, 4);
                    Cube_Move_Pixel(layer, 7, 6, layer, 7, 5);
                    Cube_Move_Pixel(layer, 7, 7, layer, 7, 6);
                    Cube_Move_Pixel(layer, 6, 7, layer, 7, 7);
                    Cube_Move_Pixel(layer, 5, 7, layer, 6, 7);
                    Cube_Move_Pixel(layer, 4, 7, layer, 5, 7);
                    Cube_Move_Pixel(layer, 3, 7, layer, 4, 7);
                    Cube_Move_Pixel(layer, 2, 7, layer, 3, 7);
                    Cube_Move_Pixel(layer, 1, 7, layer, 2, 7);
                    Cube_Move_Pixel(layer, 0, 7, layer, 1, 7);
                    Cube_Move_Pixel(layer, 0, 6, layer, 0, 7);
                    Cube_Move_Pixel(layer, 0, 5, layer, 0, 6);
                    Cube_Move_Pixel(layer, 0, 4, layer, 0, 5);
                    Cube_Move_Pixel(layer, 0, 3, layer, 0, 4);
                    Cube_Move_Pixel(layer, 0, 2, layer, 0, 3);
                    Cube_Move_Pixel(layer, 0, 1, layer, 0, 2);
                    Cube_Set_Pixel(layer, 0, 1, origin_R, origin_G, origin_B);
                    break;
                case 1:
                    // Rotate second to outermost layer
                    Cube_Get_Pixel(layer, 1, 1, &origin_R, &origin_G, &origin_B);
                    Cube_Move_Pixel(layer, 2, 1, layer, 1, 1);
                    Cube_Move_Pixel(layer, 3, 1, layer, 2, 1);
                    Cube_Move_Pixel(layer, 4, 1, layer, 3, 1);
                    Cube_Move_Pixel(layer, 5, 1, layer, 4, 1);
                    Cube_Move_Pixel(layer, 6, 1, layer, 5, 1);
                    Cube_Move_Pixel(layer, 6, 2, layer, 6, 1);
                    Cube_Move_Pixel(layer, 6, 3, layer, 6, 2);
                    Cube_Move_Pixel(layer, 6, 4, layer, 6, 3);
                    Cube_Move_Pixel(layer, 6, 5, layer, 6, 4);
                    Cube_Move_Pixel(layer, 6, 6, layer, 6, 5);
                    Cube_Move_Pixel(layer, 5, 6, layer, 6, 6);
                    Cube_Move_Pixel(layer, 4, 6, layer, 5, 6);
                    Cube_Move_Pixel(layer, 3, 6, layer, 4, 6);
                    Cube_Move_Pixel(layer, 2, 6, layer, 3, 6);
                    Cube_Move_Pixel(layer, 1, 6, layer, 2, 6);
                    Cube_Move_Pixel(layer, 1, 5, layer, 1, 6);
                    Cube_Move_Pixel(layer, 1, 4, layer, 1, 5);
                    Cube_Move_Pixel(layer, 1, 3, layer, 1, 4);
                    Cube_Move_Pixel(layer, 1, 2, layer, 1, 3);
                    Cube_Set_Pixel(layer, 1, 2, origin_R, origin_G, origin_B);
                    break;
                case 2:
                    // Rotate second to innermost layer
                    Cube_Get_Pixel(layer, 2, 2, &origin_R, &origin_G, &origin_B);
                    Cube_Move_Pixel(layer, 3, 2, layer, 2, 2);
                    Cube_Move_Pixel(layer, 4, 2, layer, 3, 2);
                    Cube_Move_Pixel(layer, 5, 2, layer, 4, 2);
                    Cube_Move_Pixel(layer, 5, 3, layer, 5, 2);
                    Cube_Move_Pixel(layer, 5, 4, layer, 5, 3);
                    Cube_Move_Pixel(layer, 5, 5, layer, 5, 4);
                    Cube_Move_Pixel(layer, 4, 5, layer, 5, 5);
                    Cube_Move_Pixel(layer, 3, 5, layer, 4, 5);
                    Cube_Move_Pixel(layer, 2, 5, layer, 3, 5);
                    Cube_Move_Pixel(layer, 2, 4, layer, 2, 5);
                    Cube_Move_Pixel(layer, 2, 3, layer, 2, 4);
                    Cube_Set_Pixel(layer, 2, 3, origin_R, origin_G, origin_B);
                    break;
                case 3:
                    // Rotate innermost layer
                    Cube_Get_Pixel(layer, 3, 3, &origin_R, &origin_G, &origin_B);
                    Cube_Move_Pixel(layer, 4, 3, layer, 3, 3);
                    Cube_Move_Pixel(layer, 4, 4, layer, 4, 3);
                    Cube_Move_Pixel(layer, 3, 4, layer, 4, 4);
                    Cube_Set_Pixel(layer, 3, 4, origin_R, origin_G, origin_B);
                    break;
            }
        }
    }
}

void Cube_Rotate(char direction) {
    // Rotate outermost layer
    Cube_Rotate_Shell(0, direction);
    // Rotate second to outermost layer
    if ((cube_data_ptr->rotation_counter != 1) && (cube_data_ptr->rotation_counter != 5)) {
        Cube_Rotate_Shell(1, direction);
    }
    // Rotate second to innermost layer
    if ((cube_data_ptr->rotation_counter != 0) && (cube_data_ptr->rotation_counter != 2) &&
        (cube_data_ptr->rotation_counter != 4) && (cube_data_ptr->rotation_counter != 6)) {
        Cube_Rotate_Shell(2, direction);
    }
    // Rotate innermost layer
    if ((cube_data_ptr->rotation_counter == 3) || (cube_data_ptr->rotation_counter == 7)) {
        Cube_Rotate_Shell(3, direction);
    }

    if (direction == 0) {
        cube_data_ptr->rotation_counter = (cube_data_ptr->rotation_counter == CUBE_ROTATIONS - 1)
                ? 0 : cube_data_ptr->rotation_counter + 1;
    } else {
        cube_data_ptr->rotation_counter = (cube_data_ptr->rotation_counter == 0)
                ? CUBE_ROTATIONS - 1 : cube_data_ptr->rotation_counter - 1;
    }
}

///////////////////////////////
// Overlay control functions //
///////////////////////////////

void Cube_Overlay_Clear(void) {
    int i,j;
    for (i = 0; i < CUBE_LAYER_COUNT; i++)
        for (j = 0; j < GCS_LAYER_SIZE; j++)
            cube_data_ptr->GCS_OVERLAY[i][j] = 0x00;
}

void Cube_Overlay_Set_Pixel(int layer, int row, int column, int R, int G, int B) {
    // Set the specified pixel to the given color
    R &= 0x0FFF;
    G &= 0x0FFF;
    B &= 0x0FFF;
    int var = row * GCS_REG_SIZE + (column / 2 * 9);
    switch (column % 2) {
        case 0:
            cube_data_ptr->GCS_OVERLAY[layer][var+0] = R & 0xFF;
            cube_data_ptr->GCS_OVERLAY[layer][var+1] = (G << 4) | (R >> 8);
            cube_data_ptr->GCS_OVERLAY[layer][var+2] = G >> 4;
            cube_data_ptr->GCS_OVERLAY[layer][var+3] = B & 0xFF;
            cube_data_ptr->GCS_OVERLAY[layer][var+4] = (cube_data_ptr->GCS_OVERLAY[layer][var+4] & 0xF0) | (B >> 8);
            break;
        case 1:
            cube_data_ptr->GCS_OVERLAY[layer][var+4] = (cube_data_ptr->GCS_OVERLAY[layer][var+4] & 0x0F) | (R << 4);
            cube_data_ptr->GCS_OVERLAY[layer][var+5] = R >> 4;
            cube_data_ptr->GCS_OVERLAY[layer][var+6] = G & 0xFF;
            cube_data_ptr->GCS_OVERLAY[layer][var+7] = (B << 4) | (G >> 8);
            cube_data_ptr->GCS_OVERLAY[layer][var+8] = B >> 4;
            break;
    }
}

void Cube_Overlay_Get_Pixel(int layer, int row, int column, int* R, int* G, int* B) {
    int var = row * GCS_REG_SIZE + (column / 2 * 9);
    switch (column % 2) {
        // Concatenate lower byte and upper byte of each color channel
        case 0:
            *R = cube_data_ptr->GCS_OVERLAY[layer][var+0] | ((cube_data_ptr->GCS_OVERLAY[layer][var+1] & 0x0F) << 8);
            *G = (cube_data_ptr->GCS_OVERLAY[layer][var+1] >> 4) | (cube_data_ptr->GCS_OVERLAY[layer][var+2] << 4);
            *B = cube_data_ptr->GCS_OVERLAY[layer][var+3] | ((cube_data_ptr->GCS_OVERLAY[layer][var+4] & 0x0F) << 8);
            break;
        case 1:
            *R = (cube_data_ptr->GCS_OVERLAY[layer][var+4] >> 4) | (cube_data_ptr->GCS_OVERLAY[layer][var+5] << 4);
            *G = cube_data_ptr->GCS_OVERLAY[layer][var+6] | ((cube_data_ptr->GCS_OVERLAY[layer][var+7] & 0x0F) << 8);
            *B = (cube_data_ptr->GCS_OVERLAY[layer][var+7] >> 4) | (cube_data_ptr->GCS_OVERLAY[layer][var+8] << 4);
            break;
    }
}

void Cube_Overlay_Move_Pixel(int layer1, int row1, int column1, int layer2, int row2, int column2) {
    // Copies data from pixel 1 to pixel 2
    // Note: destination pixel value is overwritten
    int prev_R, prev_G, prev_B;
    Cube_Overlay_Get_Pixel(layer1, row1, column1, &prev_R, &prev_G, &prev_B);
    Cube_Overlay_Set_Pixel(layer2, row2, column2, prev_R, prev_G, prev_B);
}

void Cube_Overlay_Rotate_Shell(char shell, char direction) {
    // Shell is the layer to rotate, with the outermost being 0
    int layer, origin_R, origin_G, origin_B;;
    for (layer = 0; layer < CUBE_LAYER_COUNT; layer++) {
        if (direction) {
            switch(shell) {
                case 0:
                    // Rotate outermost layer
                    Cube_Overlay_Get_Pixel(layer, 0, 0, &origin_R, &origin_G, &origin_B);
                    Cube_Overlay_Move_Pixel(layer, 0, 1, layer, 0, 0);
                    Cube_Overlay_Move_Pixel(layer, 0, 2, layer, 0, 1);
                    Cube_Overlay_Move_Pixel(layer, 0, 3, layer, 0, 2);
                    Cube_Overlay_Move_Pixel(layer, 0, 4, layer, 0, 3);
                    Cube_Overlay_Move_Pixel(layer, 0, 5, layer, 0, 4);
                    Cube_Overlay_Move_Pixel(layer, 0, 6, layer, 0, 5);
                    Cube_Overlay_Move_Pixel(layer, 0, 7, layer, 0, 6);
                    Cube_Overlay_Move_Pixel(layer, 1, 7, layer, 0, 7);
                    Cube_Overlay_Move_Pixel(layer, 2, 7, layer, 1, 7);
                    Cube_Overlay_Move_Pixel(layer, 3, 7, layer, 2, 7);
                    Cube_Overlay_Move_Pixel(layer, 4, 7, layer, 3, 7);
                    Cube_Overlay_Move_Pixel(layer, 5, 7, layer, 4, 7);
                    Cube_Overlay_Move_Pixel(layer, 6, 7, layer, 5, 7);
                    Cube_Overlay_Move_Pixel(layer, 7, 7, layer, 6, 7);
                    Cube_Overlay_Move_Pixel(layer, 7, 6, layer, 7, 7);
                    Cube_Overlay_Move_Pixel(layer, 7, 5, layer, 7, 6);
                    Cube_Overlay_Move_Pixel(layer, 7, 4, layer, 7, 5);
                    Cube_Overlay_Move_Pixel(layer, 7, 3, layer, 7, 4);
                    Cube_Overlay_Move_Pixel(layer, 7, 2, layer, 7, 3);
                    Cube_Overlay_Move_Pixel(layer, 7, 1, layer, 7, 2);
                    Cube_Overlay_Move_Pixel(layer, 7, 0, layer, 7, 1);
                    Cube_Overlay_Move_Pixel(layer, 6, 0, layer, 7, 0);
                    Cube_Overlay_Move_Pixel(layer, 5, 0, layer, 6, 0);
                    Cube_Overlay_Move_Pixel(layer, 4, 0, layer, 5, 0);
                    Cube_Overlay_Move_Pixel(layer, 3, 0, layer, 4, 0);
                    Cube_Overlay_Move_Pixel(layer, 2, 0, layer, 3, 0);
                    Cube_Overlay_Move_Pixel(layer, 1, 0, layer, 2, 0);
                    Cube_Overlay_Set_Pixel(layer, 1, 0, origin_R, origin_G, origin_B);
                    break;
                case 1:
                    // Rotate second to outermost layer
                    Cube_Overlay_Get_Pixel(layer, 1, 1, &origin_R, &origin_G, &origin_B);
                    Cube_Overlay_Move_Pixel(layer, 1, 2, layer, 1, 1);
                    Cube_Overlay_Move_Pixel(layer, 1, 3, layer, 1, 2);
                    Cube_Overlay_Move_Pixel(layer, 1, 4, layer, 1, 3);
                    Cube_Overlay_Move_Pixel(layer, 1, 5, layer, 1, 4);
                    Cube_Overlay_Move_Pixel(layer, 1, 6, layer, 1, 5);
                    Cube_Overlay_Move_Pixel(layer, 2, 6, layer, 1, 6);
                    Cube_Overlay_Move_Pixel(layer, 3, 6, layer, 2, 6);
                    Cube_Overlay_Move_Pixel(layer, 4, 6, layer, 3, 6);
                    Cube_Overlay_Move_Pixel(layer, 5, 6, layer, 4, 6);
                    Cube_Overlay_Move_Pixel(layer, 6, 6, layer, 5, 6);
                    Cube_Overlay_Move_Pixel(layer, 6, 5, layer, 6, 6);
                    Cube_Overlay_Move_Pixel(layer, 6, 4, layer, 6, 5);
                    Cube_Overlay_Move_Pixel(layer, 6, 3, layer, 6, 4);
                    Cube_Overlay_Move_Pixel(layer, 6, 2, layer, 6, 3);
                    Cube_Overlay_Move_Pixel(layer, 6, 1, layer, 6, 2);
                    Cube_Overlay_Move_Pixel(layer, 5, 1, layer, 6, 1);
                    Cube_Overlay_Move_Pixel(layer, 4, 1, layer, 5, 1);
                    Cube_Overlay_Move_Pixel(layer, 3, 1, layer, 4, 1);
                    Cube_Overlay_Move_Pixel(layer, 2, 1, layer, 3, 1);
                    Cube_Overlay_Set_Pixel(layer, 2, 1, origin_R, origin_G, origin_B);
                    break;
                case 2:
                    // Rotate second to innermost layer
                    Cube_Overlay_Get_Pixel(layer, 2, 2, &origin_R, &origin_G, &origin_B);
                    Cube_Overlay_Move_Pixel(layer, 2, 3, layer, 2, 2);
                    Cube_Overlay_Move_Pixel(layer, 2, 4, layer, 2, 3);
                    Cube_Overlay_Move_Pixel(layer, 2, 5, layer, 2, 4);
                    Cube_Overlay_Move_Pixel(layer, 3, 5, layer, 2, 5);
                    Cube_Overlay_Move_Pixel(layer, 4, 5, layer, 3, 5);
                    Cube_Overlay_Move_Pixel(layer, 5, 5, layer, 4, 5);
                    Cube_Overlay_Move_Pixel(layer, 5, 4, layer, 5, 5);
                    Cube_Overlay_Move_Pixel(layer, 5, 3, layer, 5, 4);
                    Cube_Overlay_Move_Pixel(layer, 5, 2, layer, 5, 3);
                    Cube_Overlay_Move_Pixel(layer, 4, 2, layer, 5, 2);
                    Cube_Overlay_Move_Pixel(layer, 3, 2, layer, 4, 2);
                    Cube_Overlay_Set_Pixel(layer, 3, 2, origin_R, origin_G, origin_B);
                    break;
                case 3:
                    // Rotate innermost layer
                    Cube_Overlay_Get_Pixel(layer, 3, 3, &origin_R, &origin_G, &origin_B);
                    Cube_Overlay_Move_Pixel(layer, 3, 4, layer, 3, 3);
                    Cube_Overlay_Move_Pixel(layer, 4, 4, layer, 3, 4);
                    Cube_Overlay_Move_Pixel(layer, 4, 3, layer, 4, 4);
                    Cube_Overlay_Set_Pixel(layer, 4, 3, origin_R, origin_G, origin_B);
                    break;
            }
        } else {
            switch(shell) {
                case 0:
                    // Rotate outermost layer
                    Cube_Overlay_Get_Pixel(layer, 0, 0, &origin_R, &origin_G, &origin_B);
                    Cube_Overlay_Move_Pixel(layer, 1, 0, layer, 0, 0);
                    Cube_Overlay_Move_Pixel(layer, 2, 0, layer, 1, 0);
                    Cube_Overlay_Move_Pixel(layer, 3, 0, layer, 2, 0);
                    Cube_Overlay_Move_Pixel(layer, 4, 0, layer, 3, 0);
                    Cube_Overlay_Move_Pixel(layer, 5, 0, layer, 4, 0);
                    Cube_Overlay_Move_Pixel(layer, 6, 0, layer, 5, 0);
                    Cube_Overlay_Move_Pixel(layer, 7, 0, layer, 6, 0);
                    Cube_Overlay_Move_Pixel(layer, 7, 1, layer, 7, 0);
                    Cube_Overlay_Move_Pixel(layer, 7, 2, layer, 7, 1);
                    Cube_Overlay_Move_Pixel(layer, 7, 3, layer, 7, 2);
                    Cube_Overlay_Move_Pixel(layer, 7, 4, layer, 7, 3);
                    Cube_Overlay_Move_Pixel(layer, 7, 5, layer, 7, 4);
                    Cube_Overlay_Move_Pixel(layer, 7, 6, layer, 7, 5);
                    Cube_Overlay_Move_Pixel(layer, 7, 7, layer, 7, 6);
                    Cube_Overlay_Move_Pixel(layer, 6, 7, layer, 7, 7);
                    Cube_Overlay_Move_Pixel(layer, 5, 7, layer, 6, 7);
                    Cube_Overlay_Move_Pixel(layer, 4, 7, layer, 5, 7);
                    Cube_Overlay_Move_Pixel(layer, 3, 7, layer, 4, 7);
                    Cube_Overlay_Move_Pixel(layer, 2, 7, layer, 3, 7);
                    Cube_Overlay_Move_Pixel(layer, 1, 7, layer, 2, 7);
                    Cube_Overlay_Move_Pixel(layer, 0, 7, layer, 1, 7);
                    Cube_Overlay_Move_Pixel(layer, 0, 6, layer, 0, 7);
                    Cube_Overlay_Move_Pixel(layer, 0, 5, layer, 0, 6);
                    Cube_Overlay_Move_Pixel(layer, 0, 4, layer, 0, 5);
                    Cube_Overlay_Move_Pixel(layer, 0, 3, layer, 0, 4);
                    Cube_Overlay_Move_Pixel(layer, 0, 2, layer, 0, 3);
                    Cube_Overlay_Move_Pixel(layer, 0, 1, layer, 0, 2);
                    Cube_Overlay_Set_Pixel(layer, 0, 1, origin_R, origin_G, origin_B);
                    break;
                case 1:
                    // Rotate second to outermost layer
                    Cube_Overlay_Get_Pixel(layer, 1, 1, &origin_R, &origin_G, &origin_B);
                    Cube_Overlay_Move_Pixel(layer, 2, 1, layer, 1, 1);
                    Cube_Overlay_Move_Pixel(layer, 3, 1, layer, 2, 1);
                    Cube_Overlay_Move_Pixel(layer, 4, 1, layer, 3, 1);
                    Cube_Overlay_Move_Pixel(layer, 5, 1, layer, 4, 1);
                    Cube_Overlay_Move_Pixel(layer, 6, 1, layer, 5, 1);
                    Cube_Overlay_Move_Pixel(layer, 6, 2, layer, 6, 1);
                    Cube_Overlay_Move_Pixel(layer, 6, 3, layer, 6, 2);
                    Cube_Overlay_Move_Pixel(layer, 6, 4, layer, 6, 3);
                    Cube_Overlay_Move_Pixel(layer, 6, 5, layer, 6, 4);
                    Cube_Overlay_Move_Pixel(layer, 6, 6, layer, 6, 5);
                    Cube_Overlay_Move_Pixel(layer, 5, 6, layer, 6, 6);
                    Cube_Overlay_Move_Pixel(layer, 4, 6, layer, 5, 6);
                    Cube_Overlay_Move_Pixel(layer, 3, 6, layer, 4, 6);
                    Cube_Overlay_Move_Pixel(layer, 2, 6, layer, 3, 6);
                    Cube_Overlay_Move_Pixel(layer, 1, 6, layer, 2, 6);
                    Cube_Overlay_Move_Pixel(layer, 1, 5, layer, 1, 6);
                    Cube_Overlay_Move_Pixel(layer, 1, 4, layer, 1, 5);
                    Cube_Overlay_Move_Pixel(layer, 1, 3, layer, 1, 4);
                    Cube_Overlay_Move_Pixel(layer, 1, 2, layer, 1, 3);
                    Cube_Overlay_Set_Pixel(layer, 1, 2, origin_R, origin_G, origin_B);
                    break;
                case 2:
                    // Rotate second to innermost layer
                    Cube_Overlay_Get_Pixel(layer, 2, 2, &origin_R, &origin_G, &origin_B);
                    Cube_Overlay_Move_Pixel(layer, 3, 2, layer, 2, 2);
                    Cube_Overlay_Move_Pixel(layer, 4, 2, layer, 3, 2);
                    Cube_Overlay_Move_Pixel(layer, 5, 2, layer, 4, 2);
                    Cube_Overlay_Move_Pixel(layer, 5, 3, layer, 5, 2);
                    Cube_Overlay_Move_Pixel(layer, 5, 4, layer, 5, 3);
                    Cube_Overlay_Move_Pixel(layer, 5, 5, layer, 5, 4);
                    Cube_Overlay_Move_Pixel(layer, 4, 5, layer, 5, 5);
                    Cube_Overlay_Move_Pixel(layer, 3, 5, layer, 4, 5);
                    Cube_Overlay_Move_Pixel(layer, 2, 5, layer, 3, 5);
                    Cube_Overlay_Move_Pixel(layer, 2, 4, layer, 2, 5);
                    Cube_Overlay_Move_Pixel(layer, 2, 3, layer, 2, 4);
                    Cube_Overlay_Set_Pixel(layer, 2, 3, origin_R, origin_G, origin_B);
                    break;
                case 3:
                    // Rotate innermost layer
                    Cube_Overlay_Get_Pixel(layer, 3, 3, &origin_R, &origin_G, &origin_B);
                    Cube_Overlay_Move_Pixel(layer, 4, 3, layer, 3, 3);
                    Cube_Overlay_Move_Pixel(layer, 4, 4, layer, 4, 3);
                    Cube_Overlay_Move_Pixel(layer, 3, 4, layer, 4, 4);
                    Cube_Overlay_Set_Pixel(layer, 3, 4, origin_R, origin_G, origin_B);
                    break;
            }
        }
    }
}

////////////////////////////
// Text control functions //
////////////////////////////

void Cube_Text_Init(char *string, char length, int R, int G, int B) {
    // Ensure that the length of the string does not exceed the storage buffer
    if (length > CUBE_STRING_MAX_LENGTH) length = CUBE_STRING_MAX_LENGTH;

    // Copy the passed data into the buffer
    int i;
    for (i = 0; i < length; i++)
        cube_data_ptr->string[i] = string[i];
    cube_data_ptr->string_length = length;
    cube_data_ptr->string_index = 0;
    cube_data_ptr->string_line = 0;
    cube_data_ptr->string_R = R;
    cube_data_ptr->string_G = G;
    cube_data_ptr->string_B = B;
}

void Cube_Text_Interrupt(void) {
    int layer, line;

    // Rotate before drawing the new line at (0,0)
    Cube_Overlay_Rotate_Shell(0, 0);

    // Get the next vertical line of the character currently being drawn
    if (cube_data_ptr->string_line == 5) {
        line = 0x0; // Leave a space between characters
    } else {
        line = font[(cube_data_ptr->string[cube_data_ptr->string_index] * 5)
                + cube_data_ptr->string_line];
    }

    // Draw the line onto (0,0) using the specified color
    for (layer = 7; layer >= 0; layer--) {
        if (line & 0x1) {
            Cube_Overlay_Set_Pixel(layer, 0, 0, cube_data_ptr->string_R,
                    cube_data_ptr->string_G, cube_data_ptr->string_B);
        } else {
            Cube_Overlay_Set_Pixel(layer, 0, 0, 0x00, 0x00, 0x00);
        }
        line >>= 1;
    }

    // Increment the vertical line and the character as needed
    if (cube_data_ptr->string_line == 5) {
        cube_data_ptr->string_line = 0;
        if (cube_data_ptr->string_index == cube_data_ptr->string_length-1) {
            cube_data_ptr->string_index = 0;
        } else {
            cube_data_ptr->string_index += 1;
        }
    } else {
        cube_data_ptr->string_line += 1;
    }
}

/////////////////////////////////////////////
// Functions for processing streaming data //
/////////////////////////////////////////////

void Cube_Data_In(char c) {
    // Reset upon receiving the start char
    if (c == CUBE_START_CHAR) {
        cube_data_ptr->frame_length = 0;
        cube_data_ptr->frame_index = 0;
        cube_data_ptr->frame_checksum = 0;
        cube_data_ptr->frame_command = 0;
        cube_data_ptr->frame_state = READ_LENGTH_MSB;
    } else {
        // If the input is the escape char, XOR the next char received
        if (c == CUBE_ESCAPE_CHAR) {
            cube_data_ptr->frame_escape = 1;
            return;
        }
        // XOR the input char if needed
        if (cube_data_ptr->frame_escape) {
            c ^= CUBE_ESCAPE_XOR;
            cube_data_ptr->frame_escape = 0;
        }
        // Process data
        switch (cube_data_ptr->frame_state) {
            case READ_LENGTH_MSB: // Save MSB of length
                cube_data_ptr->frame_length |= (c << 8);
                cube_data_ptr->frame_state = READ_LENGTH_LSB;
                break;
            case READ_LENGTH_LSB: // Save LSB of length
                cube_data_ptr->frame_length |= c;
                cube_data_ptr->frame_state = READ_COMMAND;
                break;
            case READ_COMMAND: // Store the command byte
                cube_data_ptr->frame_checksum += c;
                cube_data_ptr->frame_command = c;
                cube_data_ptr->frame_state = READ_DATA;
                break;
            case READ_DATA: // Read the passed data into the buffer
                cube_data_ptr->frame_checksum += c;
                cube_data_ptr->frame_buffer[cube_data_ptr->frame_index] = c;
                cube_data_ptr->frame_index++;
                if (cube_data_ptr->frame_index == cube_data_ptr->frame_length)
                    cube_data_ptr->frame_state = READ_CHECKSUM;
                break;
            case READ_CHECKSUM: // Process frame if checksum is valid
                if (0xFF - (cube_data_ptr->frame_checksum && 0xFF) == c)
                    Cube_Data_In_Process_Frame();
                cube_data_ptr->frame_state = IDLE;
                break;
            case IDLE:
            default:
                // Do nothing
                break;
        }
    }
}

void Cube_Data_In_Process_Frame(void) {
    char *frame = cube_data_ptr->frame_buffer;
    switch (cube_data_ptr->frame_command) {
        case CUBE_COMMAND_SET_PIXEL:
            Cube_Set_Pixel(frame[0], frame[1], frame[2], frame[3], frame[4], frame[5]);
            break;
        default:
            break;
    }
}