import cProfile
import numpy as np
import tqdm
import math
import matplotlib.pyplot as plt

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
from spatz.sensors.gps.erinome1 import Erinome_I
from spatz.observer import Observer


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([
    [ 0,  1,  0],
    [ 0,  0,  1],
    [-1,  0,  0]
])
accelerometer = simulation.add_sensor(WSEN_ISDS_ACC, orientation=orientation)
gyroscope = simulation.add_sensor(WSEN_ISDS_GYRO)

orientation = np.array([
    [0, 1,  0],
    [0, 0, -1],
    [1, 0,  0]
])
high_g = simulation.add_sensor(H3LIS100DL)
erinome = simulation.add_sensor(Erinome_I)


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

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