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


int main() {
    cam_board_init();
    // set PA11 and PA12 to alternate function (10 for USB)
//     enable clock for GPIOA

    // enable VDDIO2
   // enable VDDUSB
   PWR->SVMCR |= PWR_SVMCR_USV;




//    PWR->SVMCR |= PWR_SVMCR_IO2SV;
    // enable PWREN
//    RCC->AHB3ENR |= RCC_AHB3ENR_PWREN;
//    // enable VDDA
//    PWR->SVMCR |= PWR_SVMCR_ASV;
    RCC->AHB2ENR1 |= RCC_AHB2ENR1_GPIOAEN;


    // enable VDDIO2
//    PWR->SVMCR |= PWR_SVMCR_IO2SV;


////////////////////////////
    GPIOA->MODER &= ~(GPIO_MODER_MODE11 | GPIO_MODER_MODE12);
    GPIOA->MODER |= (0x2 << GPIO_MODER_MODE11_Pos) | (0x2 << GPIO_MODER_MODE12_Pos);
    GPIOA->AFR[1] &= ~(GPIO_AFRH_AFSEL11 | GPIO_AFRH_AFSEL12);
    GPIOA->AFR[1] |= (0xA << GPIO_AFRH_AFSEL11_Pos) | (0xA << GPIO_AFRH_AFSEL12_Pos);
    // output speed to very high
//    GPIOA->OSPEEDR |= (0x3 << GPIO_OSPEEDR_OSPEED11_Pos) | (0x3 << GPIO_OSPEEDR_OSPEED12_Pos);
    // set PA11 and PA12 to pull-push
//    GPIOA->OTYPER &= ~(GPIO_OTYPER_OT11 | GPIO_OTYPER_OT12);

    // enable usb clock
    RCC->AHB2ENR1 |= RCC_AHB2ENR1_OTGEN;


    //////////////////////
    // translate from cube code
//    USB_devInit
    USB_OTG_DeviceTypeDef *USB_DE = (USB_OTG_DeviceTypeDef *) (0x42040800);
    for (int i = 0; i < 15; i++)
        USB_OTG_FS->DIEPTXF[i] = 0;

    USB_DE->DCTL |= USB_OTG_DCTL_SDIS;
    USB_OTG_FS->GCCFG &= ~USB_OTG_GCCFG_VBDEN;

    USB_OTG_FS->GUSBCFG |= USB_OTG_GUSBCFG_FDMOD;
    USB_OTG_FS->GOTGCTL |= USB_OTG_GOTGCTL_BVALOEN;
    USB_OTG_FS->GOTGCTL |= USB_OTG_GOTGCTL_BVALOVAL;
//    // restart the PHY clock
#define USBx_PCGCCTL    *(__IO uint32_t *)((uint32_t)USB_OTG_FS_BASE + USB_OTG_PCGCCTL_BASE)
//    USB_OTG_PCGCCTL_BASE = 0;
    USBx_PCGCCTL = 0U;
    // set to full speed mode
//    USB_DE->DCFG |= USB_OTG_DCFG_DSPD; // 1 or 3?
    // flush all tx fifo
    USB_OTG_FS->GRSTCTL |= USB_OTG_GRSTCTL_TXFFLSH  | (0x10 << USB_OTG_GRSTCTL_TXFNUM_Pos);
    // flush rx fifo
    USB_OTG_FS->GRSTCTL |= USB_OTG_GRSTCTL_RXFFLSH;
    // clear all pending device interrupts
    USB_DE->DIEPMSK = 0;
    USB_DE->DOEPMSK = 0;
    USB_DE->DAINTMSK = 0;
////// skip endpoint config

#define USBx USB_OTG_FS
#define USBx_DEVICE USB_DE

    USBx_PCGCCTL &= ~(USB_OTG_PCGCCTL_STOPCLK | USB_OTG_PCGCCTL_GATECLK);


    // USB_OTG_FS->GRXFSIZ = 0x100;
    // USB_OTG_FS->DIEPTXF[0] = (0x40 << USB_OTG_DIEPTXF_INEPTXFD_Pos) | 0x100;
    // USB_OTG_FS->DIEPTXF[1] = (0x80 << USB_OTG_DIEPTXF_INEPTXFD_Pos) | (0x100 + 0x40);
    // USB_OTG_FS->DIEPTXF[2] = (0xC0 << USB_OTG_DIEPTXF_INEPTXFD_Pos) | (0x100 + 0x40 + 0x80);
    // USB_OTG_FS->DIEPTXF[3] = (0x100 << USB_OTG_DIEPTXF_INEPTXFD_Pos) | (0x100 + 0x40 + 0x80 + 0xC0);




    USBx_DEVICE->DCTL |= USB_OTG_DCTL_SDIS;
    USBx->GCCFG |= USB_OTG_GCCFG_PWRDWN;
    USBx_PCGCCTL &= ~(USB_OTG_PCGCCTL_STOPCLK | USB_OTG_PCGCCTL_GATECLK);

    USBx_DEVICE->DCTL &= ~USB_OTG_DCTL_SDIS;







    /////////////////////////





//
////    USB_OTG_FS->
////    USB_OTG_PCGCCTL_BASE
//
//// set DIEPTXF to 0
//    USB_OTG_FS->GCCFG |=
//            USB_OTG_GCCFG_PWRDWN | USB_OTG_GOTGCTL_BVALOEN | USB_OTG_GOTGCTL_BVALOVAL | USB_OTG_GCCFG_VBDEN;
//    // flush FIFO
//    USB_OTG_FS->GRSTCTL |= USB_OTG_GRSTCTL_TXFFLSH | USB_OTG_GRSTCTL_RXFFLSH;
//
//    // clear all pending interrupts
//    USB_OTG_FS->GINTSTS = 0xFFFFFFFF;
//
//    USB_OTG_FS->GAHBCFG |= USB_OTG_GAHBCFG_GINT;
//    // set  OTG_FS TIMEout and
//    USB_OTG_FS->GAHBCFG |= (0x6 << USB_OTG_GUSBCFG_TRDT_Pos);
//    // unmask GINTMSK OTGINT and  MMISM
//    USB_OTG_FS->GINTMSK |= USB_OTG_GINTMSK_OTGINT | USB_OTG_GINTMSK_MMISM;
//    // set to device mode
//    USB_OTG_FS->GUSBCFG |= USB_OTG_GUSBCFG_FDMOD;
//    // read CMOD to check if it is in device mode
//    while ((USB_OTG_FS->GINTSTS & USB_OTG_GINTSTS_CMOD) != 0) {}
//
//    // initialized Device
//    USB_DE->DCFG |= USB_OTG_DCFG_DSPD;
//    // device speed
//    // non-zero-length status OUT handshake
//    USB_DE->DCFG |= USB_OTG_DCFG_NZLSOHSK;
//    // periodic frame interval
//    USB_DE->DCFG |= (0x6 << USB_OTG_DCFG_PFIVL_Pos);
//    // clear all pending interrupts
//    USB_OTG_FS->GINTSTS = 0xFFFFFFFF;
//    // clear dctl.sdis
//    USB_DE->DCTL &= ~USB_OTG_DCTL_SDIS;


    while (true) {}

}