using StaticArrays: SVector

function project_to_unit_circle(x, l)
    φ = (x + l) * π / l
    return SVector(cos(φ), sin(φ))
end

function project_back_from_unit_circle(θ, l)
    x = θ * l / π - l
    return restrict_coordinate(x; l=l)
end

function center_of_mass(args)
    x_proj_sum = SVector(0, 0)
    y_proj_sum = SVector(0, 0)

    for p in args.particles
        x_proj_sum += project_to_unit_circle(p.c[1], args.l)
        y_proj_sum += project_to_unit_circle(p.c[2], args.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) == 0) &&
        (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) == 0) &&
        (round(y_proj_sum[2]; digits=digits) == 0)
        y_θ = π
    else
        y_θ = atan(y_proj_sum[2], y_proj_sum[1])
    end

    return SVector(
        project_back_from_unit_circle(x_θ, args.l),
        project_back_from_unit_circle(y_θ, args.l),
    )
end