Skeleton/src/Algebra/AdditionalProperties/Extensions/ComplexMatrixExtension.cs

using System.Numerics;
using Skeleton.Algebra.DimensionProviders;

namespace Skeleton.Algebra.AdditionalProperties.Extensions;
using C22 = CategoryOf<IDim2>.OnField<Complex>.FMatrix;
using C33 = CategoryOf<IDim3>.OnField<Complex>.FMatrix;

/// <summary>
/// extensions for complex matrix
/// </summary>
public static class ComplexMatrixExtension
{

    private struct C22B(Complex a, Complex b, Complex c, Complex d)
    {
        public Complex X11 = a;
        public Complex X12 = b;
        public Complex X21 = c;
        public Complex X22 = d;

        public static C22B operator +(C22B a, C22B b) =>
            new(a.X11 + b.X11, a.X12 + b.X12, a.X21 + b.X21, a.X22 + b.X22);
        public static C22B operator -(C22B a, C22B b) =>
            new(a.X11 - b.X11, a.X12 - b.X12, a.X21 - b.X21, a.X22 - b.X22);

        public string PythonString => new C22(X11, X12, X21, X22).PythonRepresentation;
    }

    internal static C22 FastMul(C22 self, C22 other)
    {
        C22 res = new();
        Complex a = self[1, 1];
        Complex b = self[1, 2];
        Complex c = self[2, 1];
        Complex d = self[2, 2];
        Complex A = other[1, 1];
        Complex B = other[2, 1];
        Complex C = other[1, 2];
        Complex D = other[2, 2];

        Complex CsD = C - D;
        Complex cSa = c - a;
        Complex t = a * A;
        Complex u = cSa * CsD;
        Complex v = (c + d) * (C - A);
        Complex w = t + (cSa + d) * (A - CsD);
        Complex wPv = w + v;
        res[1, 1] = t + b* B;
        res[1, 2] = wPv + (b - d - cSa) * D;
        Complex wPu = w + u;
        Complex r21f = B - A + CsD;
        res[2, 1] = wPu + d * r21f;
        res[2, 2] = wPu + v;
        return res;
    }

    private static C22B FastFullMul(C22B a, C22B b)
    {
        Complex CsD = b.X12 - b.X22;
        Complex cSa = a.X21 - a.X11;
        Complex t = a.X11 * b.X11;
        Complex u = cSa * CsD;
        Complex v = (a.X21 + a.X22) * (b.X12 - b.X11);
        Complex w = t + (cSa + a.X22) * (b.X11 - CsD);
        Complex wPv = w + v;
        Complex wPu = w + u;
        Complex r21f = b.X21 - b.X11 + CsD;
        return new(t + a.X12 * b.X21, wPv + (a.X12 - a.X22 - cSa) * b.X22, wPu + a.X22 * r21f, wPu + v);
    }

    private static C22B FastMulP1(C22B a, C22B b) =>
        new(a.X11 * b.X11 + a.X12 * b.X21, 0, a.X21 * b.X11 + a.X22 * b.X21, 0);

    private static C22B FastMulP2(C22B a, C22B b) =>
        new(a.X11 * b.X11 + a.X12 * b.X21, a.X11 * b.X12 + a.X12 * b.X22, 0, 0);
    
    internal static C33 FastMul(C33 self, C33 other)
    {
        C22B a = new(self[1, 1], self[1, 2], self[2, 1], self[2, 2]);
        C22B A =  new(other[1, 1], other[1, 2], other[2, 1], other[2, 2]);
        C22B CsD = new(other[1, 3] - other[3, 3], 0, other[2, 3], 0);
        C22B cSa = new(self[3, 1] - self[1, 1], self[3, 2] - self[1, 2], -self[2, 1], -self[2, 2]);
        C22B CsA = new(other[1, 3] - other[1, 1], -other[1, 2], other[2, 3] - other[2, 1], -other[2, 2]);
        C22B cPd = new(self[3, 1] + self[3, 3], self[3, 2], 0, 0);
        C22B Ds_CsA = new(other[3, 3] - other[1, 3] + other[1, 1], other[1, 2], other[2, 1] - other[2, 3], other[2, 2]);
        C22B t = FastFullMul(a, A);
        C22B u = FastMulP1(cSa, CsD);
        C22B v = FastMulP2(cPd, CsA);
        C22B w = t + FastFullMul(cPd - a, Ds_CsA);
        C22B wPv = w + v;
    
        C33 res = new()
        {
            [1, 1] = t.X11 + self[1, 3] * other[3, 1],
            [1, 2] = t.X12 + self[1, 3] * other[3, 2],
            [2, 1] = t.X21 + self[2, 3] * other[3, 1],
            [2, 2] = t.X22 + self[2, 3] * other[3, 2],
            [1, 3] = wPv.X11 + (self[1, 3] - self[3, 3] - cSa.X11) * other[3, 3],
            [2, 3] = wPv.X21 + (self[2, 3] - cSa.X21) * other[3, 3]
        };

        C22B wPu = w + u;
        res[3, 1] = wPu.X11 + self[3, 3] * (other[3, 1] - other[1, 1] + CsD.X11);
        res[3, 2] = wPu.X12 + self[3, 3] * (other[3, 2] - other[1, 2] + CsD.X12);
        res[3, 3] = wPu.X11 + v.X11;
        return res;
    }

    internal static void FastSelfMul(C33 self, C33 other)
    {
        C22B a = new(self[1, 1], self[1, 2], self[2, 1], self[2, 2]);
        C22B A =  new(other[1, 1], other[1, 2], other[2, 1], other[2, 2]);
        C22B CsD = new(other[1, 3] - other[3, 3], 0, other[2, 3], 0);
        C22B cSa = new(self[3, 1] - self[1, 1], self[3, 2] - self[1, 2], -self[2, 1], -self[2, 2]);
        C22B CsA = new(other[1, 3] - other[1, 1], -other[1, 2], other[2, 3] - other[2, 1], -other[2, 2]);
        C22B cPd = new(self[3, 1] + self[3, 3], self[3, 2], 0, 0);
        C22B Ds_CsA = new(other[3, 3] - other[1, 3] + other[1, 1], other[1, 2], other[2, 1] - other[2, 3], other[2, 2]);
        C22B t = FastFullMul(a, A);
        C22B u = FastMulP1(cSa, CsD);
        C22B v = FastMulP2(cPd, CsA);
        C22B w = t + FastFullMul(cPd - a, Ds_CsA);
        C22B wPv = w + v;
        self[1, 1] = t.X11 + self[1, 3] * other[3, 1];
        self[1, 2] = t.X12 + self[1, 3] * other[3, 2];
        self[2, 1] = t.X21 + self[2, 3] * other[3, 1];
        self[2, 2] = t.X22 + self[2, 3] * other[3, 2];

        self[1, 3] = wPv.X11 + (self[1, 3] - self[3, 3] - cSa.X11) * other[3, 3];
        self[2, 3] = wPv.X21 + (self[2, 3] - cSa.X21) * other[3, 3];
        C22B wPu = w + u;
        self[3, 1] = wPu.X11 + self[3, 3] * (other[3, 1] - other[1, 1] + CsD.X11);
        self[3, 2] = wPu.X12 + self[3, 3] * (other[3, 2] - other[1, 2] + CsD.X12);
        self[3, 3] = wPu.X11 + v.X11;
    }
    
    internal static void FastSelfLMul(C33 self, C33 other)
    {
        C22B a = new(self[1, 1], self[1, 2], self[2, 1], self[2, 2]);
        C22B A =  new(other[1, 1], other[1, 2], other[2, 1], other[2, 2]);
        C22B CsD = new(other[1, 3] - other[3, 3], 0, other[2, 3], 0);
        C22B cSa = new(self[3, 1] - self[1, 1], self[3, 2] - self[1, 2], -self[2, 1], -self[2, 2]);
        C22B CsA = new(other[1, 3] - other[1, 1], -other[1, 2], other[2, 3] - other[2, 1], -other[2, 2]);
        C22B cPd = new(self[3, 1] + self[3, 3], self[3, 2], 0, 0);
        C22B Ds_CsA = new(other[3, 3] - other[1, 3] + other[1, 1], other[1, 2], other[2, 1] - other[2, 3], other[2, 2]);
        C22B t = FastFullMul(a, A);
        C22B u = FastMulP1(cSa, CsD);
        C22B v = FastMulP2(cPd, CsA);
        C22B w = t + FastFullMul(cPd - a, Ds_CsA);
        C22B wPv = w + v;

        other[1, 1] = t.X11 + self[1, 3] * other[3, 1];
        other[1, 2] = t.X12 + self[1, 3] * other[3, 2];
        other[2, 1] = t.X21 + self[2, 3] * other[3, 1];
        other[2, 2] = t.X22 + self[2, 3] * other[3, 2];

        other[1, 3] = wPv.X11 + (self[1, 3] - self[3, 3] - cSa.X11) * other[3, 3];
        other[2, 3] = wPv.X21 + (self[2, 3] - cSa.X21) * other[3, 3];
        C22B wPu = w + u;
        other[3, 1] = wPu.X11 + self[3, 3] * (other[3, 1] - A.X11 + CsD.X11);
        other[3, 2] = wPu.X12 + self[3, 3] * (other[3, 2] - A.X12 + CsD.X12);
        other[3, 3] = wPu.X11 + v.X11;
    }

    internal static void UnitaryFix<TDim>(this CategoryOf<TDim>.OnField<Complex>.FMatrix a)
    {
        if (a.Property.IsDiagonal)
        {
            for (int i = 1; i <= a.Dim; i++)
                a[i, i] /= (a[i, i] * Complex.Conjugate(a[i, i]));
        }
        else
        {
            a[1] = a[1].Normalize;
            for (int i = 2; i <= a.Dim; i++)
            {
                for (int j = 1; j < i; j++)
                    a[i] -= a[i].ProjectionOn(a[j]);
                a[i] = a[i].Normalize;
            }
        }
    }

}