//#####################################################################
// Copyright 2007, Craig Schroeder, Tamar Shinar.
// This file is part of PhysBAM whose distribution is governed by the license contained in the accompanying file PHYSBAM_COPYRIGHT.txt.
//#####################################################################
// Class MATRIX
//#####################################################################
#ifndef __MATRIX__
#define __MATRIX__

#include <Core/Matrices/MATRIX_BASE.h>
#include <Core/Matrices/Singular_Value_Decomposition.h>
#include <Core/Vectors/VECTOR.h>
#include <Core/Vectors/ZERO_VECTOR.h>
#include <utility>
namespace PhysBAM{

template<class T,int m_input,int n_input> // n_input=m_input
class MATRIX:public MATRIX_BASE<T,MATRIX<T,m_input,n_input> >
{
public:
    static constexpr int m=m_input,n=n_input,size=n_input*m_input;
    static constexpr int fixed_m=m,fixed_n=n;
    typedef T SCALAR;typedef MATRIX_BASE<T,MATRIX<T,m_input,n_input> > BASE;
    using BASE::Frobenius_Norm_Squared;
    using BASE::Transpose_Times;using BASE::Times_Transpose;
    using BASE::PLU_Solve;using BASE::Column;using BASE::Row;
    using BASE::Set_Zero_Matrix;

    T x[m*n+(m*n==0)]; // pointer to the one dimensional data

    MATRIX()
        :x{}
    {
    }

    explicit MATRIX(INITIAL_SIZE mm,INITIAL_SIZE nn=INITIAL_SIZE(n))
        :x{}
    {
        assert(mm==INITIAL_SIZE(m) && nn==INITIAL_SIZE(n));
    }

    template<class ...Args,class R=std::enable_if_t<(1+sizeof...(Args)==m*n)>>
    MATRIX(const T& a,Args&& ...args)
        :x{a,((T)args)...}
    {
    }

    template<class ...Args,
        class R=std::enable_if_t<(1+sizeof...(Args)==n)&&
            (is_same<Args,VECTOR<T,m>>::value&&...)>>
    MATRIX(const VECTOR<T,m>& a,const Args& ...args)
    {
        T* p=x;
        std::copy(a.begin(),a.end(),p);
        (std::copy(args.begin(),args.end(),p+=m),...);
    }

    MATRIX(const MATRIX& A)=default;

private:    
    template<class MAT,int... i>
    MATRIX(const MAT& A,std::integer_sequence<int,i...>)
        :BASE(),x{((T)A(i%m,i/m))...}
    {
    }
    template<int... i>
    MATRIX(const DIAGONAL_MATRIX<T,m>& A,std::integer_sequence<int,i...>)
        :BASE(),x{(i%(m+1)==0?A(i%m,i/m):T{})...}
    {
    }
public:

    template<class T2>
    explicit MATRIX(const MATRIX<T2,m,n>& A)
        :MATRIX(A,std::make_integer_sequence<int,m*n>{})
    {
    }

    MATRIX(const SYMMETRIC_MATRIX<T,m>& A)
        :MATRIX(A,std::make_integer_sequence<int,m*n>{})
    {
        static_assert(m==n);
    }

    MATRIX(const DIAGONAL_MATRIX<T,m>& A)
        :MATRIX(A,std::make_integer_sequence<int,m*n>{})
    {
        static_assert(m==n);
    }

    template<class T_MATRIX>
    MATRIX(const MATRIX_BASE<T,T_MATRIX>& A)
        :MATRIX(A,std::make_integer_sequence<int,m*n>{})
    {
        static_assert(compat(m,T_MATRIX::fixed_m));
        static_assert(compat(n,T_MATRIX::fixed_n));
        assert(m==A.Rows() && n==A.Columns());
    }

    template<int p=n,class R=std::enable_if_t<m==p>>
    MATRIX(const SCALE_MATRIX<T,m>& A)
        :x{}
    {
        *this+=A.x;
    }

    template<int p=n,class R=std::enable_if_t<m==p>>
    MATRIX(const IDENTITY_MATRIX<T,m>& A)
        :x{}
    {
        *this+=1;
    }

    MATRIX(const ZERO_MATRIX<T,m,n>& A)
        :x{}
    {
    }

    // // Construct from columns
    // MATRIX(std::initializer_list<VECTOR<T,m>> a)
    // {
    //     assert(a.size()==n);
    //     T* p=x;
    //     for(const auto& i:a)
    //         std::copy(i.begin(),i.end(),p+=m);
    // }

    int Rows() const
    {return m;}

    int Columns() const
    {return n;}

    T& operator()(const int i,const int j)
    {assert((unsigned)i<m);assert((unsigned)j<n);return x[j*m+i];}

    const T& operator()(const int i,const int j) const
    {assert((unsigned)i<m);assert((unsigned)j<n);return x[j*m+i];}

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

    bool operator==(const MATRIX& A) const
    {
        return []<int...i>(const auto&t,const auto&a,std::integer_sequence<int,i...>)
            {
                return (true&&...&&(t.x[i]==a.x[i]));
            }(*this,A,std::make_integer_sequence<int,m*n>{});
    }

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

    MATRIX& operator=(const MATRIX& A)=default;

    template<class T_MATRIX>
    MATRIX& operator=(const MATRIX_BASE<T,T_MATRIX>& A)
    {assert(Rows()==A.Rows() && Columns()==A.Columns());
    for(int j=0;j<Columns();j++) for(int i=0;i<Rows();i++) (*this)(i,j)=A(i,j);
    return *this;}

    template<int mm,class R=std::enable_if_t<m==mm>>
    MATRIX& operator=(const SYMMETRIC_MATRIX<T,mm>& A)
    {static_assert(m==n);
    for(int j=0;j<n;j++) for(int i=0;i<m;i++) (*this)(i,j)=A(i,j);
    return *this;}

    template<int mm,class R=std::enable_if_t<m==mm>>
    MATRIX& operator=(const DIAGONAL_MATRIX<T,mm>& A)
    {static_assert(m==n);
    Set_Zero_Matrix();
    for(int i=0;i<m;i++) (*this)(i,i)=A(i,i);
    return *this;}

    SYMMETRIC_MATRIX<T,n> Normal_Equations_Matrix() const
    {return Transpose_Times_With_Symmetric_Result(*this,*this);}

    SYMMETRIC_MATRIX<T,m> Outer_Product_Matrix() const
    {return Times_Transpose_With_Symmetric_Result(*this,*this);}

    SYMMETRIC_MATRIX<T,m> Twice_Symmetric_Part() const
    {return ::PhysBAM::Twice_Symmetric_Part(*this);}

    SYMMETRIC_MATRIX<T,m> Symmetric_Part() const
    {return ::PhysBAM::Symmetric_Part(*this);}

    DIAGONAL_MATRIX<T,m> Diagonal_Part() const
    {static_assert(m==n);return ::PhysBAM::Diagonal_Part(*this);}

    static MATRIX Identity_Matrix()
    {return MATRIX()+1;}

    T 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[3]-x[1]*x[2];
        else if constexpr(m==3)
            return x[0]*(x[4]*x[8]-x[7]*x[5])+
                x[3]*(x[7]*x[2]-x[1]*x[8])+
                x[6]*(x[1]*x[5]-x[4]*x[2]);
        else return BASE::Determinant();
    }

    T Parallelepiped_Measure() const
    {
        if constexpr(m==n) return Determinant();
        else if constexpr(m>n) return Gram_Schmidt_QR().second.Determinant();
        else PHYSBAM_FATAL_ERROR();
    }

    std::pair<MATRIX<T,m,n>,MATRIX<T,n,n>> Gram_Schmidt_QR() const;

    MATRIX Inverse() const;
    MATRIX Cofactor_Matrix() const;

    template<int p=std::min(m,n)>
    std::tuple<MATRIX<T,m,p>,DIAGONAL_MATRIX<T,p>,MATRIX<T,n,p>>
    Singular_Value_Decomposition() const
    {
        if constexpr((m==n && m<=3) || (m==3 && n==2))
            return ::PhysBAM::Singular_Value_Decomposition(*this);
        else
        {
            std::tuple<MATRIX<T,m,p>,DIAGONAL_MATRIX<T,p>,MATRIX<T,n,p>> t;
//            ::PhysBAM::Singular_Value_Decomposition(*this,get<0>(t),get<1>(t).x,get<2>(t));
            PHYSBAM_FATAL_ERROR();
            return t;
        }
    }

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

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

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

    const T* end() const // for stl
    {return x+m*n;}
    
    template<class RW> void Read(UNTYPED_ISTREAM<RW>& input)
    {Read_Fixed_Matrix(input,*this,x);}

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

template<class T>
VECTOR<T,3> Weighted_Normal(const MATRIX<T,3,2>& m)
{return m.Column(0).Cross(m.Column(1));}

template<class T,int m> T
Double_Contract(const SYMMETRIC_MATRIX<T,m>& a,const MATRIX<T,m,m>& b)
{return b.Double_Contract(a);}

template<class T,int m> T
Double_Contract(const MATRIX<T,m,m>& b,const SYMMETRIC_MATRIX<T,m>& a)
{return b.Double_Contract(a);}

template<class T,int m> T
Double_Contract(const DIAGONAL_MATRIX<T,m>& a,const MATRIX<T,m,m>& b)
{return b.Double_Contract(a);}

template<class T,int m> T
Double_Contract(const MATRIX<T,m,m>& b,const DIAGONAL_MATRIX<T,m>& a)
{return b.Double_Contract(a);}

template<class T,int m,int n> T
Double_Contract(const MATRIX<T,m,n>& a,const MATRIX<T,m,n>& b)
{return a.Double_Contract(b);}

template<class T>
MATRIX<T,0,1> Cross_Product_Matrix(const VECTOR<T,1>& v)
{return {};}

template<class T>
MATRIX<T,1,2> Cross_Product_Matrix(const VECTOR<T,2>& v)
{return MATRIX<T,1,2>{-v.y,v.x};}

template<class T>
MATRIX<T,3> Cross_Product_Matrix(const VECTOR<T,3>& v)
{return MATRIX<T,3>(0,v.z,-v.y,-v.z,0,v.x,v.y,-v.x,0);}

template<class T>
MATRIX<T,1> Cross_Product_Matrix_Alt(const VECTOR<T,0>& v)
{return MATRIX<T,1>(0);}

template<class T>
MATRIX<T,2> Cross_Product_Matrix_Alt(const VECTOR<T,1>& v)
{return MATRIX<T,2>(0,v.x,-v.x,0);}

template<class T>
MATRIX<T,3> Cross_Product_Matrix_Alt(const VECTOR<T,3>& v)
{return Cross_Product_Matrix(v);}

template<class T>
VECTOR<T,0> Contract_Permutation_Tensor(const MATRIX<T,1>& m)
{return {};}

template<class T>
VECTOR<T,1> Contract_Permutation_Tensor(const MATRIX<T,2>& m)
{return VECTOR<T,1>(m.x[2]-m.x[1]);}

template<class T>
VECTOR<T,3> Contract_Permutation_Tensor(const MATRIX<T,3>& m)
{return VECTOR<T,3>(m.x[7]-m.x[5],m.x[2]-m.x[6],m.x[3]-m.x[1]);}

template<class T>
VECTOR<T,1> Antisymmetric_Part_Cross_Product_Vector(const MATRIX<T,2>& m)
{return -(T).5*Contract_Permutation_Tensor(m);}

template<class T>
VECTOR<T,3> Antisymmetric_Part_Cross_Product_Vector(const MATRIX<T,3>& m)
{return -(T).5*Contract_Permutation_Tensor(m);}

template<class T,int m,int n> auto MATRIX<T,m,n>::
Gram_Schmidt_QR() const -> std::pair<MATRIX<T,m,n>,MATRIX<T,n,n>>
{
    MATRIX<T,m,n> Q;
    MATRIX<T,n,n> R;
    VECTOR<VECTOR<T,m>,n> q;
    for(int i=0;i<n;i++)
    {
        auto u=Column(i);
        for(int j=0;j<i;j++)
        {
            T a=u.Dot(q(j));
            R(j,i)=a;
            u-=a*q(j);
        }
        R(i,i)=u.Normalize();
        Q.Set_Column(i,u);
        q(i)=u;
    }
    return {Q,R};
}

template<class T,int m,int n> auto MATRIX<T,m,n>::
Cofactor_Matrix() const -> MATRIX
{
    if constexpr(m==3 && n==2)
    {
        auto normal=Weighted_Normal(*this).Normalized();
        return {Column(1).Cross(normal),normal.Cross(Column(0))};
    }
    else
    {
        static_assert(m==n);
        if constexpr(m==0) return {};
        else if constexpr(m==1) return {1};
        else if constexpr(m==2) return {x[3],-x[2],-x[1],x[0]};
        else if constexpr(m==3)
            return {
                x[4]*x[8]-x[5]*x[7],x[5]*x[6]-x[3]*x[8],x[3]*x[7]-x[4]*x[6],
                x[2]*x[7]-x[1]*x[8],x[0]*x[8]-x[2]*x[6],x[1]*x[6]-x[0]*x[7],
                x[1]*x[5]-x[2]*x[4],x[2]*x[3]-x[0]*x[5],x[0]*x[4]-x[1]*x[3]};
        else PHYSBAM_NOT_IMPLEMENTED();
    }
}

template<class T,int m,int n> auto MATRIX<T,m,n>::
Inverse() const -> MATRIX
{
    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(Column(0));
    }
    else
    {
        MATRIX<T,m,n> M;
        this->PLU_Inverse(M);
        return M;
    }
}
}
#endif
