#include <stdio.h>
#include "pico/stdlib.h"
#include "hardware/spi.h"
#include "hardware/i2c.h"
#include "hardware/dma.h"
#include "hardware/pio.h"
#include "hardware/interp.h"
#include "hardware/timer.h"
#include "hardware/clocks.h"
#include "hardware/uart.h"
#include "../../hardware_dma/include/hardware/dma.h"

#include <ov2640/SCCB.hpp>
#include <ov2640/camera.hpp>
#include "cstring"

// PIO2 is reserved for the cam data retreive functions

camera_config_t left_eye_config = {
    .pin_pwdn = -1,                   /*!< GPIO pin for camera power down line */ //? Also called PWDN, or set to -1 and tie to GND
    .pin_reset = -1,                  /*!< GPIO pin for camera reset line */ //? Cam reset, or set to -1 and tie to 3.3V
    .pin_xclk = 3,                   /*!< GPIO pin for camera XCLK line */ //? in theory could be shared or perhaps ommited?
    .pin_sccb_sda = 4,               /*!< GPIO pin for camera SDA line */
    .pin_sccb_scl = 5,               /*!< GPIO pin for camera SCL line */
    .pin_data_base = 6,              /*!< this pin + 7 consecutive will be used D0-D7 PCLK HREF */
    .pin_vsync = 16,                  /*!< GPIO pin for camera VSYNC line */
    .xclk_freq_hz = 0,               /*!< Frequency of XCLK signal, in Hz. */ //! Figure out the highest it can go to
    .sccb_ctrl = 0,              /* Select i2c controller ctrl: 0 - i2c0, 1 - i2c1, 2 - pio0, 3 - pio1, 4 - pio2 */
};

int main()
{
    stdio_init_all();

    sleep_ms(4000);
    //printf("Hello World!");
    
    OV2640 left_cam = OV2640(left_eye_config);
    left_cam.begin(DMA_IRQ_0);
    //left_cam.capture_frame();
    //fwrite(left_cam.fb, 1, sizeof(left_cam.fb), stdout);
    while (true) {
    //     //printf("Hello, world!\n");
    //     //sleep_ms(1000);

        left_cam.capture_frame();
        fwrite(&left_cam.fb, 1, sizeof(left_cam.fb), stdout);
    //     memset(&left_cam.fb, 0x00, sizeof(left_cam.fb));
        sleep_ms(1000);
     }
}