#include "gravity.h"
#include "consts.h"
#include "vec_utils.h"
#include <raylib.h>

void gravity(universe_t *uni) {
  mass_body_t body_1;
  mass_body_t body_2;
  for (unsigned int i = 0; i < uni->num_bodies - 1; i++) {
    body_1 = uni->bodies[i];
    for (unsigned int y = i + 1; y < uni->num_bodies; y++) {
      body_2 = uni->bodies[y];
      float force =
          simx * NEWTON_G * body_1.mass * body_2.mass /
          (vec2_len(vec2_diff(body_2.position, body_1.position)) * TIMESTEP);
      Vector2 acceleration =
          vec2_normalize(vec2_diff(body_2.position, body_1.position));
      Vector2 acceleration_1 = vec2_scale(acceleration, force / body_1.mass);
      Vector2 acceleration_2 = vec2_scale(acceleration, -force / body_2.mass);
      body_1.speed = vec2_sum(body_1.speed, acceleration_1);
      body_2.speed = vec2_sum(body_1.speed, acceleration_2);
      uni->bodies[y] = body_2;
    }
    uni->bodies[i] = body_1;
  }
}