#include "objects.h"
#include <math.h>
#include <raymath.h>
#include <stdio.h>

#define min(a, b) (a>b?b:a);
#define max(a, b) (a>b?a:b);

object *newobj(Vector3 pos, double r, Color c, double rest, double m)
{
  object *obj = (object*)malloc(sizeof(object));
  obj->r = r;
  obj->pos = pos;
  obj->color = c;
  obj->vel = (Vector3){0, 0, 0};
  obj->restitution = rest;
	obj->mass = m;
  return obj;
}

Vector3 gravitation_force(object *obj1, object *obj2)
{
	const double GRAVITY_FACTOR = 1;
	Vector3 force;
	double dist, mass, total_g;
	mass = obj1->mass * obj2->mass;
	force = Vector3Subtract(obj2->pos, obj1->pos);

	dist = Vector3Length(force);
	force = Vector3Normalize(force);

	dist = max(dist, obj1->r+obj2->r);

	total_g = GRAVITY_FACTOR * mass / (dist * dist);
	force = Vector3Scale(force, total_g);

	return force;
}

void integrate_g(object *obj1, object *obj2, Vector3 force)
{
	Vector3 acc;
	double f, a;

	f = Vector3Length(force);
	//acc = Vector3Scale(Vector3Subtract(obj1->pos, obj2->pos), f);
	acc = force;

	a = f / obj1->mass;
	acc = Vector3Scale(Vector3Normalize(acc), a*GetFrameTime());
	obj1->vel = Vector3Add(obj1->vel, acc);

	a = f / obj2->mass;
	acc = Vector3Scale(Vector3Normalize(acc), a*GetFrameTime());
	obj2->vel = Vector3Add(obj2->vel, acc);
}

void resolve_collision(object *obj1, object *obj2)
{
	Vector3 normal = Vector3Subtract(obj1->pos, obj2->pos);
	float distsq = Vector3LengthSqr(normal);
	float sumrad = obj1->r+obj2->r;
	float mindistsq = sumrad*sumrad;

	if (distsq <= mindistsq) {
		float dist = sqrtf(distsq);

		if (dist == 0) {
			normal = Vector3RotateByAxisAngle(Vector3One(), (Vector3){0, 1, 1}, rand()*360);
			dist = sumrad;
		} else {
			normal = Vector3Scale(normal, 1.0f / dist);
		}

		float overlap = sumrad - dist;
		Vector3 separation = Vector3Scale(normal, overlap / 2.0f);
		obj1->pos = Vector3Add(obj1->pos, separation);
		obj2->pos = Vector3Subtract(obj2->pos, separation);

	} else {
		normal = Vector3Normalize(normal);
	}

	float vel1_normal = Vector3DotProduct(obj1->vel, normal);
	float vel2_normal = Vector3DotProduct(obj2->vel, normal);

	if (vel1_normal - vel2_normal > 0) return;

	float totalmass = obj1->mass + obj2->mass;
	float vel1_normal_final = (obj1->mass * vel1_normal + obj2->mass * vel2_normal - obj2->mass * (vel1_normal - vel2_normal)) * obj1->restitution / totalmass;
	float vel2_normal_final = (obj1->mass * vel1_normal + obj2->mass * vel2_normal - obj2->mass * (vel2_normal - vel1_normal)) * obj2->restitution / totalmass;

	Vector3 vel1_tang = Vector3Subtract(obj1->vel, Vector3Scale(normal, vel1_normal));
	Vector3 vel2_tang = Vector3Subtract(obj2->vel, Vector3Scale(normal, vel2_normal));

	obj1->vel = Vector3Add(Vector3Scale(normal, vel1_normal_final), vel1_tang);
	obj2->vel = Vector3Add(Vector3Scale(normal, vel2_normal_final), vel2_tang);

}