import smbus
import time
import math

class PunchSpeedSensor:
    def __init__(self, device_address=0x68, threshold=0.5, sampling_rate=100):
        self.device_address = device_address
        self.threshold = threshold
        self.sampling_rate = sampling_rate
        self.dt = 1.0 / sampling_rate
        self.bus = smbus.SMBus(1)
        self.PWR_MGMT_1 = 0x6B
        self.ACCEL_XOUT_H = 0x3B
        self.ACCEL_YOUT_H = 0x3D
        self.ACCEL_ZOUT_H = 0x3F
        self._initialize_sensor()

    def _initialize_sensor(self):
        self.bus.write_byte_data(self.device_address, self.PWR_MGMT_1, 1)

    def _read_raw_data(self, addr):
        high = self.bus.read_byte_data(self.device_address, addr)
        low = self.bus.read_byte_data(self.device_address, addr+1)
        value = ((high << 8) | low)
        if value > 32768:
            value = value - 65536
        return value

    def _get_acceleration(self):
        acc_x = self._read_raw_data(self.ACCEL_XOUT_H)
        acc_y = self._read_raw_data(self.ACCEL_YOUT_H)
        acc_z = self._read_raw_data(self.ACCEL_ZOUT_H)
        Ax = acc_x / 16384.0
        Ay = acc_y / 16384.0
        Az = acc_z / 16384.0
        return Ax, Ay, Az

    def measure_punch_speed(self):
        Ax, Ay, Az = self._get_acceleration()
        acceleration_magnitude = math.sqrt(Ax**2 + Ay**2 + Az**2)
        
        if acceleration_magnitude > self.threshold:
            print("Punch detected!")
            velocity = 0
            max_velocity = 0
            start_time = time.time()
            
            while acceleration_magnitude > self.threshold:
                Ax, Ay, Az = self._get_acceleration()
                acceleration_magnitude = math.sqrt(Ax**2 + Ay**2 + Az**2)
                velocity += acceleration_magnitude * self.dt
                if velocity > max_velocity:
                    max_velocity = velocity
                time.sleep(self.dt)
            
            end_time = time.time()
            duration = end_time - start_time
            max_velocity_kmh = max_velocity * 3.6  # Convert m/s to km/h
            return max_velocity_kmh, duration
        else:
            return 0, 0