import numpy as np
from rigid_body_models import boat
from state_space_models import simple_planar_integrator as ssm
from controls import time_based
from controls import stepping
from vis import planar_thruster_model


def main():

    DT = 0.1
    boat_model = boat.DiamondMountedThrusters()
    cts_state_model = ssm.NoDragCts(boat_model)
    cts_state_model = ssm.AccelApproxDragCts(cts_state_model, 0.6)
    discrete_state_model = ssm.ZeroOrderHoldDiscrete(cts_state_model, DT)
    x0 = np.array([0., 0, 0, 0, np.pi/2, 0])
    u_fcn = time_based.make_control_signal(
        [1, 5, 6],
        np.array([
            [.2, .2, .2, 1.2],
            [0, 0, 0, 0],
            [.6, 0, 0, .6],
        ]).T
    )
    T = 10
    x, u = stepping.solve_discrete_difference_eqns(
        discrete_state_model, x0, u_fcn, DT, T)
    np.set_printoptions(suppress=True)
    print(x.T.round(2))
    planar_thruster_model.loop_sim(
        boat_model, x.T[:, [0, 1]], x.T[:, 4], u.T, DT)


if __name__ == '__main__':
    main()