//#####################################################################
// Copyright 2024, Craig Schroeder.
// This file is part of PhysBAM whose distribution is governed by the license contained in the accompanying file PHYSBAM_COPYRIGHT.txt.
//#####################################################################
// Header MATRIX_OPERATIONS
//#####################################################################
#ifndef __MATRIX_OPERATIONS__
#define __MATRIX_OPERATIONS__

#include <Core/Arrays/ARRAYS_FORWARD.h>
#include <Core/Data_Structures/ELEMENT_ID.h>
#include <Core/Math_Tools/FIXED_NUMBER.h>
#include <Core/Math_Tools/minmax.h>
#include <Core/Matrices/MATRIX_FORWARD.h>
#include <algorithm>
#include <cassert>
#include <cmath>
#include <utility>
namespace PhysBAM{

template<class T> struct SCALE_MATRIX_DYN{T x;static constexpr int m=-1,n=-1,type=mat_scale;};
template<int i,int j,class T> auto get(const SCALE_MATRIX_DYN<T>& a)
{
    if constexpr(i==j) return a.x;
    return T{};
}
template<class T> auto diag(const SCALE_MATRIX_DYN<T>& a,int i){return a.x;}
template<class T> auto lower_(const SCALE_MATRIX_DYN<T>& a,int i,int j){return T{};}
template<class T> auto upper_(const SCALE_MATRIX_DYN<T>& a,int i,int j){return T{};}

#define ASSERT_COMPAT(a,m,b,n) \
    if constexpr(remove_reference_t<decltype(a)>::fixed_##m>=0 && remove_reference_t<decltype(b)>::fixed_##n>=0)\
        {static_assert(remove_reference_t<decltype(a)>::fixed_##m==remove_reference_t<decltype(b)>::fixed_##n);}\
    else{assert(a.m<0 || b.n<0 || a.m==b.n);}

constexpr std::pair<int,int> sym_idx(int d,int k)
{
    int j=d-1-((int)std::sqrt((d*(d+1)/2-k-1)*8+1)-1)/2;
    int i=k-(2*d-j-1)*j/2;
    return {i,j};
}

constexpr int sym_i(int d,int k){return sym_idx(d,k).first;}
constexpr int sym_j(int d,int k){return sym_idx(d,k).second;}

constexpr bool is_diag(std::pair<int,int> p){return p.first==p.second;}
constexpr bool sym_diag(int d,int k){return is_diag(sym_idx(d,k));}

constexpr int scale_type(int a)
{
    if(a==mat_id) return mat_scale;
    return a;
}

constexpr int add_type(int a,int b)
{
    if(a==mat_id && b==mat_id) return mat_scale;
    return std::min(a,b);
}

constexpr int sub_type(int a,int b)
{
    if(a==mat_id && b==mat_id) return mat_zero;
    if(a==mat_zero && b==mat_id) return mat_scale;
    return std::min(a,b);
}

constexpr int mul_type(int a,int b)
{
    if(a>b) return mul_type(b,a);
    if(b==mat_zero) return mat_zero;
    if(a>=mat_diag) return a;
    if(a==mat_sym && b>=mat_scale) return mat_sym;
    return mat_gen;
}

constexpr int mul_type_wsr(int a,int b)
{
    if(a>b) return mul_type(b,a);
    if(b==mat_zero) return mat_zero;
    if(a>=mat_diag) return a;
    return mat_sym;
}

constexpr int make_sym(int a)
{
    if(a>=mat_sym) return a;
    return mat_sym;
}

constexpr int mul_vec(int m,int v)
{
    if(m==mat_zero || v==vec_zero) return vec_zero;
    return vec_gen;
}

// matrix + matrix
template<class A,class B,class T=typename A::SCALAR,
    int m=std::max(A::fixed_m,B::fixed_m),
    int n=std::max(A::fixed_n,B::fixed_n),
    class R=MAT_TYPE<T,add_type(A::type,B::type),m,n>>
std::enable_if_t<IS_MATRIX<A>::value&&IS_MATRIX<B>::value,R>
__attribute__((flatten))
operator+(const A& a,const B& b)
{
    ASSERT_COMPAT(a,m,b,m);
    ASSERT_COMPAT(a,n,b,n);
    typedef NO_MATRIX C;C c;
    int s=0,dyn_m=a.Rows(),dyn_n=a.Columns();
#define FUNCTION(a,b,c,s) ((a)+(b))
#include <Core/Matrices/MATRIX_OPERATIONS_FILL.h>
}

// matrix - matrix
template<class A,class B,class T=typename A::SCALAR,
    int m=std::max(A::fixed_m,B::fixed_m),
    int n=std::max(A::fixed_n,B::fixed_n),
    class R=MAT_TYPE<T,sub_type(A::type,B::type),m,n>>
std::enable_if_t<IS_MATRIX<A>::value&&IS_MATRIX<B>::value,R>
__attribute__((flatten))
operator-(const A& a,const B& b)
{
    ASSERT_COMPAT(a,m,b,m);
    ASSERT_COMPAT(a,n,b,n);
    typedef NO_MATRIX C;C c;
    int s=0,dyn_m=a.Rows(),dyn_n=a.Columns();
#define FUNCTION(a,b,c,s) ((a)-(b))
#include <Core/Matrices/MATRIX_OPERATIONS_FILL.h>
}

// matrix * scalar
template<class A,class T=typename A::SCALAR,
    int m=A::fixed_m,int n=A::fixed_n,
    class R=MAT_TYPE<T,scale_type(A::type),m,n>>
std::enable_if_t<IS_MATRIX<A>::value,R>
__attribute__((flatten))
operator*(const A& a,ND<T> s)
{
    typedef NO_MATRIX B;B b;
    typedef NO_MATRIX C;C c;
    int dyn_m=a.Rows(),dyn_n=a.Columns();
#define FUNCTION(a,b,c,s) ((a)*(s))
#include <Core/Matrices/MATRIX_OPERATIONS_FILL.h>
}

// matrix / scalar
template<class A,class T=typename A::SCALAR,
    int m=A::fixed_m,int n=A::fixed_n,
    class R=MAT_TYPE<T,scale_type(A::type),m,n>>
std::enable_if_t<IS_MATRIX<A>::value,R>
__attribute__((flatten))
operator/(const A& a,ND<T> s)
{
    typedef NO_MATRIX B;B b;
    typedef NO_MATRIX C;C c;
    int dyn_m=a.Rows(),dyn_n=a.Columns();
#define FUNCTION(a,b,c,s) ((a)/(s))
#include <Core/Matrices/MATRIX_OPERATIONS_FILL.h>
}

template<class B,class U=std::enable_if_t<IS_MATRIX<B>::value>>
auto
__attribute__((flatten))
operator*(typename B::SCALAR a,const B& b)
{
    return b*a;
}

// matrix + scalar
template<class A,class T=typename A::SCALAR,
    int m=A::fixed_m,int n=A::fixed_n,
    class R=MAT_TYPE<T,add_type(A::type,mat_scale),m,n>>
std::enable_if_t<IS_MATRIX<A>::value&&compat(A::fixed_m,A::fixed_n),R>
__attribute__((flatten))
operator+(const A& a,ND<T> s)
{
    ASSERT_COMPAT(a,m,a,n);
    typedef SCALE_MATRIX_DYN<T> B;B b{s};
    typedef NO_MATRIX C;C c;
    int dyn_m=a.Rows(),dyn_n=a.Columns();
#define FUNCTION(a,b,c,s) ((a)+(b))
#include <Core/Matrices/MATRIX_OPERATIONS_FILL.h>
}

// matrix - scalar
template<class A,class T=typename A::SCALAR,
    int m=A::fixed_m,int n=A::fixed_n,
    class R=MAT_TYPE<T,sub_type(A::type,mat_scale),m,n>>
std::enable_if_t<IS_MATRIX<A>::value&&compat(A::fixed_m,A::fixed_n),R>
__attribute__((flatten))
operator-(const A& a,ND<T> s)
{
    ASSERT_COMPAT(a,m,a,n);
    typedef SCALE_MATRIX_DYN<T> B;B b{s};
    typedef NO_MATRIX C;C c;
    int dyn_m=a.Rows(),dyn_n=a.Columns();
#define FUNCTION(a,b,c,s) ((a)-(b))
#include <Core/Matrices/MATRIX_OPERATIONS_FILL.h>
}

template<class B,class U=std::enable_if_t<IS_MATRIX<B>::value>>
auto
__attribute__((flatten))
operator+(typename B::SCALAR a,const B& b)
{
    return b+a;
}

// scalar - matrix
template<class A,class T=typename A::SCALAR,
    int m=A::fixed_m,int n=A::fixed_n,
    class R=MAT_TYPE<T,sub_type(mat_scale,A::type),m,n>>
std::enable_if_t<IS_MATRIX<A>::value&&compat(A::fixed_m,A::fixed_n),R>
__attribute__((flatten))
operator-(ND<T> s,const A& a)
{
    ASSERT_COMPAT(a,m,a,n);
    typedef SCALE_MATRIX_DYN<T> B;B b{s};
    typedef NO_MATRIX C;C c;
    int dyn_m=a.Rows(),dyn_n=a.Columns();
#define FUNCTION(a,b,c,s) ((b)-(a))
#include <Core/Matrices/MATRIX_OPERATIONS_FILL.h>
}

// -matrix
template<class A,class T=typename A::SCALAR,
    int m=A::fixed_m,int n=A::fixed_n,
    class R=MAT_TYPE<T,scale_type(A::type),m,n>>
std::enable_if_t<IS_MATRIX<A>::value,R>
__attribute__((flatten))
operator-(const A& a)
{
    typedef NO_MATRIX B;B b;
    typedef NO_MATRIX C;C c;
    int s=0,dyn_m=a.Rows(),dyn_n=a.Columns();
#define FUNCTION(a,b,c,s) (-(a))
#include <Core/Matrices/MATRIX_OPERATIONS_FILL.h>
}

// matrix * matrix
#pragma GCC diagnostic ignored "-Wsequence-point"
#pragma GCC diagnostic ignored "-Wunused-value"
template<class A,class B,
    class T=typename A::SCALAR,
    int m=A::fixed_m,int n=B::fixed_n,int p=std::max(A::fixed_n,B::fixed_m),
    class R=MAT_TYPE<T,mul_type(A::type,B::type),m,n>>
std::enable_if_t<IS_MATRIX<A>::value&&IS_MATRIX<B>::value,R>
__attribute__((flatten))
operator*(const A& a,const B& b)
{
    ASSERT_COMPAT(a,n,b,m);
    int dyn_m=a.Rows(),dyn_n=b.Columns(),dyn_p=a.Columns();
#include <Core/Matrices/MATRIX_OPERATIONS_FILL_MULT.h>
}

// Transpose_Times(matrix, matrix)
#pragma GCC diagnostic ignored "-Wsequence-point"
#pragma GCC diagnostic ignored "-Wunused-value"
template<class A,class B,
    class T=typename A::SCALAR,
    int m=A::fixed_n,int n=B::fixed_n,int p=std::max(A::fixed_m,B::fixed_m),
    class R=MAT_TYPE<T,mul_type(A::type,B::type),m,n>>
std::enable_if_t<IS_MATRIX<A>::value&&IS_MATRIX<B>::value,R>
__attribute__((flatten))
Transpose_Times(const A& a,const B& b)
{
    ASSERT_COMPAT(a,m,b,m);
    int dyn_m=a.Columns(),dyn_n=b.Columns(),dyn_p=a.Rows();
#define TRANS_A
#include <Core/Matrices/MATRIX_OPERATIONS_FILL_MULT.h>
}

// Times_Transpose(matrix, matrix)
#pragma GCC diagnostic ignored "-Wsequence-point"
#pragma GCC diagnostic ignored "-Wunused-value"
template<class A,class B,
    class T=typename A::SCALAR,
    int m=A::fixed_m,int n=B::fixed_m,int p=std::max(A::fixed_n,B::fixed_n),
    class R=MAT_TYPE<T,mul_type(A::type,B::type),m,n>>
std::enable_if_t<IS_MATRIX<A>::value&&IS_MATRIX<B>::value,R>
__attribute__((flatten))
Times_Transpose(const A& a,const B& b)
{
    ASSERT_COMPAT(a,n,b,n);
    int dyn_m=a.Rows(),dyn_n=b.Rows(),dyn_p=a.Columns();
#define TRANS_B
#include <Core/Matrices/MATRIX_OPERATIONS_FILL_MULT.h>
}

// Times_With_Symmetric_Result(matrix,matrix)
#pragma GCC diagnostic ignored "-Wsequence-point"
#pragma GCC diagnostic ignored "-Wunused-value"
template<class A,class B,
    class T=typename A::SCALAR,
    int m=A::fixed_m,int n=B::fixed_n,int p=std::max(A::fixed_n,B::fixed_m),
    class R=MAT_TYPE<T,mul_type_wsr(A::type,B::type),m,n>>
std::enable_if_t<IS_MATRIX<A>::value&&IS_MATRIX<B>::value,R>
__attribute__((flatten))
Times_With_Symmetric_Result(const A& a,const B& b)
{
    ASSERT_COMPAT(a,n,b,m);
    int dyn_m=a.Rows(),dyn_n=b.Columns(),dyn_p=a.Columns();
#include <Core/Matrices/MATRIX_OPERATIONS_FILL_MULT.h>
}

// Transpose_Times_With_Symmetric_Result(matrix, matrix)
#pragma GCC diagnostic ignored "-Wsequence-point"
#pragma GCC diagnostic ignored "-Wunused-value"
template<class A,class B,
    class T=typename A::SCALAR,
    int m=A::fixed_n,int n=B::fixed_n,int p=std::max(A::fixed_m,B::fixed_m),
    class R=MAT_TYPE<T,mul_type_wsr(A::type,B::type),m,n>>
std::enable_if_t<IS_MATRIX<A>::value&&IS_MATRIX<B>::value,R>
__attribute__((flatten))
Transpose_Times_With_Symmetric_Result(const A& a,const B& b)
{
    ASSERT_COMPAT(a,m,b,m);
    int dyn_m=a.Columns(),dyn_n=b.Columns(),dyn_p=a.Rows();
#define TRANS_A
#include <Core/Matrices/MATRIX_OPERATIONS_FILL_MULT.h>
}

// Times_Transpose_With_Symmetric_Result(matrix, matrix)
#pragma GCC diagnostic ignored "-Wsequence-point"
#pragma GCC diagnostic ignored "-Wunused-value"
template<class A,class B,
    class T=typename A::SCALAR,
    int m=A::fixed_m,int n=B::fixed_m,int p=std::max(A::fixed_n,B::fixed_n),
    class R=MAT_TYPE<T,mul_type_wsr(A::type,B::type),m,n>>
std::enable_if_t<IS_MATRIX<A>::value&&IS_MATRIX<B>::value,R>
__attribute__((flatten))
Times_Transpose_With_Symmetric_Result(const A& a,const B& b)
{
    ASSERT_COMPAT(a,n,b,n);
    int dyn_m=a.Rows(),dyn_n=b.Rows(),dyn_p=a.Columns();
#define TRANS_B
#include <Core/Matrices/MATRIX_OPERATIONS_FILL_MULT.h>
}

// matrix * vector
#define FUNCTION_NAME operator*
#define TRANS(x,y) x,y
#include <Core/Matrices/MATRIX_OPERATIONS_MV_MULT.h>

template<class A,class T=typename A::SCALAR,
    class R=ZERO_VECTOR<T,A::fixed_m>>
std::enable_if_t<IS_MATRIX<A>::value,R>
operator*(const A& a,const ZERO_VECTOR<T,A::fixed_n>& y)
{return {};}

// Transpose_Times(matrix, vector)
#define FUNCTION_NAME Transpose_Times
#define TRANS(x,y) y,x
#include <Core/Matrices/MATRIX_OPERATIONS_MV_MULT.h>

template<class A,class T=typename A::SCALAR,
    class R=ZERO_VECTOR<T,A::fixed_n>>
std::enable_if_t<IS_MATRIX<A>::value,R>
Transpose_Times(const A& a,const ZERO_VECTOR<T,A::fixed_m>& y)
{return {};}

// matrix += matrix
template<class A,class B,class T=typename A::SCALAR,
    int m=std::max(A::fixed_m,B::fixed_m),
    int n=std::max(A::fixed_n,B::fixed_n)>
std::enable_if_t<IS_MATRIX<A>::value&&IS_MATRIX<B>::value,A&>
__attribute__((flatten))
operator+=(A& a,const B& b)
{
    ASSERT_COMPAT(a,m,b,m);
    ASSERT_COMPAT(a,n,b,n);
    typedef NO_MATRIX C;C c;
    int s=0;
#define FUNCTION(a,b,c,s) (a)+=(b)
#include <Core/Matrices/MATRIX_OPERATIONS_VISIT.h>
    return a;
}

// matrix -= matrix
template<class A,class B,class T=typename A::SCALAR,
    int m=std::max(A::fixed_m,B::fixed_m),
    int n=std::max(A::fixed_n,B::fixed_n)>
std::enable_if_t<IS_MATRIX<A>::value&&IS_MATRIX<B>::value,A&>
__attribute__((flatten))
operator-=(A& a,const B& b)
{
    ASSERT_COMPAT(a,m,b,m);
    ASSERT_COMPAT(a,n,b,n);
    typedef NO_MATRIX C;C c;
    int s=0;
#define FUNCTION(a,b,c,s) (a)-=(b)
#include <Core/Matrices/MATRIX_OPERATIONS_VISIT.h>
    return a;
}

// matrix *= scalar
template<class A,class T=typename A::SCALAR,int m=A::fixed_m,int n=A::fixed_n>
std::enable_if_t<IS_MATRIX<A>::value,A&>
__attribute__((flatten))
operator*=(A& a,ND<T> s)
{
    typedef NO_MATRIX B;B b;
    typedef NO_MATRIX C;C c;
#define FUNCTION(a,b,c,s) (a)*=(s)
#include <Core/Matrices/MATRIX_OPERATIONS_VISIT.h>
    return a;
}

// matrix /= scalar
template<class A,class T=typename A::SCALAR,int m=A::fixed_m,int n=A::fixed_n>
std::enable_if_t<IS_MATRIX<A>::value,A&>
__attribute__((flatten))
operator/=(A& a,ND<T> s)
{
    typedef NO_MATRIX B;B b;
    typedef NO_MATRIX C;C c;
#define FUNCTION(a,b,c,s) (a)/=(s)
#include <Core/Matrices/MATRIX_OPERATIONS_VISIT.h>
    return a;
}

// matrix += scalar
template<class A,class T=typename A::SCALAR,int m=A::fixed_m,int n=A::fixed_n>
std::enable_if_t<IS_MATRIX<A>::value,A&>
__attribute__((flatten))
operator+=(A& a,ND<T> s)
{
    typedef SCALE_MATRIX_DYN<T> B;B b{s};
    typedef NO_MATRIX C;C c;
#define FUNCTION(a,b,c,s) (a)+=(b)
#include <Core/Matrices/MATRIX_OPERATIONS_VISIT.h>
    return a;
}

// matrix -= scalar
template<class A,class T=typename A::SCALAR,int m=A::fixed_m,int n=A::fixed_n>
std::enable_if_t<IS_MATRIX<A>::value,A&>
__attribute__((flatten))
operator-=(A& a,ND<T> s)
{
    typedef SCALE_MATRIX_DYN<T> B;B b{s};
    typedef NO_MATRIX C;C c;
#define FUNCTION(a,b,c,s) (a)-=(b)
#include <Core/Matrices/MATRIX_OPERATIONS_VISIT.h>
    return a;
}

template<class A,class B,
    class C=std::enable_if_t<IS_MATRIX<A>::value&&IS_MATRIX<B>::value&&B::is_sym>>
auto Conjugate(const A& a,const B& b)
{
    return Times_Transpose_With_Symmetric_Result(a*b,a);
}

template<class A,class B,
    class C=std::enable_if_t<IS_MATRIX<A>::value&&IS_MATRIX<B>::value&&B::is_sym>>
auto Conjugate_With_Transpose(const A& a,const B& b)
{
    return Transpose_Times_With_Symmetric_Result(a,b*a);
}

template<class A,class T=typename A::SCALAR,
    int m=A::fixed_m,int n=A::fixed_n,class R=VEC_TYPE<T,vec_gen,m>>
std::enable_if_t<IS_MATRIX<A>::value&&!A::is_diag,R>
Get_Column(const A& a,int j)
{
    assert(j<a.Columns());
    if constexpr(m>=0 && m<=size_limit)
    {
        return []<int... i>(const auto& a,int j,std::integer_sequence<int,i...>)
            {
                return R{(a(i,j))...};
            }(a,j,std::make_integer_sequence<int,m>{});
    }
    else
    {
        R r((INITIAL_SIZE)a.Columns());
        for(int i=0;i<r.m;i++) r(i)=a(i,j);
        return r;
    }
}

template<class A,class T=typename A::SCALAR,
    int m=A::fixed_m,int n=A::fixed_n,class R=VEC_TYPE<T,vec_gen,n>>
std::enable_if_t<IS_MATRIX<A>::value&&!A::is_diag,R>
Get_Row(const A& a,int j)
{
    assert(j<a.Rows());
    if constexpr(n>=0 && n<=size_limit)
    {
        return []<int... i>(const auto& a,int j,std::integer_sequence<int,i...>)
            {
                return R{(a(j,i))...};
            }(a,j,std::make_integer_sequence<int,n>{});
    }
    else
    {
        R r((INITIAL_SIZE)a.Rows());
        for(int i=0;i<r.m;i++) r(i)=a(j,i);
        return r;
    }
}

template<class A,class B,class T=typename A::SCALAR,
    int m=A::fixed_m,int n=A::fixed_n,class R=VEC_TYPE<T,vec_gen,m>>
std::enable_if_t<IS_MATRIX<A>::value&&!A::is_diag>
Set_Column(A& a,int j,const ARRAY_BASE<T,B>& b)
{
    assert(j<a.Columns());
    if constexpr(m>=0 && m<=size_limit)
    {
        []<int... i>(auto& a,const auto& b,int j,std::integer_sequence<int,i...>)
            {
                ((a(i,j)=b(i)),...);
            }(a,b,j,std::make_integer_sequence<int,m>{});
    }
    else
        for(int i=0,k=b.Size();i<k;i++) a(i,j)=b(i);
}

template<class A,class B,class T=typename A::SCALAR,
    int m=A::fixed_m,int n=A::fixed_n,class R=VEC_TYPE<T,vec_gen,n>>
std::enable_if_t<IS_MATRIX<A>::value&&!A::is_diag>
Set_Row(A& a,int j,const ARRAY_BASE<T,B>& b)
{
    assert(j<a.Rows());
    if constexpr(n>=0 && n<=size_limit)
    {
        []<int... i>(auto& a,const auto& b,int j,std::integer_sequence<int,i...>)
            {
                ((a(j,i)=b(i)),...);
            }(a,b,j,std::make_integer_sequence<int,n>{});
    }
    else
        for(int i=0,k=b.Size();i<k;i++) a(j,i)=b(i);
}

template<class A,class B,class T=typename A::SCALAR,
    int m=A::fixed_m,int n=A::fixed_n,class R=VEC_TYPE<T,vec_gen,m>>
std::enable_if_t<IS_MATRIX<A>::value&&!A::is_diag>
Add_Column(A& a,int j,const ARRAY_BASE<T,B>& b)
{
    assert(j<a.Columns());
    if constexpr(m>=0 && m<=size_limit)
    {
        []<int... i>(auto& a,const auto& b,int j,std::integer_sequence<int,i...>)
            {
                ((a(i,j)+=b(i)),...);
            }(a,b,j,std::make_integer_sequence<int,m>{});
    }
    else
        for(int i=0,k=b.Size();i<k;i++) a(i,j)+=b(i);
}

template<class A,class B,class T=typename A::SCALAR,
    int m=A::fixed_m,int n=A::fixed_n,class R=VEC_TYPE<T,vec_gen,n>>
std::enable_if_t<IS_MATRIX<A>::value&&!A::is_diag>
Add_Row(A& a,int j,const ARRAY_BASE<T,B>& b)
{
    assert(j<a.Rows());
    if constexpr(n>=0 && n<=size_limit)
    {
        []<int... i>(auto& a,const auto& b,int j,std::integer_sequence<int,i...>)
            {
                ((a(j,i)+=b(i)),...);
            }(a,b,j,std::make_integer_sequence<int,n>{});
    }
    else
        for(int i=0,k=b.Size();i<k;i++) a(j,i)+=b(i);
}

template<class A,class B,class T=typename A::SCALAR,
    int am=A::fixed_m,int an=A::fixed_n,
    int bm=B::fixed_m,int bn=B::fixed_n,
    int m=std::max(am,bm),int n=std::max(an,bn)>
std::enable_if_t<
    IS_MATRIX<A>::value&&IS_MATRIX<B>::value&&
    compat(am,bm)&&compat(an,bn),T>
Double_Contract(const A& a,const B& b)
{
    ASSERT_COMPAT(a,m,b,m);
    ASSERT_COMPAT(a,n,b,n);
    // First handle cases that the VISIT implementation will not get right.
    if constexpr(A::type==mat_id) return b.Trace();
    else if constexpr(B::type==mat_id) return a.Trace();
    else if constexpr(A::type==mat_scale) return a.x*b.Trace();
    else if constexpr(B::type==mat_scale) return b.x*a.Trace();
    // Visitor does not visit all entries if A::type==mat_sym.
    if constexpr(A::type==mat_sym && B::type==mat_gen) return Double_Contract(b,a);
    else if constexpr(A::type==mat_sym && B::type==mat_sym)
    {
        // The VISIT routine will not visit both top and bottom, so a special
        // implementation is required here.
        // Accumulate strictly lower part first.
        T s=[]<int... i>(auto&& a,auto&& b,std::integer_sequence<int,i...>)
            {
                return (0+...+(a.Element_Lower(sym_i(m-1,i)+1,sym_j(m-1,i))*b.Element_Lower(sym_i(m-1,i)+1,sym_j(m-1,i))));
            }(a,b,std::make_integer_sequence<int,m*(m-1)/2>{});
        // Double the off-diagonal part, then add in diagonal part.
        return s*2+[]<int... i>(auto&& a,auto&& b,std::integer_sequence<int,i...>)
            {
                return (0+...+(a.Element_Lower(i,i)*b.Element_Lower(i,i)));
            }(a,b,std::make_integer_sequence<int,m>{});
    }
    else
    {
        T s{};
        typedef NO_MATRIX C;C c;
#define FUNCTION(a,b,c,s) (s)+=(a)*(b)
#include <Core/Matrices/MATRIX_OPERATIONS_VISIT.h>
        return s;
    }
}

template<class A,class T=typename A::SCALAR,
    int m=A::fixed_n,int n=A::fixed_m,
    class R=MAT_TYPE<T,A::type,m,n>>
std::enable_if_t<IS_MATRIX<A>::value&&!A::is_sym,R>
Transposed(const A& a)
{
    typedef NO_MATRIX B;B b;
    typedef NO_MATRIX C;C c;
    int s=0,dyn_m=a.Columns(),dyn_n=a.Rows();
#define TRANS_A
#define FUNCTION(a,b,c,s) (a)
#include <Core/Matrices/MATRIX_OPERATIONS_FILL.h>
}

template<class A>
std::enable_if_t<IS_MATRIX<A>::value&&A::is_sym,const A&>
Transposed(const A& a)
{
    return a;
}

template<class A,class T=typename A::SCALAR,
    int m=A::fixed_m,int n=A::fixed_n>
std::enable_if_t<IS_MATRIX<A>::value&&compat(m,n),T>
Trace(const A& a)
{
    if constexpr(m<0){assert(a.Rows()==a.Columns());}
    if constexpr(m==0) return T{};
    else if constexpr(m>=0 && m<=size_limit)
    {
        return []<int... i>(const auto& a,std::integer_sequence<int,i...>)
            {
                return ((a(i,i))+...);
            }(a,std::make_integer_sequence<int,m>{});
    }
    else
    {
        T z{};
        for(int i=0,mm=a.Rows();i<mm;i++)
            z+=a(i,i);
        return z;
    }
}

template<class A,class T=typename A::SCALAR,
    int m=A::fixed_m,int n=A::fixed_n,class R=DIAGONAL_MATRIX<T,m>>
std::enable_if_t<IS_MATRIX<A>::value&&m>=0&&compat(m,n),R>
Diagonal_Part(const A& a)
{
    if constexpr(m>=0 && m<=size_limit)
    {
        return []<int... i>(const auto& a,std::integer_sequence<int,i...>)
            {
                return R{(a(i,i))...};
            }(a,std::make_integer_sequence<int,m>{});
    }
    else
    {
        R z;
        for(int i=0;i<m;i++)
            z(i,i)=a(i,i);
        return z;
    }
}
template<class A,class T=typename A::SCALAR,
    int m=A::fixed_m,int n=A::fixed_n>
std::enable_if_t<IS_MATRIX<A>::value&&!A::is_sym,T>
Max_Abs(const A& a)
{
    if constexpr(m==0 || n==0) return T{};
    else if constexpr(m>=0 && m<=size_limit && n<=size_limit)
    {
        return []<int... i>(const auto& a,std::integer_sequence<int,i...>)
            {
                return maxabs((a(i%m,i/m))...);
            }(a,std::make_integer_sequence<int,m*n>{});
    }
    else
    {
        T z{};
        for(int j=0,mm=a.Rows(),nn=a.Columns();j<nn;j++)
            for(int i=0;i<mm;i++)
                z=max(z,abs(a(i,j)));
        return z;
    }
}
template<class T,int d> T
Max_Abs(const SYMMETRIC_MATRIX<T,d>& a)
{
    if constexpr(d==0) return T{};
    else if constexpr(d<=size_limit)
    {
        return []<int... i>(const auto& a,std::integer_sequence<int,i...>)
            {
                return maxabs((a.x[i])...);
            }(a,std::make_integer_sequence<int,d*(d+1)/2>{});
    }
    else
    {
        T z{};
        for(int i=0;i<d*(d+1)/2;i++)
            z=max(z,abs(a.x[i]));
        return z;
    }
}
template<class T,int d> T
Max_Abs(const DIAGONAL_MATRIX<T,d>& a)
{
    return a.x.Max_Abs();
}

template<class A,class T=typename A::SCALAR,
    int m=A::fixed_m,int n=A::fixed_n,
    class R=MAT_TYPE<T,make_sym(A::type),m,n>>
std::enable_if_t<IS_MATRIX<A>::value&&!A::is_sym,R>
__attribute__((flatten))
Twice_Symmetric_Part(const A& a)
{
    ASSERT_COMPAT(a,m,a,n);
    auto& b=a;
    typedef A B;
    typedef NO_MATRIX C;C c;
    int s=0,dyn_m=a.Rows(),dyn_n=a.Columns();
#define TRANS_B
#define FUNCTION(a,b,c,s) ((a)+(b))
#include <Core/Matrices/MATRIX_OPERATIONS_FILL.h>
}

template<class A,class K=std::enable_if_t<IS_MATRIX<A>::value&&A::is_sym>> auto
__attribute__((flatten))
Twice_Symmetric_Part(const A& a)
{
    return a*2;
}

template<class A,class T=typename A::SCALAR,
    int m=A::fixed_m,int n=A::fixed_n,
    class R=MAT_TYPE<T,make_sym(A::type),m,n>>
std::enable_if_t<IS_MATRIX<A>::value&&!A::is_sym,R>
__attribute__((flatten))
Symmetric_Part(const A& a)
{
    ASSERT_COMPAT(a,m,a,n);
    auto& b=a;
    typedef A B;
    typedef NO_MATRIX C;C c;
    int s=0,dyn_m=a.Rows(),dyn_n=a.Columns();
#define TRANS_B
#define FUNCTION(a,b,c,s) (T).5*((a)+(b))
#include <Core/Matrices/MATRIX_OPERATIONS_FILL.h>
}

template<class A,class K=std::enable_if_t<IS_MATRIX<A>::value&&A::is_sym>>
const A&
Symmetric_Part(const A& a)
{
    return a;
}
template<class A,class B,class T=typename A::SCALAR,
    int m=std::max(A::fixed_m,B::fixed_m),
    int n=std::max(A::fixed_n,B::fixed_n)>
std::enable_if_t<IS_MATRIX<A>::value&&IS_MATRIX<B>::value,bool>
__attribute__((flatten))
operator==(const A& a,const B& b)
{
    ASSERT_COMPAT(a,m,b,m);
    ASSERT_COMPAT(a,n,b,n);
    if constexpr(A::type>B::type) return b==a; // eliminate about half the cases.
    else if constexpr(m==0 || n==0) return true; // Empty matrices are equal.
    else if constexpr(A::type>=mat_id) return A::type==B::type; // identity and zero matrices.
    // scale/id/zero matrices are zero off diagonal and constant on diagonal.
    else if constexpr(A::type==mat_scale) return a.x==b(0,0);
    // at least one general diagonal matrix; must check whole diagonal, off diag is always zero.
    else if constexpr(A::type==mat_diag)
    {
        if constexpr(m<=size_limit)
        {
            return []<int... i>(auto&& a,auto&& b,std::integer_sequence<int,i...>)
                {
                    return ((a(i,i)==b(i,i))&&...);
                }(a,b,std::make_integer_sequence<int,m>{});
        }
        else // Too big, generate dynamically.
        {
            for(int i=0;i<m;i++)
                if(a(i,i)!=b(i,i))
                    return false;
            return true;
        }
    }
    // At least one general symmetric matrix, must check lower diag part.
    else if constexpr(A::type==mat_sym)
    {
        // Do small-size first.
        if constexpr(m<=size_limit)
        {
            return []<int... i>(auto&& a,auto&& b,std::integer_sequence<int,i...>)
                {
                    return ((a.x[i]==get<sym_i(m,i),sym_j(m,i)>(b))&&...);
                }(a,b,std::make_integer_sequence<int,m*(m+1)/2>{});
        }
        else if constexpr(B::type==mat_sym) // sym+sym is easy, so do that next.
        {
            for(int i=0;i<m*(m+1)/2;i++)
                if(a.x[i]!=b.x[i])
                    return false;
            return true;
        }
        else // B is diagonal; check diag and off-diag separately.
        {
            for(int i=0;i<m;i++)
                if(a(i,i)!=b(i,i))
                    return false;
            for(int j=0;j<m;j++)
                for(int i=j+1;i<m;i++)
                    if(a.Element_Lower(i,j)!=T{})
                        return false;
            return true;
        }
    }
    // A is a general matrix; do small sizes first, checking all entries.
    else if constexpr(m>=0 && m<=size_limit && n<=size_limit)
    {
        return []<int... i>(auto&& a,auto&& b,std::integer_sequence<int,i...>)
            {
                return ((get<i%m,i/m>(a)==get<i%m,i/m>(b))&&...);
            }(a,b,std::make_integer_sequence<int,m*n>{});
    }
    else if constexpr(access_flags(B::type)&2) // we can use get
    {
        int dyn_m=a.Rows(),dyn_n=a.Columns();
        for(int j=0;j<dyn_n;j++)
            for(int i=0;i<dyn_m;i++)
                if(get(a,i,j)!=get(b,i,j))
                    return false;
        return true;
    }
    else if constexpr(access_flags(B::type)&1) // we can use lower/upper
    {
        int dyn_m=a.Rows(),dyn_n=a.Columns();
        for(int j=0;j<dyn_n;j++)
        {
            for(int i=0;i<j;i++)
                if(upper(a,i,j)!=upper(b,i,j))
                    return false;
            for(int i=j;i<dyn_m;i++)
                if(lower(a,i,j)!=lower(b,i,j))
                    return false;
        }
        return true;
    }
    else // must use diag/lower_
    {
        int dyn_m=a.Rows(),dyn_n=a.Columns();
        for(int j=0;j<dyn_n;j++)
        {
            for(int i=0;i<j;i++)
                if(upper_(a,i,j)!=upper_(b,i,j))
                    return false;
            if(a(j,j)!=b(j,j)) return false;
            for(int i=j+1;i<dyn_m;i++)
                if(lower_(a,i,j)!=lower_(b,i,j))
                    return false;
        }
        return true;
    }
}

template<class A,class R=A>
enable_if_t<IS_MATRIX<A>::value,A>
__attribute__((flatten))
clamp(const A& a,const A& b,const A& c)
{
    typedef A B;
    typedef A C;
    int s=0,dyn_m=a.Rows(),dyn_n=a.Columns();
#define FUNCTION(a,b,c,s) std::clamp(a,b,c)
#include <Core/Matrices/MATRIX_OPERATIONS_FILL.h>
}

template<class A,class R=A>
enable_if_t<IS_MATRIX<A>::value,A>
__attribute__((flatten))
Componentwise_Min(const A& a,const A& b)
{
    typedef A B;
    typedef NO_MATRIX C;C c;
    int s=0,dyn_m=a.Rows(),dyn_n=a.Columns();
#define FUNCTION(a,b,c,s) min(a,b)
#include <Core/Matrices/MATRIX_OPERATIONS_FILL.h>
}

template<class A,class R=A>
enable_if_t<IS_MATRIX<A>::value,A>
__attribute__((flatten))
Componentwise_Max(const A& a,const A& b)
{
    typedef A B;
    typedef NO_MATRIX C;C c;
    int s=0,dyn_m=a.Rows(),dyn_n=a.Columns();
#define FUNCTION(a,b,c,s) max(a,b)
#include <Core/Matrices/MATRIX_OPERATIONS_FILL.h>
}

constexpr int outer_product_type(int a,int b)
{
    if(a==vec_zero || b==vec_zero) return mat_zero;
    return mat_gen;
}

constexpr int outer_product_type(int a)
{
    if(a==vec_zero) return mat_zero;
    return mat_sym;
}

//#####################################################################
// Function Outer_Product
//#####################################################################
template<class A,class B,class T=typename A::SCALAR,
    int m=A::fixed_m,int n=B::fixed_m,
    class R=MAT_TYPE<T,outer_product_type(A::type,B::type),m,n>>
enable_if_t<IS_VECTOR<A>::value&&IS_VECTOR<B>::value,R>
Outer_Product(const A& a,const B& b)
{
    if constexpr(R::type==mat_zero) return R{};
    else if constexpr(m>=0 && n>=0 && m<=size_limit && n<=size_limit)
    {
        return []<int... i>(const auto& a,const auto& b,std::integer_sequence<int,i...>)
            {
                return R{(a(i%m)*b(i/m))...};
            }(a,b,std::make_integer_sequence<int,m*n>{});
    }
    else
    {
        R r((INITIAL_SIZE)a.Size(),(INITIAL_SIZE)b.Size());
        if constexpr(A::type!=vec_zero && B::type!=vec_zero)
        {
            for(int j=0;j<r.n;j++)
                for(int i=0;i<r.m;i++)
                    r(i,j)=a(i)*b(j);
        }
        return r;
    }
}

//#####################################################################
// Function Outer_Product
//#####################################################################
template<class A,class T=typename A::SCALAR,
    int m=A::fixed_m,int n=m,
    class R=MAT_TYPE<T,outer_product_type(A::type),m,n>>
enable_if_t<IS_VECTOR<A>::value,R>
Outer_Product(const A& a)
{
    if constexpr(R::type==mat_zero || A::type==vec_zero) return R{};
    else if constexpr(m>=0 && m<=size_limit)
    {
        return []<int... i>(const auto& a,std::integer_sequence<int,i...>)
            {
                return R{(a(sym_i(m,i))*a(sym_j(m,i)))...};
            }(a,std::make_integer_sequence<int,m*(m+1)/2>{});
    }
    else
    {
        R r((INITIAL_SIZE)a.Size(),(INITIAL_SIZE)a.Size());
        if constexpr(A::type!=vec_zero)
        {
            for(int j=0;j<r.n;j++)
                for(int i=j;i<r.m;i++)
                    r(i,j)=r(j,i)=a(i)*a(j);
        }
        return r;
    }
}

//#####################################################################
// Function Symmetric_Outer_Product
//#####################################################################
template<class A,class B,class T=typename A::SCALAR,
    int m=A::fixed_m,int n=B::fixed_m,
    class R=MAT_TYPE<T,make_sym(outer_product_type(A::type,B::type)),m,n>>
enable_if_t<IS_VECTOR<A>::value&&IS_VECTOR<B>::value,R>
Symmetric_Outer_Product(const A& a,const B& b)
{
    ASSERT_COMPAT(a,m,b,m);
    if constexpr(R::type==mat_zero) return R{};
    else if constexpr(m>=0 && n>=0 && m<=size_limit && n<=size_limit)
    {
        return []<int... i>(const auto& a,const auto& b,std::integer_sequence<int,i...>)
            {
                return R{(a(sym_i(m,i))*b(sym_j(m,i))+b(sym_i(m,i))*a(sym_j(m,i)))...};
            }(a,b,std::make_integer_sequence<int,m*(m+1)/2>{});
    }
    else
    {
        R r((INITIAL_SIZE)a.Size(),(INITIAL_SIZE)b.Size());
        if constexpr(A::type!=vec_zero && B::type!=vec_zero)
        {
            for(int j=0;j<r.n;j++)
                for(int i=j;i<r.m;i++)
                    r(i,j)=r(j,i)=a(i)*b(j)+a(j)*b(i);
        }
        return r;
    }
}

template<class A> auto
Times_Self_Transpose(const A& a)
{return Times_Transpose_With_Symmetric_Result(a,a);}

template<class A> auto
Transpose_Times_Self(const A& a)
{return Transpose_Times_With_Symmetric_Result(a,a);}
}
#endif
