#include "pixelGameEngine.h"

using namespace olc;

#define WIDTH 640
#define HEIGHT 480

struct Ball{
	vf2d pos={};
	vf2d vel={};
	vf2d acc={};
	vf2d originalPos={};
	float radius=0;
	float mass=0;
	float simTimeRemaining=0;
	bool grounded=false;
};

struct Line{
	vf2d startPos={};
	vf2d endPos={};
	float radius=0;
	float bounceFactor=0.8;
	bool oneWay=false;
};

const float PI=3.14159f;

class MountainClimber : public olc::PixelGameEngine
{

public:
	std::vector<Ball*>balls;
	std::vector<Line>lines;
	Ball*selectedBall;
	Line*selectedLine;
	std::vector<std::pair<Ball*,Ball*>>collidingPairs;
	std::vector<Ball*>fakeBalls;
	bool selectedLineStart=false;
	float friction=5.f;
	float gravity=2000.f;
	float limit=1000.f;
	Ball*player;
	float moveSpd=250;
	float jumpSpd=1000;

	MountainClimber()
	{
		sAppName = "Mountain Climber";
	}

	void AddBall(vf2d pos,float r){
		Ball*b=new Ball();
		b->pos=pos;
		b->radius=r;
		b->mass=r*10;
		balls.emplace_back(b);
	}

	void AddLine(vf2d pos1,vf2d pos2,float r,float bounceFactor,bool oneway=false){
		Line l;
		l.startPos=pos1;
		l.endPos=pos2;
		l.radius=r;
		l.bounceFactor=bounceFactor;
		l.oneWay=oneway;
		lines.push_back(l);
	}

public:
	bool OnUserCreate() override
	{
		AddLine({0,360},{640,360},0.1,0);
		AddLine({340,360},{640,140},0.1,0);
		AddBall({320,300},8);

		ConsoleCaptureStdOut(true);
		player=balls[0];
		return true;
	}

	void HandlePhysics(float fElapsedTime){
		float stable=0.05f;
		int simulationUpdates=4;
		int maxSimulationSteps=15;
		float simElapsedTime=fElapsedTime/(float)simulationUpdates;
		auto DoCirclesOverlap = [](vf2d pos1, float r1, vf2d pos2, float r2){
			return fabs((pos1.x - pos2.x)*(pos1.x - pos2.x) + (pos1.y - pos2.y)*(pos1.y - pos2.y)) <= (r1 + r2)*(r1 + r2);
		};
		for(int i=0;i<simulationUpdates;i++){
			for(Ball*b:balls){
				b->simTimeRemaining=simElapsedTime;
			}
			for(int j=0;j<maxSimulationSteps;j++){
				for(Ball*b:balls){
					if(b->simTimeRemaining>0){
						b->originalPos.x=b->pos.x;
						b->originalPos.y=b->pos.y;
						b->acc.x=-b->vel.x*friction;
						b->acc.y=-b->vel.y*friction+((!b->grounded)?gravity:0); //Gravity constant;
						b->vel.x+=b->acc.x*b->simTimeRemaining;
						b->vel.y+=b->acc.y*b->simTimeRemaining;
						b->pos.x+=b->vel.x*b->simTimeRemaining;
						b->pos.y+=b->vel.y*b->simTimeRemaining;

						if(b->pos.x<0){
							b->pos.x+=ScreenWidth();
						}
						if(b->pos.y<0){
							b->pos.y+=ScreenHeight();
						}
						if(b->pos.x>=ScreenWidth()){
							b->pos.x-=ScreenWidth();
						}
						if(b->pos.y>=ScreenHeight()){
							b->pos.y-=ScreenHeight();
						}

						if(fabs(b->vel.x*b->vel.x+b->vel.y*b->vel.y)<stable){
							b->vel.x=0;
							b->vel.y=0;
						}
					}
				}

				for(Ball*b:balls){
					float deltaTime=b->simTimeRemaining;
					bool collisionOccured=false;
					for(Line&l:lines){
						if((!l.oneWay||l.oneWay&&b->vel.y<0)){
							vf2d line1={l.endPos.x-l.startPos.x,l.endPos.y-l.startPos.y};
							vf2d line2={b->pos.x-l.startPos.x,b->pos.y-l.startPos.y};
							float edgeLength=line1.x*line1.x+line1.y*line1.y;
							float t=std::max(0.f,std::min(edgeLength,(line1.x*line2.x+line1.y*line2.y)))/edgeLength;
							vf2d closestPoint={l.startPos.x+t*line1.x,l.startPos.y+t*line1.y};
							float dist=sqrtf((b->pos.x-closestPoint.x)*(b->pos.x-closestPoint.x)+(b->pos.y-closestPoint.y)*(b->pos.y-closestPoint.y));
							if(dist<=b->radius+l.radius){
								Ball*fakeBall=new Ball();
								fakeBall->radius=l.radius;
								fakeBall->mass=b->mass*l.bounceFactor;
								fakeBall->pos={closestPoint.x,closestPoint.y};
								fakeBall->vel={std::min(limit,std::max(-limit,-b->vel.x)),std::min(limit,std::max(-limit,-b->vel.y))};
								fakeBalls.push_back(fakeBall);
								collidingPairs.push_back({b,fakeBall});
								float overlap=1.1f*(dist-b->radius-fakeBall->radius);
								b->pos.x-=overlap*(b->pos.x-fakeBall->pos.x)/dist;
								b->pos.y-=overlap*(b->pos.y-fakeBall->pos.y)/dist;
								if(b==player&&fakeBall->vel.y<0){//This is an upwards force pushing on us, we have touched ground.
									b->grounded=true;
									collisionOccured=true;
								}
							}
						}
					}
					if(!collisionOccured){
						b->grounded=false;
					}
					for(Ball*b2:balls){
						if(b!=b2){
							if(DoCirclesOverlap(b->pos,b->radius,b2->pos,b2->radius)){
								collidingPairs.push_back({b,b2});
								float dist=sqrtf((b->pos.x-b2->pos.x)*(b->pos.x-b2->pos.x)+(b->pos.y-b2->pos.y)*(b->pos.y-b2->pos.y));
								float overlap=0.5f*(dist-b->radius-b2->radius);
								b->pos.x-=overlap*(b->pos.x-b2->pos.x)/dist;
								b->pos.y-=overlap*(b->pos.y-b2->pos.y)/dist;
								b2->pos.x+=overlap*(b->pos.x-b2->pos.x)/dist;
								b2->pos.y+=overlap*(b->pos.y-b2->pos.y)/dist;
							}
						}
					}
					float intendedSpeed=sqrtf(b->vel.x+b->vel.y*b->vel.y);
					float intendedDist=intendedSpeed*b->simTimeRemaining;
					float actualDist=sqrtf((b->pos.x-b->originalPos.x)*(b->pos.x-b->originalPos.x)+(b->pos.y-b->originalPos.y)*(b->pos.y-b->originalPos.y));
					float actualTime=actualDist/intendedSpeed;
					b->simTimeRemaining=b->simTimeRemaining-actualTime;
				}
				float efficiency=1;
				for(std::pair<Ball*,Ball*>&pair:collidingPairs){
					Ball*b1=pair.first;
					Ball*b2=pair.second;
					float dist=sqrtf((b1->pos.x-b2->pos.x)*(b1->pos.x-b2->pos.x)+(b1->pos.y-b2->pos.y)*(b1->pos.y-b2->pos.y));
					vf2d normal={(b2->pos.x-b1->pos.x)/dist,(b2->pos.y-b1->pos.y)/dist};
					vf2d tangent={-normal.y,normal.x};
					vf2d dpTangent={tangent.dot(b1->vel),tangent.dot(b2->vel)};
					vf2d dpNormal={normal.dot(b1->vel),normal.dot(b2->vel)};
					vf2d momentum={efficiency*(dpNormal.x*(b1->mass-b2->mass)+2.f*b2->mass*dpNormal.y)/(b1->mass+b2->mass),efficiency*(dpNormal.y*(b2->mass-b1->mass)+2.f*b1->mass*dpNormal.x)/(b1->mass+b2->mass)};
					b1->vel.x=tangent.x*dpTangent.x+normal.x*momentum.x;
					b1->vel.y=tangent.y*dpTangent.x+normal.y*momentum.x;
					b2->vel.x=tangent.x*dpTangent.y+normal.x*momentum.y;
					b2->vel.y=tangent.y*dpTangent.y+normal.y*momentum.y;
				}

				collidingPairs.clear();
				for(Ball*fb:fakeBalls){
					delete fb;
				}
				fakeBalls.clear();
			}
		}
	}

	bool OnUserUpdate(float fElapsedTime) override
	{
		auto IsPointInCircle = [](vf2d pos, float r, vf2d point){
			return fabs((pos.x - point.x)*(pos.x - point.x) + (pos.y - point.y)*(pos.y - point.y)) < (r * r);
		};

		if(GetKey(A).bHeld){
			player->vel.x=-moveSpd;
			player->grounded=false;
		}
		if(GetKey(D).bHeld){
			player->vel.x=moveSpd;
			player->grounded=false;
		}
		if(GetKey(SPACE).bPressed){
			player->vel.y=-jumpSpd;
			player->grounded=false;
		}

		if (GetMouse(0).bPressed){
			selectedBall = nullptr;
			for (Ball*b:balls){
				if (IsPointInCircle(b->pos,b->radius,GetMousePos()))
				{
					selectedBall = b;
					break;
				}
			}
			selectedLine=nullptr;
			for(Line&l:lines){
				if(IsPointInCircle(l.startPos,l.radius,GetMousePos())){
					selectedLine=&l;
					selectedLineStart=true;
					break;
				}
				if(IsPointInCircle(l.endPos,l.radius,GetMousePos())){
					selectedLine=&l;
					selectedLineStart=false;
					break;
				}
			}
		}

		if(GetMouse(0).bHeld){
			if(selectedLine!=nullptr){
				if(selectedLineStart){
					selectedLine->startPos=GetMousePos();
				}else{
					selectedLine->endPos=GetMousePos();
				}
			}
		}
		if(GetMouse(0).bReleased){
			if(selectedLine!=nullptr){
				std::cout<<selectedLine->startPos<<"/"<<selectedLine->endPos<<std::endl;
			}
			if(selectedBall!=nullptr){
				selectedBall->vel.x=5*((selectedBall->pos.x)-GetMouseX());
				selectedBall->vel.y=5*((selectedBall->pos.y)-GetMouseY());
			}
			selectedBall=nullptr;
			selectedLine=nullptr;
		}
		if(GetMouse(1).bHeld){
			for(Ball*b:balls){
				b->vel.x+=(GetMouseX()-b->pos.x)*0.01f;
				b->vel.y+=(GetMouseY()-b->pos.y)*0.01f;
			}
		}

		if(GetKey(F1).bPressed){
			ConsoleShow(F1);
		}

		Clear(BLACK);

		HandlePhysics(fElapsedTime);

		for(Ball*b:balls){
			if(selectedBall==b){
				FillCircle(b->pos,b->radius,YELLOW);
			} else {
				FillCircle(b->pos,b->radius);
			}
		}
		for(Line&l:lines){
			FillCircle(l.startPos,l.radius);
			FillCircle(l.endPos,l.radius);
			vf2d normal={-(l.endPos.y-l.startPos.y),l.endPos.x-l.startPos.x};
			float dist=sqrt(normal.x*normal.x+normal.y*normal.y);
			normal/=dist;
			DrawLine(l.startPos.x+normal.x*l.radius,l.startPos.y+normal.y*l.radius,l.endPos.x+normal.x*l.radius,l.endPos.y+normal.y*l.radius,l.oneWay?DARK_YELLOW:WHITE);
			DrawLine(l.startPos.x-normal.x*l.radius,l.startPos.y-normal.y*l.radius,l.endPos.x-normal.x*l.radius,l.endPos.y-normal.y*l.radius,l.oneWay?DARK_YELLOW:WHITE);
		}
		return true;
	}

	bool OnUserDestroy()override{
		return true;
	}
};


int main()
{	
	MountainClimber demo;
	if (demo.Construct(640, 640, 4, 4))
		demo.Start();

	return 0;
}