function calculate_force(
	left_pos,
	middle_pos,
	right_pos,
	K,
	alpha = 0,
	beta = 0,
)
	linear_force = K * (middle_pos - left_pos + middle_pos - right_pos)
	quadratic_force = alpha * (middle_pos - left_pos)^2 + alpha * (middle_pos - right_pos)^2
	cubic_force = beta * (middle_pos - left_pos)^3 + beta * (middle_pos - right_pos)^3
	return linear_force + quadratic_force + cubic_force
end

function tendency!(du, u, p, t)
	# unpack the params
	N, K, m = p

	# get the positions and momenta
	qs = u[1:2:end]
	ps = u[2:2:end]

	# go over the points in the lattice and update the state
	for i in 2:N-1
		mass = m
		if i == 2 * Int(N / 2) - 1 || i == 2 * Int(N / 2)
			mass = 10000
		end

		du[i*2-1] = ps[i] / mass
		force =
			du[i*2] = force / mass
	end

	force_end = K * (qs[2] - 2 * qs[1] + qs[N-1])
	du[1] = ps[1] / m
	du[2] = force_end / m
	du[end-1] = ps[end] / m
	du[end] = force_end / m
end

function get_initial_state(
	N,
	initial_displacement = 2,
	initial_velocity = 0,
)
	state = zeros(2 * N)

	middle_index = 2 * Int(N / 2) - 1 # middle mass
	state[middle_index] = initial_displacement
	state[middle_index+1] = initial_velocity
	return state
end

using DifferentialEquations
function run_simulation(
	N,
	K,
	m,
	final_time,
	initial_displacement = 2,
	initial_velocity = 0,
)
	println("Running simulation with N = $N, K = $K, m = $m, final_time = $final_time, initial_displacement = $initial_displacement, initial_velocity = $initial_velocity\n")
	s_0 = get_initial_state(N, initial_displacement, initial_velocity)

	# pack the params
	p = N, K, m
	t_span = (0.0, final_time)
	prob = ODEProblem(tendency!, s_0, t_span, p)
	sol = solve(prob, Tsit5(), reltol = 1e-10, abstol = 1e-10) # control simulation

	println("Done Running Sim!\n\n")
	return sol
end

using Plots
function animate_positions(
	states,
	time_steps,
	time_min = 0,
	time_max = 30,
	red_threshold = 2,
)
	println("Animating positions")
	anim = @animate for i in 1:length(time_steps)
		t = time_steps[i]
		if t < time_min
			continue
		end
		if t > time_max
			break
		end
		positions = states[i][1:2:end]
		v_middle = states[i][Int(length(states[1]) / 2)]
		# plot(positions, label = "t = $(round(t, digits = 3)), v_middle=$(round(v_middle, digits=3))", marker = :circle, xlabel = "Mass Number", ylabel = "Displacement", ylim = (-3, 3))
		if v_middle >= red_threshold
			plot(positions, label = "t = $(round(t, digits = 3)), v_middle=$(round(v_middle, digits=3))", marker = :circle, xlabel = "Mass Number", ylabel = "Displacement", ylim = (-3, 3),
				color = :red, legend = :topright,
			)
		else
			plot(positions, label = "t = $(round(t, digits = 3)), v_middle=$(round(v_middle, digits=3))", marker = :circle, xlabel = "Mass Number", ylabel = "Displacement", ylim = (-3, 3),
				color = :blue, legend = :topright,
			)
		end
	end
	mp4(anim, "t/animate-positions.mp4", fps = 30)
	println("Done animating positions")
end

function plot_starting_and_final_positions(
	states,
	time_steps,
)
	p1 = plot(states[1][1:2:end], label = "Initial", marker = :circle, xlabel = "Mass Number", ylabel = "Displacement", title = "First Three Modes")
	plot!(p1, states[end][1:2:end], label = "Final", marker = :circle)

	# plot the vels
	p2 = plot(states[1][2:2:end], label = "Initial", marker = :circle, xlabel = "Mass Number", ylabel = "Velocity", title = "First Three Modes")
	plot!(p2, states[end][2:2:end], label = "Final", marker = :circle)

	# save the plots
	savefig(p1, "t/initial-final-positions.png")
	savefig(p2, "t/initial-final-velocities.png")
end

function analyize_vels(
	states,
	time_steps,
	threshold = 1.975,
)
	println("Analyzing velocities:\n")
	output = []
	for i in 1:length(states)
		if states[i][Int(length(states[i]) / 2)] >= threshold
			push!(output, i)
			println("Time: ", time_steps[i], " Position: ", states[i][Int(length(states[i]) / 2)])
		end
	end

	# plot the first 10 seconds of Velocity
	data = []
	for i in 1:length(states)
		if time_steps[i] > 10
			break
		end
		push!(data, states[i][Int(length(states[i]) / 2)])
	end
	p = plot(data, label = "Velocity Over Time", xlabel = "Time", ylabel = "Velocity")
	savefig(p, "t/velocity-over-time.png")

	println("\nDone!\n\n")
	return output
end

# Run the simulation
N = 10 # number of masses
beta = 0 # cubic string spring
K = 100 # spring constant
A = 10 # amplitude
final_time = 10000 # seconds
m = 1 # mass of particles
plot_data = []

my_vel = 10

sol = run_simulation(N, K, m, final_time, 0, my_vel)

println("final time: ", sol.t[end])
# s = sol.u[1:2:end]
analyize_vels(sol.u, sol.t, my_vel)
plot_starting_and_final_positions(sol.u, sol.t)
animate_positions(sol.u, sol.t, 0, 1, my_vel)