#include "OV2640.hpp"

void OV2640::Init(){
    Sensor_write_register((uint8_t) 0xff, 0x01);
    Sensor_write_register((uint8_t) 0x12, 0x80);

    Sensor_write_register_bulk(OV2640_JPEG_INIT);
    Sensor_write_register_bulk(OV2640_YUV422);
    Sensor_write_register_bulk(OV2640_JPEG);

    Sensor_write_register((uint8_t) 0xff, 0x01);
    Sensor_write_register((uint8_t) 0x15, 0x00);
}

void OV2640::Init(const vector<Register_blob_8> &regs){
    Init();

    Sensor_write_register_bulk(regs);

    ArduChip_write(0x03, 0b01010000);
}

void OV2640::Capture(){
    ArduChip_write(0x04, 0x01); // Clear FIFO
    ArduChip_write(0x04, 0x01);

    HAL_Delay(1);

    ArduChip_write(0x04, 0x02); // Start capture

    HAL_Delay(1);

    // wait for capture done
    while (1) {
        uint8_t regValue        = ArduChip_read(0x41);
        uint8_t captureDoneMask = 0x8;
        if (regValue & captureDoneMask) break;
    }

    HAL_Delay(1);

    uint32_t image_size = ArduChip_FIFO_length();

    image_data.resize(image_size);

    ArduChip_start_DMA_transfer(image_size);

    // while(SPI_handle.State != HAL_SPI_STATE_READY){;}
    HAL_Delay(300); // delay to ensure full dma transmission

    // HAL_GPIO_WritePin(GPIOB, GPIO_PIN_0, GPIO_PIN_SET);
    ArduChip_CS_disable();

    HAL_UART_Transmit(&UART_handle, image_data.data(), image_size, HAL_MAX_DELAY);
} // OV2640::Capture