#define OLC_PGE_APPLICATION
#include "olcPixelGameEngine.h"
#include <vector>
#include <iostream>
#include <cmath>

#define p(X) std::cout << #X << " = " << X << std::endl;

struct vec2d {
	float x, y;
	float length() const {
		return std::sqrt(x * x + y * y);
	}
	vec2d norm() const {
		float l = this->length();
		return vec2d{this->x/l,this->y/l};
	}
	vec2d& operator+=(vec2d const& o) {
		x += o.x;
		y += o.y;
		return *this;
	}
	vec2d operator +(vec2d const & o) const {
		return vec2d{ this->x + o.x,this->y + o.y};
	}
	vec2d& operator-=(vec2d const& o) {
		x -= o.x;
		y -= o.y;
		return *this;
	}
	vec2d operator -(vec2d const& o) const {
		return vec2d{ this->x - o.x,this->y - o.y };
	}
	vec2d operator*(float fac) const {
		return vec2d{ this->x * fac,this->y * fac };
	}
	vec2d& operator*=(float fac) {
		this->x* fac; this->y * fac;
		return *this;
	}
	vec2d operator/(float fac) const {
		return vec2d{ this->x / fac,this->y / fac };
	}
	vec2d& operator/=(float fac) {
		this->x / fac; this->y / fac;
		return *this;
	}
	bool operator==(vec2d const & o) const {
		return std::abs(this->x-o.x) < 0.0001f && std::abs(this->y - o.y) < 0.0001f;
	}
	vec2d getInLength(float l) {
		return (this->norm()*l);
	}
	vec2d To(vec2d to) const {
		return vec2d{to.x - this->x, to.y - this->y};
	}
	vec2d rotated90() const {
		return vec2d{ this->y,-this->x };
	}
};
struct blob {
	vec2d pos;
	vec2d vel;
	vec2d acc;
	std::vector<blob*> pointsAt;
};

std::ostream& operator<<(std::ostream& os, vec2d const& v) {
	return os << '<' << v.x << ',' << v.y << '>';
}
std::ostream& operator<<(std::ostream& os, blob const& b) {
	return os << "blob{" << b.pos << ' ' << b.vel << ' ' << b.pointsAt.size() << '}';
}
template <typename T>
std::ostream& operator<<(std::ostream& os, std::vector<T> const& v) {
	os << "[ ";
	for (T e : v) {
		os << e << ' ';
	}
	return os << "]";
}


class Example : public olc::PixelGameEngine
{
public:
	Example()
	{
		sAppName = "Example";
	}

public:
	float randf(float low = 0, float high = 1) {
		return low + static_cast <float> (rand()) / (static_cast <float> (RAND_MAX / (high - low)));
	}
	vec2d randPos() {
		return vec2d{randf(),randf()};
	}
	float lerp(float a, float b, float t)
	{
		return a + t * (b - a);
	}
	float linearConversion(float ifrom, float ito, float v, float ofrom, float oto) {
		float t = (v - ifrom) / (ito - ifrom);
		return lerp(ofrom, oto,t);
	}
	void DrawArrow(vec2d from, vec2d to, olc::Pixel color) {
		if (from == to) return;

		DrawLine(from.x, from.y, to.x, to.y, color);

		vec2d vec = from.To(to);

		to = from + vec.getInLength(vec.length() - 3);
		vec = from.To(to);

		vec2d tipStartPos = from + vec.getInLength(vec.length() - 5); // go back 10 pixels

		float tipW = 3;
		vec2d p1 = tipStartPos + vec.rotated90().getInLength(tipW);
		vec2d p2 = tipStartPos + vec.rotated90().getInLength(-tipW);

		//FillTriangle(p1.x,p1.y,p2.x,p2.y,to.x, to.y, color);
	}

	std::vector<blob> blobs;
	std::vector<vec2d> blobPosCache;
	bool OnUserCreate() override
	{
		//p(linearConversion(-16342.5, -16342.5, maxX, 0, ScreenWidth() - 1))

		const int blobcnt = 5;

		blobs = std::vector<blob>(blobcnt);
		blobPosCache = std::vector<vec2d>(blobcnt);


		for (int i = 0; i < blobcnt; i++) {
			//blobs[i] = blob{{randf(),randf()},{randf(),randf()} };
			blobs[i] = blob{ {randf(-5000,5000),randf(-5000,5000)}};
			int to = rand() % 2;
			for (int j = 0; j < to; j++) {
				blobs[i].pointsAt.push_back(&(blobs[rand() % blobcnt]));
			}
		}

		/*
		blobs = std::vector<blob>(2);
		blobPosCache = std::vector<vec2d>(2);
		blobs[0] = blob{ {randf(),randf()}};
		blobs[1] = blob{ {randf(),randf()}};
		blobs[0].pointsAt.push_back(&(blobs[1]));
		*/
		//blobs[0].pointsAt.push_back(&(blobs[rand() % blobcnt]));

		for (int x = 0; x < ScreenWidth(); x++)
			for (int y = 0; y < ScreenHeight(); y++)
				Draw(x, y, olc::WHITE);
		//p(blobs)
		return true;
	}

	bool OnUserUpdate(float fElapsedTime) override
	{
		//p(fElapsedTime);
		for (int x = 0; x < ScreenWidth(); x++)
			for (int y = 0; y < ScreenHeight(); y++)
				Draw(x, y, olc::WHITE);

		//std::cout << blobs << std::endl;
		//dont show every simulation step
		static int speed = 1;

		speed += GetMouseWheel()/120;
		speed = std::abs(speed);
		if (GetMouseWheel() != 0)
			std::cout << speed << std::endl;
		for (int i = 0; i < speed; i++) {
			for (blob& b : blobs) {
				b.pos += b.vel * fElapsedTime;
				b.vel += (b.acc + b.vel * -0.8) * fElapsedTime;
				b.acc = vec2d{ 0,0 };
			}
			for (blob& b : blobs) {
				for (blob* bp : b.pointsAt) {
					vec2d to = b.pos.To(bp->pos);
					to *= .1;
					b.acc += to;
					bp->acc += to * -.1;
				}
				for (blob& ob : blobs) {
					if (&ob == &b) continue;
					if (std::find(b.pointsAt.begin(), b.pointsAt.end(), &ob) != b.pointsAt.end()) continue;
					vec2d to = b.pos.To(ob.pos);
					if (to.length() < 3000) {
						to = to.getInLength(1000);
						b.acc += to * -1;
					}
				}
				//b.acc += b.pos.getInLength(b.pos.length()*b.pos.length()) * -0.000001;
			}
		}

		//draw everything

		float maxX = max_element(std::begin(blobs), std::end(blobs), [](blob const& a, blob const& b) -> bool {return a.pos.x < b.pos.x; })->pos.x;
		float minX = min_element(std::begin(blobs), std::end(blobs), [](blob const& a, blob const& b) -> bool {return a.pos.x < b.pos.x; })->pos.x;
		float maxY = max_element(std::begin(blobs), std::end(blobs), [](blob const& a, blob const& b) -> bool {return a.pos.y < b.pos.y; })->pos.y;
		float minY = min_element(std::begin(blobs), std::end(blobs), [](blob const& a, blob const& b) -> bool {return a.pos.y < b.pos.y; })->pos.y;


		float xdelta = maxX - minX;
		float ydelta = maxY - minY;
		float delta;
		if (xdelta > ydelta) {
			float off = (xdelta - ydelta) / 2;
			minY -= off;
			maxY += off;
			delta = xdelta;
		} else {
			float off = (ydelta - xdelta) / 2;
			minX -= off;
			maxX += off;
			delta = xdelta;
		}


		for (int i = static_cast<int>(minX) / 500 * 500; i < maxX; i += 500) {
			float x = linearConversion(minX, maxX, i, 0, ScreenWidth());
			DrawLine(x, 0, x, ScreenHeight() - 1, olc::Pixel(200, 200, 200));
		}
		for (int i = static_cast<int>(minY) / 500 * 500; i < maxY; i += 500) {
			float y = linearConversion(minY, maxY, i, 0, ScreenHeight());
			DrawLine(0, y, ScreenWidth() - 1, y, olc::Pixel(200, 200, 200));
		}

		float x = linearConversion(minX, maxX, 0, 0, ScreenWidth());
		DrawLine(x, 0, x, ScreenHeight() - 1, olc::Pixel(0, 0, 255));
		float y = linearConversion(minY, maxY, 0, 0, ScreenHeight());
		DrawLine(0, y, ScreenWidth() - 1, y, olc::Pixel(0, 0, 255));

		for (int i = 0; i < blobs.size(); i++) {
			blob& const b = blobs[i];
			float pixelX = linearConversion(minX, maxX, b.pos.x, 10, ScreenWidth() - 10);
			float pixelY = linearConversion(minY, maxY, b.pos.y, 10, ScreenHeight() - 10);
			blobPosCache[i] = vec2d{ pixelX,pixelY };
		}

		blob* pt2fst = &(blobs[0]);

		for (int i = 0; i < blobs.size(); i++) {
			blob& const b = blobs[i];
			for (blob* bp : b.pointsAt) {
				unsigned bpi = bp - pt2fst;
				DrawArrow(blobPosCache[i],blobPosCache[bpi],olc::BLACK);
			}
		}

		float radius = std::abs(linearConversion(minX, maxX, minX, 10, ScreenWidth() - 10) - linearConversion(minX, maxX, minX + 250, 10, ScreenWidth() - 10));
		for (vec2d pos : blobPosCache) {
			FillCircle(pos.x, pos.y, radius, olc::RED);
		}

		DrawString(ScreenWidth() - 100,ScreenHeight()-30, std::to_string(maxX),olc::BLACK);
		DrawString(30, ScreenHeight() - 30, std::to_string(minX), olc::BLACK);

		return true;
	}
};



int main()
{
	Example demo;
	if (demo.Construct(1000, 1000, 1, 1))
		demo.Start();

	return 0;
}