using LinearAlgebra
using Plots
using Printf

# -------------------------------
# IPC-like Force (Barrier-inspired)
# -------------------------------
function ipc_force(p0::Vector{Float64}, p1::Vector{Float64},
 kappa::Float64, d_hat::Float64)

 diff = p1 - p0
 d = norm(diff)

 eps = 1e-6
 d_safe = max(d, eps)

 if d >= d_hat
 return zeros(3), zeros(3)
 end

 # Barrier-like force
 f_mag = kappa * (1 / d_safe - 1 / d_hat)

 n = diff / d_safe

 f0 = f_mag * n
 f1 = -f0

 return f0, f1
end


# -------------------------------
# Simulation Core
# -------------------------------
function simulate(; dt=0.002, steps=500,
 kappa=1.0, d_hat=0.02, mass=1.0)

 p0 = [-0.02, 0.0, 0.0]
 p1 = [ 0.02, 0.0, 0.0]

 # 🔥 stronger motion
 v0 = [0.5, 0.0, 0.0]
 v1 = [-0.5, 0.0, 0.0]

 traj0 = Vector{Vector{Float64}}()
 traj1 = Vector{Vector{Float64}}()
 distances = Float64[]

 push!(traj0, copy(p0))
 push!(traj1, copy(p1))
 push!(distances, norm(p1 - p0))

 for step in 1:steps
 f0, f1 = ipc_force(p0, p1, kappa, d_hat)

 v0 += dt * f0 / mass
 v1 += dt * f1 / mass

 # ✅ ADD DAMPING HERE
 damping = 0.99
 v0 *= damping
 v1 *= damping

 p0 += dt * v0
 p1 += dt * v1

 d = norm(p1 - p0)

 @printf("Step %d: distance = %.6f\n", step, d)

 push!(traj0, copy(p0))
 push!(traj1, copy(p1))
 push!(distances, d)
 end

 return traj0, traj1, distances
end

# -------------------------------
# Visualization (Animation + Graph)
# -------------------------------
function visualize(traj0, traj1, distances; dt=0.0002,
 filename="ipc_simulation.gif")

 time = collect(0:dt:dt*(length(distances)-1))

 anim = @animate for i in 1:length(traj0)

 pos0 = traj0[i]
 pos1 = traj1[i]

 # ---- LEFT: particle motion ----
 p1_plot = scatter(
 [pos0[1], pos1[1]],
 [pos0[2], pos1[2]],
 xlim=(-0.03, 0.03),
 ylim=(-0.02, 0.02),
 markersize=8,
 label="Particles",
 title="IPC Collision Avoidance"
 )

 plot!(
 [pos0[1], pos1[1]],
 [pos0[2], pos1[2]],
 label="distance"
 )

 # ---- RIGHT: distance vs time ----
 p2_plot = plot(
 time[1:i],
 distances[1:i],
 xlabel="Time",
 ylabel="Distance",
 title="Distance vs Time",
 label="d(t)"
 )

 hline!([0.02], linestyle=:dash, label="d_hat")

 # ---- Combine both ----
 plot(p1_plot, p2_plot, layout=(1,2), size=(900,400))
 end

 gif(anim, filename, fps=30)
end


# -------------------------------
# Main
# -------------------------------
function main()
 println("Running IPC simulation with diagnostics...")

 traj0, traj1, distances = simulate()

 println("Generating animation with distance plot...")

 visualize(traj0, traj1, distances)

 println("Done. Check ipc_simulation.gif")
end


# Run
main()