diff --git a/.gitignore b/.gitignore
index 608b1d4..a730a64 100644
--- a/.gitignore
+++ b/.gitignore
@@ -1,2 +1,3 @@
+.cache/
 .vscode/
 build/
\ No newline at end of file
diff --git a/Core/Src/main.c b/Core/Src/main.c
index ee6aa15..e2e4115 100644
--- a/Core/Src/main.c
+++ b/Core/Src/main.c
@@ -1,23 +1,25 @@
 /* USER CODE BEGIN Header */
 /**
-  ******************************************************************************
-  * @file           : main.c
-  * @brief          : Main program body
-  ******************************************************************************
-  * @attention
-  *
-  * Copyright (c) 2022 STMicroelectronics.
-  * All rights reserved.
-  *
-  * This software is licensed under terms that can be found in the LICENSE file
-  * in the root directory of this software component.
-  * If no LICENSE file comes with this software, it is provided AS-IS.
-  *
-  ******************************************************************************
-  */
+ ******************************************************************************
+ * @file           : main.c
+ * @brief          : Main program body
+ ******************************************************************************
+ * @attention
+ *
+ * Copyright (c) 2022 STMicroelectronics.
+ * All rights reserved.
+ *
+ * This software is licensed under terms that can be found in the LICENSE file
+ * in the root directory of this software component.
+ * If no LICENSE file comes with this software, it is provided AS-IS.
+ *
+ ******************************************************************************
+ */
 /* USER CODE END Header */
 /* Includes ------------------------------------------------------------------*/
 #include "main.h"
+#include "stm32l4xx_hal.h"
+#include "stm32l4xx_hal_i2c.h"
 
 /* Private includes ----------------------------------------------------------*/
 /* USER CODE BEGIN Includes */
@@ -31,6 +33,14 @@
 
 /* Private define ------------------------------------------------------------*/
 /* USER CODE BEGIN PD */
+#define VL6180X_ADDR (0x29 << 1)
+#define VL6180X_REG_SYSTEM_INTERRUPT_CONFIG_GPIO 0x14
+#define VL6180X_REG_SYSTEM_INTERRUPT_CLEAR 0x15
+#define VL6180X_REG_SYSTEM_FRESH_OUT_OF_RESET 0x16
+#define VL6180X_REG_SYSRANGE_START 0x18
+#define VL6180X_REG_RESULT_RANGE_STATUS 0x4D
+#define VL6180X_REG_RESULT_INTERRUPT_STATUS_GPIO 0x4F
+#define VL6180X_REG_RESULT_RANGE_VAL 0x62
 /* USER CODE END PD */
 
 /* Private macro -------------------------------------------------------------*/
@@ -58,22 +68,158 @@ static void MX_USART2_UART_Init(void);
 
 /* Private user code ---------------------------------------------------------*/
 /* USER CODE BEGIN 0 */
+volatile uint16_t range = 0;
 
+void tof_write_reg(uint16_t reg_addr, uint32_t data, int len_bytes) {
+  uint8_t buf[len_bytes + 2];
+  buf[0] = reg_addr >> 8;
+  buf[1] = reg_addr & 0xFF;
+  for (int idx = len_bytes + 1; idx >= 2; idx--) {
+    buf[idx] = data & 0xFF;
+    data >>= 8;
+  }
+
+  if (HAL_I2C_Master_Transmit(&hi2c1, VL6180X_ADDR, buf, len_bytes, 100) !=
+      HAL_OK) {
+    Error_Handler();
+  }
+}
+
+uint32_t tof_read_reg(uint16_t reg_addr, int len_bytes) {
+  // Write index
+  uint8_t addr_buf[2] = {reg_addr >> 8, reg_addr & 0xFF};
+  if (HAL_I2C_Master_Transmit(&hi2c1, VL6180X_ADDR, addr_buf, 2, 100) !=
+      HAL_OK) {
+    Error_Handler();
+  }
+
+  // Read data
+  uint8_t buf[len_bytes];
+  if (HAL_I2C_Master_Receive(&hi2c1, VL6180X_ADDR | 1, buf, len_bytes, 100) !=
+      HAL_OK) {
+    Error_Handler();
+  }
+
+  // Demarshal
+  uint32_t result = 0;
+  for (int i = 0; i < len_bytes; i++) {
+    int shift = (len_bytes - i - 1) * 8;
+    result |= ((uint32_t)buf[i]) << shift;
+  }
+
+  return result;
+}
+
+void tof_init(void) {
+  if (tof_read_reg(VL6180X_REG_SYSTEM_FRESH_OUT_OF_RESET, 1) & 1) {
+    // LOAD SETTINGS AS PER AN4545, SECTION 9
+    // Mandatory : private registers
+    tof_write_reg(0x0207, 0x01, 1);
+    tof_write_reg(0x0208, 0x01, 1);
+    tof_write_reg(0x0096, 0x00, 1);
+    tof_write_reg(0x0097, 0xfd, 1);
+    tof_write_reg(0x00e3, 0x01, 1);
+    tof_write_reg(0x00e4, 0x03, 1);
+    tof_write_reg(0x00e5, 0x02, 1);
+    tof_write_reg(0x00e6, 0x01, 1);
+    tof_write_reg(0x00e7, 0x03, 1);
+    tof_write_reg(0x00f5, 0x02, 1);
+    tof_write_reg(0x00d9, 0x05, 1);
+    tof_write_reg(0x00db, 0xce, 1);
+    tof_write_reg(0x00dc, 0x03, 1);
+    tof_write_reg(0x00dd, 0xf8, 1);
+    tof_write_reg(0x009f, 0x00, 1);
+    tof_write_reg(0x00a3, 0x3c, 1);
+    tof_write_reg(0x00b7, 0x00, 1);
+    tof_write_reg(0x00bb, 0x3c, 1);
+    tof_write_reg(0x00b2, 0x09, 1);
+    tof_write_reg(0x00ca, 0x09, 1);
+    tof_write_reg(0x0198, 0x01, 1);
+    tof_write_reg(0x01b0, 0x17, 1);
+    tof_write_reg(0x01ad, 0x00, 1);
+    tof_write_reg(0x00ff, 0x05, 1);
+    tof_write_reg(0x0100, 0x05, 1);
+    tof_write_reg(0x0199, 0x05, 1);
+    tof_write_reg(0x01a6, 0x1b, 1);
+    tof_write_reg(0x01ac, 0x3e, 1);
+    tof_write_reg(0x01a7, 0x1f, 1);
+    tof_write_reg(0x0030, 0x00, 1);
+
+    // Recommended : Public registers - See data sheet for more detail
+    tof_write_reg(0x0011, 0x10, 1); // Enables polling for ‘New Sample ready’
+    // when measurement completes
+    tof_write_reg(0x010a, 0x30, 1); // Set the averaging sample period
+    // (compromise between lower noise and
+    // increased execution time)
+    tof_write_reg(0x003f, 0x46, 1); // Sets the light and dark gain (upper
+    // nibble). Dark gain should not be
+    // changed.
+    tof_write_reg(0x0031, 0xFF, 1); // sets the # of range measurements after
+    // which auto calibration of system is
+    // performed
+    tof_write_reg(0x0041, 0x63, 1); // Set ALS integration time to 100ms
+    tof_write_reg(0x002e, 0x01, 1); // perform a single temperature calibration
+    // of the ranging sensor
+
+    // Optional: Public registers - See data sheet for more detail
+    tof_write_reg(0x001b, 0x09, 1); // Set default ranging inter-measurement
+    // period to 100ms
+    tof_write_reg(0x003e, 0x31, 1); // Set default ALS inter-measurement period
+    // to 500ms
+    tof_write_reg(0x0014, 0x24, 1); // Configures interrupt on ‘New Sample
+                                    // Ready threshold event’
+
+    tof_write_reg(VL6180X_REG_SYSTEM_FRESH_OUT_OF_RESET, 0, 1);
+  }
+  // tof_write_reg(VL6180X_REG_SYSTEM_INTERRUPT_CONFIG_GPIO, 4, 1);
+}
+
+uint8_t tof_range_poll(void) {
+  if (tof_read_reg(VL6180X_REG_RESULT_RANGE_STATUS, 1) & 1) {
+    // Not ready
+    Error_Handler();
+  }
+
+  tof_write_reg(VL6180X_REG_SYSRANGE_START, 0b01, 1);
+  while (1) {
+    uint8_t status = tof_read_reg(VL6180X_REG_RESULT_INTERRUPT_STATUS_GPIO, 1);
+    if (status & 0b11000000) {
+      // Error bits
+      Error_Handler();
+    }
+    if ((status & 0b111) == 4) {
+      // New sample ready threshold event
+      break;
+    }
+    status = tof_read_reg(VL6180X_REG_RESULT_RANGE_STATUS, 1);
+    if (status & 0xF0) {
+      // Error code
+      Error_Handler();
+    }
+    HAL_Delay(10);
+  }
+  uint8_t range = tof_read_reg(VL6180X_REG_RESULT_RANGE_VAL, 2);
+
+  // Clear interrupt
+  tof_write_reg(VL6180X_REG_SYSTEM_INTERRUPT_CLEAR, 0x07, 1);
+
+  return range;
+}
 /* USER CODE END 0 */
 
 /**
-  * @brief  The application entry point.
-  * @retval int
-  */
-int main(void)
-{
+ * @brief  The application entry point.
+ * @retval int
+ */
+int main(void) {
   /* USER CODE BEGIN 1 */
 
   /* USER CODE END 1 */
 
   /* MCU Configuration--------------------------------------------------------*/
 
-  /* Reset of all peripherals, Initializes the Flash interface and the Systick. */
+  /* Reset of all peripherals, Initializes the Flash interface and the Systick.
+   */
   HAL_Init();
 
   /* USER CODE BEGIN Init */
@@ -91,40 +237,40 @@ int main(void)
   MX_GPIO_Init();
   MX_I2C1_Init();
   MX_USART2_UART_Init();
+  HAL_Delay(100);
+  tof_init();
+  HAL_Delay(100);
   /* USER CODE BEGIN 2 */
 
   /* USER CODE END 2 */
 
   /* Infinite loop */
   /* USER CODE BEGIN WHILE */
-  while (1)
-  {
+  while (1) {
     /* USER CODE END WHILE */
-
     /* USER CODE BEGIN 3 */
+    range = tof_range_poll();
   }
   /* USER CODE END 3 */
 }
 
 /**
-  * @brief System Clock Configuration
-  * @retval None
-  */
-void SystemClock_Config(void)
-{
+ * @brief System Clock Configuration
+ * @retval None
+ */
+void SystemClock_Config(void) {
   RCC_OscInitTypeDef RCC_OscInitStruct = {0};
   RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};
 
   /** Configure the main internal regulator output voltage
-  */
-  if (HAL_PWREx_ControlVoltageScaling(PWR_REGULATOR_VOLTAGE_SCALE1) != HAL_OK)
-  {
+   */
+  if (HAL_PWREx_ControlVoltageScaling(PWR_REGULATOR_VOLTAGE_SCALE1) != HAL_OK) {
     Error_Handler();
   }
 
   /** Initializes the RCC Oscillators according to the specified parameters
-  * in the RCC_OscInitTypeDef structure.
-  */
+   * in the RCC_OscInitTypeDef structure.
+   */
   RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSI;
   RCC_OscInitStruct.HSIState = RCC_HSI_ON;
   RCC_OscInitStruct.HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT;
@@ -135,33 +281,30 @@ void SystemClock_Config(void)
   RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV7;
   RCC_OscInitStruct.PLL.PLLQ = RCC_PLLQ_DIV2;
   RCC_OscInitStruct.PLL.PLLR = RCC_PLLR_DIV2;
-  if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
-  {
+  if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK) {
     Error_Handler();
   }
 
   /** Initializes the CPU, AHB and APB buses clocks
-  */
-  RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
-                              |RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2;
+   */
+  RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK | RCC_CLOCKTYPE_SYSCLK |
+                                RCC_CLOCKTYPE_PCLK1 | RCC_CLOCKTYPE_PCLK2;
   RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
   RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
   RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV1;
   RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1;
 
-  if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_4) != HAL_OK)
-  {
+  if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_4) != HAL_OK) {
     Error_Handler();
   }
 }
 
 /**
-  * @brief I2C1 Initialization Function
-  * @param None
-  * @retval None
-  */
-static void MX_I2C1_Init(void)
-{
+ * @brief I2C1 Initialization Function
+ * @param None
+ * @retval None
+ */
+static void MX_I2C1_Init(void) {
 
   /* USER CODE BEGIN I2C1_Init 0 */
 
@@ -179,37 +322,32 @@ static void MX_I2C1_Init(void)
   hi2c1.Init.OwnAddress2Masks = I2C_OA2_NOMASK;
   hi2c1.Init.GeneralCallMode = I2C_GENERALCALL_DISABLE;
   hi2c1.Init.NoStretchMode = I2C_NOSTRETCH_DISABLE;
-  if (HAL_I2C_Init(&hi2c1) != HAL_OK)
-  {
+  if (HAL_I2C_Init(&hi2c1) != HAL_OK) {
     Error_Handler();
   }
 
   /** Configure Analogue filter
-  */
-  if (HAL_I2CEx_ConfigAnalogFilter(&hi2c1, I2C_ANALOGFILTER_ENABLE) != HAL_OK)
-  {
+   */
+  if (HAL_I2CEx_ConfigAnalogFilter(&hi2c1, I2C_ANALOGFILTER_ENABLE) != HAL_OK) {
     Error_Handler();
   }
 
   /** Configure Digital filter
-  */
-  if (HAL_I2CEx_ConfigDigitalFilter(&hi2c1, 0) != HAL_OK)
-  {
+   */
+  if (HAL_I2CEx_ConfigDigitalFilter(&hi2c1, 0) != HAL_OK) {
     Error_Handler();
   }
   /* USER CODE BEGIN I2C1_Init 2 */
 
   /* USER CODE END I2C1_Init 2 */
-
 }
 
 /**
-  * @brief USART2 Initialization Function
-  * @param None
-  * @retval None
-  */
-static void MX_USART2_UART_Init(void)
-{
+ * @brief USART2 Initialization Function
+ * @param None
+ * @retval None
+ */
+static void MX_USART2_UART_Init(void) {
 
   /* USER CODE BEGIN USART2_Init 0 */
 
@@ -228,23 +366,20 @@ static void MX_USART2_UART_Init(void)
   huart2.Init.OverSampling = UART_OVERSAMPLING_16;
   huart2.Init.OneBitSampling = UART_ONE_BIT_SAMPLE_DISABLE;
   huart2.AdvancedInit.AdvFeatureInit = UART_ADVFEATURE_NO_INIT;
-  if (HAL_UART_Init(&huart2) != HAL_OK)
-  {
+  if (HAL_UART_Init(&huart2) != HAL_OK) {
     Error_Handler();
   }
   /* USER CODE BEGIN USART2_Init 2 */
 
   /* USER CODE END USART2_Init 2 */
-
 }
 
 /**
-  * @brief GPIO Initialization Function
-  * @param None
-  * @retval None
-  */
-static void MX_GPIO_Init(void)
-{
+ * @brief GPIO Initialization Function
+ * @param None
+ * @retval None
+ */
+static void MX_GPIO_Init(void) {
   GPIO_InitTypeDef GPIO_InitStruct = {0};
 
   /* GPIO Ports Clock Enable */
@@ -268,7 +403,6 @@ static void MX_GPIO_Init(void)
   GPIO_InitStruct.Pull = GPIO_NOPULL;
   GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
   HAL_GPIO_Init(LD2_GPIO_Port, &GPIO_InitStruct);
-
 }
 
 /* USER CODE BEGIN 4 */
@@ -276,33 +410,31 @@ static void MX_GPIO_Init(void)
 /* USER CODE END 4 */
 
 /**
-  * @brief  This function is executed in case of error occurrence.
-  * @retval None
-  */
-void Error_Handler(void)
-{
+ * @brief  This function is executed in case of error occurrence.
+ * @retval None
+ */
+void Error_Handler(void) {
   /* USER CODE BEGIN Error_Handler_Debug */
   /* User can add his own implementation to report the HAL error return state */
   __disable_irq();
-  while (1)
-  {
+  while (1) {
   }
   /* USER CODE END Error_Handler_Debug */
 }
 
-#ifdef  USE_FULL_ASSERT
+#ifdef USE_FULL_ASSERT
 /**
-  * @brief  Reports the name of the source file and the source line number
-  *         where the assert_param error has occurred.
-  * @param  file: pointer to the source file name
-  * @param  line: assert_param error line source number
-  * @retval None
-  */
-void assert_failed(uint8_t *file, uint32_t line)
-{
+ * @brief  Reports the name of the source file and the source line number
+ *         where the assert_param error has occurred.
+ * @param  file: pointer to the source file name
+ * @param  line: assert_param error line source number
+ * @retval None
+ */
+void assert_failed(uint8_t *file, uint32_t line) {
   /* USER CODE BEGIN 6 */
-  /* User can add his own implementation to report the file name and line number,
-     ex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */
+  /* User can add his own implementation to report the file name and line
+     number, ex: printf("Wrong parameters value: file %s on line %d\r\n", file,
+     line) */
   /* USER CODE END 6 */
 }
 #endif /* USE_FULL_ASSERT */
diff --git a/Makefile b/Makefile
index fa2ecf4..d9096c8 100644
--- a/Makefile
+++ b/Makefile
@@ -22,7 +22,7 @@ TARGET = tof-sensing-stm
 # debug build?
 DEBUG = 1
 # optimization
-OPT = -Og
+OPT = -O0
 
 
 #######################################
@@ -35,33 +35,33 @@ BUILD_DIR = build
 # source
 ######################################
 # C sources
-C_SOURCES =  \
-Core/Src/main.c \
-Core/Src/stm32l4xx_it.c \
-Core/Src/stm32l4xx_hal_msp.c \
-Drivers/STM32L4xx_HAL_Driver/Src/stm32l4xx_hal_i2c.c \
-Drivers/STM32L4xx_HAL_Driver/Src/stm32l4xx_hal_i2c_ex.c \
-Drivers/STM32L4xx_HAL_Driver/Src/stm32l4xx_hal.c \
-Drivers/STM32L4xx_HAL_Driver/Src/stm32l4xx_hal_rcc.c \
-Drivers/STM32L4xx_HAL_Driver/Src/stm32l4xx_hal_rcc_ex.c \
-Drivers/STM32L4xx_HAL_Driver/Src/stm32l4xx_hal_flash.c \
-Drivers/STM32L4xx_HAL_Driver/Src/stm32l4xx_hal_flash_ex.c \
-Drivers/STM32L4xx_HAL_Driver/Src/stm32l4xx_hal_flash_ramfunc.c \
-Drivers/STM32L4xx_HAL_Driver/Src/stm32l4xx_hal_gpio.c \
-Drivers/STM32L4xx_HAL_Driver/Src/stm32l4xx_hal_dma.c \
-Drivers/STM32L4xx_HAL_Driver/Src/stm32l4xx_hal_dma_ex.c \
-Drivers/STM32L4xx_HAL_Driver/Src/stm32l4xx_hal_pwr.c \
-Drivers/STM32L4xx_HAL_Driver/Src/stm32l4xx_hal_pwr_ex.c \
-Drivers/STM32L4xx_HAL_Driver/Src/stm32l4xx_hal_cortex.c \
-Drivers/STM32L4xx_HAL_Driver/Src/stm32l4xx_hal_exti.c \
-Drivers/STM32L4xx_HAL_Driver/Src/stm32l4xx_hal_tim.c \
-Drivers/STM32L4xx_HAL_Driver/Src/stm32l4xx_hal_tim_ex.c \
-Drivers/STM32L4xx_HAL_Driver/Src/stm32l4xx_hal_uart.c \
-Drivers/STM32L4xx_HAL_Driver/Src/stm32l4xx_hal_uart_ex.c \
+C_SOURCES =  \
+Core/Src/main.c \
+Core/Src/stm32l4xx_it.c \
+Core/Src/stm32l4xx_hal_msp.c \
+Drivers/STM32L4xx_HAL_Driver/Src/stm32l4xx_hal_i2c.c \
+Drivers/STM32L4xx_HAL_Driver/Src/stm32l4xx_hal_i2c_ex.c \
+Drivers/STM32L4xx_HAL_Driver/Src/stm32l4xx_hal.c \
+Drivers/STM32L4xx_HAL_Driver/Src/stm32l4xx_hal_rcc.c \
+Drivers/STM32L4xx_HAL_Driver/Src/stm32l4xx_hal_rcc_ex.c \
+Drivers/STM32L4xx_HAL_Driver/Src/stm32l4xx_hal_flash.c \
+Drivers/STM32L4xx_HAL_Driver/Src/stm32l4xx_hal_flash_ex.c \
+Drivers/STM32L4xx_HAL_Driver/Src/stm32l4xx_hal_flash_ramfunc.c \
+Drivers/STM32L4xx_HAL_Driver/Src/stm32l4xx_hal_gpio.c \
+Drivers/STM32L4xx_HAL_Driver/Src/stm32l4xx_hal_dma.c \
+Drivers/STM32L4xx_HAL_Driver/Src/stm32l4xx_hal_dma_ex.c \
+Drivers/STM32L4xx_HAL_Driver/Src/stm32l4xx_hal_pwr.c \
+Drivers/STM32L4xx_HAL_Driver/Src/stm32l4xx_hal_pwr_ex.c \
+Drivers/STM32L4xx_HAL_Driver/Src/stm32l4xx_hal_cortex.c \
+Drivers/STM32L4xx_HAL_Driver/Src/stm32l4xx_hal_exti.c \
+Drivers/STM32L4xx_HAL_Driver/Src/stm32l4xx_hal_tim.c \
+Drivers/STM32L4xx_HAL_Driver/Src/stm32l4xx_hal_tim_ex.c \
+Drivers/STM32L4xx_HAL_Driver/Src/stm32l4xx_hal_uart.c \
+Drivers/STM32L4xx_HAL_Driver/Src/stm32l4xx_hal_uart_ex.c \
 Core/Src/system_stm32l4xx.c  
 
 # ASM sources
-ASM_SOURCES =  \
+ASM_SOURCES =  \
 startup_stm32l476xx.s
 
 
@@ -105,8 +105,8 @@ MCU = $(CPU) -mthumb $(FPU) $(FLOAT-ABI)
 AS_DEFS = 
 
 # C defines
-C_DEFS =  \
--DUSE_HAL_DRIVER \
+C_DEFS =  \
+-DUSE_HAL_DRIVER \
 -DSTM32L476xx
 
 
@@ -114,11 +114,11 @@ C_DEFS =  \
 AS_INCLUDES = 
 
 # C includes
-C_INCLUDES =  \
--ICore/Inc \
--IDrivers/STM32L4xx_HAL_Driver/Inc \
--IDrivers/STM32L4xx_HAL_Driver/Inc/Legacy \
--IDrivers/CMSIS/Device/ST/STM32L4xx/Include \
+C_INCLUDES =  \
+-ICore/Inc \
+-IDrivers/STM32L4xx_HAL_Driver/Inc \
+-IDrivers/STM32L4xx_HAL_Driver/Inc/Legacy \
+-IDrivers/CMSIS/Device/ST/STM32L4xx/Include \
 -IDrivers/CMSIS/Include