#include "k230_display.h"
#include "k230_sensor.h"

#include "freetype_wrap.h"

#include <Arduino.h>
#include <string.h> // For memset and memcpy

Sensor* sensor = nullptr;

// Global buffer to hold our source image data
VbBuffer g_src_buffer;

// Global frame info struct that points to our source buffer
k_video_frame_info g_src_frame;

k_s32 g_src_pool_id = VB_INVALID_POOLID;

// Define the properties of our source image
const int   IMG_WIDTH  = 96 * 2;
const int   IMG_HEIGHT = 96;
const int   IMG_FORMAT = PIXEL_FORMAT_RGB_888; // 3 bytes per pixel
const int   IMG_BPP    = 3;
const char* FONT_PATH  = "/data/test.ttf";

// --- Draw Context and Callback for FreeType ---

/**
 * @brief Context passed to the FreeType draw callback.
 * Contains necessary info about the destination buffer.
 */
struct DrawContext {
    uint8_t* virt_addr;
    int      width;
    int      height;
    int      bpp;
};

/**
 * @brief FreeType callback implementation to draw a grayscale bitmap onto the RGB_888 buffer.
 * This function performs simple alpha blending based on the coverage value (alpha).
 */
void draw_bitmap_to_buffer(void* ctx, int color, int x_off, int y_off, FT_Bitmap* bitmap)
{
    DrawContext* dc = (DrawContext*)ctx;

    // Convert 0x00RRGGBB color to components
    uint8_t r = (color >> 16) & 0xFF;
    uint8_t g = (color >> 8) & 0xFF;
    uint8_t b = color & 0xFF;

    for (int j = 0; j < bitmap->rows; j++) { // j is row in bitmap
        for (int i = 0; i < bitmap->width; i++) { // i is column in bitmap

            // Get alpha/coverage value from FreeType bitmap (assuming 8-bit grayscale format)
            uint8_t alpha = bitmap->buffer[j * bitmap->pitch + i];
            if (alpha == 0)
                continue; // Skip fully transparent pixels

            int dst_x = x_off + i;
            int dst_y = y_off + j;

            // Check bounds of the destination buffer
            if (dst_x >= 0 && dst_x < dc->width && dst_y >= 0 && dst_y < dc->height) {
                // Calculate pixel index in the destination buffer (RGB_888, 3 BPP)
                uint8_t* dst_pixel = dc->virt_addr + (dst_y * dc->width + dst_x) * dc->bpp;

                // Simple alpha blending: dst = fg * alpha + bg * (1 - alpha)
                float coverage         = (float)alpha / 255.0f;
                float inverse_coverage = 1.0f - coverage;

                // Destination is RGB_888
                uint8_t bg_r = dst_pixel[0];
                uint8_t bg_g = dst_pixel[1];
                uint8_t bg_b = dst_pixel[2];

                dst_pixel[0] = (uint8_t)(r * coverage + bg_r * inverse_coverage);
                dst_pixel[1] = (uint8_t)(g * coverage + bg_g * inverse_coverage);
                dst_pixel[2] = (uint8_t)(b * coverage + bg_b * inverse_coverage);
            }
        }
    }
}

/**
 * @brief Helper function to fill our source buffer with a solid color.
 * * @param r Red component (0-255)
 * @param g Green component (0-255)
 * @param b Blue component (0-255)
 */
void fill_buffer_color(uint8_t r, uint8_t g, uint8_t b)
{
    if (!g_src_buffer.virt_addr) {
        Serial.println("Source buffer is not allocated!");
        return;
    }

    uint8_t* pixels     = (uint8_t*)g_src_buffer.virt_addr;
    int      num_pixels = IMG_WIDTH * IMG_HEIGHT;

    for (int i = 0; i < num_pixels; ++i) {
        pixels[i * IMG_BPP + 0] = r;
        pixels[i * IMG_BPP + 1] = g;
        pixels[i * IMG_BPP + 2] = b;
    }
}

/**
 * @brief Generates a random top-left position (x, y) for a frame, ensuring it remains on screen after rotation.
 * * @param display The Display instance, providing the screen dimensions (m_width, m_height).
 * @param rotation_flag The rotation/mirror flag (e.g., Display::FLAG_ROTATION_90).
 * @return std::tuple<int, int> The random (x, y) position.
 */
std::tuple<int, int> get_random_pos(Display& display, int rotation_flag)
{
    // The dimensions of the image *after* rotation (width/height swapped for 90/270)
    int rotated_w = IMG_WIDTH;
    int rotated_h = IMG_HEIGHT;

    int rotate_op = rotation_flag & 0x0F;

    if (rotate_op == Display::FLAG_ROTATION_90 || rotate_op == Display::FLAG_ROTATION_270) {
        // Swap dimensions for 90 or 270 degree rotation
        std::swap(rotated_w, rotated_h);
    }

    // Determine the maximum possible starting position (top-left corner)
    // The starting X must be less than or equal to (Screen Width - Rotated Image Width)
    int max_x = display.width() - rotated_w;
    // The starting Y must be less than or equal to (Screen Height - Rotated Image Height)
    int max_y = display.height() - rotated_h;

    // Ensure we don't try to draw outside the screen if the image is too large
    if (max_x < 0)
        max_x = 0;
    if (max_y < 0)
        max_y = 0;

    // Generate random coordinates within the safe range [0, max_x] and [0, max_y]
    int rand_x = 0;
    if (max_x > 0) {
        rand_x = rand() % (max_x + 1);
    }

    int rand_y = 0;
    if (max_y > 0) {
        rand_y = rand() % (max_y + 1);
    }

    return { rand_x, rand_y };
}

// Assuming these are defined globally or included in your main source file
// Display::FLAG_ROTATION_0, Display::FLAG_ROTATION_90, etc.
// freetype_wrap_draw_string, FREETYPE_WRAP_ERR_NONE, FONT_PATH, draw_bitmap_to_buffer

/**
 * @brief Runs a single display test case, showing a colored block with text.
 * * @param display The Display instance.
 * @param text_draw_ctx The drawing context for Freetype.
 * @param color_r Red component for background.
 * @param color_g Green component for background.
 * @param color_b Blue component for background.
 * @param rotation_flag The rotation flag (e.g., Display::FLAG_ROTATION_90).
 * @param text The string to draw onto the buffer.
 */
void run_display_test(Display& display, DrawContext& text_draw_ctx, uint8_t color_r, uint8_t color_g, uint8_t color_b,
                      int rotation_flag, const char* text)
{
    int                  ret;
    std::tuple<int, int> pos = get_random_pos(display, rotation_flag);

    Serial.printf("\n--- Test: %s (Flag: 0x%X) ---\n", text, rotation_flag);
    Serial.printf("Showing block (R:%d, G:%d, B:%d) at random pos(%d,%d)\n", color_r, color_g, color_b, std::get<0>(pos),
                  std::get<1>(pos));

    // 1. Fill the source buffer with a background color
    fill_buffer_color(color_r, color_g, color_b);

    // 2. Draw the string onto the buffer
    if (FREETYPE_WRAP_ERR_NONE
        != (ret = freetype_wrap_draw_string(0, // Start X offset
                                            0, // Start Y offset (center-ish)
                                            30, // Character size (20pt)
                                            text, // Text string
                                            0x00FFFFFF, // Color (White)
                                            FONT_PATH, // Font file path
                                            draw_bitmap_to_buffer, // The callback function
                                            &text_draw_ctx))) { // The drawing context
        Serial.printf("Freetype draw failed: %d\n", ret);
    }

    // 3. Show the buffer with the specified rotation flag
    display.show(g_src_frame, pos, Display::LAYER_OSD0, 255, rotation_flag);

    delay(2000);
}

/**
 * @brief Arduino setup() function. Runs once on startup.
 */
void setup_display()
{
    // 2. Get the singleton instance of the Display
    Display& display = Display::instance();

    // 3. Initialize the display
    Serial.println("Initializing display...");
    // Assuming Display::ST7701 and flag 1 are correct parameters
    int ret = display.init(ST7701_V1_MIPI_2LAN_480X800_30FPS, 1);

    if (ret != 0) {
        Serial.println("Display init failed. Halting.");
        while (1)
            delay(1000);
    }
    Serial.printf("Display initialized: %dx%d\n", display.width(), display.height());

    // 4. Allocate and prepare our source video frame
    k_u32 buffer_size = IMG_WIDTH * IMG_HEIGHT * IMG_BPP;

    // Assuming VB_REMAP_MODE_NOCACHE is suitable for CPU writing
    g_src_pool_id = kd_mpi_vb_create_pool_ex(buffer_size, 1, VB_REMAP_MODE_NOCACHE);
    if ((VB_INVALID_POOLID == g_src_pool_id) || (!g_src_buffer.get(buffer_size, g_src_pool_id) || (!g_src_buffer.mmap()))) {
        Serial.println("Failed to allocate source buffer. Halting.");
        while (1)
            delay(1000);
    }
    Serial.printf("Source buffer allocated: %d bytes\n", buffer_size);

    // Fill the buffer with an initial color (e.g., Red)
    fill_buffer_color(255, 0, 0);

    // 5. Populate the k_video_frame_info struct
    memset(&g_src_frame, 0, sizeof(g_src_frame));

    g_src_frame.pool_id              = g_src_buffer.pool_id;
    g_src_frame.v_frame.width        = IMG_WIDTH;
    g_src_frame.v_frame.height       = IMG_HEIGHT;
    g_src_frame.v_frame.pixel_format = (k_pixel_format)IMG_FORMAT;
    g_src_frame.v_frame.stride[0]    = IMG_WIDTH * IMG_BPP; // Stride is bytes per row
    g_src_frame.v_frame.phys_addr[0] = g_src_buffer.phys_addr;
    g_src_frame.v_frame.virt_addr[0] = (k_u64)g_src_buffer.virt_addr;

    Serial.println("Setup complete. Entering loop...");
}

void setup_sensor(void)
{
    sensor = new Sensor(2);
    if (!sensor) {
        Serial.println("Too many sensors");
        while (1)
            delay(1000);
    }

    if (0x00 != sensor->reset()) {
        Serial.println("Failed to probe sensor");
        while (1)
            delay(1000);
    }

    if (0x00 != sensor->set_framesize(800, 480)) {
        Serial.println("Failed to set sensor framesize");
        while (1)
            delay(1000);
    }

    if (0x00 != sensor->set_pixformat(SensorPixelFormat::YUV420SP)) {
        Serial.println("Failed to set sensor pixelformat");
        while (1)
            delay(1000);
    }

    if (0x00 != sensor->run()) {
        Serial.println("Failed run sensor");
        while (1)
            delay(1000);
    }
}

void bind_sensor_to_display(void)
{
    // 2. Get the singleton instance of the Display
    Display& display = Display::instance();

    int sensor_mod = K_ID_VI;
    int sensor_dev = sensor->_dev_id();
    int sensor_chn = VICAP_CHN_ID_0;

    std::tuple<int, int> sensor_frame_size       = sensor->framesize();
    k_pixel_format       sensor_frame_pix_format = Sensor::to_k_pixel_format(sensor->pixformat());

    display.bind_layer({ sensor_mod, sensor_dev, sensor_chn }, Display::LAYER_VIDEO1,
                       { 0, 0, std::get<0>(sensor_frame_size), std::get<1>(sensor_frame_size) }, sensor_frame_pix_format,
                       Display::FLAG_ROTATION_90, 0);
}

void setup(void)
{
    Serial.begin(115200);
    while (!Serial)
        ; // Wait for serial console
    Serial.println("K230 Display Example Start");

    setup_display();

    setup_sensor();

    // Bind sensor to display video layer
    bind_sensor_to_display();
}

/**
 * @brief Arduino loop() function. Runs repeatedly.
 */
void loop(void)
{
    Display& display = Display::instance();

    // 1. Prepare the drawing context (remains outside the loop)
    DrawContext text_draw_ctx
        = { .virt_addr = (uint8_t*)g_src_buffer.virt_addr, .width = IMG_WIDTH, .height = IMG_HEIGHT, .bpp = IMG_BPP };

    // --- Test 1: Solid Color Block (No Rotation) ---
    run_display_test(display, text_draw_ctx, 255, 0, 0, // Red background
                     Display::FLAG_ROTATION_0, "Rotate 0");

    // --- Test 2: Text Drawing Test (Rotation 90) ---
    run_display_test(display, text_draw_ctx, 0, 255, 0, // GREEN background
                     Display::FLAG_ROTATION_90, "Rotate 90");

    // --- Test 3: Rotated Solid Color Block (Rotation 180) ---
    run_display_test(display, text_draw_ctx, 0, 0, 255, // Blue background
                     Display::FLAG_ROTATION_180, "Rotate 180");

    // --- Test 4: Rotated Solid Color Block (Rotation 270) ---
    run_display_test(display, text_draw_ctx, 0, 0, 0, // Black background
                     Display::FLAG_ROTATION_270, "Rotate 270");
}
