use symplectic_double_pendulum::*;

use macroquad::prelude::*;

fn draw(dp: &DoublePendulum, fg: Color) {
	let rod_length = 120.0;

	let pivot = vec2(screen_width() / 2.0, screen_height() / 2.0);
	let bob1 = vec2(
		pivot.x + rod_length * (dp.theta.sin() as f32),
		pivot.y + rod_length * (dp.theta.cos() as f32));
	let bob2 = vec2(
		bob1.x + rod_length * ((dp.theta + dp.phi).sin() as f32),
		bob1.y + rod_length * ((dp.theta + dp.phi).cos() as f32));

	draw_line(pivot.x, pivot.y, bob1.x, bob1.y, 2.0, fg);
	draw_line(bob1.x, bob1.y, bob2.x, bob2.y, 2.0, fg);

	draw_circle(bob1.x, bob1.y, 8.0, fg);
	draw_circle(bob2.x, bob2.y, 8.0, fg);
}

#[macroquad::main("Double Pendulum")]
async fn main() {
	let omega = 500.0;
	let h = 1e-3;

	let mut state = Augmented::from(State {
		q: DoublePendulum { theta: std::f64::consts::PI - 0.01, phi: 0.0 },
		p: DoublePendulum { theta: 0.0, phi: 0.0 },
	}, omega);

	loop {
		clear_background(WHITE);

		let t = get_frame_time() as f64;
		for _ in 0..((t / h) as i32) {
			state.evolve(h);
		}
		draw(&state.average().q, BLACK);

		next_frame().await
	}
}