//#define DUAL_CORE_BOOT_SYNC_SEQUENCE

/* USER CODE BEGIN Private defines */
#define CAMERA_FRAME_BUFFER               0xD0200000
#define QVGA_RES_X      320
#define QVGA_RES_Y      240
/* USER CODE END Private defines */

/* USER CODE START PV */
static volatile uint8_t frame_captured = 0;
static OV5640_Object_t CameraObj;
/* USER CODE END PV */

/* USER CODE BEGIN 0 */
static void UART_Print(const char *str)
{
  HAL_UART_Transmit(&huart1, (uint8_t *)str, (uint16_t)strlen(str), HAL_MAX_DELAY);
}

static int32_t OV5640_IO_Init(void)      { return OV5640_OK; }
static int32_t OV5640_IO_DeInit(void)    { return OV5640_OK; }
static int32_t OV5640_IO_GetTick(void)   { return (int32_t)HAL_GetTick(); }

static int32_t OV5640_IO_WriteReg(uint16_t addr, uint16_t reg,
                                   uint8_t *pData, uint16_t len)
{
  if (HAL_I2C_Mem_Write(&hi2c4, addr, reg, I2C_MEMADD_SIZE_16BIT,
                         pData, len, 1000) != HAL_OK)
    return OV5640_ERROR;
  return OV5640_OK;
}

static int32_t OV5640_IO_ReadReg(uint16_t addr, uint16_t reg,
                                  uint8_t *pData, uint16_t len)
{
  if (HAL_I2C_Mem_Read(&hi2c4, addr, reg, I2C_MEMADD_SIZE_16BIT,
                        pData, len, 1000) != HAL_OK)
    return OV5640_ERROR;
  return OV5640_OK;
}

void HAL_DCMI_FrameEventCallback(DCMI_HandleTypeDef *hdcmi_cb)
{
  (void)hdcmi_cb;
  frame_captured = 1;
}

static int32_t Camera_Init(void)
{
  OV5640_IO_t  CameraIO;
  uint32_t     camera_id = 0;
  char         buf[256];

  CameraIO.Init      = OV5640_IO_Init;
  CameraIO.DeInit    = OV5640_IO_DeInit;
  CameraIO.Address   = 0x78U;   /* 7-bit 0x3C shifted left */
  CameraIO.WriteReg  = OV5640_IO_WriteReg;
  CameraIO.ReadReg   = OV5640_IO_ReadReg;
  CameraIO.ModifyReg = NULL;
  CameraIO.GetTick   = OV5640_IO_GetTick;

  if (OV5640_RegisterBusIO(&CameraObj, &CameraIO) != OV5640_OK)
    return -1;

  if (OV5640_ReadID(&CameraObj, &camera_id) != OV5640_OK)
    return -1;

  if (camera_id != 0x5640)
    return -1;

  if (OV5640_Init(&CameraObj, OV5640_R320x240, OV5640_JPEG) != OV5640_OK)
    return -1;

  if (OV5640_Start(&CameraObj) != OV5640_OK)
    return -1;

  return 0;
}

static int32_t Camera_CaptureSnapshot(uint32_t *out_jpeg_offset,
                                       uint32_t *out_jpeg_size)
{
  const uint32_t CAPTURE_TIMEOUT_MS = 3000U;

  *out_jpeg_offset = 0;
  *out_jpeg_size   = 0;
  frame_captured   = 0;

  __HAL_DCMI_ENABLE_IT(&hdcmi, DCMI_IT_FRAME);

  /* Allocate the full DMA budget upfront – the actual JPEG size is unknown
   * before compression, so we hand the DMA the worst-case word count for a
   * YUV422 frame at the configured resolution.                              */
  uint32_t dma_size_words = (QVGA_RES_X * QVGA_RES_Y * 2U) / 4U;

  /* Start DCMI in snapshot mode – one frame then stop automatically */
  if (HAL_DCMI_Start_DMA(&hdcmi, DCMI_MODE_SNAPSHOT,
                          (uint32_t)CAMERA_FRAME_BUFFER,
                          dma_size_words) != HAL_OK)
  {
    UART_Print("[CAM] HAL_DCMI_Start_DMA failed\r\n");
    return -1;
  }

  uint32_t t_start = HAL_GetTick();

  /* Spin until the DCMI frame-complete ISR sets the flag or we time out */
  while (frame_captured == 0)
  {
    if ((HAL_GetTick() - t_start) >= CAPTURE_TIMEOUT_MS)
    {
      HAL_DCMI_Stop(&hdcmi);
      UART_Print("[CAM] Capture timeout\r\n");
      frame_captured = 0;
      return -1;
    }
  }

  uint32_t remaining_words = __HAL_DMA_GET_COUNTER(hdcmi.DMA_Handle);
  HAL_DCMI_Stop(&hdcmi);
  frame_captured = 0;

  /* Total bytes written by DMA into the frame buffer */
  uint32_t dma_bytes = (dma_size_words - remaining_words) * 4U;

  /* --------------------------------------------------------------------- *
   * Locate JPEG SOI marker (0xFF 0xD8) and EOI marker (0xFF 0xD9)         *
   * The OV5640 may prepend padding bytes before the actual JPEG stream.   *
   * --------------------------------------------------------------------- */
  uint8_t *buf_ptr = (uint8_t *)CAMERA_FRAME_BUFFER;
  uint32_t soi_offset = 0;
  uint8_t  soi_found  = 0;

  for (uint32_t i = 0; i + 1U < dma_bytes; i++)
  {
    if (buf_ptr[i] == 0xFFU && buf_ptr[i + 1U] == 0xD8U)
    {
      soi_offset = i;
      soi_found  = 1;
      break;
    }
  }

  if (!soi_found)
  {
    UART_Print("[CAM] JPEG SOI marker (0xFFD8) not found\r\n");
    return -1;
  }

  /* Search for EOI marker (0xFF 0xD9) starting just after SOI */
  uint8_t  eoi_found  = 0;
  uint32_t eoi_offset = 0;

  for (uint32_t i = soi_offset + 2U; i + 1U < dma_bytes; i++)
  {
    if (buf_ptr[i] == 0xFFU && buf_ptr[i + 1U] == 0xD9U)
    {
      /* EOI is the two-byte sequence itself; include both bytes */
      eoi_offset = i + 2U;
      eoi_found  = 1;
      break;
    }
  }

  if (!eoi_found)
  {
    UART_Print("[CAM] JPEG EOI marker (0xFFD9) not found\r\n");
    return -1;
  }

  *out_jpeg_offset = soi_offset;
  *out_jpeg_size   = eoi_offset - soi_offset;

  char dbg[80];
  snprintf(dbg, sizeof(dbg),
           "[CAM] JPEG markers found: offset=%lu size=%lu bytes\r\n",
           (unsigned long)soi_offset, (unsigned long)*out_jpeg_size);
  UART_Print(dbg);

  return 0;


}

static int32_t SD_SaveJPEG(uint32_t jpeg_offset, uint32_t jpeg_size)
{
  static uint32_t file_index = 0;
  char     filename[32];
  FRESULT  res;
  FIL      file;
  UINT     bw;
  char     buf[64];

  if (jpeg_size == 0)
  {
    UART_Print("[SD] Zero bytes to write, skipping\r\n");
    return -1;
  }

  res = f_mount(&SDFatFS, (TCHAR const *)SDPath, 1);
  if (res != FR_OK)
  {
    snprintf(buf, sizeof(buf), "[SD] f_mount failed (%d)\r\n", res);
    UART_Print(buf);
    return -1;
  }

  /* Find the next unused filename */
  do
  {
    file_index++;
    if (file_index > 9999U)
    {
      UART_Print("[SD] File index overflow\r\n");
      f_mount(NULL, (TCHAR const *)SDPath, 0);
      return -1;
    }
    snprintf(filename, sizeof(filename), "IMG_%04lu.JPG", (unsigned long)file_index);
  }
  while (f_stat(filename, NULL) == FR_OK);

  res = f_open(&file, filename, FA_CREATE_NEW | FA_WRITE);
  if (res != FR_OK)
  {
    snprintf(buf, sizeof(buf), "[SD] f_open failed (%d)\r\n", res);
    UART_Print(buf);
    f_mount(NULL, (TCHAR const *)SDPath, 0);
    return -1;
  }

  /* Write only the valid JPEG payload: from SOI (0xFFD8) to EOI (0xFFD9)
   * inclusive, skipping any padding bytes the DMA may have prepended.    */
  const uint8_t *jpeg_start = (const uint8_t *)CAMERA_FRAME_BUFFER + jpeg_offset;
  res = f_write(&file, jpeg_start, jpeg_size, &bw);
  f_close(&file);
  f_mount(NULL, (TCHAR const *)SDPath, 0);

  if (res != FR_OK || bw != jpeg_size)
  {
    snprintf(buf, sizeof(buf), "[SD] Write error (%d), wrote %u/%lu bytes\r\n",
             res, (unsigned)bw, (unsigned long)jpeg_size);
    UART_Print(buf);
    return -1;
  }

  snprintf(buf, sizeof(buf), "[SD] Saved %s (%lu bytes)\r\n",
           filename, (unsigned long)jpeg_size);
  UART_Print(buf);
  return 0;
}


/* USER CODE END 0 */

/* USER CODE BEGIN 2 */
if(BSP_SDRAM_Init(0) != BSP_ERROR_NONE){
    while(1){}
}
Camera_Init();
HAL_Delay(1000);
uint32_t jpeg_offset = 0;
uint32_t jpeg_size   = 0;
if (Camera_CaptureSnapshot(&jpeg_offset, &jpeg_size) == 0)
    SD_SaveJPEG(jpeg_offset, jpeg_size);
/* USER CODE END 2 */