#include <stddef.h>
#include <raylib.h>
#include <raymath.h>
#include <stdio.h>
#include "objects.h"


struct global {
  const int scwidth, scheight;
  int fps;
  object *objs[128];
  size_t obj_count;
  double terminal_v;
};

struct global gl = {1600, 900, 144, {}, 0, 15};

float min(float a, float b)
{return a>b?b:a;}

float max(float a, float b)
{return a>b?a:b;}

void addobj(object *obj)
{
  gl.objs[gl.obj_count++] = obj;
}

void drawobjs()
{
  for (int i = 0; i < gl.obj_count; i++)
  {
    object *obj = gl.objs[i];
    DrawSphere(obj->pos, obj->r, obj->color);
  }
}

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 updateobjs()
{
  for (int i = 0; i < gl.obj_count; i++)
  {
    object *obj = gl.objs[i];
    obj->vel.y -= 0 * GetFrameTime();
    //if (obj->vel.y < gl.terminal_yv)
    if (obj->pos.y < obj->r) {
      obj->pos.y = obj->r;
      obj->vel.y *= -obj->restitution;
    }
		for (int j = 0; j < gl.obj_count; j++)
		{
			if (i==j) continue;
			object *obj2 = gl.objs[j];
			integrate_g(obj, obj2, gravitation_force(obj, obj2));
			//float min_dist_delta = Vector3Distance(obj->pos, obj2->pos) - (obj->r+obj2->r);
			//if (min_dist_delta < 0)
			//{
			//	Vector3SubtractValue(obj->pos, min_dist_delta);
			//}
		}
		float ratio_speed_to_terminal = Vector3Length(obj->vel)/gl.terminal_v;
		if (Vector3Length(obj->vel) > gl.terminal_v)
    {
			obj->vel = Vector3Scale(obj->vel, 1.0f/ratio_speed_to_terminal);
		}
		DrawLine3D(obj->pos, Vector3Add(obj->pos, obj->vel), GREEN);
		obj->pos = Vector3Add(obj->pos, Vector3Scale(obj->vel, GetFrameTime()));
  } 
}

void freeobjs()
{
  for (int i = 0; i < gl.obj_count; i++)
  {
    object *obj = gl.objs[i];
    free(obj);
  } 
}

int main(void) {

  InitWindow(gl.scwidth, gl.scheight, "raylib engine");

  SetTargetFPS(gl.fps);
  addobj(newobj((Vector3){0, 20, 0}, 3, RED, 1, 50000));
  addobj(newobj((Vector3){4, 20, 0}, 1, BLUE, 1, 1000));
  addobj(newobj((Vector3){-6, 18, 0}, 2, GREEN, 1, 2000));
	gl.objs[1]->vel = (Vector3){0, 0, 2};
  Camera3D camera = {{0, 20, -30}, {0, 0, 0}, {0, 1, 0}, 45, CAMERA_PERSPECTIVE};
	DisableCursor();

  while(!WindowShouldClose())
  {

    UpdateCamera(&camera, CAMERA_FREE);
    BeginDrawing();
    {
      ClearBackground(BLACK);
      BeginMode3D(camera);
      DrawPlane((Vector3){0, 0, 0}, (Vector2){100, 100}, GRAY);
      updateobjs();
      drawobjs();
      EndMode3D();
    }
    EndDrawing();
  }

  freeobjs();

  CloseWindow();

  return 0;

}