From 9434f5999d1ee5b9d9821fb4bdc17e770c229418 Mon Sep 17 00:00:00 2001
From: Marek Kaluba <kalmar@amu.edu.pl>
Date: Fri, 6 Oct 2023 14:20:34 +0200
Subject: [PATCH] add functions to decompose vector in invariant basis

---
 src/sos_sdps.jl | 34 ++++++++++++++++++++++++++++++++++
 1 file changed, 34 insertions(+)

diff --git a/src/sos_sdps.jl b/src/sos_sdps.jl
index 2b264d7..df311ec 100644
--- a/src/sos_sdps.jl
+++ b/src/sos_sdps.jl
@@ -36,6 +36,40 @@ function sos_problem_dual(
     return model
 end
 
+function decompose(
+    elt::StarAlgebras.AlgebraElement,
+    wd::WedderburnDecomposition,
+)
+    v = StarAlgebras.coeffs(elt)
+    cfs, error = decompose(v, invariant_vectors(wd))
+    _eps = length(v) * eps(typeof(error))
+    error < _eps || @warn "elt does not seem to be invariant" error
+    return cfs
+end
+
+function decompose(v::AbstractVector, invariant_vecs)
+    # TODO: eltype for current, res ?
+    current = similar(v, Float64)
+    current .= 0.0
+    res = SparseArrays.spzeros(length(invariant_vecs))
+
+    _eps = length(current) * eps(eltype(res))
+    diff = zero(current)
+
+    for (i, iv) in enumerate(invariant_vecs)
+        cf = dot(v, iv) / dot(iv, iv)
+        if !iszero(cf)
+            res[i] = cf
+            current .+= cf .* iv
+            diff .= current .- v
+            if norm(diff) < _eps
+                break
+            end
+        end
+    end
+    return res, norm(current - v)
+end
+
 """
     sos_problem_primal(X, [u = zero(X); upper_bound=Inf])
 Formulate sum of squares decomposition problem for `X - λ·u`.