M = 10
m = 1 # mass of particles

function calculate_force(
	left_pos,
	middle_pos,
	right_pos,
	K,
	alpha = 0.0,
	beta = 1000.0,
)
	linear_force = K * (left_pos + right_pos - 2 * middle_pos)
	quadratic_force = alpha * (left_pos - middle_pos)^2 + alpha * (right_pos - middle_pos)^2
	cubic_force = beta * (left_pos - middle_pos)^3 + beta * (right_pos - middle_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 1:N-1
		mass = m
		if i == Int(N / 2)
			mass = M
		end

		left_index = max(1, i - 1)
		right_index = min(N, i + 1)

		du[i*2-1] = ps[i] / mass
		force = calculate_force(qs[left_index], qs[i], qs[right_index], K)
		du[i*2] = force / mass
	end

	# make last point same as first
	du[N*2-1] = du[1] = 0 # set to 0
	du[N*2] = du[2] = 0


	if t % 100000 == 0
		println("TIME UPDATE: ", t)
	end

	# if ps[Int(N / 2)] / M >= 1
	# 	println("(in sim!) Time: ", t, " Vel: ", ps[Int(N / 2)] / M)
	# 	# println("Other Positions: ", qs)
	# 	println("Other Velocities: ", ps, "\n")
	# end
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 * M
	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)

	calculate_energy(s_0)

	# 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-5, abstol = 1e-5, maxiters = 1e10) # control simulation

	calculate_energy(sol.u[end])

	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,
	shift = true,
)
	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)] / M
		p_middle = states[i][Int(length(states[1]) / 2)-1]
		y_lims = shift ? (-3 + p_middle, 3 + p_middle) : (-3, 3)
		# 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 = 7)), v_middle=$(round(v_middle, digits=7))", marker = :circle, xlabel = "Mass Number", ylabel = "Displacement", ylim = y_lims,
				color = :red, legend = :topright,
			)
		else
			plot(positions, label = "t = $(round(t, digits = 7)), v_middle=$(round(v_middle, digits=7))", marker = :circle, xlabel = "Mass Number", ylabel = "Displacement", ylim = y_lims,
				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,
)
	# plot the positions
	middle_index = Int(length(states[1]) / 2) - 1
	pos_init = [x - states[1][middle_index] for x in states[1][1:2:end]]
	pos_final = [x - states[end][middle_index] for x in states[end][1:2:end]]
	p1 = plot(pos_init, label = "Initial", marker = :circle, xlabel = "Mass Number", ylabel = "Displacement", title = "Positions Over Time")
	plot!(p1, pos_final, label = "Final", marker = :circle)

	# plot the vels
	vels_init = [x / M for x in states[1][2:2:end]]
	vels_final = [x / M for x in states[end][2:2:end]]
	p2 = plot(states[1][2:2:end], label = "Initial", marker = :circle, xlabel = "Mass Number", ylabel = "Velocity", title = "Velocities Over Time")
	plot!(p2, states[end][2:2:end], label = "Final t = $(time_steps[end])", 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)
		v = states[i][Int(length(states[i]) / 2)] / M
		if v >= threshold - 10e-6
			push!(output, i)
			println("Time: ", time_steps[i], " Vel: ", v)
		end
	end

	data = []
	for i in 1:length(states)
		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

function analyize_pos(
	states,
	time_steps,
	threshold = 1.975,
)
	println("Analyzing positions:\n")
	output = []
	for i in 1:length(states)
		if states[i][Int(length(states[i]) / 2)-1] >= 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)] - 1)
	end
	p = plot(data, label = "Position Over Time", xlabel = "Time", ylabel = "Position")
	savefig(p, "t/pos-over-time.png")

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

function calculate_energy(state)
	# calculate the kinetic energy
	kinetic_energy = 0
	vels = state[2:2:end]
	for i in 1:N
		mass = i == Int(N / 2) - 1 ? M : m
		# calculate the kinetic energy
		kinetic_energy += 0.5 * vels[i] * vels[i] / mass
	end

	# calcaute the potential energy
	potential_energy = 0
	pos = state[1:2:end]
	for i in 1:N-1
		left_index = max(1, i - 1)
		right_index = min(N, i + 1)
		potential_energy += 0.5 * K * (pos[left_index] - pos[i])^2
		potential_energy += 0.5 * K * (pos[right_index] - pos[i])^2
	end

	# print the energy
	println("Kinetic Energy: ", kinetic_energy)
	println("Potential Energy: ", potential_energy)
	println("Total Energy: ", kinetic_energy + potential_energy, "\n")
end

function plot_middle_mass_phase_space(states)
	# get the index of the middle mass
	middle_index = Int(length(states[1]) / 2) - 1
	# build an array of the pos and vel over time
	pos = []
	vel = []
	for i in 1:length(states)
		push!(pos, states[i][middle_index])
		push!(vel, states[i][middle_index+1])
	end

	# plot the phase space
	p = plot(pos, vel, xlabel = "Position", ylabel = "Momentum", title = "Phase Space of Middle Mass in FPU", label = "Beta = 10, K = 1, N = 64, M = $M")

	# save the plot
	savefig(p, "t/phase-space.png")
end


# Run the simulation
N = 64 # number of masses
K = 1 # spring constant
final_time = 1000 # seconds
plot_data = []

my_vel = 100

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)
# analyize_pos(sol.u, sol.t, 1.4)
plot_starting_and_final_positions(sol.u, sol.t)
# animate_positions(sol.u, sol.t, 0, 100, my_vel) # expect 80913.35854226245 for k=10?? rip
plot_middle_mass_phase_space(sol.u)