import cProfile
import numpy as np
import tqdm

from spatz.simulation import Simulation
from spatz.simulations.rocketpy import RocketPyCSV
from spatz.sensors.pressure.ms5611 import MS5611
from spatz.sensors.imu.wsen_isds import WSEN_ISDS_ACC, WSEN_ISDS_GYRO
from spatz.sensors.imu.h3lis100dl import H3LIS100DL


simulation = Simulation().load(RocketPyCSV('nominal_wind.csv'))

barometer = simulation.add_sensor(MS5611)

# Orientation matrix of the WSEN-ISDS IMU on the rocket.
orientation = np.array([
    [ 1,  0,  0],
    [ 0,  0, -1],
    [ 0, -1,  0]
])
accelerometer = simulation.add_sensor(WSEN_ISDS_ACC, orientation=orientation)
gyroscope = simulation.add_sensor(WSEN_ISDS_GYRO)
high_g = simulation.add_sensor(H3LIS100DL)


for i in tqdm.tqdm(range(5000)):
    simulation.advance(0.1)
    pressure = barometer()
    acc = accelerometer()
    omegas = gyroscope()
    g = high_g()


df = simulation.get_logger().get_dataframe()
df.to_csv('dummy2.csv')