import time
import math
class FakeMPU6050:
    def __init__(self, address):
        self.address = address
    def get_accel_data(self):
        return {"x": 0.1, "y": -0.2, "z": 9.7} 
    def get_gyro_data(self):
        return {"x": 0.01, "y": -0.02, "z": 0.03} 
sensor = FakeMPU6050(0x68) 

NUM_SAMPLES = 1000  

def calibrate_sensor(num_samples):
    """
    Калибрует акселерометр и гироскоп, вычисляя смещения.
    """
    print("Начинаем калибровку. Убедитесь, что датчик находится в состоянии покоя.")
    time.sleep(2)  

    accel_x_sum = 0.0
    accel_y_sum = 0.0
    accel_z_sum = 0.0
    gyro_x_sum = 0.0
    gyro_y_sum = 0.0
    gyro_z_sum = 0.0

    for i in range(num_samples):
        try:
            accel_data = sensor.get_accel_data()
            gyro_data = sensor.get_gyro_data()

            accel_x_sum += accel_data["x"]
            accel_y_sum += accel_data["y"]
            accel_z_sum += accel_data["z"]
            gyro_x_sum += gyro_data["x"]
            gyro_y_sum += gyro_data["y"]
            gyro_z_sum += gyro_data["z"]
            if (i + 1) % 100 == 0:
                print(f"Сделано {i + 1}/{num_samples} измерений")
            time.sleep(0.002) 
        except Exception as e:
            print(f"Ошибка при чтении данных: {e}")
            return None 
    accel_x_offset = accel_x_sum / num_samples
    accel_y_offset = accel_y_sum / num_samples
    accel_z_offset = accel_z_sum / num_samples - 9.8 
    gyro_x_offset = gyro_x_sum / num_samples
    gyro_y_offset = gyro_y_sum / num_samples
    gyro_z_offset = gyro_z_sum / num_samples

    print("Калибровка завершена!")
    return {
        "accel_x": accel_x_offset,
        "accel_y": accel_y_offset,
        "accel_z": accel_z_offset,
        "gyro_x": gyro_x_offset,
        "gyro_y": gyro_y_offset,
        "gyro_z": gyro_z_offset,
    }

def apply_calibration(accel_data, gyro_data, calibration_offsets):
    """
    Применяет калибровочные смещения к новым измерениям.
    """
    if calibration_offsets is None:
        print("Ошибка: Сначала необходимо выполнить калибровку!")
        return None

    accel_x_calibrated = accel_data["x"] - calibration_offsets["accel_x"]
    accel_y_calibrated = accel_data["y"] - calibration_offsets["accel_y"]
    accel_z_calibrated = accel_data["z"] - calibration_offsets["accel_z"]
    gyro_x_calibrated = gyro_data["x"] - calibration_offsets["gyro_x"]
    gyro_y_calibrated = gyro_data["y"] - calibration_offsets["gyro_y"]
    gyro_z_calibrated = gyro_data["z"] - calibration_offsets["gyro_z"]

    return {
        "accel_x": accel_x_calibrated,
        "accel_y": accel_y_calibrated,
        "accel_z": accel_z_calibrated,
        "gyro_x": gyro_x_calibrated,
        "gyro_y": gyro_y_calibrated,
        "gyro_z": gyro_z_calibrated,
    }

if __name__ == "__main__":
    calibration_offsets = calibrate_sensor(NUM_SAMPLES)

    if calibration_offsets:
        print("Калибровочные смещения:")
        print(f"  Акселерометр: X={calibration_offsets['accel_x']:.4f}, Y={calibration_offsets['accel_y']:.4f}, Z={calibration_offsets['accel_z']:.4f}")
        print(f"  Гироскоп: X={calibration_offsets['gyro_x']:.4f}, Y={calibration_offsets['gyro_y']:.4f}, Z={calibration_offsets['gyro_z']:.4f}")
        print("\nПример использования откалиброванных данных:")
        try:
            raw_accel_data = sensor.get_accel_data()
            raw_gyro_data = sensor.get_gyro_data()

            calibrated_data = apply_calibration(raw_accel_data, raw_gyro_data, calibration_offsets)

            if calibrated_data:
                print("  Неоткалиброванные данные:")
                print(f"    Акселерометр: X={raw_accel_data['x']:.4f}, Y={raw_accel_data['y']:.4f}, Z={raw_accel_data['z']:.4f}")
                print(f"    Гироскоп: X={raw_gyro_data['x']:.4f}, Y={raw_gyro_data['y']:.4f}, Z={raw_gyro_data['z']:.4f}")

                print("  Откалиброванные данные:")
                print(f"    Акселерометр: X={calibrated_data['accel_x']:.4f}, Y={calibrated_data['accel_y']:.4f}, Z={calibrated_data['accel_z']:.4f}")
                print(f"    Гироскоп: X={calibrated_data['gyro_x']:.4f}, Y={calibrated_data['gyro_y']:.4f}, Z={calibrated_data['gyro_z']:.4f}")
        except Exception as e:
            print(f"Ошибка при чтении/калибровке данных: {e}")