1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127

use std::ops::{SubAssign, DivAssign, AddAssign};

use array__ops::max_len;
use num::{One, Signed, Zero};

use crate::MatrixMath;

#[const_trait]
pub trait SquareMatrixMath<T, const N: usize>: ~const MatrixMath<T, N, N>
{
    fn inv_matrix(&self) -> Option<[[T; N]; N]>
    where
        T: Signed + PartialOrd + One + Zero + Copy + SubAssign + DivAssign + AddAssign,
        [(); max_len(N, N)]:;
        
    fn solve_matrix(&self, b: &[T; N]) -> [T; N]
    where
        T: Copy + Signed + Zero + One + PartialOrd + AddAssign + SubAssign + DivAssign,
        [(); max_len(N, N)]:;
}

impl<T, const N: usize> SquareMatrixMath<T, N> for [[T; N]; N]
{
    fn inv_matrix(&self) -> Option<[[T; N]; N]>
    where
        T: Signed + PartialOrd + One + Zero + Copy + SubAssign + DivAssign + AddAssign,
        [(); max_len(N, N)]:
    {
        let (p, l, u) = self.lup_matrix();
        
        let mut n = 0;
        while n != N
        {
            if l[n][n].is_zero()
            {
                return None
            }
            if u[n][n].is_zero()
            {
                return None
            }
            n += 1;
        }
    
        let mut ia = [[T::zero(); N]; N];
    
        let mut j = 0;
        while j < N
        {
            let mut i = 0;
            while i < N
            {
                ia[i][j] = p[i][j];
    
                let mut k = 0;
                while k != i
                {
                    ia[i][j] -= l[i][k]*ia[k][j];
                    k += 1;
                }
    
                i += 1;
            }
    
            let mut i = N;
            while i != 0
            {
                i -= 1;
    
                let mut k = i + 1;
                while k != N
                {
                    ia[i][j] -= u[i][k]*ia[k][j];
                    k += 1;
                }
    
                ia[i][j] /= u[i][i];
            }
    
            j += 1;
        }
    
        Some(ia)
    }

    fn solve_matrix(&self, b: &[T; N]) -> [T; N]
    where
        T: Copy + Signed + Zero + One + PartialOrd + AddAssign + SubAssign + DivAssign,
        [(); max_len(N, N)]:
    {
        let (l, u, p) = self.lup_matrix();
    
        let [bp] = core::array::from_ref(b).mul_matrix(&p);
    
        let mut x = bp;
        
        let mut m = 0;
        while m != N
        {
            let mut k = 0;
            while k != m
            {
                x[m] -= l[m][k] * x[k];
                k += 1;
            }
            
            m += 1;
        }
    
        let mut m = N;
        while m != 0
        {
            m -= 1;
    
            let mut k = m + 1;
            while k != N
            {
                x[m] -= u[m][k] * x[k];
                k += 1;
            }
    
            x[m] /= u[m][m];
        }
    
        x
    }
}