#include "mechanics.h"
#include "objects.h"
#include <stdio.h>

Vector2 body_collision(mass_body_t *a, mass_body_t *b) {
  Vector2 diff = vec2_diff(a->position, b->position);
  float len = vec2_len(diff);
  if (len > a->radius + b->radius) {
    return NULL_VEC2;
  }
  if (len == 0) {
    return (Vector2){1, 0};
  }
  Vector2 result = vec2_normalize(diff);
  return result;
}

void collide(mass_body_t *a, mass_body_t *b, const Vector2 normal,
             float resitution) {
  float inverse_a = 1.0f / a->mass;
  float inverse_b = 1.0f / b->mass;
  float inverse_total = inverse_a + inverse_b;
  float overlap =
      a->radius + b->radius - vec2_len(vec2_diff(a->position, b->position));
  Vector2 correction = vec2_scale(normal, overlap / inverse_total);
  Vector2 a_move = vec2_scale(correction, inverse_a);
  Vector2 b_move = vec2_scale(correction, inverse_b);
  a->position = vec2_sum(a->position, a_move);
  b->position = vec2_diff(b->position, b_move);

  Vector2 relative = vec2_diff(b->speed, a->speed);
  float v_normal = vec2_dot(relative, normal);
  if (v_normal <= 0) {
    return;
  }

  float factor = -(1 + resitution);
  float j = factor * v_normal;

  j /= inverse_total;

  Vector2 final = vec2_scale(normal, j);

  Vector2 a_diff = vec2_scale(final, inverse_a);
  a->speed = vec2_diff(a->speed, a_diff);
  Vector2 b_diff = vec2_scale(final, inverse_b);
  b->speed = vec2_sum(b->speed, b_diff);
}

void collide_rigid(mass_body_t *a, float restitution) {
  float max_x = GetRenderWidth() * 10 / 2.0f - a->radius;
  float max_y = GetRenderHeight() * 10 / 2.0f - a->radius;

  if (a->position.x > max_x) {
    a->position.x = max_x;
    if (a->speed.x > 0)
      a->speed.x *= -restitution;
  } else if (a->position.x < -max_x) {
    a->position.x = -max_x;
    if (a->speed.x < 0)
      a->speed.x *= -restitution;
  }

  if (a->position.y > max_y) {
    a->position.y = max_y;
    if (a->speed.y > 0)
      a->speed.y *= -restitution;
  } else if (a->position.y < -max_y) {
    a->position.y = -max_y;
    if (a->speed.y < 0)
      a->speed.y *= -restitution;
  }
}

void mechanics(universe_t *uni) {
  mass_body_t body_1;
  mass_body_t body_2;
  Vector2 collision;
  Vector2 scaled_speed;
  for (unsigned int i = 0; i < uni->num_bodies; i++) {
    body_1 = uni->bodies[i];
    scaled_speed = vec2_scale(body_1.speed, 1.0 / simx);
    body_1.position = vec2_sum(body_1.position, scaled_speed);
    if (i < uni->num_bodies - 1) {
      for (unsigned int y = i + 1; y < uni->num_bodies; y++) {
        body_2 = uni->bodies[y];
        collision = body_collision(&body_1, &body_2);
        if (!vec2_is_null(collision)) {
          collide(&body_1, &body_2, collision, 1.0);
        }
        uni->bodies[y] = body_2;
      }
    }
    collide_rigid(&body_1, 1.1);
    uni->bodies[i] = body_1;
  }
}