#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);

    // Setup camera, H-sync: High, V-sync:high, Sensor_delay: no Delay, FIFO_mode:FIFO enabled, power_mode:Low_power
    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(500); // delay to ensure full dma transmission

    ArduChip_CS_disable();

} // OV2640::Capture