#include "bsp.h"
#include "ov5640.h"
#include "prepherials.h"
#include <string>
#include "dcmi.hpp"
#include "custom_prephrals.h"
#include "ov5640_configs.h"

uint32_t picture[PICTURE_BUFFER_SIZE];

#define OV5640_I2C_ADDR (0x78)
#include "fx_api.h"
extern "C" {
VOID fx_ggeta_driver(FX_MEDIA *media_ptr);
}
FX_MEDIA sdio_disk;
FX_FILE file;
UINT status;
UINT media_memory[1024];

hal_status_e ov5640_read_reg(uint16_t reg[2]) {
    uint8_t reg_addr[2];
    reg_addr[0] = (reg[0] >> 8) & 0xFF;
    reg_addr[1] = reg[0] & 0xFF;
    auto ret = cam_i2c.send(OV5640_I2C_ADDR, reg_addr, 2);
    uint8_t data;
    ret = cam_i2c.read(OV5640_I2C_ADDR, &data, 1);
    reg[1] = (uint16_t) data;
    return HAL_OK;
}

hal_status_e ov5640_read_id() {
    uint8_t data[3];
    data[0] = 0x30, data[1] = 0x0A;
    uint16_t reg[2] = {0x300A, 0};
    ov5640_read_reg(reg);
    reg[0] = 0x300B;
    ov5640_read_reg(reg);
    return HAL_OK;
}

hal_status_e ov5640_write_reg(const uint16_t reg[][2], uint16_t len) {
    for (int i = 0; i < len; i++) {
        uint8_t data[3];
        data[0] = (reg[i][0] >> 8) & 0xFF;
        data[1] = reg[i][0] & 0xFF;
        data[2] = reg[i][1] & 0xFF;
        // wait for receive complete
        cam_i2c.send(OV5640_I2C_ADDR, data, 3);
    }
    return HAL_OK;
}

int main() {
    cam_board_init();
//    delay.ms(20);              // some delay for camera to be ready

//    delay_ms(200, true);
//    auto ret = sdmmc2.sdmmc_init();
//    while (ret != HAL_OK)
//        ret = sdmmc2.sdmmc_init();
//    delay.ms(20);              // some delay for camera to be ready

//    status = fx_media_open(&sdio_disk, "STM32_SDIO", fx_ggeta_driver, 0, (VOID *) media_memory,
//                           sizeof(media_memory));
    led1.set_high();
//    sdmmc2.sdmmc_init();
//    while(1);
    ov5640_read_id();
//    cam_pdwn.set_high();
    // config camera
    ov5640_write_reg(OV5640_Common, sizeof(OV5640_Common));
    ov5640_write_reg(OV5640_PF_JPEG, sizeof(OV5640_PF_JPEG));
    // setup for JPEG mode
    uint16_t tmp_addr[1][2] = {{OV5640_TIMING_TC_REG21, 0xA8}};
    IF_ERROR_STUCK(ov5640_read_reg(tmp_addr[0]));
    tmp_addr[0][1] |= (uint8_t) (1 << 5);
    IF_ERROR_STUCK(ov5640_write_reg(tmp_addr, 1));
    tmp_addr[0][0] = OV5640_SYSREM_RESET02;
    IF_ERROR_STUCK(ov5640_read_reg(tmp_addr[0]));
    tmp_addr[0][1] &= ~((1 << 2) | (1 << 3) | (1 << 4));
    IF_ERROR_STUCK(ov5640_write_reg(tmp_addr, 1));
    tmp_addr[0][0] = OV5640_CLOCK_ENABLE02;
    IF_ERROR_STUCK(ov5640_read_reg(tmp_addr[0]));
    tmp_addr[0][1] |= (1 << 3) | (1 << 5);
    IF_ERROR_STUCK(ov5640_write_reg(tmp_addr, 1));

    // end of config camera
    dcmi.init_5640(&dma10, picture, PICTURE_BUFFER_SIZE);
    while (true) {
        cam_pdwn.set_low();
        led1.set_low();
        if (dcmi.capture_block() == HAL_OK) {
            delay.ms(500);
            cam_pdwn.set_high();
            led1.set_high();
            delay.ms(500);
            // save picture
            dcmi_buffer buf = dcmi.dcmi_get_buf();
//            uint32_t * buf  = dcmi.dcmi_get_buf();
            static int pic_cnt = 0;
            std::string file_name = "picture" + std::to_string(pic_cnt) + ".jpg";
            pic_cnt++;

            status = fx_file_create(&sdio_disk, (char *) file_name.c_str());
            if (status == FX_ALREADY_CREATED) {
                // delete file and create empty file
                status = fx_file_delete(&sdio_disk, (char *) file_name.c_str());
                status = fx_file_create(&sdio_disk, (char *) file_name.c_str());
            }
            status = fx_file_open(&sdio_disk, &file, (char *) file_name.c_str(), FX_OPEN_FOR_WRITE);
            status = fx_file_seek(&file, 0);
            status = fx_file_write(&file, (uint8_t *) dcmi.dcmi_get_buf().data, dcmi.dcmi_get_buf().picture_length);
            status = fx_file_close(&file);
        }
    };


}