#include <iostream>
#include <fstream>
#include "k230_display.h"
#include "k230_sensor.h"
#include "mpi_sys_api.h"

// Global sensor pointer
Sensor *sensor = nullptr;

// Default display resolution
size_t display_width = 800;
size_t display_height = 480;

// Setup function – executed once at program startup
void setup() {
    // Initialize the camera sensor with ID 2
    sensor = new Sensor(2);
    if (!sensor) {
        std::cout << "Too many sensors" << std::endl;
        return;
    }

    // Reset and probe the sensor hardware
    if (sensor->reset() != 0x00) {
        std::cout << "Failed to probe sensor" << std::endl;
        return;
    }

    // Configure the sensor frame size (width x height)
    if (sensor->set_framesize(display_width, display_height, false, VICAP_CHN_ID_0) != 0x00) {
        std::cout << "Failed to set sensor frame size" << std::endl;
        return;
    }

    // Set the pixel format to YUV420SP for channel 0
    if (sensor->set_pixformat(SensorPixelFormat::YUV420SP, VICAP_CHN_ID_0) != 0x00) {
        std::cout << "Failed to set sensor pixel format" << std::endl;
        return;
    }

    // Start the sensor streaming
    if (sensor->run() != 0x00) {
        std::cout << "Failed to start sensor" << std::endl;
        return;
    }

    // Initialize the display
    Display& display = Display::instance();

    // Initialize the ST7701 display panel (MIPI 2-Lane, 480x800 @ 30FPS)
    // The second parameter (1) may indicate rotation, layer enable, or other configuration flag
    int ret = display.init(ST7701_V1_MIPI_2LAN_480X800_30FPS, 1);
    if (ret != 0) {
        printf("Display init failed. Halting.\n");
        while (1)
            delay(1000);  // Infinite wait on failure
    }

    // Print display information
    printf("Display initialized: %dx%d\n", display.width(), display.height());

    // Get sensor module, device, and channel identifiers
    int sensor_mod = K_ID_VI;
    int sensor_dev = sensor->_dev_id();
    int sensor_chn = VICAP_CHN_ID_0;

    // Retrieve sensor frame size and pixel format
    std::tuple<int, int> sensor_frame_size = sensor->framesize(VICAP_CHN_ID_0);
    k_pixel_format sensor_frame_pix_format = Sensor::to_k_pixel_format(sensor->pixformat(VICAP_CHN_ID_0));

    // Bind the sensor output to the display layer
    // This maps the video stream to Display::LAYER_VIDEO1 with a 90-degree rotation
    display.bind_layer(
        { sensor_mod, sensor_dev, sensor_chn },           // Source: sensor module, device, channel
        Display::LAYER_VIDEO1,                            // Target layer
        { 0, 0, std::get<0>(sensor_frame_size), std::get<1>(sensor_frame_size) }, // Frame region
        sensor_frame_pix_format,                          // Pixel format
        Display::FLAG_ROTATION_90,                        // Apply 90° rotation
        0                                                 // Additional flags (none)
    );
}

// Main loop – runs continuously after setup()
void loop() {
    // Currently empty – can be used for frame processing or UI updates
}
