-module(expresso@internal@solver).
-compile([no_auto_import, nowarn_unused_vars, nowarn_unused_function, nowarn_nomatch, inline]).
-define(FILEPATH, "src/expresso/internal/solver.gleam").
-export([solve_position_constraints/3, solve_velocity_constraints/3]).

-if(?OTP_RELEASE >= 27).
-define(MODULEDOC(Str), -moduledoc(Str)).
-define(DOC(Str), -doc(Str)).
-else.
-define(MODULEDOC(Str), -compile([])).
-define(DOC(Str), -compile([])).
-endif.

?MODULEDOC(false).

-file("src/expresso/internal/solver.gleam", 92).
?DOC(false).
-spec apply_contact_correction(
    gleam@dict:dict(MUI, expresso@body:body(MUI)),
    expresso@collision:contact(MUI),
    float()
) -> gleam@dict:dict(MUI, expresso@body:body(MUI)).
apply_contact_correction(Bodies, Contact, Relaxation) ->
    case {gleam_stdlib:map_get(Bodies, erlang:element(2, Contact)),
        gleam_stdlib:map_get(Bodies, erlang:element(3, Contact))} of
        {{ok, Body_a}, {ok, Body_b}} ->
            case erlang:element(16, Body_a) orelse erlang:element(16, Body_b) of
                true ->
                    Bodies;

                false ->
                    Inv_mass_a = expresso@body:inverse_mass(Body_a),
                    Inv_mass_b = expresso@body:inverse_mass(Body_b),
                    Total_inv_mass = Inv_mass_a + Inv_mass_b,
                    case Total_inv_mass < 0.0001 of
                        true ->
                            Bodies;

                        false ->
                            Correction_magnitude = case Total_inv_mass of
                                +0.0 -> +0.0;
                                -0.0 -> -0.0;
                                Gleam@denominator -> erlang:element(5, Contact)
                                * Relaxation
                                / Gleam@denominator
                            end,
                            Correction_a = vec@vec3f:scale(
                                erlang:element(4, Contact),
                                (-1.0 * Correction_magnitude) * Inv_mass_a
                            ),
                            Correction_b = vec@vec3f:scale(
                                erlang:element(4, Contact),
                                Correction_magnitude * Inv_mass_b
                            ),
                            New_body_a = {body,
                                erlang:element(2, Body_a),
                                vec@vec3f:add(
                                    erlang:element(3, Body_a),
                                    Correction_a
                                ),
                                erlang:element(4, Body_a),
                                erlang:element(5, Body_a),
                                erlang:element(6, Body_a),
                                erlang:element(7, Body_a),
                                erlang:element(8, Body_a),
                                erlang:element(9, Body_a),
                                erlang:element(10, Body_a),
                                erlang:element(11, Body_a),
                                erlang:element(12, Body_a),
                                erlang:element(13, Body_a),
                                erlang:element(14, Body_a),
                                erlang:element(15, Body_a),
                                erlang:element(16, Body_a)},
                            New_body_b = {body,
                                erlang:element(2, Body_b),
                                vec@vec3f:add(
                                    erlang:element(3, Body_b),
                                    Correction_b
                                ),
                                erlang:element(4, Body_b),
                                erlang:element(5, Body_b),
                                erlang:element(6, Body_b),
                                erlang:element(7, Body_b),
                                erlang:element(8, Body_b),
                                erlang:element(9, Body_b),
                                erlang:element(10, Body_b),
                                erlang:element(11, Body_b),
                                erlang:element(12, Body_b),
                                erlang:element(13, Body_b),
                                erlang:element(14, Body_b),
                                erlang:element(15, Body_b),
                                erlang:element(16, Body_b)},
                            _pipe = Bodies,
                            _pipe@1 = gleam@dict:insert(
                                _pipe,
                                erlang:element(2, Contact),
                                New_body_a
                            ),
                            gleam@dict:insert(
                                _pipe@1,
                                erlang:element(3, Contact),
                                New_body_b
                            )
                    end
            end;

        {_, _} ->
            Bodies
    end.

-file("src/expresso/internal/solver.gleam", 72).
?DOC(false).
-spec solve_position_constraints(
    gleam@dict:dict(MTZ, expresso@body:body(MTZ)),
    list(expresso@collision:contact(MTZ)),
    integer()
) -> gleam@dict:dict(MTZ, expresso@body:body(MTZ)).
solve_position_constraints(Bodies, Contacts, Iterations) ->
    Relaxation = 0.6,
    _pipe = gleam@list:range(0, Iterations - 1),
    gleam@list:fold(
        _pipe,
        Bodies,
        fun(Current_bodies, _) ->
            gleam@list:fold(
                Contacts,
                Current_bodies,
                fun(Bodies_acc, Contact) ->
                    apply_contact_correction(Bodies_acc, Contact, Relaxation)
                end
            )
        end
    ).

-file("src/expresso/internal/solver.gleam", 271).
?DOC(false).
-spec apply_torque(expresso@body:body(any()), vec@vec3:vec3(float()), float()) -> vec@vec3:vec3(float()).
apply_torque(Body_val, Torque, Inverse_mass) ->
    case Inverse_mass < 0.0001 of
        true ->
            {vec3, +0.0, +0.0, +0.0};

        false ->
            Mass = case Inverse_mass of
                +0.0 -> +0.0;
                -0.0 -> -0.0;
                Gleam@denominator -> 1.0 / Gleam@denominator
            end,
            Radius = expresso@body:bounding_radius(Body_val),
            Moment_of_inertia = ((0.4 * Mass) * Radius) * Radius,
            case Moment_of_inertia < 0.0001 of
                true ->
                    {vec3, +0.0, +0.0, +0.0};

                false ->
                    vec@vec3f:scale(Torque, case Moment_of_inertia of
                            +0.0 -> +0.0;
                            -0.0 -> -0.0;
                            Gleam@denominator@1 -> 1.0 / Gleam@denominator@1
                        end)
            end
    end.

-file("src/expresso/internal/solver.gleam", 300).
?DOC(false).
-spec apply_friction_impulse(
    expresso@body:body(MVL),
    expresso@body:body(MVL),
    expresso@collision:contact(MVL),
    float()
) -> {expresso@body:body(MVL), expresso@body:body(MVL)}.
apply_friction_impulse(Body_a, Body_b, Contact, Normal_impulse_magnitude) ->
    Relative_velocity = vec@vec3f:subtract(
        erlang:element(4, Body_b),
        erlang:element(4, Body_a)
    ),
    Velocity_along_normal = vec@vec3f:dot(
        Relative_velocity,
        erlang:element(4, Contact)
    ),
    Normal_velocity = vec@vec3f:scale(
        erlang:element(4, Contact),
        Velocity_along_normal
    ),
    Tangential_velocity = vec@vec3f:subtract(Relative_velocity, Normal_velocity),
    Tangential_speed = vec@vec3f:length(Tangential_velocity),
    case Tangential_speed < 0.001 of
        true ->
            {Body_a, Body_b};

        false ->
            Tangent = vec@vec3f:normalize(Tangential_velocity),
            Static_friction = (erlang:element(11, Body_a) + erlang:element(
                11,
                Body_b
            ))
            / 2.0,
            Dynamic_friction = (erlang:element(12, Body_a) + erlang:element(
                12,
                Body_b
            ))
            / 2.0,
            Inv_mass_a = expresso@body:inverse_mass(Body_a),
            Inv_mass_b = expresso@body:inverse_mass(Body_b),
            Total_inv_mass = Inv_mass_a + Inv_mass_b,
            case Total_inv_mass < 0.0001 of
                true ->
                    {Body_a, Body_b};

                false ->
                    Static_impulse_magnitude = case Total_inv_mass of
                        +0.0 -> +0.0;
                        -0.0 -> -0.0;
                        Gleam@denominator -> Tangential_speed / Gleam@denominator
                    end,
                    Dynamic_impulse_magnitude = Dynamic_friction * Normal_impulse_magnitude,
                    Friction_impulse_magnitude = case Static_impulse_magnitude < (Static_friction
                    * Normal_impulse_magnitude) of
                        true ->
                            Static_impulse_magnitude;

                        false ->
                            Dynamic_impulse_magnitude
                    end,
                    Friction_impulse = vec@vec3f:scale(
                        Tangent,
                        -1.0 * Friction_impulse_magnitude
                    ),
                    New_body_a = {body,
                        erlang:element(2, Body_a),
                        erlang:element(3, Body_a),
                        vec@vec3f:add(
                            erlang:element(4, Body_a),
                            vec@vec3f:scale(Friction_impulse, -1.0 * Inv_mass_a)
                        ),
                        erlang:element(5, Body_a),
                        erlang:element(6, Body_a),
                        erlang:element(7, Body_a),
                        erlang:element(8, Body_a),
                        erlang:element(9, Body_a),
                        erlang:element(10, Body_a),
                        erlang:element(11, Body_a),
                        erlang:element(12, Body_a),
                        erlang:element(13, Body_a),
                        erlang:element(14, Body_a),
                        erlang:element(15, Body_a),
                        erlang:element(16, Body_a)},
                    New_body_b = {body,
                        erlang:element(2, Body_b),
                        erlang:element(3, Body_b),
                        vec@vec3f:add(
                            erlang:element(4, Body_b),
                            vec@vec3f:scale(Friction_impulse, Inv_mass_b)
                        ),
                        erlang:element(5, Body_b),
                        erlang:element(6, Body_b),
                        erlang:element(7, Body_b),
                        erlang:element(8, Body_b),
                        erlang:element(9, Body_b),
                        erlang:element(10, Body_b),
                        erlang:element(11, Body_b),
                        erlang:element(12, Body_b),
                        erlang:element(13, Body_b),
                        erlang:element(14, Body_b),
                        erlang:element(15, Body_b),
                        erlang:element(16, Body_b)},
                    {New_body_a, New_body_b}
            end
    end.

-file("src/expresso/internal/solver.gleam", 167).
?DOC(false).
-spec apply_velocity_impulse(
    gleam@dict:dict(MUZ, expresso@body:body(MUZ)),
    expresso@collision:contact(MUZ),
    float()
) -> gleam@dict:dict(MUZ, expresso@body:body(MUZ)).
apply_velocity_impulse(Bodies, Contact, Restitution) ->
    case {gleam_stdlib:map_get(Bodies, erlang:element(2, Contact)),
        gleam_stdlib:map_get(Bodies, erlang:element(3, Contact))} of
        {{ok, Body_a}, {ok, Body_b}} ->
            case erlang:element(16, Body_a) orelse erlang:element(16, Body_b) of
                true ->
                    Bodies;

                false ->
                    Relative_velocity = vec@vec3f:subtract(
                        erlang:element(4, Body_b),
                        erlang:element(4, Body_a)
                    ),
                    Velocity_along_normal = vec@vec3f:dot(
                        Relative_velocity,
                        erlang:element(4, Contact)
                    ),
                    case Velocity_along_normal >= +0.0 of
                        true ->
                            Bodies;

                        false ->
                            Inv_mass_a = expresso@body:inverse_mass(Body_a),
                            Inv_mass_b = expresso@body:inverse_mass(Body_b),
                            Total_inv_mass = Inv_mass_a + Inv_mass_b,
                            case Total_inv_mass < 0.0001 of
                                true ->
                                    Bodies;

                                false ->
                                    Impulse_magnitude = case Total_inv_mass of
                                        +0.0 -> +0.0;
                                        -0.0 -> -0.0;
                                        Gleam@denominator -> (-1.0 * (1.0 + Restitution))
                                        * Velocity_along_normal
                                        / Gleam@denominator
                                    end,
                                    Impulse = vec@vec3f:scale(
                                        erlang:element(4, Contact),
                                        Impulse_magnitude
                                    ),
                                    R_a = vec@vec3f:subtract(
                                        erlang:element(6, Contact),
                                        erlang:element(3, Body_a)
                                    ),
                                    Torque_a = vec@vec3f:cross(
                                        R_a,
                                        vec@vec3f:scale(Impulse, -1.0)
                                    ),
                                    Angular_impulse_a = apply_torque(
                                        Body_a,
                                        Torque_a,
                                        Inv_mass_a
                                    ),
                                    R_b = vec@vec3f:subtract(
                                        erlang:element(6, Contact),
                                        erlang:element(3, Body_b)
                                    ),
                                    Torque_b = vec@vec3f:cross(R_b, Impulse),
                                    Angular_impulse_b = apply_torque(
                                        Body_b,
                                        Torque_b,
                                        Inv_mass_b
                                    ),
                                    Body_a_after_normal = {body,
                                        erlang:element(2, Body_a),
                                        erlang:element(3, Body_a),
                                        vec@vec3f:add(
                                            erlang:element(4, Body_a),
                                            vec@vec3f:scale(
                                                Impulse,
                                                -1.0 * Inv_mass_a
                                            )
                                        ),
                                        erlang:element(5, Body_a),
                                        vec@vec3f:add(
                                            erlang:element(6, Body_a),
                                            Angular_impulse_a
                                        ),
                                        erlang:element(7, Body_a),
                                        erlang:element(8, Body_a),
                                        erlang:element(9, Body_a),
                                        erlang:element(10, Body_a),
                                        erlang:element(11, Body_a),
                                        erlang:element(12, Body_a),
                                        erlang:element(13, Body_a),
                                        erlang:element(14, Body_a),
                                        erlang:element(15, Body_a),
                                        erlang:element(16, Body_a)},
                                    Body_b_after_normal = {body,
                                        erlang:element(2, Body_b),
                                        erlang:element(3, Body_b),
                                        vec@vec3f:add(
                                            erlang:element(4, Body_b),
                                            vec@vec3f:scale(Impulse, Inv_mass_b)
                                        ),
                                        erlang:element(5, Body_b),
                                        vec@vec3f:add(
                                            erlang:element(6, Body_b),
                                            Angular_impulse_b
                                        ),
                                        erlang:element(7, Body_b),
                                        erlang:element(8, Body_b),
                                        erlang:element(9, Body_b),
                                        erlang:element(10, Body_b),
                                        erlang:element(11, Body_b),
                                        erlang:element(12, Body_b),
                                        erlang:element(13, Body_b),
                                        erlang:element(14, Body_b),
                                        erlang:element(15, Body_b),
                                        erlang:element(16, Body_b)},
                                    {New_body_a, New_body_b} = apply_friction_impulse(
                                        Body_a_after_normal,
                                        Body_b_after_normal,
                                        Contact,
                                        Impulse_magnitude
                                    ),
                                    _pipe = Bodies,
                                    _pipe@1 = gleam@dict:insert(
                                        _pipe,
                                        erlang:element(2, Contact),
                                        New_body_a
                                    ),
                                    gleam@dict:insert(
                                        _pipe@1,
                                        erlang:element(3, Contact),
                                        New_body_b
                                    )
                            end
                    end
            end;

        {_, _} ->
            Bodies
    end.

-file("src/expresso/internal/solver.gleam", 156).
?DOC(false).
-spec solve_velocity_constraints(
    gleam@dict:dict(MUQ, expresso@body:body(MUQ)),
    list(expresso@collision:contact(MUQ)),
    float()
) -> gleam@dict:dict(MUQ, expresso@body:body(MUQ)).
solve_velocity_constraints(Bodies, Contacts, Restitution) ->
    gleam@list:fold(
        Contacts,
        Bodies,
        fun(Bodies_acc, Contact) ->
            apply_velocity_impulse(Bodies_acc, Contact, Restitution)
        end
    ).