# -*- coding: utf-8 -*-
# Filename: user_imu_gen.py

"""
The simplest demo of Sim.
Only generate reference trajectory (pos, vel, sensor output). No algorithm.
Created on 2021-08-09
@author: jiangtianyu
"""

import os
import math
import time
import numpy as np

from gnss_ins_sim.sim import imu_model
from gnss_ins_sim.sim import ins_sim

# globals
D2R = math.pi/180

motion_def_path = os.path.abspath('.//user_motion_def_files//')
fs = 100.0          # IMU sample frequency
fs_gps = 10.0       # GPS sample frequency
fs_mag = fs         # magnetometer sample frequency, not used for now

def test_path_gen():
    '''
    test only path generation in Sim.
    '''
    # imu_err
    imu_err = {
                # gyro bias, deg/hr
                'gyro_b': np.array([0, 0, 0]) * 3600,
                # gyro angle random walk, deg/rt-hr
                'gyro_arw': np.array([0, 0, 0]),
                 # gyro bias instability, deg/hr
                'gyro_b_stability': np.array([0, 0, 0]),
                # gyro bias instability correlation, sec.
                # set this to 'inf' to use a random walk model
                # set this to a positive real number to use a first-order Gauss-Markkov model
                'gyro_b_corr': np.array([float("inf"), float("inf"), float("inf")]),
                # accelerometer bias, m/s^2
                'accel_b': np.array([0, 0, 0]),
                # accelerometer velocity random walk, m/s/rt-hr
                'accel_vrw': np.array([0, 0, 0]),
                # accelerometer bias instability, m/s^2
                'accel_b_stability': np.array([0, 0, 0]),
                # accelerometer bias instability correlation, sec. Similar to gyro_b_corr
                'accel_b_corr': np.array([float("inf"), float("inf"), float("inf")]),
               }
    # gps_err
    gps_err = {
        'stdp': np.array([1, 1, 1]) * 1e-6,
        'stdv': np.array([1, 1, 1]) * 1e-6,
    }

    # generate GPS and magnetometer data
    imu = imu_model.IMU(accuracy=imu_err, axis=6, gps=True, gps_opt=gps_err)

    #### start simulation
    sim = ins_sim.Sim([fs, fs_gps, fs_mag],
                      motion_def_path+"//motion_straight.csv",
                      ref_frame=0,
                      imu=imu,
                      mode=None,
                      env=None,
                      algorithm=None)
    sim.run(1)
    # save simulation data to files
    sim.results('./user_saved_data/' + time.strftime('%Y-%m-%d-%H-%M-%S', time.localtime()) + '//')
    # plot data, 3d plot of reference positoin, 2d plots of gyro and accel
    # sim.plot(['ref_pos', 'gyro', 'accel'], opt={'ref_pos': '3d'})

if __name__ == '__main__':
    test_path_gen()