struct Impulse {
    Vector2 force = { 0, 0 };
    Vector2 pointOfApplication = { 0, 0 };
    float32 timeOfApplicationSeconds = 0.25f;
    float32 timeAppliedSeconds = 0.f;
    bool isDead = false;
};

const int32 NUM_IMPULSES = 4;

struct Rigidbody {
    int32 numImpulses = 0;
    Impulse activeImpulses[NUM_IMPULSES];
    Vector2 velocity = { 0, 0 };
    Vector2 position = { 0, 0 };
    float32 mass = 1.f;

	float32 rotationalVelocity = 0.f;
	float32 rotation = 0.f;
	float32 momentOfInertia = 1.f;

    void reset() {
        numImpulses = 0;
        velocity = { 0, 0 };
        rotationalVelocity = 0.f;
        rotation = 0.f;
    }

    void applyImpulse(Impulse i) {
        if (numImpulses > NUM_IMPULSES) {
            printf("Unable to apply impulse. Buffer full.\n");
            return;
        }

        activeImpulses[numImpulses] = i;
        numImpulses++;
    }

    void applyGravity(float32 deltaTimeSeconds) {
        velocity += (Vector2 { 0.f, -9.8f } * deltaTimeSeconds);
    }

    void update(float32 deltaTimeSeconds) {
        applyGravity(deltaTimeSeconds);

        Vector2 force;
        float32 torque = 0.f;
        for (int32 idx = 0; idx < numImpulses; idx++) {
            Impulse& i = activeImpulses[idx];

            float32 nextTimeAppliedSeconds = i.timeAppliedSeconds + deltaTimeSeconds;
            if (nextTimeAppliedSeconds >= i.timeOfApplicationSeconds) {
                nextTimeAppliedSeconds = i.timeOfApplicationSeconds;
                i.isDead = true;
            }
            
            float32 impulseDtSeconds = nextTimeAppliedSeconds - i.timeAppliedSeconds;
            Vector2 forceToApply = i.force * (impulseDtSeconds / i.timeOfApplicationSeconds);
            force += forceToApply * impulseDtSeconds;

            // New! Increment the torque for each force
            torque += i.pointOfApplication.getPerp().dot(forceToApply);

            i.timeAppliedSeconds = nextTimeAppliedSeconds;
        }
        
        Vector2 acceleration = force / mass;
        velocity += (acceleration * deltaTimeSeconds);
        position += (velocity * deltaTimeSeconds);

        // New! Update the rotational velocity as well
        float32 rotationalAcceleration = torque / momentOfInertia;
        rotationalVelocity += (rotationalAcceleration * deltaTimeSeconds);
        rotation += (rotationalVelocity * deltaTimeSeconds);

        for (int32 idx = 0; idx < numImpulses; idx++) {
            if (activeImpulses[idx].isDead) {
                for (int j = idx + 1; j < numImpulses; j++) {
                    activeImpulses[j - 1] = activeImpulses[j];
                }

                idx = idx - 1;
                numImpulses--;
            }
        }
    }
};