#include <string>
#include <vector>
#include <cmath>
#include <algorithm>
#include <functional>

#define LIGHTSPEED 10

#define GAMMA(v) (1.0/sqrt(1.0 - (v*v)/(LIGHTSPEED * LIGHTSPEED)))

typedef std::vector<double> Vec;
typedef std::vector<Vec> Mat;

#define LORENTZ_BOOST(v) (Mat{{GAMMA(v), -GAMMA(v) * (v/LIGHTSPEED)},{-GAMMA(v) * (v/LIGHTSPEED), GAMMA(v)}})

Vec operator*(const Mat &a, const Vec &x) {
	int i, j;
	int m = a.size();
	int n = x.size();

	Vec prod(m);

	for (i = 0; i < m; i++) {
		prod[i] = 0.;
		for (j = 0; j < n; j++)
			prod[i] += a[i][j] * x[j];
	}
	return prod;
}


namespace flatland {

	enum State 
	{
		INERTIAL,
		ACCELERATING,
	};

	class Flatlandobject
	{

		Flatlandobject(/*std::vector<std::tuple<Flatlandobject, double>> theparts,*/ std::tuple<State, double, double, int, double, int> initialstate, Mainworldobserver* thegame)
		{
			//parts = theparts;
			objecthistory.push_back(initialstate);
			thegame->objectlist.push_back(std::make_tuple(*this, std::get<3>(initialstate)));
		}

	public:
		bool isPlayer = false;
		//std::vector<std::tuple<Flatlandobject, double>> parts = {};
		std::vector<std::tuple<State, double, double, int, double, int>> objecthistory = {}; 
		// state, time(in main observer clock) at statechange, proper time clock at state change, location(in main observer coordinates) at state change,
		// velocity(if inertial) relative to mainobserver, proper acceleration(if accelerating) 
		double propertime;
		double currentvelocity;
		int currentproperacceleration;
		char name;
		

		auto changestate(double maintime) -> void
		{
			return;
		}

	};

	class Player : public Flatlandobject
	{
	public:
		bool isPlayer = true;
		std::vector<std::tuple<Flatlandobject, double>> world = {};
		char pastlightcone[81] = "--------------------------------------------------------------------------------";

		auto updatepastlightcone(Mainworldobserver* mainobserver) -> void
		{
			for (auto object : world)
			{
				auto theobject = std::get<0>(object);

				for (auto objectstate : theobject.objecthistory)
				{
					auto mainclocktimeatstatechange = std::get<1>(objectstate);
					auto propertimeclockatstatechange = std::get<2>(objectstate);
					auto maincoordinatelocationatstatechange = std::get<3>(objectstate);
					auto mainrelativevelocity = std::get<4>(objectstate);
					auto properacceleration = std::get<5>(objectstate);

					if (std::get<0>(objectstate) == INERTIAL)
					{
						auto lightconeeq1 = std::vector<double>{ LIGHTSPEED, mainobserver->currenttime - std::get<1>(mainobserver->objectlist[0]) };
						auto lightconeeq2 = std::vector<double>{ -LIGHTSPEED, mainobserver->currenttime - std::get<1>(mainobserver->objectlist[0]) };
						auto worldlineeq = std::vector<double>{ mainrelativevelocity, mainclocktimeatstatechange - maincoordinatelocationatstatechange};

						auto intersection1 = std::vector<double>{
							(lightconeeq1[0] * worldlineeq[1] - worldlineeq[0] * lightconeeq1[1]) / (lightconeeq1[0] - worldlineeq[0]),
							(worldlineeq[1] - lightconeeq1[1])/(lightconeeq1[0]-worldlineeq[0]),
						};
						auto intersection2 = std::vector<double>{
							(lightconeeq2[0] * worldlineeq[1] - worldlineeq[0] * lightconeeq2[1]) / (lightconeeq2[0] - worldlineeq[0]),
							(worldlineeq[1] - lightconeeq2[1]) / (lightconeeq2[0] - worldlineeq[0]),
						};

						if (mainclocktimeatstatechange < intersection1[0] && intersection1[0] < mainobserver->currenttime)
						{
							intersection1 = std::vector<double>{intersection1[0] - mainobserver->currenttime, intersection1[1] - std::get<1>(mainobserver->objectlist[0]) };
							intersection1 = LORENTZ_BOOST(-currentvelocity)*intersection1;

							if (-40.0 < intersection1[1] && intersection1[1] < 40.0)
							{
								pastlightcone[(int)round(intersection1[1])] = name;
							}
						}
						if (mainclocktimeatstatechange < intersection2[0] && intersection2[0] < mainobserver->currenttime)
						{
							intersection2 = std::vector<double>{ intersection2[0] - mainobserver->currenttime, intersection2[1] - std::get<1>(mainobserver->objectlist[0]) };
							intersection2 = LORENTZ_BOOST(-currentvelocity)*intersection2;

							if (-40.0 < intersection2[1] && intersection2[1] < 40.0)
							{
								pastlightcone[(int)round(intersection2[1])] = name;
							}
						}
					}
					else
					{
						auto lightconeeq1 = std::vector<double>{ LIGHTSPEED, mainobserver->currenttime - std::get<1>(mainobserver->objectlist[0]) };
						auto lightconeeq2 = std::vector<double>{ -LIGHTSPEED, mainobserver->currenttime - std::get<1>(mainobserver->objectlist[0]) };



					}
				}
			}
		}

	};

	class Mainworldobserver
	{
	public:

		std::vector<std::tuple<Flatlandobject, double>> objectlist = {}; // first object is always player
		//int playervelocity;
		double currenttime = 0.0;

		auto updatebytick(double tick) -> void;


	};

	auto Mainworldobserver::updatebytick(double tick) -> void
	{
		this->currenttime += tick;
		for (std::tuple<Flatlandobject, double> a: this->objectlist)
		{
			auto theobject = std::get<0>(a);
			auto objectstate = theobject.objecthistory.back();
			auto mainclocktimeatstatechange = std::get<1>(objectstate);
			auto propertimeclockatstatechange = std::get<2>(objectstate);
			auto maincoordinatelocationatstatechange = std::get<3>(objectstate);
			auto mainrelativevelocity = std::get<4>(objectstate);
			auto properacceleration = std::get<5>(objectstate);
			double newx;
			if (std::get<0>(objectstate) == INERTIAL)
			{
				newx = maincoordinatelocationatstatechange + mainrelativevelocity * (this->currenttime - mainclocktimeatstatechange);
				std::tuple<Flatlandobject, double> newobjectinfo = std::make_tuple(theobject, newx);
				a.swap(newobjectinfo);
				free(&newobjectinfo);
				auto a = std::vector<double>{this->currenttime - mainclocktimeatstatechange, newx- maincoordinatelocationatstatechange};
				a = LORENTZ_BOOST(theobject.currentvelocity) * a;
				theobject.propertime = propertimeclockatstatechange + a[0];
				theobject.currentproperacceleration = 0;
			}
			else
			{

			}
			/*for (std::tuple<Flatlandobject, double> apart : (theobject.parts))
			{
				auto thepart = std::get<0>(apart);
				auto relativelocationtoCoM = std::get<1>(apart);
				auto partstate = thepart.objecthistory.back();
				if (std::get<0>(partstate) == INERTIAL)
				{
					auto a = std::vector<double>{ theobject.propertime, (double)relativelocationtoCoM };
					a = LORENTZ_BOOST(-theobject.currentvelocity) * a;
					a = std::vector<double>{ a[0] + this->currenttime , a[1] + newx };
					thepart.currentvelocity = theobject.currentvelocity;
				}
			}*/
			theobject.changestate(this->currenttime);
		}
	}

	
}
