const drone = @import("./drone.zig");
const vec3 = @import("./vec3.zig");
const forces = @import("./forces.zig");

pub fn timestep(dt: f64, d: drone.Drone, props: []const drone.Propeller, s: *drone.State) void {
    var force = vec3.Vec3F64.init(0, 0, 0);
    var moment = vec3.Vec3F64.init(0, 0, 0);

    for (props) |prop| {
        const prop_force, const prop_moment = forces.propThrust(s.attitude, prop);
        force = vec3.vec3Add(force, prop_force);
        moment = vec3.vec3Add(moment, prop_moment);
    }

    force = vec3.vec3Add(force, forces.drag(drone.drag_coeff, s.v));
    force = vec3.vec3Add(force, forces.gravity(d.mass_kg));

    const accel = vec3.vec3Scale(force, 1.0 / d.mass_kg);
    s.v = vec3.vec3Add(s.v, vec3.vec3Scale(accel, dt));
    s.x_cg = vec3.vec3Add(s.x_cg, vec3.vec3Scale(accel, dt));

    const I_omega = vec3.vec3MulMat3(d.moment_of_inertia, s.omega);
    const omega_dot = vec3.vec3MulMat3(
        vec3.mat3Inv(d.moment_of_inertia),
        vec3.vec3Sub(moment, vec3.vec3Cross(s.omega, I_omega)),
    );

    s.omega = vec3.vec3Add(s.omega, vec3.vec3Scale(omega_dot, dt));
    const half_omege_quat: vec3.QuatF64 = .init(
        s.omega.x() * 0.5,
        s.omega.y() * 0.5,
        s.omega.z() * 0.5,
        0,
    );

    const qdot = vec3.quatMul(half_omege_quat, s.q);
    s.q = vec3.quatAdd(s.q, vec3.quatScale(qdot, dt));
    return;
}