import numpy as np
from scipy.spatial.transform import Rotation as R

# Dados de entrada
acc_x, acc_y, acc_z = 0.030, -0.197, 0.980
gyro_x, gyro_y, gyro_z = 0.000, 0.000, 0.000
angle_x, angle_y, angle_z = -11.426, -1.725, 21.127
mag_x, mag_y, mag_z = -13.708, 250.175, 100.842
quat_0, quat_1, quat_2, quat_3 = 0.97827, -0.09509, -0.03299, 0.18091

# Função para calcular a orientação magnética
def calculate_magnetic_orientation(acc, gyro, mag, quat):
    # Converter quaternions para ângulos de Euler
    r = R.from_quat([quat[1], quat[2], quat[3], quat[0]])
    euler = r.as_euler('xyz', degrees=True)
    
    # Calcular a direção do norte magnético
    pitch = np.radians(euler[0])
    roll = np.radians(euler[1])
    
    # Corrigir os dados do magnetômetro com base na inclinação e rotação
    mag_x_corr = mag[0] * np.cos(pitch) + mag[2] * np.sin(pitch)
    mag_y_corr = mag[0] * np.sin(roll) * np.sin(pitch) + mag[1] * np.cos(roll) - mag[2] * np.sin(roll) * np.cos(pitch)
    
    # Calcular o ângulo de direção (yaw) em radianos e converter para graus
    yaw = np.arctan2(-mag_y_corr, mag_x_corr) * 180.0 / np.pi
    
    # Ajustar yaw para o intervalo [0, 360)
    if yaw < 0:
        yaw += 360
    
    return yaw

# Dados de entrada organizados
acc = [acc_x, acc_y, acc_z]
gyro = [gyro_x, gyro_y, gyro_z]
mag = [mag_x, mag_y, mag_z]
quat = [quat_0, quat_1, quat_2, quat_3]

# Calcular a orientação magnética
yaw = calculate_magnetic_orientation(acc, gyro, mag, quat)
print(f"Orientação Magnética (Yaw): {yaw:.2f} graus")