/*========================================================================
 *  Rubik’s Cube  RLFB Solver
 *========================================================================
 *
 *  Summary
 *  - Core 0: reads MPU6050 + GY271, handles button inputs, updates rotation matrix
 *  - Core 1: VGA rendering (3D cube + UI screens + move hint grid)
 *  - User enters 6 faces (3x3 each) with color buttons, then solver runs (R/L/F/B moves)
 *
 *  Hardware Connections
 *  - VGA (GPIO 16–21): use vga16_graphics_v2 default wiring (HSync/VSync + RGB resistor DAC)
 *  - MPU6050 (I2C0):
 *      SDA = GPIO 8
 *      SCL = GPIO 9
 *      VCC = 3.3V, GND = GND
 *  - GY271  (I2C1):
 *      SDA = GPIO 6
 *      SCL = GPIO 7
 *      VCC = 3.3V, GND = GND
 *
 *  Button Inputs (active-low w/ internal pull-ups)
 *  - NEXT / STEP: GPIO 11
 *  - Color buttons:
 *      RED    = GPIO 2
 *      YELLOW = GPIO 3
 *      BLUE   = GPIO 4
 *      GREEN  = GPIO 5
 *      ORANGE = GPIO 10
 *      WHITE  = GPIO 12
 *  - DELETE (backspace): GPIO 13
 *
 *  Display States
 *  0: Start screen + IMU-tracked 3D cube
 *  1~6: Face color input (UP, FRONT, BACK, LEFT, RIGHT, DOWN)
 *  7: Solution view (press NEXT to apply next move)
 *
 *  Notes
 *  - I2C lines require pull-ups (internal pull-ups enabled in code; external pull-ups recommended).
 *  - Ensure common ground between VGA resistor network, sensors, and RP2040.
 *========================================================================*/

//========================================================================
// Includes
//========================================================================

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <string.h>
#include "pico/stdlib.h"
#include "pico/multicore.h"
#include "hardware/pwm.h"
#include "hardware/dma.h"
#include "hardware/irq.h"
#include "hardware/adc.h"
#include "hardware/pio.h"
#include "hardware/i2c.h"
#include "hardware/gpio.h"
#include "hardware/clocks.h"
#include "vga16_graphics_v2.h"
#include "mpu6050.h"
#include "gy271.h"
#include "pt_cornell_rp2040_v1_4.h"

//========================================================================
// Define
//========================================================================

// ---------- Display state ----------
#define NUM_DISPLAY_STATES 8
// ---------- Button GPIO pins ----------
#define BUTTON_RED_GPIO    2
#define BUTTON_YELLOW_GPIO 3
#define BUTTON_BLUE_GPIO   4
#define BUTTON_GREEN_GPIO  5
#define BUTTON_ORANGE_GPIO 10
#define BUTTON_GPIO        11
#define BUTTON_WHITE_GPIO  12
#define BUTTON_DELETE_GPIO 13
// ---------- Utility macros ----------
#define min(a,b) ((a<b) ? a:b)
#define max(a,b) ((a<b) ? b:a)
#define abs(a) ((a>0) ? a:-a)
// ---------- Solver settings ----------
#define MAX_MOVE_SEQ 128
// ---------- Cube drawing ----------
#define FACE_SIZE    90
#define CUBE_X      240
#define CUBE_Y      170
#define UP_DX       -30
#define UP_DY       -30
#define LEFT_DX     -30
#define LEFT_DY       0
// ---------- Move grid layout ----------
#define MOVE_CELL_W      100
#define MOVE_CELL_H      100
#define MOVE_CELL_GAP_X  5
#define MOVE_CELL_GAP_Y  10
#define MOVE_CELL_X0     10
#define MOVE_CELL_Y0     40
#define MOVE_COL0_X  (MOVE_CELL_X0)
#define MOVE_COL1_X  (MOVE_CELL_X0 + MOVE_CELL_W + MOVE_CELL_GAP_X)
#define MOVE_COL2_X  (MOVE_COL1_X + MOVE_CELL_W + MOVE_CELL_GAP_X)
#define MOVE_ROW0_Y  (MOVE_CELL_Y0)
#define MOVE_ROW1_Y  (MOVE_CELL_Y0 + MOVE_CELL_H + MOVE_CELL_GAP_Y)
#define MOVE_ROW2_Y  (MOVE_ROW1_Y + MOVE_CELL_H + MOVE_CELL_GAP_Y)
#define MOVE_ROW3_Y  (MOVE_ROW2_Y + MOVE_CELL_H + MOVE_CELL_GAP_Y)
// ---------- Face input grid ----------
#define GRID_SIZE 9

//========================================================================
// Globals
//========================================================================

// ---------- IMU sensor data (fix15) ----------
fix15 acceleration[3];
fix15 gyro[3];
fix15 magnetometer[3];

// ---------- UI / state machine ----------
volatile int display_state = 0;      // 0~7
char         screentext[80];         // text buffer
int          threshold = 10;         // VGA throttle

static int   case0_static_drawn = 0; // title drawn
static int   side_cubes_drawn   = 0; // side cubes drawn
volatile int case7_dirty        = 1; // solver screen redraw

// ---------- Face color input ----------
volatile uint16_t color_grid[6][GRID_SIZE];    // 6 faces × 9
volatile int      fill_index[6] = {0,0,0,0,0,0}; // 0~9 per face

// ---------- Debug counters ----------
volatile int debug_imu_ticks = 0;
volatile int debug_vga_ticks = 0;

// ---------- Cube state ----------
unsigned char cube_stickers[6][3][3]; // 0..5 face id

// ---------- Display mapping ----------
int disp_rot[6]    = { 0, 3, 3, 2, 0, 3 }; // 0/90/180/270
int disp_flip_h[6] = { 1, 0, 0, 1, 0, 0 }; // mirror LR
int disp_flip_v[6] = { 1, 0, 1, 0, 1, 1 }; // mirror UD

// ---------- Move encoding ----------
typedef enum {
    MOVE_NONE = 0,
    MOVE_R, MOVE_R_PRIME, MOVE_R2,
    MOVE_L, MOVE_L_PRIME, MOVE_L2,
    MOVE_F, MOVE_F_PRIME, MOVE_F2,
    MOVE_B, MOVE_B_PRIME, MOVE_B2,
} MoveType;

// ---------- Solver runtime ----------
static MoveType g_moves[MAX_MOVE_SEQ]; // solution moves
static int      g_num_moves       = 0; // move count
static int      g_curr_move_index = -1;// next index
static int      g_solver_active   = 0; // step enabled
static int      g_solution_built  = 0; // valid solution

// ---------- Thread sync ----------
static struct pt_sem vga_semaphore;

// ---------- IMU orientation ----------
volatile float g_rotation_matrix[3][3] = {
    {1.0f, 0.0f, 0.0f},
    {0.0f, 1.0f, 0.0f},
    {0.0f, 0.0f, 1.0f}
};

// ---------- Cube render params ----------
int g_cube_center_x = 320;
int g_cube_center_y = 240;

// ---------- Sensor values (float) ----------
volatile float g_mx = 0.0f, g_my = 0.0f, g_mz = 0.0f;
volatile float g_ax = 0.0f, g_ay = 0.0f, g_az = 0.0f;

// ---------- 3D cube geometry ----------
typedef struct { float x, y, z; } Vec3;

static const Vec3 cube_vertices[8] = {
    {-1.0f,-1.0f,-1.0f}, { 1.0f,-1.0f,-1.0f},
    { 1.0f, 1.0f,-1.0f}, {-1.0f, 1.0f,-1.0f},
    {-1.0f,-1.0f, 1.0f}, { 1.0f,-1.0f, 1.0f},
    { 1.0f, 1.0f, 1.0f}, {-1.0f, 1.0f, 1.0f}
};

static const int cube_edges[12][2] = {
    {0,1},{1,2},{2,3},{3,0},
    {4,5},{5,6},{6,7},{7,4},
    {0,4},{1,5},{2,6},{3,7}
};

static const int cube_faces[6][4] = {
    {0,1,2,3}, // D
    {4,7,6,5}, // U
    {3,7,6,2}, // F
    {0,1,5,4}, // B
    {1,2,6,5}, // R
    {0,4,7,3}  // L
};

enum {
    FACE_D = 0,
    FACE_U = 1,
    FACE_F = 2,
    FACE_B = 3,
    FACE_L = 4,
    FACE_R = 5
};

static const int face_colors[6] = {
    MED_GREEN, // D
    BLUE,      // U
    ORANGE,    // F
    RED,       // B
    YELLOW,    // L
    WHITE      // R
};

static const int edge_faces[12][2] = {
    {FACE_D, FACE_B}, // e0
    {FACE_D, FACE_L}, // e1
    {FACE_D, FACE_F}, // e2
    {FACE_D, FACE_R}, // e3
    {FACE_U, FACE_B}, // e4
    {FACE_U, FACE_L}, // e5
    {FACE_U, FACE_F}, // e6
    {FACE_U, FACE_R}, // e7
    {FACE_B, FACE_R}, // e8
    {FACE_B, FACE_L}, // e9
    {FACE_F, FACE_L}, // e10
    {FACE_F, FACE_R}  // e11
};

//========================================================================
// Functions
//========================================================================
const char* move_to_string(MoveType m)
{
    switch (m) {
        case MOVE_R:       return "R";
        case MOVE_R_PRIME: return "R'";
        case MOVE_R2:      return "R2";
        case MOVE_L:       return "L";
        case MOVE_L_PRIME: return "L'";
        case MOVE_L2:      return "L2";
        case MOVE_F:       return "F";
        case MOVE_F_PRIME: return "F'";
        case MOVE_F2:      return "F2";
        case MOVE_B:       return "B";
        case MOVE_B_PRIME: return "B'";
        case MOVE_B2:      return "B2";
        default:           return "-";
    }
}

// Rotate the face clockwise
static void rotate_face_cw(int f)
{
    unsigned char tmp[3][3];
    for (int r = 0; r < 3; r++)
        for (int c = 0; c < 3; c++)
            tmp[r][c] = cube_stickers[f][r][c];

    // new[r][c] = old[2-c][r]
    for (int r = 0; r < 3; r++) {
        for (int c = 0; c < 3; c++) {
            cube_stickers[f][r][c] = tmp[2 - c][r];
        }
    }
}
// Rotate the face counter clockwise
static void rotate_face_ccw(int f)
{
    rotate_face_cw(f);
    rotate_face_cw(f);
    rotate_face_cw(f);
}

// Rotate the face 180'
static void rotate_face_180(int f)
{
    rotate_face_cw(f);
    rotate_face_cw(f);
}

// (face, r, c) → solver reads cube_stickers (src_r, src_c)
static void map_for_display(int face, int r, int c, int* src_r, int* src_c)
{
    int rr = r;
    int cc = c;

    // 1) Mapping correctly
    switch (disp_rot[face] & 3) {
        case 0:
            // no rotation
            break;

        case 1: {
            // 90° CW: new[r][c] = old[2-c][r]
            int tr = 2 - cc;
            int tc = rr;
            rr = tr;
            cc = tc;
            break;
        }

        case 2: {
            // 180°: new[r][c] = old[2-r][2-c]
            rr = 2 - rr;
            cc = 2 - cc;
            break;
        }

        case 3: {
            // 270° CW (= 90° CCW): new[r][c] = old[c][2-r]
            int tr = cc;
            int tc = 2 - rr;
            rr = tr;
            cc = tc;
            break;
        }
    }

    // 2) mirror left/right
    if (disp_flip_h[face]) {
        cc = 2 - cc;
    }

    // 3) mirror top/bottom
    if (disp_flip_v[face]) {
        rr = 2 - rr;
    }

    *src_r = rr;
    *src_c = cc;
}

// Reset the color_grids
void reset_all_color_grids(void) {
    for (int i = 0; i < 6; i++) {
        fill_index[i] = 0;
        for (int j = 0; j < GRID_SIZE; j++) {
            color_grid[i][j] = BLACK;
        }
    }
}
// Initialize a button
void init_button(void) {
    gpio_init(BUTTON_GPIO);
    gpio_set_dir(BUTTON_GPIO, GPIO_IN);
    gpio_pull_up(BUTTON_GPIO);
}
// Initialize a color button
void init_color_buttons(void) {
    gpio_init(BUTTON_RED_GPIO);
    gpio_set_dir(BUTTON_RED_GPIO, GPIO_IN);
    gpio_pull_up(BUTTON_RED_GPIO);
    
    gpio_init(BUTTON_ORANGE_GPIO);
    gpio_set_dir(BUTTON_ORANGE_GPIO, GPIO_IN);
    gpio_pull_up(BUTTON_ORANGE_GPIO);
    
    gpio_init(BUTTON_BLUE_GPIO);
    gpio_set_dir(BUTTON_BLUE_GPIO, GPIO_IN);
    gpio_pull_up(BUTTON_BLUE_GPIO);
    
    gpio_init(BUTTON_WHITE_GPIO);
    gpio_set_dir(BUTTON_WHITE_GPIO, GPIO_IN);
    gpio_pull_up(BUTTON_WHITE_GPIO);
    
    gpio_init(BUTTON_GREEN_GPIO);
    gpio_set_dir(BUTTON_GREEN_GPIO, GPIO_IN);
    gpio_pull_up(BUTTON_GREEN_GPIO);
    
    gpio_init(BUTTON_YELLOW_GPIO);
    gpio_set_dir(BUTTON_YELLOW_GPIO, GPIO_IN);
    gpio_pull_up(BUTTON_YELLOW_GPIO);
    
    gpio_init(BUTTON_DELETE_GPIO);
    gpio_set_dir(BUTTON_DELETE_GPIO, GPIO_IN);
    gpio_pull_up(BUTTON_DELETE_GPIO);
    
    // color_grid initialize
    for (int i = 0; i < 6; i++) {
        for (int j = 0; j < GRID_SIZE; j++) {
            color_grid[i][j] = BLACK;
        }
        fill_index[i] = 0;
    }
}

void check_color_buttons(void) {
    // Input the colors
    if (display_state < 1 || display_state > 6) {
        return;
    }
    
    int state_idx = display_state - 1;  // 0~4
    
    static int prev_red = 1, prev_orange = 1, prev_blue = 1;
    static int prev_white = 1, prev_green = 1, prev_yellow = 1;
    static int prev_delete = 1;
    
    int curr_red = gpio_get(BUTTON_RED_GPIO);
    int curr_orange = gpio_get(BUTTON_ORANGE_GPIO);
    int curr_blue = gpio_get(BUTTON_BLUE_GPIO);
    int curr_white = gpio_get(BUTTON_WHITE_GPIO);
    int curr_green = gpio_get(BUTTON_GREEN_GPIO);
    int curr_yellow = gpio_get(BUTTON_YELLOW_GPIO);
    int curr_delete = gpio_get(BUTTON_DELETE_GPIO);
    
    // DELETE the last input
    if (prev_delete == 1 && curr_delete == 0) {
        if (fill_index[state_idx] > 0) {
            fill_index[state_idx]--;
            color_grid[state_idx][fill_index[state_idx]] = BLACK;
        }
    }

    // Ignore the additional inputs
    if (fill_index[state_idx] >= GRID_SIZE) {
        prev_delete = curr_delete;
        return;
    }

    // Check the button press
    if (prev_red == 1 && curr_red == 0) {
        color_grid[state_idx][fill_index[state_idx]] = RED;
        fill_index[state_idx]++;
    }
    if (prev_orange == 1 && curr_orange == 0) {
        color_grid[state_idx][fill_index[state_idx]] = ORANGE;
        fill_index[state_idx]++;
    }
    if (prev_blue == 1 && curr_blue == 0) {
        color_grid[state_idx][fill_index[state_idx]] = BLUE;
        fill_index[state_idx]++;
    }
    if (prev_white == 1 && curr_white == 0) {
        color_grid[state_idx][fill_index[state_idx]] = WHITE;
        fill_index[state_idx]++;
    }
    if (prev_green == 1 && curr_green == 0) {
        color_grid[state_idx][fill_index[state_idx]] = MED_GREEN;
        fill_index[state_idx]++;
    }
    if (prev_yellow == 1 && curr_yellow == 0) {
        color_grid[state_idx][fill_index[state_idx]] = YELLOW;
        fill_index[state_idx]++;
    }
    
    // update prev value
    prev_red = curr_red;
    prev_orange = curr_orange;
    prev_blue = curr_blue;
    prev_white = curr_white;
    prev_green = curr_green;
    prev_yellow = curr_yellow;
    prev_delete = curr_delete;
}

// Fill the color inputs
void draw_face_preview(int face_idx, int start_x, int start_y, int square_size, int is_filled) {
    int border = 1;
    int fill_size = square_size - 2 * border;
    
    for (int row = 0; row < 3; row++) {
        for (int col = 0; col < 3; col++) {
            int idx = row * 3 + col;
            int x = start_x + col * square_size;
            int y = start_y + row * square_size;
            
            uint16_t color;
            if (is_filled) {
                color = (idx < fill_index[face_idx]) ? color_grid[face_idx][idx] : BLACK;
            } else {
                color = BLACK;
            }
            
            drawRect(x, y, square_size, square_size, WHITE);
            fillRect(x + border, y + border, fill_size, fill_size, color);
        }
    }
}
// Display the previous faces input
void draw_all_faces_preview(int current_case, int skip_face_idx) {
    int preview_square_size = 16;
    int preview_start_y = 320;
    int face_width = preview_square_size * 3;
    int preview_spacing = 10;
    int num_faces_to_show = 6;
    int total_width = num_faces_to_show * face_width + (num_faces_to_show - 1) * preview_spacing;
    int preview_start_x = (640 - total_width) / 2;
    
    const char* face_names[6] = {"UP", "FRONT", "BACK", "LEFT", "RIGHT", "DOWN"};
    
    for (int i = 0; i < 6; i++) {
        int x = preview_start_x + i * (face_width + preview_spacing);
        int y = preview_start_y;
        int is_filled = (fill_index[i] > 0);
        draw_face_preview(i, x, y, preview_square_size, is_filled);
        
        int label_x = x + (face_width - (int)strlen(face_names[i]) * 6) / 2;
        setCursor(label_x, preview_start_y + face_width + 2);
        sprintf(screentext, "%s", face_names[i]);
        writeString(screentext);
    }
}

static int  is_solved(void);
static int  move_face_index(MoveType m);
static void apply_move(MoveType m);
static void undo_move(MoveType m);

static int dfs_search(int depth, int depth_left, int last_face)
{
    if (is_solved()) {
        g_num_moves = depth;
        return 1;
    }

    if (depth_left == 0) {
        return 0;
    }

    static const MoveType candidates[] = {
        MOVE_R, MOVE_R_PRIME, MOVE_R2,
        MOVE_L, MOVE_L_PRIME, MOVE_L2,
        MOVE_F, MOVE_F_PRIME, MOVE_F2,
        MOVE_B, MOVE_B_PRIME, MOVE_B2
    };
    const int NUM_CAND = sizeof(candidates)/sizeof(candidates[0]);

    for (int i = 0; i < NUM_CAND; i++) {
        MoveType m = candidates[i];
        int mf = move_face_index(m);

        // 1) Skip if (current face == last face)
        if (mf == last_face) {
            continue;
        }
        // 2) Record the move
        g_moves[depth] = m;
        // 3) Apply the move
        apply_move(m);
        if (dfs_search(depth + 1, depth_left - 1, mf)) {
            return 1;
        }
        // 4) Undo the move
        undo_move(m);
    }

    return 0;
}

static int solve_with_iddfs(int max_depth)
{
    for (int d = 0; d <= max_depth; d++) {
        // Calculate the solution with max_depth
        if (dfs_search(0, d, -1)) {
            return 1; // g_moves[] --> solution
        }
    }
    return 0; // No solution in max_depth
}

static int is_solved(void)
{
    for (int f = 0; f < 6; f++) {
        for (int r = 0; r < 3; r++) {
            for (int c = 0; c < 3; c++) {
                if (cube_stickers[f][r][c] != (unsigned char)f) {
                    return 0;
                }
            }
        }
    }
    return 1;
}

static void move_R_cw(void)
{
    // 1) Rotate R face
    rotate_face_cw(FACE_R);

    unsigned char t0, t1, t2;

    t0 = cube_stickers[FACE_U][0][2];
    t1 = cube_stickers[FACE_U][1][2];
    t2 = cube_stickers[FACE_U][2][2];

    // U.right_col <- F.right_col
    cube_stickers[FACE_U][0][2] = cube_stickers[FACE_F][0][2];
    cube_stickers[FACE_U][1][2] = cube_stickers[FACE_F][1][2];
    cube_stickers[FACE_U][2][2] = cube_stickers[FACE_F][2][2];

    // F.right_col <- D.right_col
    cube_stickers[FACE_F][0][2] = cube_stickers[FACE_D][0][2];
    cube_stickers[FACE_F][1][2] = cube_stickers[FACE_D][1][2];
    cube_stickers[FACE_F][2][2] = cube_stickers[FACE_D][2][2];

    // D.right_col <- B.left_col
    cube_stickers[FACE_D][0][2] = cube_stickers[FACE_B][2][0];
    cube_stickers[FACE_D][1][2] = cube_stickers[FACE_B][1][0];
    cube_stickers[FACE_D][2][2] = cube_stickers[FACE_B][0][0];

    // B.left_col <- U.right_col
    cube_stickers[FACE_B][2][0] = t0;
    cube_stickers[FACE_B][1][0] = t1;
    cube_stickers[FACE_B][0][0] = t2;
}


static void move_L_cw(void)
{
    // Rotate the L face
    rotate_face_cw(FACE_L);

    unsigned char t0, t1, t2;

    t0 = cube_stickers[FACE_U][0][0];
    t1 = cube_stickers[FACE_U][1][0];
    t2 = cube_stickers[FACE_U][2][0];

    cube_stickers[FACE_U][0][0] = cube_stickers[FACE_B][2][2];
    cube_stickers[FACE_U][1][0] = cube_stickers[FACE_B][1][2];
    cube_stickers[FACE_U][2][0] = cube_stickers[FACE_B][0][2];

    cube_stickers[FACE_B][2][2] = cube_stickers[FACE_D][0][0];
    cube_stickers[FACE_B][1][2] = cube_stickers[FACE_D][1][0];
    cube_stickers[FACE_B][0][2] = cube_stickers[FACE_D][2][0];

    cube_stickers[FACE_D][0][0] = cube_stickers[FACE_F][0][0];
    cube_stickers[FACE_D][1][0] = cube_stickers[FACE_F][1][0];
    cube_stickers[FACE_D][2][0] = cube_stickers[FACE_F][2][0];

    cube_stickers[FACE_F][0][0] = t0;
    cube_stickers[FACE_F][1][0] = t1;
    cube_stickers[FACE_F][2][0] = t2;
}


static void move_F_cw(void)
{
    // Rotate the F face
    rotate_face_cw(FACE_F);

    unsigned char t0, t1, t2;

    t0 = cube_stickers[FACE_U][2][0];
    t1 = cube_stickers[FACE_U][2][1];
    t2 = cube_stickers[FACE_U][2][2];

    cube_stickers[FACE_U][2][0] = cube_stickers[FACE_L][2][2];
    cube_stickers[FACE_U][2][1] = cube_stickers[FACE_L][1][2];
    cube_stickers[FACE_U][2][2] = cube_stickers[FACE_L][0][2];

    cube_stickers[FACE_L][2][2] = cube_stickers[FACE_D][0][2];
    cube_stickers[FACE_L][1][2] = cube_stickers[FACE_D][0][1];
    cube_stickers[FACE_L][0][2] = cube_stickers[FACE_D][0][0];

    cube_stickers[FACE_D][0][2] = cube_stickers[FACE_R][0][0];
    cube_stickers[FACE_D][0][1] = cube_stickers[FACE_R][1][0];
    cube_stickers[FACE_D][0][0] = cube_stickers[FACE_R][2][0];

    cube_stickers[FACE_R][0][0] = t0;
    cube_stickers[FACE_R][1][0] = t1;
    cube_stickers[FACE_R][2][0] = t2;
}


static void move_B_cw(void)
{
    // Rotate the B face
    rotate_face_cw(FACE_B);

    unsigned char t0, t1, t2;

    t0 = cube_stickers[FACE_U][0][0];
    t1 = cube_stickers[FACE_U][0][1];
    t2 = cube_stickers[FACE_U][0][2];

    cube_stickers[FACE_U][0][0] = cube_stickers[FACE_R][0][2];
    cube_stickers[FACE_U][0][1] = cube_stickers[FACE_R][1][2];
    cube_stickers[FACE_U][0][2] = cube_stickers[FACE_R][2][2];

    cube_stickers[FACE_R][0][2] = cube_stickers[FACE_D][2][2];
    cube_stickers[FACE_R][1][2] = cube_stickers[FACE_D][2][1];
    cube_stickers[FACE_R][2][2] = cube_stickers[FACE_D][2][0];

    cube_stickers[FACE_D][2][2] = cube_stickers[FACE_L][2][0];
    cube_stickers[FACE_D][2][1] = cube_stickers[FACE_L][1][0];
    cube_stickers[FACE_D][2][0] = cube_stickers[FACE_L][0][0];

    cube_stickers[FACE_L][2][0] = t0;
    cube_stickers[FACE_L][1][0] = t1;
    cube_stickers[FACE_L][0][0] = t2;
}

// Apply the move
static void apply_single_move(MoveType m)
{
    switch (m) {
        case MOVE_R:       move_R_cw();               break;
        case MOVE_R_PRIME: move_R_cw(); move_R_cw(); move_R_cw(); break;
        case MOVE_R2:      move_R_cw(); move_R_cw();  break;

        case MOVE_L:       move_L_cw();               break;
        case MOVE_L_PRIME: move_L_cw(); move_L_cw(); move_L_cw(); break;
        case MOVE_L2:      move_L_cw(); move_L_cw();  break;

        case MOVE_F:       move_F_cw();               break;
        case MOVE_F_PRIME: move_F_cw(); move_F_cw(); move_F_cw(); break;
        case MOVE_F2:      move_F_cw(); move_F_cw();  break;

        case MOVE_B:       move_B_cw();               break;
        case MOVE_B_PRIME: move_B_cw(); move_B_cw(); move_B_cw(); break;
        case MOVE_B2:      move_B_cw(); move_B_cw();  break;

        default: break;
    }
}

static void step_solver_one_move(void)
{
    if (!g_solver_active) return;

    if (g_curr_move_index >= 0 && g_curr_move_index < g_num_moves) {
        apply_single_move(g_moves[g_curr_move_index]);
        g_curr_move_index++;
    }

    if (g_curr_move_index >= g_num_moves) {
        g_solver_active = 0;
    }
}

static void build_rlfb_solution_for_current_cube(void)
{
    g_solution_built  = 0;
    g_solver_active   = 0;
    g_curr_move_index = -1;

    // Already solved
    if (is_solved()) {
        g_num_moves      = 0;
        g_solution_built = 1;
        return;
    }

    unsigned char backup[6][3][3];
    for (int f = 0; f < 6; f++) {
        for (int r = 0; r < 3; r++) {
            for (int c = 0; c < 3; c++) {
                backup[f][r][c] = cube_stickers[f][r][c];
            }
        }
    }

    const int MAX_DEPTH = 6;
    if (solve_with_iddfs(MAX_DEPTH)) {
        for (int f = 0; f < 6; f++) {
            for (int r = 0; r < 3; r++) {
                for (int c = 0; c < 3; c++) {
                    cube_stickers[f][r][c] = backup[f][r][c];
                }
            }
        }

        g_curr_move_index = 0;
        g_solver_active   = 1;
        g_solution_built  = 1;
    }
    else {
        for (int f = 0; f < 6; f++) {
            for (int r = 0; r < 3; r++) {
                for (int c = 0; c < 3; c++) {
                    cube_stickers[f][r][c] = backup[f][r][c];
                }
            }
        }

        g_num_moves       = 0;
        g_curr_move_index = -1;
        g_solver_active   = 0;
        g_solution_built  = 0;
    }
}

// rot = 0,1,2,3  → 0°, 90° CW, 180°, 270° CW(=90° CCW)
static void copy_face_with_rotation(
    int cube_face,   // FACE_U ... FACE_R
    int cg_idx,      // color_grid index (0~5)
    int rot          // 0,1,2,3
){
    for (int r = 0; r < 3; r++) {
        for (int c = 0; c < 3; c++) {

            int src_r, src_c;
            switch (rot) {
                case 0:  // No move
                    src_r = r;
                    src_c = c;
                    break;

                case 1:  // 90' clockwise
                    src_r = 2 - c;
                    src_c = r;
                    break;

                case 2:  // 180'
                    src_r = 2 - r;
                    src_c = 2 - c;
                    break;

                case 3:  // 90' counter clockwise
                    src_r = c;
                    src_c = 2 - r;
                    break;
            }

            int idx = src_r * 3 + src_c;
            uint16_t col = color_grid[cg_idx][idx];

            int sticker_face = cube_face;

            if (col != BLACK) {
                for (int k = 0; k < 6; k++) {
                    if (face_colors[k] == col) {
                        sticker_face = k;
                        break;
                    }
                }
            }

            cube_stickers[cube_face][r][c] = (unsigned char)sticker_face;
        }
    }
}

// 16.16 float point slope calculation
static inline fix15 slope_fix15(int dx, int dy)
{
    return (fix15)(((long long)dx << 16) / dy);
}

void init_cube_solved(void)
{
    for (int f = 0; f < 6; f++) {
        for (int r = 0; r < 3; r++) {
            for (int c = 0; c < 3; c++) {
                cube_stickers[f][r][c] = (unsigned char)f;  // FACE_D..FACE_R
            }
        }
    }
}

static MoveType inverse_move(MoveType m)
{
    switch (m) {
        case MOVE_R:       return MOVE_R_PRIME;
        case MOVE_R_PRIME: return MOVE_R;
        case MOVE_R2:      return MOVE_R2;

        case MOVE_L:       return MOVE_L_PRIME;
        case MOVE_L_PRIME: return MOVE_L;
        case MOVE_L2:      return MOVE_L2;

        case MOVE_F:       return MOVE_F_PRIME;
        case MOVE_F_PRIME: return MOVE_F;
        case MOVE_F2:      return MOVE_F2;

        case MOVE_B:       return MOVE_B_PRIME;
        case MOVE_B_PRIME: return MOVE_B;
        case MOVE_B2:      return MOVE_B2;

        default:           return MOVE_NONE;
    }
}

static void apply_move(MoveType m)
{
    apply_single_move(m);
}

static void undo_move(MoveType m)
{
    MoveType inv = inverse_move(m);
    if (inv != MOVE_NONE) {
        apply_single_move(inv);
    }
}

static int move_face_index(MoveType m)
{
    switch (m) {
        case MOVE_R:
        case MOVE_R_PRIME:
        case MOVE_R2:
            return FACE_R;

        case MOVE_L:
        case MOVE_L_PRIME:
        case MOVE_L2:
            return FACE_L;

        case MOVE_F:
        case MOVE_F_PRIME:
        case MOVE_F2:
            return FACE_F;

        case MOVE_B:
        case MOVE_B_PRIME:
        case MOVE_B2:
            return FACE_B;

        default:
            return -1;
    }
}

void update_cube_stickers_from_color_grid(void)
{
    copy_face_with_rotation(FACE_U, 0, 0);  // rot=0
    // FRONT
    copy_face_with_rotation(FACE_F, 1, 0);  // rot=0
    // BACK
    copy_face_with_rotation(FACE_B, 2, 2);  // rot=0
    // LEFT:  90° CCW
    copy_face_with_rotation(FACE_L, 3, 3);  // rot=3
    // RIGHT: 90° CW
    copy_face_with_rotation(FACE_R, 4, 1);  // rot=1
    // DOWN
    copy_face_with_rotation(FACE_D, 5, 0);  // rot=0
}

// Fill the flat-bottom triangle
static void fillFlatBottom(
    int x0, int y0,
    int x1, int y1,
    int x2, int y2,
    int color
){
    int dy1 = y1 - y0;
    int dy2 = y2 - y0;
    if (dy1 == 0 || dy2 == 0) return;

    fix15 invslope1 = slope_fix15(x1 - x0, dy1);
    fix15 invslope2 = slope_fix15(x2 - x0, dy2);

    fix15 curx1 = int2fix15(x0);
    fix15 curx2 = int2fix15(x0);

    for (int y = y0; y <= y2; y++) {
        int x_start = fix2int15(curx1);
        int x_end   = fix2int15(curx2);
        if (x_start > x_end) {
            int t = x_start; x_start = x_end; x_end = t;
        }
        drawLine(x_start, y, x_end, y, color);

        curx1 += invslope1;
        curx2 += invslope2;
    }
}

// Fill the flat-top triangle
static void fillFlatTop(
    int x0, int y0,
    int x1, int y1,
    int x2, int y2,
    int color
){
    int dy1 = y2 - y0;
    int dy2 = y2 - y1;
    if (dy1 == 0 || dy2 == 0) return;

    fix15 invslope1 = slope_fix15(x2 - x0, dy1);
    fix15 invslope2 = slope_fix15(x2 - x1, dy2);

    fix15 curx1 = int2fix15(x2);
    fix15 curx2 = int2fix15(x2);

    for (int y = y2; y >= y0; y--) {
        int x_start = fix2int15(curx1);
        int x_end   = fix2int15(curx2);
        if (x_start > x_end) {
            int t = x_start; x_start = x_end; x_end = t;
        }
        drawLine(x_start, y, x_end, y, color);

        curx1 -= invslope1;
        curx2 -= invslope2;
    }
}

// Fill triangle using scanline algorithm (fix15)
void fillTriangle(int x0, int y0,
                  int x1, int y1,
                  int x2, int y2,
                  int color)
{
    if (y0 > y1) {
        int tx = x0; x0 = x1; x1 = tx;
        int ty = y0; y0 = y1; y1 = ty;
    }
    if (y1 > y2) {
        int tx = x1; x1 = x2; x2 = tx;
        int ty = y1; y1 = y2; y2 = ty;
    }
    if (y0 > y1) {
        int tx = x0; x0 = x1; x1 = tx;
        int ty = y0; y0 = y1; y1 = ty;
    }

    if (y0 == y2) {
        int minx = x0;
        int maxx = x0;
        if (x1 < minx) minx = x1;
        if (x2 < minx) minx = x2;
        if (x1 > maxx) maxx = x1;
        if (x2 > maxx) maxx = x2;
        drawLine(minx, y0, maxx, y0, color);
        return;
    }

    if (y1 == y2) {
        // flat-bottom : (x0,y0) -> (x1,y1),(x2,y2)
        fillFlatBottom(x0, y0, x1, y1, x2, y2, color);
    }
    else if (y0 == y1) {
        // flat-top : (x0,y0),(x1,y1) -> (x2,y2)
        fillFlatTop(x0, y0, x1, y1, x2, y2, color);
    }
    else {
        int dy_total = y2 - y0;
        if (dy_total == 0) return;

        int x3 = x0 + (int)(((long long)(y1 - y0) * (x2 - x0)) / dy_total);
        int y3 = y1;

        // (flat-bottom)
        fillFlatBottom(x0, y0, x1, y1, x3, y3, color);

        // (flat-top)
        fillFlatTop(x1, y1, x3, y3, x2, y2, color);
    }
}

// Matrix multiplication: result = Q × v
void matrix_multiply_vector(const float Q[3][3], const float v[3], float result[3]) {
    result[0] = Q[0][0] * v[0] + Q[0][1] * v[1] + Q[0][2] * v[2];
    result[1] = Q[1][0] * v[0] + Q[1][1] * v[1] + Q[1][2] * v[2];
    result[2] = Q[2][0] * v[0] + Q[2][1] * v[1] + Q[2][2] * v[2];
}

// Interpolate the screen coordinates at (u, v) on the quad face (i0, i1, i2, i3)
static void interp_face_point(
    int i0, int i1, int i2, int i3,
    float u, float v,
    int sx[], int sy[],
    int *x_out, int *y_out
){
    // Four vertices (screen coordinates)
    float x0 = (float)sx[i0], y0 = (float)sy[i0];
    float x1 = (float)sx[i1], y1 = (float)sy[i1];
    float x2 = (float)sx[i2], y2 = (float)sy[i2];
    float x3 = (float)sx[i3], y3 = (float)sy[i3];

    float ax = x0 + (x1 - x0) * u;
    float ay = y0 + (y1 - y0) * u;

    float bx = x3 + (x2 - x3) * u;
    float by = y3 + (y2 - y3) * u;

    float px = ax + (bx - ax) * v;
    float py = ay + (by - ay) * v;

    *x_out = (int)px;
    *y_out = (int)py;
}


// Draw 3D rotated cube using rotation matrix Q (from inertial to body frame)
void draw_cube(const float Q[3][3])
{
    const int   cx    = g_cube_center_x;
    const int   cy    = g_cube_center_y;
    const float scale = 80.0f;

    // Q transpose : body -> world
    float Q_T[3][3];
    for (int i = 0; i < 3; i++) {
        for (int j = 0; j < 3; j++) {
            Q_T[i][j] = Q[j][i];
        }
    }

    int   sx[8], sy[8];        // screen coords
    float vx[8], vy[8], vz[8]; // "world" coords (for depth/face test)

    // 1) Vertex rotation + screen coordinate transform
    for (int i = 0; i < 8; i++) {
        float v_original[3] = {
            cube_vertices[i].x,
            cube_vertices[i].y,
            cube_vertices[i].z
        };
        float v_rotated[3];
        matrix_multiply_vector(Q_T, v_original, v_rotated);

        // Flip signs for x,y to match the screen coordinate system; keep z as-is
        float x = -v_rotated[0];
        float y = -v_rotated[1];
        float z =  v_rotated[2];

        sx[i] = (int)(cx + x * scale);
        sy[i] = (int)(cy - y * scale);

        vx[i] = x;
        vy[i] = y;
        vz[i] = z;
    }

    // 2) Use each face's center z-value
    //    to determine visibility toward the camera (+z direction)
    int   face_visible[6] = {0};
    float face_depth[6];  // use the center z-value as depth

    for (int f = 0; f < 6; f++) {
        int i0 = cube_faces[f][0];
        int i1 = cube_faces[f][1];
        int i2 = cube_faces[f][2];
        int i3 = cube_faces[f][3];

        float cz = (vz[i0] + vz[i1] + vz[i2] + vz[i3]) * 0.25f;
        face_depth[f] = cz;

        // Assume camera direction is (0,0,+1);
        // if the face center is on the +z side, treat it as visible
        if (cz > 0.0f) {
            face_visible[f] = 1;
        }
    }

    // 3) Sort only visible faces by depth (back -> front)
    typedef struct {
        int   idx;
        float depth;
    } FaceOrder;

    FaceOrder order[6];
    int n_front = 0;
    for (int f = 0; f < 6; f++) {
        if (face_visible[f]) {
            order[n_front].idx   = f;
            order[n_front].depth = face_depth[f];
            n_front++;
        }
    }

    // Sort by ascending depth (render farther faces first)
    for (int i = 0; i < n_front; i++) {
        for (int j = i + 1; j < n_front; j++) {
            if (order[j].depth < order[i].depth) {
                FaceOrder tmp = order[i];
                order[i]      = order[j];
                order[j]      = tmp;
            }
        }
    }

    // 4) Fill faces with colors in sorted order (Painter's algorithm)
    for (int k = 0; k < n_front; k++) {
        int f = order[k].idx;

        int i0 = cube_faces[f][0];
        int i1 = cube_faces[f][1];
        int i2 = cube_faces[f][2];
        int i3 = cube_faces[f][3];
        // ---------- 4-1. Fill 3x3 sticker tiles ----------
        for (int row = 0; row < 3; row++) {
            for (int col = 0; col < 3; col++) {

                // u: left -> right, v: top -> bottom (0~1)
                float u0 = (float)col     / 3.0f;
                float u1 = (float)(col+1) / 3.0f;
                float v0 = (float)row     / 3.0f;
                float v1 = (float)(row+1) / 3.0f;

                int x00, y00; // (u0, v0)
                int x10, y10; // (u1, v0)
                int x11, y11; // (u1, v1)
                int x01, y01; // (u0, v1)

                // Interpolate the four corner coordinates of the sub-tile on the face quad
                interp_face_point(i0, i1, i2, i3, u0, v0, sx, sy, &x00, &y00);
                interp_face_point(i0, i1, i2, i3, u1, v0, sx, sy, &x10, &y10);
                interp_face_point(i0, i1, i2, i3, u1, v1, sx, sy, &x11, &y11);
                interp_face_point(i0, i1, i2, i3, u0, v1, sx, sy, &x01, &y01);

                // Determine the sticker color for face f at (row, col)
                int src_r, src_c;
                map_for_display(f, row, col, &src_r, &src_c);

                unsigned char sticker_face = cube_stickers[f][src_r][src_c];
                int sticker_color = face_colors[sticker_face];

                // Fill the tile using two triangles
                fillTriangle(x00, y00, x10, y10, x11, y11, sticker_color);
                fillTriangle(x00, y00, x11, y11, x01, y01, sticker_color);
            }
        }
        // ---------- 4-2. Draw black grid lines ----------
        // Vertical lines (u = 1/3, 2/3)
        for (int s = 1; s <= 2; s++) {
            float u = (float)s / 3.0f;
            int xs0, ys0, xs1, ys1;
            interp_face_point(i0, i1, i2, i3, u, 0.0f, sx, sy, &xs0, &ys0);
            interp_face_point(i0, i1, i2, i3, u, 1.0f, sx, sy, &xs1, &ys1);
            drawLine(xs0, ys0, xs1, ys1, BLACK);
        }

        // Horizontal lines (v = 1/3, 2/3)
        for (int s = 1; s <= 2; s++) {
            float v = (float)s / 3.0f;
            int xs0, ys0, xs1, ys1;
            interp_face_point(i0, i1, i2, i3, 0.0f, v, sx, sy, &xs0, &ys0);
            interp_face_point(i0, i1, i2, i3, 1.0f, v, sx, sy, &xs1, &ys1);
            drawLine(xs0, ys0, xs1, ys1, BLACK);
        }
    }

    // 5) Draw only edges adjacent to visible faces in white
    for (int e = 0; e < 12; e++) {
        int fA = edge_faces[e][0];
        int fB = edge_faces[e][1];

        // If both adjacent faces are not visible, hide this edge
        if (!face_visible[fA] && !face_visible[fB]) {
            continue;
        }

        int v0 = cube_edges[e][0];
        int v1 = cube_edges[e][1];

        drawLine(sx[v0], sy[v0], sx[v1], sy[v1], WHITE);
    }
}

// Vector normalization
float normalize_vector(float v[3]) {
    float len = sqrtf(v[0]*v[0] + v[1]*v[1] + v[2]*v[2]);
    if (len > 0.0001f) {
        v[0] /= len;
        v[1] /= len;
        v[2] /= len;
    }
    return len;
}

// Cross product
void cross_product(const float a[3], const float b[3], float result[3]) {
    result[0] = a[1] * b[2] - a[2] * b[1];
    result[1] = a[2] * b[0] - a[0] * b[2];
    result[2] = a[0] * b[1] - a[1] * b[0];
}

// Dot product
float dot_product(const float a[3], const float b[3]) {
    return a[0]*b[0] + a[1]*b[1] + a[2]*b[2];
}

// Build rotation matrix Q from two basis vectors: TN = [v1, v1×v2, v1×(v1×v2)]
void build_triad(const float v1[3], const float v2[3], float Q[3][3]) {
    float e1[3] = {v1[0], v1[1], v1[2]};
    normalize_vector(e1);

    float e2[3];
    cross_product(e1, v2, e2);
    normalize_vector(e2);

    float e3[3];
    cross_product(e1, e2, e3);
    normalize_vector(e3);
        
    // Build Q: columns are body frame basis vectors in inertial frame
    Q[0][0] = e1[0]; Q[1][0] = e1[1]; Q[2][0] = e1[2];
    Q[0][1] = e2[0]; Q[1][1] = e2[1]; Q[2][1] = e2[2];
    Q[0][2] = e3[0]; Q[1][2] = e3[1]; Q[2][2] = e3[2];
}

// IMU data reading and rotation matrix calculation
void update_imu_and_filter(void) {
    mpu6050_read_raw(acceleration, gyro);
    gy271_read_raw(magnetometer);

    float ax = fix2float15(acceleration[0]);
    float ay = fix2float15(acceleration[1]);
    float az = fix2float15(acceleration[2]);

    float mx = fix2float15(magnetometer[0]);
    float my = fix2float15(magnetometer[1]);
    float mz = fix2float15(magnetometer[2]);

    g_ax = ax; g_ay = ay; g_az = az;
    g_mx = mx; g_my = my; g_mz = mz;

    static float T_N[3][3];
    static int triad_inited = 0;
    if (!triad_inited) {
        float Nv1[3] = { ax, ay, az };
        float Nv2[3] = { mx, my, mz };
        build_triad(Nv1, Nv2, T_N);

        triad_inited = 1;
    }

    float Bv1[3] = { ax, ay, az };
    float Bv2[3] = { mx, my, mz };
    float T_B[3][3];
    build_triad(Bv1, Bv2, T_B);

    // --- 3) Q = T_B * T_N^T  ---
    float Q_temp[3][3];
    for (int i = 0; i < 3; i++) {       // row of T_B
        for (int j = 0; j < 3; j++) {   // row of T_N (== col of T_N^T)
            Q_temp[i][j] =
                T_B[i][0]*T_N[j][0] +
                T_B[i][1]*T_N[j][1] +
                T_B[i][2]*T_N[j][2];
        }
    }

    for (int i = 0; i < 3; i++) {
        for (int j = 0; j < 3; j++) {
            g_rotation_matrix[i][j] = Q_temp[i][j];
        }
    }
}

void check_button_press(void) {
    static int prev = 1;
    static int prev_display_state = 0;
    int curr = gpio_get(BUTTON_GPIO);

    if (prev == 1 && curr == 0) { // falling edge
        if (display_state == 7 && g_solution_built) {
            step_solver_one_move();
        }
        else {
            prev_display_state = display_state;
            display_state = display_state + 1;
            if (display_state > 7) {
                display_state = 0;
            }

            if (display_state == 6) {
            case7_dirty = 1;
        }

            // Reset the color inputs
            if (display_state == 1 && (prev_display_state == 0 || prev_display_state == 7)) {
                reset_all_color_grids();
            }

            // State 6 → 7
            // Copy the color inputs to color_grid + build the solution
            if (display_state == 7 && prev_display_state == 6) {
                update_cube_stickers_from_color_grid();
                build_rlfb_solution_for_current_cube();
            }

            // State =! 7
            if (display_state != 7) {
                g_solver_active   = 0;
                g_solution_built  = 0;
                g_curr_move_index = -1;
            }
        }
    }
    prev = curr;
}

// fill the triangle
static void fill_Triangle(int x0, int y0, int x1, int y1, int x2, int y2, uint16_t color)
{
    if (y0 > y1) { int t=y0; y0=y1; y1=t; t=x0; x0=x1; x1=t; }
    if (y1 > y2) { int t=y1; y1=y2; y2=t; t=x1; x1=x2; x2=t; }
    if (y0 > y1) { int t=y0; y0=y1; y1=t; t=x0; x0=x1; x1=t; }

    if (y0 == y2) {
        int minx = x0, maxx = x0;
        if (x1 < minx) minx = x1;
        if (x2 < minx) minx = x2;
        if (x1 > maxx) maxx = x1;
        if (x2 > maxx) maxx = x2;
        drawLine(minx, y0, maxx, y0, color);
        return;
    }

    float inv_dy02 = (y2 != y0) ? 1.0f / (float)(y2 - y0) : 0.0f;
    float inv_dy01 = (y1 != y0) ? 1.0f / (float)(y1 - y0) : 0.0f;
    float inv_dy12 = (y2 != y1) ? 1.0f / (float)(y2 - y1) : 0.0f;

    for (int y = y0; y <= y2; y++) {
        int xa, xb;
        float alpha = (float)(y - y0) * inv_dy02;

        if (y <= y1) {
            float beta = (y1 != y0) ? (float)(y - y0) * inv_dy01 : 0.0f;
            float fax = x0 + (x2 - x0) * alpha;
            float fbx = x0 + (x1 - x0) * beta;
            xa = (int)fax;
            xb = (int)fbx;
        } else {
            float beta = (y2 != y1) ? (float)(y - y1) * inv_dy12 : 0.0f;
            float fax = x0 + (x2 - x0) * alpha;
            float fbx = x1 + (x2 - x1) * beta;
            xa = (int)fax;
            xb = (int)fbx;
        }

        if (xa > xb) {
            int t = xa; xa = xb; xb = t;
        }

        drawLine(xa, y, xb, y, color);
    }
}

static void draw_face_grid_parallelogram(
    int x0,int y0, int x1,int y1,
    int x2,int y2, int x3,int y3,
    uint16_t grid_color
);

void draw_solid_cube_diag(float angle_rad, uint16_t color)
{
    const int   cx    = 320;
    const int   cy    = 260;
    const float scale = 50.0f;

    const float ax = 0.57735027f;
    const float ay = 0.57735027f;
    const float az = 0.57735027f;

    float c = cosf(angle_rad);
    float s = sinf(angle_rad);
    float one_c = 1.0f - c;

    float R[3][3];
    R[0][0] = c + ax*ax*one_c;
    R[0][1] = ax*ay*one_c - az*s;
    R[0][2] = ax*az*one_c + ay*s;

    R[1][0] = ay*ax*one_c + az*s;
    R[1][1] = c + ay*ay*one_c;
    R[1][2] = ay*az*one_c - ax*s;

    R[2][0] = az*ax*one_c - ay*s;
    R[2][1] = az*ay*one_c + ax*s;
    R[2][2] = c + az*az*one_c;

    float vx[8], vy[8], vz[8];
    int   sx[8], sy[8];

    for (int i = 0; i < 8; i++) {
        float x = cube_vertices[i].x;
        float y = cube_vertices[i].y;
        float z = cube_vertices[i].z;

        float rx = R[0][0]*x + R[0][1]*y + R[0][2]*z;
        float ry = R[1][0]*x + R[1][1]*y + R[1][2]*z;
        float rz = R[2][0]*x + R[2][1]*y + R[2][2]*z;

        vx[i] = rx;
        vy[i] = ry;
        vz[i] = rz;

        sx[i] = (int)(cx + rx * scale);
        sy[i] = (int)(cy - ry * scale);
    }

    typedef struct {
        int   idx;
        float avg_z;
    } FaceDepth;

    FaceDepth fd[6];
    for (int f = 0; f < 6; f++) {
        float avg = (vz[cube_faces[f][0]] +
                     vz[cube_faces[f][1]] +
                     vz[cube_faces[f][2]] +
                     vz[cube_faces[f][3]]) * 0.25f;
        fd[f].idx   = f;
        fd[f].avg_z = avg;
    }

    for (int i = 0; i < 6; i++) {
        for (int j = i + 1; j < 6; j++) {
            if (fd[j].avg_z < fd[i].avg_z) {
                FaceDepth tmp = fd[i];
                fd[i] = fd[j];
                fd[j] = tmp;
            }
        }
    }

    for (int k = 0; k < 6; k++) {
        int f  = fd[k].idx;
        int i0 = cube_faces[f][0];
        int i1 = cube_faces[f][1];
        int i2 = cube_faces[f][2];
        int i3 = cube_faces[f][3];

        float ux = vx[i1] - vx[i0];
        float uy = vy[i1] - vy[i0];
        float uz = vz[i1] - vz[i0];

        float vx2 = vx[i2] - vx[i0];
        float vy2 = vy[i2] - vy[i0];
        float vz2 = vz[i2] - vz[i0];

        float nz = ux * vy2 - uy * vx2;

        fill_Triangle(sx[i0], sy[i0], sx[i1], sy[i1], sx[i2], sy[i2], color);
        fill_Triangle(sx[i0], sy[i0], sx[i2], sy[i2], sx[i3], sy[i3], color);

        draw_face_grid_parallelogram(
            sx[i0], sy[i0],
            sx[i1], sy[i1],
            sx[i2], sy[i2],
            sx[i3], sy[i3],
            WHITE
        );
    }

    for (int e = 0; e < 12; e++) {
        drawLine(
            sx[cube_edges[e][0]], sy[cube_edges[e][0]],
            sx[cube_edges[e][1]], sy[cube_edges[e][1]],
            WHITE
        );
    }
}

static void draw_parallelogram_outline(
    int x0,int y0, int x1,int y1,
    int x2,int y2, int x3,int y3,
    uint16_t color
){
    drawLine(x0,y0, x1,y1, color);
    drawLine(x1,y1, x2,y2, color);
    drawLine(x2,y2, x3,y3, color);
    drawLine(x3,y3, x0,y0, color);
}

static void fill_parallelogram(
    int x0,int y0, int x1,int y1,
    int x2,int y2, int x3,int y3,
    uint16_t color
){
    const int STEPS = FACE_SIZE;
    for (int i = 0; i <= STEPS; i++) {
        float t = (float)i / (float)STEPS;

        float top_x = x0 + (x1 - x0) * t;
        float top_y = y0 + (y1 - y0) * t;
        float bot_x = x3 + (x2 - x3) * t;
        float bot_y = y3 + (y2 - y3) * t;

        drawLine((int)top_x, (int)top_y,
                 (int)bot_x, (int)bot_y,
                 color);
    }
}

static void draw_front_grid(int x,int y,int size, uint16_t border_color)
{
    int step = size / 3;

    drawRect(x, y, size, size, border_color);

    drawLine(x+step,     y,       x+step,     y+size, border_color);
    drawLine(x+2*step,   y,       x+2*step,   y+size, border_color);

    drawLine(x,          y+step,  x+size,     y+step, border_color);
    drawLine(x,          y+2*step,x+size,     y+2*step, border_color);
}

static void draw_face_grid_parallelogram(
    int x0,int y0, int x1,int y1,
    int x2,int y2, int x3,int y3,
    uint16_t grid_color
){
    for (int i = 1; i <= 2; i++) {
        float t = i / 3.0f;
        float top_x = x0 + (x1 - x0) * t;
        float top_y = y0 + (y1 - y0) * t;
        float bot_x = x3 + (x2 - x3) * t;
        float bot_y = y3 + (y2 - y3) * t;
        drawLine((int)top_x, (int)top_y,
                 (int)bot_x, (int)bot_y,
                 grid_color);
    }

    for (int i = 1; i <= 2; i++) {
        float t = i / 3.0f;
        float left_x  = x0 + (x3 - x0) * t;
        float left_y  = y0 + (y3 - y0) * t;
        float right_x = x1 + (x2 - x1) * t;
        float right_y = y1 + (y2 - y1) * t;
        drawLine((int)left_x, (int)left_y,
                 (int)right_x, (int)right_y,
                 grid_color);
    }
}

void draw_small_cube_3faces(int fx, int fy, int size, uint16_t front_col, uint16_t up_col, uint16_t left_col)
{
    fillRect(fx, fy, size, size, front_col);
    draw_front_grid(fx, fy, size, BLACK);

    int F0x = fx,        F0y = fy;
    int F1x = fx+size,   F1y = fy;
    int F2x = fx+size,   F2y = fy+size;
    int F3x = fx,        F3y = fy+size;
    int up_dx = -size / 3;
    int up_dy = -size / 3;

    int U0x = F0x;           int U0y = F0y;
    int U1x = F1x;           int U1y = F1y;
    int U2x = F1x + up_dx;   int U2y = F1y + up_dy;
    int U3x = F0x + up_dx;   int U3y = F0y + up_dy;

    fill_parallelogram(
        U3x, U3y,
        U2x, U2y,
        U1x, U1y,
        U0x, U0y,
        up_col
    );
    draw_parallelogram_outline(
        U3x, U3y,
        U2x, U2y,
        U1x, U1y,
        U0x, U0y,
        BLACK
    );
    draw_face_grid_parallelogram(
        U3x, U3y,
        U2x, U2y,
        U1x, U1y,
        U0x, U0y,
        BLACK
    );

    int L0x = F0x;           int L0y = F0y;
    int L1x = F3x;           int L1y = F3y;
    int L3x = U3x;           int L3y = U3y;
    int L2x = U3x;           int L2y = U3y + (F3y - F0y);

    fill_parallelogram(
        L3x, L3y,
        L0x, L0y,
        L1x, L1y,
        L2x, L2y,
        left_col
    );
    draw_parallelogram_outline(
        L3x, L3y,
        L0x, L0y,
        L1x, L1y,
        L2x, L2y,
        BLACK
    );
    draw_face_grid_parallelogram(
        L3x, L3y,
        L0x, L0y,
        L1x, L1y,
        L2x, L2y,
        BLACK
    );
}

typedef struct {
    uint16_t up;
    uint16_t front;
    uint16_t side;
} SmallCubeColor;

static const SmallCubeColor left_column_cubes[8] = {
    { BLUE,  ORANGE, YELLOW }, // U-F-L
    { BLUE,  YELLOW, RED  }, // U-F-R
    { BLUE,  WHITE,  ORANGE }, // U-B-L
    { BLUE,  RED,    WHITE  }, // U-B-R

    { WHITE, BLUE, RED }, // D-F-L
    { YELLOW, MED_GREEN, RED  }, // D-F-R
    { BLUE,  ORANGE, YELLOW }, // U-F-L
    { BLUE,  YELLOW, RED  }, // U-F-R
};

static const SmallCubeColor right_column_cubes[8] = {
    { WHITE, BLUE, RED }, // D-F-L
    { YELLOW, MED_GREEN, RED  }, // D-F-R
    { BLUE,  ORANGE, YELLOW }, // U-F-L
    { BLUE,  YELLOW, RED  }, // U-F-R
    { BLUE,  ORANGE, YELLOW }, // U-F-L
    { BLUE,  YELLOW, RED  }, // U-F-R
    { BLUE,  WHITE,  ORANGE }, // U-B-L
    { BLUE,  RED,    WHITE  }, // U-B-R
};

void draw_case0_static_cubes(void)
{
    int size   = 15;
    int left_x = 40;
    int right_x = 640 - 40 - size;

    int base_y = 120;
    int step_y = 35;

    for (int i = 0; i < 8; i++) {
        int y = base_y + i * step_y;

        const SmallCubeColor *L = &left_column_cubes[i];
        const SmallCubeColor *R = &right_column_cubes[i];

        draw_small_cube_3faces(
            left_x, y,
            size,
            L->front,
            L->up,
            L->side
        );

        draw_small_cube_3faces(
            right_x, y,
            size,
            R->front,
            R->up,
            R->side
        );
    }
}

static void draw_curved_arrow_L(int x0, int y0, int x1, int y1, float curvature, uint16_t color)
{
    float p0x = (float)x0, p0y = (float)y0;
    float p2x = (float)x1, p2y = (float)y1;

    float midx = 0.5f * (p0x + p2x);
    float midy = 0.5f * (p0y + p2y) - curvature;
    float p1x = midx;
    float p1y = midy;

    const int STEPS = 40;
    int prevx = x0;
    int prevy = y0;

    for (int i = 1; i <= STEPS; i++) {
        float t = (float)i / (float)STEPS;
        float u = 1.0f - t;

        float bx = u*u*p0x + 2*u*t*p1x + t*t*p2x;
        float by = u*u*p0y + 2*u*t*p1y + t*t*p2y;

        int ix = (int)(bx + 0.5f);
        int iy = (int)(by + 0.5f);

        drawLine(prevx, prevy, ix, iy, color);

        prevx = ix;
        prevy = iy;
    }

    float t = 0.99f;
    float u = 1.0f - t;
    float dx = 2*u*(p1x - p0x) + 2*t*(p2x - p1x);
    float dy = 2*u*(p1y - p0y) + 2*t*(p2y - p1y);

    float len = sqrtf(dx*dx + dy*dy);
    if (len < 1.0f) return;

    dx /= len;
    dy /= len;

    float head_len   = 12.0f;
    float head_width = 6.0f;

    float bx2 = (float)x1 - dx * head_len;
    float by2 = (float)y1 - dy * head_len;

    float px = -dy;
    float py =  dx;

    int hx1 = (int)(bx2 + px * head_width);
    int hy1 = (int)(by2 + py * head_width);
    int hx2 = (int)(bx2 - px * head_width);
    int hy2 = (int)(by2 - py * head_width);

    fill_Triangle(x1, y1, hx1, hy1, hx2, hy2, color);
}

static void draw_mid_triangle_on_curve( int x0, int y0, int x1, int y1, float curvature, uint16_t color )
{
    float p0x = (float)x0, p0y = (float)y0;
    float p2x = (float)x1, p2y = (float)y1;

    float midx = 0.5f * (p0x + p2x);
    float midy = 0.5f * (p0y + p2y) - curvature;
    float p1x = midx;
    float p1y = midy;

    float t = 0.5f;
    float u = 1.0f - t;

    float cx = u*u*p0x + 2*u*t*p1x + t*t*p2x;
    float cy = u*u*p0y + 2*u*t*p1y + t*t*p2y;

    float dx = 2*u*(p1x - p0x) + 2*t*(p2x - p1x);
    float dy = 2*u*(p1y - p0y) + 2*t*(p2y - p1y);

    float len = sqrtf(dx*dx + dy*dy);
    if ( len < 1.0f ) return;

    dx /= len;
    dy /= len;

    float head_len   = 12.0f;
    float head_width = 6.0f;

    float tipx = cx;
    float tipy = cy;

    float bx = tipx - dx * head_len;
    float by = tipy - dy * head_len;

    float px = -dy;
    float py =  dx;

    int hx1 = (int)(bx + px * head_width + 0.5f);
    int hy1 = (int)(by + py * head_width + 0.5f);
    int hx2 = (int)(bx - px * head_width + 0.5f);
    int hy2 = (int)(by - py * head_width + 0.5f);

    fill_Triangle(
        (int)(tipx + 0.5f),
        (int)(tipy + 0.5f),
        hx1, hy1,
        hx2, hy2,
        color
    );
}

typedef struct {
    int F0x, F0y;
    int F1x, F1y;

    int U0x, U0y;
    int U1x, U1y;
    int U2x, U2y;
    int U3x, U3y;
} CubeGeom;

static void draw_cube_in_cell(int cell_x, int cell_y, CubeGeom* g)
{
    int s = FACE_SIZE / 2;

    int fx = cell_x + (MOVE_CELL_W - s) / 2;
    int fy = cell_y + (MOVE_CELL_H - s) / 2;

    int up_dx   = UP_DX   / 2;
    int up_dy   = UP_DY   / 2;
    // ---------- Front ----------
    fillRect(fx, fy, s, s, ORANGE);
    draw_front_grid(fx, fy, s, BLACK);

    int F0x = fx,     F0y = fy;
    int F1x = fx+s,   F1y = fy;
    int F2x = fx+s,   F2y = fy+s;
    int F3x = fx,     F3y = fy+s;

    // ---------- Up ----------
    int U0x = F0x;           int U0y = F0y;
    int U1x = F1x;           int U1y = F1y;
    int U2x = F1x + up_dx;   int U2y = F1y + up_dy;
    int U3x = F0x + up_dx;   int U3y = F0y + up_dy;

    fill_parallelogram(
        U3x, U3y,
        U2x, U2y,
        U1x, U1y,
        U0x, U0y,
        BLUE
    );

    draw_parallelogram_outline(
        U3x, U3y,
        U2x, U2y,
        U1x, U1y,
        U0x, U0y,
        BLACK
    );

    draw_face_grid_parallelogram(
        U3x, U3y,
        U2x, U2y,
        U1x, U1y,
        U0x, U0y,
        BLACK
    );

    // ---------- Left ----------
    int L0x = F0x;           int L0y = F0y;
    int L1x = F3x;           int L1y = F3y;
    int L2x = U3x;           int L2y = U3y + (F3y - F0y);
    int L3x = U3x;           int L3y = U3y;

    fill_parallelogram(
        L3x, L3y,
        L0x, L0y,
        L1x, L1y,
        L2x, L2y,
        YELLOW
    );

    draw_parallelogram_outline(
        L3x, L3y,
        L0x, L0y,
        L1x, L1y,
        L2x, L2y,
        BLACK
    );

    draw_face_grid_parallelogram(
        L3x, L3y,
        L0x, L0y,
        L1x, L1y,
        L2x, L2y,
        BLACK
    );

    g->F0x = F0x; g->F0y = F0y;
    g->F1x = F1x; g->F1y = F1y;

    g->U0x = U0x; g->U0y = U0y;
    g->U1x = U1x; g->U1y = U1y;
    g->U2x = U2x; g->U2y = U2y;
    g->U3x = U3x; g->U3y = U3y;
}

typedef enum {
    ARROW_COL_BACK_TO_FRONT,
    ARROW_COL_FRONT_TO_BACK,

    ARROW_ROW_LEFT_TO_RIGHT,
    ARROW_ROW_RIGHT_TO_LEFT
} ArrowMode;

static void draw_cube_arrow(const CubeGeom* g, ArrowMode mode, float t, float curvature, uint16_t color, int with_mid_triangle)
{
    float x_start, y_start;
    float x_end,   y_end;

    switch (mode) {

    case ARROW_COL_BACK_TO_FRONT: {
        float front_x = (float)g->F0x + (float)(g->F1x - g->F0x) * t;
        float front_y = (float)g->F0y + (float)(g->F1y - g->F0y) * t;
        float back_x  = (float)g->U3x + (float)(g->U2x - g->U3x) * t;
        float back_y  = (float)g->U3y + (float)(g->U2y - g->U3y) * t;

        x_start = back_x;  y_start = back_y;
        x_end   = front_x; y_end   = front_y;
        break;
    }

    case ARROW_COL_FRONT_TO_BACK: {
        float front_x = (float)g->F0x + (float)(g->F1x - g->F0x) * t;
        float front_y = (float)g->F0y + (float)(g->F1y - g->F0y) * t;
        float back_x  = (float)g->U3x + (float)(g->U2x - g->U3x) * t;
        float back_y  = (float)g->U3y + (float)(g->U2y - g->U3y) * t;

        x_start = front_x; y_start = front_y;
        x_end   = back_x;  y_end   = back_y;
        break;
    }

    case ARROW_ROW_LEFT_TO_RIGHT: {
        float left_x  = (float)g->U0x + (float)(g->U3x - g->U0x) * t;
        float left_y  = (float)g->U0y + (float)(g->U3y - g->U0y) * t;
        float right_x = (float)g->U1x + (float)(g->U2x - g->U1x) * t;
        float right_y = (float)g->U1y + (float)(g->U2y - g->U1y) * t;

        x_start = left_x;  y_start = left_y;
        x_end   = right_x; y_end   = right_y;
        break;
    }

    case ARROW_ROW_RIGHT_TO_LEFT: {
        float left_x  = (float)g->U0x + (float)(g->U3x - g->U0x) * t;
        float left_y  = (float)g->U0y + (float)(g->U3y - g->U0y) * t;
        float right_x = (float)g->U1x + (float)(g->U2x - g->U1x) * t;
        float right_y = (float)g->U1y + (float)(g->U2y - g->U1y) * t;

        x_start = right_x; y_start = right_y;
        x_end   = left_x;  y_end   = left_y;
        break;
    }
    }
    int sx0 = (int)(x_start + 0.5f);
    int sy0 = (int)(y_start + 0.5f);
    int sx1 = (int)(x_end   + 0.5f);
    int sy1 = (int)(y_end   + 0.5f);
    draw_curved_arrow_L(
        sx0, sy0,
        sx1, sy1,
        curvature,
        color
    );

    if (with_mid_triangle) {
        draw_mid_triangle_on_curve(
            sx0, sy0,
            sx1, sy1,
            curvature,
            color
        ); 
    };
}

static void draw_rubik_3faces_generic(int cell_x, int cell_y, ArrowMode mode, float t, int with_mid_triangle)
{
    CubeGeom g;
    draw_cube_in_cell(cell_x, cell_y, &g);
    draw_cube_arrow(&g, mode, t, 18.0f, WHITE, with_mid_triangle);
}

void draw_rubik_3faces_L(void){ 
    draw_rubik_3faces_generic(MOVE_COL0_X, MOVE_ROW0_Y,ARROW_COL_BACK_TO_FRONT,1.0f / 6.0f,0); 
}

void draw_rubik_3faces_L1(void){
    draw_rubik_3faces_generic(MOVE_COL1_X, MOVE_ROW0_Y,ARROW_COL_FRONT_TO_BACK,1.0f / 6.0f,0);
}

void draw_rubik_3faces_R1(void){
    draw_rubik_3faces_generic(MOVE_COL1_X, MOVE_ROW1_Y,ARROW_COL_BACK_TO_FRONT,5.0f / 6.0f,0);
}

void draw_rubik_3faces_R(void){
    draw_rubik_3faces_generic(MOVE_COL0_X, MOVE_ROW1_Y,ARROW_COL_FRONT_TO_BACK,5.0f / 6.0f,0);
}

void draw_rubik_3faces_B1(void){
    draw_rubik_3faces_generic(MOVE_COL1_X, MOVE_ROW3_Y,ARROW_ROW_LEFT_TO_RIGHT,5.0f / 6.0f,0);
}

void draw_rubik_3faces_B(void){
    draw_rubik_3faces_generic(MOVE_COL0_X, MOVE_ROW3_Y,ARROW_ROW_RIGHT_TO_LEFT,5.0f / 6.0f,0);
}

void draw_rubik_3faces_F(void){
    draw_rubik_3faces_generic(MOVE_COL0_X, MOVE_ROW2_Y,ARROW_ROW_LEFT_TO_RIGHT,1.0f / 6.0f,0);
}

void draw_rubik_3faces_F1(void){
    draw_rubik_3faces_generic(MOVE_COL1_X, MOVE_ROW2_Y,ARROW_ROW_RIGHT_TO_LEFT,1.0f / 6.0f,0);
}

void draw_rubik_3faces_L2(void){
    draw_rubik_3faces_generic(MOVE_COL2_X, MOVE_ROW0_Y,ARROW_COL_BACK_TO_FRONT,1.0f / 6.0f,1);
}

void draw_rubik_3faces_R2(void){
    draw_rubik_3faces_generic(MOVE_COL2_X, MOVE_ROW1_Y,ARROW_COL_FRONT_TO_BACK,5.0f / 6.0f,1);
}

void draw_rubik_3faces_B2(void){
    draw_rubik_3faces_generic(MOVE_COL2_X, MOVE_ROW3_Y,ARROW_ROW_LEFT_TO_RIGHT,5.0f / 6.0f,1);
}

void draw_rubik_3faces_F2(void){
    draw_rubik_3faces_generic(MOVE_COL2_X, MOVE_ROW2_Y,ARROW_ROW_RIGHT_TO_LEFT,1.0f / 6.0f,1);
}

void draw_move_label(int cell_x, int cell_y, const char *label)
{
    const int CELL_SIZE = 160;
    int text_y = cell_y + MOVE_CELL_H - 12;

    int text_width = strlen(label) * 6;
    int text_x = cell_x + (MOVE_CELL_W - text_width) / 2;

    setCursor(text_x, text_y);
    sprintf(screentext, "%s", label);
    writeString(screentext);
}

static const uint16_t title_palette[6] = { RED, YELLOW, BLUE, GREEN, WHITE, ORANGE };

void draw_letter_shape(char ch, int x, int y, int w, int h, uint16_t color)
{
    int t = w / 4;
    if (t < 6) t = 6;

    switch (ch) {

    case 'C': {
        fillRect(x, y, w, t, color);
        fillRect(x, y + h - t, w, t, color);
        fillRect(x, y, t, h, color);
        break;
    }

    case 'U': {
        fillRect(x, y, t, h - t, color);
        fillRect(x + w - t, y, t, h - t, color);
        fillRect(x, y + h - t, w, t, color);
        break;
    }

    case 'B': {
        int mid_y = y + h / 2 - t / 2;

        fillRect(x, y, t, h, color);

        fillRect(x, y, w - t, t, color);
        fillRect(x, mid_y, w - t, t, color);
        fillRect(x, y + h - t, w - t, t, color);

        fillRect(x + w - t, y + t, t, h - 2 * t, color);
        break;
    }

    case 'E': {
        int mid_y = y + h / 2 - t / 2;

        fillRect(x, y, t, h, color);
        fillRect(x, y, w, t, color);
        fillRect(x, mid_y, w * 3 / 4, t, color);
        fillRect(x, y + h - t, w, t, color);
        break;
    }

    case 'S': {
        int mid_y = y + h / 2 - t / 2;

        fillRect(x, y, w, t, color);
        fillRect(x, mid_y, w, t, color);
        fillRect(x, y + h - t, w, t, color);

        fillRect(x, y + t, t, h / 2 - t, color);
        fillRect(x + w - t, mid_y + t, t, h / 2 - t, color);
        break;
    }

    case 'O': {
        fillRect(x, y, w, t, color);
        fillRect(x, y + h - t, w, t, color);
        fillRect(x, y, t, h, color);
        fillRect(x + w - t, y, t, h, color);
        break;
    }

    case 'L': {
        fillRect(x, y, t, h, color);
        fillRect(x, y + h - t, w, t, color);
        break;
    }

    case 'V': {
        int bx1 = x + w/2 - t/2;
        int by1 = y + h;
        int bx2 = x + w/2;
        int by2 = y + h;

        int x0 = x;       int y0 = y;
        int x1 = x + t;   int y1 = y;
        int x3p = bx1;    int y3p = by1;
        int x2p = bx2;    int y2p = by2;

        fill_parallelogram(
            x0,  y0,
            x1,  y1,
            x2p, y2p,
            x3p, y3p,
            color
        );

        int x0b = x + w - t;
        int y0b = y;
        int x1b = x + w;
        int y1b = y;
        int x3b = bx2;
        int y3b = by2;
        int x2b = x + w/2 + t/2;
        int y2b = y + h;

        fill_parallelogram(
            x0b, y0b,
            x1b, y1b,
            x2b, y2b,
            x3b, y3b,
            color
        );
        break;
    }

    case 'R': {
        int mid_y = y + h / 2 - t / 2;

        fillRect(x, y, t, h, color);
        fillRect(x, y, w - t, t, color);
        fillRect(x, mid_y, w - t, t, color);
        fillRect(x + w - t, y + t, t, h/2 - t, color);

        int sx0 = x + w - t;
        int sy0 = mid_y + t;
        int sx1 = sx0 + t;
        int sy1 = sy0;

        int bx = x + w - t;
        int by = y + h;
        int bx2 = bx + t;
        int by2 = by;

        fill_parallelogram(
            sx0, sy0,
            sx1, sy1,
            bx2, by2,
            bx,  by,
            color
        );
        break;
    }

    default: {
        fillRect(x, y, w, h, color);
        break;
    }
    }
}

void draw_title(void)
{
    const char* text = "CUBE SOLVER";
    int len = strlen(text);

    int cell_w = 40;
    int cell_h = 60;
    int gap   = 8;

    int total_w = len * cell_w + (len - 1) * gap;
    int start_x = (640 - total_w) / 2;
    int top_y   = 30;

    int color_idx = 0;

    for (int i = 0; i < len; i++) {
        char c = text[i];

        int x = start_x + i * (cell_w + gap);

        if (c == ' ') {
            continue;
        }

        uint16_t col = title_palette[color_idx];
        color_idx = (color_idx + 1) % 6;

        draw_letter_shape(c, x, top_y, cell_w, cell_h, col);
    }
}
//========================================================================
// Protothread
//========================================================================

// IMU data acquisition thread (100Hz)
static PT_THREAD (protothread_imu(struct pt *pt))
{
    PT_BEGIN(pt);

    while (1) {
        debug_imu_ticks++;
        update_imu_and_filter();
        check_button_press();
        check_color_buttons();

        PT_SEM_SIGNAL(pt, &  vga_semaphore);
        PT_YIELD_usec(10000); 
    }

    PT_END(pt);
}

// VGA display thread
static PT_THREAD (protothread_vga(struct pt *pt))
{
    PT_BEGIN(pt);
    static int throttle;
    static int last_state = -1;
    setTextSize(1);
    setTextColor(WHITE);

    while (true) {
        PT_SEM_WAIT(pt, &vga_semaphore);
        throttle += 1;
        
        if (throttle >= threshold) {
            throttle = 0;
            if (display_state != last_state) {
              fillRect(0, 0, 640, 480, BLACK);
              if (display_state == 0) {
              case0_static_drawn = 0;
              }

              if (display_state >= 1 && display_state <= 6) {
              side_cubes_drawn = 0;
              }

              if (display_state != last_state || case7_dirty) {
                clearLowFrame(0, BLACK);
              }
            }

            switch (display_state) {

                // State 0 : Start Screen
                case 0: {
                    float ax_abs = fabsf(g_ax);
                    float ay_abs = fabsf(g_ay);
                    float az_abs = fabsf(g_az);
                    char dir_ch = 'U';

                    if (ax_abs >= ay_abs && ax_abs >= az_abs) {
                        dir_ch = (g_ax > 0.0f) ? 'B' : 'F';
                    }
                    else if (ay_abs >= ax_abs && ay_abs >= az_abs) {
                        dir_ch = (g_ay > 0.0f) ? 'L' : 'R';
                    }
                    else {
                        dir_ch = (g_az > 0.0f) ? 'D' : 'U';
                    }

                    g_cube_center_x = 320;
                    g_cube_center_y = 240;
                    clearRect(100, 100, 540, 400, BLACK);
                    draw_cube((const float(*)[3])g_rotation_matrix);
                    if (!case0_static_drawn) {
                      draw_title();
                      draw_case0_static_cubes();
                      int x_end = 600;
                      int char_w = 6;
                      const char* line1 = "Made by:";
                      int x1 = x_end - strlen(line1) * char_w;
                      setCursor(x1, 400);
                      sprintf(screentext, "%s", line1);
                      writeString(screentext);
                      const char* line2 = "Jeongyoon Rhee (jr2487)";
                      int x2 = x_end - strlen(line2) * char_w;
                      setCursor(x2, 415);
                      sprintf(screentext, "%s", line2);
                      writeString(screentext);
                      const char* line3 = "Tianyi Liu (tl955)";
                      int x3 = x_end - strlen(line3) * char_w;
                      setCursor(x3, 430);
                      sprintf(screentext, "%s", line3);
                      writeString(screentext);
                      setCursor(40, 415);
                      sprintf(screentext, "Cornell University");
                      writeString(screentext);
                      setCursor(40, 430);
                      sprintf(screentext, "ECE5730");
                      writeString(screentext);
                      case0_static_drawn = 1;
                    }
                    break;
                }

                // State 1: Input the UP face
                case 1: {
                    setTextSize(3);
                    setTextColor(YELLOW);
                    setCursor(304, 140);
                    sprintf(screentext, "UP");
                    writeString(screentext);
                    setTextSize(2);
                    setTextColor(WHITE);
                    setCursor(90, 80);
                    sprintf(screentext, "Please fill the grids by using buttons!");
                    writeString(screentext);
                    setTextSize(1);
                    setTextColor(LIGHT_PINK);
                    setCursor(80, 400);
                    sprintf(screentext, "o BROWN -> NEXT");
                    writeString(screentext);
                    setCursor(80, 410);
                    sprintf(screentext, "o BLACK -> DELETE");
                    writeString(screentext);
                    setTextColor(WHITE);
                    setTextSize(1);

                    if (!side_cubes_drawn) {
                        draw_case0_static_cubes();
                        side_cubes_drawn = 1;
                    }

                    int state_idx = 0;
                    int x[9] = {260, 300, 340, 260, 300, 340, 260, 300, 340};
                    int y[9] = {180, 180, 180, 220, 220, 220, 260, 260, 260};
                    int square_size = 40;
                    int border = 3;
                    int fill_size = square_size - 2 * border;
                    for (int i = 0; i < 9; i++) {
                        uint16_t color = (i < fill_index[state_idx]) ? color_grid[state_idx][i] : BLACK;
                        drawRect(x[i], y[i], square_size, square_size, WHITE);
                        fillRect(x[i] + border, y[i] + border, fill_size, fill_size, color);
                    }
                    draw_all_faces_preview(1, 0);  // current_case=1, skip face 0(UP)
                    drawRect(151, 320, 48, 48, YELLOW);
                    break;
                }
                // State 2: Input the FRONT face
                case 2: {
                    setTextSize(3);
                    setTextColor(YELLOW);
                    setCursor(276, 140);
                    sprintf(screentext, "FRONT");
                    writeString(screentext);
                    setTextSize(2);
                    setTextColor(WHITE);
                    setCursor(90, 80);
                    sprintf(screentext, "Please fill the grids by using buttons!");
                    writeString(screentext);
                    setTextSize(1);
                    setTextColor(LIGHT_PINK);
                    setCursor(80, 400);
                    sprintf(screentext, "o BROWN -> NEXT");
                    writeString(screentext);
                    setCursor(80, 410);
                    sprintf(screentext, "o BLACK -> DELETE");
                    writeString(screentext);
                    setTextColor(WHITE);
                    setTextSize(1);

                    if (!side_cubes_drawn) {
                        draw_case0_static_cubes();
                        side_cubes_drawn = 1;
                    }
                    
                    int state_idx = 1;
                    int x[9] = {260, 300, 340, 260, 300, 340, 260, 300, 340};
                    int y[9] = {180, 180, 180, 220, 220, 220, 260, 260, 260};
                    int square_size = 40;
                    int border = 3;  // Border width
                    int fill_size = square_size - 2 * border;  // Fill area size
                    for (int i = 0; i < 9; i++) {
                        uint16_t color = (i < fill_index[state_idx]) ? color_grid[state_idx][i] : BLACK;
                        // Draw border first
                        drawRect(x[i], y[i], square_size, square_size, WHITE);
                        // Draw filled area (slightly smaller)
                        fillRect(x[i] + border, y[i] + border, fill_size, fill_size, color);
                    }
                    // Draw all faces preview at bottom (skip FRONT, current editing face)
                    draw_all_faces_preview(2, 1);  // current_case=2, skip face 1 (FRONT)
                    drawRect(209, 320, 48, 48, YELLOW);
                    break;
                }
                // State 3: Input the BACK face
                case 3: {
                    setTextSize(3);
                    setTextColor(YELLOW);
                    setCursor(285, 140);
                    sprintf(screentext, "BACK");
                    writeString(screentext);
                    setTextSize(2);
                    setTextColor(WHITE);
                    setCursor(90, 80);
                    sprintf(screentext, "Please fill the grids by using buttons!");
                    writeString(screentext);
                    setTextSize(1);
                    setTextColor(LIGHT_PINK);
                    setCursor(80, 400);
                    sprintf(screentext, "o BROWN -> NEXT");
                    writeString(screentext);
                    setCursor(80, 410);
                    sprintf(screentext, "o BLACK -> DELETE");
                    writeString(screentext);
                    setTextColor(WHITE);
                    setTextSize(1);

                    if (!side_cubes_drawn) {
                        draw_case0_static_cubes();
                        side_cubes_drawn = 1;
                    }

                    int state_idx = 2;
                    int x[9] = {260, 300, 340, 260, 300, 340, 260, 300, 340};
                    int y[9] = {180, 180, 180, 220, 220, 220, 260, 260, 260};
                    int square_size = 40;
                    int border = 3;  // Border width
                    int fill_size = square_size - 2 * border;  // Fill area size
                    for (int i = 0; i < 9; i++) {
                        uint16_t color = (i < fill_index[state_idx]) ? color_grid[state_idx][i] : BLACK;
                        // Draw border first
                        drawRect(x[i], y[i], square_size, square_size, WHITE);
                        // Draw filled area (slightly smaller)
                        fillRect(x[i] + border, y[i] + border, fill_size, fill_size, color);
                    }
                    // Draw all faces preview at bottom (skip BACK, current editing face)
                    draw_all_faces_preview(3, 2);  // current_case=3, skip face 2 (BACK)
                    drawRect(267, 320, 48, 48, YELLOW);
                    break;
                }
                // State 4: Input the LEFT face
                case 4: {
                    setTextSize(3);
                    setTextColor(YELLOW);
                    setCursor(285, 140);
                    sprintf(screentext, "LEFT");
                    writeString(screentext);
                    setTextSize(2);
                    setTextColor(WHITE);
                    setCursor(90, 80);
                    sprintf(screentext, "Please fill the grids by using buttons!");
                    writeString(screentext);
                    setTextSize(1);
                    setTextColor(LIGHT_PINK);
                    setCursor(80, 400);
                    sprintf(screentext, "o BROWN -> NEXT");
                    writeString(screentext);
                    setCursor(80, 410);
                    sprintf(screentext, "o BLACK -> DELETE");
                    writeString(screentext);
                    setTextColor(WHITE);
                    setTextSize(1);
                    if (!side_cubes_drawn) {
                        draw_case0_static_cubes();
                        side_cubes_drawn = 1;
                    }

                    int state_idx = 3;
                    int x[9] = {260, 300, 340, 260, 300, 340, 260, 300, 340};
                    int y[9] = {180, 180, 180, 220, 220, 220, 260, 260, 260};
                    int square_size = 40;
                    int border = 3;  // Border width
                    int fill_size = square_size - 2 * border;  // Fill area size
                    for (int i = 0; i < 9; i++) {
                        uint16_t color = (i < fill_index[state_idx]) ? color_grid[state_idx][i] : BLACK;
                        // Draw border first
                        drawRect(x[i], y[i], square_size, square_size, WHITE);
                        // Draw filled area (slightly smaller)
                        fillRect(x[i] + border, y[i] + border, fill_size, fill_size, color);
                    }
                    // Draw all faces preview at bottom (skip LEFT, current editing face)
                    draw_all_faces_preview(4, 3);  // current_case=4, skip face 3 (LEFT)
                    drawRect(325, 320, 48, 48, YELLOW);
                    break;
                }
                // State 5: Input the RIGHT face
                case 5: {
                    setTextSize(3);
                    setTextColor(YELLOW);
                    setCursor(276, 140);
                    sprintf(screentext, "RIGHT");
                    writeString(screentext);
                    setTextSize(2);
                    setTextColor(WHITE);
                    setCursor(90, 80);
                    sprintf(screentext, "Please fill the grids by using buttons!");
                    writeString(screentext);
                    setTextSize(1);
                    setTextColor(LIGHT_PINK);
                    setCursor(80, 400);
                    sprintf(screentext, "o BROWN -> NEXT");
                    writeString(screentext);
                    setCursor(80, 410);
                    sprintf(screentext, "o BLACK -> DELETE");
                    writeString(screentext);
                    setTextColor(WHITE);
                    setTextSize(1);

                    if (!side_cubes_drawn) {
                        draw_case0_static_cubes();
                        side_cubes_drawn = 1;
                    }

                    int state_idx = 4;
                    int x[9] = {260, 300, 340, 260, 300, 340, 260, 300, 340};
                    int y[9] = {180, 180, 180, 220, 220, 220, 260, 260, 260};
                    int square_size = 40;
                    int border = 3;  // Border width
                    int fill_size = square_size - 2 * border;  // Fill area size
                    for (int i = 0; i < 9; i++) {
                        uint16_t color = (i < fill_index[state_idx]) ? color_grid[state_idx][i] : BLACK;
                        // Draw border first
                        drawRect(x[i], y[i], square_size, square_size, WHITE);
                        // Draw filled area (slightly smaller)
                        fillRect(x[i] + border, y[i] + border, fill_size, fill_size, color);
                    }
                    // Draw all faces preview at bottom (skip RIGHT, current editing face)
                    draw_all_faces_preview(5, 4);  // current_case=5, skip face 4 (RIGHT)
                    drawRect(383, 320, 48, 48, YELLOW);
                    break;
                }
                // State 6: Input the DOWN face
                case 6: {
                    setTextSize(3);
                    setTextColor(YELLOW);
                    setCursor(285, 140);
                    sprintf(screentext, "DOWN");
                    writeString(screentext);
                    setTextSize(2);
                    setTextColor(WHITE);
                    setCursor(90, 80);
                    sprintf(screentext, "Please fill the grids by using buttons!");
                    writeString(screentext);
                    setTextSize(1);
                    setTextColor(LIGHT_PINK);
                    setCursor(80, 400);
                    sprintf(screentext, "o BROWN -> NEXT");
                    writeString(screentext);
                    setCursor(80, 410);
                    sprintf(screentext, "o BLACK -> DELETE");
                    writeString(screentext);
                    setTextColor(WHITE);
                    setTextSize(1);

                    if (!side_cubes_drawn) {
                        draw_case0_static_cubes();
                        side_cubes_drawn = 1;
                    }

                    int state_idx = 5;
                    int x[9] = {260, 300, 340, 260, 300, 340, 260, 300, 340};
                    int y[9] = {180, 180, 180, 220, 220, 220, 260, 260, 260};
                    int square_size = 40;
                    int border = 3;
                    int fill_size = square_size - 2 * border;
                    for (int i = 0; i < 9; i++) {
                        uint16_t color = (i < fill_index[state_idx]) ? color_grid[state_idx][i] : BLACK;
                        drawRect(x[i], y[i], square_size, square_size, WHITE);
                        fillRect(x[i] + border, y[i] + border, fill_size, fill_size, color);
                    }

                    draw_all_faces_preview(0, 0);
                    drawRect(441, 320, 48, 48, YELLOW);
                    break;
                }
                // State 7: Show the solution of the cube
                case 7: {
                    setTextSize(2);
                    setCursor(250, 20);
                    sprintf(screentext, "RLFB SOLVER");
                    writeString(screentext);

                    setTextSize(1);
                    setCursor(400, 60);
                    sprintf(screentext, "Press button to apply next move");
                    writeString(screentext);

                    if (!g_solution_built) {
                        // NO solution
                        setTextSize(1);
                        setCursor(400, 80);
                        sprintf(screentext, "No solution. Check the inputs agian.");
                        writeString(screentext);
                    }
                    else if (g_num_moves == 0) {
                        // Already solved
                        setTextSize(1);
                        setCursor(400, 80);
                        sprintf(screentext, "Already solved (0 moves).");
                        writeString(screentext);
                        setTextSize(2);
                        setCursor(450, 400);
                        sprintf(screentext, "DONE!");
                        writeString(screentext);
                    }
                    else if (g_curr_move_index >= 0 && g_curr_move_index < g_num_moves) {
                        clearRect(320, 240, 640, 480, BLACK);
                        setTextSize(2);
                        setCursor(400, 400);
                        sprintf(screentext, "Next: %s  (%d/%d)",
                                move_to_string(g_moves[g_curr_move_index]),
                                g_curr_move_index + 1, g_num_moves);
                        writeString(screentext);
                    }
                    else {
                        // After solving
                        clearRect(320, 240, 640, 480, BLACK);
                        setTextSize(2);
                        setCursor(450, 400);
                        sprintf(screentext, "DONE!");
                        writeString(screentext);
                    }

                    g_cube_center_x = 480;
                    g_cube_center_y = 240;
                    clearRect(320, 100, 640, 400, BLACK);
                    draw_cube((const float(*)[3])g_rotation_matrix);
                    if (display_state != last_state || case7_dirty){
                    draw_rubik_3faces_L();
                    draw_move_label(MOVE_COL0_X - 4, MOVE_ROW0_Y, "L");
                    draw_rubik_3faces_L1();
                    draw_move_label(MOVE_COL1_X - 2, MOVE_ROW0_Y, "L'");
                    draw_rubik_3faces_L2();
                    draw_move_label(MOVE_COL2_X - 8, MOVE_ROW0_Y, "L2");
                    draw_rubik_3faces_R();
                    draw_move_label(MOVE_COL0_X - 4, MOVE_ROW1_Y, "R");
                    draw_rubik_3faces_R1();
                    draw_move_label(MOVE_COL1_X - 2, MOVE_ROW1_Y, "R'");
                    draw_rubik_3faces_R2();
                    draw_move_label(MOVE_COL2_X - 8, MOVE_ROW1_Y, "R2");
                    draw_rubik_3faces_F();
                    draw_move_label(MOVE_COL0_X - 4, MOVE_ROW2_Y, "F");
                    draw_rubik_3faces_F1();
                    draw_move_label(MOVE_COL1_X - 2, MOVE_ROW2_Y, "F'");
                    draw_rubik_3faces_F2();
                    draw_move_label(MOVE_COL2_X - 8, MOVE_ROW2_Y, "F2");
                    draw_rubik_3faces_B();
                    draw_move_label(MOVE_COL0_X - 4, MOVE_ROW3_Y, "B");
                    draw_rubik_3faces_B1();
                    draw_move_label(MOVE_COL1_X - 2, MOVE_ROW3_Y, "B'");
                    draw_rubik_3faces_B2();
                    draw_move_label(MOVE_COL2_X - 8, MOVE_ROW3_Y, "B2");
                    case7_dirty = 0;
                    }
                    break;
                }

                default:
                    display_state = 0;
                    break;
            }
            last_state = display_state;
        }
    }
    PT_END(pt);
}

// Core 1 entry: VGA display thread
void core1_entry() {
    pt_add_thread(protothread_vga);
    pt_schedule_start;
}
//========================================================================
// Main()
//========================================================================

// Main function (Core 0)
int main() {
    set_sys_clock_khz(150000, true);
    stdio_init_all();
    initVGA();

    init_cube_solved();
    init_button();
    init_color_buttons();

    // I2C configuration
    i2c_init(i2c0, I2C_BAUD_RATE);
    gpio_set_function(8, GPIO_FUNC_I2C);
    gpio_set_function(9, GPIO_FUNC_I2C);
    i2c_init(i2c1, I2C_BAUD_RATE);
    gpio_set_function(6, GPIO_FUNC_I2C);
    gpio_set_function(7, GPIO_FUNC_I2C);
    gpio_pull_up(8);
    gpio_pull_up(9);
    gpio_pull_up(6);
    gpio_pull_up(7);

    // Sensor initialization
    mpu6050_reset();
    sleep_ms(100);
    mpu6050_read_raw(acceleration, gyro);
    gy271_reset();
    sleep_ms(100);
    gy271_read_raw(magnetometer);

    // Start multicore processing
    multicore_reset_core1();
    multicore_launch_core1(core1_entry);
    pt_add_thread(protothread_imu);
    pt_schedule_start;

    return 0;
}
