//#####################################################################
// Copyright 2013.
// This file is part of PhysBAM whose distribution is governed by the license contained in the accompanying file PHYSBAM_COPYRIGHT.txt.
//#####################################################################
// Class SYMMETRIC_MATRIX
//#####################################################################
#ifndef __SYMMETRIC_MATRIX__
#define __SYMMETRIC_MATRIX__
#include <Core/Log/DEBUG_UTILITIES.h>
#include <Core/Math_Tools/Componentwise_Min_Max.h>
#include <Core/Matrices/Eigenvalues.h>
#include <Core/Matrices/GENERAL_MATRIX_BASE.h>
#include <Core/Matrices/MATRIX_OPERATIONS.h>
#include <Core/Vectors/VECTOR.h>
namespace PhysBAM{

template<class T> class RANDOM_NUMBERS;
template<class T,int d>
class SYMMETRIC_MATRIX:public GENERAL_MATRIX_BASE<T,SYMMETRIC_MATRIX<T,d>>
{
public:
    typedef SCALAR_TYPE<T> SCALAR;
    static constexpr int m=d,n=d,size=d*(d+1)/2;
    static constexpr int fixed_m=m,fixed_n=n;
    static constexpr bool is_sym=true,is_diag=false;
    static constexpr int type=mat_sym;

    T x[size+!size];

    SYMMETRIC_MATRIX()
        :x{}
    {
    }

    explicit SYMMETRIC_MATRIX(INITIAL_SIZE mm,INITIAL_SIZE nn=(INITIAL_SIZE)d)
        :x{}
    {
        assert(Value(mm)==d);
        assert(Value(mm)==d);
    }

    template<class ...Args,class R=std::enable_if_t<(1+sizeof...(Args)==d*(d+1)/2)>>
    SYMMETRIC_MATRIX(const T& a,Args&& ...args)
        :x{a,((T)args)...}
    {
    }

    SYMMETRIC_MATRIX(const SYMMETRIC_MATRIX& A)=default;

private:    
    template<class U,int... i>
    SYMMETRIC_MATRIX(const SYMMETRIC_MATRIX<U,d>& A,std::integer_sequence<int,i...>)
        :x{((T)A.x[i])...}
    {
    }
    template<int... i>
    SYMMETRIC_MATRIX(const DIAGONAL_MATRIX<T,m>& A,std::integer_sequence<int,i...>)
        :x{(sym_diag(d,i)?A.x(sym_i(d,i)):T{})...}
    {
    }
public:

    template<class T2>
    explicit SYMMETRIC_MATRIX(const SYMMETRIC_MATRIX<T2,d>& A)
        :SYMMETRIC_MATRIX(A,std::make_integer_sequence<int,d*(d+1)/2>{})
    {
    }

    SYMMETRIC_MATRIX(const DIAGONAL_MATRIX<T,d>& A)
        :SYMMETRIC_MATRIX(A,std::make_integer_sequence<int,d*(d+1)/2>{})
    {
    }

    SYMMETRIC_MATRIX(const SCALE_MATRIX<T,d>& A)
    {
        *this+=A.x;
    }

    SYMMETRIC_MATRIX(const IDENTITY_MATRIX<T,d>& A)
    {
        *this+=1;
    }

    SYMMETRIC_MATRIX(const ZERO_MATRIX<T,d>& A)
    {
    }

    int Rows() const
    {return d;}

    int Columns() const
    {return d;}

    T& operator()(int i,int j)
    {return i<j?Element_Upper(i,j):Element_Lower(i,j);}

    const T& operator()(int i,int j) const
    {return i<j?Element_Upper(i,j):Element_Lower(i,j);}

    bool Valid_Index(const int i,const int j) const
    {return (unsigned)i<d && (unsigned)j<d;}

    T& Element_Upper(int i,int j)
    {return Element_Lower(j,i);}

    const T& Element_Upper(int i,int j) const
    {return Element_Lower(j,i);}

    int Raw_Index(int i,int j) const
    {
        if constexpr(d<=1) return 0;
        else if constexpr(d==2) return i+j;
        else if constexpr(d==3) return (0542431210>>(9*j+3*i))&7;
        else return ((2*d-j-1)*j>>1)+i;
    }
    
    T& Element_Lower(int i,int j)
    {assert((unsigned)i<(unsigned)d && (unsigned)j<=(unsigned)i);return x[Raw_Index(i,j)];}

    const T& Element_Lower(int i,int j) const
    {assert((unsigned)i<(unsigned)d && (unsigned)j<=(unsigned)i);return x[Raw_Index(i,j)];}

    VECTOR<T,d> Column(const int j) const
    {return Get_Column(*this,j);}

    VECTOR<T,d> Row(const int j) const
    {return Get_Row(*this,j);}

    bool operator!=(const SYMMETRIC_MATRIX& A) const
    {return !(*this==A);}

    void From_Lower(const MATRIX<T,d>& m)
    {
        []<int... i>(auto& a,const auto& b,std::integer_sequence<int,i...>)
            {
                ((a.x[i]=b(sym_i(d,i),sym_j(d,i))),...);
            }(*this,m,std::make_integer_sequence<int,d*(d+1)/2>{});
    }

    void From_Upper(const MATRIX<T,d>& m)
    {
        []<int... i>(auto& a,const auto& b,std::integer_sequence<int,i...>)
            {
                ((a.x[i]=b(sym_j(d,i),sym_i(d,i))),...);
            }(*this,m,std::make_integer_sequence<int,d*(d+1)/2>{});
    }

    T Determinant() const;
    SYMMETRIC_MATRIX Cofactor_Matrix() const;
    SYMMETRIC_MATRIX Inverse() const;
    VECTOR<T,d> Inverse_Times(const VECTOR<T,d>& u) const
    {return MATRIX<T,d>(*this).Inverse_Times(u);}

    static SYMMETRIC_MATRIX Unit_Matrix(const T scale=1)
    {SYMMETRIC_MATRIX s;for(int i=0;i<size;i++) s.x[i]=scale;return s;}

    SYMMETRIC_MATRIX Positive_Definite_Part() const
    {auto [D,V]=Solve_Eigenproblem();return Conjugate(V,DIAGONAL_MATRIX<T,d>(Componentwise_Max(D.x,(T)0)));}

    DIAGONAL_MATRIX<T,d> Eigenvalues() const
    {return ::PhysBAM::Eigenvalues(*this);}

    std::pair<DIAGONAL_MATRIX<T,d>,MATRIX<T,d>> Solve_Eigenproblem() const
    {return Eigenvalue_Decomposition(*this);}

    template<class RW> void Read(UNTYPED_ISTREAM<RW>& input)
    {Read_Fixed_Matrix_Upper(input,*this,x);}

    template<class RW> void Write(UNTYPED_OSTREAM<RW>& output) const
    {Write_Fixed_Matrix(output,*this,x);}

    void Fill_Uniform(RANDOM_NUMBERS<T>& rand,const T a,const T b)
    {for(auto&i:x) rand.Fill_Uniform(i,a,b);}

    T* begin() // for stl
    {return x;}

    const T* begin() const // for stl
    {return x;}

    T* end() // for stl
    {return x+size;}

    const T* end() const // for stl
    {return x+size;}
};

template<int i,int j,class T,int d> auto get(const SYMMETRIC_MATRIX<T,d>& a)
{
    if constexpr(j<=i) return a.Element_Lower(i,j);
    else return a.Element_Lower(j,i);
}

template<int i,int j,class T,int d> void set(SYMMETRIC_MATRIX<T,d>& a, T x)
{
    if constexpr(j<=i) a.Element_Lower(i,j)=x;
    else a.Element_Lower(j,i)=x;
}
template<class T,int d> auto diag(const SYMMETRIC_MATRIX<T,d>& a,int i){return a.Element_Lower(i,i);}
template<class T,int d> auto lower(const SYMMETRIC_MATRIX<T,d>& a,int i,int j){return a.Element_Lower(i,j);}
template<class T,int d> auto lower_(const SYMMETRIC_MATRIX<T,d>& a,int i,int j){return a.Element_Lower(i,j);}
template<class T,int d> auto upper(const SYMMETRIC_MATRIX<T,d>& a,int i,int j){return a.Element_Lower(j,i);}
template<class T,int d> auto upper_(const SYMMETRIC_MATRIX<T,d>& a,int i,int j){return a.Element_Lower(j,i);}

template<class T,int m> T
Double_Contract(const SYMMETRIC_MATRIX<T,m>& a,const SYMMETRIC_MATRIX<T,m>& b)
{return b.Double_Contract(a);}
//#####################################################################
// Function Determinant
//#####################################################################
template<class T,int d> T SYMMETRIC_MATRIX<T,d>::
Determinant() const
{
    static_assert(m==n);
    if constexpr(m==0) return 0;
    else if constexpr(m==1) return x[0];
    else if constexpr(m==2) return x[0]*x[2]-x[1]*x[1];
    else if constexpr(m==3)
        return x[0]*(x[3]*x[5]-x[4]*x[4])+x[1]*(2*x[4]*x[2]-x[1]*x[5])-x[2]*x[2]*x[3];
    else return MATRIX<T,d>(*this).Determinant();
}
//#####################################################################
// Function Cofactor_Matrix
//#####################################################################
template<class T,int d> SYMMETRIC_MATRIX<T,d> SYMMETRIC_MATRIX<T,d>::
Cofactor_Matrix() const
{
    if constexpr(d==0) return {};
    else if constexpr(d==1) return {1};
    else if constexpr(d==2) return {x[2],-x[1],x[0]};
    else if constexpr(d==3)
        return {
            x[3]*x[5]-x[4]*x[4],x[4]*x[2]-x[1]*x[5],x[1]*x[4]-x[3]*x[2],
            x[0]*x[5]-x[2]*x[2],x[1]*x[2]-x[0]*x[4],x[0]*x[3]-x[1]*x[1]};
    else PHYSBAM_NOT_IMPLEMENTED();
}
//#####################################################################
// Function Inverse
//#####################################################################
template<class T,int d> SYMMETRIC_MATRIX<T,d> SYMMETRIC_MATRIX<T,d>::
Inverse() const
{
    static_assert(m==n);
    if constexpr(m==0) return {};
    else if constexpr(m<=3)
    {
        auto M=Cofactor_Matrix();
        return M.Transposed()/M.Column(0).Dot(Row(0));
    }
    else
    {
        MATRIX<T,d> M;
        MATRIX<T,d>(*this).PLU_Inverse(M);
        SYMMETRIC_MATRIX<T,d> R;
        R.From_Lower(M);
        return R;
    }
}
template<class T,int d> inline std::ostream&
operator<<(std::ostream& o,const SYMMETRIC_MATRIX<T,d>& A)
{o<<"[";for(int i=0;i<d;i++){for(int j=0;j<d;j++){o<<A(i,j);if(j<d-1) o<<" ";}if(i<d-1) o<<"; ";}o<<"]";return o;}
}
#endif
