#ifndef PARTICLE_H
#define PARTICLE_H

#include <math.h>
#include <pthread.h>
#include <stdatomic.h>
#include <stdlib.h>
#include <unistd.h>

#define G_CONSTANT 0.65f

typedef struct {
	float x, y, z;
} Vec3;

typedef struct {
	float r, g, b, a;
} Color;

typedef struct {
	Vec3 position;
	Vec3 velocity;
	Color color;

	float dampening;
	float life;
	float radius;
} Particle;

typedef struct {
	Particle *particle;
	unsigned long long thread_id;
	unsigned long long interval_start;
	unsigned long long interval_end;
	int alive_count;
	float time;

	pthread_mutex_t *mutex;
	pthread_cond_t *cond;
	pthread_barrier_t *barrier;
	volatile int run;
	atomic_int exit;
} Physics_params;

// INFO: This adds adder to out, basicaly out + adder
void vec_add(Vec3 &out, Vec3 adder) {
	out.x += adder.x;
	out.y += adder.y;
	out.z += adder.z;
}

float vec_length(Vec3 v) {
	return sqrtf(v.x * v.x + v.y * v.y + v.z * v.z);
}

float vec_dot(Vec3 a, Vec3 b) {
	return a.x * b.x + a.y * b.y + a.z * b.z;
}

Particle particle_init(Vec3 position, Color color) {
	Particle p;
	p.position = position;
	p.color = color;
	p.velocity = {0.0f, 0.0f, 0.0f};

	p.life = -2.0f; // INFO: Imorta
	p.dampening = 0.9f;
	p.radius = 0.05f;
	return p;
}

int particle_update(Particle *particles, unsigned long long size, float time) {
	int alive_count = 0;
	for (unsigned long long i = 0; i < size; i++) {
		Particle p = particles[i];
		if (p.life == 0) {
			continue;
		}

		// p.velocity.y -= (G_CONSTANT * time);

		p.position.x += p.velocity.x * time;
		p.position.y += p.velocity.y * time;
		p.position.z += p.velocity.z * time;

		if (p.position.y <= 0) {
			p.position.y = 0;
			p.velocity.y *= (-1 * p.dampening);
		}

		if (p.life > 0)
			p.life -= time;
		if (p.life < 0 && p.life >= -1.0f)
			p.life = 0;

		particles[i] = p;
		alive_count++;
	}

	return alive_count;
}

void collision_update(Particle *particles, unsigned long long count, float restitution) {
	for (unsigned long long i = 0; i < count; i++) {
		for (unsigned long long j = i + 1; j < count; j++) {
			Particle &p1 = particles[i];
			Particle &p2 = particles[j];

			if (p1.life == 0 || p2.life == 0) {
				continue;
			}

			Vec3 diff = {p2.position.x - p1.position.x, p2.position.y - p1.position.y, p2.position.z - p1.position.z};
			float dist = vec_length(diff);

			float overlap = p1.radius + p2.radius - dist;
			Vec3 normal;
			if (overlap > 0 && dist > 0.00001f) {
				normal = {diff.x / dist, diff.y / dist, diff.z / dist};
				p1.position.x -= normal.x * overlap * 0.5f;
				p1.position.y -= normal.y * overlap * 0.5f;
				p1.position.z -= normal.z * overlap * 0.5f;

				p2.position.x += normal.x * overlap * 0.5f;
				p2.position.y += normal.y * overlap * 0.5f;
				p2.position.z += normal.z * overlap * 0.5f;

				float velocity_relative_x = p1.velocity.x - p2.velocity.x;
				float velocity_relative_y = p1.velocity.y - p2.velocity.y;
				float velocity_relative_z = p1.velocity.z - p2.velocity.z;

                float v_rel = vec_dot({velocity_relative_x, velocity_relative_y, velocity_relative_z}, normal);
				if (v_rel < 0) {
                    float impulse = -(1 + restitution) * v_rel / 2.0f;

					p1.velocity.x -= impulse * normal.x;
					p1.velocity.y -= impulse * normal.y;
					p1.velocity.z -= impulse * normal.z;

					p2.velocity.x += impulse * normal.x;
					p2.velocity.y += impulse * normal.y;
					p2.velocity.z += impulse * normal.z;
				}
			}
		}
	}
}

void *t_update(void *args) {
	Physics_params *params = (Physics_params *)args;
	while (params->exit != 1) {
		pthread_mutex_lock(params->mutex);
		while (params->run == 0) {
			pthread_cond_wait(params->cond, params->mutex); // sleeps here
		}
		pthread_mutex_unlock(params->mutex);

		params->alive_count = 0;
		for (unsigned long long i = params->interval_start; i < params->interval_end; i++) {
			Particle p = params->particle[i];

			if (p.life == 0) {
				continue;
			}

			p.velocity.y -= (G_CONSTANT * params->time);

			p.position.x += p.velocity.x * params->time;
			p.position.y += p.velocity.y * params->time;
			p.position.z += p.velocity.z * params->time;

			if (p.position.y <= 0) {
				p.position.y = 0;
				p.velocity.y *= (-1 * p.dampening);
			}

			if (p.life > 0)
				p.life -= params->time;
			if (p.life < 0 && p.life >= -1.0f)
				p.life = 0;

			// DEBUG
			/*
			if (params->thread_id == 0) {
				p.color = {1.0f, 1.0f, 1.0f, 1.0f};
			} else if (params->thread_id == 1) {
				p.color = {1.0f, 0.0f, 0.0f, 1.0f};
			} else if (params->thread_id == 2) {
				p.color = {0.0f, 1.0f, 0.0f, 1.0f};
			} else if (params->thread_id == 3) {
				p.color = {0.0f, 0.0f, 1.0f, 1.0f};
			} else {
				p.color = {0.7f, 0.7f, 0.7f, 1.0f};
				;
			}
			*/

			params->particle[i] = p;
			params->alive_count++;
		}
		params->run = 0;
		pthread_barrier_wait(params->barrier);
	}

	return NULL;
}

#endif // PARTICLE_H