#include <Wire.h>

#define SENSOR_ADDR 0x68

void setup() {
  Wire.begin();
  Wire.setClock(400000);  // Fast mode 400 kHz
}

uint8_t readRegister(uint8_t reg) {
  Wire.beginTransmission(SENSOR_ADDR);
  Wire.write(reg);
  Wire.endTransmission(false);  // Repeated start
  Wire.requestFrom(SENSOR_ADDR, 1);
  return Wire.read();
}

#include <SPI.h>

#define CS_PIN 10

void setup() {
  SPI.begin();
  pinMode(CS_PIN, OUTPUT);
  digitalWrite(CS_PIN, HIGH);
}

uint8_t readSPI(uint8_t reg) {
  digitalWrite(CS_PIN, LOW);
  SPI.transfer(reg | 0x80);  // Read bit
  uint8_t data = SPI.transfer(0x00);
  digitalWrite(CS_PIN, HIGH);
  return data;
}

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Range
import board
import adafruit_vl53l0x

class ToFPublisher(Node):
    def __init__(self):
        super().__init__('tof_publisher')
        self.pub = self.create_publisher(Range, 'range', 10)
        
        i2c = board.I2C()
        self.sensor = adafruit_vl53l0x.VL53L0X(i2c)
        
        self.timer = self.create_timer(0.05, self.publish)  # 20 Hz
    
    def publish(self):
        msg = Range()
        msg.header.stamp = self.get_clock().now().to_msg()
        msg.header.frame_id = 'tof_link'
        msg.radiation_type = Range.INFRARED
        msg.field_of_view = 0.436  # 25° in radians
        msg.min_range = 0.03
        msg.max_range = 2.0
        msg.range = self.sensor.range / 1000.0  # mm to m
        self.pub.publish(msg)

def main():
    rclpy.init()
    node = ToFPublisher()
    rclpy.spin(node)

float medianFilter(float *buffer, int size) {
  std::sort(buffer, buffer + size);
  return buffer[size / 2];
}

class KalmanFilter {
  float Q = 0.001;  // Process noise
  float R = 0.1;    // Measurement noise
  float x = 0;      // State estimate
  float P = 1;      // Error covariance
  
public:
  float update(float measurement) {
    // Predict
    P = P + Q;
    // Update
    float K = P / (P + R);
    x = x + K * (measurement - x);
    P = (1 - K) * P;
    return x;
  }
};