using StaticArrays: SVector, SMatrix
using LinearAlgebra: eigvals, Hermitian

function project_to_unit_circle(x::Float64, l::Float64)
    φ = (x + l) * π / l
    si, co = sincos(φ)
    return SVector(co, si)
end

function project_back_from_unit_circle(θ::T, l::Float64) where {T<:Real}
    x = θ * l / π - l
    return restrict_coordinate(x, l)
end

function center_of_mass(particles::Vector{Particle}, l::Float64)
    x_proj_sum = SVector(0.0, 0.0)
    y_proj_sum = SVector(0.0, 0.0)

    for p in particles
        x_proj_sum += project_to_unit_circle(p.c[1], l)
        y_proj_sum += project_to_unit_circle(p.c[2], l)
    end

    # Prevent for example atan(1e-16, 1e-15) != 0 with rounding
    digits = 5

    # No need for 1/N with atan
    # If proj is (0, 0) then COM is 0 or L or -L. Here, 0 is choosen with θ = π
    if round(x_proj_sum[1]; digits=digits) == round(x_proj_sum[2]; digits=digits) == 0
        x_θ = π
    else
        x_θ = atan(x_proj_sum[2], x_proj_sum[1])
    end

    if round(y_proj_sum[1]; digits=digits) == round(y_proj_sum[2]; digits=digits) == 0
        y_θ = π
    else
        y_θ = atan(y_proj_sum[2], y_proj_sum[1])
    end

    COM_x = project_back_from_unit_circle(x_θ, l)
    COM_y = project_back_from_unit_circle(y_θ, l)

    return SVector(COM_x, COM_y)
end

function gyration_tensor(particles::Vector{Particle}, l::Float64)
    COM = center_of_mass(particles, l)

    S11 = 0.0
    S12 = 0.0
    S22 = 0.0

    for p in particles
        shifted_c = restrict_coordinates!(p.c - COM, l)

        S11 += shifted_c[1]^2
        S12 += shifted_c[1] * shifted_c[2]
        S22 += shifted_c[2]^2
    end

    return Hermitian(SMatrix{2,2}(S11, S12, S12, S22))
end

function gyration_tensor_eigvals_ratio(particles::Vector{Particle}, l::Float64)
    ev = eigvals(gyration_tensor(particles, l)) # Eigenvalues are sorted
    return ev[1] / ev[2]
end