#include "chain.hpp"

void chain::initialize()
{
	// Initial position and speed of particles
	// ******************************************* //
	n = 30;

	f_spring.resize(n);
	f_weight.resize(n);
	f_damping.resize(n);
	f.resize(n);

	v.resize(n, { 0, 0, 0 });
	p.resize(n);

	for (int i = 0; i < n; i++) p[i] = { 0.0f, 0.35f * i, 4.0f };

	L0 = 0.4f;

	particle_sphere.initialize_data_on_gpu(mesh_primitive_sphere(0.05f));
	segment.display_type = curve_drawable_display_type::Segments;
	segment.initialize_data_on_gpu({ {0,0,0},{1,0,0} });



}

void chain::simulation_step(float dt, vec3 bird_pos)
{
	// Simulation parameters
	float m = 0.01f;       // particle mass
	float K = 5.0f;        // spring stiffness
	float mu = 0.01f;      // damping coefficient

	vec3 g = { 0,0,-9.81f }; // gravity

	// 1st ball tracks bird
	p[0] = bird_pos;

	// Forces
	for (int i = 1; i < n; i++) {
		f_spring[i] = spring_force(p[i], p[i - 1], L0, K);
		f_weight[i] = m * g;
		f_damping[i] = -mu * v[i];
	}
	for (int i = 1; i < n - 1; i++) {
		f[i] = f_spring[i] + f_weight[i] + f_damping[i] - f_spring[i + 1];
	}

	// Numerical Integration
	for (int i = 1; i < n - 1; i++) {
		v[i] = v[i] + dt * f[i] / m;
		p[i] = p[i] + dt * v[i];
	}


}

vec3 chain::spring_force(vec3 const& p_i, vec3 const& p_j, float L0, float K)
{
	vec3 const p = p_i - p_j;
	float const L = norm(p);
	vec3 const u = p / L;

	vec3 const F = -K * (L - L0) * u;
	return F;
}