void CAMERA_IO_Init(void)
{
  I2C1_Init();
}

void CAMERA_IO_Write(uint8_t Addr, uint8_t Reg, uint8_t Value)
{
  I2C1_WriteMultiple(Addr, (uint16_t)Reg, I2C_MEMADD_SIZE_8BIT, (uint8_t*)&Value, 1);
}

uint8_t CAMERA_IO_Read(uint8_t Addr, uint8_t Reg)
{
  uint8_t read_value = 0;
  I2C1_ReadMultiple(Addr, Reg, I2C_MEMADD_SIZE_8BIT, (uint8_t*)&read_value, 1);
  return read_value;
}

static void I2C1_Init(void)
{
  if(HAL_I2C_GetState(&heval_I2c1) == HAL_I2C_STATE_RESET)
  {
    heval_I2c1.Instance = I2C1;

    // NOTA PER STM32H7: Si usa il registro Timing al posto di ClockSpeed e DutyCycle.
    // Questo valore (es. 0x10707DBC o simile generato da CubeMX) imposta I2C a 100kHz con clock a 64MHz.
    // Puoi calcolarlo facilmente in STM32CubeMX configurando I2C1 su PB8/PB9.
    heval_I2c1.Init.Timing          = 0x10707DBC;
    heval_I2c1.Init.OwnAddress1     = 0;
    heval_I2c1.Init.AddressingMode  = I2C_ADDRESSINGMODE_7BIT;
    heval_I2c1.Init.DualAddressMode = I2C_DUALADDRESS_DISABLE;
    heval_I2c1.Init.OwnAddress2     = 0;
    heval_I2c1.Init.GeneralCallMode = I2C_GENERALCALL_DISABLE;
    heval_I2c1.Init.NoStretchMode   = I2C_NOSTRETCH_DISABLE;

    HAL_I2C_Init(&heval_I2c1);
  }
}

#ifdef NOT_INCLUDE 
// function in the stm32h7xx_hal_msp.c

void HAL_I2C_MspInit(I2C_HandleTypeDef* i2cHandle)
{
  GPIO_InitTypeDef GPIO_InitStruct = {0};
  if(i2cHandle->Instance == I2C1)
  {
    /* Abilitazione clock GPIO B e I2C1 */
    __HAL_RCC_GPIOB_CLK_ENABLE();
    __HAL_RCC_I2C1_CLK_ENABLE();

    /**I2C1 GPIO Configuration
    PB8     ------> I2C1_SCL
    PB9     ------> I2C1_SDA
    */
    GPIO_InitStruct.Pin = GPIO_PIN_8 | GPIO_PIN_9;
    GPIO_InitStruct.Mode = GPIO_MODE_AF_OD;
    GPIO_InitStruct.Pull = GPIO_PULLUP;
    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH;
    GPIO_InitStruct.Alternate = GPIO_AF4_I2C1;
    HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);

    /* Abilitazione interrupt I2C1 */
    HAL_NVIC_SetPriority(I2C1_EV_IRQn, 5, 0);
    HAL_NVIC_EnableIRQ(I2C1_EV_IRQn);
    HAL_NVIC_SetPriority(I2C1_ER_IRQn, 5, 0);
    HAL_NVIC_EnableIRQ(I2C1_ER_IRQn);
  }
}
#endif

static HAL_StatusTypeDef I2C1_ReadMultiple(uint8_t Addr, uint16_t Reg, uint16_t MemAddress, uint8_t *Buffer, uint16_t Length)
{
  HAL_StatusTypeDef status = HAL_I2C_Mem_Read(&heval_I2c1, Addr, Reg, MemAddress, Buffer, Length, 1000);
  if(status != HAL_OK) {
    I2C1_Error(Addr);
  }
  return status;
}

static HAL_StatusTypeDef I2C1_WriteMultiple(uint8_t Addr, uint16_t Reg, uint16_t MemAddress, uint8_t *Buffer, uint16_t Length)
{
  HAL_StatusTypeDef status = HAL_I2C_Mem_Write(&heval_I2c1, Addr, Reg, MemAddress, Buffer, Length, 1000);
  if(status != HAL_OK) {
    I2C1_Error(Addr);
  }
  return status;
}

static void I2C1_Error(uint8_t Addr)
{
  HAL_I2C_DeInit(&heval_I2c1);
  I2C1_Init();
}