#include "raylib.h"
#include "raymath.h"
#include "rlgl.h"
#include <cmath>
#include <optional>

#include "boid-playground.hpp"
#include "raycast.cpp"

static void boid_rand_init(Boid *boid, int min_x, int max_x, int min_y, int max_y)
{
	boid->pos.x = GetRandomValue(min_x, max_x);
	boid->pos.y = GetRandomValue(min_y, max_y);

	float facing = GetRandomValue(0, 2*PI);
	boid->dir = Vector2Rotate({ 1, 0 }, facing);
}

static Vector2 get_center_point(std::vector<Vector2> &points)
{
	Vector2 center = { 0, 0 };
	for (int i = 0; i < points.size(); i++) {
		center.x += points[i].x;
		center.y += points[i].y;
	}
	center.x /= points.size();
	center.y /= points.size();
	return center;
}

static void draw_obstacle(Obstacle *obstacle, Color color)
{
	std::vector<Vector2> *points = &obstacle->points;
	int point_count = points->size();

	rlBegin(RL_TRIANGLES);
	{
		rlColor4ub(color.r, color.g, color.b, color.a);
		for (int j = 0; j < point_count-1; j++) {
			Vector2 *point1 = &(*points)[j];
			Vector2 *point2 = &(*points)[j+1];
			rlVertex2f(point1->x, point1->y);
			rlVertex2f(obstacle->center.x, obstacle->center.y);
			rlVertex2f(point2->x, point2->y);
		}

		rlVertex2f((*points)[point_count-1].x, (*points)[point_count-1].y);
		rlVertex2f(obstacle->center.x, obstacle->center.y);
		rlVertex2f((*points)[0].x, (*points)[0].y);
	}
	rlEnd();
}

static void draw_debug_boid_obstacle_avoidance(Visuals *visuals, World *world, Boid *boid) {
	Vector2 pos = boid->pos;

	int ray_count = world->avoidance_ray_count * 2 + 1;
	float ray_angles[ray_count];
	fill_avoidance_ray_angles(ray_angles, ray_count, world->avoidance_ray_angle);

	float facing = std::atan2(boid->dir.y, boid->dir.x);
	for (int i = 0; i < ray_count; i++) {
		Vector2 ray_dir = {
			std::cos(facing + ray_angles[i]),
			std::sin(facing + ray_angles[i])
		};

		RayHitResult hit_result;
		get_intersect_with_world(&hit_result, pos, ray_dir, world);
		bool hit_obstacle = (hit_result.hit != -1 && hit_result.hit <= world->avoidance_distance);

		Color ray_color = GREEN;
		float ray_length = world->avoidance_distance;
		if (hit_obstacle) {
			ray_length = hit_result.hit;
			ray_color = BLUE;
		}

		Vector2 hit_pos = Vector2Add(pos, Vector2Multiply(ray_dir, { ray_length, ray_length }));
		DrawLine(pos.x, pos.y, hit_pos.x, hit_pos.y, ray_color);
		if (hit_obstacle) {
			DrawCircle(hit_pos.x, hit_pos.y, visuals->boid_edge_size * 0.05, ray_color);
		}
	}
}

static void try_avoiding_obstacles(World *world, Boid *boid, float dt) {
	int ray_count = world->avoidance_ray_count * 2 + 1;
	float ray_angles[ray_count];
	fill_avoidance_ray_angles(ray_angles, ray_count, world->avoidance_ray_angle);

	float facing = std::atan2(boid->dir.y, boid->dir.x);

	bool got_hit = false;
	RayHitResult hit_results[ray_count];
	int best_avoidance = 0;
	for (int i = 0; i < ray_count; i++) {
		Vector2 ray_dir = {
			std::cos(facing + ray_angles[i]),
			std::sin(facing + ray_angles[i])
		};

		get_intersect_with_world(&hit_results[i], boid->pos, ray_dir, world);
		if (hit_results[i].hit != -1 && hit_results[i].hit <= world->avoidance_distance) {
			got_hit = true;
		}

		if (hit_results[i].hit > hit_results[best_avoidance].hit) {
			best_avoidance = i;
		}
	}

	if (got_hit) {
		float turn_angle = ray_angles[best_avoidance];
		boid->dir = Vector2Rotate(boid->dir, turn_angle * world->boid_turn_speed * dt);
	}
}

static int count_out_of_bounds_boids(World *world) {
	int count = 0;
	for (int i = 0; i < world->boids.size(); i++) {
		Vector2 *pos = &world->boids[i].pos;

		bool x_out_of_bounds = (pos->x <= 0 || pos->x >= world->size.x);
		bool y_out_of_bounds = (pos->y <= 0 || pos->y >= world->size.y);
		if (x_out_of_bounds || y_out_of_bounds)  {
			count++;
		}
	}
	return count;
}

static void draw_circle_sector(Vector2 center, float radius, float start_angle, float end_angle, int segments, Color color) {
	rlBegin(RL_TRIANGLES);
	float angle_step = (end_angle - start_angle) / segments;
	for (int i = 0; i < segments; i++)
	{
		rlColor4ub(color.r, color.g, color.b, color.a);
		float angle = start_angle + i * angle_step;
		float nextAngle = start_angle + (i+1) * angle_step;

		rlVertex2f(center.x, center.y);
		rlVertex2f(center.x + cosf(nextAngle)*radius, center.y + sinf(nextAngle)*radius);
		rlVertex2f(center.x + cosf(angle)    *radius, center.y + sinf(angle)    *radius);
	}
	rlEnd();
}

static int get_boids_in_view_cone(Boid **boids_in_view, Boid *boid, float view_radius, float view_angle, Boid *boids, int boid_count) {
	int count = 0;
	float dot_threshold = Vector2DotProduct(boid->dir, Vector2Rotate(boid->dir, view_angle/2));
	for (int i = 0; i < boid_count; i++) {
		if (&boids[i] == boid) continue;

		Vector2 dir_to_boid = Vector2Normalize(Vector2Subtract(boids[i].pos, boid->pos));
		float dot = Vector2DotProduct(boid->dir, dir_to_boid);
		if (dot >= dot_threshold && Vector2Distance(boids[i].pos, boid->pos) <= view_radius) {
			boids_in_view[count] = &boids[i];
			count++;
		}
	}
	return count;
}

static float vector2_atan2(Vector2 a)
{
	return std::atan2(a.x, a.y);
}

int main() {
	SetTraceLogLevel(LOG_TRACE);

    int screen_width = 1280;
    int screen_height = 720;

    raylib::Color text_color(LIGHTGRAY);
    raylib::Window window(screen_width, screen_height, "Boid Playground");

    SetTargetFPS(60);

	World world;
	world.size = { (float)screen_width, (float)screen_height };
	Visuals visuals;

	float border = visuals.boid_edge_size;
	for (int i = 0; i < 10; i++) {
		Boid boid;
		boid_rand_init(&boid, border, world.size.x - border, border, world.size.y - border);
		world.boids.push_back(boid);
	}

	// world.boids.push_back({ .pos = { 150, 100 }, .dir = { 1, 0 }});
	// world.boids.push_back({ .pos = { 200, 180 }, .dir = { 0, -1 }});

    // Main game loop
    while (!window.ShouldClose()) {
		// TODO: Show this on screen
		// LogTrace("%d", count_out_of_bounds_boids(&world));

		float dt = GetFrameTime();
		for (int i = 0; i < world.boids.size(); i++) {
			Boid *boid = &world.boids[i];

			Vector2 step = Vector2Multiply(boid->dir, { world.boid_speed * dt, world.boid_speed * dt });
			Vector2 pos = Vector2Add(boid->pos, step);

			RayHitResult hit_result;
			get_intersect_with_world(&hit_result, pos, step, &world);
			if (hit_result.hit == -1 || hit_result.hit > 2) {
				boid->pos = pos;
				Boid *boids_in_view[world.boids.size()];
				int boids_in_view_count = get_boids_in_view_cone(boids_in_view, boid, world.boid_view_radius, world.boid_view_angle, world.boids.data(), world.boids.size());

				if (boids_in_view_count > 0) {
					float current_facing = vector2_atan2(boid->dir);

					float average_facing = 0;
					Vector2 average_pos = { 0, 0 };
					for (int j = 0; j < boids_in_view_count; j++) {
						if (boids_in_view[j] == boid) continue;
						average_facing += std::atan2(boids_in_view[j]->dir.y, boids_in_view[j]->dir.x);
						average_pos.x += boids_in_view[j]->pos.x;
						average_pos.y += boids_in_view[j]->pos.y;
					}
					average_facing /= boids_in_view_count;
					average_pos.x /= boids_in_view_count;
					average_pos.y /= boids_in_view_count;

					Vector2 dir_to_average_pos = Vector2Subtract(average_pos, boid->pos);
					float average_pos_angle = vector2_atan2(dir_to_average_pos);
					float angle_to_average_pos = current_facing - vector2_atan2(dir_to_average_pos);
					if (angle_to_average_pos > PI) {
						angle_to_average_pos -= 2*PI;
					} else if (angle_to_average_pos < -PI) {
						angle_to_average_pos += 2*PI;
					}

					float angle_to_average_facing = (average_facing - current_facing);

					float turn_angle = (angle_to_average_pos + angle_to_average_facing) / 2;

					boid->dir = Vector2Rotate(boid->dir, turn_angle * world.boid_turn_speed * dt);
					// boid->dir = Vector2Rotate(boid->dir, () * world.boid_turn_speed * dt);
				}
			}


			try_avoiding_obstacles(&world, boid, dt);
		}

        // Draw
        BeginDrawing();
        ClearBackground(RAYWHITE);

		for (int i = 0; i < world.obstacles.size(); i++) {
			draw_obstacle(&world.obstacles[i], GRAY);
		}


		Color view_cone_color = Fade(GRAY, 0.4);
		for (int i = 0; i < world.boids.size(); i++) {
			Boid *boid = &world.boids[i];
			Vector2 pos = boid->pos;
			float facing = std::atan2(boid->dir.y, boid->dir.x);

			float view_angle = world.boid_view_angle;
			float segments = 16;

			draw_circle_sector(pos, world.boid_view_radius, facing - view_angle/2, facing + view_angle/2, segments, view_cone_color);
		}

		float boid_length = visuals.boid_edge_size * std::sqrt(3)/2;
		float boid_width = visuals.boid_edge_size * 0.6;
		for (int i = 0; i < world.boids.size(); i++) {
			Boid *boid = &world.boids[i];

			draw_debug_boid_obstacle_avoidance(&visuals, &world, boid);

			Vector2 triangle[] = {
				{  boid_length*2/3.0f, 0 },
				{ -boid_length*1/3.0f, -boid_width/2 },
				{ -boid_length*1/3.0f, boid_width/2 },
			};

			float facing = std::atan2(boid->dir.y, boid->dir.x);
			for (int i = 0; i < 3; i++) {
				triangle[i] = Vector2Add(boid->pos, Vector2Rotate(triangle[i], facing));
			}

			DrawTriangle(triangle[0], triangle[1], triangle[2], BLACK);
			DrawCircle(boid->pos.x, boid->pos.y, visuals.boid_edge_size * 0.05, RED);

			Vector2 look_pos = Vector2Add(boid->pos, Vector2Multiply(boid->dir, { 30, 30 }));
			DrawLine(boid->pos.x, boid->pos.y, look_pos.x, look_pos.y, RED);
		}

        EndDrawing();
    }

    return 0;
}