//  [python]
def test_fun1(self):
        system = setup_johansson_continuous_time_system1()
        x0 = torch.tensor([0.3, 0.8], dtype=torch.float64)
        (cost, x_traj, cost_to_go_traj, sol) = hybrid_linear_system.\
            compute_continuous_time_system_cost_to_go(
                system, x0, 15., lambda x: torch.linalg.norm(x, p=2))
        np.testing.assert_allclose(x_traj[:, -1].detach().numpy(),
                                   np.zeros(2),
                                   atol=1.2e-6)
        self.assertAlmostEqual(cost_to_go_traj[-1], 0)
        self.assertTrue(
            torch.all(cost_to_go_traj[:-1] - cost_to_go_traj[1:] > 0))
        self.assertAlmostEqual(cost.item(), cost_to_go_traj[0].item())
        np.testing.assert_allclose(
            cost_to_go_traj.detach().numpy() + sol.y[-1, :], cost)
        (cost_next, x_traj_next, cost_to_go_traj_next, sol_next) = \
            hybrid_linear_system.compute_continuous_time_system_cost_to_go(
                system, x_traj[:, 1], sol.t[-1] - sol.t[1],
                lambda x: torch.linalg.norm(x, p=2))
        self.assertAlmostEqual(cost_to_go_traj[1].item(),
                               cost_next.item(),
                               places=4)
        # The trajectory starts in mode 1, will first hit the mode boundary
        # x[0] = 0, and then stay in mode 0.
        A1_numpy = system.A[1].detach().numpy()

        def hit_boundary(t, x):
            return x[0]

        hit_boundary.terminal = True
        sol1 = solve_ivp(lambda t, x: A1_numpy @ x, (0, np.inf),
                         x0.detach().numpy(),
                         events=[hit_boundary])
        # t1 is the time to hit the mode boundary x[0] = 0
        t1 = sol1.t[-1]
        x1 = sol1.y[:, -1]
        # The trajectory of x(t) is exp(A1*t) for 0 <= t <= t1
        num_sample1 = 10000
        t1_sample = np.linspace(0, t1, num_sample1)
        cost1_sample = np.empty(num_sample1)
        for i in range(num_sample1):
            cost1_sample[i] = np.linalg.norm(scipy.linalg.expm(
                A1_numpy * t1_sample[i]) @ x0.detach().numpy(),
                                             ord=2)
        A0_numpy = system.A[0].detach().numpy()

        def hit_origin(t, x):
            return np.linalg.norm(x, ord=2) - 1e-4

        hit_origin.terminal = True
        sol2 = solve_ivp(lambda t, x: A0_numpy @ x, (0, np.inf),
                         x1,
                         events=[hit_origin])
        num_sample2 = 10000
        t2_sample = np.linspace(0, sol2.t[-1], num_sample2)
        cost2_sample = np.empty(num_sample2)
        for i in range(num_sample2):
            cost2_sample[i] = np.linalg.norm(
                scipy.linalg.expm(A0_numpy * t2_sample[i]) @ x1, ord=2)
            cost_expected = np.sum(
                (cost1_sample[:-1] + cost1_sample[1:])/2 *
                (t1_sample[1:] - t1_sample[:-1])) +\
                np.sum(
                    (cost2_sample[:-1] + cost2_sample[1:])/2 *
                    (t2_sample[1:] - t2_sample[:-1]))
        # cost_expected is computed with low accuracy (because it uses
        # trapezoidal integration), so only compare up to 4'th decimal place.
        self.assertAlmostEqual(cost.item(), cost_expected, places=4)

        x_equilibrium = torch.tensor([0, 0], dtype=system.dtype)
        cost_inf, x_inf_traj, cost_to_go_inf_traj, sol_inf = \
            hybrid_linear_system.compute_continuous_time_system_cost_to_go(
                system, x0, np.inf,
                lambda x: torch.linalg.norm(x, p=2), x_goal=x_equilibrium)
        self.assertLessEqual(np.linalg.norm(sol_inf.y[:2, -1]), 2e-3)