#include <SDL2/SDL.h>
#include <assert.h>
#include <math.h>

#define WIN_WIDTH 800
#define WIN_HEIGHT 800
#define MARGIN_X 100
#define MARGIN_Y 100

#define SCENE_WIDTH (WIN_WIDTH - 2 * MARGIN_X)
#define SCENE_HEIGHT (WIN_HEIGHT - 2 * MARGIN_Y)

#define CART_WIDTH 50
#define CART_HEIGHT 20
#define MASS_RADIUS 10

typedef struct {
	double s, ps; // Cart displacement
	double a, pa; // Pendulum angle (from vertical)
} state_t;

static const double mc = 2; // Cart mass
static const double mp = 2; // Pendulum mass
static const double l = 0.5; // Pendulum length
static const double g = 9.8; // Gravity

static void update(state_t *s, double dt, double k)
{
	const double co = cos(s->a);
	const double si = sin(s->a);

	const double s_dot
	    = (s->ps * l - s->pa * co) / (l * (mc + mp * pow(si, 2)));
	const double ps_dot = k;
	const double a_dot = (s->pa * (mc + mp) - s->ps * mp * l * co)
	    / (mp * pow(l, 2) * (mc + mp * pow(si, 2)));
	const double pa_dot
	    = (co * si
	       * (pow(s->ps, 2) * mp * pow(l, 2)
	          - 2 * s->ps * s->pa * mp * l * co + pow(s->pa, 2) * (mc + mp)))
	        / (pow(l, 2) * pow(mc + mp * pow(si, 2), 2))
	    - (s->ps * s->pa * si) / (l * (mc + mp * pow(si, 2)))
	    + mp * g * l * si;

	s->s += dt * s_dot;
	s->ps += dt * ps_dot;
	s->a += dt * a_dot;
	s->pa += dt * pa_dot;
}

static void render(SDL_Renderer *r, const state_t *s)
{
	static const double sf = SCENE_WIDTH;
	static const double sc_l = sf * l;
	static const double sc_vdisp = (SCENE_HEIGHT - sc_l) / 2;
	static const SDL_Point sc_anchor = {
		.x = WIN_WIDTH / 2,
		.y = MARGIN_Y + (int)(sc_vdisp + sc_l),
	};

	// Cart centre
	const SDL_Point sc_a = {
		.x = sc_anchor.x + (int)(sf * s->s),
		.y = sc_anchor.y,
	};
	// Pendulum centre
	const SDL_Point sc_b = {
		.x = sc_a.x + (int)(sc_l * sin(s->a)),
		.y = sc_a.y - (int)(sc_l * cos(s->a)),
	};

	// Cart
	const SDL_Rect c = {
		.x = sc_a.x - CART_WIDTH / 2,
		.y = sc_a.y - CART_HEIGHT / 2,
		.w = CART_WIDTH,
		.h = CART_HEIGHT,
	};
	SDL_RenderFillRect(r, &c);

	// Rod
	SDL_RenderDrawLine(r, sc_a.x, sc_a.y, sc_b.x, sc_b.y);

	// Mass
	const SDL_Rect m = {
		.x = sc_b.x - MASS_RADIUS,
		.y = sc_b.y - MASS_RADIUS,
		.w = 2 * MASS_RADIUS,
		.h = 2 * MASS_RADIUS,
	};
	SDL_RenderFillRect(r, &m);
}

int main(void)
{
	int err = SDL_Init(SDL_INIT_VIDEO);
	assert(err == 0);

	SDL_Window *window = SDL_CreateWindow(
	    "Cart and Rod Simulation", SDL_WINDOWPOS_UNDEFINED,
	    SDL_WINDOWPOS_UNDEFINED, WIN_WIDTH, WIN_HEIGHT, 0);
	assert(NULL != window);

	SDL_Renderer *renderer = SDL_CreateRenderer(
	    window, -1, SDL_RENDERER_ACCELERATED | SDL_RENDERER_PRESENTVSYNC);
	assert(NULL != renderer);

	SDL_DisplayMode mode;
	assert(SDL_GetWindowDisplayMode(window, &mode) == 0);
	const double dt = 1.0 / mode.refresh_rate;

	state_t state = { .a = 0.1 };
	while (1) {
		SDL_Event e;
		while (SDL_PollEvent(&e)) {
			if (e.type == SDL_QUIT)
				goto quit;
		}

		update(&state, dt, 0.0);

		SDL_SetRenderDrawColor(renderer, 255, 255, 255, 255);
		SDL_RenderClear(renderer);
		SDL_SetRenderDrawColor(renderer, 0, 0, 0, 255);
		render(renderer, &state);

		SDL_RenderPresent(renderer);
	}

quit:
	SDL_DestroyRenderer(renderer);
	SDL_DestroyWindow(window);
	SDL_Quit();

	return 0;
}