using LinearAlgebra
using SpecialFunctions, FastGaussQuadrature, QuadGK
include("ho_basis.jl")

function gausslegendre_shifted(a, b, n)
    scale = (b - a) / 2
    shift = (a + b) / 2
    p, w = gausslegendre(n)
    p = p .* scale .+ shift
    w = w .* scale
    return (p, w)
end

function get_mesh(vertices::Vector, subdivs::Vector)
    p = Vector{ComplexF64}()
    w = Vector{ComplexF64}()
    for (a, b, subdiv) in zip(vertices, vertices[2:end], subdivs)
        p_new, w_new = gausslegendre_shifted(a, b, subdiv)
        append!(p, p_new)
        append!(w, w_new)
    end
    return (p, w)
end

get_V_matrix(V_pq, p, w) = V_pq.(p, transpose(p)) .* transpose(w)

get_T_matrix(p, μ) = collect(Diagonal(p.*p ./ (2*μ)))

get_H_matrix(V_pq, p, w, μ=0.5) = get_T_matrix(p, μ) + get_V_matrix(V_pq, p, w)

function identify_pole_i(p, evals, μ=0.5)
    mesh_Es = (p.*p) ./ (2*μ)
    
    current_i = 0
    current_min_ΔE = -1.0
    for i in eachindex(evals)
        min_ΔE = minimum(abs.(mesh_Es .- evals[i]))
        if min_ΔE > current_min_ΔE
            current_i = i
            current_min_ΔE = min_ΔE
        end
    end
    return current_i
end

function quick_pole_E(V_pq, μ=0.5; cs_angle=0.4, cutoff=8.0, meshpoints=256)
    p, w = get_mesh([0, cutoff * exp(-1im * cs_angle)], [meshpoints])
    evals = eigvals(get_H_matrix(V_pq, p, w, μ))
    return evals[identify_pole_i(p, evals, μ)]
end

# Gaussian potentials in momentum space
g0(R, p, q) = (exp(-(1/4)*(p + q)^2*R^2)*(-1 + exp(p*q*R^2))*R)/(2*sqrt(π))
g1(R, p, q) = (exp(-(1/4)*(p + q)^2*R^2)*(2 + p*q*R^2 + exp(p*q*R^2)*(-2 + p*q*R^2)))/(2*p*sqrt(π)*q*R)

# general potential (numerical integration)
jHat(l, z) = z * sphericalbesselj(l, z)
function Vl_mat_elem(V_of_r, l, p, q; atol=0, maxevals=10^7, R_cutoff=Inf)
    integrand(r) = jHat(l, p * r) * V_of_r(r) * jHat(l, q * r)
    (integral, _) = quadgk(integrand, 0, R_cutoff; atol=atol, maxevals=maxevals)
    return (2 / pi) * integral
end

"Return the Hamiltonian matrix (and the array of weights) for the given system."
function get_3b_H_matrix(coord_system::coordinate_system, V_of_r, vertices, subdivisions, jmax, μω_global, E_max, Λ, m=1.0, kinetic_part=true, potential_part=true; atol=10^-5, maxevals=10^5, R_cutoff=16, verbose=true)
    if coord_system == jacobi
        μ1ω1 = μω_global * 1/2
        μ2ω2 = μω_global * 2
        μ1 = m * 1/2
        μ2 = m * 2/3
    else
        error("Only Jacobi coordinates are implemented")
    end

    verbose && println("No of threads = ", Threads.nthreads())

    V_l(j, k, kp) = Vl_mat_elem(V_of_r, j, k, kp; atol=atol, maxevals=maxevals, R_cutoff=R_cutoff)

    ks, ws = get_mesh(vertices, subdivisions)
    weights = repeat(kron(ws, ws), jmax + 1)
    block_size = length(ks)
    tri((j1, j2)) = triangle_ineq(j1, j2, Λ)
    js = collect(Iterators.filter(tri, iter_prod(0:jmax, 0:jmax)))
    basis = iter_prod(js, zip(ks, ws), zip(ks, ws)) # basis = ((j1, j2), (k1, w1), (k2, w2))
    basis_size = length(js) * length(ks)^2
    @assert length(basis) == basis_size "Something wrong with the basis"
    verbose && println("Basis size = $basis_size")

    out = spzeros(basis_size, basis_size)

    @time "Block diagonal part" begin
        if kinetic_part & potential_part
            Hb_blocks = [kron_sum(get_H_matrix((k, kp) -> V_l(j1, k, kp), ks, ws, μ1), get_T_matrix(ks, μ2)) for (j1, _) in js]
        elseif kinetic_part
            Hb_blocks = [kron_sum(get_T_matrix(ks, μ1), get_T_matrix(ks, μ2)) for _ in js]
        elseif potential_part
            Hb_blocks = [kron_sum(get_V_matrix((k, kp) -> V_l(j1, k, kp), ks, ws), spzeros(block_size, block_size)) for (j1, _) in js]
        end
        out += blockdiag(sparse.(Hb_blocks)...)
    end

    if potential_part
        basis_ho = ho_basis_2B(E_max, Λ)
        verbose && println("HO basis size = ", basis_ho.dim)
    
        @time "V2_HO" V2_HO =  get_jacobi_V2_matrix(V_of_r, basis_ho, μω_global)

        @time "W_right" W_right = get_W_matrix(basis, basis_ho, μ1ω1, μ2ω2; weights=true)
        @time "W_left" W_left = get_W_matrix(basis, basis_ho, μ1ω1, μ2ω2; weights=false)

        @time "V2" out += W_left * V2_HO * transpose(W_right)
    end

    return (out, weights)
end