#!/usr/bin/env python
# -*- coding: utf-8 -*-
import numpy as np
import scipy
from dtk.bicycle import benchmark_state_space_vs_speed, benchmark_matrices


def simulate(v, x0, dt, n, u=None):
    _, A, B = benchmark_state_space_vs_speed(*benchmark_matrices(), [v])
    A = np.squeeze(A)
    B = np.squeeze(B)

    M = np.zeros((6, 6))
    M[:4, :4] = A
    M[:4, 4:] = B
    M *= dt

    Md = scipy.linalg.expm(M)
    Md_zero = Md[4:, :4]
    Md_eye = Md[4:, 4:]
    if not np.array_equal(Md_zero, np.zeros(Md_zero.shape)):
        print('WARNING: Failure in system discretization')
        print(Md_zero)
        print('should equal 0')
    if not np.array_equal(Md_eye, np.eye(2)):
        print('WARNING: Failure in system discretization')
        print(Md_eye)
        print('should equal I')
    Ad = Md[:4, :4]
    Bd = Md[:4, 4:]

    if u is None:
        u = np.zeros((2, n))
    x = np.zeros((4, n))
    for i in range(n):
        x[:, i:i+1] = np.dot(Ad, x0) + np.dot(Bd, u[:, i:i+1])
        x0 = x[:, i:i+1]
    return x


if __name__ == '__main__':
    import sys
    import matplotlib.pyplot as plt
    from plot_sim import plot_states

    sim_time = 5 # time for our Whipple simulation [s]

    if len(sys.argv) > 1:
        from load_sim import (load_messages, get_records_from_messages,
                              get_time_vector)
        messages = load_messages(sys.argv[1])
        records = get_records_from_messages(messages)
        t = get_time_vector(records)
        t -= t[0] # redefine zero time for simulator data
        m = messages[0]
        states = records.state[:, 1:]
        fig2, ax2 = plot_states(t, states)

        v = m.model.v
        dt = m.model.dt
        n = int(sim_time/dt)
        x0 = np.array(messages[1].state.x[1:]).reshape((4, 1))
    else:
        v = 5
        dt = 0.005
        n = int(sim_time/dt)
        x0 = np.array(
            [ 0.0031247 ,  0.09299604, -0.03369007, -0.05003717]
            ).reshape((4, 1))

    x = simulate(v, x0, dt, n)
    t = np.array(range(n + 1)) * dt
    fig, ax = plot_states(t, np.hstack((x0, x)).T)
    plt.show()

@@ -1159,17 +1159,17 @@
 _time = 
-5
+1
  # time 
@@ -1195,16 +1195,31 @@
 ulation 
+or upper limit 
 %5Bs%5D%0A%0A   
@@ -1495,17 +1495,41 @@
 (records
-)
+%5B1:%5D) # skip first record
 %0A       
@@ -1579,16 +1579,17 @@
 or data%0A
+%0A
         
@@ -1604,16 +1604,150 @@
 ages%5B0%5D%0A
+        v = m.model.v%0A        dt = m.model.dt%0A        if t%5B-1%5D %3C sim_time:%0A            sim_time = t%5B-1%5D%0A        n = int(sim_time/dt)%0A%0A
         
@@ -1762,16 +1762,20 @@
  records
+%5B1:%5D
 .state%5B:
@@ -1817,16 +1817,20 @@
 es(t
+%5B:n%5D
 , states
 )%0A%0A 
@@ -1829,84 +1829,74 @@
 ates
-)%0A%0A        v = m.model.v%0A        dt = m.model.dt%0A        n = int(sim_time/dt
+%5B:n, :%5D, to_degrees=True)%0A        fig2.suptitle('Phobos simulator'
 )%0A  
@@ -2263,17 +2263,74 @@
 0, x)).T
-)
+, to_degrees=True)%0A    fig.suptitle('Whipple simulation')%0A
 %0A    plt

Fix simulator plot in simulation comparison script