#include "physics/particle_contact.hpp"

namespace physics {

  void particle_contact::resolve(float duration)
  {
    resolve_velocity(duration);
    resolve_interpenetration(duration);
  }

  float particle_contact::calculate_separating_velocity() const
  {
    vec3 relative_velocity = particle[0]->velocity;
    if (particle[1])
      relative_velocity -= particle[1]->velocity;

    return dot(relative_velocity, contact_normal);
  }

  void particle_contact::resolve_velocity(float duration)
  {
    float separating_velocity = calculate_separating_velocity();

    if (separating_velocity > 0) {
      // no impulse required
      return;
    }

    float new_separating_velocity = -separating_velocity * restitution;

    // velocity build-up due to acceleration only
    vec3 acceleration_caused_velocity = particle[0]->acceleration;
    if (particle[1]) acceleration_caused_velocity -= particle[1]->acceleration;
    float acceleration_caused_separating_velocity = dot(acceleration_caused_velocity, contact_normal) * duration;

    // remove acceleration velocity from separating velocity
    if (acceleration_caused_separating_velocity < 0) {
      new_separating_velocity += restitution * acceleration_caused_separating_velocity;

      if (new_separating_velocity < 0) new_separating_velocity = 0;
    }

    float delta_velocity = new_separating_velocity - separating_velocity;

    float total_inverse_mass = get_total_inverse_mass();

    if (total_inverse_mass <= 0)
      return;

    float impulse = delta_velocity / total_inverse_mass;

    vec3 impulse_per_inverse_mass = contact_normal * impulse;

    particle[0]->velocity
      = particle[0]->velocity
      + impulse_per_inverse_mass * particle[0]->inverse_mass;

    if (particle[1]) {
      particle[1]->velocity
        = particle[1]->velocity
        + impulse_per_inverse_mass * -particle[1]->inverse_mass;
    }
  }

  void particle_contact::resolve_interpenetration(float duration)
  {
    if (penetration <= 0) return;

    float total_inverse_mass = get_total_inverse_mass();

    if (total_inverse_mass <= 0) return;

    vec3 move_per_inverse_mass = contact_normal * (penetration / total_inverse_mass);

    particle[0]->position += move_per_inverse_mass * particle[0]->inverse_mass;
    if (particle[1]) {
      particle[1]->position += move_per_inverse_mass * -particle[1]->inverse_mass;
    }
  }

  void particle_contact_resolver::resolve_contacts(particle_contact * contacts,
                                                   int contacts_length,
                                                   float duration)
  {
    iterations = 0;

    while (iterations < max_iterations) {
      float max = 0;
      int max_index = -1;
      for (int i = 0; i < contacts_length; i++) {
        float separating_velocity = contacts[i].calculate_separating_velocity();
        if (separating_velocity < max) {
          max = separating_velocity;
          max_index = i;
        }
      }

      if (max_index == -1) break;

      contacts[max_index].resolve(duration);

      iterations += 1;
    }
  }
}