module sim

const sim_test_mock_params = SimParams{
	rope_length: 0.25
	bearing_mass: 0.03
	magnet_spacing: 0.05
	magnet_height: 0.03
	magnet_strength: 10
	gravity: 4.9
}
const sim_test_mock_state = SimState{
	position: vector(
		x: -0.016957230930171364
		y: -0.02937078552673521
		z: 0.002311063475327252
	)
	velocity: vector(
		x: -7.251158929833104
		y: -12.559375680227724
		z: -105.91539687686381
	)
	accel: vector(
		x: -8.337034766251843e-11
		y: -2.842170943040401e-10
		z: 1.2126596023639044e-10
	)
}

pub fn test_satisfy_rope_constraint() {
	mut state := SimState{
		...sim.sim_test_mock_state
	}

	state.satisfy_rope_constraint(sim.sim_test_mock_params)
	assert state.position.x == -0.016957230930171364
	assert state.position.y == -0.02937078552673521
	assert state.position.z == 0.002311063475327252
	assert state.velocity.x == -7.251158929833104
	assert state.velocity.y == -12.559375680227724
	assert state.velocity.z == -105.91539687686381
	assert state.accel.x == -8.337034766251843e-11
	assert state.accel.y == -2.842170943040401e-10
	assert state.accel.z == 1.2126596023639044e-10
}

pub fn test_increment() {
	mut state := SimState{
		...sim.sim_test_mock_state
	}

	delta_t := 0.0005
	state.increment(delta_t, sim.sim_test_mock_params)
	assert state.position.x == -0.016957230930171364
	assert state.position.y == -0.02937078552673524
	assert state.position.z == 0.0023110634753272796
	assert state.velocity.x == -7.251158929833047
	assert state.velocity.y == -12.559375680227637
	assert state.velocity.z == -105.9153968768638
	assert state.accel.x == 1.1368683772161604e-10
	assert state.accel.y == 1.7431981783981126e-10
	assert state.accel.z == 3.031649005909761e-11
}
